• OpenCV实现求解单目相机位姿

            单目相机通过对极约束来求解相机运动的位姿。参考了ORBSLAM中单目实现的代码，这里用opencv来实现最简单的位姿估计.

     

    1.  
    2. mLeftImg = cv::imread(lImg, cv::IMREAD_GRAYSCALE);
    3. mRightImg = cv::imread(rImg, cv::IMREAD_GRAYSCALE);
    4. cv::Ptr<ORB> OrbLeftExtractor = cv::ORB::create();
    5. cv::Ptr<ORB> OrbRightExtractor = cv::ORB::create();
    6.  
    7. OrbLeftExtractor->detectAndCompute(mLeftImg, noArray(), mLeftKps, mLeftDes);
    8. OrbRightExtractor->detectAndCompute(mRightImg, noArray(), mRightKps, mRightDes);
    9. Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create(DescriptorMatcher::BRUTEFORCE_HAMMING);
    10. matcher->match(mLeftDes, mRightDes, mMatches);

     首先通过ORB特征提取，获取两幅图像的匹配度，我将其连线出来的效果:

     

    RANSAC的算法原理可以google，很容易理解。先看看ORBSLAM中的实现：

    1. bool Initializer::Initialize(const Frame &CurrentFrame, const vector<int> &vMatches12, cv::Mat &R21, cv::Mat &t21,
    2.                              vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated)
    3. {
    4.     // Fill structures with current keypoints and matches with reference frame
    5.     // Reference Frame: 1, Current Frame: 2
    6.     // Frame2 特征点
    7.     mvKeys2 = CurrentFrame.mvKeysUn;
    8.  
    9.     // mvMatches12记录匹配上的特征点对
    10.     mvMatches12.clear();
    11.     mvMatches12.reserve(mvKeys2.size());
    12.     // mvbMatched1记录每个特征点是否有匹配的特征点，
    13.     // 这个变量后面没有用到，后面只关心匹配上的特征点
    14.     mvbMatched1.resize(mvKeys1.size());
    15.  
    16.     // 步骤1：组织特征点对
    17.     for(size_t i=0, iend=vMatches12.size();i<iend; i++)
    18.     {
    19.         if(vMatches12[i]>=0)
    20.         {
    21.             mvMatches12.push_back(make_pair(i,vMatches12[i]));
    22.             mvbMatched1[i]=true;
    23.         }
    24.         else
    25.             mvbMatched1[i]=false;
    26.     }
    27.  
    28.     // 匹配上的特征点的个数
    29.     const int N = mvMatches12.size();
    30.  
    31.     // Indices for minimum set selection
    32.     // 新建一个容器vAllIndices，生成0到N-1的数作为特征点的索引
    33.     vector<size_t> vAllIndices;
    34.     vAllIndices.reserve(N);
    35.     vector<size_t> vAvailableIndices;
    36.  
    37.     for(int i=0; i<N; i++)
    38.     {
    39.         vAllIndices.push_back(i);
    40.     }
    41.  
    42.     // Generate sets of 8 points for each RANSAC iteration
    43.     // **步骤2：在所有匹配特征点对中随机选择8对匹配特征点为一组，共选择mMaxIterations组 **
    44.      // 用于FindHomography和FindFundamental求解
    45.     // mMaxIterations:200
    46.     mvSets = vector< vector<size_t> >(mMaxIterations,vector<size_t>(8,0));
    47.  
    48.     DUtils::Random::SeedRandOnce(0);
    49.  
    50.     for(int it=0; it<mMaxIterations; it++)
    51.     {
    52.         vAvailableIndices = vAllIndices;
    53.  
    54.         // Select a minimum set
    55.         for(size_t j=0; j<8; j++)
    56.         {
    57.             // 产生0到N-1的随机数
    58.             int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);
    59.             // idx表示哪一个索引对应的特征点被选中
    60.             int idx = vAvailableIndices[randi];
    61.  
    62.             mvSets[it][j] = idx;
    63.  
    64.             // randi对应的索引已经被选过了，从容器中删除
    65.             // randi对应的索引用最后一个元素替换，并删掉最后一个元素
    66.             vAvailableIndices[randi] = vAvailableIndices.back();
    67.             vAvailableIndices.pop_back();
    68.         }
    69.     }
    70.  
    71.     // Launch threads to compute in parallel a fundamental matrix and a homography
    72.     // 步骤3：调用多线程分别用于计算fundamental matrix和homography
    73.     vector<bool> vbMatchesInliersH, vbMatchesInliersF;
    74.     float SH, SF; // score for H and F
    75.     cv::Mat H, F; // H and F
    76.  
    77.     // ref是引用的功能:http://en.cppreference.com/w/cpp/utility/functional/ref
    78.     // 计算homograpy并打分
    79.     thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
    80.     // 计算fundamental matrix并打分
    81.     thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));
    82.  
    83.     // Wait until both threads have finished
    84.     threadH.join();
    85.     threadF.join();
    86.  
    87.     // Compute ratio of scores
    88.     // 步骤4：计算得分比例，选取某个模型
    89.     float RH = SH/(SH+SF);
    90.  
    91.     // Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
    92.     // 步骤5：从H矩阵或F矩阵中恢复R,t
    93.     if(RH>0.40)
    94.         return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
    95.     else //if(pF_HF>0.6)
    96.         return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
    97.  
    98.     return false;
    99. }

     

    orbslam首先是从配对特征中随机迭代mMaxIterations次，每一次都从配对点中选出8个点用来计算homography和fundamental矩阵，都是用SVD来计算的，如下：

    FindFundamental：

    1.  
    2. void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21)
    3. {
    4.     // Number of putative matches
    5.     const int N = vbMatchesInliers.size();
    6.  
    7.     // 分别得到归一化的坐标P1和P2
    8.     vector<cv::Point2f> vPn1, vPn2;
    9.     cv::Mat T1, T2;
    10.     Normalize(mvKeys1,vPn1, T1);
    11.     Normalize(mvKeys2,vPn2, T2);
    12.     cv::Mat T2t = T2.t();
    13.  
    14.     // Best Results variables
    15.     score = 0.0;
    16.     vbMatchesInliers = vector<bool>(N,false);
    17.  
    18.     // Iteration variables
    19.     vector<cv::Point2f> vPn1i(8);
    20.     vector<cv::Point2f> vPn2i(8);
    21.     cv::Mat F21i;
    22.     vector<bool> vbCurrentInliers(N,false);
    23.     float currentScore;
    24.  
    25.     // Perform all RANSAC iterations and save the solution with highest score
    26.     for(int it=0; it<mMaxIterations; it++)
    27.     {
    28.         // Select a minimum set
    29.         for(int j=0; j<8; j++)
    30.         {
    31.             int idx = mvSets[it][j];
    32.  
    33.             vPn1i[j] = vPn1[mvMatches12[idx].first];
    34.             vPn2i[j] = vPn2[mvMatches12[idx].second];
    35.         }
    36.  
    37.         cv::Mat Fn = ComputeF21(vPn1i,vPn2i);
    38.  
    39.         F21i = T2t*Fn*T1;  //解除归一化
    40.  
    41.         // 利用重投影误差为当次RANSAC的结果评分
    42.         currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);
    43.  
    44.         if(currentScore>score)
    45.         {
    46.             F21 = F21i.clone();
    47.             vbMatchesInliers = vbCurrentInliers;
    48.             score = currentScore;
    49.         }
    50.     }
    51. }

    通过ComputeF21计算本质矩阵，

    1. cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1,const vector<cv::Point2f> &vP2)
    2. {
    3.     const int N = vP1.size();
    4.  
    5.     cv::Mat A(N,9,CV_32F); // N*9
    6.  
    7.     for(int i=0; i<N; i++)
    8.     {
    9.         const float u1 = vP1[i].x;
    10.         const float v1 = vP1[i].y;
    11.         const float u2 = vP2[i].x;
    12.         const float v2 = vP2[i].y;
    13.  
    14.         A.at<float>(i,0) = u2*u1;
    15.         A.at<float>(i,1) = u2*v1;
    16.         A.at<float>(i,2) = u2;
    17.         A.at<float>(i,3) = v2*u1;
    18.         A.at<float>(i,4) = v2*v1;
    19.         A.at<float>(i,5) = v2;
    20.         A.at<float>(i,6) = u1;
    21.         A.at<float>(i,7) = v1;
    22.         A.at<float>(i,8) = 1;
    23.     }
    24.  
    25.     cv::Mat u,w,vt;
    26.  
    27.     cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
    28.  
    29.     cv::Mat Fpre = vt.row(8).reshape(0, 3); // v的最后一列
    30.  
    31.     cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
    32.  
    33.     w.at<float>(2)=0; // 秩2约束，将第3个奇异值设为0 //强迫约束
    34.  
    35.     return  u*cv::Mat::diag(w)*vt;
    36. }

     

    看到用的是线性SVD解。

    通过重投影来评估本质矩阵的好坏。

    1.  
    2. float Initializer::CheckFundamental(const cv::Mat &F21, vector<bool> &vbMatchesInliers, float sigma)
    3. {
    4.     const int N = mvMatches12.size();
    5.  
    6.     const float f11 = F21.at<float>(0,0);
    7.     const float f12 = F21.at<float>(0,1);
    8.     const float f13 = F21.at<float>(0,2);
    9.     const float f21 = F21.at<float>(1,0);
    10.     const float f22 = F21.at<float>(1,1);
    11.     const float f23 = F21.at<float>(1,2);
    12.     const float f31 = F21.at<float>(2,0);
    13.     const float f32 = F21.at<float>(2,1);
    14.     const float f33 = F21.at<float>(2,2);
    15.  
    16.     vbMatchesInliers.resize(N);
    17.  
    18.     float score = 0;
    19.  
    20.     // 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
    21.     const float th = 3.841;  //置信度95%,自由度1
    22.     const float thScore = 5.991;//置信度95%,自由度2
    23.  
    24.     const float invSigmaSquare = 1.0/(sigma*sigma);
    25.  
    26.     for(int i=0; i<N; i++)
    27.     {
    28.         bool bIn = true;
    29.  
    30.         const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];
    31.         const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];
    32.  
    33.         const float u1 = kp1.pt.x;
    34.         const float v1 = kp1.pt.y;
    35.         const float u2 = kp2.pt.x;
    36.         const float v2 = kp2.pt.y;
    37.  
    38.         // Reprojection error in second image
    39.         // l2=F21x1=(a2,b2,c2)
    40.         // F21x1可以算出x1在图像中x2对应的线l
    41.         const float a2 = f11*u1+f12*v1+f13;
    42.         const float b2 = f21*u1+f22*v1+f23;
    43.         const float c2 = f31*u1+f32*v1+f33;
    44.  
    45.         // x2应该在l这条线上:x2点乘l = 0 
    46.         const float num2 = a2*u2+b2*v2+c2;
    47.  
    48.         const float squareDist1 = num2*num2/(a2*a2+b2*b2); // 点到线的几何距离 的平方
    49.  
    50.         const float chiSquare1 = squareDist1*invSigmaSquare;
    51.  
    52.         if(chiSquare1>th)
    53.             bIn = false;
    54.         else
    55.             score += thScore - chiSquare1;
    56.  
    57.         // Reprojection error in second image
    58.         // l1 =x2tF21=(a1,b1,c1)
    59.  
    60.         const float a1 = f11*u2+f21*v2+f31;
    61.         const float b1 = f12*u2+f22*v2+f32;
    62.         const float c1 = f13*u2+f23*v2+f33;
    63.  
    64.         const float num1 = a1*u1+b1*v1+c1;
    65.  
    66.         const float squareDist2 = num1*num1/(a1*a1+b1*b1);
    67.  
    68.         const float chiSquare2 = squareDist2*invSigmaSquare;
    69.  
    70.         if(chiSquare2>th)
    71.             bIn = false;
    72.         else
    73.             score += thScore - chiSquare2;
    74.  
    75.         if(bIn)
    76.             vbMatchesInliers[i]=true;
    77.         else
    78.             vbMatchesInliers[i]=false;
    79.     }
    80.  
    81.     return score;
    82. }

    最后回到Initializer::Initialize，将单映矩阵和本质矩阵的得分进行比对，选出最合适的，就求出RT了。

    ORBSLAM2同时考虑了单应和本质，SLAM14讲中也说到，工程实践中一般都讲两者都计算出来选择较好的，不过效率上会影响比较多感觉。

    opencv实现就比较简单了，思路和上面的类似，只是现在只考虑本质矩阵。在之前获取到特征点之后，

    1. /*add ransac method for accurate match*/
    2.           vector<Point2f> vLeftP2f;
    3.           vector<Point2f> vRightP2f;
    4.           for(auto& each:mMatches)
    5.           {
    6.               vLeftP2f.push_back(mLeftKps[each.queryIdx].pt);
    7.               vRightP2f.push_back(mRightKps[each.trainIdx].pt);
    8.           }
    9.           vector<unsigned char> vTemp(vLeftP2f.size());
    10.             /*计算本质矩阵，用RANSAC*/
    11.           Mat transM = findEssentialMat(vLeftP2f, vRightP2f, cameraMatrix,RANSAC, 0.999, 1.0, vTemp);
    12.           vector<DMatch> optimizeM;
    13.           for(int i = 0; i < vTemp.size(); i++)
    14.           {
    15.              if(vTemp[i])
    16.               {
    17.                   optimizeM.push_back(mMatches[i]);
    18.               }
    19.           }
    20.           mMatches.swap(optimizeM);
    21.           cout << transM<<endl;
    22.          Mat optimizeP;
    23.           drawMatches(mLeftImg, mLeftKps, mRightImg, mRightKps, mMatches, optimizeP);
    24.           imshow("output5", optimizeP);

    看下结果：

    图片

     确实效果好多了，匹配准确度比之前的要好，之后我们就可以通过这个本质矩阵来计算RT了。

    1. Mat R, t, mask;
    2. recoverPose(transM, vLeftP2f, vRightP2f, cameraMatrix, R, t, mask);

    一个接口搞定。最后我们可以通过验证对极约束，来看看求出的位姿是否准确。

    定义检查函数：

    1. Mat cameraMatrix = (Mat_<double>(3,3) << CAM_FX, 0.0, CAM_CX, 0.0, CAM_FY, CAM_CY, 0.0, 0.0, 1.0);
    2.  
    3.   bool epipolarConstraintCheck(Mat CameraK, vector<Point2f>& p1s, vector<Point2f>& p2s, Mat R, Mat t)
    4.   {
    5.       for(int i = 0; i < p1s.size(); i++)
    6.       {
    7.           Mat y1 = (Mat_<double>(3,1)<<p1s[i].x, p1s[i].y, 1);
    8.          Mat y2 = (Mat_<double>(3,1)<<p2s[i].x, p2s[i].y, 1);
    9.           //T 的凡对称矩阵
    10.           Mat t_x = (Mat_<double>(3,3)<< 0, -t.at<double>(2,0), t.at<double>(1,0),
    11.                     t.at<double>(2,0), 0, -t.at<double>(0,0),
    12.                     -t.at<double>(1,0),t.at<double>(0,0),0);
    13.           Mat d = y2.t() * cameraMatrix.inv().t() * t_x * R * cameraMatrix.inv()* y1;
    14.           cout<<"epipolar constraint = "<<d<<endl;
    15.       }
    16.   }

    最后可以看到结果都是趋近于0的，证明位姿还是比较准确的。

  • 相关阅读:
    2022-09-16 Android app 让图片在ScrollView里面等比例完整显示不变形，继承ImageView ，对ImageView 进行修改。
    程序员保密协议(对外合作)
    拉格朗日插值法（理论详解）
    苹果官宣新品发布会 10月31日发布会与Mac有关
    ES6汇总
    分析和比较深度学习框架 PyTorch 和 Tensorflow
    python df.apply()函数
    [附源码]java毕业设计社区生鲜仓库管理系统
    RTP相关
    C++基础知识要点--函数(Primer C++ 第五版 · 阅读笔记)
  • 原文地址：https://blog.csdn.net/hulinhulin/article/details/133660027