视觉里程计与三维重建-CSDN博客

本文链接：https://blog.csdn.net/weixin_40107913/article/details/108063116

本文探讨了视觉里程计中的关键点匹配与三维点云重建技术。详细解析了如何通过匹配点计算视差角，使用三角测量恢复空间点，以及如何判断关键帧之间的距离是否足够短，以确保三维重建的准确性。此外，还介绍了关键帧剔除策略，以减少冗余信息，提高系统效率。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

1、从一堆数字中找出最好和次好的描述子

     int bestDist=256;
      int bestLevel= -1;
      int bestDist2=256;
      int bestLevel2 = -1;
      int bestIdx =-1 ;
      if(dist<bestDist)
      {
              bestDist2=bestDist;
              bestDist=dist;
              bestLevel2 = bestLevel;
              bestLevel = F.mvKeysUn[idx].octave;
              bestIdx=idx;
      }
      else if(dist<bestDist2)
     {
              bestLevel2 = F.mvKeysUn[idx].octave;
              bestDist2=dist;
     }

2、vector 数组的大小

vector<int> rotHist[30];

表示rotHist是vector数组,每一个vector是个class,因此是个数组class

3、三角测量求空间点

这里讲清楚哪来的https://blog.csdn.net/weixin_43795395/article/details/93769148
如何使用SVDhttps://www.cnblogs.com/yepeichu/p/10792899.html
![在这里插入图片描述](https://img-blog.csdnimg.cn/20200824103924945.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80MDEwNzkxMw==,size_16,color_FFFFFF,t_70#pic_center

4、如何判断关键帧之间的距离足够短

const int &idx1 = vMatchedIndices[ikp].first;
            
            // 当前匹配对在邻接关键帧中的索引
            const int &idx2 = vMatchedIndices[ikp].second;

            // 当前匹配在当前关键帧中的特征点
            const cv::KeyPoint &kp1 = mpCurrentKeyFrame->mvKeysUn[idx1];
            // mvuRight中存放右目的横坐标，如果不是双目，其值将为-1
            const float kp1_ur=mpCurrentKeyFrame->mvuRight[idx1];
            bool bStereo1 = kp1_ur>=0;

            // 当前匹配在邻接关键帧中的特征点
            const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2];
            // mvuRight中存放着右目的横坐标，如果不是双目，其值将为-1
            const float kp2_ur = pKF2->mvuRight[idx2];
            bool bStereo2 = kp2_ur>=0;

            // Check parallax between rays
            // step 6.2：利用匹配点反投影得到视差角
            // 特征点反投影,其实得到的是在各自相机坐标系下的一个非归一化的方向向量,和这个点的反投影射线重合
            cv::Mat xn1 = (cv::Mat_<float>(3,1) << (kp1.pt.x-cx1)*invfx1, (kp1.pt.y-cy1)*invfy1, 1.0);
            cv::Mat xn2 = (cv::Mat_<float>(3,1) << (kp2.pt.x-cx2)*invfx2, (kp2.pt.y-cy2)*invfy2, 1.0);

            // 由相机坐标系转到世界坐标系(得到的是那条反投影射线的一个同向向量在世界坐标系下的表示,还是只能够表示方向)，得到视差角余弦值
            cv::Mat ray1 = Rwc1*xn1;
            cv::Mat ray2 = Rwc2*xn2;
            // 这个就是求向量之间角度公式
            const float cosParallaxRays = ray1.dot(ray2)/(cv::norm(ray1)*cv::norm(ray2));

            // 加1是为了让cosParallaxStereo随便初始化为一个很大的值;不懂的话先向下看就好了
            float cosParallaxStereo = cosParallaxRays+1;  //一般相邻的关键帧应该不会有这么大的视角吧,90°多,不过感觉这个也不好说啊;的
            float cosParallaxStereo1 = cosParallaxStereo;  //不过下面的程序中认为如果视差角的余弦值大于1是不正常
            float cosParallaxStereo2 = cosParallaxStereo;

            // step 6.3：对于双目，利用双目得到视差角
            if(bStereo1)//传感器是双目相机,并且当前的关键帧的这个点有对应的深度
                // 其实就是利用双目成像的原理,计算出双目相机两个相机观察这个点的时候的视差角;画个图就一目了然了
                // 不过无论是这个公式还是下面的公式， 都只能计算“等腰三角形”情况下的视角，所以不一定是准确的
                // ? 感觉直接使用向量夹角的方式计算会准确一些啊（双目的时候），那么为什么不直接使用那个呢？
                cosParallaxStereo1 = cos(2*atan2(mpCurrentKeyFrame->mb/2,mpCurrentKeyFrame->mvDepth[idx1]));
            else if(bStereo2)//传感器是双目相机,并且邻接的关键帧的这个点有对应的深度
                cosParallaxStereo2 = cos(2*atan2(pKF2->mb/2,pKF2->mvDepth[idx2]));
            
            // 如果是单目相机,那么就没有啥操作

            // 得到双目观测的视差角
            cosParallaxStereo = min(cosParallaxStereo1,cosParallaxStereo2);

            // step 6.4：三角化恢复3D点
            cv::Mat x3D;
            // cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998)表明视差角正常
            // cosParallaxRays < cosParallaxStereo 表明视差角很小
            // 视差角度小时用三角法恢复3D点，视差角大时用双目恢复3D点（双目以及深度有效）
            if(cosParallaxRays<cosParallaxStereo && cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998))
            {
                // Linear Triangulation Method
                // 见Initializer.cpp的 Triangulate 函数,实现是一毛一样的,顶多就是把投影矩阵换成了变换矩阵
                cv::Mat A(4,4,CV_32F);
                A.row(0) = xn1.at<float>(0)*Tcw1.row(2)-Tcw1.row(0);
                A.row(1) = xn1.at<float>(1)*Tcw1.row(2)-Tcw1.row(1);
                A.row(2) = xn2.at<float>(0)*Tcw2.row(2)-Tcw2.row(0);
                A.row(3) = xn2.at<float>(1)*Tcw2.row(2)-Tcw2.row(1);

                cv::Mat w,u,vt;
                cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);

                x3D = vt.row(3).t();
                // 归一化之前的检查
                if(x3D.at<float>(3)==0)
                    continue;
                // Euclidean coordinates 归一化成为齐次坐标,然后提取前面三个维度作为欧式坐标
                x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
            }
            else if(bStereo1 && cosParallaxStereo1<cosParallaxStereo2)  // 视差大的时候使用双目信息来恢复 - 直接反投影了
            {
                x3D = mpCurrentKeyFrame->UnprojectStereo(idx1);                
            }
            else if(bStereo2 && cosParallaxStereo2<cosParallaxStereo1)  // 同上
            {
                x3D = pKF2->UnprojectStereo(idx2);
            }
            else
                continue; //No stereo and very low parallax, 放弃

5、删除冗余关键帧

// 关键帧剔除,在Covisibility Graph中的关键帧，其90%以上的MapPoints能被其他关键帧（至少3个）观测到，则认为该关键帧为冗余关键帧。
void LocalMapping::KeyFrameCulling()
{
    // Check redundant keyframes (only local keyframes)
    // A keyframe is considered redundant if the 90% of the MapPoints it sees, are seen
    // in at least other 3 keyframes (in the same or finer scale)
    // We only consider close stereo points

    // STEP1：根据Covisibility Graph提取当前帧的共视关键帧 (所有)
    vector<KeyFrame*> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();

    // 对所有的局部关键帧进行遍历 ; 这里的局部关键帧就理解为当前关键帧的共视帧
    for(vector<KeyFrame*>::iterator vit=vpLocalKeyFrames.begin(), vend=vpLocalKeyFrames.end(); vit!=vend; vit++)
    {
        KeyFrame* pKF = *vit;
        if(pKF->mnId==0)
            continue;
        // STEP2：提取每个共视关键帧的MapPoints
        const vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();

        int nObs = 3;                       // 接下来程序中使用到的循环变量,记录了某个点的被观测次数 (其实这里初始化是没有意义的)
        const int thObs=nObs;               // 观测阈值,默认为3
        int nRedundantObservations=0;       // 冗余观测地图点的计数器
                                            // 不是说我从地图点中得到了其观测数据我就信了,这里还要求这个地图点在当前关键帧和在邻接关键帧中的特征尺度变化不太大才可以
                                            // 认为这个地图点被"冗余观测"了
        int nMPs=0;                         // 计数器,参与到检测的地图点的总数目

        // STEP3：遍历该局部关键帧的MapPoints，判断是否90%以上的MapPoints能被其它关键帧（至少3个）观测到
        for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
        {
            MapPoint* pMP = vpMapPoints[i];
            if(pMP)
            {
                if(!pMP->isBad())
                {
                    if(!mbMonocular)
                    {
                        // 对于双目，仅考虑近处的MapPoints，不超过mbf * 35 / fx 
                        // 不过配置文件中给的是近点的定义是 40 * fx
                        if(pKF->mvDepth[i]>pKF->mThDepth || pKF->mvDepth[i]<0)
                            continue;
                    }

                    nMPs++;
                    // MapPoints至少被三个关键帧观测到
                    if(pMP->Observations()>thObs)
                    {
                        const int &scaleLevel = pKF->mvKeysUn[i].octave;
                        const map<KeyFrame*, size_t> observations = pMP->GetObservations();
                        // 判断该MapPoint是否同时被三个关键帧观测到
                        int nObs=0;
                        // 遍历当前这个邻接关键帧上的地图点的所有观测信息
                        for(map<KeyFrame*, size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
                        {
                            KeyFrame* pKFi = mit->first;
                            if(pKFi==pKF)
                                continue;
                            const int &scaleLeveli = pKFi->mvKeysUn[mit->second].octave;

                            // Scale Condition 
                            // 尺度约束，要求MapPoint在该局部关键帧的特征尺度大于（或近似于）其它关键帧的特征尺度
                            if(scaleLeveli<=scaleLevel+1)
                            {
                                nObs++;
                                // 已经找到三个同尺度的关键帧可以观测到该MapPoint，不用继续找了
                                if(nObs>=thObs)
                                    break;
                            }
                        }
                        // 该MapPoint至少被三个关键帧观测到
                        if(nObs>=thObs)
                        {
                            nRedundantObservations++;
                        }
                    }
                }
            }
        }

        // STEP4：该局部关键帧90%以上的MapPoints能被其它关键帧（至少3个）观测到，则认为是冗余关键帧
        if(nRedundantObservations>0.9*nMPs)
            // 剔除的时候就设置一个 bad flag 就可以了
            pKF->SetBadFlag();
    }
}