c++学习【21】g2o

  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));
  }

改写
在函数参数中使用引用可以避免拷贝数据，提高程序效率。引用的语法是在变量名前加上 & 符号。普通参数和引用参数的区别在于，普通参数会创建一个新的变量来存储传递进来的值，而引用参数则直接使用原变量的别名。另外，普通参数传递的是变量的值，而引用参数传递的是变量的地址。

#include <vector>
#include <cmath>
#include <opencv2/core/core.hpp>

void generateData(double ar, double br, double cr,
                 cv::RNG rng, double w_sigma, int N, 
                 std::vector<double>& x_data, 
                 std::vector<double>& 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));
    }
}

int main() {
    // 设置随机数种子
    cv::RNG rng(cv::getTickCount());

    // 真实参数值
    double ar = 1.0, br = 2.0, cr = 1.0;

    // 噪声sigma值
    double w_sigma = 1.0;

    // 数据数量
    int N = 100;

    // 产生数据
    std::vector<double> x_data, y_data;
    generateData(ar, br, cr, rng, w_sigma, N, x_data, y_data);

    // ...
    return 0;
}

  // 构建图优化，先设定g2o
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;  // 每个误差项优化变量维度为3，误差值维度为1
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型

  // 梯度下降方法，可以从GN, LM, DogLeg 中选
  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
    g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));

1)auto 关键字可以用于自动推导变量类型 
2)solver = new g2o::OptimizationAlgorithmGaussNewton()用于创建一个 g2o::OptimizationAlgorithmGaussNewton 类型的对象，并将其指针赋值给 solver 变量，g2o::OptimizationAlgorithmGaussNewton 是一个 G2O 中的优化算法类。
3）BlockSolverType 类型用于表示一种稠密矩阵块求解器，LinearSolverType 则表示一种用于求解线性方程的稠密矩阵求解器。
4）g2o::make_unique<>
5）g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>>这是一个数据类型，是g2o库中的一个求解器类型，用于解决由6个块和3个块特征组成的问题：6是相机位姿3是地图点的三维坐标
6）这里相机位姿通常使用6个自由度来表示，这是因为相机在三维空间中可以进行平移和旋转。平移包含三个自由度（x、y和z），表示相机在空间中的位置。旋转包含三个自由度，通常使用欧拉角（pitch、yaw和roll）或四元数来表示相机的朝向和旋转角度。
 

创建图

typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;
// 每个误差项优化变量维度为3，误差值维度为1
/*定义一种Block类型
BlockSolver是一个使用稀疏矩阵块方式解决非线性优化问题的求解器*/

/*************** 第1步：创建一个线性求解器LinearSolver*************************/
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); 


/*************** 第2步：创建BlockSolver。并用上面定义的线性求解器初始化**********/
Block* solver_ptr = new Block( linearSolver );      


/*************** 第3步：创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化****/
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );


/*************** 第4步：创建图优化的核心：稀疏优化器（SparseOptimizer）**********/
g2o::SparseOptimizer optimizer;     // 图模型
optimizer.setAlgorithm( solver );   // 设置求解器
optimizer.setVerbose( true );       // 打开调试输出

BaseVertex是g2o中定义顶点的通用类模板，有两个参数D/T
D 是int 类型，表示vertex的最小维度
T 是待估计vertex的数据类型
9）_estimate表示顶点的估计值。（_estimate的初始化值通常设置为问题的初始估计值，然后在优化过程中通过非线性最小化方法更新_estimate的值，最终得到问题的最优参数估计值。在优化完成后，用户可以通过访问_estimate来获取最后的优化结果..))))））））。。.
10)update在g2o内部
11）edge->setVertex(0, vertex_pose);0是什么？

定义顶点类

包含四个步骤：

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;
      }
  }

7）顶点的更新是基于最小化代价函数的过程，通过计算代价函数的梯度和Hessian矩阵，使得代价函数的值最小化
8）_jacobianOplusXi是g2o库中边的一个成员变量，用于表示边的雅可比矩阵。
其行数等于边的维度（即代价函数的维度）

创建顶点实例

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

 定义 边

图优化中的边：BaseUnaryEdge，BaseBinaryEdge，BaseMultiEdge 分别表示一元边，两元边，多元边。用到的成员变量和成员函数：
一般模板

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
  }

setId(int);  // 定义边的编号（决定了在H矩阵中的位置）
setMeasurement(type);  // 定义观测值
setVertex(int, vertex);  // 定义顶点
setInformation();  // 定义协方差矩阵的逆

添加 边 到图

// 往图中增加边
    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) ); // 信息矩阵：协方差矩阵之逆
        optimizer.addEdge( edge );
    }

第6步：

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

optimizer.initializeOptimization();
optimizer.optimize(100);    //设置迭代次数