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

  在添加每个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 = 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 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核等等。当然具体的我们也不展开细说了。

这段摘自 深入理解图优化与g2o：图优化篇

高斯牛顿法

• 牛顿法
  

  牛顿法是从泰勒公式展开得到的
  
  迭代式为:
  

上式为一维的情况，扩展到多维时牛顿法的迭代式为：

其中H为海塞矩阵，

为梯度

• 高斯牛顿法
  

  高斯牛顿法通过下面方法替代海塞矩阵：
  

• LM法
  高斯-牛顿法中为了避免发散，有两种解决方法
  1.调整下降步伐：
  2.调整下降方向：4(J^TJ+λD)Δ=JTrλ→+∞Δ/λ→J^Tr$，即方向和梯度方向一样，变成了梯度下降法。

相反，如果λ为0，就变成了高斯牛顿法。
Levenberg-Marquardt方法的好处在于可以调节:
如果下降太快，使用较小的λ，使之更接近高斯牛顿法
如果下降太慢，使用较大的λ，使之更接近梯度下降法

此外，高斯牛顿法中涉及求逆矩阵的操作， 加入λ 也可以保证该矩阵为一个正定矩阵。