SLAM学习——Ceres

版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。

本文链接： https://blog.csdn.net/Kalenee/article/details/100183433

一、安装配置

• 依赖

# CMake
sudo apt-get install cmake
# google-glog + gflags
sudo apt-get install libgoogle-glog-dev
# BLAS & LAPACK
sudo apt-get install libatlas-base-dev
# Eigen3
sudo apt-get install libeigen3-dev
# SuiteSparse and CXSparse (optional)
# - If you want to build Ceres as a *static* library (the default)
#   you can use the SuiteSparse package in the main Ubuntu package
#   repository:
sudo apt-get install libsuitesparse-dev

• 安装

tar zxf ceres-solver-1.14.0.tar.gz
mkdir ceres-bin
cd ceres-bin
cmake ../ceres-solver-1.14.0
make -j3
make test
# Optionally install Ceres, it can also be exported using CMake which
# allows Ceres to be used without requiring installation, see the documentation
# for the EXPORT_BUILD_DIR option for more information.
make install

• CMakeLists.txt配置

# 添加cmake模块以使用ceres库
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )

# 寻找Ceres库并添加它的头文件
find_package( Ceres REQUIRED )
include_directories( ${CERES_INCLUDE_DIRS} )

# 添加CERES_LIBRARIES依赖
add_executable(main  # 输出名为main的可执行文件
   ./src/main.cpp
)
target_link_libraries( main ${CERES_LIBRARIES} )

二、使用

求解步骤

1. 定义Cost Function(损失函数)模型，也就是寻优的目标式。这个部分使用仿函数（functor）来实现，做法是定义一个Cost Function的结构体，在结构体内重载()运算符。
2. 通过代价函数构建待求解的优化问题。
3. 配置求解器参数并求解问题，这个步骤就是设置方程怎么求解、求解过程是否输出等，然后调用一下Solve方法。

2.1 构造代价函数结构体

• 仿函数

仿函数(functor)，就是使一个类的使用看上去象一个函数。其实现就是类中实现一个operator()，这个类就有了类似函数的行为，就是一个仿函数类了。

此处实现Myclass类，重载operator()可实现仿函数，即建立类后，可以将类作为函数，类似Myclass()。

#include<iostream>
class Myclass{
public:
  Myclass(int x):_x(x){};
  int operator()(const int n)const{
    return n*_x;
  }
private:
    int _x;
};

int main(){
    Myclass Obj1(5);
    std::cout<<Obj1(3)<<std::endl;
}

• 一次函数

$y = \frac{1}{2} \cdot (10-x)^2$

求 $x$使得 $y$最小（最接近0）

struct CostFunctor
{
    template <typename T>
    bool operator()(const T *const x,// 模型参数，一维
                          T *residual) const // 残差  
    {
        residual[0] = T(10.0) - x[0];
        return true;
    }
};

• 曲线拟合

$y = exp(ax^2+bx+c)+w$

假设有一条满足该方程的曲线，其中 $a,b,c$为曲线参数， $w$为噪声(理想条件下为0)

struct CURVE_FITTING_COST
{
    CURVE_FITTING_COST(double x,double y):x_(x),y_(y){}
    template <template 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;
};

2.2 构建待求解的优化问题

Problem problem;
CostFunction* cost_function =
      new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
problem.AddResidualBlock(cost_function, NULL, &x);

1. 设置优化梯度，可选项为：

• Ceres 的自动求导（Auto Diff）（最简单）

  CostFunction* cost_function =
        new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
  

  自动求导需要指定误差项和优化变量的维度，此处误差为标量，维度为1，一次函数优化值维度为1，设置为(1,1)，曲线拟合优化值维度为3，设置为(1,3)。

• 数值求导（Numeric Diff）

  CostFunction* cost_function =
    new NumericDiffCostFunction<CostFunctor, ceres::CENTRAL, 1, 1>(new CostFunctor);
  

  一般比自动求导法收敛更慢，且更容易出现数值错误，其构造上相比自动求导法多出一个模板参数ceres::CENTRAL。

• 自行推导解析的导数形式，提供给 Ceres。

2. 调用 AddResidualBlock 将误差项添加到目标函数中

problem.AddResidualBlock(cost_function, nullptr, &x);

• nullptr，核函数，可用于数据处理，比如去除离散点等。
• &x，待优化参数。

2.3 配置问题并求解

Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;	// 配置增量方程的解法
options.minimizer_progress_to_stdout = true;	// 输出到cout
Solver::Summary summary;						// 优化信息
Solve(options, &problem, &summary);				// 开始优化

• option，优化选项，涉及优化方法，迭代次数，步长等等，详细可参考solver-options
• Summary，优化信息

三 应用

3.1 解方程

#include <iostream>
#include <ceres/ceres.h>

using namespace std;
using namespace ceres;

// 第一部分：构建代价函数，重载()符号，仿函数的小技巧
// 求x使得1/2*(10-x)^2取到最小值
struct CostFunctor
{
    template <typename T>
    bool operator()(const T *const x,  // 模型参数，一维
                    T *residual) const // 残差
    {
        residual[0] = T(10.0) - x[0];
        return true;
    }
};

int main(int argc, char **argv)
{
    google::InitGoogleLogging(argv[0]);

    // 一次函数，寻优参数x的初始值，为5
    double initial_x = 20.0;
    double x = initial_x;

    // 第二部分：构建寻优问题
    Problem problem;
    // 使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
    CostFunction *cost_function =
        new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
    // 向问题中添加误差项，本问题比较简单，添加一个就行。
    problem.AddResidualBlock(cost_function, NULL, &x);               

    // 第三部分： 配置并运行求解器
    Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
    options.minimizer_progress_to_stdout = true;  //输出到cout
    Solver::Summary summary;                      //优化信息
    Solve(options, &problem, &summary);           //求解!!!

    // 输出结果
    std::cout << summary.BriefReport() << "\n"; //输出优化的简要信息
                                                //最终结果
    std::cout << "x : " << initial_x
              << " -> " << x << "\n";
}

3.2 曲线拟合

#include <iostream>
#include <ceres/ceres.h>
#include <opencv2/core/core.hpp>

using namespace std;
using namespace ceres;

// 第一部分：构建代价函数，重载()符号，仿函数的小技巧
// 拟合曲线
// 曲线方程: y = exp(ax^2+b^x+c)+w
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)
{
    google::InitGoogleLogging(argv[0]);

    // 拟合曲线
    double a = 3, b = 2, c = 1, w = 1;
    cv::RNG rng;
    double abc[3] = {0, 0, 0};

    vector<double> x_data(1000), y_data(1000);
    for (int i = 0; i < 1000; i++)
    {
        double x_tmp = i / 1000.0;
        x_data[i] = x_tmp;
        y_data[i] = ceres::exp(a * x_tmp * x_tmp + b * x_tmp + c) + rng.gaussian(w);
    }

    // 第二部分：构建寻优问题i
    Problem problem;
    for (int i = 0; i < 1000; i++)
    {
        problem.AddResidualBlock(
            new AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(new CURVE_FITTING_COST(x_data[i], y_data[i])),
            nullptr,
            abc);
    }

    // 第三部分： 配置并运行求解器
    Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
    options.minimizer_progress_to_stdout = true;  //输出到cout
    Solver::Summary summary;                      //优化信息
    Solve(options, &problem, &summary);           //求解!!!

    // 输出结果
    std::cout << summary.BriefReport() << "\n"; //输出优化的简要信息
                                                //最终结果
    std::cout << "a : " << abc[0] << 
        		" b : " << abc[1] << 
        		" c : " << abc[2] << std::endl;
    return 0;
}

3.3 鲁棒曲线拟合

求解优化问题中（比如拟合曲线），数据中往往会有离群点、错误值什么的，最终得到的寻优结果很容易受到影响，此时就可以使用一些损失核函数来对离群点的影响加以消除。要使用核函数，只需要把上述代码中的NULL或nullptr换成损失核函数结构体的实例。

Ceres库中提供的核函数主要有：TrivialLoss 、HuberLoss、 SoftLOneLoss 、 CauchyLoss。

比如此时要使用CauchyLoss，只需要将nullptr换成new CauchyLoss(0.5)就行（0.5为参数）

Problem problem;
for (int i = 0; i < 1000; i++)
{
    problem.AddResidualBlock(
        new AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(new CURVE_FITTING_COST(x_data[i], y_data[i])),
        new CauchyLoss(0.5),
        abc);
}

参考

Ceres Solver

Ceres入门