SLAM系統中，一般的做法是通過P3P或者EPnP等方法進行投影估算出相機位姿，進而建構最小二乘對估計的相機位姿進行優化，進而得到精度比較高的位姿。優化完後可以剔除一些outlier點，這樣的話後邊再進行投影和優化的時候精度會有所保證。ORB-SLAM系統當中的優化分為下面5個，從優化的範圍層次逐漸擴大。，
1.位姿優化函數PoseOptimization
一些說明：PoseOptimization優化的是單幀圖像的位姿。相機在運動的時候每個圖像幀可以對應三維世界中多個地圖點，那麼此時我們可以知道三維世界中的多個3D地圖點，也可以知道圖像幀中對應的2D像素點坐标，就可以通過3D-2D的投影關系估計出相機的位姿。PoseOptimization就是對單個圖像幀中的多個地圖點建立多個一進制連接配接邊，構成圖進行優化， 優化單個圖像幀的SE3位姿。
調用時機：當一個圖像幀中的特征點和3D空間中多個地圖點比對，并且有一個“初始位姿”的時候，就可以通過位姿優化函數來對目前幀的位姿進行優化。ORB-SLAM系統當中在Tracking線程中多次調用了位姿優化函數，可以試想一下，畢竟Tracking線程的主要業務就是進行跟蹤，一幀一幀圖像的位姿描述了相機的軌迹。系統的前兩幀圖像的位姿都初始化為機關矩陣，在通過2D點比對建立關聯關系後，通過三角化建立了地圖點，此時才可以對圖像幀的位姿進行優化更新。隻要3D-2D比對關系發生變化，就需要對位姿進行優化。
ORB-SLAM系統中下面幾個函數裡邊都調用了位姿優化函數：
1）Tracking::TrackReferenceKeyFrame()
2）Tracking::TrackWithMotionModel()
3）Tracking::TrackLocalMap()
4）Tracking::Relocalization()
代碼：
/**
* 這裡僅優化幀的位姿，不優化地圖點的坐标。(之後會剔除優化後的外點，這個操作不再本函數中做)
* (3D-2D)将地圖點投影到相機坐标系下的相機平面
*/
int Optimizer::PoseOptimization(Frame *pFrame)
{
g2o::SparseOptimizer optimizer;
g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
linearSolver = new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();
g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
int nInitialCorrespondences=0;
// Set Frame vertex 設定幀結點
//VertexSE3Expmap為李代數位姿
g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
//目前幀的變換矩陣
vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
vSE3->setId(0);
vSE3->setFixed(false);
optimizer.addVertex(vSE3);
// Set MapPoint vertices 設定地圖點結點
const int N = pFrame->N;
vector<g2o::EdgeSE3ProjectXYZOnlyPose*> vpEdgesMono;
vector<size_t> vnIndexEdgeMono;
vpEdgesMono.reserve(N);
vnIndexEdgeMono.reserve(N);
vector<g2o::EdgeStereoSE3ProjectXYZOnlyPose*> vpEdgesStereo;
vector<size_t> vnIndexEdgeStereo;
vpEdgesStereo.reserve(N);
vnIndexEdgeStereo.reserve(N);
const float deltaMono = sqrt(5.991);
const float deltaStereo = sqrt(7.815);
{
unique_lock<mutex> lock(MapPoint::mGlobalMutex);
//周遊幀中的MapPoint
for(int i=0; i<N; i++)
{
MapPoint* pMP = pFrame->mvpMapPoints[i];
if(pMP)
{
// Monocular observation 單目
if(pFrame->mvuRight[i]<0)
{
nInitialCorrespondences++;
pFrame->mvbOutlier[i] = false;
Eigen::Matrix<double,2,1> obs;
//擷取關鍵點
const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i];
//obs中輸入關鍵點的x,y坐标
obs << kpUn.pt.x, kpUn.pt.y;
/**
* EdgeSE3ProjectXYZOnlyPose()：PoseEstimation中的重投影誤差，将地圖點投影到相機坐标系下的相機平面。
*/
//構造pose一進制邊
g2o::EdgeSE3ProjectXYZOnlyPose* e = new g2o::EdgeSE3ProjectXYZOnlyPose();
//這裡隻設定一個頂點
//optimizer.vertex(0)傳回的就是上邊的vSE3結點，也就是目前幀的Tcw
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
//設定觀測數值，obs中為幀中的關鍵點坐标，也就是三維坐标中物體投影到相機平面的坐标
e->setMeasurement(obs);
//擷取關鍵點所在層的sigma2值
const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(deltaMono);//設定門檻值
//相機内參
e->fx = pFrame->fx;
e->fy = pFrame->fy;
e->cx = pFrame->cx;
e->cy = pFrame->cy;
//地圖中MapPoint的世界坐标系的坐标值
cv::Mat Xw = pMP->GetWorldPos();
e->Xw[0] = Xw.at<float>(0);
e->Xw[1] = Xw.at<float>(1);
e->Xw[2] = Xw.at<float>(2);
optimizer.addEdge(e);
vpEdgesMono.push_back(e);
vnIndexEdgeMono.push_back(i);
}
else // Stereo observation 雙目
{
nInitialCorrespondences++;
pFrame->mvbOutlier[i] = false;
//SET EDGE 設定邊
Eigen::Matrix<double,3,1> obs;
const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i];
const float &kp_ur = pFrame->mvuRight[i];
//obs中存入的是雙目相機中像素坐标下一個關鍵點的坐标
obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
//優化變量隻有pose，地圖點位置固定，是一邊元，雙目中使用的是EdgeStereoSE3ProjectXYZOnlyPoze()。
g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = new g2o::EdgeStereoSE3ProjectXYZOnlyPose();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
e->setMeasurement(obs);
const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
e->setInformation(Info);
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(deltaStereo);
e->fx = pFrame->fx;
e->fy = pFrame->fy;
e->cx = pFrame->cx;
e->cy = pFrame->cy;
e->bf = pFrame->mbf;
cv::Mat Xw = pMP->GetWorldPos();
e->Xw[0] = Xw.at<float>(0);
e->Xw[1] = Xw.at<float>(1);
e->Xw[2] = Xw.at<float>(2);
optimizer.addEdge(e);
vpEdgesStereo.push_back(e);
vnIndexEdgeStereo.push_back(i);
}
}
}
}
//至少需要使用三對點進行P3P求解，三對3D-2D比對點
if(nInitialCorrespondences<3)
return 0;
// We perform 4 optimizations, after each optimization we classify observation as inlier/outlier
// At the next optimization, outliers are not included, but at the end they can be classified as inliers again.
const float chi2Mono[4]={5.991,5.991,5.991,5.991};
const float chi2Stereo[4]={7.815,7.815,7.815, 7.815};
const int its[4]={10,10,10,10};
int nBad=0;
//優化四次
for(size_t it=0; it<4; it++)
{
vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
optimizer.initializeOptimization(0);
optimizer.optimize(its[it]);
nBad=0;
//單目
for(size_t i=0, iend=vpEdgesMono.size(); i<iend; i++)
{
g2o::EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i];
const size_t idx = vnIndexEdgeMono[i];
if(pFrame->mvbOutlier[idx])
{
e->computeError();
}
const float chi2 = e->chi2();
if(chi2>chi2Mono[it])
{
pFrame->mvbOutlier[idx]=true;
e->setLevel(1);
nBad++;
}
else
{
pFrame->mvbOutlier[idx]=false;
e->setLevel(0);
}
if(it==2)
e->setRobustKernel(0);
}
//雙目
for(size_t i=0, iend=vpEdgesStereo.size(); i<iend; i++)
{
g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = vpEdgesStereo[i];
const size_t idx = vnIndexEdgeStereo[i];
if(pFrame->mvbOutlier[idx])
{
e->computeError();
}
const float chi2 = e->chi2();
if(chi2>chi2Stereo[it])
{
pFrame->mvbOutlier[idx]=true;
e->setLevel(1);
nBad++;
}
else
{
e->setLevel(0);
pFrame->mvbOutlier[idx]=false;
}
if(it==2)
e->setRobustKernel(0);
}
if(optimizer.edges().size()<10)
break;
}
// Recover optimized pose and return number of inliers
g2o::VertexSE3Expmap* vSE3_recov = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(0));
g2o::SE3Quat SE3quat_recov = vSE3_recov->estimate();
cv::Mat pose = Converter::toCvMat(SE3quat_recov);
//設定優化完成後的位姿
pFrame->SetPose(pose);
return nInitialCorrespondences-nBad;
}
2.優化兩幀之間的位姿OptimizeSim3
一些說明：對單目相機來說具有尺度不确定性，當相機運動一段時間後不可避免會産生尺度漂移累積誤差增大，當進行閉環檢測的時候如果尺度不一緻就不能進行很好的對應。相比于SE3來說，相似變換可以提供尺度的表達。那麼這時候就可以借助相似變換群sim3來求解目前關鍵幀和閉環候選幀之間的sim3位姿，同時sim3位姿也可以和SE3進行轉換。
要優化的兩幀都能觀測到多個相同的地圖點，并且已經知道有哪些地圖點是一緻的。此時可以構造一個超定方程組，并對其求解最小化誤差優化。優化幀間變化的SIM3位姿與地圖的VertexSBAPointXYZ位姿。
調用時機：單目閉環檢測中檢測出閉環後，對目前關鍵幀和閉環幀之間計算sim3位姿，當計算出sim3位姿後，就需要調用sim3位姿優化函數。
ORB-SLAM當中在LoopClosing::ComputetSim3()中調用了OptimizeSim3函數。
代碼：
int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches1, g2o::Sim3 &g2oS12, const float th2, const bool bFixScale)
{
g2o::SparseOptimizer optimizer;
g2o::BlockSolverX::LinearSolverType * linearSolver;
linearSolver = new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();
g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
// Calibration
const cv::Mat &K1 = pKF1->mK;
const cv::Mat &K2 = pKF2->mK;
// Camera poses
const cv::Mat R1w = pKF1->GetRotation();
const cv::Mat t1w = pKF1->GetTranslation();
const cv::Mat R2w = pKF2->GetRotation();
const cv::Mat t2w = pKF2->GetTranslation();
// Set Sim3 vertex 設定sim3頂點，将pKF1到pKF2之間的sim3位姿變換加入到圖中作為結點0
//g2o::VertexSim3Expmap用于建立sim3位姿結點
g2o::VertexSim3Expmap * vSim3 = new g2o::VertexSim3Expmap();
vSim3->_fix_scale=bFixScale;
vSim3->setEstimate(g2oS12);
vSim3->setId(0);
vSim3->setFixed(false);
//擷取相機内參
vSim3->_principle_point1[0] = K1.at<float>(0,2);
vSim3->_principle_point1[1] = K1.at<float>(1,2);
vSim3->_focal_length1[0] = K1.at<float>(0,0);
vSim3->_focal_length1[1] = K1.at<float>(1,1);
vSim3->_principle_point2[0] = K2.at<float>(0,2);
vSim3->_principle_point2[1] = K2.at<float>(1,2);
vSim3->_focal_length2[0] = K2.at<float>(0,0);
vSim3->_focal_length2[1] = K2.at<float>(1,1);
optimizer.addVertex(vSim3);
// Set MapPoint vertices
const int N = vpMatches1.size();
//pKF1當中所有的地圖點
const vector<MapPoint*> vpMapPoints1 = pKF1->GetMapPointMatches();
//sim3投影邊建立
vector<g2o::EdgeSim3ProjectXYZ*> vpEdges12;
vector<g2o::EdgeInverseSim3ProjectXYZ*> vpEdges21;
vector<size_t> vnIndexEdge;
vnIndexEdge.reserve(2*N);
vpEdges12.reserve(2*N);
vpEdges21.reserve(2*N);
const float deltaHuber = sqrt(th2);
int nCorrespondences = 0;
for(int i=0; i<N; i++)
{
if(!vpMatches1[i])
continue;
//vpMapPoints1中存放的是pKF1當中的地圖點
MapPoint* pMP1 = vpMapPoints1[i];
//vpMatches1當中存放的是pKF1和pKF2兩個幀中比對的地圖點
MapPoint* pMP2 = vpMatches1[i];
const int id1 = 2*i+1;
const int id2 = 2*(i+1);
const int i2 = pMP2->GetIndexInKeyFrame(pKF2);
if(pMP1 && pMP2)
{
if(!pMP1->isBad() && !pMP2->isBad() && i2>=0)
{
//g2o::VertexSBAPointXYZ用于建立特征點空間坐标結點
g2o::VertexSBAPointXYZ* vPoint1 = new g2o::VertexSBAPointXYZ();
cv::Mat P3D1w = pMP1->GetWorldPos();
cv::Mat P3D1c = R1w*P3D1w + t1w;
vPoint1->setEstimate(Converter::toVector3d(P3D1c));
vPoint1->setId(id1);
vPoint1->setFixed(true);
optimizer.addVertex(vPoint1);
g2o::VertexSBAPointXYZ* vPoint2 = new g2o::VertexSBAPointXYZ();
cv::Mat P3D2w = pMP2->GetWorldPos();
cv::Mat P3D2c = R2w*P3D2w + t2w;
vPoint2->setEstimate(Converter::toVector3d(P3D2c));
vPoint2->setId(id2);
vPoint2->setFixed(true);
optimizer.addVertex(vPoint2);
}
else
continue;
}
else
continue;
nCorrespondences++;
// Set edge x1 = S12*X2
Eigen::Matrix<double,2,1> obs1;
const cv::KeyPoint &kpUn1 = pKF1->mvKeysUn[i];
obs1 << kpUn1.pt.x, kpUn1.pt.y;
//g2o::EdgeSim3ProjectXYZ用于建立重投影誤差，優化變量sim3位姿和地圖點
g2o::EdgeSim3ProjectXYZ* e12 = new g2o::EdgeSim3ProjectXYZ();
e12->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id2)));
e12->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
e12->setMeasurement(obs1);
const float &invSigmaSquare1 = pKF1->mvInvLevelSigma2[kpUn1.octave];
e12->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare1);
g2o::RobustKernelHuber* rk1 = new g2o::RobustKernelHuber;
e12->setRobustKernel(rk1);
rk1->setDelta(deltaHuber);
optimizer.addEdge(e12);
// Set edge x2 = S21*X1
Eigen::Matrix<double,2,1> obs2;
const cv::KeyPoint &kpUn2 = pKF2->mvKeysUn[i2];
obs2 << kpUn2.pt.x, kpUn2.pt.y;
g2o::EdgeInverseSim3ProjectXYZ* e21 = new g2o::EdgeInverseSim3ProjectXYZ();
e21->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id1)));
e21->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
e21->setMeasurement(obs2);
float invSigmaSquare2 = pKF2->mvInvLevelSigma2[kpUn2.octave];
e21->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare2);
g2o::RobustKernelHuber* rk2 = new g2o::RobustKernelHuber;
e21->setRobustKernel(rk2);
rk2->setDelta(deltaHuber);
optimizer.addEdge(e21);
vpEdges12.push_back(e12);
vpEdges21.push_back(e21);
vnIndexEdge.push_back(i);
}
// Optimize!
optimizer.initializeOptimization();
optimizer.optimize(5);
// Check inliers
int nBad=0;
for(size_t i=0; i<vpEdges12.size();i++)
{
g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
if(!e12 || !e21)
continue;
if(e12->chi2()>th2 || e21->chi2()>th2)
{
size_t idx = vnIndexEdge[i];
vpMatches1[idx]=static_cast<MapPoint*>(NULL);
optimizer.removeEdge(e12);
optimizer.removeEdge(e21);
vpEdges12[i]=static_cast<g2o::EdgeSim3ProjectXYZ*>(NULL);
vpEdges21[i]=static_cast<g2o::EdgeInverseSim3ProjectXYZ*>(NULL);
nBad++;
}
}
int nMoreIterations;
if(nBad>0)
nMoreIterations=10;
else
nMoreIterations=5;
if(nCorrespondences-nBad<10)
return 0;
// Optimize again only with inliers
optimizer.initializeOptimization();
optimizer.optimize(nMoreIterations);
int nIn = 0;
for(size_t i=0; i<vpEdges12.size();i++)
{
g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
if(!e12 || !e21)
continue;
if(e12->chi2()>th2 || e21->chi2()>th2)
{
size_t idx = vnIndexEdge[i];
vpMatches1[idx]=static_cast<MapPoint*>(NULL);
}
else
nIn++;
}
// Recover optimized Sim3 恢複優化後的sim3位姿
g2o::VertexSim3Expmap* vSim3_recov = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(0));
g2oS12= vSim3_recov->estimate();
return nIn;
}
3.局部BA優化LocalBundleAdjustment
一些說明：局部BA優化，優化的是局部關鍵幀的位姿和這些局部關鍵幀可以觀測到的地圖點的3D坐标。相機在運動過程中，目前時刻的圖像幀何其前後的若幹個幀是能觀測到一些相同的路标點的，也就是說這些幀之間有共視關系。這些共視幀和他們的所有地圖點放在一塊就展現了“局部”的意思。将局部的所有地圖點與可以觀測到他們的關鍵幀放在一起進行優化，将關鍵幀的SE3位姿和地圖點的3D坐标作為待優化量添加到頂點當中。
調用時機：LocalMapping線程當中調用。LocalMapping線程中處理的就是目前關鍵幀和其共視關鍵幀、以及這些關鍵幀能夠觀測到的地圖點之間的連接配接關系，當然少不了對局部關鍵幀位姿和地圖點3D坐标的更新。
代碼：
void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap)
{
// Local KeyFrames: First Breath Search from Current Keyframe
list<KeyFrame*> lLocalKeyFrames;
lLocalKeyFrames.push_back(pKF);
pKF->mnBALocalForKF = pKF->mnId;
//1.擷取局部關鍵幀清單存放在lLocalKeyFrames中。也就是和目前關鍵幀有共視關系的關鍵幀清單。
const vector<KeyFrame*> vNeighKFs = pKF->GetVectorCovisibleKeyFrames();
for(int i=0, iend=vNeighKFs.size(); i<iend; i++)
{
KeyFrame* pKFi = vNeighKFs[i];
pKFi->mnBALocalForKF = pKF->mnId;
if(!pKFi->isBad())
lLocalKeyFrames.push_back(pKFi);
}
// Local MapPoints seen in Local KeyFrames
//2.建立在局部關鍵幀中所看到的局部地圖點清單，存放在lLocalMapPoints中。
list<MapPoint*> lLocalMapPoints;
for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin() , lend=lLocalKeyFrames.end(); lit!=lend; lit++)
{
vector<MapPoint*> vpMPs = (*lit)->GetMapPointMatches();
for(vector<MapPoint*>::iterator vit=vpMPs.begin(), vend=vpMPs.end(); vit!=vend; vit++)
{
MapPoint* pMP = *vit;
if(pMP)
if(!pMP->isBad())
if(pMP->mnBALocalForKF!=pKF->mnId)
{
lLocalMapPoints.push_back(pMP);
pMP->mnBALocalForKF=pKF->mnId;
}
}
}
// Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframes
//3.建立固定關鍵幀清單lFixedCameras。這些關鍵幀可以看到局部地圖點，但是這些關鍵幀并不是局部關鍵幀。
list<KeyFrame*> lFixedCameras;
for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
{
map<KeyFrame*,size_t> observations = (*lit)->GetObservations();
for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
KeyFrame* pKFi = mit->first;
if(pKFi->mnBALocalForKF!=pKF->mnId && pKFi->mnBAFixedForKF!=pKF->mnId)
{
//TODO:設定BAFixed辨別
pKFi->mnBAFixedForKF=pKF->mnId;
if(!pKFi->isBad())
lFixedCameras.push_back(pKFi);
}
}
}
// Setup optimizer
g2o::SparseOptimizer optimizer;
g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
if(pbStopFlag)
optimizer.setForceStopFlag(pbStopFlag);
unsigned long maxKFid = 0;
// Set Local KeyFrame vertices
// 4. 設定局部關鍵幀頂點并加入優化器中。
for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
{
KeyFrame* pKFi = *lit;
g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
vSE3->setId(pKFi->mnId);
vSE3->setFixed(pKFi->mnId==0);
optimizer.addVertex(vSE3);
if(pKFi->mnId>maxKFid)
maxKFid=pKFi->mnId;
}
// Set Fixed KeyFrame vertices
// 5. 設定固定關鍵幀頂點，并加入到優化器清單中。
for(list<KeyFrame*>::iterator lit=lFixedCameras.begin(), lend=lFixedCameras.end(); lit!=lend; lit++)
{
KeyFrame* pKFi = *lit;
g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
vSE3->setId(pKFi->mnId);
vSE3->setFixed(true);
optimizer.addVertex(vSE3);
if(pKFi->mnId>maxKFid)
maxKFid=pKFi->mnId;
}
// Set MapPoint vertices
// 6.設定MapPoint頂點
const int nExpectedSize = (lLocalKeyFrames.size()+lFixedCameras.size())*lLocalMapPoints.size();
vector<g2o::EdgeSE3ProjectXYZ*> vpEdgesMono;
vpEdgesMono.reserve(nExpectedSize);
vector<KeyFrame*> vpEdgeKFMono;
vpEdgeKFMono.reserve(nExpectedSize);
vector<MapPoint*> vpMapPointEdgeMono;
vpMapPointEdgeMono.reserve(nExpectedSize);
vector<g2o::EdgeStereoSE3ProjectXYZ*> vpEdgesStereo;
vpEdgesStereo.reserve(nExpectedSize);
vector<KeyFrame*> vpEdgeKFStereo;
vpEdgeKFStereo.reserve(nExpectedSize);
vector<MapPoint*> vpMapPointEdgeStereo;
vpMapPointEdgeStereo.reserve(nExpectedSize);
const float thHuberMono = sqrt(5.991);
const float thHuberStereo = sqrt(7.815);
// 7.周遊局部地圖點清單，設定優化對應的邊
for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
{
MapPoint* pMP = *lit;
g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
int id = pMP->mnId+maxKFid+1;
vPoint->setId(id);
vPoint->setMarginalized(true);
optimizer.addVertex(vPoint);
const map<KeyFrame*,size_t> observations = pMP->GetObservations();
//Set edges
for(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
KeyFrame* pKFi = mit->first;
if(!pKFi->isBad())
{
const cv::KeyPoint &kpUn = pKFi->mvKeysUn[mit->second];
// Monocular observation
if(pKFi->mvuRight[mit->second]<0)
{
Eigen::Matrix<double,2,1> obs;
obs << kpUn.pt.x, kpUn.pt.y;
g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));//地圖點
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));//關鍵幀
e->setMeasurement(obs);
const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(thHuberMono);
e->fx = pKFi->fx;
e->fy = pKFi->fy;
e->cx = pKFi->cx;
e->cy = pKFi->cy;
optimizer.addEdge(e);
vpEdgesMono.push_back(e);
vpEdgeKFMono.push_back(pKFi);
vpMapPointEdgeMono.push_back(pMP);
}
else // Stereo observation
{
Eigen::Matrix<double,3,1> obs;
const float kp_ur = pKFi->mvuRight[mit->second];
obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
g2o::EdgeStereoSE3ProjectXYZ* e = new g2o::EdgeStereoSE3ProjectXYZ();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));
e->setMeasurement(obs);
const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
e->setInformation(Info);
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(thHuberStereo);
e->fx = pKFi->fx;
e->fy = pKFi->fy;
e->cx = pKFi->cx;
e->cy = pKFi->cy;
e->bf = pKFi->mbf;
optimizer.addEdge(e);
vpEdgesStereo.push_back(e);
vpEdgeKFStereo.push_back(pKFi);
vpMapPointEdgeStereo.push_back(pMP);
}
}
}
}
if(pbStopFlag)
if(*pbStopFlag)
return;
//8.進行優化。這裡進行了5次疊代優化。
optimizer.initializeOptimization();
optimizer.optimize(5);
bool bDoMore= true;
if(pbStopFlag)
if(*pbStopFlag)
bDoMore = false;
// 9.檢查上邊5次疊代優化正确點的觀測值，并将異常點排除掉。完成後再次進行10次疊代優化。
if(bDoMore)
{
// Check inlier observations
for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
{
g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
MapPoint* pMP = vpMapPointEdgeMono[i];
if(pMP->isBad())
continue;
if(e->chi2()>5.991 || !e->isDepthPositive())
{
e->setLevel(1);
}
e->setRobustKernel(0);
}
for(size_t i=0, iend=vpEdgesStereo.size(); i<iend;i++)
{
g2o::EdgeStereoSE3ProjectXYZ* e = vpEdgesStereo[i];
MapPoint* pMP = vpMapPointEdgeStereo[i];
if(pMP->isBad())
continue;
if(e->chi2()>7.815 || !e->isDepthPositive())
{
e->setLevel(1);
}
e->setRobustKernel(0);
}
// Optimize again without the outliers
//排除掉不好的點後，再次進行優化
optimizer.initializeOptimization(0);
optimizer.optimize(10);
}
vector<pair<KeyFrame*,MapPoint*> > vToErase;
vToErase.reserve(vpEdgesMono.size()+vpEdgesStereo.size());
// Check inlier observations
//10.優化後進行優化結果檢查
for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
{
g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
MapPoint* pMP = vpMapPointEdgeMono[i];
if(pMP->isBad())
continue;
if(e->chi2()>5.991 || !e->isDepthPositive())
{
KeyFrame* pKFi = vpEdgeKFMono[i];
vToErase.push_back(make_pair(pKFi,pMP));
}
}
for(size_t i=0, iend=vpEdgesStereo.size(); i<iend;i++)
{
g2o::EdgeStereoSE3ProjectXYZ* e = vpEdgesStereo[i];
MapPoint* pMP = vpMapPointEdgeStereo[i];
if(pMP->isBad())
continue;
if(e->chi2()>7.815 || !e->isDepthPositive())
{
KeyFrame* pKFi = vpEdgeKFStereo[i];
vToErase.push_back(make_pair(pKFi,pMP));
}
}
// Get Map Mutex
unique_lock<mutex> lock(pMap->mMutexMapUpdate);
if(!vToErase.empty())
{
for(size_t i=0;i<vToErase.size();i++)
{
KeyFrame* pKFi = vToErase[i].first;
MapPoint* pMPi = vToErase[i].second;
pKFi->EraseMapPointMatch(pMPi);
pMPi->EraseObservation(pKFi);
}
}
// Recover optimized data
//11.設定優化後的資料。
//Keyframes
for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
{
KeyFrame* pKF = *lit;
g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
//優化後的關鍵幀位姿
g2o::SE3Quat SE3quat = vSE3->estimate();
pKF->SetPose(Converter::toCvMat(SE3quat));
}
//Points
for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
{
MapPoint* pMP = *lit;
g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));
//優化後的地圖點世界坐标
pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
pMP->UpdateNormalAndDepth();
}
}
4.本質圖優化OptimizeEssentialGraph
一些說明：函數名稱是OptimizeEssentialGraph，那麼隻有形成了EssentialGraph之後才需要進行優化。EssentialGraph中不但有一般的共視關鍵幀之間的邊，還有閉環邊和共視圖邊。是以其中加入了幀間閉環的考慮，也加入了糾正後的幀的SIM3位姿。所有共視的關鍵幀、地圖點、一般的邊和閉環邊放在一起進行優化，優化的目标是關鍵幀的sim3位姿和地圖點的坐标。 将優化後的sim3位姿轉換為SE3，就得到了相機的位姿，并對地圖點的坐标進行投影得到更新後的坐标。
調用時機：LoopClosing線程當中通過sim3調整完關鍵幀和其閉環幀之間位姿後調用。
代碼：
/** 參數解釋：
* KeyFrame* pLoopKF 為和目前關鍵幀pCurKF形成閉環的關鍵幀
* KeyFrame* pCurKF 目前關鍵幀
* NonCorrectedSim3 目前關鍵幀及其共視關鍵幀最初的位姿
* CorrectedSim3 目前關鍵幀的共視關鍵幀進行sim3糾正後的位姿
* LoopConnections 其中的index為與目前關鍵幀有共視關系的關鍵幀
* 值為set<KeyFrame *>表示與目前關鍵幀的共視關鍵幀相連接配接的關鍵幀
*/
void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF,
const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,
const LoopClosing::KeyFrameAndPose &CorrectedSim3,
const map<KeyFrame *, set<KeyFrame *> > &LoopConnections, const bool &bFixScale)
{
// Setup optimizer
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(false);
//這裡為什麼選擇7*3的矩陣？因為除了在三個軸方向上的旋轉+平移外，還有尺度
g2o::BlockSolver_7_3::LinearSolverType * linearSolver =
new g2o::LinearSolverEigen<g2o::BlockSolver_7_3::PoseMatrixType>();
g2o::BlockSolver_7_3 * solver_ptr= new g2o::BlockSolver_7_3(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
solver->setUserLambdaInit(1e-16);
optimizer.setAlgorithm(solver);
//擷取地圖中所有的關鍵幀和地圖點
const vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();
const vector<MapPoint*> vpMPs = pMap->GetAllMapPoints();
const unsigned int nMaxKFid = pMap->GetMaxKFid();
vector<g2o::Sim3,Eigen::aligned_allocator<g2o::Sim3> > vScw(nMaxKFid+1);
vector<g2o::Sim3,Eigen::aligned_allocator<g2o::Sim3> > vCorrectedSwc(nMaxKFid+1);
vector<g2o::VertexSim3Expmap*> vpVertices(nMaxKFid+1);
const int minFeat = 100;
// Set KeyFrame vertices
for(size_t i=0, iend=vpKFs.size(); i<iend;i++)
{
KeyFrame* pKF = vpKFs[i];
if(pKF->isBad())
continue;
g2o::VertexSim3Expmap* VSim3 = new g2o::VertexSim3Expmap();
const int nIDi = pKF->mnId;
LoopClosing::KeyFrameAndPose::const_iterator it = CorrectedSim3.find(pKF);
if(it!=CorrectedSim3.end())
{
//擷取nIDi這個index的關鍵幀的糾正過的位姿
vScw[nIDi] = it->second;
VSim3->setEstimate(it->second);
}
else
{
Eigen::Matrix<double,3,3> Rcw = Converter::toMatrix3d(pKF->GetRotation());
Eigen::Matrix<double,3,1> tcw = Converter::toVector3d(pKF->GetTranslation());
g2o::Sim3 Siw(Rcw,tcw,1.0);
vScw[nIDi] = Siw;
VSim3->setEstimate(Siw);
}
//pLoopKF為查找到的比對閉環幀
if(pKF==pLoopKF)
VSim3->setFixed(true);
VSim3->setId(nIDi);
VSim3->setMarginalized(false);
VSim3->_fix_scale = bFixScale;
optimizer.addVertex(VSim3);
vpVertices[nIDi]=VSim3;
}
set<pair<long unsigned int,long unsigned int> > sInsertedEdges;
const Eigen::Matrix<double,7,7> matLambda = Eigen::Matrix<double,7,7>::Identity();
// Set Loop edges
for(map<KeyFrame *, set<KeyFrame *> >::const_iterator mit = LoopConnections.begin(), mend=LoopConnections.end(); mit!=mend; mit++)
{
//pKF為和目前關鍵幀有共視關系的關鍵幀
KeyFrame* pKF = mit->first;
const long unsigned int nIDi = pKF->mnId;
//mit->second表示和pKF相連接配接的關鍵幀
const set<KeyFrame*> &spConnections = mit->second;
const g2o::Sim3 Siw = vScw[nIDi];
const g2o::Sim3 Swi = Siw.inverse();
for(set<KeyFrame*>::const_iterator sit=spConnections.begin(), send=spConnections.end(); sit!=send; sit++)
{
const long unsigned int nIDj = (*sit)->mnId;
if((nIDi!=pCurKF->mnId || nIDj!=pLoopKF->mnId) && pKF->GetWeight(*sit)<minFeat)
continue;
const g2o::Sim3 Sjw = vScw[nIDj];
const g2o::Sim3 Sji = Sjw * Swi;
g2o::EdgeSim3* e = new g2o::EdgeSim3();
//兩個頂點一個為目前關鍵幀共視的關鍵幀，另一個為共視關鍵幀
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDj)));
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));
e->setMeasurement(Sji);
e->information() = matLambda;
optimizer.addEdge(e);
sInsertedEdges.insert(make_pair(min(nIDi,nIDj),max(nIDi,nIDj)));
}
}
// Set normal edges
for(size_t i=0, iend=vpKFs.size(); i<iend; i++)
{
KeyFrame* pKF = vpKFs[i];
const int nIDi = pKF->mnId;
g2o::Sim3 Swi;
LoopClosing::KeyFrameAndPose::const_iterator iti = NonCorrectedSim3.find(pKF);
if(iti!=NonCorrectedSim3.end())
Swi = (iti->second).inverse();
else
Swi = vScw[nIDi].inverse();
KeyFrame* pParentKF = pKF->GetParent();
// Spanning tree edge
if(pParentKF)
{
int nIDj = pParentKF->mnId;
g2o::Sim3 Sjw;
LoopClosing::KeyFrameAndPose::const_iterator itj = NonCorrectedSim3.find(pParentKF);
if(itj!=NonCorrectedSim3.end())
Sjw = itj->second;
else
Sjw = vScw[nIDj];
g2o::Sim3 Sji = Sjw * Swi;
g2o::EdgeSim3* e = new g2o::EdgeSim3();
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDj)));
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));
e->setMeasurement(Sji);
e->information() = matLambda;
optimizer.addEdge(e);
}
// Loop edges
const set<KeyFrame*> sLoopEdges = pKF->GetLoopEdges();
for(set<KeyFrame*>::const_iterator sit=sLoopEdges.begin(), send=sLoopEdges.end(); sit!=send; sit++)
{
KeyFrame* pLKF = *sit;
if(pLKF->mnId<pKF->mnId)
{
g2o::Sim3 Slw;
LoopClosing::KeyFrameAndPose::const_iterator itl = NonCorrectedSim3.find(pLKF);
if(itl!=NonCorrectedSim3.end())
Slw = itl->second;
else
Slw = vScw[pLKF->mnId];
g2o::Sim3 Sli = Slw * Swi;
g2o::EdgeSim3* el = new g2o::EdgeSim3();
el->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pLKF->mnId)));
el->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));
el->setMeasurement(Sli);
el->information() = matLambda;
optimizer.addEdge(el);
}
}
// Covisibility graph edges
const vector<KeyFrame*> vpConnectedKFs = pKF->GetCovisiblesByWeight(minFeat);
for(vector<KeyFrame*>::const_iterator vit=vpConnectedKFs.begin(); vit!=vpConnectedKFs.end(); vit++)
{
KeyFrame* pKFn = *vit;
if(pKFn && pKFn!=pParentKF && !pKF->hasChild(pKFn) && !sLoopEdges.count(pKFn))
{
if(!pKFn->isBad() && pKFn->mnId<pKF->mnId)
{
if(sInsertedEdges.count(make_pair(min(pKF->mnId,pKFn->mnId),max(pKF->mnId,pKFn->mnId))))
continue;
g2o::Sim3 Snw;
LoopClosing::KeyFrameAndPose::const_iterator itn = NonCorrectedSim3.find(pKFn);
if(itn!=NonCorrectedSim3.end())
Snw = itn->second;
else
Snw = vScw[pKFn->mnId];
g2o::Sim3 Sni = Snw * Swi;
g2o::EdgeSim3* en = new g2o::EdgeSim3();
en->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFn->mnId)));
en->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));
en->setMeasurement(Sni);
en->information() = matLambda;
optimizer.addEdge(en);
}
}
}
}
// Optimize!
optimizer.initializeOptimization();
optimizer.optimize(20);
unique_lock<mutex> lock(pMap->mMutexMapUpdate);
// SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1]
for(size_t i=0;i<vpKFs.size();i++)
{
KeyFrame* pKFi = vpKFs[i];
const int nIDi = pKFi->mnId;
g2o::VertexSim3Expmap* VSim3 = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(nIDi));
g2o::Sim3 CorrectedSiw = VSim3->estimate();
vCorrectedSwc[nIDi]=CorrectedSiw.inverse();
Eigen::Matrix3d eigR = CorrectedSiw.rotation().toRotationMatrix();
Eigen::Vector3d eigt = CorrectedSiw.translation();
double s = CorrectedSiw.scale();
eigt *=(1./s); //[R t/s;0 1]
cv::Mat Tiw = Converter::toCvSE3(eigR,eigt);
pKFi->SetPose(Tiw);
}
// Correct points. Transform to "non-optimized" reference keyframe pose and transform back with optimized pose
for(size_t i=0, iend=vpMPs.size(); i<iend; i++)
{
MapPoint* pMP = vpMPs[i];
if(pMP->isBad())
continue;
int nIDr;
if(pMP->mnCorrectedByKF==pCurKF->mnId)
{
nIDr = pMP->mnCorrectedReference;
}
else
{
KeyFrame* pRefKF = pMP->GetReferenceKeyFrame();
nIDr = pRefKF->mnId;
}
g2o::Sim3 Srw = vScw[nIDr];
g2o::Sim3 correctedSwr = vCorrectedSwc[nIDr];
cv::Mat P3Dw = pMP->GetWorldPos();
Eigen::Matrix<double,3,1> eigP3Dw = Converter::toVector3d(P3Dw);
Eigen::Matrix<double,3,1> eigCorrectedP3Dw = correctedSwr.map(Srw.map(eigP3Dw));
cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
pMP->SetWorldPos(cvCorrectedP3Dw);
pMP->UpdateNormalAndDepth();
}
}
5.全局BA優化GlobalBundleAdjustment
一些說明：這個是ORB-SLAM系統當中最大的優化，将全局地圖當中所有的關鍵幀和地圖點都放進來一起進行優化。對關鍵幀的位姿和地圖點3D坐标進行優化。
調用時機：LoopClosing線程當中所有工作完成後建立線程進行整體優化。函數調用關系為：RunGlobalBundleAdjustment-->Optimizer::GlobalBundleAdjustemnt-->BundleAdjustment
代碼：
RunGlobalBundleAdjustment函數：
void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF)
{
cout << "Starting Global Bundle Adjustment" << endl;
int idx = mnFullBAIdx;
Optimizer::GlobalBundleAdjustemnt(mpMap,10,&mbStopGBA,nLoopKF,false);
// Update all MapPoints and KeyFrames
// Local Mapping was active during BA, that means that there might be new keyframes
// not included in the Global BA and they are not consistent with the updated map.
// We need to propagate the correction through the spanning tree
{
unique_lock<mutex> lock(mMutexGBA);
if(idx!=mnFullBAIdx)
return;
if(!mbStopGBA)
{
cout << "Global Bundle Adjustment finished" << endl;
cout << "Updating map ..." << endl;
mpLocalMapper->RequestStop();
// Wait until Local Mapping has effectively stopped
while(!mpLocalMapper->isStopped() && !mpLocalMapper->isFinished())
{
usleep(1000);
}
// Get Map Mutex
unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
// Correct keyframes starting at map first keyframe
list<KeyFrame*> lpKFtoCheck(mpMap->mvpKeyFrameOrigins.begin(),mpMap->mvpKeyFrameOrigins.end());
while(!lpKFtoCheck.empty())
{
KeyFrame* pKF = lpKFtoCheck.front();
const set<KeyFrame*> sChilds = pKF->GetChilds();
cv::Mat Twc = pKF->GetPoseInverse();
for(set<KeyFrame*>::const_iterator sit=sChilds.begin();sit!=sChilds.end();sit++)
{
KeyFrame* pChild = *sit;
if(pChild->mnBAGlobalForKF!=nLoopKF)
{
cv::Mat Tchildc = pChild->GetPose()*Twc;
pChild->mTcwGBA = Tchildc*pKF->mTcwGBA;//*Tcorc*pKF->mTcwGBA;
pChild->mnBAGlobalForKF=nLoopKF;
}
lpKFtoCheck.push_back(pChild);
}
pKF->mTcwBefGBA = pKF->GetPose();
pKF->SetPose(pKF->mTcwGBA);
lpKFtoCheck.pop_front();
}
// Correct MapPoints
const vector<MapPoint*> vpMPs = mpMap->GetAllMapPoints();
for(size_t i=0; i<vpMPs.size(); i++)
{
MapPoint* pMP = vpMPs[i];
if(pMP->isBad())
continue;
if(pMP->mnBAGlobalForKF==nLoopKF)
{
// If optimized by Global BA, just update
pMP->SetWorldPos(pMP->mPosGBA);
}
else
{
// Update according to the correction of its reference keyframe
KeyFrame* pRefKF = pMP->GetReferenceKeyFrame();
if(pRefKF->mnBAGlobalForKF!=nLoopKF)
continue;
// Map to non-corrected camera
cv::Mat Rcw = pRefKF->mTcwBefGBA.rowRange(0,3).colRange(0,3);
cv::Mat tcw = pRefKF->mTcwBefGBA.rowRange(0,3).col(3);
cv::Mat Xc = Rcw*pMP->GetWorldPos()+tcw;
// Backproject using corrected camera
cv::Mat Twc = pRefKF->GetPoseInverse();
cv::Mat Rwc = Twc.rowRange(0,3).colRange(0,3);
cv::Mat twc = Twc.rowRange(0,3).col(3);
pMP->SetWorldPos(Rwc*Xc+twc);
}
}
mpMap->InformNewBigChange();
mpLocalMapper->Release();
cout << "Map updated!" << endl;
}
mbFinishedGBA = true;
mbRunningGBA = false;
}
}
Optimizer::GlobalBundleAdjustemnt函數：
/**
* 優化相機位姿和關鍵點坐标
* 優化中使用的幾個資料結構解釋如下：
* VertexSE3Expmap李代數位姿
* VertexSBAPointXYZ 空間點位置
* EdgeProjectXYZ2UV 投影方程邊
*
*/
void Optimizer::GlobalBundleAdjustemnt(Map* pMap, int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)
{
vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();
vector<MapPoint*> vpMP = pMap->GetAllMapPoints();
BundleAdjustment(vpKFs,vpMP,nIterations,pbStopFlag, nLoopKF, bRobust);
}
Optimizer::BundleAdjustment函數：
/**
* vpKFs 地圖中所有的關鍵幀
* vpMP 地圖中所有的MapPoint
* 将3D點投影至圖像平面優化投影誤差。
* 初始化g2o優化器，添加所有的關鍵幀位姿作為頂點，所有的地圖點作為頂點，同時對每一個地圖點擷取其觀測資訊，之後建立點-關鍵幀之間的投影關系作為優化器的邊
*/
void Optimizer::BundleAdjustment(const vector<KeyFrame *> &vpKFs, const vector<MapPoint *> &vpMP,
int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)
{
vector<bool> vbNotIncludedMP;
vbNotIncludedMP.resize(vpMP.size());
g2o::SparseOptimizer optimizer;
//第一步：建立一個線性求解器LinearSolver。指定pose次元為6, landmark次元為3
g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
//第二步：建立BlockSolver，并用上邊定義的線性求解器初始化。這裡是個稀疏矩陣塊求解器。
g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
//第三步：建立總求解器solver，并從GN, LM, DogLeg中選一個梯度下降算法，再用上述塊求解器BlockSolver初始化，這裡選擇ML算法
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
//第四步：建立終極大boss稀疏優化器（SparseOptimizer），并進行設定
optimizer.setAlgorithm(solver);//設定求解器
if(pbStopFlag)
optimizer.setForceStopFlag(pbStopFlag);
long unsigned int maxKFid = 0;
//第五步：定義圖的頂點和邊，并添加到SparseOptimizer中
// Set KeyFrame vertices 設定關鍵幀頂點并加入優化器
for(size_t i=0; i<vpKFs.size(); i++)
{
KeyFrame* pKF = vpKFs[i];
if(pKF->isBad())
continue;
//5.1 VertexSE3Expmap建立相機位姿結點，并加入優化器
g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose()));//優化的是位姿，也就是變換矩陣
vSE3->setId(pKF->mnId);
vSE3->setFixed(pKF->mnId==0);
optimizer.addVertex(vSE3);
//maxKFid用于記錄關鍵幀id的最大值
if(pKF->mnId > maxKFid)
maxKFid=pKF->mnId;
}
const float thHuber2D = sqrt(5.99);
const float thHuber3D = sqrt(7.815);
// Set MapPoint vertices
// 設定MapPoint頂點并加入優化器。周遊MapPoints
for(size_t i=0; i<vpMP.size(); i++)
{
MapPoint* pMP = vpMP[i];
if(pMP->isBad())
continue;
//5.2VertexSBAPointXYZ建立特征點空間坐标結點，并加入優化器
g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
//【1】設定待優化空間點3D位置XYZ
vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
//id為什麼是MapPoint的id加上關鍵幀的最大id還要加個1？
const int id = pMP->mnId + maxKFid+1;
//【2】設定Id号
vPoint->setId(id);
//【3】是否邊緣化（以便稀疏化求解）
vPoint->setMarginalized(true);
optimizer.addVertex(vPoint);
//擷取能夠觀察到該MapPoint的關鍵幀清單
const map<KeyFrame*,size_t> observations = pMP->GetObservations();
int nEdges = 0;
//SET EDGES
//設定邊并加入優化器。周遊每個MapPoint對應的關鍵幀，擷取該MapPoint在能看到它的關鍵幀中對應的關鍵點
for(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(); mit!=observations.end(); mit++)
{
KeyFrame* pKF = mit->first;
if(pKF->isBad() || pKF->mnId>maxKFid)
continue;
//計算邊數
nEdges++;
//mit->second為pMP這個MapPoint在pKF這個關鍵幀中對應的關鍵點的index。
const cv::KeyPoint &kpUn = pKF->mvKeysUn[mit->second];
//單目相機時pKF->mvuRight為負數
if(pKF->mvuRight[mit->second]<0)
{
Eigen::Matrix<double,2,1> obs;
//obs中存儲的是關鍵點坐标
obs << kpUn.pt.x, kpUn.pt.y;
/**
* EdgeSE3ProjectXYZ為(Point-Pose)二進制邊，既要優化MapPoints的位置，又要優化相機位姿
* class EdgeSE3ProjectXYZ: public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>
* 繼承于BaseBinaryEdge這個二進制邊模闆類，需要設定的模闆參數：
* 參數2 ：觀測值（這裡是3D點在像素坐标系下的投影坐标）的次元
* 參數Vector ：觀測值類型，piexl.x，piexl.y
* 參數VertexSBAPointXYZ：第一個頂點類型 MapPoint類型
* 參數VertexSE3Expmap ：第二個頂點類型 關鍵幀類型
* */
g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();
//【1】設定第一個頂點，注意該頂點類型與模闆參數第一個頂點類型對應
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
//【2】設定第二個頂點
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));
//【3】設定觀測值，類型與模闆參數對應
e->setMeasurement(obs);
//kpUn.octave表示該關鍵點所處的金字塔圖的第幾層
const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
//【4】設定資訊矩陣，也就是協方差矩陣
e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
if(bRobust)
{
/**
* 【5】設定魯棒核函數。
* 之是以要設定魯棒核函數是為了平衡誤差，不讓二範數的誤差增加的過快。
* 魯棒核函數裡要自己設定delta值，這個delta值是，當誤差的絕對值小于等于它的時候，誤差函數不變。
* */
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(thHuber2D);
}
//【6】設定相機内參
e->fx = pKF->fx;
e->fy = pKF->fy;
e->cx = pKF->cx;
e->cy = pKF->cy;
optimizer.addEdge(e);
}
else//雙目
{
Eigen::Matrix<double,3,1> obs;
const float kp_ur = pKF->mvuRight[mit->second];
obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
g2o::EdgeStereoSE3ProjectXYZ* e = new g2o::EdgeStereoSE3ProjectXYZ();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));
e->setMeasurement(obs);
const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
e->setInformation(Info);
if(bRobust)
{
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(thHuber3D);
}
e->fx = pKF->fx;
e->fy = pKF->fy;
e->cx = pKF->cx;
e->cy = pKF->cy;
e->bf = pKF->mbf;
optimizer.addEdge(e);
}
}
//如果圖中的邊數為0，則删除圖中的結點
if(nEdges==0)
{
optimizer.removeVertex(vPoint);
vbNotIncludedMP[i]=true;
}
else
{
vbNotIncludedMP[i]=false;
}
}
// Optimize!
//第六步：設定優化參數，開始執行優化
optimizer.initializeOptimization();
//設定疊代次數
optimizer.optimize(nIterations);
// Recover optimized data
//Keyframes
//擷取優化後keyframe的位姿
for(size_t i=0; i<vpKFs.size(); i++)
{
KeyFrame* pKF = vpKFs[i];
if(pKF->isBad())
continue;
g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
g2o::SE3Quat SE3quat = vSE3->estimate();
if(nLoopKF==0)
{
pKF->SetPose(Converter::toCvMat(SE3quat));
}
else
{
pKF->mTcwGBA.create(4,4,CV_32F);
Converter::toCvMat(SE3quat).copyTo(pKF->mTcwGBA);
pKF->mnBAGlobalForKF = nLoopKF;
}
}
//Points
//擷取優化後MapPoint的坐标
for(size_t i=0; i<vpMP.size(); i++)
{
if(vbNotIncludedMP[i])
continue;
MapPoint* pMP = vpMP[i];
if(pMP->isBad())
continue;
g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));
if(nLoopKF==0)
{
pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
pMP->UpdateNormalAndDepth();
}
else
{
pMP->mPosGBA.create(3,1,CV_32F);
Converter::toCvMat(vPoint->estimate()).copyTo(pMP->mPosGBA);
pMP->mnBAGlobalForKF = nLoopKF;
}
}
}
ORB-SLAM當中的優化，在論文的附錄中有進行解釋。
