ORB_SLAM中Optimizer的优化分析及g2o点和边在ORB中的应用

• 1.Optimizer::PoseOptimization
  • EdgeSE3ProjectXYZOnlyPose
• 2.Optimizer::BundleAdjustment
  • EdgeSE3ProjectXYZ
• 3.Optimizer::LocalBundleAdjustment
• 4.Optimizer::OptimizeEssentialGraph
  • EdgeSim3
• 5.Optimizer::OptimizeSim3
  • EdgeSim3ProjectXYZ
• 6.ORB LoopClosing
• 7.Sim3

Optimizer::PoseOptimization

用于Tracking中匀速运动模型跟踪等

EdgeSE3ProjectXYZOnlyPose

class  EdgeSE3ProjectXYZOnlyPose: public  BaseUnaryEdge<2, Vector2d, VertexSE3Expmap>

测量值是2维的Vector2d数据，即像素坐标，这条边连接着pose节点，是一元Unary边，只优化SE3的顶点变量，地图点XYZ固定，世界坐标系下的地图点是成员变量Xw，在Optimizer::PoseOptimization(Frame *pFrame)中遍历当前帧可视的地图点调用，一个顶点N条边。
（一元边里面只有_jacobianOplusXi ， 二元边BaseBinaryEdge有_jacobianOplusXi和_jacobianOplusXj，这个比较好理解）
$$e=(u,v)-project(Tcw\ast Pw)$$
$$Tcw=CameraPose$$

观测二维，所以残差也是二维，即当前帧的像素观测和地图点在当前帧的投影位置的残差，从而优化位姿。残差对位姿的求导所以雅克比维度是2X6。

void EdgeSE3ProjectXYZOnlyPose::linearizeOplus() {
  VertexSE3Expmap * vi = static_cast<VertexSE3Expmap *>(_vertices[0]);
  Vector3d xyz_trans = vi->estimate().map(Xw);

  double x = xyz_trans[0];
  double y = xyz_trans[1];
  double invz = 1.0/xyz_trans[2];
  double invz_2 = invz*invz;

  _jacobianOplusXi(0,0) =  x