g2o库的应用

0. 参考

【1】https://blog.csdn.net/Hansry/article/details/100061312
【2】https://www.cnblogs.com/gaoxiang12/p/5304272.html

1. 在orbslam中的应用

1.1 全局BA

//pMap中所有的MapPoints和关键帧做bundle adjustment优化
//这个全局BA优化在ORB-SLAM2中有俩个地方有用
//a.单目初始化：CreateInitialMapMonocular 函数
//b.闭环优化：RunGlobalBundleAdjustment函数
/** 
 * @brief bundle ajustment Optimization
 * 3D-2D 最小化重投影误差：e = (u, v) - project(Tcw*Pw)
 * 1. Vertex : g2o::VertexSE3Expmap(), 即当前的Tcw
 *             g2o::VertexSBAPointXYZ(),MapPoint的mWorldPos
 * 2.Edge: 
 *    - g2o::EdgeSE3ProjectXYZ(), BaseBinaryEdge
 *         + Vertex: 待优化当前帧的Tcw
 *         + Vertex: 待优化MapPoint的mWorldPos
 *         + measurement：MapPoint在当前帧中的二维位置(u ,v )
 *         + InfoMatrix: invSigma2 (与特征点所在的尺度有关)
 * @param  vpKFs 关键帧
 *         vpMP  MapPoints
 *         nIterations 迭代次数（20次）
 *         pbStopFlag 是否强制暂停
 *         nLoopKF 关键帧的个数
 *         bRobust 是否使用该函数
 */
 void Optimizer::BundleAdjustment(const vector<KeyFrame *> &vpKFs, const vector<MapPoint *> &vpMP,
                                 int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)

1.2 单帧位姿优化

// 该优化函数主要用于Tracking线程中：运动跟踪、参考帧跟踪、地图跟踪、重定位
/**
 * @brief Pose Only Optimization
 * 3D-2D 最小二乘投影误差 e = (u, v) - project(Tcw*Pw)
 * 只优化Frame的Tcw, 不优化MapPoints的坐标
 * 1. Vertex： g2o::VertexSE3Expmap()，即当前帧的Tcw
 * 2. Edge：
 *       - g2o::EdgeSE3ProjectXYZOnlyPose(), BaseUnaryEdge
 *             + Vertex: 待优化当前帧的Tcw
 *             + measurement: MapPoint 在当前帧的二维位置（u，v）
 *             + InfoMatrix: invSigma2 (与特征点所在的尺度有关)
 *       - g2o::EdgeStereoSE3ProjectXYZOnlyPose(), BaseUnaryEdge
 *             + Vertex: 待优化当前帧的Tcw
 *             + measurement: MapPoint在当前帧的二维位置（ul，v，ur）
 *             + InfoMatrix: invSigma2 (与特征点所在的尺度有关)
 * @param pFrame Frame
 * @return inliers 数量
 * /
 int Optimizer::PoseOptimization(Frame *pFrame)

1.3 局部地图BA优化

/** 
* @brief  Local Bundle Adjustment
* 1. Vertex:
*     -g2o::VertexSE3Expmap(), LocalKeyFrames, 即当前关键帧的位姿、与当前关键帧相连的关键帧的位姿
*     -g2o::VertexSE3Expmap(), FixedCameras, 即能观测到LocalMapPoints的关键帧（并且不属于LocalKeyFrame）的位姿，在优化中这些关键帧的位姿不变
*     -g2o::VertexSBAPointXYZ(), LocalMapPoints, 即LocalKeyFrames能观测到的所有MapPoints的位姿
* 2. Edge：
*     -g2o::EdgeSE3ProjectXYZ()，BaseBinaryEdge
*      +Vertex：关键帧的Tcw, MapPoint的Pw
*      +measurement: MapPoint在关键帧中的二维位置（u,v）
*      +InfoMatrix: invSigma2 (与特征点所在的尺度有关)
*    -g2o::EdgeStereoProjectXYZ()，BaseBinaryEdge
*      +Vertex: 关键帧的Tcw, MapPoint的Pw
*      +measurement: MapPoint在关键帧中的二维位置（ul, v, ur）
*      +InfoMatrix: invSigma2 (与特征点所在的尺度有关)
*
*   @param pKF  KeyFrame
*   @param pbStopFlag 是否停止优化的标志
*   @param pMap 在优化后，更新状态时需要用到Map的互斥量mMutexMapUpdate
* /
void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap)

1.4 本质图位姿优化

/** 
* @brief 闭环检测后，Essential Graph优化
* 1. Vertex：
*      -g2o::VertexSE3ExpMap，Essential graph中关键帧的位姿
* 2. Edge：
*     -g2o::EdgeSim3(), BaseBinaryEdge
*           + Vertex: 关键帧的Tcw
*           + measurement: 经过CorrectLoop函数步骤2，Sim3传播校正后的关键帧相对位姿 ，若没有校正则用原来的位姿
*           + InfoMatrix: 单位矩阵
* @param pMap   全局地图
* @param pLoopKF  闭环匹配上的关键帧
* @param pCurKF 当前关键帧
* @param NonCorrectedSim3 未经过Sim3传播调整过的关键帧位姿
* @param CorrectedSim3 经过Sim3传播调整过的关键帧位姿
* @param LoopConnections 因闭环时MapPoints调整而新生成的边
*/
void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF,
                                       const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,
                                       const LoopClosing::KeyFrameAndPose &CorrectedSim3,
                                       const map<KeyFrame *, set<KeyFrame *> > &LoopConnections, const bool &bFixScale)

1.5 两帧间sim3优化

/** 
* @brief 形成闭环时进行Sim3优化
* 1. Vertex: 
*         - g2o::VertexSE3Expmap()，俩个关键帧的位姿
*         - g2o::VertexSBAPointXYZ()，俩个关键帧共有的MapPoints
* 2. Edge:
*         -g2o::EdgeSim3ProjectXYZ(), BaseBinaryEdge
*                 +Vertex：关键帧的Sim3, MapPoint的Pw
*                 +measurement: MapPoint在关键帧的二维位置（u,v）
*                 +InfoMatrix：InvSigma2 (与特征点所在的尺度有关)
*         -g2o::EdgeInverseSim3ProjectXYZ(), BaseBinaryEdge
*                 +Vertex: 关键帧的Sim3, MapPoint的Pw
*                 +measurement: MapPoint在关键帧中的二维位置（u, v）
*                 +InfoMatrix: invSigma2(与特征点所在的尺度有关)
* 
*  @param pKF1  KeyFrame
*  @param pKF2  KeyFrame
*  @param vpMatches1 俩个关键帧的匹配关系
* @param g2oS12 俩个关键帧间的Sim3变换
* @param th2 核函数阈值
* @param bFixScale 是否优化尺度，单目进行尺度优化，双目不进行尺度优化
*/
int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches1, g2o::Sim3 &g2oS12, const float th2, const bool bFixScale)

2. 在激光slam中的应用

先看一下下面的代码.
在这一整套的代码中，地图的（0，0，0）点，是sync后的第一针gnss位置，而gnss pose经过下面这句话的变换，到达了激光雷达的坐标原点处，

gnss_pose_ *= lidar_to_imu_;

所以，经过下面这一个指令发布出来的,/synced_gnss topic, 都是指的激光雷达原点在地图(0,0,0)坐标系下的表示。

gnss_pub_ptr_->Publish(gnss_pose_, current_gnss_data_.time);

bool BackEnd::AddNodeAndEdge(const PoseData& gnss_data) {
    Eigen::Isometry3d isometry;
    // 添加关键帧节点
    isometry.matrix() = current_key_frame_.pose.cast<double>();
    
    if (!graph_optimizer_config_.use_gnss && graph_optimizer_ptr_->GetNodeNum() == 0)
        graph_optimizer_ptr_->AddSe3Node(isometry, true);
    else
        graph_optimizer_ptr_->AddSe3Node(isometry, false);
    new_key_frame_cnt_ ++;

    // 添加激光里程计对应的边
    static KeyFrame last_key_frame = current_key_frame_;
    int node_num = graph_optimizer_ptr_->GetNodeNum();
    if (node_num > 1) {
        Eigen::Matrix4f relative_pose = last_key_frame.pose.inverse() * current_key_frame_.pose;
        isometry.matrix() = relative_pose.cast<double>();
        graph_optimizer_ptr_->AddSe3Edge(node_num-2, node_num-1, isometry, graph_optimizer_config_.odom_edge_noise);
    }
    last_key_frame = current_key_frame_;

    // 添加gnss位置对应的先验边
    if (graph_optimizer_config_.use_gnss) {
        Eigen::Vector3d xyz(static_cast<double>(gnss_data.pose(0,3)),
                            static_cast<double>(gnss_data.pose(1,3)),
                            static_cast<double>(gnss_data.pose(2,3)));
        graph_optimizer_ptr_->AddSe3PriorXYZEdge(node_num - 1, xyz, graph_optimizer_config_.gnss_noise);
        new_gnss_cnt_ ++;
    }

    return true;
}