SLAM学习笔记

编译环境ubuntu20.04 vscode

ceres库2.0.0

g2o库同gaoxiang12/slambook2中的版本号一致

cmake文件

cmake_minimum_required(VERSION 2.8)
project(ch6)

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

list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})

find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})

find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})

include_directories("/usr/include/eigen3")

add_executable(GN GN.cpp)
target_link_libraries(GN ${OpenCV_LIBS})

add_executable(doceres doceres.cpp)
target_link_libraries(doceres ${OpenCV_LIBS} ${CERES_LIBRARIES})

add_executable(dog2o dog2o.cpp)
target_link_libraries(dog2o ${OpenCV_LIBS} ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY})

手写高斯牛顿

#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;

int main()
{
    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;
    double inv_sigma = 1.0 /w_sigma;
    cv::RNG rng;

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

    int iterations = 100;
    double cost = 0, lastCost =0;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    for (int i = 0; i < iterations; i++)
    {
        Matrix3d H = Matrix3d::Zero();
        Vector3d b = Vector3d::Zero();
        cost = 0;

        for (int j = 0; j < N; j++)
        {
            double xj = x_data[j], yj = y_data[j];
            double error = yj - exp(ae*xj*xj+be*xj+ce);
            Vector3d J;
            J[0] = -xj*xj*exp(ae*xj*xj+be*xj+ce);
            J[1] = -xj*exp(ae*xj*xj+be*xj+ce);
            J[2] = -exp(ae*xj*xj+be*xj+ce);

            H += inv_sigma * inv_sigma *J *J.transpose();
            b += -inv_sigma*inv_sigma*error*J;

            cost+=error*error;
        }
        
        Vector3d dx = H.ldlt().solve(b);
        if(isnan(dx[0]))
        {
            cout<< "result is nan!"<<endl;
            break;
        }

        if(i > 0 && cost >= lastCost)
        {
            cout<<"cost: "<<cost<<" >= last cost: "<<lastCost<<", break."<<endl;
            break;
        }

        ae += dx[0];
        be += dx[1];
        ce += dx[2];

        lastCost = cost;

        cout<<"total cost : "<<cost<<", \t\tupdate: "<<dx.transpose()
            <<"\t\testimated params: "<<ae<<" , "<<be<<" , "<<ce<<endl;
    }

    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<<"estimate abc = "<<ae<<" , "<<be<<" , "<<ce<<endl;
    return 0;
    
       
}

使用ceres

#include <iostream>
#include <opencv2/opencv.hpp>
#include <ceres/ceres.h>
#include <chrono>
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,
        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;

};

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;                                                       
    double inv_sigma = 1.0 /w_sigma;                                           
    cv::RNG rng;                                                                

    vector<double> x_data, y_data;                                             
    for (size_t 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;                                               //ceres构建最小二乘问题
    for (int i = 0; i < N; i++)
    {
        problem.AddResidualBlock                                          //添加误差项
        (
            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;            //现行求解器，cholesky
    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;
}

使用g2o

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

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

编译出问题，如果代码完全一致，要么是g2o太新，要么是ceres太旧，自行调整；

有的版本解答将auto的那边改为自动指针，我试了还是显示ceres库中未声明data和fill，还得是换g2o的库