物体空间姿态估计// Robust Planar Pose (RPP)algorithm

http://blog.sina.com.cn/s/blog_60f11afd0100ttc9.html

姿态估计问题就是：确定某一三维目标物体的方位指向问题。姿态估计在机器人视觉、动作跟踪和单照相机定标等很多领域都有应用。

在不同领域用于姿态估计的传感器是不一样的，在这里主要讲基于视觉的姿态估计。

基于视觉的姿态估计根据使用的摄像机数目又可分为单目视觉姿态估计和多目视觉姿态估计。根据算法的不同又可分为基于模型的姿态估计和基于学习的姿态估计。

一基于模型的姿态估计方法

基于模型的方法通常利用物体的几何关系或者物体的特征点来估计。其基本思想是利用某种几何模型或结构来表示物体的结构和形状，并通过提取某些物体特征，在模型和图像之间建立起对应关系，然后通过几何或者其它方法实现物体空间姿态的估计。这里所使用的模型既可能是简单的几何形体，如平面、圆柱，也可能是某种几何结构，也可能是通过激光扫描或其它方法获得的三维模型。

基于模型的姿态估计方法是通过比对真实图像和合成图像，进行相似度计算更新物体姿态。目前基于模型的方法为了避免在全局状态空间中进行优化搜索，一般都将优化问题先降解成多个局部特征的匹配问题，非常依赖于局部特征的准确检测。当噪声较大无法提取准确的局部特征的时候，该方法的鲁棒性受到很大影响。

二基于学习的姿态估计方法

基于学习的方法借助于机器学习(machine learning)方法，从事先获取的不同姿态下的训练样本中学习二维观测与三维姿态之间的对应关系，并将学习得到的决策规则或回归函数应用于样本，所得结果作为对样本的姿态估计。基于学习的方法一般采用全局观测特征，不需检测或识别物体的局部特征，具有较好的鲁棒性。其缺点是由于无法获取在高维空间中进行连续估计所需要的密集采样，因此无法保证姿态估计的精度与连续性。

基于学习的姿态估计方法源于姿态识别方法的思想。姿态识别需要预先定义多个姿态类别，每个类别包含了一定的姿态范围；然后为每个姿态类别标注若干训练样本，通过模式分类的方法训练姿态分类器以实现姿态识别。

这一类方法并不需要对物体进行建模，一般通过图像的全局特征进行匹配分析，可以有效的避免局部特征方法在复杂姿态和遮挡关系情况下出现的特征匹配歧义性问题。然而姿态识别方法只能将姿态划分到事先定义的几个姿态类别中，并不能对姿态进行连续的精确的估计。

基于学习的方法一般采用全局观测特征，可以保证算法具有较好的鲁棒性。然而这一类方法的姿态估计精度很大程度依赖于训练的充分程度。要想比较精确地得到二维观测与三维姿态之间的对应关系，就必须获取足够密集的样本来学习决策规则和回归函数。而一般来说所需要样本的数量是随状态空间的维度指数级增加的，对于高维状态空间，事实上不可能获取进行精确估计所需要的密集采样。因此，无法得到密集采样而难以保证估计的精度与连续性，是基于学习的姿态估计方法无法克服的根本困难。

和姿态识别等典型的模式分类问题不同的是，姿态估计输出的是一个高维的姿态向量，而不是某个类别的类标。因此这一类方法需要学习的是一个从高维观测向量到高维姿态向量的映射，目前这在机器学习领域中还是一个非常困难的问题。

 

【姿态估计】Pose estimation algorithm 之 Robust Planar Pose (RPP)algorithm

http://blog.csdn.net/leowangzi/article/details/16846405

The RPP algorithm gives a more stable tracking (less jitter) than ARToolKit's pose estimation algorithm.

The robust pose estimator algorithm has been provided by G. Schweighofer and A. Pinz (Inst.of l.Measurement and Measurement Signal Processing, Graz University of Technology). Details about the algorithm are given in a Technical Report: TR-EMT-2005-01, available here. Thanks go to Thomas Pintaric for implementing the C++ version of this algorithm.

计算机视觉

1. 内参数标定

2. 外参数标定即姿态估计问题。从一组2D点的映射中估计物体的3D姿态。

3. 从三个对应点中恢复姿态，需要的信息是最少的，称为“三点透视问题”即P3P。同理，扩展到N个点，就称为“PnP”。

4. 基于视觉的姿态估计根据使用的摄像机数目分为单目视觉和多目视觉。根据算法又可以分为基于模型的姿态估计和基于学习的姿态估计。

5. OpenCV中有solvePnP以及solvePnPRansac用来实现已知平面四点坐标确定摄像头相对世界坐标系的平移和旋转。cvPOSIT基于正交投影，用仿射投影模型近似透视投影模型，不断迭代计算出估计值。此算法在物体深度相对于物体到相机的距离比较大的时候，算法可能不收敛。

6. 从世界坐标系到相机坐标系的转换，需要矩阵[R|t]，其中R是旋转矩阵，t是位移向量。如果世界坐标系为X，相机坐标系对应坐标为X‘，那么X' = [R|t]*X。从相机坐标系到理想屏幕坐标系的变换就需要内参数矩阵C。那么理想屏幕坐标系L = C*[R|t]*X。如何获得[R|t]，大致是已知模板上的几个关键点在世界坐标系的坐标即X已知，然后在摄像头捕获的帧里获得模板上对应点在屏幕坐标系的坐标即L已知，通过求解线性方程组得到[R|t]的初值，再利用非线性最小二乘法迭代求得最优变换矩阵[R|t]。

7. 大多数情况下，背景是二维平面，识别的物体也是二维平面。对于ARToolkit，识别的Targets就是平面的（但是这种方法鲁棒性不好）。如果内参数矩阵是已知的，那么知道4个或者更多共面不共线的点就可以计算出相机的姿态。

8. 相机姿态估计的问题就是寻找相机的外参数，即是最小化误差函数的问题。误差函数有的基于image-space，有的基于object-space。

9. RPP算法基于object-space为误差函数提供了一种可视化的方法。误差函数有两个局部极小值。在无噪声条件下，第一个局部极小值跟正确的姿态对应。另外的误差函数的极小值就是标准姿态估计算法为什么会抖动的原因。由于姿态估计算法最小化误差函数总是要使用迭代算法，因此需要一个初值。如果初值接近第二个局部极小值，那么迭代算法就收敛到错误的结果。

10. 估计第一个姿态，RPP算法使用任何已知的姿态估计算法，在这里里，使用迭代算法。从第一个姿态使用P3P算法估计第二个姿态。这个姿态跟误差函数的第二个局部极小值接近。使用估算的第二个姿态作为初值，使用迭代算法获得第二个姿态。最终正确的姿态是有最小误差的那个。

11. 这类问题最终都是解线性方程组AX=b的问题。当b∈R(A)时，x=A的广义逆*b;当b∈不R(A)时，能否是Ax接近b呢，即是否有x使||Ax-b||最小，习惯上用2-范数即欧式范数来度量。最小二乘解常存在，然后这样的解未必是唯一的。当在方程无解的情况下，要找到最优解。就是要最小化所有误差的平方和，要找拥有最小平方和的解，即最小二乘。最小化就是把误差向量的长度最小化。

///////

三维姿态：关于solvePnP与cvPOSIT 

转：  http://blog.csdn.net/abc20002929/article/details/8520063

之所以写：

场景：给定物体3D点集与对应的图像2D点集，之后进行姿态计算(即求旋转与位移矩阵)。

在翻阅opencv api时看到这2个函数输出都是旋转与位移，故做简单分析并记录于此。

官方解释：

solvePnP(http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#solvepnp)

Finds an object pose from 3D-2D point correspondences.

bool solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec,bool useExtrinsicGuess=false, int flags=ITERATIVE )

cvPOSIT(http://www.opencv.org.cn/index.php/Cv%E7%85%A7%E7%9B%B8%E6%9C%BA%E5%AE%9A%E6%A0%87%E5%92%8C%E4%B8%89%E7%BB%B4%E9%87%8D%E5%BB%BA#POSIT)

执行POSIT算法

void cvPOSIT( CvPOSITObject* posit_object, CvPoint2D32f* image_points, double focal_length, CvTermCriteria criteria, CvMatr32f rotation_matrix,  CvVect32f translation_vector );

理解：

相同点：1.输入都是3D点集和对应的2D点集，其中cvPOSIT的3D点包含在posit_object结构中

              2.输出均包括旋转矩阵和位移向量

不同点：solvePnP有摄像机的一些内参

solvePnP源码：

void cv::solvePnP( InputArray _opoints, InputArray _ipoints,
                  InputArray _cameraMatrix, InputArray _distCoeffs,
                  OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess )
{
    Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
    int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
    CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
    
    _rvec.create(3, 1, CV_64F);
    _tvec.create(3, 1, CV_64F);
    Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
    CvMat c_objectPoints = opoints, c_imagePoints = ipoints;
    CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs;
    CvMat c_rvec = _rvec.getMat(), c_tvec = _tvec.getMat();
    cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix,
                                 c_distCoeffs.rows*c_distCoeffs.cols ? &c_distCoeffs : 0,
                                 &c_rvec, &c_tvec, useExtrinsicGuess );
}

结论：可以看到，除了前面的一堆数据类型检查和转化外，其实solvePnP调用的是cvFindExtrinsicCameraParams2通过已知的内参进行未知外参求解，是一个精确解；而cvPOSIT是用仿射投影模型近似透视投影模型下，不断迭代计算出来的估计值(在物体深度变化相对于物体到摄像机的距离比较大的时候,这种算法可能不收敛)。

透视变换矩阵(单应矩阵)计算：findHomography 与 getPerspectiveTransform

http://blog.csdn.net/abc20002929/article/details/8709902

两者联系：

    都用于计算单应矩阵，即解一个线性方程组。由于单应矩阵有8个未知数（3*3，其中第9个数为1），所以至少需要4个点（每个点-x,y,提供2个约束方程）。

两者区别：

    1.计算方法不同：通过跟踪源码，发现getPerspectiveTransform用的是SVD分解，findHomography看不出是用什么方法（没注释，一堆等式）。但两者计算结果是一样的。

    2.输入参数不同：getPerspectiveTransform只会拿前4个点去计算，findHomography则会拿一堆点(>=4)去计算(其是不断从一堆点中重复拿出4个点去计算出一个结果，再采用一些优化算法RANSAC/LMEDS去筛选出最优解)。

摄像机模型与标定——单应性

http://www.360doc.com/content/14/0410/14/10724725_367760906.shtml

在计算机视觉中，平面的单应性被定义为一个平面到另外一个平面的投影映射。因此一个二维平面上的点映射到摄像机成像仪上的映射就是平面单应性的例子。如果点Q到成像仪上的点q的映射使用齐次坐标，这种映射可以用矩阵相乘的方式表示。若有一下定义：

则可以将单应性简单的表示为：

这里引入参数s，它是任意尺度的比例（目的是使得单应性定义到该尺度比例）。通常根据习惯放在H的外面。

H有两部分组成：用于定位观察的物体平面的物理变换和使用摄像机内参数矩阵的投影。

物理变换部分是与观测到的图像平面相关的部分旋转R和部分平移t的影响之和，表示如下

这里R为3*3大小的矩阵，t表示一个一个3维的列矢量。

摄像机内参数矩阵用M表示，那么我们重写单应性如下：

我们知道单应性研究的是一个平面上到另外一个平面的映射，那么上述公式中的~Q，就可以简化为平面坐标中的~Q'，即我们使Z=0。即物体平面上的点我们用x,y表示，相机平面上的点，

我们也是用二维点表示。我们去掉了Z方向的坐标，那么相对于旋转矩阵R，R可以分解为R=[r1 r2 r3]，那么r3也就不要了，参考下面的推导：

其中H为：

是一个3×3大小的矩阵.

 故最终的单应性矩阵可表示如下：

OpenCV就是利用上述公式来计算单应性矩阵。它使用同一物体的多个图像来计算每个视场的旋转和平移，同时也计算摄像机的内参数。我们知道旋转和平移共6个参数，摄像机内参数为4个参数。对于每一个视场有6个要求解的新参数和4个不变的相机内参数。对于平面物体如棋盘，能够提供8个方差，即映射一个正方形到四边形可以用4个(x,y)来描述。那么对于两个视场，我们就有8*2=16=2*6+4,即求解所有的参数，至少需要两个视场。

为什么正方形到四边形的四个点的映射可以确定8个方程呢，结果是显然的，我们假设物体平面上的正方形的一个顶点坐标为(u,v)，成像仪与该点对应的点坐标为(x,y)，我们假设它们之间的关系如下：

u=f(x,y);

v=g(x,y);

显然，我们把四点的对应坐标带入到上述公式可以得到8个方程。

这里我们会想物体平面上正方形的四个顶点坐标如何确定，其实我们就可以理解为角点的个数，对于尺度的话，我们有s进行控制。对于图像平面上的角点的位置，我们可以可以通过寻找角点来定位他们的位置。其实对于具体的操作，由于还没细读代码和相关原理，在这里只能大体猜测一下。等日后学习了，再来纠正。

单应性矩阵H把源图像平面上的点集位置与目标图像平面上（通常是成像仪平面）的点集位置联系起来：

OpenCV提供了一个方便的C函数cvFindHomography(),函数接口如下：

[cpp] view plain copy

1. void cvFindHomography(  
2. const CvMat* src_points,  
3. const CvMat* dst_points,  
4. CvMat* homography  
5. );  

1、src_points，dst_points为N×2或者N×3的矩阵，N×2表示点是以像素坐标表示。N×3表示以齐次坐标表示。

2、homography,为3*3大小的矩阵，用来存储输出的结果。

C++函数的接口：

[cpp] view plain copy

1. Mat findHomography( const Mat& srcPoints, const Mat& dstPoints,  
2. Mat& status, int method=0,  
3. double ransacReprojThreshold=3 );  
4. Mat findHomography( const Mat& srcPoints, const Mat& dstPoints,  
5. vector& status, int method=0,  
6. double ransacReprojThreshold=3 );  
7. Mat findHomography( const Mat& srcPoints, const Mat& dstPoints,  
8. int method=0, double ransacReprojThreshold=3 );  

1、srcPoints,dstPoints为CV_32FC2或者vector类型

2、method:0表示使用所有点的常规方法；CV_RANSAC 基于RANSAC鲁棒性的方法；CV_LMEDS 最小中值鲁棒性方法

3、ransacReprojThreshod 仅在RANSAC方法中使用，一个点对被认为是内层围值（非异常值）所允许的最大投影误差。即如果：

那么点i被认为是异常值。如果srcPoints和dstPoints单位是像素，通常意味着在某些情况下这个参数的范围在1到10之间。

4、status，可选的输出掩码，用在CV_RANSAC或者CV_LMEDS方法中。注意输入掩码将被忽略。

这个函数找到并且返回源图像平面和目的图像平面之间的透视变换矩阵H:

使得下面的返回投影误差(back-projection)最小：

如果参数method设置为默认值0，该函数使用一个简单的最小二乘方案来计算初始的单应性估计。

然而，如果不是所有的点对（srcPoints,dstPoints）都适应这个严格的透视变换。（也就是说，有一些异常值），这个初始估计值将很差。在这种情况下，我们可以使用两个鲁棒性算法中的一个。RANSCA和LMEDS这两个方法都尝试不同的随机的相对应点对的子集，每四对点集一组，使用这个子集和一个简单的最小二乘算法来估计单应性矩阵，然后计算得到单应性矩阵的质量quality/goodness。（对于RANSAC方法是内层围点的数量，对于LMeDs是中间的重投影误差）。然后最好的子集用来产生单应性矩阵的初始化估计和inliers/outliers的掩码。

忽略方法，鲁棒性与否，计算得到的单应性矩阵使用Levenberg-Marquardt方法来进一步减少重投影误差，从而进一步提纯。（对于鲁棒性的方法仅使用内围层点(inliers)）。

RANSAC方法，几乎可以处理任含有何异常值比率的情况，但是它需要一个阈值用来区分inliers和outliers。LMeDS方法不需要任何阈值，但是它仅在inliers大于50%的情况下才能正确的工作。最后，如果你确信在你计算得到的特征点仅含一些小的噪声，但是没有异常值，默认的方法可能是最好的选择。（因此，在计算相机参数时，我们或许仅使用默认的方法）

这个函数用来找到初始化内参数和外参数矩阵。单应性矩阵取决于一个尺度，那么通常归一化，以使得h33=1。

///

OpenCV学习笔记（17）双目测距与三维重建的OpenCV实现问题集锦（二）双目定标与双目校正

http://blog.csdn.net/chenyusiyuan/article/details/5963256

三、双目定标和双目校正

双目摄像头定标不仅要得出每个摄像头的内部参数，还需要通过标定来测量两个摄像头之间的相对位置（即右摄像头相对于左摄像头的三维平移 t 和旋转 R 参数）。

图6

要计算目标点在左右两个视图上形成的视差，首先要把该点在左右视图上两个对应的像点匹配起来。然而，在二维空间上匹配对应点是非常耗时的，为了减少匹配搜索范围，我们可以利用极线约束使得对应点的匹配由二维搜索降为一维搜索。

       

图7

而双目校正的作用就是要把消除畸变后的两幅图像严格地行对应，使得两幅图像的对极线恰好在同一水平线上，这样一幅图像上任意一点与其在另一幅图像上的对应点就必然具有相同的行号，只需在该行进行一维搜索即可匹配到对应点。

图8

 

1． 关于cvStereoCalibrate的使用

如果按照 Learning OpenCV 的例程，直接通过cvStereoCalibrate来实现双目定标，很容易产生比较大的图像畸变，边角处的变形较厉害。最好先通过cvCalibrateCamera2() 对每个摄像头单独进行定标，再利用cvStereoCalibrate进行双目定标。这样定标所得参数才比较准确，随后的校正也不会有明显的畸变。我使用的程序主要基于Learning OpenCV 的例程ch12_ex12_3.cpp，其中主要部分如下：

    //////////////////////////////////////////////////////////////////////////
    // 是否首先进行单目定标？
    cvCalibrateCamera2(&_objectPoints, &_imagePoints1, &_npoints, imageSize,    
        &t_M1, &t_D1, NULL, NULL, CV_CALIB_FIX_K3);
    cvCalibrateCamera2(&_objectPoints, &_imagePoints2, &_npoints, imageSize,    
        &t_M2, &t_D2, NULL, NULL, CV_CALIB_FIX_K3);
    //////////////////////////////////////////////////////////////////////////
    // 进行双目定标
    cvStereoCalibrate( &_objectPoints, &_imagePoints1,
        &_imagePoints2, &_npoints,
        &t_M1, &t_D1, &t_M2, &t_D2,
        imageSize, &t_R, &t_T, &t_E, &t_F,
        cvTermCriteria(CV_TERMCRIT_ITER+
        CV_TERMCRIT_EPS, 100, 1e-5));  // flags为默认的CV_CALIB_FIX_INTRINSIC

上面的t_M1(2), t_D1(2) 分别是单目定标后获得的左（右）摄像头的内参矩阵（3*3）和畸变参数向量（1*5）；t_R, t_T 分别是右摄像头相对于左摄像头的旋转矩阵（3*3）和平移向量（3*1）, t_E是包含了两个摄像头相对位置关系的Essential Matrix（3*3），t_F 则是既包含两个摄像头相对位置关系、也包含摄像头各自内参信息的Fundamental Matrix（3*3）。

图9

 

2． cvStereoCalibrate 是怎样计算 Essential Matrix 和 Fundamental Matrix 的？

首先我们以Learning OpenCV第422页为基础，讨论下 Essential Matrix 和 Fundamental Matrix 的构造过程，再看看 OpenCV 是如何进行计算的。

图10

 注：原文中对p_l、p_r 和q_l、q_r 物理意义和计算公式的表述有误，已修正。（2011-04-12）

（1）Essential Matrix

如上图所示，给定一个目标点P，以左摄像头光心O_l为原点。点P相对于光心O_l的观察位置为P_l，相对于光心O_r的观察位置为P_r。点P在左摄像头成像平面上的位置为p_l，在右摄像头成像平面上的位置为p_r。注意P_l、P_r、p_l、p_r 都处于摄像机坐标系，其量纲是与平移向量T相同的（p_l、p_r 在图像坐标系中对应的像素坐标为 q_l、q_r ）。

假设右摄像头相对于左摄像头的相对位置关系由旋转矩阵R和平移向量T表示，则可得：P_r = R(P_l-T)。

现在我们要寻找由点P、O_l和O_r确定的对极平面的表达式。注意到平面上任意一点x与点a的连线垂直于平面法向量n，即向量 (x-a) 与向量 n 的点积为0：(x-a)·n = 0。在Ol坐标系中，光心Or的位置为T，则P、O_l和O_r确定的对极平面可由下式表示：(P_l-T)^T·(P_l×T) = 0。

由P_r = R(P_l-T) 和 R^T=R^-1 可得：(R^TP_r)^T·(P_l×T) = 0。

另一方面，向量的叉积又可表示为矩阵与向量的乘积，记向量T的矩阵表示为S，得：P_l×T = SP_l。

图11

那么就得到：(P_r)^TRSP_l = 0。这样，我们就得到Essential Matrix：E = RS。

通过矩阵E我们知道P_l和P_r的关系满足：(P_r)^TEP_l = 0。进一步地，由 p_l = f_l*P_l/Z_l 和 p_r = f_r*P_r/Z_r 我们可以得到点P在左右两个摄像机坐标系中的观察点 p_l 和 p_r 应满足的极线约束关系为：(p_r)^TEp_l = 0。

注意到 E 是不满秩的，它的秩为2，那么 (p_r)^TEp_l = 0 表示的实际上是一条直线，也就是对极线。

（2）Fundamental Matrix

由于矩阵E并不包含摄像头内参信息，且E是面向摄像头坐标系的。实际上我们更感兴趣的是在图像像素坐标系上去研究一个像素点在另一视图上的对极线，这就需要用到摄像机的内参信息将摄像头坐标系和图像像素坐标系联系起来。在（1）中，p_l和p_r是物理坐标值，对应的像素坐标值为q_l和q_r，摄像头内参矩阵为M，则有：p=M^-1q。从而：(p_r)^TEp_l = 0 à q_r^T(M_r^-1)^TE M_l^-1q_l = 0。这里，我们就得到Fundamental Matrix：F = (M_r^-1)^TE M_l^-1。并有 q_r^TFq_l = 0。

（3）OpenCV的相关计算

由上面的分析可见，求取矩阵E和F关键在于旋转矩阵R和平移向量T的计算，而cvStereoCalibrate的代码中大部分（cvcalibration.cpp的第1886-2180行）也是计算和优化R和T的。在cvcalibration.cpp的第1913-1925行给出了计算R和T初始估计值的基本方法：

    /*
       Compute initial estimate of pose

       For each image, compute:
          R(om) is the rotation matrix of om
          om(R) is the rotation vector of R
          R_ref = R(om_right) * R(om_left)'
          T_ref_list = [T_ref_list; T_right - R_ref * T_left]
          om_ref_list = {om_ref_list; om(R_ref)]

       om = median(om_ref_list)
       T = median(T_ref_list)
    */

具体的计算过程比较繁杂，不好理解，这里就不讨论了，下面是计算矩阵E和F的代码：

 

    if( matE || matF )
    {
        double* t = T_LR.data.db;
        double tx[] =
        {
            0, -t[2], t[1],
            t[2], 0, -t[0],
            -t[1], t[0], 0
        };
        CvMat Tx = cvMat(3, 3, CV_64F, tx);
        double e[9], f[9];
        CvMat E = cvMat(3, 3, CV_64F, e);
        CvMat F = cvMat(3, 3, CV_64F, f);
        cvMatMul( &Tx, &R_LR, &E );
        if( matE )
            cvConvert( &E, matE );
        if( matF )
        {
            double ik[9];
            CvMat iK = cvMat(3, 3, CV_64F, ik);
            cvInvert(&K[1], &iK);
            cvGEMM( &iK, &E, 1, 0, 0, &E, CV_GEMM_A_T );
            cvInvert(&K[0], &iK);
            cvMatMul(&E, &iK, &F);
            cvConvertScale( &F, matF, fabs(f[8]) > 0 ? 1./f[8] : 1 );
        }
    }

 

3． 通过双目定标得出的向量T中，Tx符号为什么是负的？

“@scyscyao：这个其实我也不是很清楚。个人的解释是，双目定标得出的T向量指向是从右摄像头指向左摄像头(也就是Tx为负)，而在OpenCV坐标系中，坐标的原点是在左摄像头的。因此，用作校准的时候，要把这个向量的三个分量符号都要换一下，最后求出的距离才会是正的。

图12

但是这里还有一个问题，就是Learning OpenCV中Q的表达式，第四行第三列元素是-1/Tx，而在具体实践中，求出来的实际值是1/Tx。这里我和maxwellsdemon讨论下来的结果是，估计书上Q表达式里的这个负号就是为了抵消T向量的反方向所设的，但在实际写OpenCV代码的过程中，那位朋友却没有把这个负号加进去。”

图13

scyscyao 的分析有一定道理，不过我觉得还有另外一种解释：如上图所示，摄像机C₁（C₂）与世界坐标系相对位置的外部参数为旋转矩阵R₁（R₂）和平移向量 t₁（t₂），如果下标1代表左摄像机，2代表右摄像机，显然在平移向量的水平分量上有 t_1x > t_2x；若以左摄像机C₁为坐标原点，则可得到如上图所示的旋转矩阵R和平移向量t，由于t_1x > t_2x，则有 t_x < 0。

为了抵消T_x为负，在矩阵Q中元素(4,3)应该加上负号，但在cvStereoRectify代码中并没有添加上，这就使得我们通过 cvReprojectImageTo3D 计算得到的三维数据都与实际值反号了。

 

    if( matQ )
    {
        double q[] =
        {
            1, 0, 0, -cc_new[0].x,
            0, 1, 0, -cc_new[0].y,
            0, 0, 0, fc_new,
            0, 0, 1./_t[idx],
            (idx == 0 ? cc_new[0].x - cc_new[1].x : cc_new[0].y - cc_new[1].y)/_t[idx]
        };
        CvMat Q = cvMat(4, 4, CV_64F, q);
        cvConvert( &Q, matQ );
    }

 

为了避免上述反号的情况，可以在计算得到Q矩阵后，添加以下代码更改 Q[3][2] 的值。

 

// Q 是 Mat 类型矩阵，OpenCV2.1 C++ 模式
    Q.at<double>(3, 2) = -Q.at<double>(3, 2);    
// Q 是 double 数组定义时
    double Q[4][4];
    CvMat t_Q = cvMat(4, 4, CV_64F, Q );
    cvStereoRectify(…);
    Q[3][2]=-Q[3][2];

 

4． 双目校正原理及cvStereoRectify 的应用。

图14

如图14所示，双目校正是根据摄像头定标后获得的单目内参数据（焦距、成像原点、畸变系数）和双目相对位置关系（旋转矩阵和平移向量），分别对左右视图进行消除畸变和行对准，使得左右视图的成像原点坐标一致（CV_CALIB_ZERO_DISPARITY 标志位设置时发生作用）、两摄像头光轴平行、左右成像平面共面、对极线行对齐。在OpenCV2.1版之前，cvStereoRectify 的主要工作就是完成上述操作，校正后的显示效果如图14(c) 所示。可以看到校正后左右视图的边角区域是不规则的，而且对后续的双目匹配求取视差会产生影响，因为这些边角区域也参与到匹配操作中，其对应的视差值是无用的、而且一般数值比较大，在三维重建和机器人避障导航等应用中会产生不利影响。

因此，OpenCV2.1 版中cvStereoRectify新增了4个参数用于调整双目校正后图像的显示效果，分别是 double alpha, CvSize newImgSize, CvRect* roi1, CvRect* roi2。下面结合图15-17简要介绍这4个参数的作用：

（1）newImgSize：校正后remap图像的分辨率。如果输入为(0,0)，则是与原图像大小一致。对于图像畸变系数比较大的，可以把newImgSize 设得大一些，以保留图像细节。

（2）alpha：图像剪裁系数，取值范围是-1、0~1。当取值为 0 时，OpenCV会对校正后的图像进行缩放和平移，使得remap图像只显示有效像素（即去除不规则的边角区域），如图17所示，适用于机器人避障导航等应用；当alpha取值为1时，remap图像将显示所有原图像中包含的像素，该取值适用于畸变系数极少的高端摄像头；alpha取值在0-1之间时，OpenCV按对应比例保留原图像的边角区域像素。Alpha取值为-1时，OpenCV自动进行缩放和平移，其显示效果如图16所示。

（3）roi1, roi2：用于标记remap图像中包含有效像素的矩形区域。对应代码如下：

 

02433     if(roi1)
02434     {
02435         *roi1 = cv::Rect(cvCeil((inner1.x - cx1_0)*s + cx1),
02436                      cvCeil((inner1.y - cy1_0)*s + cy1),
02437                      cvFloor(inner1.width*s), cvFloor(inner1.height*s))
02438             & cv::Rect(0, 0, newImgSize.width, newImgSize.height);
02439     }
02440     
02441     if(roi2)
02442     {
02443         *roi2 = cv::Rect(cvCeil((inner2.x - cx2_0)*s + cx2),
02444                      cvCeil((inner2.y - cy2_0)*s + cy2),
02445                      cvFloor(inner2.width*s), cvFloor(inner2.height*s))
02446             & cv::Rect(0, 0, newImgSize.width, newImgSize.height);
02447     }

 

图15

图16

图17

在cvStereoRectify 之后，一般紧接着使用 cvInitUndistortRectifyMap 来产生校正图像所需的变换参数（mapx, mapy）。

 

//////////////////////////////////////////////////////////////////////////
// 执行双目校正
// 利用BOUGUET方法或HARTLEY方法来校正图像
    mx1 = cvCreateMat( imageSize.height, imageSize.width, CV_32F );
    my1 = cvCreateMat( imageSize.height, imageSize.width, CV_32F );
    mx2 = cvCreateMat( imageSize.height, imageSize.width, CV_32F );
    my2 = cvCreateMat( imageSize.height, imageSize.width, CV_32F );
    double R1[3][3], R2[3][3], P1[3][4], P2[3][4], Q[4][4];
    CvMat t_R1 = cvMat(3, 3, CV_64F, R1);
    CvMat t_R2 = cvMat(3, 3, CV_64F, R2);
    CvMat t_Q = cvMat(4, 4, CV_64F, Q );
    CvRect roi1, roi2;

// IF BY CALIBRATED (BOUGUET'S METHOD)    
    CvMat t_P1 = cvMat(3, 4, CV_64F, P1);
    CvMat t_P2 = cvMat(3, 4, CV_64F, P2);
    cvStereoRectify( &t_M1, &t_M2, &t_D1, &t_D2, imageSize,
        &t_R, &t_T, &t_R1, &t_R2, &t_P1, &t_P2, &t_Q,
        CV_CALIB_ZERO_DISPARITY, 
        0, imageSize, &roi1, &roi2); 
// Precompute maps for cvRemap()
    cvInitUndistortRectifyMap(&t_M1,&t_D1,&t_R1,&t_P1, mx1, my1);
    cvInitUndistortRectifyMap(&t_M2,&t_D2,&t_R2,&t_P2, mx2, my2);

 

5． 为什么cvStereoRectify求出的Q矩阵cx, cy, f都与原来的不同？

“@scyscyao：在实际测量中，由于摄像头摆放的关系，左右摄像头的f, cx, cy都是不相同的。而为了使左右视图达到完全平行对准的理想形式从而达到数学上运算的方便，立体校准所做的工作事实上就是在左右像重合区域最大的情况下，让两个摄像头光轴的前向平行，并且让左右摄像头的f, cx, cy相同。因此，Q矩阵中的值与两个instrinsic矩阵的值不一样就可以理解了。”

注：校正后得到的变换矩阵Q，Q[0][3]、Q[1][3]存储的是校正后左摄像头的原点坐标（principal point）cx和cy，Q[2][3]是焦距f。