第十章g2oBA的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"//定义相机位姿顶点类，由于相机内参也作为优化变量，所以包含了：//焦距f，畸变系数k1 k2， 3个参数的平移，3个参数的旋转。一共九个量，9维，类型为Eigen::VectorXdclass VertexCameraBAL : public g2o::BaseVertex<9,Eigen::VectorXd>{public:    //这个玩意每处都有，具体啥用不清楚    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;    //默认构造函数    VertexCameraBAL() {}    //这里的读写功能函数就需要用了，参数分别是输入输出流类型实例的引用    virtual bool read ( std::istream& /*is*/ ) { return false; }    virtual bool write ( std::ostream& /*os*/ ) const { return false; }    //设定顶点的初始值    virtual void setToOriginImpl() {}    //增量函数，增量为传进的参数update，这里是9个double值，所以就是double类型指针了（其实也就是数组）    virtual void oplusImpl ( const double* update )    {        //由于update是个double类型数组，而增量需要的是个矩阵，        //所以用update构造一个增量矩阵v，下面更新估计值时，直接将v加上就好了。        //关于ConstMapType，单开一贴说。        Eigen::VectorXd::ConstMapType v ( update, VertexCameraBAL::Dimension );        //直接把增量加到估计值上        _estimate += v;    }};//landmark类型顶点，维度3维，类型是Eigen::Vector3dclass 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中        residuals[0] = predictions[0] - T ( measurement() ( 0 ) );        residuals[1] = predictions[1] - T ( measurement() ( 1 ) );        //这里一直没明白为什么要返回bool值，表征计算成功？        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,不过具体这个double指什么，还不大清楚，基本数据的类型？        //VertexCameraBAL::Dimension和VertexPointBAL::Dimension就是对应的两个N0和N1，误差函数参数的维度，这里直接把维度取出来了(Dimension即是取得维度)，也可以直接输入9和3        typedef ceres::internal::AutoDiff<EdgeObservationBAL, double, VertexCameraBAL::Dimension, VertexPointBAL::Dimension> BalAutoDiff;        //这里的Dimension就是边的维度(这里还是在边类定义中的linearizeOplus()函数定义)。定义如下，可知Dimension为2维。        // static const int Dimension = BaseEdge<D, E>::Dimension;        //定义一个行优先的double类型矩阵，大小为Dimension*VertexCameraBAL::Dimension,也就是2*9。这里就是误差对相机的导数        Eigen::Matrix<double, Dimension, VertexCameraBAL::Dimension, Eigen::RowMajor> dError_dCamera;        //定义一个行优先的double类型矩阵，大小为Dimension*VertexPointBAL::Dimension,也就是2*3。这里就是误差对空间点的导数        Eigen::Matrix<double, Dimension, VertexPointBAL::Dimension, Eigen::RowMajor> dError_dPoint;        //double*类型的数组，成员为double*,这里装了相机估计值数组指针和空间点估计值数组指针。        double * parameters[] = { const_cast<double*> ( cam->estimate().data() ), const_cast<double*> ( point->estimate().data() ) };        //雅克比矩阵为两块导数拼合起来的，一块是误差对相机的导数，一块是误差对空间点的导数。也就是上方定义的2*9的dError_dCamera和2*3的dError_dPoint        double * jacobians[] = { dError_dCamera.data(), dError_dPoint.data() };        //创建一个double类型的value数组，大小为Dimension，2个元素。干啥的？？        double value[Dimension];        //这里就是一直所说的利用ceres的现行求导，这个Differentiate()就是在AutoDiff结构体中定义的。        /*static bool Differentiate(const Functor& functor,                                  T const *const *parameters,                                  int num_outputs,                                  T *function_value,                                  T **jacobians) {...}*/        //看一下参数：        //const Functor& functor，代价函数，这里也就是这个边类了，直接用*this        //T const *const *parameters，参数列表，就是上面定义的有两个double指针的parameters数组，这两个指针一个指向相机参数数组，一个指向空间点数组        //int num_outputs，输出的维度，这里就是边的维度Dimension，也就是2维        //T *function_value，误差函数functor的输出值，用于承接functor的输出，也就是*this计算出来的误差。        //T **jacobians，这就是最终要求的雅克比矩阵了。用于承接。        bool diffState = BalAutoDiff::Differentiate ( *this, parameters, Dimension, value, jacobians );        //复制一下雅克比矩阵，将行优先转换为列优先。为什么？行列优先有啥区别        // copy over the Jacobians (convert row-major -> column-major)        //雅克比矩阵到这里就计算完成了，最后就是赋值给_jacobianOplusXi和_jacobianOplusXj了。        //防呆用的，判断一下是够计算成功        if ( diffState )        {            //成功了，将值赋过去            _jacobianOplusXi = dError_dCamera;            _jacobianOplusXj = dError_dPoint;        }        else        {            //不成功就输出警告，并将雅克比矩阵置为0矩阵。            assert ( 0 && "Error while differentiating" );            _jacobianOplusXi.setZero();            _jacobianOplusXi.setZero();        }    }};

直观来看这个头文件就是将g2o中的顶点类和边类的定义抽了出来。这里总结一下顶点和边的定义。

整个头文件定义了三个类：

1、9维的包含相机内外参的顶点：
class VertexCameraBAL : public g2o::BaseVertex<9,Eigen::VectorXd>

class VertexCameraBAL : public g2o::BaseVertex<9,Eigen::VectorXd>

2、3维的空间点类型顶点：

class VertexPointBAL : public g2o::BaseVertex<3, Eigen::Vector3d>

3、重投影二元边，2维像素坐标，连接上方的两种顶点

class EdgeObservationBAL : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, VertexCameraBAL, VertexPointBAL>

再来看每个类中需要定义的东西。

首先就是每个类中都要有一句：
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

顶点类中：
1、构造函数。
这里分别是：
VertexCameraBAL() {}；和
VertexPointBAL() {}

2、输入输出函数：
virtual bool read ( std::istream& /is/ ) { return false; }
virtual bool write ( std::ostream& /os/ ) const { return false; }

3、顶点初始值设定函数：setToOriginImpl()
两个都是默认的：
virtual void setToOriginImpl() {}

4、增量添加方式函数：virtual void oplusImpl ( ){}。
这个函数就是定义增量如何加到原来的估计值上的。函数参数是增量。要的结果是：
如何将增量加到旧的_estimate上，得到新的_estimate。这里就是直接加：_estimate += v;

边：
1、类构造函数：
EdgeObservationBAL() {}

2、输入输出函数：
virtual bool read ( std::istream& /is/ ) { return false; }
virtual bool write ( std::ostream& /os/ ) const { return false; }

3、误差计算函数：computeError()
这个函数就是定义误差的计算方式，需要的参数就是一些顶点，定义一下如何由各个顶点计算出误差。比如这里就是重投影误差，需要知道相机内外参，空间点位置。
这个函数最后的结果就是要计算出_error，这里的_error类型是ErrorVector，而ErrorVector由typedef而来：

typedef Eigen::Matrix <double, D, 1, Eigen::ColMajor> ErrorVector;

由此可见，这里_error是一个D*1的double类型Matrix，也就是个向量。

4、线性增量方程：linearizeOplus()
也就是求雅克比矩阵，之前用的都是公式计算好了，直接对雅克比矩阵各个元素赋值。这里是利用了ceres库中的AutoDiff去计算的。不管怎么搞，要的结果就是把雅克比矩阵_jacobianOplusXi和_jacobianOplusXj定义出来。
具体看一下，说白了这个函数就是求得误差关于优化变量的导数。由于这里误差跟两个优化变量有关，所以是二元边，所以求导就是两个优化变量的偏导，也就是_jacobianOplusXi和_jacobianOplusXj了。相应的如果是一元边，那就只有_jacobianOplusXi，三元边那就再来个_jacobianOplusXk。