ORB-SLAM2代码阅读笔记（优化） ORB-SLAM2代码阅读笔记：优化

ORBSLAM是一种基于优化方法的SLAM方法，工程中引入了第三方库g2o，g2o是基于图优化的优化算法库。图优化是将普通的优化问题用图的方式(变量用节点表示，关系用边来表示)来表示。

void Optimizer::BundleAdjustment
3D-2D BA，在GlobalBundleAdjustemnt中调用，计算量比较大
向优化器添加关键帧位姿顶点

g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap(); vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose())); vSE3->setId(pKF->mnId); vSE3->setFixed(pKF->mnId==0); optimizer.addVertex(vSE3);

向优化器添加MapPoints顶点

g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ(); vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos())); const int id = pMP->mnId+maxKFid+1; //避免和位姿顶点ID重复 vPoint->setId(id); vPoint->setMarginalized(true); optimizer.addVertex(vPoint);

【ORB-SLAM2代码阅读笔记（优化）】在添加每个MapPoints顶点时遍历观察到当前地图点的所有关键帧，向优化器中添加边。这条边连着3D地图点和位姿，由于观测数据格式不同，所以这里要分成单目和双目两种情况添加边。
创建边并填充数据

g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ(); e->setVertex(0, dynamic_cast(optimizer.vertex(id))); e->setVertex(1, dynamic_cast(optimizer.vertex(pKF->mnId)));

观测数据需分两种情况：
- 单目：投影点的x、y坐标
// 构造观测 Eigen::Matrix obs; obs << kpUn.pt.x, kpUn.pt.y;
- 双目：投影点的x、y坐标，以及投影点在右目中的x坐标（默认y方向上已经对齐了）
Eigen::Matrix obs; const float kp_ur = pKF->mvuRight[mit->second]; obs << kpUn.pt.x, kpUn.pt.y, kp_ur;

顶点和边设置完后进行优化，获取优化后的位姿和地图点进行保存。
int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector
&vpMatches1, g2o::Sim3 &g2oS12, const float th2, const bool bFixScale)
形成闭环时进行Sim3优化，优化目标是是两关键帧之间的相似变换矩阵
- 设置优化器算法
- 将变量（当前待优化帧的初始位姿）作为非固定节点添加进入图优化
- 将变量（两关键帧的地图点）作为固定节点添加进入图优化
- 添加边将两关键帧地图点和待优化帧的位姿连接，每一个位姿对应两条边，从关键帧1像素坐标映射到关键帧2像素坐标，从关键帧2像素坐标映射到关键帧1像素坐标。
// step 2.3 添加两个顶点（3D点）到相机投影的边 -- 投影到当前关键帧 -- 正向投影 g2o::EdgeSim3ProjectXYZ* e12 = new g2o::EdgeSim3ProjectXYZ(); e12->setVertex(0, dynamic_cast(optimizer.vertex(id2))); // ? 没看懂为什么这里添加的节点的id为0？ e12->setVertex(1, dynamic_cast(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 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(optimizer.vertex(id1))); // ? 这里添加的节点id也为0 e21->setVertex(1, dynamic_cast(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);
- 开始迭代优化，迭代下降5次，然后根据当前优化结果位姿筛选内点，用内点重新进行迭代下降优化，优化结果为计算得到的两关键帧之间的相似变换矩阵
void Optimizer::LocalBundleAdjustment(KeyFrame pKF, bool pbStopFlag, Map* pMap)
局部BA优化（tracking线程）,得到当前帧连接的KF和这些 KF中的地图点后（lLocalKeyFrames和lLocalMapPoints），和能观测到这些地图点但没有和当前帧相连的KF（lFixedCameras）。
- 将变量（当前关键帧及其共视关键帧的初始位姿）作为非固定（初始帧固定）节点添加进入图优化，这里如果帧的id是0(第一帧)，则固定:
vSE3->setFixed(pKFi->mnId==0); //第一帧位置固定

固定的原因应该是，如果第一帧不固定，优化后的结果乘上一个变换，同样是最优解，也就是说如果第一帧不固定，会有很多很多可能的解
- 将变量（观察到当前关键帧及其共视关键帧中地图点的关键帧初始位姿）作为固定节点添加进入图优化
- 将变量（当前关键帧及其共视关键帧的地图点）作为非固定节点添加进入图优化
- 在添加每个地图点时遍历关键帧，创建边并填充数据（和BA一样）
- 开始迭代优化，迭代下降5次，然后根据当前优化结果位姿筛选内点，用内点重新进行迭代下降优化。
- 恢复优化后的各关键帧位姿和关键帧地图点。
int Optimizer::PoseOptimization(Frame *pFrame)
用于Tracking中匀速运动模型跟踪等，这个优化中只优化Frame的Tcw，不优化MapPoints的坐标,所以在构造图优化的时候，是构造的一元边。观测是2维的Vector2d数据，即像素坐标。

误差函数为：
文章图片

添加当前位姿顶点，遍历地图点添加边和观测，这里也是分成单目和双目两种情况，因为观测数据不同以及定义的边的类型不同：
- 单目：
g2o::EdgeSE3ProjectXYZOnlyPose* e = newg2o::EdgeSE3ProjectXYZOnlyPose();
- 双目：
g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = new g2o::EdgeStereoSE3ProjectXYZOnlyPose();
Optimizer::OptimizeEssentialGraph
闭环检测后的优化
首先拿到地图中所有KF和地图点，声明vScw和vCorrectedSwc，分别代表未sim3优化前的sim3位姿和优化后的。
添加顶点时有两种情况：如果该关键帧在闭环时通过Sim3传播调整过，用校正后的位姿；如果该关键帧在闭环时没有通过Sim3传播调整过，用自身的位姿

if(it!=CorrectedSim3.end()) { vScw[nIDi] = it->second; VSim3->setEstimate(it->second); } else { Eigen::Matrix Rcw = Converter::toMatrix3d(pKF->GetRotation()); Eigen::Matrix tcw = Converter::toVector3d(pKF->GetTranslation()); g2o::Sim3 Siw(Rcw,tcw,1.0); vScw[nIDi] = Siw; VSim3->setEstimate(Siw); }

连接两个Sim3节点的二元边，顶点0的位姿是Tiw,1的位姿是Tjw，那么边的测量是Tji，即from i to j。

const g2o::Sim3 Sjw = vScw[nIDj]; // 得到两个pose间的Sim3变换 const g2o::Sim3 Sji = Sjw * Swi; g2o::EdgeSim3* e = new g2o::EdgeSim3(); e->setVertex(1, dynamic_cast(optimizer.vertex(nIDj))); e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); // 根据两个Pose顶点的位姿算出相对位姿作为边，那还存在误差？优化有用？因为闭环MapPoints调整新形成的边不优化？（wubo???） // REVIEW 我和师兄有同样的疑问 e->setMeasurement(Sji); // 信息矩阵是单位阵,说明sim3中每个自由度的贡献都是一样的,并且所有的这个边对总误差的贡献也都是一样大的 e->information() = matLambda; optimizer.addEdge(e);

图优化中的鲁棒核函数
SLAM中可能给出错误的边。SLAM中的数据关联让科学家头疼了很长时间。出于变化、噪声等原因，机器人并不能确定它看到的某个路标，如果出现错误，就会出现一条误差很大的边，然后试图调整这条边所连接的节点的估计值，使它们顺应这条边的无理要求。由于这个边的误差真的很大，往往会抹平了其他正确边的影响，使优化算法专注于调整一个错误的值。
核函数作用就是保证每条边的误差不会大的没边，掩盖掉其他的边。具体的方式是，把原先误差的二范数度量，替换成一个增长没有那么快的函数，同时保证自己的光滑性质（不然没法求导啊！）。因为它们使得整个优化结果更为鲁棒，所以又叫它们为robust kernel（鲁棒核函数）。很多鲁棒核函数都是分段函数，在输入较大时给出线性的增长速率，例如cauchy核，huber核等等。当然具体的我们也不展开细说了。