双目视觉(1) -- 坐标系变换_双目视觉之空间坐标计算

Stereo vision summary

相机坐标系与像素坐标系之间的关系

• 通过内参矩阵
  z ( u v 1 ) = K ( X Y Z ) z

  $$\begin{pmatrix} u\\ v\\ 1 \end{pmatrix}$$ = K $$\begin{pmatrix} X\\ Y\\ Z\\ \end{pmatrix}$$ z⎝⎛​uv1​⎠⎞​=K⎝⎛​XYZ​⎠⎞​
  其中$K$为内参矩阵，$(X,Y,Z{)}^T$为相机坐标系下的点，$(u,v,1{)}^T$为像素坐标系下的点。$z$为深度。
  反过来：
  $$Z=z$$
  $$Y=\frac{(v-c_y)Z}{f_y}$$
  $$X=\frac{(u-c_x)Z}{f_x}$$
  如果是双目，深度可以由以下公式求出：
  $$z=\frac{fb}{d}$$

• 像素坐标系到相机坐标系的转换也可以通过Q矩阵得到

  • Q矩阵的具体形式
    $$Q=\left[\begin{array}{cccc}1& 0& 0& -c_x\\ 0& 1& 0& -c_y\\ 0& 0& 0& f\\ 0& 0& \frac{-1}{T_x}& \frac{c_x-c_x^{{}^{\prime }}}{T_x}\end{array}\right]$$

• 由Q矩阵转换到相机坐标系
  Q [ u v d 1 ] = [ X Y Z W ] Q

  $$\begin{bmatrix} u\\ v \\ d \\ 1 \end{bmatrix}$$ = $$\begin{bmatrix} X\\ Y\\ Z\\ W \end{bmatrix}$$ Q⎣⎢⎢⎡​uvd1​⎦⎥⎥⎤​=⎣⎢⎢⎡​XYZW​⎦⎥⎥⎤​
  最终将到的向量$(X,Y,Z,W{)}^T$ 用$W$归一化就得到三维的点。
  opecv中可以直接调用reprojectImageTo3D函数：

void reprojectImageTo3D(InputArray disparity,
                        OutputArray _3dImage,
                        InputArray Q, 
                        bool handleMissingValues=false, 
                        int ddepth=-1 ) 
1
2
3
4
5

stereoRectify

• R1 – Output 3x3 rectification transform (rotation matrix) for the first camera.
• R2 – Output 3x3 rectification transform (rotation matrix) for the second camera.
• P1 – Output 3x4 projection matrix in the new (rectified) coordinate systems for the first camera.
• P2 – Output 3x4 projection matrix in the new (rectified) coordinate systems for the second camera.
• Alpha - Free scaling parameter, -1 or absent means the default scaling. alpha = 0 means that the rectified image are zoomed and shifted.

void stereoRectify(InputArray cameraMatrix1,
                   InputArray distCoeffs1,
                   InputArray cameraMatrix2,
                   InputArray distCoeffs2,
                   Size imageSize, 
                   InputArray R, 
                   InputArray T,
                   OutputArray R1,
                   OutputArray R2,
                   OutputArray P1,
                   OutputArray P2, 
                   OutputArray Q,
                   int flags=CALIB_ZERO_DISPARITY,
                   double alpha=-1,
                   Size newImageSize=Size(),
                   Rect* validPixROI1=0, 
                   Rect* validPixROI2=0 
                  )
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

解释一下，对于双目相机，由左相机到右相机有一个旋转$R$和一个平移$t$，OpenCv标定后的坐相机坐标系中，左相机和右相机各自旋转一半使得内参一致。这样在rectify的时候就可以使用同一套内参参数。
Rectify生成remap矩阵的过程如下(针对左摄像头)：

1. $(u,v)$是rectify后像素坐标系中的点
2. $(x,y,z{)}^T=R1.inv()\ast P1.colRange(0,3).inv()\ast (u,v,1{)}^T$ 将像素坐标系中的点重映射到世界坐标系中，现由旋转矩阵的逆转到以前的位置。
3. 由distCoeffs1矩阵对$(x,y,z{)}^T$进行undistortion得到 $(x^{\prime },y^{\prime },z^{\prime }{)}^T$
4. 由cameraMatrix1将$(x^{\prime },y^{\prime },z^{\prime }{)}^T$映射到像素坐标系中，得到$(u^{\prime },v^{\prime })$
5. 所以$(u,v)$对应$(u^{\prime },v^{\prime })$, 这里得到的$u^{\prime }$和$v^{\prime }$都是float， 并非整像素点，rectify后的值可以用bilinear插值得到。