slam优化库，优化方法，G2o Ceres的学习

ch6 - ceres、g2o等

前言

年底我会总结和精简的。 G2o和Ceres 也是希望大家掌握自动求导和解析解求解的方法（g2o：单边、双边、多边 ； Ceres：优化变量李群李代数的不同表示、残差块中自动求导的模板类和解析解雅克比的推导等）希望看完本次内容，能对大家有所帮助。
前言部分，大家跳过即可。只是一个人的自言自语而已。
建议小伙伴们，有精力和时间的一定一定去看官方教程或者查询官方的手册、相应函数之类的。 Eigen Opencv PCL Ceres等等库都一样，wiki这个百科工具也很棒。
一些代码的注释也建议初学者多看看，可以copy到你的代码IDE中去看，那样注释可能明显些

1. 这些部分也是我之前学习和总结下来的，由于精力和时间原因，没怎么排版直接导入了。可能会发现很乱，排版很烂，也正常，毕竟这是我的学习过程，一致断断续续的增删改查。 【在这里，建议大家去看Ceres官方指导手册 ；g2o的话有点基础，直接阅读源代码即可，理清脉络和框架，遇啥跳转进去看就行。 后面也提供了一些例程，集合视觉slam十四讲的，等等】
2. 第二个问题：为啥自己感觉写的烂还上传上来呢？不觉得“丢人现眼”吗？ 针对这些或者这种问题，就要谈及为啥上传的初衷了。受到业内大牛，大佬们的教导，也受到他们的思想“熏陶”：知识是用来分享的。哪怕目前我的能力不足，写的很烂，但我想分享一下我的学习经历和遇到坑之类的，即便目前的我的能力不足以反补这个行业，这说明我仍需要学习。【引用前人所说的一句话，读的书越多，会发现自己越无知】当我对某一方面学了个大概的时候，就会沾沾自喜，但在看到大佬或者小伙伴的学习感悟，我会发现我仍有“很大上升空间”。现在也在继续看相关方向的数据，补充知识，学无止境吧。
   跑偏了，目的也就两点：1.分享自己的学习经验，收到他人指导自己的同时，能对他人提高微小帮助，哪怕避一个坑也是可以的 ； 2.算是记录下自己的近期所学吧，后面有余力会精简过程，保留精华，去其糟粕。
3. 我目前仍是一名学生，且今年也是找工作的一年，有很多我的学习经历和坑之类的想要分享，但还是时间和精力。 【挖个坑把】在2022.10月底，等工作找完之后，会总结《视觉slam十四讲》的内容和课后题(后面有第七章的部分答案)、VIO slam 理论和代码的学习(基于VINS)、激光slam(LOAM A-LOAM F-LOAM LeGo-LOAM )以及LIO-SAM、LVI-SAM等开源代码论文、理论内容、代码的学习。 这些大部分我都保存了零零散散的笔记和思维框图、流程图等，需要去整合，精简。【这个坑很大】
4. 为啥时间和精力不足呢？我又不是工作党，学生时间还紧缺吗？我搞过车赛和电赛，调过车的小伙伴应该也有感触，为了一个好些的参数和方法，需要调个个把月。 我目前的阶段就处于既需要补充和总结理论、又需要去完成项目内容、还需要准备工作需要的刷题之类的。那我之前干啥了？为啥不在前面就完成这些任务？前面的我刚入门，不，刚开始学SLAM，这还多亏有师兄带我视觉slam，不然我现在可能对很多东西都不熟悉。只能说一个阶段有一个阶段的学习过程，我所做的就是尽量提高效率，压缩过程所花费的时间的同时保证质量。
5. 一扯就扯远了，为啥会这样？可能跟我身边环境相关，我目标是多传感器slam定位，身边都是深度学习DL或者医疗，机械合作的，与我一样的没有。确实，没个讨论的。 但想要个和你能力相当，且能方向一样，一块学习的也很难。幸运的是，我现在需要学习的内容很多，没有太多时间去愁这愁那的。 每当我学习到一个新的阶段，就发现后面还有很多需要学习的内容和知识。当然选择当前阶段最适合我的是肯定的，很多理论非常复杂，可以之后在学或者知道结论即可。

1.G2o

​ 图优化，是把优化问题表现成图的一种方式，这里的图是图论意义上的图。一个图由若干个顶点,以及连着这些顶点的边组成。在这里，我们用顶点表示优化变量，而用边表示误差项。

1.1 代码 ：【G2o: exp(ax^2+bx+c)】

1. 构建图优化，先设定g2o - 维度、求解器类型、优化选择方法-GN、LM、Dogleg等、创建图优化的核心-稀疏优化器（SparseOptimizer optimizer）

   【定义图的顶点和边，并添加到SparseOptimizer中 】

2. 添加顶点： (顶点的类型需要自己定义)；setEstimate setId等

3. 添加边：(边同理，也需要自己定义类型-误差-雅克比求解等)；setId、setVertex、setMeasurement、setInformation等

4. 执行优化：
   optimizer.initializeOptimization(); //先初始化
   optimizer.optimize(次数);

5. 输出最优值： 也即是顶点

1.2 理论-原理方面：

Ceres库向通用的最小二乘问题的求解，定义优化问题，设置一些选项，可通过Ceres求解。

G2O在数学上主要分为四个求解步骤：

1.3 G2O常见函数分析

如下图所示，这个图反应了上述的前五个步骤

附上自己在论文流程图上的写写画画，丑图勿喷。
节点和边的部分，殊途同归，自己理解有啥用即可，图中的部分有错误，仅供参考。
用g2o的时候，建议大家画图去分析，比较g2o与图密不可分。我自己画的丑图，就不拿出来献丑了。只有你去动手画了，去推导了，去代码实现了，你才能发现自己的不足和理论短板。这句话送给“我自己”

1.3.1 选择不同的求解方式来求解线性方程，g2o中提供的求解方式主要有：

LinearSolverCholmod	使用sparse cholesky分解法。继承自LinearSolverCCS
LinearSolverCSparse	使用CSparse法。继承自LinearSolverCCS
LinearSolverDense	使用dense cholesky分解法。继承自LinearSolver
LinearSolverEigen	依赖项只有eigen，使用eigen中sparse Cholesky 求解，因此编译好后可以方便的在其他地方使用，性能和CSparse差不多。继承自LinearSolver
LinearSolverPCG	使用preconditioned conjugate gradient 法，继承自LinearSolver

1.3.2 创建BlockSolver。并用上面定义的线性求解器初始化

​ BlockSolver 内部包含 LinearSolver，用上面我们定义的线性求解器LinearSolver来初始化。它的定义在如下文件夹内：

g2o/g2o/core/block_solver.h

  template<int p, int l>
  using BlockSolverPL = BlockSolver< BlockSolverTraits<p, l> >;
  //variable size solver
  using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;
  // solver for BA/3D SLAM
  using BlockSolver_6_3 = BlockSolverPL<6, 3>;
  // solver fo BA with scale
  using BlockSolver_7_3 = BlockSolverPL<7, 3>;
  // 2Dof landmarks 3Dof poses
  using BlockSolver_3_2 = BlockSolverPL<3, 2>;

block_solver.h的最后，预定义了比较常用的几种类型

BlockSolver_6_3 ：表示pose 是6维，观测点是3维。用于3D SLAM中的BA
BlockSolver_7_3：在BlockSolver_6_3 的基础上多了一个scale
BlockSolver_3_2：表示pose 是3维，观测点是2维

1.3.3 创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化

g2o::OptimizationAlgorithmGaussNewton
g2o::OptimizationAlgorithmLevenberg 
g2o::OptimizationAlgorithmDogleg

eg:

    auto solver = new g2o::OptimizationAlgorithmLevenberg( // LM方法
        	g2o::make_unique<BlockSolverType>(
            g2o::make_unique<LinearSolverType>()));

1.3.4 创建终极大boss 稀疏优化器（SparseOptimizer），并用已定义求解器作为求解方法

创建稀疏优化器

g2o::SparseOptimizer    optimizer; //稀疏求解器
optimizer.setAlgorithm(solver);   // 设置求解器
optimizer.setVerbose(true);       // 打开调试输出

---

---

【1.3.5 定义图的顶点和边。并添加到SparseOptimizer中】 【关键】

​ 在g2o中定义Vertex有一个通用的类模板：BaseVertex。在结构框图中可以看到它的位置就是HyperGraph继承的根源。

• D 是int 类型，表示vertex的最小维度，例如3D空间中旋转是3维的，则 D = 3
• T 是待估计vertex的数据类型，例如用四元数表达三维旋转，则 T 就是Quaternion 类型

/**
 * \brief Templatized BaseVertex
 *
 * Templatized BaseVertex
 * D  : minimal dimension of the vertex, e.g., 3 for rotation in 3D
 * T  : internal type to represent the estimate, e.g., Quaternion for rotation in 3D
 */
  template <int D, typename T>
  class BaseVertex : public OptimizableGraph::Vertex

如何定义自己的Vertex

在我们动手定义自己的Vertex之前，可以先看下g2o本身已经定义了一些常用的顶点类型：

VertexSE2 : public BaseVertex<3, SE2>  //2D pose Vertex, (x,y,theta)
VertexSE3 : public BaseVertex<6, Isometry3> //Isometry3使欧式变换矩阵T，实质是4*4矩阵//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>

但是！如果在使用中发现没有我们可以直接使用的Vertex，那就需要自己来定义了。一般来说定义Vertex需要重写这几个函数（注意注释）：

virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;// 分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以
virtual void oplusImpl(const number_t* update);//顶点更新函数【更新】
virtual void setToOriginImpl();	//顶点重置函数，设定被优化变量的原始值。初值

根据上面四个函数可以得到定义顶点的基本格式：

class myVertex: public g2o::BaseVertex<Dim, Type>
  {
      public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      myVertex(){}

      virtual void read(std::istream& is) {}
      virtual void write(std::ostream& os) const {}

      virtual void setOriginImpl()
      {
          _estimate = Type();
      }
      virtual void oplusImpl(const double* update) override
      {
          _estimate += update;
      }
  }
///eg1 手写位姿节点///
class myVertexPose: public g2o::BaseVertex<6, SE3>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    virtual void setToOriginImpl() override { _estimate = SE3(); }

    virtual void oplusImpl(const double* update) override
    {
        Vec6 update_eigen;
        update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
        _estimate = SE3::exp(update_eigen) * _estimate; // 左乘更新 SE3 - 旋转矩阵R
    }

    virtual bool read(std::istream& in) override {return true;}
    virtual bool write(std::ostream& out) const override {return true;}
}
eg2 手写路标点节点 /
//new: 添加空间点为优化变量
class VertexPoint3 : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  virtual void setToOriginImpl() override {
    _estimate = Eigen::Vector3d::Zero();
  }

  virtual void oplusImpl(const double *update) override {
    _estimate +=  Eigen::Vector3d( update[0],update[1],update[2] );
  }

  virtual bool read(istream &in) override {}

  virtual bool write(ostream &out) const override {}
};

另外值得注意的是，优化变量更新并不是所有时候都可以像上面两个一样直接 += 就可以，这要看优化变量使用的类型（是否对加法封闭）。

向图中添加顶点
接着上面定义完的顶点，我们把它添加到图中：

CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate( Eigen::Vector3d(0,0,0) )；  // 设定初始值
v->setId(0);                               // 定义节点编号
optimizer.addVertex( v );                  // 把节点添加到图中

// 指令好像是啊，可能写错
if ( i == 0)
	v->setFixed( true ); // 第一个点固定为零
/*********************/
  g2o::VertexSE3Expmap *vertex_pose = new g2o::VertexSE3Expmap(); // camera vertex_pose
  vertex_pose->setId(0);
  vertex_pose->setEstimate(g2o::SE3Quat()); // 设置初始值，四元数
  optimizer.addVertex(vertex_pose);
/********************/
  // vertex2 p3d点
  for (size_t i = 0; i < points_3d.size(); i++)
  {
    g2o::VertexSBAPointXYZ *vertex_point = new g2o::VertexSBAPointXYZ(); 
    vertex_point->setId(i + 1);                                         
    vertex_point->setMarginalized(true);
    vertex_point->setEstimate(points_3d[i]); //Eigen::Vector3d
    optimizer.addVertex(vertex_point);
  }

---

---

图优化中的边：BaseUnaryEdge，BaseBinaryEdge，BaseMultiEdge 分别表示一元边，两元边，多元边。

顾名思义，一元边可以理解为一条边只连接一个顶点，两元边理解为一条边连接两个顶点（常见），多元边理解为一条边可以连接多个（3个以上）顶点。

以最常见的二元边为例分析一下他们的参数：D, E, VertexXi, VertexXj：

• D 是 int 型，表示测量值的维度 （dimension）
• E 表示测量值的数据类型
• VertexXi，VertexXj 分别表示不同顶点的类型

BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>

​ 上面这行代码表示二元边，参数1是说测量值是2维的；参数2对应测量值的类型是Vector2D，参数3和4表示两个顶点也就是优化变量分别是三维点 VertexSBAPointXYZ，和李群位姿VertexSE3Expmap。

常见的边的类型

g2o::EdgeSE3ProjectXYZOnlyPose // 一元边
g2o::EdgeProjectXYZ2UV* // 二元边
g2o::EdgeSE3ProjectXYZ* // 包含内参k：fx cx等变量 

如何手动定义一个边
除了上面那行定义语句，还要复写一些重要的成员函数：

virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;// 分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以virtual 
void computeError();// 非常重要，是使用当前顶点值计算的测量值与真实测量值之间的误差
virtual void linearizeOplus();// 非常重要，是在当前顶点的值下，该误差对优化变量的偏导数，也就是Jacobian矩阵

除了上面四个函数，还有几个重要的成员变量以及函数：

_measurement； // 存储观测值
_error;  // 存储computeError() 函数计算的误差
_vertices[]; // 存储顶点信息，比如二元边，
_vertices[]大小为2//存储顺序和调用setVertex(int, vertex) 和设定的int有关（0或1）
setId(int);  // 定义边的编号（决定了在H矩阵中的位置）
setMeasurement(type);  // 定义观测值
setVertex(int, vertex);  // 定义顶点
setInformation();  // 定义协方差矩阵的逆

有了上面那些重要的成员变量和成员函数，就可以用来定义一条边[一元、二元、多元]了：

class myEdge: public g2o::BaseBinaryEdge<errorDim, errorType, Vertex1Type, Vertex2Type>
  {
      public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW      
      myEdge(){}    
      virtual bool read(istream& in) {}
      virtual bool write(ostream& out) const {}      
      virtual void computeError() override
      {
          // ...          _error = _measurement - Something;
      }    
      virtual void linearizeOplus() override  {	// 求误差对优化变量的偏导数，雅克比矩阵   
          _jacobianOplusXi(pos, pos) = something;
          // ...        
          _jocobianOplusXj(pos, pos) = something; 
      }      
      private:
      data
  }
/*********************eg1 手写一元边 ICP**********************/
/// g2o edge  
class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexPose> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  EdgeProjectXYZRGBDPoseOnly(const Eigen::Vector3d &point) : _point(point) {}

  virtual void computeError() override {
    const VertexPose *pose = static_cast<const VertexPose *> ( _vertices[0] );
    _error = _measurement - pose->estimate() * _point;
  }

  virtual void linearizeOplus() override {
    VertexPose *pose = static_cast<VertexPose *>(_vertices[0]);
    Sophus::SE3d T = pose->estimate();
    Eigen::Vector3d xyz_trans = T * _point;
    _jacobianOplusXi.block<3, 3>(0, 0) = -Eigen::Matrix3d::Identity();
    _jacobianOplusXi.block<3, 3>(0, 3) = Sophus::SO3d::hat(xyz_trans);
  }

  bool read(istream &in) {}

  bool write(ostream &out) const {}

protected:
  Eigen::Vector3d _point;
};

/*****************eg1 手写二元边 ICP【简单】********************/
# define Jacobian_handle 1
/// g2o edge：new 二元边
class EdgeProjectXYZPose : public g2o::BaseBinaryEdge<3, Eigen::Vector3d, VertexPoint3 ,VertexPose> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  EdgeProjectXYZPose(){}

  virtual void computeError() override {
    const VertexPoint3 *point3 = static_cast<const VertexPoint3 *>(_vertices[0]);
    const VertexPose *pose = static_cast<const VertexPose *>(_vertices[1]);
    _error = _measurement - pose->estimate() * point3->estimate();
  }
#if Jacobian_handle==1
  // 雅克比求导，可以让g2o自动求导
  virtual void linearizeOplus() override {
    VertexPoint3 *point3 = static_cast< VertexPoint3 *>(_vertices[0]);
    VertexPose *pose = static_cast< VertexPose *>(_vertices[1]);
    Eigen::Vector3d _point = point3->estimate();
    Sophus::SE3d T = pose->estimate();

    _jacobianOplusXi.block<3, 3>(0, 0) = -(T.matrix()).block<3, 3>(0, 0);

    Eigen::Vector3d xyz_trans = T * _point;
    _jacobianOplusXj.block<3, 3>(0, 0) = -Eigen::Matrix3d::Identity();
    _jacobianOplusXj.block<3, 3>(0, 3) = Sophus::SO3d::hat(xyz_trans);
    cout<<"Jacobian_handle==1 "<<endl;
  }
#endif
  bool read(istream &in) {}

  bool write(ostream &out) const {}

protected:
  // Eigen::Vector3d _point;
};

向图中添加边
和添加点有一点类似，下面是添加一元边：

// 往图中增加边    
for ( int i=0; i<N; i++ )
{
    CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
    edge->setId(i);
    edge->setVertex( 0, v );                // 设置连接的顶点        
    edge->setMeasurement( y_data[i] );      // 观测数值        
    edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵：协方差矩阵之逆        
    edge->setRobustKernel(new g2o::RobustKernelHuber);
    optimizer.addEdge( edge );
/**************************************///[调库]
  for (size_t i = 0; i < points_2d.size(); ++i) // 观测方程
  {
    auto p3d = points_3d[i];

    g2o::EdgeSE3ProjectXYZOnlyPose *edge = new g2o::EdgeSE3ProjectXYZOnlyPose();
    edge->setId(i);
    edge->setVertex(0,vertex_pose); 
    edge->setMeasurement(points_2d[i]);
    edge->setInformation(Eigen::Matrix2d::Identity());

    edge->Xw = p3d;
    edge->fx = K.at<double>(0, 0);
    edge->fy = K.at<double>(1, 1);
    edge->cx = K.at<double>(0, 2);
    edge->cy = K.at<double>(1, 2);

    optimizer.addEdge(edge);
  }

但在SLAM中我们经常要使用的二元边（前后两个位姿），那么此时：

// 【二元边： 2d点】
index = 1;
for ( const Point2f p:points_2d ){
    g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
    edge->setId ( index );  // 边的b编号    
    edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
    edge->setVertex ( 1, pose );
    edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );  // 设置观测的特征点图像坐标    
    edge->setParameterId ( 0,0 );
    edge->setInformation ( Eigen::Matrix2d::Identity() );
    edge->setRobustKernel(new g2o::RobustKernelHuber);
    optimizer.addEdge ( edge );
    index++;
 //【6】 设置相机内参 [补充的部分]
 edge->fx = fx; // 看二元边用的哪个参数了
 edge->fy = fy;
 edge->cx = cx;
 edge->cy = cy;
}

/**************/ // 【二元边 3d点 】
g2o::EdgeStereoSE3ProjectXYZ *edge = new g2o::EdgeStereoSE3ProjectXYZ();

**1.3.6 设置优化参数，开始执行优化 **

设置SparseOptimizer的初始化、迭代次数、保存结果等。

初始化

SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset)

设置迭代次数，然后就开始执行图优化了。

SparseOptimizer::optimize(int iterations, bool online)

一个二元边的例程：

/**
 * BA Example
 * Author: Xiang Gao
 * Date: 2016.3
 * Email: [email protected]
 *
 * 在这个程序中，我们读取两张图像，进行特征匹配。然后根据匹配得到的特征，计算相机运动以及特征点的位置。这是一个典型的Bundle Adjustment，我们用g2o进行优化。
 */

// for std
#include 
// for opencv
#include 
#include 
#include 
#include 
// for g2o
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 


using namespace std;

// 寻找两个图像中的对应点，像素坐标系
// 输入：img1, img2 两张图像
// 输出：points1, points2, 两组对应的2D点
int     findCorrespondingPoints( const cv::Mat& img1, const cv::Mat& img2, vector<cv::Point2f>& points1, vector<cv::Point2f>& points2 );

// 相机内参
double cx = 325.5;
double cy = 253.5;
double fx = 518.0;
double fy = 519.0;

int main( int argc, char** argv )
{
    // 调用格式：命令 [第一个图] [第二个图]
    if (argc != 3)
    {
        cout<<"Usage: ba_example img1, img2"<<endl;
        exit(1);
    }

    // 读取图像
    cv::Mat img1 = cv::imread( argv[1] );
    cv::Mat img2 = cv::imread( argv[2] );

    // 找到对应点
    vector<cv::Point2f> pts1, pts2;
    if ( findCorrespondingPoints( img1, img2, pts1, pts2 ) == false )
    {
        cout<<"匹配点不够！"<<endl;
        return 0;
    }
    cout<<"找到了"<<pts1.size()<<"组对应特征点。"<<endl;
    // 构造g2o中的图
    // 先构造求解器
    g2o::SparseOptimizer    optimizer;
    // 使用Cholmod中的线性方程求解器
    g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new  g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType> ();
    // 6*3 的参数
    g2o::BlockSolver_6_3* block_solver = new g2o::BlockSolver_6_3( linearSolver );
    // L-M 下降
    g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg( block_solver );

    optimizer.setAlgorithm( algorithm );
    optimizer.setVerbose( false );

    // 添加节点
    // 两个位姿节点
    for ( int i=0; i<2; i++ )
    {
        g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap();
        v->setId(i);
        if ( i == 0)
            v->setFixed( true ); // 第一个点固定为零
        // 预设值为单位Pose，因为我们不知道任何信息
        v->setEstimate( g2o::SE3Quat() );
        optimizer.addVertex( v );
    }
    // 很多个特征点的节点
    // 以第一帧为准
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        g2o::VertexSBAPointXYZ* v = new g2o::VertexSBAPointXYZ();
        v->setId( 2 + i );
        // 由于深度不知道，只能把深度设置为1了
        double z = 1;
        double x = ( pts1[i].x - cx ) * z / fx;
        double y = ( pts1[i].y - cy ) * z / fy;
        v->setMarginalized(true);
        v->setEstimate( Eigen::Vector3d(x,y,z) );
        optimizer.addVertex( v );
    }

    // 准备相机参数
    g2o::CameraParameters* camera = new g2o::CameraParameters( fx, Eigen::Vector2d(cx, cy), 0 );
    camera->setId(0);
    optimizer.addParameter( camera );

    // 准备边
    // 第一帧
    vector<g2o::EdgeProjectXYZ2UV*> edges;
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        g2o::EdgeProjectXYZ2UV*  edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*>   (optimizer.vertex(i+2)) );
        edge->setVertex( 1, dynamic_cast<g2o::VertexSE3Expmap*>     (optimizer.vertex(0)) );
        edge->setMeasurement( Eigen::Vector2d(pts1[i].x, pts1[i].y ) );
        edge->setInformation( Eigen::Matrix2d::Identity() );
        edge->setParameterId(0, 0);
        // 核函数
        edge->setRobustKernel( new g2o::RobustKernelHuber() );
        optimizer.addEdge( edge );
        edges.push_back(edge);
    }
    // 第二帧
    for ( size_t i=0; i<pts2.size(); i++ )
    {
        g2o::EdgeProjectXYZ2UV*  edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*>   (optimizer.vertex(i+2)) );
        edge->setVertex( 1, dynamic_cast<g2o::VertexSE3Expmap*>     (optimizer.vertex(1)) );
        edge->setMeasurement( Eigen::Vector2d(pts2[i].x, pts2[i].y ) );
        edge->setInformation( Eigen::Matrix2d::Identity() );
        edge->setParameterId(0,0);
        // 核函数
        edge->setRobustKernel( new g2o::RobustKernelHuber() );
        optimizer.addEdge( edge );
        edges.push_back(edge);
    }

    cout<<"开始优化"<<endl;
    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    cout<<"优化完毕"<<endl;

    //我们比较关心两帧之间的变换矩阵
    g2o::VertexSE3Expmap* v = dynamic_cast<g2o::VertexSE3Expmap*>( optimizer.vertex(1) );
    Eigen::Isometry3d pose = v->estimate();
    cout<<"Pose="<<endl<<pose.matrix()<<endl;

    // 以及所有特征点的位置
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        g2o::VertexSBAPointXYZ* v = dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(i+2));
        cout<<"vertex id "<<i+2<<", pos = ";
        Eigen::Vector3d pos = v->estimate();
        cout<<pos(0)<<","<<pos(1)<<","<<pos(2)<<endl;
    }

    // 估计inlier的个数
    int inliers = 0;
    for ( auto e:edges )
    {
        e->computeError();
        // chi2 就是 error*\Omega*error, 如果这个数很大，说明此边的值与其他边很不相符
        if ( e->chi2() > 1 )
        {
            cout<<"error = "<<e->chi2()<<endl;
        }
        else
        {
            inliers++;
        }
    }

    cout<<"inliers in total points: "<<inliers<<"/"<<pts1.size()+pts2.size()<<endl;
    optimizer.save("ba.g2o");
    return 0;
}


int     findCorrespondingPoints( const cv::Mat& img1, const cv::Mat& img2, vector<cv::Point2f>& points1, vector<cv::Point2f>& points2 )
{
    cv::ORB orb;
    vector<cv::KeyPoint> kp1, kp2;
    cv::Mat desp1, desp2;
    orb( img1, cv::Mat(), kp1, desp1 );
    orb( img2, cv::Mat(), kp2, desp2 );
    cout<<"分别找到了"<<kp1.size()<<"和"<<kp2.size()<<"个特征点"<<endl;

    cv::Ptr<cv::DescriptorMatcher>  matcher = cv::DescriptorMatcher::create( "BruteForce-Hamming");

    double knn_match_ratio=0.8;
    vector< vector<cv::DMatch> > matches_knn;
    matcher->knnMatch( desp1, desp2, matches_knn, 2 );
    vector< cv::DMatch > matches;
    for ( size_t i=0; i<matches_knn.size(); i++ )
    {
        if (matches_knn[i][0].distance < knn_match_ratio * matches_knn[i][1].distance )
            matches.push_back( matches_knn[i][0] );
    }

    if (matches.size() <= 20) //匹配点太少
        return false;

    for ( auto m:matches )
    {
        points1.push_back( kp1[m.queryIdx].pt );
        points2.push_back( kp2[m.trainIdx].pt );
    }

    return true;
}

{附加}李代数二维&三维转化

g2o::SE3Quat SE2ToSE3(const g2o::SE2& _se2)
{
    SE3Quat ret;
    ret.setTranslation(Eigen::Vector3d(_se2.translation()(0), _se2.translation()(1), 0));
    ret.setRotation(Eigen::Quaterniond(AngleAxisd(_se2.rotation().angle(), Vector3d::UnitZ())));
    return ret;
}

g2o::SE2 SE3ToSE2(const SE3Quat &_se3)
{
    Eigen::Vector3d eulers = g2o::internal::toEuler(_se3.rotation().matrix());
    return g2o::SE2(_se3.translation()(0), _se3.translation()(1), eulers(2));
}

---

---

2.Ceres

​ 尤其是Ceres函数库在激光SLAM和V-SLAM的优化中均有着大量的应用。所以作者从Ceres作为开端，来对手写SLAM开个头，来方便各位后续的开发。

​ 目前Bundle Adjustment 其本质还是离不开最小二乘原理(几乎所有优化问题其本质都是最小二乘),目前Bundle Adjustment 优化框架最为代表的是Ceres solver和G2O(这里主要介绍ceres solver)。

​ Ceres中的优化需要四步，构建优化的残差函数，构建优化问题，在每次获取到数据后添加残差块，总体优化。

【Ceres: 步骤: exp(ax^2+bx+c) 】

1. 构建最小二乘：

   AddParameterBlock - 添加参数块

   AddResidualBlock - 自动求导、模板参数、误差类型等。

2. 配置求解器：选择密集增量cholesky

3. ceres::Solve(options, &problem, &summary); 执行优化

// ****************构建残差函数******************
// 代价函数的计算模型 :f(x) = e || 【1.定义残差块的类/结构体】
struct CURVE_FITTING_COST {
  CURVE_FITTING_COST(double x, double y) : _x(x), _y(y) {}

  // 残差的计算
  template<typename T>
  bool operator()(
    const T *const abc, // 模型参数，有3维
    T *residual) const {
    residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]); // y-exp(ax^2+bx+c)
    return true;
  }

  const double _x, _y;    // x,y数据
};
// ****************NO.1******************
  double abc[3] = {ae, be, ce};

  // 构建最小二乘问题
  ceres::Problem problem;
  for (int i = 0; i < N; i++) { //【主要部分：配置-前置的准备】
    problem.AddResidualBlock(     // 向问题中添加误差项
      // 使用自动求导，模板参数：【误差类型】，输出维度，输入维度，维数要与前面struct中一致
      new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>( 
        new CURVE_FITTING_COST(x_data[i], y_data[i]) // 代价函数 f(x) = e || 重载了()运算符，代价函数求出
      ),
      nullptr,            // 核函数，这里不使用，为空
      abc                 // 待估计参数 || 参数块
    );
  }
  // ****************NO.2******************
  ceres::Solver::Options options;     // 这里有很多配置项可以填
  options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;  // 密集增量cholesky||增量方程如何求解
  options.minimizer_progress_to_stdout = true;   // 输出到cout

  // ****************NO.3******************
  ceres::Solver::Summary summary;                // 优化信息
  ceres::Solve(options, &problem, &summary);  // 开始优化 || 配置-最小二乘问题-优化信息

  // 输出结果
  cout << summary.BriefReport() << endl;
  cout << "estimated a,b,c = ";
  for (auto a:abc) cout << a << " ";
  cout << endl;

–1.1.构建残差函数

//构建代价函数结构体，residual 为残差。
//last_point_a_为这一帧中的点a，curr_point_b_为点a旋转后和上一帧里最近的点
//curr_point_c_为点b同线或上线号的点，curr_point_d_为点b下线号的点
//b，c，d与a点距离不超过3m
//plane_norm为根据向量bc和bd求出的法向量
struct CURVE_FITTING_COST
{
    //类似构造函数
  CURVE_FITTING_COST(Eigen::Vector3d _curr_point_a_, Eigen::Vector3d _last_point_b_,
             Eigen::Vector3d _last_point_c_, Eigen::Vector3d _last_point_d_):
             curr_point_a_(_curr_point_a_),last_point_b_(_last_point_b_),
             last_point_c_(_last_point_c_),last_point_d_(_last_point_d_)
    {
        plane_norm = (last_point_d_ - last_point_b_).cross(last_point_c_ - last_point_b_);
        plane_norm.normalize();
    }
  template <typename T>
  //plane_norm点乘向量ab为a点距面bcd的距离，即残差
  bool operator()(const T* q,const T* t,T* residual)const
  {
    Eigen::Matrix<T, 3, 1> p_a_curr{T(curr_point_a_.x()), T(curr_point_a_.y()), T(curr_point_a_.z())};
    Eigen::Matrix<T, 3, 1> p_b_last{T(last_point_b_.x()), T(last_point_b_.y()), T(last_point_b_.z())};
    Eigen::Quaternion<T> rot_q{q[3], q[0], q[1], q[2]};
    Eigen::Matrix<T, 3, 1> rot_t{t[0], t[1], t[2]};
    Eigen::Matrix<T, 3, 1> p_a_last;
    p_a_last=rot_q * p_a_curr + rot_t;
    residual[0]=abs((p_a_last - p_b_last).dot(plane_norm));
    return true;
  }
  const Eigen::Vector3d curr_point_a_,last_point_b_,last_point_c_,last_point_d_;
  Eigen::Vector3d plane_norm;
};

–1.2. 构建优化问题

//优化问题构建
ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
ceres::LocalParameterization *q_parameterization = new ceres::EigenQuaternionParameterization();
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
problem.AddParameterBlock(para_q, 4, q_parameterization);
problem.AddParameterBlock(para_t, 3);

每次求出abcd点后，将他们的坐标构建成Eigen::Vector3d数据，添加残差块：

Eigen::Vector3d curr_point_a(laserCloudIn_plane.points[i].x,
                laserCloudIn_plane.points[i].y,
                laserCloudIn_plane.points[i].z);

Eigen::Vector3d last_point_b(laserCloudIn_plane_last.points[closestPointInd].x,laserCloudIn_plane_last.points[closestPointInd].y,laserCloudIn_plane_last.points[closestPointInd].z);

Eigen::Vector3d last_point_c(laserCloudIn_plane_last.points[minPointInd2].x,
                laserCloudIn_plane_last.points[minPointInd2].y,
                laserCloudIn_plane_last.points[minPointInd2].z);

Eigen::Vector3d last_point_d(laserCloudIn_plane_last.points[minPointInd3].x,
                laserCloudIn_plane_last.points[minPointInd3].y,
                laserCloudIn_plane_last.points[minPointInd3].z);

problem.AddResidualBlock(new ceres::AutoDiffCostFunction<CURVE_FITTING_COST,1,4,3>
            (new CURVE_FITTING_COST(last_point_a,curr_point_b,
              curr_point_c,curr_point_d)),loss_function,para_q,para_t);

遍历过所有的a点后，就可以优化求解了。

//所有前一帧里的点都当a点遍历过后，进行优化
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
//迭代数
options.max_num_iterations = 5;
//进度是否发到STDOUT
options.minimizer_progress_to_stdout = false;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);

而在V-SLAM中Ceres则用的更多，我们可以看下下面的例子

double para_Pose[7];
para_Pose[0] = 0.0;
para_Pose[1] = 0.0;
para_Pose[2] = 0.0;
para_Pose[6] =  1.0;
para_Pose[3] =  0.0;
para_Pose[4] =  0.0;
para_Pose[5] =  0.0;


int kNumObservations = cur_pts.size();
double invDepth[kNumObservations][1];

ceres::LossFunction *loss_function;
    //loss_function = new ceres::HuberLoss(1.0);
loss_function = new ceres::CauchyLoss(1.0);// 柯西核函数 

ceres::LocalParameterization *local_parameterizationP = new PoseLocalParameterization();
problem.AddParameterBlock(para_Pose, 7, local_parameterizationP);//对Pose重新参数化

for (int i = 0; i < kNumObservations; ++i) {
    invDepth[i][0] = 1;
    problem.AddParameterBlock(invDepth[i], 1); //对深度重新参数化
    if (!invdepths.empty()&&invdepths[i]>0){
        // cout << "depth observations "<< 1./invdepths[i] <<" "<< invdepths[i] <
        invDepth[i][0] = invdepths[i];
        problem.SetParameterBlockConstant(invDepth[i]);//把任何参数块设为常数，并且使用SetParameterBlockVariable()来撤销这一操作

        ceres::CostFunction *f_d;
        //自动求导方法，AutoDiffCostFunction
        f_d = new ceres::AutoDiffCostFunction<autoIvDepthFactor, 1,1>(
                new autoIvDepthFactor(invdepths[i]) );
        problem.AddResidualBlock(f_d, loss_function, invDepth[i]);
    }

    ceres::CostFunction *f;
    f = new ceres::AutoDiffCostFunction<autoMonoFactor, 3,7,1>(
            new autoMonoFactor(Vector3d(un_prev_pts[i].x, un_prev_pts[i].y, 1),Vector3d(un_cur_pts[i].x, un_cur_pts[i].y, 1)) );

    problem.AddResidualBlock(f, loss_function, para_Pose,invDepth[i]);
}

ceres::Solver::Options options;
// options.max_num_iterations = 7;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.trust_region_strategy_type = ceres::DOGLEG;
options.minimizer_progress_to_stdout = false;

ceres::Solver::Summary summary;

TicToc solveTime;
ceres::Solve(options, &problem, &summary);

---

---

2.Ceres 常见函数总结

–2.1 创建优化问题与损失核函数

​ 用于处理参数中含有野值的情况，避免错误量测对估计的影响，常用参数包括HuberLoss、CauchyLoss等；该参数可以取NULL或nullptr，此时损失函数为单位函数。

ceres::Problem problem;
ceres::LossFunction *loss_function;                           // 损失核函数 
//loss_function = new ceres::HuberLoss(1.0); 				  //huber核函数
loss_function = new ceres::CauchyLoss(1.0);                   // 柯西核函数

–2.2 在problem中添加参数块- AddParameterBlock【重构】

​ （该函数常用的重载有两个）用户在调用 AddResidualBlock( ) 时其实已经隐式地向Problem传递了参数模块，但在一些情况下，需要用户显式地向Problem传入参数模块（通常出现在需要对优化参数进行重新参数化的情况）。Ceres提供了**Problem::AddParameterBlock( )**函数用于用户显式传递参数模块：

void AddParameterBlock(double* values, int size);
void AddParameterBlock(double* values,
                         int size,
                         LocalParameterization* local_parameterization);

注：values表示优化变量，size表示优化变量的维度。
其中，第一种函数原型除了会增加一些额外的参数检查之外，功能上和隐式传递参数并没有太大区别。第二种函数原型则会额外传入LocalParameterization参数，用于重构优化参数的维数。

LocalParameterization是在优化Manifold(流形)上的变量时需要考虑的，Manifold上变量是过参数的，即Manifold上变量的维度大于其自由度。这会导致Manifold上变量各个量之间存在约束，如果直接对这些量求导、优化，那么这就是一个有约束的优化，实现困难。为了解决这个问题，在数学上对Manifold在当前变量值处形成的切空间求导，在切空间上优化，最后投影回Manifold。【防止变量的维度 > 其自由度啊】

对于SLAM问题，广泛遇到的Manifold是旋转，旋转仅需要3个量，但实际运用中涉及到万向锁问题，在更高维空间表达旋转，四元数就是在维度4表达3个自由度的三维空间的旋转。

【这块没有很理解】bool ComputeJacobian()计算得到一个4*3的矩阵(global_to_local)，含义是Manifold上变量对Tangent Space上变量的导数，在ceres::CostFunction处提供residuals对Manifold上变量的倒数，乘以这个矩阵，之后变就变成了对Tangent Space上变量的导数。

  //版本1.0 : q t分开计算， 调用Ceres的库，不需要重构
  double para_q[4] = {0,0,0,1};
  double para_t[3] = {0,0,0};
  ceres::LocalParameterization *q_parameterization = new ceres::EigenQuaternionParameterization();
  problem.AddParameterBlock(para_q, 4, q_parameterization);
  problem.AddParameterBlock(para_t, 3);
/ 上面是调库 || 下面是重构se3
  // 版本2.0： se3 代替 q t 分开的 || 手写雅克比
  double parameters[7] = {0,0,0,1,0,0,0};
  problem.AddParameterBlock(parameters, 7,  new PoseSE3Parameterization());

----2.2 【补充】：

LocalParameterization类的作用是解决非线性优化中的过参数化问题。所谓过参数化，即待优化参数的实际自由度小于参数本身的自由度。例如在SLAM中，当采用四元数表示位姿时，由于四元数本身的约束（模长为1），实际的自由度为3而非4。此时，若直接传递四元数进行优化，冗余的维数会带来计算资源的浪费，需要使用Ceres预先定义的QuaternionParameterization对优化参数进行重构：

problem.AddParameterBlock(quaternion, 4);// 直接传递4维参数
// QuaternionParameterization 继承于 LocalParameterization
ceres::LocalParameterization* local_param = new ceres::QuaternionParameterization(); // EigenQuaternionParameterization
problem.AddParameterBlock(quaternion, 4, local_param)//重构参数，优化时实际使用的是3维的等效旋转矢量

四元数的使用问题：

四元数表示的是一个SO3，四元数表示的这个东西是一个有三个自由度的东西，然而四元数却有四维也就是四个自由度，这显然是不合理的，所以也就产生了一个单位四元数这么一个东西，单位四元数顾名思义 就是说四元数的四个量的二范数是1.这个其实是一个约束，这个约束就约束了四元数的一个自由度，这样其实四元数就只剩下三个自由度了正好符合一个SO3的维数。

然后在ceres里面，如果使用的是自动求导，然后再结合爬山法，那么每步迭代中都会产生一个四维的delta(迭代的增量，参考LM等算法)，那么根据常规的爬山法，这样就仅仅需要将 原四元数“加上”这个迭代产生的delta就能够得到新的四元数了，这里问题就来了，直接加上以后这个四元数就不再是一个单位四元数了，就没有意义了，如果非得这么用的话就得每次迭代过后都将这个四元数进行一个归一化处理。

解决方案：

	对于四元数或者旋转矩阵这种使用过参数化表示旋转的方式，它们是**不支持广义的加法**（因为使用普通的加法就会打破其 constraint，比如旋转矩阵加旋转矩阵得到的就不再是旋转矩阵），所以我们在使用ceres对其进行

迭代更新的时候就需要自定义其更新方式了，具体的做法是实现一个**参数本地化**的子类，需要继承于`LocalParameterization`，`LocalParameterization`是纯虚类，所以我们继承的时候要把所有的纯虚函数都实现一遍才能使用该类生成对象.

	除了不支持广义加法要自定义参数本地化的子类外，如果你要对优化变量做一些限制也可以如法炮制，比如ceres中slam2d example中对角度范围进行了限制.

**自定义 LocalParameterization **

LocalParameterization本身是一个虚基类，详细定义如下。用户可以自行定义自己需要使用的子类，或使用Ceres预先定义好的子类。

class LocalParameterization {
 public:
  virtual ~LocalParameterization() {}
  //
  virtual bool Plus(const double* x,
                    const double* delta,
                    double* x_plus_delta) const = 0;//参数正切空间上的更新函数
  virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0; //雅克比矩阵
  virtual bool MultiplyByJacobian(const double* x,
                                  const int num_rows,
                                  const double* global_matrix,
                                  double* local_matrix) const;//一般不用
  virtual int GlobalSize() const = 0; // 参数的实际维数
  virtual int LocalSize() const = 0; // 正切空间上的参数维数
};

上述成员函数中，需要我们改写的主要为

• Plus(const double* x,const double* delta,double* x_plus_delta)：定义变量的加法（具体参考下面例子）
• ComputeJacobian()：x对delta的雅克比矩阵
• GlobalSize()：传入的实际参数的维度
• LocalSize()：参数实际的维度（自由度）

案例：QuaternionParameterization解析

这里我们以ceres预先定义好的QuaternionParameterization为例具体说明LocalParameterization用法，类声明如下：

注意。

• 在 ceres 源码中没有明确说明之处都认为矩阵 raw memory 存储方式是 Row Major 的，这与 Eigen 默认的 Col Major 是相反的。

• ceres 默认的 Quaternion raw memory 存储方式是 w, x, y, z，而 Eigen Quaternion 的存储方式是 x, y, z, w，这就导致在 ceres 代码中除ceres::QuaternionParameterization 之外还有ceres::EigenQuaternionParameterization。

  Eigen Quaternion指的是eigen库中的函数Eigen::Quaternion(w,x,y,z)函数中，实数w在首；但是实际上它的内部存储顺序是[x y z w]，对其访问的时候最后一个元素才是w

  对三个函数内部存储顺序总结

  ceres::QuaternionParameterization：内部存储顺序为(w,x,y,z)

  ceres::EigenQuaternionParameterization：内部存储顺序为(x,y,z,w)

  Eigen::Quaternion(w,x,y,z)：内部存储顺序为(x,y,z,w)（与构造函数没有保持一致）

• ceres 中 Quaternion 是 Hamilton Quaternion，遵循 Hamilton 乘法法则。

class CERES_EXPORT QuaternionParameterization : public LocalParameterization {
 public:
  virtual ~QuaternionParameterization() {}
  //重载的Plus函数给出了四元数的更新方法，接受参数分别为优化前的四元数【x】，用旋转矢量表示的增量【delta】，以及更新后的四元数【x_plus_delta】。
  //函数首先将增量【delta】由旋转矢量转换为四元数，随后采用标准四元数乘法对四元数进行更新。
  virtual bool Plus(const double* x,
                    const double* delta,
                    double* x_plus_delta) const;
  virtual bool ComputeJacobian(const double* x,
                               double* jacobian) const;
  //GlobalSize 返回值为4，即四元数本身的实际维数。由于在内部优化时，ceres采用的是旋转矢量，维数为3，因此LocalSize()的返回值为3。
  //GlobalSize 就是表示他真正的维数是一个4维的 || 【如果是q+t=7】
  virtual int GlobalSize() const { return 4; }
  //LocalSize是告诉Ceres他表示的东西是一个三维的 ||【如果是q+t=６】
  virtual int LocalSize() const { return 3; }
};
//============================================================================= [实践的代码]
		virtual bool Plus(const double *x,
						  const double *delta,
						  double *x_plus_delta) const //参数正切空间上的更新函数
		{
			// Eigen::Map quater(x);		//   待更新的四元数
			// Eigen::Map delta_so3(delta); //    delta 值,使用流形 so3 更新

			// Eigen::Quaterniond delta_quater = Sophus::SO3d::exp(delta_so3).unit_quaternion(); //   so3 转换位 delta_p  四元数
			// Eigen::Map quter_plus(x_plus_delta); //   更新后的四元数

			// // 旋转更新公式
			// quter_plus = (delta_quater * quater).normalized();

	
			Eigen::Quaterniond delta_q;
			getTransformFromSo3(Eigen::Map<const Eigen::Matrix<double, 3, 1>>(delta), delta_q); // 【F-LOAM中的】
			Eigen::Map<const Eigen::Quaterniond> quater(x);			  // 优化前的四元数
			Eigen::Map<Eigen::Quaterniond> quater_plus(x_plus_delta); // 优化后的四元数

			quater_plus = (delta_q * quater).normalize();

			return true;
		}
//=============================================================================【官方-参考的代码】
//重载的Plus函数给出了四元数的更新方法，接受参数分别为优化前的四元数【x】，用旋转矢量表示的增量【delta】，以及更新后的四元数【x_plus_delta】。
//函数首先将增量【delta】由旋转矢量转换为四元数，随后采用标准四元数乘法对四元数进行更新。
bool QuaternionParameterization::Plus(const double* x,
                                      const double* delta,
                                      double* x_plus_delta) const {
  // 将旋转矢量转换为四元数形式
  const double norm_delta =
      sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
  if (norm_delta > 0.0) {
    const double sin_delta_by_delta = (sin(norm_delta) / norm_delta);
    double q_delta[4];
    q_delta[0] = cos(norm_delta);
    q_delta[1] = sin_delta_by_delta * delta[0];
    q_delta[2] = sin_delta_by_delta * delta[1];
    q_delta[3] = sin_delta_by_delta * delta[2];
    // 采用四元数乘法更新
    QuaternionProduct(q_delta, x, x_plus_delta);
  } else {
    for (int i = 0; i < 4; ++i) {
      x_plus_delta[i] = x[i];
    }
  }
  return true;
}
//=============================================================================

Plus()

实现了优化变量的更新，即使用GN法得到的 $ \Delta \tilde{\mathbf{x}}^{} Δx~$更新原来优化变量$ x ˇ \check{\mathbf{x}} xˇ$，使用的是 ⊞ 广义加法运算符.
x $ = x ˘ ⊕ Δ x ~ ∗ \mathbf{x}^{}=\breve{\mathbf{x}} \oplus \Delta \tilde{\mathbf{x}}^{*} x∗=x˘⊕Δx~$
⊞运算符，首先将四元数的向量部分（与旋转向量相差一个系数）变成一个完整的四元数（纯虚四元数的指数），即得到过参数化的增量，然后将该增量应用到待估计变量上.

ComputeJacobian 函数给出了四元数相对于旋转矢量的雅克比矩阵计算方法, 即：
J 4 × 3 = d q / d v = d [ q w , q x , q y , q z ] T / d [ [ x , y , z ] $\boldsymbol{J}{4 \times 3}=d \boldsymbol{q} / d \boldsymbol{v}=d\left[q{w}, q_{x}, q_{y}, q_{z}\right]^{T} / d[x, y, z] $
对应Jacobian维数为4行3列，存储方式为行主序。

		virtual bool ComputeJacobian(const double* x, double* jacobian) const  //  四元数对so3的偏导数
        {
			// jacobian[0] = -x[1]; jacobian[1]  = -x[2]; jacobian[2]  = -x[3];  // NOLINT
			// jacobian[3] =  x[0]; jacobian[4]  =  x[3]; jacobian[5]  = -x[2];  // NOLINT
			// jacobian[6] = -x[3]; jacobian[7]  =  x[0]; jacobian[8]  =  x[1];  // NOLINT
			// jacobian[9] =  x[2]; jacobian[10] = -x[1]; jacobian[11] =  x[0];  // NOLINT

			Eigen::Map<Eigen::Matrix<double, 4, 3, Eigen::RowMajor>> j(jacobian); // 优化后的四元数
			(j.topRows(3)).setIdentity();
			(j.bottomRows(1)).setZero();

			return true;
		}

---

---

附加程序-from F-LOAM

skew 和 getTransformFromSe3

template <typename T>
Eigen::Matrix<T,3,3> skew(Eigen::Matrix<T,3,1>& mat_in){
    Eigen::Matrix<T,3,3> skew_mat;
    skew_mat.setZero();
    skew_mat(0,1) = -mat_in(2);
    skew_mat(0,2) =  mat_in(1);
    skew_mat(1,2) = -mat_in(0);
    skew_mat(1,0) =  mat_in(2);
    skew_mat(2,0) = -mat_in(1);
    skew_mat(2,1) =  mat_in(0);
    return skew_mat;
};

void getTransformFromSe3(const Eigen::Matrix<double,6,1>& se3, Eigen::Quaterniond& q, Eigen::Vector3d& t){
    Eigen::Vector3d omega(se3.data());
    Eigen::Vector3d upsilon(se3.data()+3);
    Eigen::Matrix3d Omega = skew(omega);

    double theta = omega.norm();
    double half_theta = 0.5*theta;

    double imag_factor;
    double real_factor = cos(half_theta);
    if(theta<1e-10)
    {
        double theta_sq = theta*theta;
        double theta_po4 = theta_sq*theta_sq;
        imag_factor = 0.5-0.0208333*theta_sq+0.000260417*theta_po4;
    }
    else
    {
        double sin_half_theta = sin(half_theta);
        imag_factor = sin_half_theta/theta;
    }

    q = Eigen::Quaterniond(real_factor, imag_factor*omega.x(), imag_factor*omega.y(), imag_factor*omega.z());


    Eigen::Matrix3d J;
    if (theta<1e-10)
    {
        J = q.matrix();
    }
    else
    {
        Eigen::Matrix3d Omega2 = Omega*Omega;
        J = (Eigen::Matrix3d::Identity() + (1-cos(theta))/(theta*theta)*Omega + (theta-sin(theta))/(pow(theta,3))*Omega2);
    }

    t = J*upsilon;
}

–2.3 添加残差块

​ 一个优化问题可以看成通过调整参数将一大堆各种各样的残差降到最小，因此，残差的提供是至关重要的，一个残差的构建离不开残差的数学定义以及关联的参数，ceres添加残差块通过 AddResidualBlock() 完成 ， 有两个重载貌似最为常用

template <typename... Ts>
  ResidualBlockId AddResidualBlock(CostFunction* cost_function, // 残差
                                   LossFunction* loss_function, //损失函数-核函数
                                   double* x0, //待优化变量 - double || 残差函数关联的变量
                                   Ts*... xs)
ResidualBlockId AddResidualBlock(
      CostFunction* cost_function,
      LossFunction* loss_function,
      const std::vector<double*>& parameter_blocks); // -vector容器

​ 也就是需要提供三种参数 —— cost_function对象、鲁棒核函数对象、 该残差的关联参数 。

代价函数：包含了参数模块的维度信息，内部使用仿函数定义误差函数的计算方式。AddResidualBlock( )函数会检测传入的参数模块是否和代价函数模块中定义的维数一致，维度不一致时程序会强制退出。代价函数模块的详解参见Ceres详解（二） CostFunction。
损失函数：用于处理参数中含有野值的情况，避免错误量测对估计的影响，常用参数包括HuberLoss、CauchyLoss等（完整的参数列表参见Ceres API文档）；该参数可以取NULL或nullptr，此时损失函数为单位函数。
参数模块：待优化的参数，可一次性传入所有参数的指针容器vector或依次传入所有参数的指针double*。

其中重点是cost_function对象的给定，它有三种常见的提供方式：

• 自动导数（AutoDiffCostFunction）：由ceres自行决定导数的计算方式，最常用的求导方式。不能模板化成参数类

template <typename CostFunctor,
       int kNumResiduals,  // Number of residuals, or ceres::DYNAMIC. || 残差的维度
       int... Ns>          // Size of each parameter block || 参数块的维度
class AutoDiffCostFunction : public
SizedCostFunction<kNumResiduals, Ns> {
 public:
  AutoDiffCostFunction(CostFunctor* functor, ownership = TAKE_OWNERSHIP);
  // Ignore the template parameter kNumResiduals and use
  // num_residuals instead.
  AutoDiffCostFunction(CostFunctor* functor,
                       int num_residuals, // 残差num
                       ownership = TAKE_OWNERSHIP); //参数size
};

• 数值导数（NumericDiffCostFunction）：由用户手动编写导数的数值求解形式，通常在残差函数的计算使用无法直接调用库函数，导致调用AutoDiffCostFunction类构建时使用；但手动编写的精度和计算效率不如模板类，因此不到不得已，官方并不建议使用该方法。

数值求导 NumericDiffCostFunction

template <typename CostFunctor,
          NumericDiffMethodType method = CENTRAL,
          int kNumResiduals,  // Number of residuals, or ceres::DYNAMIC.
          int... Ns>          // Size of each parameter block.
class NumericDiffCostFunction : public
SizedCostFunction<kNumResiduals, Ns> {
};

需要计算CostFunctor，需要定义一个具有operator()计算残差的（函子）的类。函子必须将计算值写入最后一个参数（唯一的非参数const）并返回true以指示成功。

struct ScalarFunctor {
 public:
  bool operator()(const double* const x1, // 计算残差
                  const double* const x2,
                  double* residuals) const;
}

//eg 例如： e=k-x'y ； 两者x和y是二维列向量参数，撇号表示转置，和k是一个常数。
class MyScalarCostFunctor {
  MyScalarCostFunctor(double k): k_(k) {}

  bool operator()(const double* const x,
                  const double* const y,
                  double* residuals) const {
    residuals[0] = k_ - x[0] * y[0] + x[1] * y[1];
    return true;
  }

 private:
  double k_;
};

请注意，在operator()输入参数 的声明中， xandy先出现，并且作为 const 指针传递给doubles 的数组。如果有三个输入参数，那么第三个输入参数将在 之后y。输出始终是最后一个参数，也是指向数组的指针。在上面的例子中，残差是一个标量，所以只residuals[0]设置了。

CostFunction用于计算导数的具有中心差分的数值微分可以构造如下。

CostFunction* cost_function
    = new NumericDiffCostFunction<MyScalarCostFunctor, CENTRAL, 1, 2, 2>(
        new MyScalarCostFunctor(1.0));                    ^     ^  ^  ^
                                                          |     |  |  |
                              Finite Differencing Scheme -+     |  |  |
                              Dimension of residual ------------+  |  |
                              Dimension of x ----------------------+  |
                              Dimension of y -------------------------+
    
//eg 对应上面的例子 || MyScalarCostFunctor在上面的实例化中，, 之后的模板参数 将仿函数描述为从两个二维参数计算一维输出。1, 2, 2
CostFunction* cost_function
    = new NumericDiffCostFunction<MyScalarCostFunctor, CENTRAL, DYNAMIC, 2, 2>(
        new CostFunctorWithDynamicNumResiduals(1.0),               ^     ^  ^
        TAKE_OWNERSHIP,                                            |     |  |
        runtime_number_of_residuals); <----+                       |     |  |
                                           |                       |     |  |
                                           |                       |     |  |
          Actual number of residuals ------+                       |     |  |
          Indicate dynamic number of residuals --------------------+     |  |
          Dimension of x ------------------------------------------------+  |
          Dimension of y ---------------------------------------------------+

三种数值微分方案：

1. 差分法FORWARD，近似于f′(x) 通过计算f(x+h)−f(x)h, 再计算一次成本函数x+h. 这是最快但最不准确的方法。

2. 与前向差分相比，CENTRAL差分方法更准确，代价是函数评估次数的两倍，估计f′(x)通过计算 f(x+h)−f(x−h)2h.

3. 差分方法[Ridders]_ 是一种自适应方案，它RIDDERS通过在不同尺度上执行多个中心差分来估计导数。具体来说，该算法从某个特定位置开始h并且随着导数的估计，这个步长减小。为了保存函数评估和估计导数误差，该方法在测试步长之间执行理查森外推。该算法表现出相当高的准确性，但通过对成本函数的额外评估来做到这一点。

   考虑使用CENTRAL差异开始。根据结果，要么尝试前向差异以提高性能，要么尝试 Ridders 的方法来提高准确性。

warnning： 初次使用时，初学者的一个常见错误 NumericDiffCostFunction是尺寸错误。特别是，倾向于将模板参数设置为（残差的维度，参数的数量），而不是为每个参数传递一个维度参数。在上面的示例中，这将是，它缺少最后一个 参数。设置尺寸参数时请注意。``2

• 解析导数（AnalyticDerivatives）：当导数存在闭合解析形式时使用，用于可基于CostFunciton基类自行编写；但由于需要自行管理残差和雅克比矩阵，除非闭合解具有具有明显的精度和效率优势，否则同样不建议使用。

[例子]ICP-slam十四讲 自动求导实现（q t 分开）

/***********参数快 + 残差块**********/
  //版本1.0 : q t分开计算， 调用Ceres的库，不需要重构
  double para_q[4] = {0,0,0,1};
  double para_t[3] = {0,0,0};
  ceres::LocalParameterization *q_parameterization = new ceres::EigenQuaternionParameterization();
  problem.AddParameterBlock(para_q, 4, q_parameterization);
  problem.AddParameterBlock(para_t, 3);

  ceres::CostFunction *cost_function = VisualP3d_2d::Create(points_3d[i], points_2d[i], K_eigen);
    problem.AddResidualBlock(cost_function, nullptr, para_q, para_t);
/***********结构体 [注意：需要模板函数，内部全是模板参数才行]**********/
struct VisualP3d_2d
{
  // 版本1.0： q t分开，自动求导
  VisualP3d_2d(Eigen::Vector3d p3d_, Eigen::Vector2d p2d_, Eigen::Matrix3d K_) : P3d(p3d_), P2d(p2d_), K(K_) {}

  template <typename T>
	bool operator()(const T *q, const T *t, T *residual) const
	{
    Eigen::Quaternion<T> para_q{q[3], q[0], q[1], q[2]};
    Eigen::Matrix<T, 3, 1> para_t{t[0], t[1], t[2]};

    Eigen::Matrix<T, 3, 1> TP3d{T(P3d[0]),T(P3d[1]),T(P3d[2])};
    Eigen::Matrix<T, 3, 3> TK;
    TK << T(K(0, 0)), T(K(0, 1)), T(K(0, 2)),
        T(K(1, 0)), T(K(1, 1)), T(K(1, 2)),
        T(K(2, 0)), T(K(2, 1)), T(K(2, 2));
    Eigen::Matrix<T, 3, 1> p2d_p3d;

    p2d_p3d = TK * (para_q * TP3d + para_t);
    p2d_p3d /= p2d_p3d[2];

    residual[0] = T(P2d[0]) - T(p2d_p3d(0)); //
    residual[1] = T(P2d[1]) - T(p2d_p3d(1)); //
    return true;
	}

	static ceres::CostFunction *Create(const Eigen::Vector3d p3d_,const Eigen::Vector2d p2d_,const Eigen::Matrix3d K_ ){
		return (new ceres::AutoDiffCostFunction<VisualP3d_2d, 2,4,3>(
			new VisualP3d_2d(p3d_, p2d_, K_)));
	}

  Eigen::Matrix3d K;
  Eigen::Vector3d P3d;
  Eigen::Vector2d P2d;
};

[例子]例如loam-livox 的ICP的ceres 自动求导实现：

// p2p with motion deblur 点-点 ICP
template <typename _T>
struct ceres_icp_point2point_mb
{
    Eigen::Matrix<_T, 3, 1> m_current_pt;   // 当前的点
    Eigen::Matrix<_T, 3, 1> m_closest_pt;   // 最近的点
    _T m_motion_blur_s;                     // 用于畸变去除            
    // 上一次变换     
    Eigen::Matrix<_T, 4, 1> m_q_last;
    Eigen::Matrix<_T, 3, 1> m_t_last;
    _T m_weigh;
    ceres_icp_point2point_mb( const Eigen::Matrix<_T, 3, 1> current_pt,
                           const Eigen::Matrix<_T, 3, 1> closest_pt,
                           const _T &motion_blur_s = 1.0,
                           Eigen::Matrix<_T, 4, 1> q_s = Eigen::Matrix<_T, 4, 1>( 1, 0, 0, 0 ),
                           Eigen::Matrix<_T, 3, 1> t_s = Eigen::Matrix<_T, 3, 1>( 0, 0, 0 ) ) : m_current_pt( current_pt ),
                                                                                                m_closest_pt( closest_pt ),
                                                                                                m_motion_blur_s( motion_blur_s ),
                                                                                                m_q_last( q_s ),
                                                                                                m_t_last( t_s )

    {
        m_weigh = 1.0;
    };

    // operator() 重载计算残差，通过输入的参数，并返回 
    template <typename T>
    bool operator()( const T *_q, const T *_t, T *residual ) const
    {
        // 上一次的变换
        Eigen::Quaternion<T> q_last{ ( T ) m_q_last( 0 ), ( T ) m_q_last( 1 ), ( T ) m_q_last( 2 ), ( T ) m_q_last( 3 ) };
        Eigen::Matrix<T, 3, 1> t_last = m_t_last.template cast<T>();
        // 畸变去除
        Eigen::Quaternion<T> q_incre{ _q[ 3 ], _q[ 0 ], _q[ 1 ], _q[ 2 ] };
        Eigen::Matrix<T, 3, 1> t_incre{ _t[ 0 ], _t[ 1 ], _t[ 2 ] };
        Eigen::Quaternion<T> q_interpolate = Eigen::Quaternion<T>::Identity().slerp( ( T ) m_motion_blur_s, q_incre );
        Eigen::Matrix<T, 3, 1> t_interpolate = t_incre * T( m_motion_blur_s );
        // 当前的点
        Eigen::Matrix<T, 3, 1> pt{ T( m_current_pt( 0 ) ), T( m_current_pt( 1 ) ), T( m_current_pt( 2 ) ) };   
        // 当前点经过变换后的位置
        Eigen::Matrix<T, 3, 1> pt_transfromed;
        pt_transfromed = q_last * ( q_interpolate * pt + t_interpolate ) + t_last;

        residual[ 0 ] = ( pt_transfromed( 0 ) - T( m_closest_pt( 0 ) ) ) * T( m_weigh );
        residual[ 1 ] = ( pt_transfromed( 1 ) - T( m_closest_pt( 1 ) ) ) * T( m_weigh );
        residual[ 2 ] = ( pt_transfromed( 2 ) - T( m_closest_pt( 2 ) ) ) * T( m_weigh );
        return true;
    };

    static ceres::CostFunction *Create( const Eigen::Matrix<_T, 3, 1> current_pt,
                                        const Eigen::Matrix<_T, 3, 1> closest_pt,
                                        const _T motion_blur_s = 1.0,
                                        Eigen::Matrix<_T, 4, 1> q_s = Eigen::Matrix<_T, 4, 1>( 1, 0, 0, 0 ),
                                        Eigen::Matrix<_T, 3, 1> t_s = Eigen::Matrix<_T, 3, 1>( 0, 0, 0 ) )
    {
        return ( new ceres::AutoDiffCostFunction<        // 自动求导 
                 ceres_icp_point2point_mb, 3, 4, 3>(     // 对应的是operator中的维度
            new ceres_icp_point2point_mb( current_pt, closest_pt, motion_blur_s ) ) );
    }
};

​ 其中重载操作符()（必有）操作符()是一个模板方法，返回值为bool型，接受参数依次为待优化变量和残差变量，且待优化变量的传入方式应和Problem::AddResidualBlock()一致。

[例子] ICPslam十四讲解析解-se3（qt合成7维）：

注意：求解雅克比的时候，借用了F-LOAM的重构，q在前，t在后 ； 以及q的顺序问题等。

需要谨慎对齐。

/******************参数快 + 残差块*******************/
// 版本2.0： se3 代替 q t 分开的 || 手写雅克比
  double parameters[7] = {0,0,0,1,0,0,0};
  problem.AddParameterBlock(parameters, 7,  new PoseSE3Parameterization());

    ceres::CostFunction *cost_function = new VisualP3d2d_hand(points_3d[i], points_2d[i], K_eigen);
    problem.AddResidualBlock(cost_function, nullptr, parameters);
/******************参数快 + 残差块*******************/
class VisualP3d2d_hand : public ceres::SizedCostFunction<2, 7> {
	public:

		VisualP3d2d_hand(Eigen::Vector3d p3d_, Eigen::Vector2d p2d_, Eigen::Matrix3d K_) : P3d(p3d_), P2d(p2d_), K(K_) {}
		virtual ~VisualP3d2d_hand() {}

		virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const{
      Eigen::Map<const Eigen::Quaterniond> q(parameters[0]);
      Eigen::Map<const Eigen::Vector3d> t(parameters[0] + 4);

      Eigen::Vector3d p2d_p3d = q * P3d + t;
      p2d_p3d = K * p2d_p3d;
      p2d_p3d /= p2d_p3d[2];

      residuals[0] = P2d[0] - p2d_p3d(0); 
      residuals[1] = P2d[1] - p2d_p3d(1);

      if (jacobians != NULL)
      {
        if (jacobians[0] != NULL)
        {
          Eigen::Vector3d pos_cam = q * P3d + t;
          double fx = K(0, 0);
          double fy = K(1, 1);
          double cx = K(0, 2);
          double cy = K(1, 2);
          double X = pos_cam[0];
          double Y = pos_cam[1];
          double Z = pos_cam[2];
          double Z2 = Z * Z;

          Eigen::Matrix<double, 2, 3> parte_P;
          parte_P << -fx / Z, 0, fx * X / Z2,
              0, -fy / Z, fy * Y / Z2;
          Eigen::Matrix3d partP_I = Eigen::Matrix3d::Identity();
          Eigen::Matrix3d partP_qt;
          partP_qt = -skew(pos_cam);

          Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> J_se3(jacobians[0]);
          J_se3.setZero();
          J_se3.block<2, 3>(0, 0) <<  parte_P * partP_qt;
          J_se3.block<2, 3>(0, 3) <<  parte_P * partP_I;

        }
      }

      return true;
    }

    Eigen::Matrix3d K;
    Eigen::Vector3d P3d;
    Eigen::Vector2d P2d;
};

[例子]解析解，求雅克比:如何验证解析解正确性呢？？？

F-LOAM的手动解析解求导

   ceres::CostFunction *factor_analytic_edge =   new EdgeAnalyticCostFunction(
        source, 
        target_x, target_y, 
        ratio
   );
// #endif
    
    problem_.AddResidualBlock(
        factor_analytic_edge, 
        config_.loss_function_ptr, 
        param_.q, param_.t
    );

class EdgeAnalyticCostFunction : public ceres::SizedCostFunction<1, 4, 3>
{ // 优化参数维度：1     输入维度 ： q : 4   t : 3
public:
	double s;
	Eigen::Vector3d curr_point, last_point_a, last_point_b;
	EdgeAnalyticCostFunction(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_,
							 const Eigen::Vector3d last_point_b_, const double s_)
		: curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {}

	virtual bool Evaluate(double const *const *parameters,
						  double *residuals,
						  double **jacobians) const //   定义残差模型
	{
		Eigen::Map<const Eigen::Quaterniond> q_last_curr(parameters[0]); //   存放 w  x y z
		Eigen::Map<const Eigen::Vector3d> t_last_curr(parameters[1]);
		Eigen::Vector3d lp; //   line point
		Eigen::Vector3d lp_r;
		lp_r = q_last_curr * curr_point;
		lp = q_last_curr * curr_point + t_last_curr; //   new point
		Eigen::Vector3d nu = (lp - last_point_a).cross(lp - last_point_b);
		Eigen::Vector3d de = last_point_a - last_point_b;

		residuals[0] = nu.norm() / de.norm(); //  线残差

		//  归一单位化
		nu.normalize();

		if (jacobians != NULL)
		{
			if (jacobians[0] != NULL)
			{
				Eigen::Vector3d re = last_point_b - last_point_a;
				Eigen::Matrix3d skew_re = skew(re);
				Eigen::Matrix3d skew_de = skew(de);

				//  J_so3_Rotation
				Eigen::Matrix3d skew_lp_r = skew(lp_r);
				Eigen::Matrix3d dp_by_dr;
				dp_by_dr.block<3, 3>(0, 0) = -skew_lp_r;
				Eigen::Map<Eigen::Matrix<double, 1, 4, Eigen::RowMajor>> J_so3_r(jacobians[0]);
				J_so3_r.setZero();
				J_so3_r.block<1, 3>(0, 0) = nu.transpose() * skew_de * dp_by_dr / (de.norm() * nu.norm());

				//  J_so3_Translation
				Eigen::Matrix3d dp_by_dt;
				(dp_by_dt.block<3, 3>(0, 0)).setIdentity();
				Eigen::Map<Eigen::Matrix<double, 1, 3, Eigen::RowMajor>> J_so3_t(jacobians[1]);
				J_so3_t.setZero();
				J_so3_t.block<1, 3>(0, 0) = nu.transpose() * skew_de / (de.norm() * nu.norm());
			}
		}
		return true;
	}
};

class PlaneAnalyticCostFunction : public ceres::SizedCostFunction<1, 4, 3>
{
public:
	Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;
	Eigen::Vector3d ljm_norm;
	double s;

	PlaneAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_,
							  Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_)
		: curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_), last_point_m(last_point_m_), s(s_) {}

	virtual bool Evaluate(double const *const *parameters,
						  double *residuals,
						  double **jacobians) const
	{ //   定义残差模型
		// 叉乘运算， j,l,m 三个但构成的平行四边面积(摸)和该面的单位法向量(方向)
		Eigen::Vector3d ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m);
		ljm_norm.normalize(); //  单位法向量

		Eigen::Map<const Eigen::Quaterniond> q_last_curr(parameters[0]);
		Eigen::Map<const Eigen::Vector3d> t_last_curr(parameters[1]);

		Eigen::Vector3d lp;								 // “从当前阵的当前点” 经过转换矩阵转换到“上一阵的同线束激光点”
		Eigen::Vector3d lp_r = q_last_curr * curr_point; //  for compute jacobian o rotation  L: dp_dr
		lp = q_last_curr * curr_point + t_last_curr;

		// 残差函数
		double phi1 = (lp - last_point_j).dot(ljm_norm);
		residuals[0] = std::fabs(phi1);

		if (jacobians != NULL)
		{
			if (jacobians[0] != NULL)
			{
				phi1 = phi1 / residuals[0];
				//  Rotation
				Eigen::Matrix3d skew_lp_r = skew(lp_r);
				Eigen::Matrix3d dp_dr;
				dp_dr.block<3, 3>(0, 0) = -skew_lp_r;
				Eigen::Map<Eigen::Matrix<double, 1, 4, Eigen::RowMajor>> J_so3_r(jacobians[0]);
				J_so3_r.setZero();
				J_so3_r.block<1, 3>(0, 0) = phi1 * ljm_norm.transpose() * (dp_dr);

				Eigen::Map<Eigen::Matrix<double, 1, 3, Eigen::RowMajor>> J_so3_t(jacobians[1]);
				J_so3_t.block<1, 3>(0, 0) = phi1 * ljm_norm.transpose();
			}
		}
		return true;
	}
};

–2.4 其他成员函数

// 设定对应的参数模块在优化过程中保持不变
void Problem::SetParameterBlockConstant(double *values)
// 设定对应的参数模块在优化过程中可变
void Problem::SetParameterBlockVariable(double *values)
    
// 设定优化下界
void Problem::SetParameterLowerBound(double *values, int index, double lower_bound)
// 设定优化上界
void Problem::SetParameterUpperBound(double *values, int index, double upper_bound)
    
// 该函数紧跟在参数赋值后，在给定的参数位置求解Problem，给出当前位置处的cost、梯度以及Jacobian矩阵；
bool Problem::Evaluate(const Problem::EvaluateOptions &options, 
                       double *cost, vector<double>* residuals, 
                       vector<double> *gradient, CRSMatrix *jacobian)

–2.5 求解

  // 配置求解器
  ceres::Solver::Options options;     // 这里有很多配置项可以填
  options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;  // 密集增量cholesky||增量方程如何求解
  options.minimizer_progress_to_stdout = true;   // 输出到cout

  ceres::Solver::Summary summary;                // 优化信息
  ceres::Solve(options, &problem, &summary);  // 开始优化 || 配置-最小二乘问题-优化信息

ceres::Solver::Options options;
// options.max_num_iterations = 7;
options.linear_solver_type = ceres::DENSE_SCHUR; // 稠密舒尔补
options.trust_region_strategy_type = ceres::DOGLEG; // 狗腿法
options.minimizer_progress_to_stdout = false;

ceres::Solver::Summary summary;

TicToc solveTime;
ceres::Solve(options, &problem, &summary);

ceres::Solve(options, &problem, &summary);

3. 成品例子

#include 
#include 
#include 
#include 

using namespace std;

// 代价函数的计算模型
struct CURVE_FITTING_COST {
  CURVE_FITTING_COST(double x, double y) : _x(x), _y(y) {}

  // 残差的计算
  template<typename T>
  bool operator()(
    const T *const abc, // 模型参数，待优化的参数，有3维
    T *residual) const {
    residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]); // y-exp(ax^2+bx+c)  //残差，也就是代价函数的输出
    return true;
  }

  const double _x, _y;    // x,y数据
};

int main(int argc, char **argv) {
  double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
  double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值
  int N = 100;                                 // 数据点
  double w_sigma = 1.0;                        // 噪声Sigma值
  double inv_sigma = 1.0 / w_sigma;
  cv::RNG rng;                                 // OpenCV随机数产生器

  vector<double> x_data, y_data;      // 数据
  for (int i = 0; i < N; i++) {
    double x = i / 100.0;
    x_data.push_back(x);
    y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
  }

  double abc[3] = {ae, be, ce};

  // 构建最小二乘问题
  ceres::Problem problem;
  for (int i = 0; i < N; i++) {
    problem.AddResidualBlock(     // 向问题中添加误差项
      // 使用自动求导，将定义的代价函数结构体传入。模板参数：误差类型，输出维度即残差的维度，输入维度即优化参数的维度，维数要与前面struct中一致
      new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(
        new CURVE_FITTING_COST(x_data[i], y_data[i])
      ),
      nullptr,            // 核函数，这里不使用，为空
      abc                 // 待估计参数
    );
  }

  // 配置求解器
  ceres::Solver::Options options;     // 这里有很多配置项可以填
  options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;  // 增量方程如何求解
  //options.linear_solver_type = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = true;   // 输出到cout

  ceres::Solver::Summary summary;                // 优化信息
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  ceres::Solve(options, &problem, &summary);  // 开始优化，求解
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

  // 输出结果
  cout << summary.BriefReport() << endl;   //输出优化的简要信息
  cout << "estimated a,b,c = ";
  for (auto a:abc) cout << a << " ";
  cout << endl;

  return 0;
}

cmakelists.txt:

cmake_minimum_required(VERSION 2.8)
project(gaussnewton)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
    
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})
include_directories("/usr/include/eigen3")
    
set(SOURCE_FILES main.cpp)
add_executable(gaussnewton ${SOURCE_FILES})
target_link_libraries(gaussnewton ${OpenCV_LIBS})
target_link_libraries(gaussnewton ${CERES_LIBRARIES})