目录
1.4.2 三角化恢复三维点Initializer::Triangulate
1 Initializer::CheckRT
1.1 函数作用
用位姿来对特征匹配点三角化,从中筛选中合格的三维点。
1.2 构造函数
* @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 ¶llax)
传入参数:
①参考帧到当前帧的旋转矩阵R和平移矩阵t
②参考帧(第一帧)和当前帧(第二帧)的特征点容器vKeys1、vKeys2
③两帧特征点的匹配标记vMatches12以及特征点对内点标记vbMatchesInliers
④相机内参K,重投影误差阈值th2
传出参数:
①三角化测量之后的特征点的空间坐标v3D
②标记成功三角化点vGoog
③返回三角化点的数量
1.3 代码
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 ¶llax) { // 对给出的特征点对及其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, //矩阵的大小是3x4 CV_32F, //数据类型是浮点数 cv::Scalar(0)); //初始的数值是0 //将整个K矩阵拷贝到P1矩阵的左侧3x3矩阵,因为 K*I = K K.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点计数设置为0 int nGood=0; // 开始遍历所有的特征点对 for(size_t i=0, iend=vMatches12.size();i<iend;i++) { // 跳过outliers if(!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; // 利用三角法恢复三维点p3dC1 Triangulate(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 第二关:通过三维点深度值正负、两相机光心视差角大小来检查是否合法 //得到向量PO1 cv::Mat normal1 = p3dC1 - O1; //求取模长,其实就是距离 float dist1 = cv::norm(normal1); //同理构造向量PO2 cv::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个相机坐标系下变为p3dC2 cv::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,y float 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; }
1.4 流程解析
1.4.0 初始化参数
①特征点是否是good点的标记,这里的特征点指的是参考帧中的特征点,将vGood初始化为第一帧特征点的数量,其为bool类型。
②三角化测量之后的特征点的空间坐标v3D初始化大小为第一帧中特征点的数量。
1.4.1 计算初始化两帧的投影矩阵
投影矩阵P是一个 3x4 的矩阵,可以将空间中的一个点投影到平面上,获得其平面坐标,这里均指的是齐次坐标。
由于以第一个相机的光心作为世界坐标系。 其投影矩阵计算推导如下:
我们默认第一个相机的
矩阵为
,因此第一个相机的投影矩阵为
。第一个相机的光心坐标为
。
我们从传入参数可知第一个相机到第二个相机的
变换
,因此再左乘相机内参矩阵
就能得到像素坐标,即世界坐标向像素坐标的投影矩阵为
。
同时计算第二帧的光心坐标在原点(第一帧)的坐标,计算如下图所示:
第二个相机的光心在世界坐标系下的坐标,即我们要求第二个相机的光心在第一个相机坐标系下的坐标。
1.4.2 三角化恢复三维点Initializer::Triangulate
1.数学原理
我们将投影方程进行如下描述:
为方便推导,简单记为:
为了化为齐次方程,左右两边同时叉乘,得到:
利用两对匹配点,得到:
SVD求解,右奇异矩阵的最后一行就是最终的解。
2.代码
/** 给定投影矩阵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 frame const cv::KeyPoint &kp2, //特征点, in current frame const cv::Mat &P1, //投影矩阵P1 const cv::Mat &P2, //投影矩阵P2 cv::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求解,右奇异矩阵的最后一行就是最终的解. //这个就是上面注释中的矩阵A cv::Mat A(4,4,CV_32F); //构造参数矩阵A A.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(); //为了符合其次坐标的形式,使最后一维为1 x3D = x3D.rowRange(0,3)/x3D.at<float>(3); }
至此我们恢复一对匹配特征点的3D点。
1.4.3 遍历所有的特征点对检查三维点是否合适
我们遍历所有匹配的特征点对,如果不是外点,则将匹配好的特征点对传入Initializer::Triangulate函数内得到三维点。我们对三维点进行判断:
①检查三角化的三维点坐标是否合法:防止三角化出的点有一维坐标是无穷。
②通过三维点深度值正负、两相机光心视差角大小来检查是否合法 。即判断如果深度值为负值,为非法三维点跳过该匹配点对。计算视差角,当视差角比较小时,重投影误差比较大。
③计算空间点在参考帧和当前帧上的重投影误差,如果大于阈值则舍弃。计算3D点在第一、二个图像上的投影误差,看是否超过阈值选择是否抛弃。
如果如上满足了,这个3D点可以被留下来,用vCosParallax向量存储合格3D点生成时计算出来的视差,存储这个三角化测量后的3D点在世界坐标系下的坐标。并将这两帧累计成功初始化的3D点nGood累加。
vP3D[vMatches12[i].first] = cv::Point3f(p3dC1.at<float>(0),p3dC1.at<float>(1),p3dC1.at<float>(2));
1.4.4 最后处理
如果我们的变量nGood大于0(成功三角化得到的3D点的数目),我们将视差角从小到大排序,排序后并没有取最小的视差角,而是取一个较小的视差角。如果经过检验过后的有效3D点小于50个,那么就取最后那个最小的视差角(cos值最大),如果大于50个,就取排名第50个的较小的视差角即可,为了避免3D点太多时出现太小的视差角。将这个选中的角弧度制转换为角度制输出。