SLAM学习——Ceres

最新推荐文章于 2023-11-25 10:06:16 发布

jack_201316888

最新推荐文章于 2023-11-25 10:06:16 发布

阅读量315

点赞数 1

分类专栏： SLAM

原文链接：https://blog.csdn.net/kalenee/article/details/100183433

版权

SLAM 专栏收录该内容

44 篇文章 5 订阅

订阅专栏

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

二、使用

求解步骤

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

2.1 构造代价函数结构体

2.1.1 自动求导

自动求导构造误差函数变量需要全部为T类型，operator()为模板函数。

一次函数

y = 1 2 ⋅ ( 10 − x ) 2 y = \frac{1}{2} \cdot (10-x)^2y=21⋅(10−x)2

求x xx使得y yy最小（最接近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 = e x p ( a x 2 + b x + c ) + w y = exp(ax^2+bx+c)+wy=exp(ax2+bx+c)+w

假设有一条满足该方程的曲线，其中a , b , c a,b,ca,b,c为曲线参数，w ww为噪声(理想条件下为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.1.2 数值求导

数值求导得误差函数变量不需要变换为T类型。
一次函数

struct NumericDiffCostFunctor {
    bool operator()(const double* const x, double* residual) const {
        residual[0] = 10.0 - x[0];
        return true;
    }
};

2.1.3 解析导数

某种情况下，在封闭形式下计算导数比自动微分依赖的链式法则计算更有效。在这种情况下，需要你自己提供计算剩余函数和Jacobian矩阵的代码，根据编译时参数和剩余量(residuals)的维数定义CostFunction或SizedCostFunction的子类。

注意：Jacobian矩阵为误差函数关于优化参数的偏导

链式法则，连锁律、链式法则或链锁定则（英语：chain rule），是求复合函数导数的一个法则：
y = f ( g ( x ) ) ⇔ y = f ( u ) , u = g ( x ) d y d x = d y d g d g d x y = f(g(x))\Leftrightarrow y =f(u),u = g(x)\\ \frac{dy}{dx} = \frac{dy}{dg}\frac{dg}{dx}y=f(g(x))⇔y=f(u),u=g(x)dxdy=dgdydxdg

定义

template <typename CostFunctor,
       int kNumResiduals,  // Number of residuals, or ceres::DYNAMIC.
       int N0,       // Number of parameters in block 0.
       int N1 = 0,   // Number of parameters in block 1.
       int N2 = 0,   // Number of parameters in block 2.
       int N3 = 0,   // Number of parameters in block 3.
       int N4 = 0,   // Number of parameters in block 4.
       int N5 = 0,   // Number of parameters in block 5.
       int N6 = 0,   // Number of parameters in block 6.
       int N7 = 0,   // Number of parameters in block 7.
       int N8 = 0,   // Number of parameters in block 8.
       int N9 = 0>   // Number of parameters in block 9.
class AutoDiffCostFunction : public
SizedCostFunction<kNumResiduals, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> {
 public:
  explicit AutoDiffCostFunction(CostFunctor* functor);
  // Ignore the template parameter kNumResiduals and use
  // num_residuals instead.
  AutoDiffCostFunction(CostFunctor* functor, int num_residuals);
};

kNumResiduals为残差个数，
N0,N1...参数块，数字的大小代表参数块包含的参数的个数

virtual bool Evaluate(
    double const* const* parameters,
    double* residuals,   
    double** jacobians) 　
    const { }

parameters二维参数块指针，和SizedCostFunction设置的参数块对应
residuals一维残差指针
jacobians二维jacobian矩阵元素指针，与parameters对应，每个参数块对应一组jacobian，每组jacobian元素的数目顺序必须与对应参数块参数个数顺序一致，即jacobian[i][j]应为parameters[i][j]的偏导

一次函数

e r r = 10 − p err = 10 - perr=10−p

优化参数为p，对p求偏导即可
f ( p ) ′ = − 1 {f(p)}' = -1f(p)′=−1

//第一个参数为残差个数，第二个开始一个数字代表一个参数快，数字的大小代表参数快包含的参数的个数
class QuadraticCostFunction: public ceres::SizedCostFunction<1,1> {
public:
    virtual ~QuadraticCostFunction() {}
    virtual bool Evaluate(
        double const* const* parameters,//二维参数块指针
    	double* residuals,    //一维残差指针
    	double** jacobians) 　//二维jacobian矩阵元素指针,与parameters对应
        const { 
        residuals[0] = 10.0 - parameters[0][0];
        
        // Compute the Jacobian if asked for.
        if(jacobians && jacobians[0]) {
            jacobians[0][0] = -1;
        }
        return true;
    }
};

QuadraticCostFunction::Evaluate包含了输入变量parameters、输出剩余值residuals和雅各比矩阵jacobians，其中jacobians 为可选参数，如果非空，则在函数Evaluate 中会计算剩余函数的导数值。在以上的例子中，由于剩余函数为线性函数，所以雅各比是一个常数。

曲线拟合

e r r = e x p ( a x 2 + b x + c ) − y err = exp(ax^2+bx+c)-yerr=exp(ax2+bx+c)−y

这里优化参数为a , b , c a,b,ca,b,c，x , y x,yx,y为已知数据（这和定义的公式不一样），分别计算误差函数关于三个参数的偏导：
f ( a ) ′ = x 2 e x p ( a x 2 + b x + c ) f ( b ) ′ = x e x p ( a x 2 + b x + c ) f ( c ) ′ = e x p ( a x 2 + b x + c ) f(a)' = x^2 exp(ax^2+bx+c)\\ f(b)' = x exp(ax^2+bx+c)\\ f(c)' = exp(ax^2+bx+c)f(a)′=x2exp(ax2+bx+c)f(b)′=xexp(ax2+bx+c)f(c)′=exp(ax2+bx+c)

//残差个数，一个数字代表一个参数快，数字的大小代表参数快包含的参数的个数
class CurveFittingCostFunction : public ceres::SizedCostFunction<1, 3>
{
public:
    CurveFittingCostFunction(double x, double y) : _x(x), _y(y) {} 
    virtual ~CurveFittingCostFunction() {}
    virtual bool Evaluate(double const *const *parameters,
                          double *residuals,
                          double **jacobians) const
    {
        double a = parameters[0][0];
        double b = parameters[0][1];
        double c = parameters[0][2];
        residuals[0] = ceres::exp(a * _x * _x + b * _x + c) - _y;

        // Compute the Jacobian if asked for.
        if (jacobians != NULL && jacobians[0] != NULL)
        {
            jacobians[0][0] = _x * _x * ceres::exp(a * _x * _x + b * _x + c);
            jacobians[0][1] = _x * ceres::exp(a * _x * _x + b * _x + c);
            jacobians[0][2] = ceres::exp(a * _x * _x + b * _x + c);
        }
        return true;
    }
private:
    double _x, _y;
};

2.2 构建待求解的优化问题

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

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

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。

QuadraticCostFunction继承自SizedCostFunction，而SizedCostFunction继承自CostFunction，因此可以将解析导数的类直接赋值。

CostFunction* cost_function = new QuadraticCostFunction();

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

//残差个数，一个数字代表一个参数快，数字的大小代表参数快包含的参数的个数
class lineCostFunction : public ceres::SizedCostFunction<1, 1> 
{
public:
    virtual ~lineCostFunction() {}
    virtual bool Evaluate(double const *const *parameters,
                          double *residuals,
                          double **jacobians) const
    {
        residuals[0] = 10.0 - parameters[0][0];

        // Compute the Jacobian if asked for.
        if (jacobians != NULL && jacobians[0] != NULL)
        {
            jacobians[0][0] = -1;
        }
        return true;
    }
};

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

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

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

//残差个数，一个数字代表一个参数快，数字的大小代表参数快包含的参数的个数
class CurveFittingCostFunction : public ceres::SizedCostFunction<1, 3> 
{
public:
    CurveFittingCostFunction(double x, double y) : _x(x), _y(y) {} 
    virtual ~CurveFittingCostFunction() {}
    virtual bool Evaluate(double const *const *parameters,
                          double *residuals,
                          double **jacobians) const
    {
        double a = parameters[0][0];
        double b = parameters[0][1];
        double c = parameters[0][2];
        residuals[0] = ceres::exp(a * _x * _x + b * _x + c) - _y;

        // Compute the Jacobian if asked for.
        if (jacobians != NULL && jacobians[0] != NULL)
        {
            jacobians[0][0] = _x * _x * ceres::exp(a * _x * _x + b * _x + c);
            jacobians[0][1] = _x * ceres::exp(a * _x * _x + b * _x + c);
            jacobians[0][2] = ceres::exp(a * _x * _x + b * _x + c);
        }
        return true;
    }
private:
    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);
    }

    // 第二部分：构建寻优问题
    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);
        
        // 解析导数
       /*problem.AddResidualBlock(
            new CurveFittingCostFunction(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);
}

四、补充

仿函数

仿函数(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;
}

参考

Ceres Solver

Ceres入门

Ceres入门详解

Ceres-Solver库使用（三）–导数(Derivatives)

Ceres总结

非线性优化Ceres手动求导数值求导解析求导使用示例

jack_201316888

关注

1
点赞
踩
0

收藏

觉得还不错? 一键收藏
0
评论
SLAM学习——Ceres

https://blog.csdn.net/kalenee/article/details/100183433一、安装配置依赖# CMakesudo apt-get install cmake# google-glog + gflagssudo apt-get install libgoogle-glog-dev# BLAS & LAPACKsudo apt-get install libatlas-base-dev# Eigen3sudo apt-get insta.
复制链接

扫一扫