从零开始学SLAM: Ceres/G2O求解优化问题

Ceres求解优化问题

从《视觉SLAM十四讲》和ceres的tutorial开始学起，同时复习一下C++中的语法。

struct CURVE_FITTING_COST
{
        CURVE_FITTING_COST ( double x,double y): _x(x),_y(y) {}
        template <typename T>
        bool operator()(
                        const T* const abc,
                        T* residual
                        )const
        {
                residual[0]=T(_y)-ceres::exp(abc[0]*T(_x)*T(_x)+abc[1]*T(_x)+abc[2]);
                return true;
        }
        const double _x,_y;
}

要使用Ceres，首先要定义求解问题的cost function<CURVE_FITTING_COST>。
在结构体中首先构造构造函数：

CURVE_FITTING_COST ( double x,double y): _x(x),_y(y) {} //结构体的构造函数初始化列表
//相当于CURVE_FITTING_COST ( double x,double y) {_x=x; _y=y}

接下来，定义模板template

 template <typename T>

template的使用是为了简化不同类型的函数和类的重复定义，模版实例化时可以替换任意类型，不仅包括内置类型（int等），也包括自定义类型class。编译器在实例化之后才知道的数据的类型。

bool operator()( const T* const abc,  T* residual )const
        {
                residual[0]=T(_y)-ceres::exp(abc[0]*T(_x)*T(_x)+abc[1]*T(_x)+abc[2]);
                return true;
        }

重载（）符号，仿函数的小技巧，使结构体的一个实例具有类似一个函数的性质，在代码编写过程中能当做一个函数一样来使用。
对结构体、类的一个实例，比如Myclass类的一个实例Obj1，如果Myclass里对（）进行了重载，那Obj1被创建之后，就可以将Obj1这个实例当做函数来用，比如Obj（x）。在这里CURVE_FITTING_COST（）中共传入了两个变量，包括3维参数向量abc以及残差residual。

int main ( int argc, char** argv )
{
    double a=1.0, b=2.0, c=1.0;         // 真实参数值
    int N=100;                          // 数据点
    double w_sigma=1.0;                 // 噪声Sigma值
    cv::RNG rng;                        // OpenCV随机数产生器
    double abc[3] = {0,0,0};            // abc参数的估计值

    vector<double> x_data, y_data;      // 数据

    cout<<"generating data: "<<endl;
    for ( int i=0; i<N; i++ )
    {
        double x = i/100.0;
        x_data.push_back ( x );
        y_data.push_back (
            exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
        );
        cout<<x_data[i]<<" "<<y_data[i]<<endl;
    }

    // 构建最小二乘问题
    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_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;
}

这里主要对之前我们构建的costfunction是怎么使用的进行讲解，也就是下面的代码

        problem.AddResidualBlock (   
            new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3> ( 
                new CURVE_FITTING_COST ( x_data[i], y_data[i] )
            ),
            nullptr,       
            abc          
        );

这一段代码可以写成另外一种简单的形式

       CostFunction* cost_function = 
       new AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(new CURVE_FITTING_COST( x_data[i], y_data[i] ));
       problem.AddResidualBlock (cost_function， nullptr, abc );

其中，我们第一段代码new CURVE_FITTING_COST( x_data[i], y_data[i] )调用了结构体的构造函数，而<CURVE_FITTING_COST, 1, 3>中，1代表残差维度，3代表待优化的参数维度。回顾之前的构造函数，重载（）时传入的参数分别是优化变量和残差（残差就是代价函数的输出）。然后将CURVE_FITTING_COST传入AutoDiffCostFunction方法来构建寻优问题。
第二段是向问题中添加误差项，nullptr表示空指针（相较于NULL，优点在于在编译器进行解释程序时，NULL会被直接解释成0，变为一个数字，而nullptr不会），abc为待寻优参数。

for ( auto a:abc ) 

表示的则是基于范围的for循环，例如

#include<iostream>
#include<stdlib.h>
#include<string> 
using namespace std; 
int main() 
{ 
  int arr[] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 }; 
  for (auto val : arr) 
  { cout << val << " "; } 
  system("pause"); 
  return 0;
 } 
//输出结果： 1 2 3 4 5 6 7 8 9 10 

CMakeList代码为

cmake_minimum_required(VERSION 2.8)
project(ceres)
find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})
add_executable(use_ceres use_ceres.cpp)
target_link_libraries(use_ceres ${CERES_LIBRARIES})

参考
https://blog.csdn.net/cqrtxwd/article/details/78956227 一文助你Ceres 入门——Ceres Solver新手向全攻略
https://blog.csdn.net/lixiaogang_theanswer/article/details/79969012 基于范围的for循环
https://blog.csdn.net/fightingforcv/article/details/51472586 C++中的模板template
http://www.ceres-solver.org/nnls_tutorial.html Ceres: Tutorial: Non-linear Least Squares
https://blog.csdn.net/tianshuai1111/article/details/7687983 【C++ STL】深入解析神秘的 — 仿函数

G2O解决优化问题

在代码的编译过程中，出现了无法找到FindG2O.cmake的问题，解决这个问题的方法可以将g2o下载文件中cmake_modules文件夹下的对应cmake文件拷进usr/share/cmake-3.10/Modules下，在自己package中加入

list( APPEND CMAKE_MODULE_PATH /home/g2o/cmake_modules)
set(G2O_ROOT /home/g2o)

可以解决这个问题。
在解决这个问题之后，由于使用的ubuntu版本为18.04，高翔的代码需要作出修改，详情可以参考 https://blog.csdn.net/robinhjwy/article/details/78084210

修改后代码为

#include <iostream>
#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 <g2o/solvers/csparse/linear_solver_csparse.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() // 重置
    {
        _estimate << 0,0,0;
    }
    
    virtual void oplusImpl( const double* update ) // 更新
    {
        _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) {}
    // 计算曲线模型误差
    void computeError()
    {
        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 bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
public:
    double _x;  // x 值， y 值为 _measurement
};

int main( int argc, char** argv )
{
    double a=1.0, b=2.0, c=1.0;         // 真实参数值
    int N=100;                          // 数据点
    double w_sigma=1.0;                 // 噪声Sigma值
    cv::RNG rng;                        // OpenCV随机数产生器
    double abc[3] = {0,0,0};            // abc参数的估计值

    vector<double> x_data, y_data;      // 数据
    
    cout<<"generating data: "<<endl;
    for ( int i=0; i<N; i++ )
    {
        double x = i/100.0;
        x_data.push_back ( x );
        y_data.push_back (
            exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
        );
        cout<<x_data[i]<<" "<<y_data[i]<<endl;
    }
    
// 初始化g2o 
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;
//std::unique_ptr<Block::LinearSolverType> linearSolver ( new g2o::LinearSolverDense<Block::PoseMatrixType>()); 
std::unique_ptr<Block::LinearSolverType> linearSolver ( new g2o::LinearSolverDense<Block::PoseMatrixType>());
std::unique_ptr<Block> solver_ptr ( new Block ( std::move(linearSolver))); 
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr)); 
g2o::SparseOptimizer optimizer; 
optimizer.setAlgorithm (solver);
    optimizer.setVerbose( true );       // 打开调试输出
    
    // 往图中增加顶点
    CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate( Eigen::Vector3d(0,0,0) );
    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(100);
    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 2.8 )
project( g2o_curve_fitting )

set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

# 添加cmake模块以使用ceres库

list( APPEND CMAKE_MODULE_PATH /home/g2o/cmake_modules)
set(G2O_ROOT /home/g2o)
find_package(G2O REQUIRED)
include_directories(
${G2O_INCLUDE_DIRS}
"/usr/include/eigen3"
)
find_package( OpenCV REQUIRED )

# OpenCV
include_directories( ${OpenCV_DIRS} )

add_executable( curve_fitting main.cpp )
# 与G2O和OpenCV链接
target_link_libraries( curve_fitting 
    ${OpenCV_LIBS}
    g2o_core g2o_stuff
)