ORBSLAM2——单目初始化器Initializer
文章目录
- 查找初始的匹配点对：
- 并行计算两个模型
- 模型选择：
- 运动恢复（sfm）
- Bundle adjustment
ORB_SLAM2单目模式的初始化过程可以分为以下阶段：
- 通过匹配选取两个可以作为起始两帧的初始帧
- 根据匹配计算两帧之间的位姿，获得初始位姿
- 三角化测量得到初始的特征点深度，包装成MapPoint，构建初始地图。
地图初始化的目的是计算两帧图像之间的相对位姿来三角化一组初始的地图云点。ORBSLAM2提出并行计算两个几何模型，一个是面向平面视图的单映矩阵，另一个是面向非平面视图的基础矩阵。然后，采用启发式的方法选择模型，并使用所选的模型从两图像的相对位姿中对地图点云进行重构。只有当两个视图之间的视差达到安全阈值时，才进行地图初始化。如果检测到低视差的情况或已知两视图模糊的情况，则为了避免生成一个有缺陷的地图而推迟初始化。
第一次新建Initializer对象实在Tracking的构造函数里，初始化调用MonocularInitialization()函数
代码如下：
void Tracking::MonocularInitialization()//单目初始化
{
// 如果单目初始器还没有被创建，则创建单目初始器
if(!mpInitializer)
{
// Set Reference Frame
// 单目初始帧的特征点数必须大于100
if(mCurrentFrame.mvKeys.size()>100)
{
// 步骤1：得到用于初始化的第一帧，初始化需要两帧
mInitialFrame = Frame(mCurrentFrame);
// 记录最后的一帧
mLastFrame = Frame(mCurrentFrame);
// mvbPrevMatched最大的情况就是所有特征点都被跟踪上
mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());
for(size_t i=0; i<mCurrentFrame.mvKeysUn.size(); i++)
mvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt;
if(mpInitializer)
delete mpInitializer;
// 由当前帧构造初始器 sigma:1.0 iterations:200
mpInitializer = new Initializer(mCurrentFrame,1.0,200);
fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
return;
}
}
else //如果是第二次进入，已经创建了初始器
{
// Try to initialize
// 步骤2：如果当前帧特征点数大于100，则得到用于单目初始化的第二帧
// 如果当前帧特征点太少，重新构造初始器
// 因此只有连续两帧的特征点个数都大于100时，才能继续进行初始化过程
if((int)mCurrentFrame.mvKeys.size()<=100)
{
delete mpInitializer;
mpInitializer = static_cast<Initializer*>(NULL);
fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
return;
}
// Find correspondences
// 步骤3：在mInitialFrame与mCurrentFrame中找匹配的特征点对
// mvbPrevMatched为前一帧的特征点，存储了mInitialFrame中哪些点将进行接下来的匹配
// mvIniMatches存储mInitialFrame,mCurrentFrame之间匹配的特征点
ORBmatcher matcher(0.9,true);
int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
// Check if there are enough correspondences
// 步骤4：如果初始化的两帧之间的匹配点太少，重新初始
if(nmatches<100)
{
delete mpInitializer;
mpInitializer = static_cast<Initializer*>(NULL);
return;
}
cv::Mat Rcw; // Current Camera Rotation
cv::Mat tcw; // Current Camera Translation
vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)
// 步骤5：通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints
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);
// 步骤7：将三角化得到的3D点包装成MapPoints
// Initialize函数会得到mvIniP3D，
// mvIniP3D是cv::Point3f类型的一个容器，是个存放3D点的临时变量，
// CreateInitialMapMonocular将3D点包装成MapPoint类型存入KeyFrame和Map中
CreateInitialMapMonocular();
}
}
}
查找初始的匹配点对：
从当前帧中提取ORB特征 F c F_c Fc，与在参考帧 F r F_r Fr搜索匹配点对 x c ↔ x r x_c\leftrightarrow x_r xc↔xr。如果找不到足够的匹配点对，就重置参考帧。
Tracking.cc中的Tracking::MonocularInitialization函数中。
// 步骤3：在mInitialFrame与mCurrentFrame中找匹配的特征点对
// mvbPrevMatched为前一帧的特征点，存储了mInitialFrame中哪些点将进行接下来的匹配
// mvIniMatches存储mInitialFrame,mCurrentFrame之间匹配的特征点
ORBmatcher matcher(0.9,true);
int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
并行计算两个模型
当匹配成功之后，开始调用Initializer.cc中的Initializer::Initialized函数进行初始化工作
// 步骤5：通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints
if(mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated))
为了计算R和t，ORB_SLAM为了针对平面和非平面场景选择最合适的模型，同时开启了两个线程，分别计算单应矩阵 H c r H_{cr} Hcr和基础矩阵 F c r F_{cr} Fcr。如下所示：
thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));
// Wait until both threads have finished
threadH.join();
threadF.join();
为了使两个模型的计算流程尽量一样，将两个模型的迭代循环次数预先设置100，每次迭代的特征点数目也预先设置好，基础矩阵是8个特征点对，单映矩阵是4个特征点对。每次迭代中，我们给每个模型M（H表示单映射，F表示基本矩阵）计算分值 S M S_M SM：
其中， ρ M ( . ) \rho _M(.) ρM(.)判断该点的重投影误差是否小于阈值,小于则有效,结果累加进score中,大于则代表无效,舍弃.
T M T_M TM是无效数据的排除阈值，该阈值的选择主要依赖 χ 2 \chi ^2 χ2统计量 (即正态分布的平方的累加,累加个数即为自由度),显著性水平为5%,在单应矩阵H中,自由度为2,对应的阈值 T H T_H TH=5.991在基础矩阵F中,自由度为1,对应的阈值 T F T_F TF=3.841.如下图所示:
模型选择：
orbslam2中采用并行计算单应矩阵和本质矩阵,在算出两个矩阵对应的score之后,按照如下公式计算 R H R_H RH
// Compute ratio of scores
float RH = SH/(SH+SF);
// Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
if(RH>0.40)
return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
else //if(pF_HF>0.6)
return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
如果 R H > 0.4 R_H>0.4 RH>0.4 , 则选择采用单应矩阵H恢复相机位姿,否则使用基础矩阵
- 本质矩阵获取模型评分
ORB_SLAM2/src/Initializer.cpp--void Initializer::FindHomography()
...
/**
* @brief 计算单应矩阵
*
* 假设场景为平面情况下通过前两帧求取Homography矩阵(current frame 2 到 reference frame 1),并得到该模型的评分
*/
void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21)
{
...
//1.特征点归一化
Normalize(mvKeys1,vPn1, T1);
Normalize(mvKeys2,vPn2, T2);
cv::Mat T2inv = T2.inv();
//2.求解单应矩阵
cv::Mat Hn = ComputeH21(vPn1i,vPn2i);
// 恢复原始的均值和尺度（解除归一化）
H21i = T2inv*Hn*T1;
H12i = H21i.inv();
// 3.利用重投影误差为当次RANSAC的结果评分
currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);
...
}
- 基础矩阵获取模型评分
ORB_SLAM2/src/Initializer.cpp--void Initializer::FindHomography()
...
/**
* @brief 计算单应矩阵
*
* 假设场景为平面情况下通过前两帧求取Homography矩阵(current frame 2 到 reference frame 1),并得到该模型的评分
*/
void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21)
{
...
//1.特征点归一化
Normalize(mvKeys1,vPn1, T1);
Normalize(mvKeys2,vPn2, T2);
cv::Mat T2inv = T2.inv();
//2.求解单应矩阵
cv::Mat Hn = ComputeH21(vPn1i,vPn2i);
// 恢复原始的均值和尺度（解除归一化）
H21i = T2inv*Hn*T1;
H12i = H21i.inv();
// 3.利用重投影误差为当次RANSAC的结果评分
currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);
...
}
运动恢复（sfm）
(1)单应矩阵恢复
系统成功使用单应矩阵进行相机恢复具备如下条件:
- 单应矩阵的三个奇异值必须要有足够的差异
- 成功三角化测量的内点的个数必须足够突出,远好过第二好,且个数要大于最低阈值
- 要有足够的视差
//vbMatchesInliersH: true--inliners false--outliners
//H:单应矩阵 ,mK:相机内参
//R21,t21:待求相机位姿
//vP3D:三角化测量后的空间三维点坐标
//vbTriangulated:true--成功三角化测量 false--测量失败
//1.0:最小视差为1° 50:三角化测量成功点最低阈值
ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50)
(2)基础矩阵恢复
在基本矩阵的情况下，我们使用内参矩阵K用下式将其转换为本质矩阵：．
使用奇异值分解方法计算4个运动解，将四个解用于三角化特征点，以选择正解。程序中调用Initializer::ReconstructF函数恢复运动。
//vbMatchesInliersH: true--inliners false--outliners
//F:基础矩阵 ,mK:相机内参
//R21,t21:待求相机位姿
//vP3D:三角化测量后的空间三维点坐标
//vbTriangulated:true--成功三角化测量 false--测量失败
//1.0:最小视差为1° 50:三角化测量成功点最低阈值
ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
Bundle adjustment
执行全局BA，以优化初始重构得到的点云地图。
- 当第一次进入该方法的时候，没有先前的帧数据，将当前帧保存为初始帧和最后一帧，并初始化一个初始化器。
- 利用ORB匹配器，对当前帧和初始帧进行匹配，对应关系小于100个时失败。
- 利用八点法的对极约束，启动两个线程分别计算单应矩阵和基础矩阵，并通过score判断用单应矩阵恢复运动轨迹还是使用基础矩阵恢复运动轨迹。
- 将初始帧和当前帧创建为关键帧，并创建地图点MapPoint
- 通过全局BundleAdjustment优化相机位姿和关键点坐标
- 设置单位深度并缩放初试基线和地图点。
- 其他变量的初始化。
这部分是在Tracking.cc中的CreateInitialMapMonocular()函数中，代码如下
void Tracking::CreateInitialMapMonocular()
void Tracking::CreateInitialMapMonocular() { // 创建两帧，一个为初始帧，一个为当前帧 KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB); KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB); // 步骤1：将初始关键帧的描述子转为BoW pKFini->ComputeBoW(); // 步骤2：将当前关键帧的描述子转为BoW pKFcur->ComputeBoW(); // Insert KFs in the map // 步骤3：将关键帧插入到地图 // 凡是关键帧，都要插入地图 mpMap->AddKeyFrame(pKFini); mpMap->AddKeyFrame(pKFcur); // Create MapPoints and asscoiate to keyframes // 步骤4：将3D点包装成MapPoints for(size_t i=0; i<mvIniMatches.size();i++)//遍历所有匹配 { if(mvIniMatches[i]<0) continue; //Create MapPoint. cv::Mat worldPos(mvIniP3D[i]); // 步骤4.1：用3D点构造MapPoint MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpMap); // 步骤4.2：为该MapPoint添加属性： // a.观测到该MapPoint的关键帧 // b.该MapPoint的描述子 // c.该MapPoint的平均观测方向和深度范围 // 步骤4.3：表示该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; //Add to Map // 步骤4.4：在地图中添加该MapPoint mpMap->AddMapPoint(pMP); } // Update Connections // 步骤5：更新关键帧间的连接关系 // 在3D点和关键帧之间建立边，每个边有一个权重，边的权重是该关键帧与当前帧公共3D点的个数 pKFini->UpdateConnections(); pKFcur->UpdateConnections(); // Bundle Adjustment cout << "New Map created with " << mpMap->MapPointsInMap() << " points" << endl； // 步骤5：BA优化 Optimizer::GlobalBundleAdjustemnt(mpMap,20); // Set median depth to 1 // 步骤6：!!!将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 point // 把3D点的尺度也归一化到 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;// 初始化成功，至此，初始化过程完成 }
PTAM和LSD_SLAM在这个数据集中，会将所有点初始化在一个平面上，而ORB_SLAM会一直等到有足够的视差，才使用基础矩阵，得到最正确的初始化。由于ORB-SLAM对初始化的要求较高，因此初始化时可以选择一个特征丰富的场景，移动摄像机给它提供足够的视差。另外，由于坐标系会附着在初始化成功的那帧图像的位置，因此每次初始化不能保证在同一个位置。
参考：
ORBSLAM2之单目初始化(2)
一步步带你看懂orbslam2源码–orb特征点提取(二)
一步步带你看懂orbslam2源码–单目初始化(三)
认真的虎ORBSLAM2源码解读(七)：单目初始化器Initializer