ORB-SLAM2 ---- Tracking::CreateInitialMapMonocular函数

目录

1.函数作用

2.函数解析

2.1 调用函数解析

2.2  Tracking::CreateInitialMapMonocular函数总体思路

2.2.1 代码

2.2.2 总体思路解析 

2.3 MapPoint::ComputeDistinctiveDescriptors函数解析

2.3.1 函数作用

2.3.2 代码 

2.3.3 函数解析 

2.4 MapPoint::UpdateNormalAndDepth函数解析 

2.4.1 函数作用

2.4.2 代码 

2.4.3 函数解析  

---

1.函数作用

        单目相机成功初始化后用三角化得到的点生成MapPoints

2.函数解析

2.1 调用函数解析

        单目地图初始化Tracking::MonocularInitialization函数中调用本函数，这是初始化单目SLAM的最后一步，我们在之前已经完成了单目初始化器的创建、匹配了两帧的特征点、恢复了两帧的运动位姿、用三角化生成了地图点，接下来我们要调用此函数利用生成的3D点实现生成地图点。

2.2  Tracking::CreateInitialMapMonocular函数总体思路

2.2.1 代码

/**
 * @brief 单目相机成功初始化后用三角化得到的点生成MapPoints
 * 
 */
void Tracking::CreateInitialMapMonocular()
{
    // Create KeyFrames 认为单目初始化时候的参考帧和当前帧都是关键帧
    KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB);  // 第一帧
    KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);  // 第二帧

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

    // Insert KFs in the map
    // Step 2 将关键帧插入到地图
    mpMap->AddKeyFrame(pKFini);
    mpMap->AddKeyFrame(pKFcur);

    // Create MapPoints and asscoiate to keyframes
    // Step 3 用初始化得到的3D点来生成地图点MapPoints
    //  mvIniMatches[i] 表示初始化两帧特征点匹配关系。
    //  具体解释：i表示帧1中关键点的索引值，vMatches12[i]的值为帧2的关键点索引值,没有匹配关系的话，vMatches12[i]值为 -1
    for(size_t i=0; iAddMapPoint(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
        //mvIniMatches下标i表示在初始化参考帧中的特征点的序号
        //mvIniMatches[i]是初始化当前帧中的特征点的序号
        mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;
        mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;

        //Add to Map
        mpMap->AddMapPoint(pMP);
    }

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

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

    // Step 4 全局BA优化，同时优化所有位姿和三维点
    Optimizer::GlobalBundleAdjustemnt(mpMap,20);

    // Set median depth to 1
    // Step 5 取场景的中值深度，用于尺度归一化 
    // 为什么是 pKFini 而不是 pKCur ? 答：都可以的，内部做了位姿变换了
    float medianDepth = pKFini->ComputeSceneMedianDepth(2);
    float invMedianDepth = 1.0f/medianDepth;
    
    //两个条件,一个是平均深度要大于0,另外一个是在当前帧中被观测到的地图点的数目应该大于100
    if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<100)
    {
        cout << "Wrong initialization, reseting..." << endl;
        Reset();
        return;
    }

    // Step 6 将两帧之间的变换归一化到平均深度1的尺度下
    // 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
    // Step 7 把3D点的尺度也归一化到1
    // 为什么是pKFini? 是不是就算是使用 pKFcur 得到的结果也是相同的? 答：是的，因为是同样的三维点
    vector vpAllMapPoints = pKFini->GetMapPointMatches();
    for(size_t iMP=0; iMPSetWorldPos(pMP->GetWorldPos()*invMedianDepth);
        }
    }

    //  Step 8 将关键帧插入局部地图，更新归一化后的位姿、局部地图点
    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;// 初始化成功，至此，初始化过程完成
}

2.2.2 总体思路解析 

        我们认为单目初始化时候的参考帧和当前帧都是关键帧

        ①我们对单目初始化的第一帧和第二帧构造成关键帧pKFini 、pKFcur，并计算他们的Bow向量（BowVector、FeatureVector）

        ②将关键帧pKFini 、pKFcur插入到地图mnmap中。

        ③用初始化得到的3D点来生成地图点MapPoints，包括以下步骤：对跟踪初始化时前两帧之间的匹配mvIniMatches进行遍历。

        若没有匹配，则跳过。有匹配的情况对应于mvIniMatches[i] = j

        对有匹配点构造的世界坐标点构造地图点

MapPoint* pMP = new MapPoint(worldPos,pKFcur, mpMap);

        对第一帧第二帧分别添加构造的地图点pMp（让帧能看到该地图点）

pKFini->AddMapPoint(pMP,i);
pKFcur->AddMapPoint(pMP,mvIniMatches[i]);

        该MapPoint可以被哪个KeyFrame的哪个特征点观测到（让地图点能看见帧）

pMP->AddObservation(pKFini,i);
pMP->AddObservation(pKFcur,mvIniMatches[i]);

        从众多观测到该MapPoint的特征点中挑选最有代表性的描述子

pMP->ComputeDistinctiveDescriptors();

        更新该MapPoint平均观测方向以及观测距离的范围

pMP->UpdateNormalAndDepth();

        更新每个特征点对应的MapPoint，如果特征点没有对应的地图点，那么将存储一个空指针。

mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;

        将该特征点属于外点的特征点标记置为false

 mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;

        将地图能看见地图点

mpMap->AddMapPoint(pMP);

        ④更新关键帧间的连接关系，在3D点和关键帧之间建立边，每个边有一个权重，边的权重是该关键帧与当前帧公共3D点的个数。

        ⑤全局BA优化，同时优化所有位姿和三维点

        ⑥取场景的中值深度，用于尺度归一化 

        ⑦把3D点的尺度也归一化到1

        ⑧将关键帧插入局部地图，更新归一化后的位姿、局部地图点

        ⑨置位单目初始化成功状态state位OK。

2.3 MapPoint::ComputeDistinctiveDescriptors函数解析

2.3.1 函数作用

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

2.3.2 代码 

void MapPoint::ComputeDistinctiveDescriptors()
{
    // Retrieve all observed descriptors
    vector vDescriptors;

    map observations;

    // Step 1 获取该地图点所有有效的观测关键帧信息
    {
        unique_lock lock1(mMutexFeatures);
        if(mbBad)
            return;
        observations=mObservations;
    }

    if(observations.empty())
        return;

    vDescriptors.reserve(observations.size());

    // Step 2 遍历观测到该地图点的所有关键帧，对应的orb描述子，放到向量vDescriptors中
    for(map::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    {
        // mit->first取观测到该地图点的关键帧
        // mit->second取该地图点在关键帧中的索引
        KeyFrame* pKF = mit->first;

        if(!pKF->isBad())        
            // 取对应的描述子向量                                               
            vDescriptors.push_back(pKF->mDescriptors.row(mit->second));     
    }

    if(vDescriptors.empty())
        return;

    // Compute distances between them
    // Step 3 计算这些描述子两两之间的距离
    // N表示为一共多少个描述子
    const size_t N = vDescriptors.size();
	
    // 将Distances表述成一个对称的矩阵
    // float Distances[N][N];
	std::vector > Distances;
	Distances.resize(N, vector(N, 0));
	for (size_t i = 0; i vDists(Distances[i],Distances[i]+N);
		vector vDists(Distances[i].begin(), Distances[i].end());
		sort(vDists.begin(), vDists.end());

        // 获得中值
        int median = vDists[0.5*(N-1)];
        
        // 寻找最小的中值
        if(median lock(mMutexFeatures);
        mDescriptor = vDescriptors[BestIdx].clone();       
    }
}

2.3.3 函数解析 

        mObservations是一个map型向量，第一维取观测到该地图点的关键帧，第二维是该地图点在关键帧中的索引。

        我们遍历观测到该地图点的所有关键帧，对应的orb描述子，放到向量vDescriptors中。

        接着我们构造一个的矩阵Distances，维度是一共有多少个描述子即有多少关键帧能观测到当前地图点。Distances[i][j]与Distances[j][i]表示第i个描述子与第j个描述子的距离。

        我们对这个矩阵的每行进行遍历，即第个描述子与其他描述子的汉明距离，我们将它排序取中值，遍历完次后我们得到了一个描述子，这个描述子距离其他描述子有最小的距离中值，我们记录这个中值值为BestMedian，这个中值对应的索引为BestIdx。

        这个索引对应的描述子就是我们选择的这个地图点最具有代表性的地图点。

2.4 MapPoint::UpdateNormalAndDepth函数解析 

2.4.1 函数作用

        更新地图点的平均观测方向、观测距离范围。

2.4.2 代码 

void MapPoint::UpdateNormalAndDepth()
{
    // Step 1 获得观测到该地图点的所有关键帧、坐标等信息
    map observations;
    KeyFrame* pRefKF;
    cv::Mat Pos;
    {
        unique_lock lock1(mMutexFeatures);
        unique_lock lock2(mMutexPos);
        if(mbBad)
            return;

        observations=mObservations; // 获得观测到该地图点的所有关键帧
        pRefKF=mpRefKF;             // 观测到该点的参考关键帧（第一次创建时的关键帧）
        Pos = mWorldPos.clone();    // 地图点在世界坐标系中的位置
    }

    if(observations.empty())
        return;

    // Step 2 计算该地图点的平均观测方向
    // 能观测到该地图点的所有关键帧，对该点的观测方向归一化为单位向量，然后进行求和得到该地图点的朝向
    // 初始值为0向量，累加为归一化向量，最后除以总数n
    cv::Mat normal = cv::Mat::zeros(3,1,CV_32F);
    int n=0;
    for(map::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    {
        KeyFrame* pKF = mit->first;
        cv::Mat Owi = pKF->GetCameraCenter();
        // 获得地图点和观测到它关键帧的向量并归一化
        cv::Mat normali = mWorldPos - Owi;
        normal = normal + normali/cv::norm(normali);                       
        n++;
    } 

    cv::Mat PC = Pos - pRefKF->GetCameraCenter();                           // 参考关键帧相机指向地图点的向量（在世界坐标系下的表示）
    const float dist = cv::norm(PC);                                        // 该点到参考关键帧相机的距离
    const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;        // 观测到该地图点的当前帧的特征点在金字塔的第几层
    const float levelScaleFactor =  pRefKF->mvScaleFactors[level];          // 当前金字塔层对应的尺度因子，scale^n，scale=1.2，n为层数
    const int nLevels = pRefKF->mnScaleLevels;                              // 金字塔总层数，默认为8

    {
        unique_lock lock3(mMutexPos);
        // 使用方法见PredictScale函数前的注释
        mfMaxDistance = dist*levelScaleFactor;                              // 观测到该点的距离上限
        mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1];    // 观测到该点的距离下限
        mNormalVector = normal/n;                                           // 获得地图点平均的观测方向
    }
}

2.4.3 函数解析  

        我们先用变量observations接收能观测到该地图点的所有关键帧及该地图点再该关键帧中的索引，pRefKF接收第一次创建该地图点的关键帧，Pos接收该地图点在世界坐标系中的位置。

        接着我们遍历所有可以观测到该地图点的关键帧，提取能观测到该地图点的相机坐标与地图点坐标，形成指向地图点的向量并归一化进行累加normal。

         最后我们更新获得地图点平均的观测方向为（normal/能观测到该地图点的数量），并更新观测到该点的距离上限与观测到该点的距离下限。