g2o从入门到精通

前言

这里将我对g2o的一些常见用法，做个简单总结，便于后续温习回顾，如能对大家起到半点借鉴作用，实乃我之幸事。

一　先看两个案例 1.1 案例一：

// compute uwb pose by mutli distances using least square estimate
// fix_xy means only optimize z
EdgeUwbDistanceToPose::EdgeUwbDistanceToPose()
{

}

EdgeUwbDistanceToPose::EdgeUwbDistanceToPose(Eigen::Vector3d anchor_pose, double distance, bool fix_xy)
    : anchor_pose_ ( anchor_pose ), distance_(distance), fix_xy_(fix_xy)
{

}

void EdgeUwbDistanceToPose::computeError()
{
    const g2o::VertexSBAPointXYZ* v1  = static_cast ( _vertices[0] );

    double tag_anchor_dist =  (v1->estimate() - anchor_pose_).norm();

    _error(0) = tag_anchor_dist - distance_;
    return;
}

void EdgeUwbDistanceToPose::linearizeOplus()
{
    if ( level() == 1 )
    {
        _jacobianOplusXi = Eigen::Matrix