SLAM - 后端 - 1

g2o图优化法：

g2o_bal_class.h

#include <Eigen/Core>
#include "g2o/core/base_vertex.h"
#include "g2o/core/base_binary_edge.h"

#include "ceres/autodiff.h"

#include "tools/rotation.h"
#include "common/projection.h"
//g2o自身顶点类型：
//VertexSE2 : public BaseVertex<3, SE2>  //2D pose Vertex, (x,y,theta)
 
//VertexSE3 : public BaseVertex<6, Isometry3>  //6d vector (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion)
 
//VertexPointXY : public BaseVertex<2, Vector2>
 
//VertexPointXYZ : public BaseVertex<3, Vector3>
 
//VertexSBAPointXYZ : public BaseVertex<3, Vector3>
 
    // SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential map
 
//VertexSE3Expmap : public BaseVertex<6, SE3Quat>
 
    // SBACam Vertex, (x,y,z,qw,qx,qy,qz),(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.
 
    // qw is assumed to be positive, otherwise there is an ambiguity in qx,qy,qz as a rotation
 
//VertexCam : public BaseVertex<6, SBACam>
 
    // Sim3 Vertex, (x,y,z,qw,qx,qy,qz),7d vector,(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.
 
//VertexSim3Expmap : public BaseVertex<7, Sim3>



//重新定义顶点
//相机位姿顶点类
class VertexCameraBAL : public g2o::BaseVertex<9,Eigen::VectorXd>
焦距f，畸变系数k1 k2， 3个参数的平移，3个参数的旋转。一共九个量，9维，类型为Eigen::VectorXd
{
public:
//Eigen库为了使用SSE加速，在内存上分配了128位的指针，在生成定长的Matrix或Vector对象时，需要开辟内存，调用默认构造函数，通常x86下的指针是32位，内存位数没对齐就会导致程序运行出错。
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;//这个宏在new一个对象时会总是返回一个对齐的指针。
    VertexCameraBAL() {}//默认构造函数
    //分别是读盘、存盘函数，一般不需要读/写操作，仅仅声明一下，但是这里需要用到
    //参数分别是输入输出流类型实例的引用
    virtual bool read ( std::istream& /*is*/ )
    {
        return false;
    }

    virtual bool write ( std::ostream& /*os*/ ) const
    {
        return false;
    }
    //顶点重置函数，设定被优化变量的原始值
    virtual void setToOriginImpl() {}
    //顶点更新函数，用于优化过程中增量△x 的计算。根据增量方程算出增量后，通过此函数进行调整。
    virtual void oplusImpl ( const double* update )
    {
        //由于update是个double类型数组，而增量需要的是个矩阵
        //用update构造一个增量矩阵v，下面更新估计值时，直接将v加上就好了
        Eigen::VectorXd::ConstMapType v ( update, VertexCameraBAL::Dimension );
        //直接把增量加到估计值上
        _estimate += v;
    }

};

//landmark路标类型顶点，维度3维，类型是Eigen::Vector3d
class VertexPointBAL : public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    VertexPointBAL() {}

    virtual bool read ( std::istream& /*is*/ )
    {
        return false;
    }

    virtual bool write ( std::ostream& /*os*/ ) const
    {
        return false;
    }

    virtual void setToOriginImpl() {}

    virtual void oplusImpl ( const double* update )
    {
        //这里也是一样，将增量数组构造成增量矩阵，再将增量矩阵加到估计上
        Eigen::Vector3d::ConstMapType v ( update );
        _estimate += v;
    }
};
//BAL观测边，边即误差，继承自基础二元边。这里误差应该是重投影的像素误差
// 参数为：误差维度2维，误差类型为Eigen::Vector2d，连接两个顶点：VertexCameraBAL和VertexPointBAL(也就是说误差和这两个优化变量有关)
class EdgeObservationBAL : public g2o::BaseBinaryEdge<2, Eigen::Vector2d,VertexCameraBAL, VertexPointBAL>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeObservationBAL() {}

    virtual bool read ( std::istream& /*is*/ )
    {
        return false;
    }

    virtual bool write ( std::ostream& /*os*/ ) const
    {
        return false;
    }
    //误差计算函数
    virtual void computeError() override   // The virtual function comes from the Edge base class. Must define if you use edge.
    {
        //将第0个顶点，相机位姿取出来
        const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*> ( vertex ( 0 ) );
        //第1个顶点，空间点位置取出来
        const VertexPointBAL* point = static_cast<const VertexPointBAL*> ( vertex ( 1 ) );
        //将相机位姿估计值，空间点位姿估计值， 传给了重载的()运算符，这个重载，将计算好的结果输出到_error.data()，完成了误差的计算
        ( *this ) ( cam->estimate().data(), point->estimate().data(), _error.data() );

    }
    //这里即为重载的()函数，为模板函数，需要数据为相机位姿指针，空间点位置指针，用于承接输出误差的residuals
    // 上面调用时，用的_error.data()承接，完成误差计算
    //这个模板类其实还是用的重投影误差
    template<typename T>
    bool operator() ( const T* camera, const T* point, T* residuals ) const
    {
        //这里创建一个承接重投影像素坐标，也就是根据相机内外参和空间点坐标去投影得到的像素坐标，是估计值
        T predictions[2];
        //这个函数就是反应的投影过程，camera，point参数，然后用predictions承接算得的像素坐标。二维
        CamProjectionWithDistortion ( camera, point, predictions );
        //误差是估计值减观测值
        residuals[0] = predictions[0] - T ( measurement() ( 0 ) );
        residuals[1] = predictions[1] - T ( measurement() ( 1 ) );

        return true;
    }
//从computeError()到这里，是为了计算一个重投影误差，误差的计算被写进了重载的()中，投影过程被写进了CamProjectionWithDistortion()中


    //重写线性增量方程，也就是雅克比矩阵
    virtual void linearizeOplus() override
    {
        // 使用数值求导
        // use numeric Jacobians
        // BaseBinaryEdge<2, Vector2d, VertexCameraBAL, VertexPointBAL>::linearizeOplus();
        // return;
        // 使用ceres的自动求导，不然系统将调用g2o的数值求导
        // using autodiff from ceres. Otherwise, the system will use g2o numerical diff for Jacobians
        //将相机顶点取出，赋值给cam，这里是顶点类型指针
        const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*> ( vertex ( 0 ) );
        //将landmark顶点取出，赋值point，这里是顶点类型指针
        const VertexPointBAL* point = static_cast<const VertexPointBAL*> ( vertex ( 1 ) );
        
        //AutoDiff的定义如下，是一个模板结构体，模板参数为代价函数类型，模板类型，代价函数的各参数维度(这里就两个了，相机顶点维度，空间点维度)
        /*template <typename Functor, typename T,
                int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0,
                int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0>
        struct AutoDiff {
            static bool Differentiate(const Functor& functor,
                                      T const *const *parameters,
                                      int num_outputs,
                                      T *function_value,
                                      T **jacobians) {...}*/
 
        //这里来一个typedef,将模板类简化定义一下，定义成BalAutoDiff
        //看一下模板参数：
        //EdgeObservationBAL，就是代价函数类型，这里就是边的类型了
        //模板类型为double
        //VertexCameraBAL::Dimension和VertexPointBAL::Dimension就是对应的两个N0和N1，误差函数参数的维度，这里直接把维度取出来了(Dimension即是取得维度)，也可以直接输入9和3
        typedef ceres::internal::AutoDiff<EdgeObservationBAL, double, VertexCameraBAL::Dimension, VertexPointBAL::Dimension> BalAutoDiff;
//      typedef ceres::internal::AutoDiff<EdgeObservationBAL, double, 9, 3> BalAutoDiff;

        //这里的Dimension就是边的维度