先贴上代码:
#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