ORBSLAM是一種基于優(yōu)化方法的SLAM方法，工程中引入了第三方庫(kù)g2o墅垮，g2o是基于圖優(yōu)化的優(yōu)化算法庫(kù)筏餐。圖優(yōu)化是將普通的優(yōu)化問題用圖的方式(變量用節(jié)點(diǎn)表示鸿秆，關(guān)系用邊來(lái)表示)來(lái)表示。
-
void Optimizer::BundleAdjustment
3D-2D BA帚稠，在GlobalBundleAdjustemnt中調(diào)用谣旁，計(jì)算量比較大
向優(yōu)化器添加關(guān)鍵幀位姿頂點(diǎn)g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap(); vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose())); vSE3->setId(pKF->mnId); vSE3->setFixed(pKF->mnId==0); optimizer.addVertex(vSE3);向優(yōu)化器添加MapPoints頂點(diǎn)
g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ(); vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos())); const int id = pMP->mnId+maxKFid+1;//避免和位姿頂點(diǎn)ID重復(fù) vPoint->setId(id); vPoint->setMarginalized(true); optimizer.addVertex(vPoint);在添加每個(gè)MapPoints頂點(diǎn)時(shí)遍歷觀察到當(dāng)前地圖點(diǎn)的所有關(guān)鍵幀，向優(yōu) 化器中添加邊滋早。這條邊連著3D地圖點(diǎn)和位姿榄审，由于觀測(cè)數(shù)據(jù)格式不同，所 以這里要分成單目和雙目?jī)煞N情況添加邊杆麸。
創(chuàng)建邊并填充數(shù)據(jù)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(pKF->mnId)));觀測(cè)數(shù)據(jù)需分兩種情況：
- 單目：投影點(diǎn)的x搁进、y坐標(biāo)
// 構(gòu)造觀測(cè) Eigen::Matrix<double,2,1> obs; obs << kpUn.pt.x, kpUn.pt.y;- 雙目：投影點(diǎn)的x、y坐標(biāo)昔头，以及投影點(diǎn)在右目中的x坐標(biāo)（默認(rèn)y方向上已經(jīng)對(duì)齊了）
Eigen::Matrix<double,3,1> obs; const float kp_ur = pKF->mvuRight[mit->second]; obs << kpUn.pt.x, kpUn.pt.y, kp_ur;頂點(diǎn)和邊設(shè)置完后進(jìn)行優(yōu)化饼问，獲取優(yōu)化后的位姿和地圖點(diǎn)進(jìn)行保存。
-
int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches1, g2o::Sim3 &g2oS12, const float th2, const bool bFixScale)
形成閉環(huán)時(shí)進(jìn)行Sim3優(yōu)化揭斧，優(yōu)化目標(biāo)是是兩關(guān)鍵幀之間的相似變換矩陣- 設(shè)置優(yōu)化器算法
- 將變量（當(dāng)前待優(yōu)化幀的初始位姿）作為非固定節(jié)點(diǎn)添加進(jìn)入圖優(yōu)化
- 將變量（兩關(guān)鍵幀的地圖點(diǎn)）作為固定節(jié)點(diǎn)添加進(jìn)入圖優(yōu)化
- 添加邊將兩關(guān)鍵幀地圖點(diǎn)和待優(yōu)化幀的位姿連接莱革，每一個(gè)位姿對(duì)應(yīng)兩條邊，從關(guān)鍵幀1像素坐標(biāo)映射到關(guān)鍵幀2像素坐標(biāo)讹开，從關(guān)鍵幀2像素坐標(biāo)映射到關(guān)鍵幀1像素坐標(biāo)盅视。
// step 2.3 添加兩個(gè)頂點(diǎn)（3D點(diǎn)）到相機(jī)投影的邊 -- 投影到當(dāng)前關(guān)鍵幀 -- 正向投影 g2o::EdgeSim3ProjectXYZ* e12 = new g2o::EdgeSim3ProjectXYZ(); e12->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id2))); // ? 沒看懂為什么這里添加的節(jié)點(diǎn)的id為0？ e12->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0))); e12->setMeasurement(obs1); // 信息矩陣也和這個(gè)點(diǎn)的可靠程度（在圖像金字塔中的圖層）有關(guān) const float &invSigmaSquare1 = pKF1->mvInvLevelSigma2[kpUn1.octave]; e12->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare1); // 使用魯棒核函數(shù) g2o::RobustKernelHuber* rk1 = new g2o::RobustKernelHuber; e12->setRobustKernel(rk1); rk1->setDelta(deltaHuber); optimizer.addEdge(e12); // Set edge x2 = S21*X1 // 接下來(lái)是添加投影到 閉環(huán)關(guān)鍵幀 -- 反向投影 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))); // ? 這里添加的節(jié)點(diǎn)id也為0 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);- 開始迭代優(yōu)化萧吠，迭代下降5次左冬，然后根據(jù)當(dāng)前優(yōu)化結(jié)果位姿篩選內(nèi)點(diǎn)，用內(nèi)點(diǎn)重新進(jìn)行迭代下降優(yōu)化纸型，優(yōu)化結(jié)果為計(jì)算得到的兩關(guān)鍵幀之間的相似變換矩陣
-
void Optimizer::LocalBundleAdjustment(KeyFrame pKF, bool pbStopFlag, Map* pMap)
局部BA優(yōu)化（tracking線程）,得到當(dāng)前幀連接的KF和這些 KF中的地圖點(diǎn)后（lLocalKeyFrames和lLocalMapPoints）拇砰，和能觀測(cè)到這些地圖點(diǎn)但沒有和當(dāng)前幀相連的KF（lFixedCameras）。- 將變量（當(dāng)前關(guān)鍵幀及其共視關(guān)鍵幀的初始位姿）作為非固定（初始幀固定）節(jié)點(diǎn)添加進(jìn)入圖優(yōu)化狰腌，這里如果幀的id是0(第一幀)除破，則固定:
vSE3->setFixed(pKFi->mnId==0);//第一幀位置固定固定的原因應(yīng)該是，如果第一幀不固定琼腔，優(yōu)化后的結(jié)果乘上一個(gè)變換瑰枫，同樣是最優(yōu)解，也就是說(shuō)如果第一幀不固定，會(huì)有很多很多可能的解
- 將變量（觀察到當(dāng)前關(guān)鍵幀及其共視關(guān)鍵幀中地圖點(diǎn)的關(guān)鍵幀初始位姿）作為固定節(jié)點(diǎn)添加進(jìn)入圖優(yōu)化
- 將變量（當(dāng)前關(guān)鍵幀及其共視關(guān)鍵幀的地圖點(diǎn)）作為非固定節(jié)點(diǎn)添加進(jìn)入圖優(yōu)化
- 在添加每個(gè)地圖點(diǎn)時(shí)遍歷關(guān)鍵幀光坝，創(chuàng)建邊并填充數(shù)據(jù)（和BA一樣）
- 開始迭代優(yōu)化尸诽，迭代下降5次，然后根據(jù)當(dāng)前優(yōu)化結(jié)果位姿篩選內(nèi)點(diǎn)盯另，用內(nèi)點(diǎn)重新進(jìn)行迭代下降優(yōu)化性含。
- 恢復(fù)優(yōu)化后的各關(guān)鍵幀位姿和關(guān)鍵幀地圖點(diǎn)。
-
int Optimizer::PoseOptimization(Frame *pFrame)
誤差函數(shù)為：
用于Tracking中勻速運(yùn)動(dòng)模型跟蹤等鸳惯，這個(gè)優(yōu)化中只優(yōu)化Frame的Tcw商蕴，不優(yōu)化MapPoints的坐標(biāo),所以在構(gòu)造圖優(yōu)化的時(shí)候，是構(gòu)造的一元邊芝发。觀測(cè)是2維的Vector2d數(shù)據(jù)绪商，即像素坐標(biāo)。
添加當(dāng)前位姿頂點(diǎn)辅鲸，遍歷地圖點(diǎn)添加邊和觀測(cè)格郁，這里也是分成單目和雙目?jī)煞N情況，因?yàn)橛^測(cè)數(shù)據(jù)不同以及定義的邊的類型不同：- 單目：
g2o::EdgeSE3ProjectXYZOnlyPose* e = new g2o::EdgeSE3ProjectXYZOnlyPose();- 雙目：
g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = new g2o::EdgeStereoSE3ProjectXYZOnlyPose(); -
Optimizer::OptimizeEssentialGraph
閉環(huán)檢測(cè)后的優(yōu)化
首先拿到地圖中所有KF和地圖點(diǎn)瓢湃，聲明vScw和vCorrectedSwc理张，分別代表未sim3優(yōu)化前的sim3位姿和優(yōu)化后的。添加頂點(diǎn)時(shí)有兩種情況：如果該關(guān)鍵幀在閉環(huán)時(shí)通過(guò)Sim3傳播調(diào)整過(guò)绵患，用校正后的位姿雾叭；如果該關(guān)鍵幀在閉環(huán)時(shí)沒有通過(guò)Sim3傳播調(diào)整過(guò)，用自身的位姿
if(it!=CorrectedSim3.end()) { 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); }連接兩個(gè)Sim3節(jié)點(diǎn)的二元邊落蝙，頂點(diǎn)0的位姿是Tiw,1的位姿是Tjw织狐，那么邊的測(cè)量是Tji，即from i to j筏勒。
const g2o::Sim3 Sjw = vScw[nIDj]; // 得到兩個(gè)pose間的Sim3變換 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))); // 根據(jù)兩個(gè)Pose頂點(diǎn)的位姿算出相對(duì)位姿作為邊移迫，那還存在誤差？?jī)?yōu)化有用管行？因?yàn)殚]環(huán)MapPoints調(diào)整新形成的邊不優(yōu)化厨埋？（wubo???） // REVIEW 我和師兄有同樣的疑問 e->setMeasurement(Sji); // 信息矩陣是單位陣,說(shuō)明sim3中每個(gè)自由度的貢獻(xiàn)都是一樣的,并且所有的這個(gè)邊對(duì)總誤差的貢獻(xiàn)也都是一樣大的 e->information() = matLambda; optimizer.addEdge(e);
圖優(yōu)化中的魯棒核函數(shù)
SLAM中可能給出錯(cuò)誤的邊。SLAM中的數(shù)據(jù)關(guān)聯(lián)讓科學(xué)家頭疼了很長(zhǎng)時(shí)間捐顷。出于變化荡陷、噪聲等原因，機(jī)器人并不能確定它看到的某個(gè)路標(biāo)迅涮，如果出現(xiàn)錯(cuò)誤废赞，就會(huì)出現(xiàn)一條誤差很大的邊，然后試圖調(diào)整這條邊所連接的節(jié)點(diǎn)的估計(jì)值叮姑，使它們順應(yīng)這條邊的無(wú)理要求唉地。由于這個(gè)邊的誤差真的很大，往往會(huì)抹平了其他正確邊的影響，使優(yōu)化算法專注于調(diào)整一個(gè)錯(cuò)誤的值耘沼。
核函數(shù)作用就是保證每條邊的誤差不會(huì)大的沒邊极颓，掩蓋掉其他的邊。具體的方式是群嗤，把原先誤差的二范數(shù)度量讼昆，替換成一個(gè)增長(zhǎng)沒有那么快的函數(shù)，同時(shí)保證自己的光滑性質(zhì)（不然沒法求導(dǎo)吧铡！）闰围。因?yàn)樗鼈兪沟谜麄€(gè)優(yōu)化結(jié)果更為魯棒赃绊，所以又叫它們?yōu)閞obust kernel（魯棒核函數(shù)）。很多魯棒核函數(shù)都是分段函數(shù)羡榴，在輸入較大時(shí)給出線性的增長(zhǎng)速率碧查，例如cauchy核，huber核等等校仑。當(dāng)然具體的我們也不展開細(xì)說(shuō)了忠售。
高斯牛頓法
-
牛頓法
牛頓法是從泰勒公式展開得到的
迭代式為:
上式為一維的情況，擴(kuò)展到多維時(shí)牛頓法的迭代式為：
-
高斯牛頓法
高斯牛頓法通過(guò)下面方法替代海塞矩陣：
- LM法
高斯-牛頓法中為了避免發(fā)散稻扬，有兩種解決方法
1.調(diào)整下降步伐：
2.調(diào)整下降方向：4(JTJ+λD)Δ=JTrλ→+∞
Δ/λ→J^Tr$，即方向和梯度方向一樣羊瘩，變成了梯度下降法泰佳。
相反，如果λ為0尘吗，就變成了高斯牛頓法逝她。
Levenberg-Marquardt方法的好處在于可以調(diào)節(jié):
如果下降太快，使用較小的λ睬捶，使之更接近高斯牛頓法
如果下降太慢黔宛，使用較大的λ，使之更接近梯度下降法
此外擒贸，高斯牛頓法中涉及求逆矩陣的操作臀晃， 加入λ 也可以保證該矩陣為一個(gè)正定矩陣。
