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

2019-08-27 本文已影响0人 liampayne_66d0

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);

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

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)));

观测数据需分两种情况：

单目：投影点的x、y坐标

 // 构造观测
Eigen::Matrix<double,2,1> obs;
obs << kpUn.pt.x, kpUn.pt.y;

双目：投影点的x、y坐标，以及投影点在右目中的x坐标（默认y方向上已经对齐了）

   Eigen::Matrix<double,3,1> 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<MapPoint *> &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<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id2)));
    // ? 没看懂为什么这里添加的节点的id为0？
    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)));
    // ? 这里添加的节点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);

开始迭代优化，迭代下降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 = new     g2o::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<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);
      }

连接两个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<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDj)));
          e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));
          // 根据两个Pose顶点的位姿算出相对位姿作为边，那还存在误差？优化有用？因为闭环MapPoints调整新形成的边不优化？（wubo???）
          // REVIEW 我和师兄有同样的疑问
          e->setMeasurement(Sji);

          // 信息矩阵是单位阵,说明sim3中每个自由度的贡献都是一样的,并且所有的这个边对总误差的贡献也都是一样大的
          e->information() = matLambda;

          optimizer.addEdge(e);

图优化中的鲁棒核函数

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