第十章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::VectorXd
class 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::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 setToOrigi