G2O 小记1: 验证解析偏导的正确性

Posted 2022-01-14 wkangk

G2O 小记1: 验证解析偏导的正确性

<[e-mail] wkangk>

标签: g2o 图优化 偏导 正确性 解析 验证

目录

• G2O 小记1: 验证解析偏导的正确性
• • 说明
  • 示例

说明

当我们推导出偏导数的解析解时, 可以与数值微分进行对比, 来判断我们推导或实现的解析解是否正确. G2O
中 BaseFixedSizedEdge 下的 linearizeOplusN() 计算的就是数值偏导, 因此我们就可以利用这个函数的计算结果来与我们的解析解进行对比. 具体的请参考下面的示例中的 CheckGradients() 函数.

template <int D, typename E, typename... VertexTypes>
class BaseFixedSizedEdge : public BaseEdge<D, E> 
...

template <int N>
void linearizeOplusN();

...

Note:
(1) 值的注意的一点是 template void linearizeOplusN(); 中的模板参数 N 指的是我们要计算哪个顶点的偏导, 这里的 0, 1, 2, … 应该与我们连接顶点时的顺序对起来, 也就是 edge->setVertex(0, v); 使设置的顶点顺序.
(2) 关于解析解与数值解的相似程度, 这个我是参考了之前吴恩达老师一个课程中的计算方法, 老师说一般 diff 小于 1e-9 就认为它们非常相似, 但是我觉得这个自己觉得好差不多就可以了, -5, -6, -7, -8 也不是不行

示例

示例修改自 《视觉slam十四讲》 第 6 讲 g2oCurveFutting.cpp
该示例生成带有噪声的数据, 拟合曲线
y = exp(ax² + bx + c) + w
w~(0, σ²)

experiemnt_g2o_check_analysis_jac.cpp

/***************************************************************
Copyright © wkangk <[e-mail]>
文件名		: experiemnt_g2o_check_analysis_jac.cpp
作者	  	: wkangk <[e-mail]>
版本	   	: v1.0
描述	   	: 利用数值求导, 验证我们计算的解析解对不对
时间	   	: 2021-12-25 16:28
***************************************************************/
#include <iostream>
#include <g2o/core/g2o_core_api.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>

#include <cmath>
#include <chrono>

using namespace std;

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  // 重置
  virtual void setToOriginImpl() override 
    _estimate << 0, 0, 0;
  

  // 更新
  virtual void oplusImpl(const double *update) override 
    _estimate += Eigen::Vector3d(update);
  

  // 存盘和读盘：留空
  virtual bool read(istream &in) 

  virtual bool write(ostream &out) const 
;

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) 

  // 计算曲线模型误差
    virtual void computeError() override 
        static int i = 0;
        const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        _error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
    

  // 计算雅可比矩阵
    virtual void linearizeOplus() override 
        const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
        _jacobianOplusXi[0] = -_x * _x * y;
        _jacobianOplusXi[1] = -_x * y;
        _jacobianOplusXi[2] = -y;
        
        CheckGradients();
    

    /**
     *     通过比较梯度的解析值与数值求解结果, 来验证我们计算的梯度是否正确
     * @return     无
     */
    void CheckGradients() 
    
        Eigen::Vector3d analysis = _jacobianOplusXi.block<1, 3>(0, 0);
        // cout << "解析解: " <<  analysis.transpose() << endl;

        linearizeOplusN<0>();
        Eigen::Vector3d numerical = _jacobianOplusXi.block<1, 3>(0, 0);

        // cout << "数值解: " <<  numerical.transpose() << endl;

        double diff = (numerical - analysis).norm() / (numerical + analysis).norm();
        cout << "diff: " << diff << endl;
    

    virtual bool read(istream &in) 

    virtual bool write(ostream &out) const 

public:
    double _x;  // x 值， y 值为 _measurement
;


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

  // 构建图优化，先设定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>()));
  g2o::SparseOptimizer optimizer;     // 图模型
  optimizer.setAlgorithm(solver);   // 设置求解器
  optimizer.setVerbose(true);       // 打开调试输出

  // 往图中增加顶点
  CurveFittingVertex *v = new CurveFittingVertex();
  v->setEstimate(Eigen::Vector3d(ae, be, ce));
  v->setId(0);
  optimizer.addVertex(v);

  // 往图中增加边
  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);
  

  // 执行优化
  cout << "start optimization" << endl;
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  optimizer.initializeOptimization();
  optimizer.optimize(1);
  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;

  // 输出优化值
  Eigen::Vector3d abc_estimate = v->estimate();
  cout << "estimated model: " << abc_estimate.transpose() << endl;

  return 0;

CMakeLists.txt

cmake_minimum_required(VERSION 3.0)
project(experiment_g2o)
set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_FLAGS "-std=c++14 -O3")

# FindG2O.cmake 所在的路径, 要设置成自己的
list(APPEND CMAKE_MODULE_PATH "/home/wkangk/slam/slambook2/ch6/cmake")


# opencv 
find_package( OpenCV  REQUIRED )
include_directories( $OpenCV_INCLUDE_DIRS )

find_package(Eigen3  REQUIRED)
include_directories($EIGEN3_INCLUDE_DIR)

find_package(G2O REQUIRED)
include_directories($G2O_INCLUDE_DIRS)


add_executable(experiemnt_g2o_check_analysis_jac experiemnt_g2o_check_analysis_jac.cpp)
target_link_libraries(experiemnt_g2o_check_analysis_jac $OpenCV_LIBS $G2O_CORE_LIBRARY $G2O_STUFF_LIBRARY)

结果