单目初始化

1.前提条件（640*480）

• 参与初始化的两帧各自的特征点数目都需要大于100.
• 两帧特征点成功匹配的数目需要大于或等于100.
• 两帧特征点三角化成功的三维点数目需要大于50.

2.针对条件三 流程如下

1. 记录当前帧和参考帧（第一帧）之间的特征匹配关系。
2. 在特征匹配点对中随机选择8对匹配点作为一组。
3. 对这8对点分别计算基础矩阵F和单应矩阵H，并得到得分。
4. 计算得分比例，根据判断选择基础矩阵还是单应矩阵求位姿和三角化。

3. 求单应矩阵--平面

1. 对特征匹配点坐标进行归一化。
2. 开始迭代，每次选择8对点计算单应矩阵。
3. 根据当次计算的单应矩阵的重投影误差对结果进行评分。
4. 重复2、3步，以得分最高的单应矩阵作为最优的单应矩阵。

3.1坐标归一化 

目的：防止坐标值量级差别较大。

好处：能够提高运算结果的精度；利用归一化处理后的图像坐标，对尺度缩放和原点的选择不敏感。

归一化操作步骤如下：

1.计算每个特征点坐标分别在两个坐标轴方向上的均值meanX，meanY

2.求出平均到每个点上，其坐标偏离均值meanX、meanY的程度，记为meanDevX，meanDevY，并将其倒数作为一个尺度缩放因子sX，sY。

3.用均值和尺度缩放因子对坐标进行归一化

4.用矩阵表示上述变换关系，得到归一化矩阵T

归一化代码如下：

 *  为什么要归一化？*  在相似变换之后(点在不同的坐标系下),他们的单应性矩阵是不相同的*  如果图像存在噪声,使得点的坐标发生了变化,那么它的单应性矩阵也会发生变化*  我们采取的方法是将点的坐标放到同一坐标系下,并将缩放尺度也进行统一 *  对同一幅图像的坐标进行相同的变换,不同图像进行不同变换*  缩放尺度是为了让噪声对于图像的影响在一个数量级上* *  Step 1 计算特征点X,Y坐标的均值 *  Step 2 计算特征点X,Y坐标离均值的平均偏离程度*  Step 3 将x坐标和y坐标分别进行尺度归一化，使得x坐标和y坐标的一阶绝对矩分别为1 *  Step 4 计算归一化矩阵：其实就是前面做的操作用矩阵变换来表示而已* * @param[in] vKeys                               待归一化的特征点* @param[in & out] vNormalizedPoints             特征点归一化后的坐标* @param[in & out] T                             归一化特征点的变换矩阵*/
void Initializer::Normalize(const vector<cv::KeyPoint> &vKeys, vector<cv::Point2f> &vNormalizedPoints, cv::Mat &T)                           //将特征点归一化的矩阵
{// 归一化的是这些点在x方向和在y方向上的一阶绝对矩（随机变量的期望）。// Step 1 计算特征点X,Y坐标的均值 meanX, meanYfloat meanX = 0;float meanY = 0;//获取特征点的数量const int N = vKeys.size();//设置用来存储归一后特征点的向量大小，和归一化前保持一致vNormalizedPoints.resize(N);//开始遍历所有的特征点for(int i=0; i<N; i++){//分别累加特征点的X、Y坐标meanX += vKeys[i].pt.x;meanY += vKeys[i].pt.y;}//计算X、Y坐标的均值meanX = meanX/N;meanY = meanY/N;// Step 2 计算特征点X,Y坐标离均值的平均偏离程度 meanDevX, meanDevY，注意不是标准差float meanDevX = 0;float meanDevY = 0;// 将原始特征点减去均值坐标，使x坐标和y坐标均值分别为0for(int i=0; i<N; i++){vNormalizedPoints[i].x = vKeys[i].pt.x - meanX;vNormalizedPoints[i].y = vKeys[i].pt.y - meanY;//累计这些特征点偏离横纵坐标均值的程度meanDevX += fabs(vNormalizedPoints[i].x);meanDevY += fabs(vNormalizedPoints[i].y);}// 求出平均到每个点上，其坐标偏离横纵坐标均值的程度；将其倒数作为一个尺度缩放因子meanDevX = meanDevX/N;meanDevY = meanDevY/N;float sX = 1.0/meanDevX;float sY = 1.0/meanDevY;// Step 3 将x坐标和y坐标分别进行尺度归一化，使得x坐标和y坐标的一阶绝对矩分别为1 // 这里所谓的一阶绝对矩其实就是随机变量到取值的中心的绝对值的平均值（期望）for(int i=0; i<N; i++){//对，就是简单地对特征点的坐标进行进一步的缩放vNormalizedPoints[i].x = vNormalizedPoints[i].x * sX;vNormalizedPoints[i].y = vNormalizedPoints[i].y * sY;}// Step 4 计算归一化矩阵：其实就是前面做的操作用矩阵变换来表示而已// |sX  0  -meanx*sX|// |0   sY -meany*sY|// |0   0      1    |T = cv::Mat::eye(3,3,CV_32F);T.at<float>(0,0) = sX;T.at<float>(1,1) = sY;T.at<float>(0,2) = -meanX*sX;T.at<float>(1,2) = -meanY*sY;
}

3.2求解单应矩阵

单应矩阵H的自由度是8，一对特征点可以提供两个约束方程，所以只需要4对点提供8个约束方程就可以求解了。

为什么自由度是8？因为Ah=0，矩阵H共有9个元素，去掉一个自由度，就是8个自由度。

 对矩阵A进行SVD分解

 代码：

/*** @brief 用DLT方法求解单应矩阵H* 这里最少用4对点就能够求出来，不过这里为了统一还是使用了8对点求最小二乘解* * @param[in] vP1               参考帧中归一化后的特征点* @param[in] vP2               当前帧中归一化后的特征点* @return cv::Mat              计算的单应矩阵H*/
cv::Mat Initializer::ComputeH21(const vector<cv::Point2f> &vP1, //归一化后的点, in reference frameconst vector<cv::Point2f> &vP2) //归一化后的点, in current frame
{// 基本原理：见附件推导过程：// |x'|     | h1 h2 h3 ||x|// |y'| = a | h4 h5 h6 ||y|  简写: x' = a H x, a为一个尺度因子// |1 |     | h7 h8 h9 ||1|// 使用DLT(direct linear tranform)求解该模型// x' = a H x // ---> (x') 叉乘 (H x)  = 0  (因为方向相同) (取前两行就可以推导出下面的了)// ---> Ah = 0 // A = | 0  0  0 -x -y -1 xy' yy' y'|  h = | h1 h2 h3 h4 h5 h6 h7 h8 h9 |//     |-x -y -1  0  0  0 xx' yx' x'|// 通过SVD求解Ah = 0，A^T*A最小特征值对应的特征向量即为解// 其实也就是右奇异值矩阵的最后一列//获取参与计算的特征点的数目const int N = vP1.size();// 构造用于计算的矩阵 A cv::Mat A(2*N,				//行，注意每一个点的数据对应两行9,				//列CV_32F);      	//float数据类型// 构造矩阵A，将每个特征点添加到矩阵A中的元素for(int i=0; i<N; i++){//获取特征点对的像素坐标const float u1 = vP1[i].x;const float v1 = vP1[i].y;const float u2 = vP2[i].x;const float v2 = vP2[i].y;//生成这个点的第一行A.at<float>(2*i,0) = 0.0;A.at<float>(2*i,1) = 0.0;A.at<float>(2*i,2) = 0.0;A.at<float>(2*i,3) = -u1;A.at<float>(2*i,4) = -v1;A.at<float>(2*i,5) = -1;A.at<float>(2*i,6) = v2*u1;A.at<float>(2*i,7) = v2*v1;A.at<float>(2*i,8) = v2;//生成这个点的第二行A.at<float>(2*i+1,0) = u1;A.at<float>(2*i+1,1) = v1;A.at<float>(2*i+1,2) = 1;A.at<float>(2*i+1,3) = 0.0;A.at<float>(2*i+1,4) = 0.0;A.at<float>(2*i+1,5) = 0.0;A.at<float>(2*i+1,6) = -u2*u1;A.at<float>(2*i+1,7) = -u2*v1;A.at<float>(2*i+1,8) = -u2;}// 定义输出变量，u是左边的正交矩阵U， w为奇异矩阵，vt中的t表示是右正交矩阵V的转置cv::Mat u,w,vt;//使用opencv提供的进行奇异值分解的函数cv::SVDecomp(A,							//输入，待进行奇异值分解的矩阵w,							//输出，奇异值矩阵u,							//输出，矩阵Uvt,						//输出，矩阵V^Tcv::SVD::MODIFY_A | 		//输入，MODIFY_A是指允许计算函数可以修改待分解的矩阵，官方文档上说这样可以加快计算速度、节省内存cv::SVD::FULL_UV);		//FULL_UV=把U和VT补充成单位正交方阵// 返回最小奇异值所对应的右奇异向量// 注意前面说的是右奇异值矩阵的最后一列，但是在这里因为是vt，转置后了，所以是行；由于A有9列数据，故最后一列的下标为8return vt.row(8).reshape(0, 			//转换后的通道数，这里设置为0表示是与前面相同3); 			//转换后的行数,对应V的最后一列
}

3.3检验单应矩阵并评分

通过单应矩阵对已经判断为内点的特征点进行双向投影，计算加权重投影误差，最终选择误差最小的单应矩阵作为最优解。

特征点对之间双向投影，累计所有特征点对中的内点误差，误差越大评分越低。

/*** @brief 对给定的homography matrix打分,需要使用到卡方检验的知识* * @param[in] H21                       从参考帧到当前帧的单应矩阵* @param[in] H12                       从当前帧到参考帧的单应矩阵* @param[in] vbMatchesInliers          匹配好的特征点对的Inliers标记* @param[in] sigma                     方差，默认为1* @return float                        返回得分*/
float Initializer::CheckHomography(const cv::Mat &H21,                 //从参考帧到当前帧的单应矩阵const cv::Mat &H12,                 //从当前帧到参考帧的单应矩阵vector<bool> &vbMatchesInliers,     //匹配好的特征点对的Inliers标记float sigma)                        //估计误差
{// 说明：在已值n维观测数据误差服从N(0，sigma）的高斯分布时// 其误差加权最小二乘结果为  sum_error = SUM(e(i)^T * Q^(-1) * e(i))// 其中：e(i) = [e_x,e_y,...]^T, Q维观测数据协方差矩阵，即sigma * sigma组成的协方差矩阵// 误差加权最小二次结果越小，说明观测数据精度越高// 那么，score = SUM((th - e(i)^T * Q^(-1) * e(i)))的分数就越高// 算法目标： 检查单应变换矩阵// 检查方式：通过H矩阵，进行参考帧和当前帧之间的双向投影，并计算起加权最小二乘投影误差// 算法流程// input: 单应性矩阵 H21, H12, 匹配点集 mvKeys1//    do://        for p1(i), p2(i) in mvKeys://           error_i1 = ||p2(i) - H21 * p1(i)||2//           error_i2 = ||p1(i) - H12 * p2(i)||2//           //           w1 = 1 / sigma / sigma//           w2 = 1 / sigma / sigma// //           if error1 < th//              score +=   th - error_i1 * w1//           if error2 < th//              score +=   th - error_i2 * w2// //           if error_1i > th or error_2i > th//              p1(i), p2(i) are inner points//              vbMatchesInliers(i) = true//           else //              p1(i), p2(i) are outliers//              vbMatchesInliers(i) = false//           end//        end//   output: score, inliers// 特点匹配个数const int N = mvMatches12.size();// Step 1 获取从参考帧到当前帧的单应矩阵的各个元素const float h11 = H21.at<float>(0,0);const float h12 = H21.at<float>(0,1);const float h13 = H21.at<float>(0,2);const float h21 = H21.at<float>(1,0);const float h22 = H21.at<float>(1,1);const float h23 = H21.at<float>(1,2);const float h31 = H21.at<float>(2,0);const float h32 = H21.at<float>(2,1);const float h33 = H21.at<float>(2,2);// 获取从当前帧到参考帧的单应矩阵的各个元素const float h11inv = H12.at<float>(0,0);const float h12inv = H12.at<float>(0,1);const float h13inv = H12.at<float>(0,2);const float h21inv = H12.at<float>(1,0);const float h22inv = H12.at<float>(1,1);const float h23inv = H12.at<float>(1,2);const float h31inv = H12.at<float>(2,0);const float h32inv = H12.at<float>(2,1);const float h33inv = H12.at<float>(2,2);// 给特征点对的Inliers标记预分配空间vbMatchesInliers.resize(N);// 初始化score值float score = 0;// 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）// 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值const float th = 5.991;//信息矩阵，方差平方的倒数const float invSigmaSquare = 1.0/(sigma * sigma);// Step 2 通过H矩阵，进行参考帧和当前帧之间的双向投影，并计算起加权重投影误差// H21 表示从img1 到 img2的变换矩阵// H12 表示从img2 到 img1的变换矩阵 for(int i = 0; i < N; i++){// 一开始都默认为Inlierbool bIn = true;// Step 2.1 提取参考帧和当前帧之间的特征匹配点对const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];const float u1 = kp1.pt.x;const float v1 = kp1.pt.y;const float u2 = kp2.pt.x;const float v2 = kp2.pt.y;// Step 2.2 计算 img2 到 img1 的重投影误差// x1 = H12*x2// 将图像2中的特征点通过单应变换投影到图像1中// |u1|   |h11inv h12inv h13inv||u2|   |u2in1|// |v1| = |h21inv h22inv h23inv||v2| = |v2in1| * w2in1inv// |1 |   |h31inv h32inv h33inv||1 |   |  1  |// 计算投影归一化坐标const float w2in1inv = 1.0/(h31inv * u2 + h32inv * v2 + h33inv);const float u2in1 = (h11inv * u2 + h12inv * v2 + h13inv) * w2in1inv;const float v2in1 = (h21inv * u2 + h22inv * v2 + h23inv) * w2in1inv;// 计算重投影误差 = ||p1(i) - H12 * p2(i)||2const float squareDist1 = (u1 - u2in1) * (u1 - u2in1) + (v1 - v2in1) * (v1 - v2in1);const float chiSquare1 = squareDist1 * invSigmaSquare;// Step 2.3 用阈值标记离群点，内点的话累加得分if(chiSquare1>th)bIn = false;    else// 误差越大，得分越低score += th - chiSquare1;// 计算从img1 到 img2 的投影变换误差// x1in2 = H21*x1// 将图像2中的特征点通过单应变换投影到图像1中// |u2|   |h11 h12 h13||u1|   |u1in2|// |v2| = |h21 h22 h23||v1| = |v1in2| * w1in2inv// |1 |   |h31 h32 h33||1 |   |  1  |// 计算投影归一化坐标const float w1in2inv = 1.0/(h31*u1+h32*v1+h33);const float u1in2 = (h11*u1+h12*v1+h13)*w1in2inv;const float v1in2 = (h21*u1+h22*v1+h23)*w1in2inv;// 计算重投影误差 const float squareDist2 = (u2-u1in2)*(u2-u1in2)+(v2-v1in2)*(v2-v1in2);const float chiSquare2 = squareDist2*invSigmaSquare;// 用阈值标记离群点，内点的话累加得分if(chiSquare2>th)bIn = false;elsescore += th - chiSquare2;   // Step 2.4 如果从img2 到 img1 和 从img1 到img2的重投影误差均满足要求，则说明是Inlier pointif(bIn)vbMatchesInliers[i]=true;elsevbMatchesInliers[i]=false;}return score;
}

4.求基础矩阵F--非平面

1. 对特征匹配点坐标进行归一化
2. 选择8个点对来计算基础矩阵。
3. 根据当次计算的基础矩阵的重投影误差对结果进行评分。
4. 重复2、3步，以得分最高的基础矩阵作为最优的基础矩阵。

4.1使用八点法求解基础矩阵 

 基础矩阵共有9个元素，其中尺度等价性去掉1个自由度，基础矩阵秩为2，再去掉1个自由度，所以自由度应该为7. 

SVD求解基础矩阵

/*** @brief 根据特征点匹配求fundamental matrix（normalized 8点法）* 注意F矩阵有秩为2的约束，所以需要两次SVD分解* * @param[in] vP1           参考帧中归一化后的特征点* @param[in] vP2           当前帧中归一化后的特征点* @return cv::Mat          最后计算得到的基础矩阵F*/
cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1, //归一化后的点, in reference frameconst vector<cv::Point2f> &vP2) //归一化后的点, in current frame
{// 原理详见附件推导// x'Fx = 0 整理可得：Af = 0// A = | x'x x'y x' y'x y'y y' x y 1 |, f = | f1 f2 f3 f4 f5 f6 f7 f8 f9 |// 通过SVD求解Af = 0，A'A最小特征值对应的特征向量即为解//获取参与计算的特征点对数const int N = vP1.size();//初始化A矩阵cv::Mat A(N,9,CV_32F); // N*9维// 构造矩阵A，将每个特征点添加到矩阵A中的元素for(int i=0; i<N; i++){const float u1 = vP1[i].x;const float v1 = vP1[i].y;const float u2 = vP2[i].x;const float v2 = vP2[i].y;A.at<float>(i,0) = u2*u1;A.at<float>(i,1) = u2*v1;A.at<float>(i,2) = u2;A.at<float>(i,3) = v2*u1;A.at<float>(i,4) = v2*v1;A.at<float>(i,5) = v2;A.at<float>(i,6) = u1;A.at<float>(i,7) = v1;A.at<float>(i,8) = 1;}//存储奇异值分解结果的变量cv::Mat u,w,vt;// 定义输出变量，u是左边的正交矩阵U， w为奇异矩阵，vt中的t表示是右正交矩阵V的转置cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);// 转换成基础矩阵的形式cv::Mat Fpre = vt.row(8).reshape(0, 3); // v的最后一列//基础矩阵的秩为2,而我们不敢保证计算得到的这个结果的秩为2,所以需要通过第二次奇异值分解,来强制使其秩为2// 对初步得来的基础矩阵进行第2次奇异值分解cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);// 秩2约束，强制将第3个奇异值设置为0w.at<float>(2)=0; // 重新组合好满足秩约束的基础矩阵，作为最终计算结果返回 return  u*cv::Mat::diag(w)*vt;
}

4.2验证基础矩阵并评分

计算点到极线的距离，并求和，距离越大，误差越大。p2到l2和p1到l1.

代码：

/*** @brief 对给定的Fundamental matrix打分* * @param[in] F21                       当前帧和参考帧之间的基础矩阵* @param[in] vbMatchesInliers          匹配的特征点对属于inliers的标记* @param[in] sigma                     方差，默认为1* @return float                        返回得分*/
float Initializer::CheckFundamental(const cv::Mat &F21,             //当前帧和参考帧之间的基础矩阵vector<bool> &vbMatchesInliers, //匹配的特征点对属于inliers的标记float sigma)                    //方差
{// 说明：在已值n维观测数据误差服从N(0，sigma）的高斯分布时// 其误差加权最小二乘结果为  sum_error = SUM(e(i)^T * Q^(-1) * e(i))// 其中：e(i) = [e_x,e_y,...]^T, Q维观测数据协方差矩阵，即sigma * sigma组成的协方差矩阵// 误差加权最小二次结果越小，说明观测数据精度越高// 那么，score = SUM((th - e(i)^T * Q^(-1) * e(i)))的分数就越高// 算法目标：检查基础矩阵// 检查方式：利用对极几何原理 p2^T * F * p1 = 0// 假设：三维空间中的点 P 在 img1 和 img2 两图像上的投影分别为 p1 和 p2（两个为同名点）//   则：p2 一定存在于极线 l2 上，即 p2*l2 = 0. 而l2 = F*p1 = (a, b, c)^T//      所以，这里的误差项 e 为 p2 到 极线 l2 的距离，如果在直线上，则 e = 0//      根据点到直线的距离公式：d = (ax + by + c) / sqrt(a * a + b * b)//      所以，e =  (a * p2.x + b * p2.y + c) /  sqrt(a * a + b * b)// 算法流程// input: 基础矩阵 F 左右视图匹配点集 mvKeys1//    do://        for p1(i), p2(i) in mvKeys://           l2 = F * p1(i)//           l1 = p2(i) * F//           error_i1 = dist_point_to_line(x2,l2)//           error_i2 = dist_point_to_line(x1,l1)//           //           w1 = 1 / sigma / sigma//           w2 = 1 / sigma / sigma// //           if error1 < th//              score +=   thScore - error_i1 * w1//           if error2 < th//              score +=   thScore - error_i2 * w2// //           if error_1i > th or error_2i > th//              p1(i), p2(i) are inner points//              vbMatchesInliers(i) = true//           else //              p1(i), p2(i) are outliers//              vbMatchesInliers(i) = false//           end//        end//   output: score, inliers// 获取匹配的特征点对的总对数const int N = mvMatches12.size();// Step 1 提取基础矩阵中的元素数据const float f11 = F21.at<float>(0,0);const float f12 = F21.at<float>(0,1);const float f13 = F21.at<float>(0,2);const float f21 = F21.at<float>(1,0);const float f22 = F21.at<float>(1,1);const float f23 = F21.at<float>(1,2);const float f31 = F21.at<float>(2,0);const float f32 = F21.at<float>(2,1);const float f33 = F21.at<float>(2,2);// 预分配空间vbMatchesInliers.resize(N);// 设置评分初始值（因为后面需要进行这个数值的累计）float score = 0;// 基于卡方检验计算出的阈值// 自由度为1的卡方分布，显著性水平为0.05，对应的临界阈值// ?是因为点到直线距离是一个自由度吗？const float th = 3.841;// 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值const float thScore = 5.991;// 信息矩阵，或 协方差矩阵的逆矩阵const float invSigmaSquare = 1.0/(sigma*sigma);// Step 2 计算img1 和 img2 在估计 F 时的score值for(int i=0; i<N; i++){//默认为这对特征点是Inliersbool bIn = true;// Step 2.1 提取参考帧和当前帧之间的特征匹配点对const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];// 提取出特征点的坐标const float u1 = kp1.pt.x;const float v1 = kp1.pt.y;const float u2 = kp2.pt.x;const float v2 = kp2.pt.y;// Reprojection error in second image// Step 2.2 计算 img1 上的点在 img2 上投影得到的极线 l2 = F21 * p1 = (a2,b2,c2)const float a2 = f11*u1+f12*v1+f13;const float b2 = f21*u1+f22*v1+f23;const float c2 = f31*u1+f32*v1+f33;// Step 2.3 计算误差 e = (a * p2.x + b * p2.y + c) /  sqrt(a * a + b * b)const float num2 = a2*u2+b2*v2+c2;const float squareDist1 = num2*num2/(a2*a2+b2*b2);// 带权重误差const float chiSquare1 = squareDist1*invSigmaSquare;// Step 2.4 误差大于阈值就说明这个点是Outlier // ? 为什么判断阈值用的 th（1自由度），计算得分用的thScore（2自由度）// ? 可能是为了和CheckHomography 得分统一？if(chiSquare1>th)bIn = false;else// 误差越大，得分越低score += thScore - chiSquare1;// 计算img2上的点在 img1 上投影得到的极线 l1= p2 * F21 = (a1,b1,c1)const float a1 = f11*u2+f21*v2+f31;const float b1 = f12*u2+f22*v2+f32;const float c1 = f13*u2+f23*v2+f33;// 计算误差 e = (a * p2.x + b * p2.y + c) /  sqrt(a * a + b * b)const float num1 = a1*u1+b1*v1+c1;const float squareDist2 = num1*num1/(a1*a1+b1*b1);// 带权重误差const float chiSquare2 = squareDist2*invSigmaSquare;// 误差大于阈值就说明这个点是Outlier if(chiSquare2>th)bIn = false;elsescore += thScore - chiSquare2;// Step 2.5 保存结果if(bIn)vbMatchesInliers[i]=true;elsevbMatchesInliers[i]=false;}//  返回评分return score;
}

5.特征点对三角化 

 

 

 

代码：

/** 给定投影矩阵P1,P2和图像上的匹配特征点点kp1,kp2，从而计算三维点坐标* @brief * * @param[in] kp1               特征点, in reference frame* @param[in] kp2               特征点, in current frame* @param[in] P1                投影矩阵P1* @param[in] P2                投影矩阵P2* @param[in & out] x3D         计算的三维点*/
void Initializer::Triangulate(const cv::KeyPoint &kp1,    //特征点, in reference frameconst cv::KeyPoint &kp2,    //特征点, in current frameconst cv::Mat &P1,          //投影矩阵P1const cv::Mat &P2,          //投影矩阵P2cv::Mat &x3D)               //三维点
{// 原理// Trianularization: 已知匹配特征点对{x x'} 和 各自相机矩阵{P P'}, 估计三维点 X// x' = P'X  x = PX// 它们都属于 x = aPX模型//                         |X|// |x|     |p1 p2  p3  p4 ||Y|     |x|    |--p0--||.|// |y| = a |p5 p6  p7  p8 ||Z| ===>|y| = a|--p1--||X|// |z|     |p9 p10 p11 p12||1|     |z|    |--p2--||.|// 采用DLT的方法：x叉乘PX = 0// |yp2 -  p1|     |0|// |p0 -  xp2| X = |0|// |xp1 - yp0|     |0|// 两个点:// |yp2   -  p1  |     |0|// |p0    -  xp2 | X = |0| ===> AX = 0// |y'p2' -  p1' |     |0|// |p0'   - x'p2'|     |0|// 变成程序中的形式：// |xp2  - p0 |     |0|// |yp2  - p1 | X = |0| ===> AX = 0// |x'p2'- p0'|     |0|// |y'p2'- p1'|     |0|// 然后就组成了一个四元一次正定方程组，SVD求解，右奇异矩阵的最后一行就是最终的解.//这个就是上面注释中的矩阵Acv::Mat A(4,4,CV_32F);//构造参数矩阵AA.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);//奇异值分解的结果cv::Mat u,w,vt;//对系数矩阵A进行奇异值分解cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);//根据前面的结论，奇异值分解右矩阵的最后一行其实就是解，原理类似于前面的求最小二乘解，四个未知数四个方程正好正定//别忘了我们更习惯用列向量来表示一个点的空间坐标x3D = vt.row(3).t();//为了符合其次坐标的形式，使最后一维为1x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
}

6.检验三角化结果

 确定一个合格的三维点需要通过以下关卡

1. 三维点的3个坐标必须是有限的实数
2. 三维点深度值必须为正
3. 三维点和两帧图像光轴中心夹角需要满足一定的条件。夹角越大，视差越大，三角化结果越准确。
4. 三维点的重投影误差小于设定的阈值。

我们会记录当前位姿对应的合格的三维点数目和视差。位姿可能有多组解，根据如下条件选择最佳的解。

1. 最优解成功三角化点数目明显大于次优解的点数目。
2. 最优解的视差大于设定点阈值。
3. 最优解成功三角化点数目大于设定的阈值。
4. 最优解成功三角化点数目占所有特征点数目的90%以上。 

代码： 

/*** @brief 用位姿来对特征匹配点三角化，从中筛选中合格的三维点* * @param[in] R                                     旋转矩阵R* @param[in] t                                     平移矩阵t* @param[in] vKeys1                                参考帧特征点  * @param[in] vKeys2                                当前帧特征点* @param[in] vMatches12                            两帧特征点的匹配关系* @param[in] vbMatchesInliers                      特征点对内点标记* @param[in] K                                     相机内参矩阵* @param[in & out] vP3D                            三角化测量之后的特征点的空间坐标* @param[in] th2                                   重投影误差的阈值* @param[in & out] vbGood                          标记成功三角化点？* @param[in & out] parallax                        计算出来的比较大的视差角（注意不是最大，具体看后面代码）* @return int */
int Initializer::CheckRT(const cv::Mat &R, const cv::Mat &t, const vector<cv::KeyPoint> &vKeys1, const vector<cv::KeyPoint> &vKeys2,const vector<Match> &vMatches12, vector<bool> &vbMatchesInliers,const cv::Mat &K, vector<cv::Point3f> &vP3D, float th2, vector<bool> &vbGood, float &parallax)
{   // 对给出的特征点对及其R t , 通过三角化检查解的有效性，也称为 cheirality check// Calibration parameters//从相机内参数矩阵获取相机的校正参数const float fx = K.at<float>(0,0);const float fy = K.at<float>(1,1);const float cx = K.at<float>(0,2);const float cy = K.at<float>(1,2);//特征点是否是good点的标记，这里的特征点指的是参考帧中的特征点vbGood = vector<bool>(vKeys1.size(),false);//重设存储空间坐标的点的大小vP3D.resize(vKeys1.size());//存储计算出来的每对特征点的视差vector<float> vCosParallax;vCosParallax.reserve(vKeys1.size());// Camera 1 Projection Matrix K[I|0]// Step 1：计算相机的投影矩阵  // 投影矩阵P是一个 3x4 的矩阵，可以将空间中的一个点投影到平面上，获得其平面坐标，这里均指的是齐次坐标。// 对于第一个相机是 P1=K*[I|0]// 以第一个相机的光心作为世界坐标系, 定义相机的投影矩阵cv::Mat P1(3,4,				//矩阵的大小是3x4CV_32F,			//数据类型是浮点数cv::Scalar(0));	//初始的数值是0//将整个K矩阵拷贝到P1矩阵的左侧3x3矩阵，因为 K*I = KK.copyTo(P1.rowRange(0,3).colRange(0,3));// 第一个相机的光心设置为世界坐标系下的原点cv::Mat O1 = cv::Mat::zeros(3,1,CV_32F);// Camera 2 Projection Matrix K[R|t]// 计算第二个相机的投影矩阵 P2=K*[R|t]cv::Mat P2(3,4,CV_32F);R.copyTo(P2.rowRange(0,3).colRange(0,3));t.copyTo(P2.rowRange(0,3).col(3));//最终结果是K*[R|t]P2 = K*P2;// 第二个相机的光心在世界坐标系下的坐标cv::Mat O2 = -R.t()*t;//在遍历开始前，先将good点计数设置为0int nGood=0;// 开始遍历所有的特征点对for(size_t i=0, iend=vMatches12.size();i<iend;i++){// 跳过outliersif(!vbMatchesInliers[i])continue;// Step 2 获取特征点对，调用Triangulate() 函数进行三角化，得到三角化测量之后的3D点坐标// kp1和kp2是匹配好的有效特征点const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first];const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second];//存储三维点的的坐标cv::Mat p3dC1;// 利用三角法恢复三维点p3dC1Triangulate(kp1,kp2,	//特征点P1,P2,		//投影矩阵p3dC1);		//输出，三角化测量之后特征点的空间坐标		// Step 3 第一关：检查三角化的三维点坐标是否合法（非无穷值）// 只要三角测量的结果中有一个是无穷大的就说明三角化失败，跳过对当前点的处理，进行下一对特征点的遍历 if(!isfinite(p3dC1.at<float>(0)) || !isfinite(p3dC1.at<float>(1)) || !isfinite(p3dC1.at<float>(2))){//其实这里就算是不这样写也没问题，因为默认的匹配点对就不是good点vbGood[vMatches12[i].first]=false;//继续对下一对匹配点的处理continue;}// Check parallax// Step 4 第二关：通过三维点深度值正负、两相机光心视差角大小来检查是否合法 //得到向量PO1cv::Mat normal1 = p3dC1 - O1;//求取模长，其实就是距离float dist1 = cv::norm(normal1);//同理构造向量PO2cv::Mat normal2 = p3dC1 - O2;//求模长float dist2 = cv::norm(normal2);//根据公式：a.*b=|a||b|cos_theta 可以推导出来下面的式子float cosParallax = normal1.dot(normal2)/(dist1*dist2);// Check depth in front of first camera (only if enough parallax, as "infinite" points can easily go to negative depth)// 如果深度值为负值，为非法三维点跳过该匹配点对// ?视差比较小时，重投影误差比较大。这里0.99998 对应的角度为0.36°,这里不应该是 cosParallax>0.99998 吗？// ?因为后面判断vbGood 点时的条件也是 cosParallax<0.99998 // !可能导致初始化不稳定if(p3dC1.at<float>(2)<=0 && cosParallax<0.99998)continue;// Check depth in front of second camera (only if enough parallax, as "infinite" points can easily go to negative depth)// 讲空间点p3dC1变换到第2个相机坐标系下变为p3dC2cv::Mat p3dC2 = R*p3dC1+t;	//判断过程和上面的相同if(p3dC2.at<float>(2)<=0 && cosParallax<0.99998)continue;// Step 5 第三关：计算空间点在参考帧和当前帧上的重投影误差，如果大于阈值则舍弃// Check reprojection error in first image// 计算3D点在第一个图像上的投影误差//投影到参考帧图像上的点的坐标x,yfloat im1x, im1y;//这个使能空间点的z坐标的倒数float invZ1 = 1.0/p3dC1.at<float>(2);//投影到参考帧图像上。因为参考帧下的相机坐标系和世界坐标系重合，因此这里就直接进行投影就可以了im1x = fx*p3dC1.at<float>(0)*invZ1+cx;im1y = fy*p3dC1.at<float>(1)*invZ1+cy;//参考帧上的重投影误差，这个的确就是按照定义来的float squareError1 = (im1x-kp1.pt.x)*(im1x-kp1.pt.x)+(im1y-kp1.pt.y)*(im1y-kp1.pt.y);// 重投影误差太大，跳过淘汰if(squareError1>th2)continue;// Check reprojection error in second image// 计算3D点在第二个图像上的投影误差，计算过程和第一个图像类似float im2x, im2y;// 注意这里的p3dC2已经是第二个相机坐标系下的三维点了float invZ2 = 1.0/p3dC2.at<float>(2);im2x = fx*p3dC2.at<float>(0)*invZ2+cx;im2y = fy*p3dC2.at<float>(1)*invZ2+cy;// 计算重投影误差float squareError2 = (im2x-kp2.pt.x)*(im2x-kp2.pt.x)+(im2y-kp2.pt.y)*(im2y-kp2.pt.y);// 重投影误差太大，跳过淘汰if(squareError2>th2)continue;// Step 6 统计经过检验的3D点个数，记录3D点视差角 // 如果运行到这里就说明当前遍历的这个特征点对靠谱，经过了重重检验，说明是一个合格的点，称之为good点 vCosParallax.push_back(cosParallax);//存储这个三角化测量后的3D点在世界坐标系下的坐标vP3D[vMatches12[i].first] = cv::Point3f(p3dC1.at<float>(0),p3dC1.at<float>(1),p3dC1.at<float>(2));//good点计数++nGood++;//判断视差角，只有视差角稍稍大一丢丢的才会给打good点标记//? bug 我觉得这个写的位置不太对。你的good点计数都++了然后才判断，不是会让good点标志和good点计数不一样吗if(cosParallax<0.99998)vbGood[vMatches12[i].first]=true;}// Step 7 得到3D点中较小的视差角，并且转换成为角度制表示if(nGood>0){// 从小到大排序，注意vCosParallax值越大，视差越小sort(vCosParallax.begin(),vCosParallax.end());// !排序后并没有取最小的视差角，而是取一个较小的视差角// 作者的做法：如果经过检验过后的有效3D点小于50个，那么就取最后那个最小的视差角(cos值最大)// 如果大于50个，就取排名第50个的较小的视差角即可，为了避免3D点太多时出现太小的视差角 size_t idx = min(50,int(vCosParallax.size()-1));//将这个选中的角弧度制转换为角度制parallax = acos(vCosParallax[idx])*180/CV_PI;}else//如果没有good点那么这个就直接设置为0了parallax=0;//返回good点计数return nGood;
}