ORBSLAM2之单目初始化(3)

最新推荐文章于 2023-06-12 09:40:44 发布

windistance

最新推荐文章于 2023-06-12 09:40:44 发布

阅读量959

点赞数 1

分类专栏： c++ 计算机视觉 opencv SLAM

本文链接：https://blog.csdn.net/weixin_42587961/article/details/97391700

版权

c++ 同时被 3 个专栏收录

19 篇文章 13 订阅

订阅专栏

计算机视觉

14 篇文章 2 订阅

订阅专栏

opencv

12 篇文章 1 订阅

订阅专栏

文章目录

第三阶段：三角化测量得到初始的特征点深度

在这里插入图片描述

如果恢复相机初始位姿成功，那么我们能得到前两帧图像的位姿，以及三角测量后的3维地图点，这些都是在Initialize（）函数里面完成的。

 if(mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated))
  {
// 步骤6：删除那些无法进行三角化的匹配点
            for(size_t i=0, iend=mvIniMatches.size(); i<iend;i++)
            {
                if(mvIniMatches[i]>=0 && !vbTriangulated[i])
                {
                    mvIniMatches[i]=-1;
                    nmatches--;
                }
            }

            // Set Frame Poses
            // 将初始化的第一帧作为世界坐标系，因此第一帧变换矩阵为单位矩阵
            mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
            // 由Rcw和tcw构造Tcw,并赋值给mTcw，mTcw为世界坐标系到该帧的变换矩阵
            cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F);
            Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3));
            tcw.copyTo(Tcw.rowRange(0,3).col(3));
            mCurrentFrame.SetPose(Tcw);
     
 ...

先将当前帧位姿Tcw设置好，之后第四阶段CreateInitialMapMonocular () 创建初始地图，并进行相应的3D点数据关联操作。

三角化的程序：

ORB_SLAM2/src/Initializer.cpp--void Initializer::Triangulate()
...

//给定投影矩阵P1,P2和图像上的点kp1,kp2，从而恢复3D坐标
void Initializer::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D)
{
    // 在DecomposeE函数和ReconstructH函数中对t有归一化
    // 这里三角化过程中恢复的3D点深度取决于 t 的尺度，
    // 但是这里恢复的3D点并没有决定单目整个SLAM过程的尺度
    // 因为CreateInitialMapMonocular函数对3D点深度会缩放，然后反过来对 t 有改变

    cv::Mat A(4,4,CV_32F);

    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;
    cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
    x3D = vt.row(3).t();
    x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
}

第四阶段: 将三角化得到的3D点包装成MapPoints,然后做 BA

MapPoints类的主要目的是对环境中真实存在的点进行描述,因此不仅要记录点的信息，还有点的观测信息等，主要包含的成员变量：

// Position in absolute coordinates
cv::Mat mWorldPos; ///< MapPoint在世界坐标系下的坐标

// Keyframes observing the point and associated index in keyframe
std::map<KeyFrame*,size_t> mObservations; ///< 观测到该MapPoint的KF和该MapPoint在KF中的索引

// Mean viewing direction
// 该MapPoint平均观测方向
cv::Mat mNormalVector;

// Best descriptor to fast matching
// 每个3D点也有一个descriptor
// 如果MapPoint与很多帧图像特征点对应（由keyframe来构造时），那么距离其它描述子的平均距离最小的描述子是最佳描述子
// MapPoint只与一帧的图像特征点对应（由frame来构造时），那么这个特征点的描述子就是该3D点的描述子
cv::Mat mDescriptor; ///< 通过 ComputeDistinctiveDescriptors() 得到的最优描述子

// Reference KeyFrame
KeyFrame* mpRefKF;

// Tracking counters
int mnVisible; //理论上该地图点被观测到的次数
int mnFound;//实际上该地图被观测到的次数

1.创建并插入关键帧

因为只有前两帧，所以将这两帧都设置为关键帧（KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB);），并计算对应的BoW（pKFini->ComputeBoW();），并在地图中插入这两个关键帧（mpMap->AddKeyFrame；）。这个两个关键帧会被 mspKeyFrames (std::set<KeyFrame*> mspKeyFrames;)这个集合记录，并且会更新记录此时关键帧的id（mnMaxKFid）。

2.创建地图点，并且关联到关键帧

将3D点包装成地图点，将地图点与关键帧，地图进行数据关联。

地图点的构建需要三个参数：Pos（ MapPoint的坐标（wrt世界坐标系））、pRefKF（关键帧）、pMap （Map），调用MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpMap);生成新的地图点。

地图点与关键帧关联

一个地图点可被多个关键帧观测到，将观测到这个地图点的关键帧与这个地图点进行关联，同时记录关键帧上哪一个特征点与这个地图点有关联。

void KeyFrame::AddMapPoint(MapPoint *pMP, const size_t &idx)

函数，将地图点关联到关键帧，记录在关键帧的mvpMapPoints中（std::vector<MapPoint*> mvpMapPoints;），此时还没有加入到map

void MapPoint::AddObservation(KeyFrame* pKF, size_t idx)

函数记录KeyFrame的那个特征点能观测到该MapPoint ，这些信息被记录在地图点上mObservations（ std::map<KeyFrame*,size_t> mObservations;），并增加观测的相机数目nObs，单目+1，双目或者grbd+2，这个函数是建立关键帧共视关系的核心函数，能共同观测到某些MapPoints的关键帧是共视关键帧

void MapPoint::ComputeDistinctiveDescriptors()计算具有代表的描述子
由于一个MapPoint会被许多相机观测到，因此在插入关键帧后，需要判断是否更新当前点的最适合的描述子先获得当前点的所有描述子，然后计算描述子之间的两两距离，最好的描述子与其他描述子应该具有最小的距离中值

void MapPoint::UpdateNormalAndDepth()更新平均观测方向以及观测距离范围
因此在插入关键帧后，需要更新相应变量（获得观测到该MapPoint的所有关键帧，观测到该点的参考关键帧，MapPoint在世界坐标系中的位置，观测到该MapPoint的平均观测方向以及距离范围）

跟踪并且更新当前的帧信息：

ORB_SLAM2/src/Tracking.cpp-成员变量
...
    
Frame mCurrentFrame;

ORB_SLAM2/src/Tracking.cpp--void Tracking::CreateInitialMapMonocular()
...
    
mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;
mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;

将地图点加入到地图中

调用成员函数（mpMap->AddMapPoint），将地图点加入到地图的mspMapPoints中（std::set<MapPoint*> mspMapPoints;）

ORB_SLAM2/src/Map.cpp--void Map::AddMapPoint()
...
    
void Map::AddMapPoint(MapPoint *pMP)
{
    unique_lock<mutex> lock(mMutexMap);
    mspMapPoints.insert(pMP);
}

3.关键帧与关键帧关联

void KeyFrame::UpdateConnections()

在没有执行这个函数前，关键帧只和MapPoints之间有连接关系，这个函数可以更新关键帧之间的连接关系

首先获得该关键帧的所有MapPoint点，统计观测到这些3d点的每个关键与其它所有关键帧之间的共视程度

对每一个找到的关键帧，建立一条边，边的权重是该关键帧与当前关键帧公共3d点的个数。并且该权重必须大于一个阈值，如果没有超过该阈值的权重，那么就只保留权重最大的边（与其它关键帧的共视程度比较高）对这些连接按照权重从大到小进行排序，以方便将来的处理更新完covisibility图之后，如果没有初始化过，则初始化为连接权重最大的边（与其它关键帧共视程度最高的那个关键帧），类似于最大生成树

4. 执行BA优化

ORB_SLAM2/src/Tracking.cpp--成员变量
...

Map* mpMap;

将跟踪的地图进行BA优化

 Optimizer::GlobalBundleAdjustemnt(mpMap,20);

5.尺度归一化

由于单目SLAM的尺度不确定性，也就是场景的绝对尺度具有不可观性。因此将场景地图深度中值作为单位一，归一化第二帧的位姿与所有的地图点。最后需要更新局部地图，局部关键帧和局部地图点，并且更新上一帧，上一关键帧，参考关键帧等变量。

所有流程程序：

ORB_SLAM2/src/Tracking.cpp--void Tracking::CreateInitialMapMonocular()
...

/**
 * @brief CreateInitialMapMonocular
 *
 * 为单目摄像头三角化生成MapPoints
 */
void Tracking::CreateInitialMapMonocular()
{
    /*
    -----------1.Create KeyFrames 创建关键帧------------
    */
   
    KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB);
    KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);

    // 将初始关键帧的描述子转为BoW
    pKFini->ComputeBoW();
    // 将当前关键帧的描述子转为BoW
    pKFcur->ComputeBoW();

    // Insert KFs in the map
    // 将关键帧插入到地图
    // 凡是关键帧，都要插入地图
    mpMap->AddKeyFrame(pKFini);
    mpMap->AddKeyFrame(pKFcur);

     /*
    -----------2.Create MapPoints 创建地图点，并关联关键帧------------
    */
    
    // Create MapPoints and asscoiate to keyframes
    for(size_t i=0; i<mvIniMatches.size();i++)
    {
        if(mvIniMatches[i]<0)
            continue;

        //Create MapPoint.
        cv::Mat worldPos(mvIniP3D[i]);

        // 用3D点构造MapPoint
        MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpMap);

        // 为该MapPoint添加属性：
        // a.观测到该MapPoint的关键帧
        // b.该MapPoint的描述子
        // c.该MapPoint的平均观测方向和深度范围

        // 表示该KeyFrame的哪个特征点可以观测到哪个3D点
        pKFini->AddMapPoint(pMP,i);
        pKFcur->AddMapPoint(pMP,mvIniMatches[i]);

        // a.表示该MapPoint可以被哪个KeyFrame的哪个特征点观测到
        pMP->AddObservation(pKFini,i);
        pMP->AddObservation(pKFcur,mvIniMatches[i]);

        // b.从众多观测到该MapPoint的特征点中挑选区分读最高的描述子
        pMP->ComputeDistinctiveDescriptors();
        
        // c.更新该MapPoint平均观测方向以及观测距离的范围
        pMP->UpdateNormalAndDepth();

        //Fill Current Frame structure
        mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;
        mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;
        
     /*
    -----------3.Add to Map 在地图中添加该MapPoint------------
    */
        
        //Add to Map
        //在地图中添加该MapPoint
        mpMap->AddMapPoint(pMP);
    }

    
     /*
    -----------4.Update Connections and Bundle Adjustment -----------
    */
        
    // Update Connections
    // 更新关键帧间的连接关系
    // 在3D点和关键帧之间建立边，每个边有一个权重，边的权重是该关键帧与当前帧公共3D点的个数
    pKFini->UpdateConnections();
    pKFcur->UpdateConnections();

    // Bundle Adjustment
    cout << "New Map created with " << mpMap->MapPointsInMap() << " points" << endl;

    //BA优化
    Optimizer::GlobalBundleAdjustemnt(mpMap,20);

    /*
    -----------5.尺度归一化到1 -----------
    */
    
    // Set median depth to 1
    // 将MapPoints的中值深度归一化到1，并归一化两帧之间变换
    // 评估关键帧场景深度，q=2表示中值
    float medianDepth = pKFini->ComputeSceneMedianDepth(2);
    float invMedianDepth = 1.0f/medianDepth;

    if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<100)
    {
        cout << "Wrong initialization, reseting..." << endl;
        Reset();
        return;
    }

    // Scale initial baseline
    cv::Mat Tc2w = pKFcur->GetPose();
    // x/z y/z 将z归一化到1 
    Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth;
    pKFcur->SetPose(Tc2w);

    // Scale points
    // 把3D点的尺度也归一化到1
    vector<MapPoint*> vpAllMapPoints = pKFini->GetMapPointMatches();
    for(size_t iMP=0; iMP<vpAllMapPoints.size(); iMP++)
    {
        if(vpAllMapPoints[iMP])
        {
            MapPoint* pMP = vpAllMapPoints[iMP];
            pMP->SetWorldPos(pMP->GetWorldPos()*invMedianDepth);
        }
    }

    // 这部分和SteroInitialization()相似
    mpLocalMapper->InsertKeyFrame(pKFini);
    mpLocalMapper->InsertKeyFrame(pKFcur);

    mCurrentFrame.SetPose(pKFcur->GetPose());
    mnLastKeyFrameId=mCurrentFrame.mnId;
    mpLastKeyFrame = pKFcur;

    mvpLocalKeyFrames.push_back(pKFcur);
    mvpLocalKeyFrames.push_back(pKFini);
    mvpLocalMapPoints=mpMap->GetAllMapPoints();
    mpReferenceKF = pKFcur;
    mCurrentFrame.mpReferenceKF = pKFcur;

    mLastFrame = Frame(mCurrentFrame);

    mpMap->SetReferenceMapPoints(mvpLocalMapPoints);

    mpMapDrawer->SetCurrentCameraPose(pKFcur->GetPose());

    mpMap->mvpKeyFrameOrigins.push_back(pKFini);

    mState=OK;// 初始化成功，至此，初始化过程完成
}

参考

https://www.zhihu.com/question/50385799/answer/120902345

https://blog.csdn.net/zhubaohua_bupt/article/details/78560966

https://blog.csdn.net/u011380574/article/details/77802355

ORB-SLAM: A Versatile and Accurate Monocular SLAM System

http://webdiis.unizar.es/~raulmur/orbslam/

https://github.com/raulmur/ORB_SLAM2

https://en.cppreference.com/w/cpp/algorithm/fill

ORB-SLAM2源码中文注释

ORB-SLAM2源码中文详解

《计算机视觉中的多视图几何》

windistance

关注

1
点赞
踩
2

收藏

觉得还不错? 一键收藏
1
评论
ORBSLAM2之单目初始化(3)

文章目录第三阶段：三角化测量得到初始的特征点深度第四阶段: 将三角化得到的3D点包装成MapPoints,然后做 BA1.创建并插入关键帧2.创建地图点，并且关联到关键帧3.关键帧与关键帧关联4. 执行BA优化5.尺度归一化参考第三阶段：三角化测量得到初始的特征点深度如果恢复相机初始位姿成功，那么我们能得到前两帧图像的位姿，以及三角测量后的3维地图点，这些都是在Initialize（）函数...
复制链接

扫一扫