Ceres入门指南

发表于 更新于 分类于
我爱Ceres-Solver，你呢？

Cere Tutorial

参考网址

Ceres求解问题的模板

  • 定义CostFuntion

    • 相当于自己写一个costFunction

    • 注意这个东西是在进行梯度下降以及求解估计结果那个函数之外去定义的

    • 举个例子，比如要估计下面这个直线方程里面的参数a和b

      y=ax+by = ax + b 

      1
      2
      3
      4
      5
      6
      7
      8
      9
      10
      11
      12
      13
      14
      15
      16
      struct CURVE_FITTING_COST {
        CURVE_FITTING_COST(double x, double y) : _x(x), _y(y) {}

        // 残差的计算
        template<typename T>
        bool operator()(
          // 模型参数，有2维分别是a和b
          const T *const ab, 
          T *residual) const {
              // e_cost = y' - ( ax + b )
              residual[0] = T(_y) - (ab[0] * T(_x) + ab[1]); 
              return true;
        }
         // x,y数据
        const double _x, _y;   
      };

  • 构建Problem

    • 添加误差项 AddResidualBlock

      • 添加Cosfuncition(包含误差类型，输出维度，输入维度)
      • 添加核函数
      • 添加估计变量所在地址

    • 上面这一步的意义其实也可以理解为把所有数据加载到求解模块里面

    • 还是按照上面那个例子举一个例子哈哈哈

      1
      2
      3
      4
      5
      6
      7
      8
      9
      10
      11
      12
      13
      14
      15
      16
      17
      18
      // define the class of "problem"
      ceres::Problem problem;
      // configure it and solve the "problem"
      for (int i = 0; i < 100; i++) {
          problem.AddResidualBlock(     
          // add cost param to the "ceres problem"
          // auto—differetial, remember: set the dims of output and input in order as follow
          new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(
              // Here is the "Cost Function" we define above up
              // Do not forget to initialize it with the generate function inside 
              new CURVE_FITTING_COST(x_data[i], y_data[i])
          ),
          // core-function, just for test so i set nothing here 
          nullptr,            
          // param waiting for the estimation    
          ab
          );
      }

  • 配置变量约束范围

    • 添加上限: problem.SetParameterUpperBound()

    • 添加下限: problem.SetParmeterLowerBound()

    • 参数详解：

      1
      2
      3
      4
      5
      6
      7
      8
      9
      10
      11
      // Set the lower/upper bound for the parameter at position "index".
      // 用英文装个*
      /**
      * @brief Set the Parameter Lower/Upper Bound object
      * 
      * @param values array for stored the estimation of ceres_sovler problem
      * @param index  the location of parameter for estimation in this array
      * @param lower_bound/upper_bound the bound of this parameter
      */
      void SetParameterLowerBound(double* values, int index, double lower_bound);
      void SetParameterUpperBound(double* values, int index, double upper_bound);

    • 接着举例子，比如我要设置估计的a一定要有a>0.1，那么就会有

      1
      problem.SetParameterLowerBound(ab, 0, 0.1);

  • 配置Solver::Options

    • 添加线性求解器(这里有很多选项)

      • 添加QR分解器：options.linear_solver_type = ceres::DENSE_QR
      • 添加LLT分解器：options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY

    • 添加最大迭代次数：options.max_num_iterations = 100

    • 添加log信息打印选项：options.minimizer_progress_to_stdout = true

    • 等等还有很多，详情见网址

      https://blog.csdn.net/DumpDoctorWang/article/details/84890792

    • 一个完整的定义过程是

      1
      2
      3
      4
      5
      6
      ceres::Solver::Options options;     
      options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
      // set the times of iteration
      options.max_num_iterations = 100;
      options.minimizer_progress_to_stdout = true; 
      ceres::Solver::Summary summary;

  • 定义Solver::Summary

    • 啥也不用做，这个东西就是用来打印优化过程的信息的

    • 分为BriefReport()和FullReport()

    • 我们这里直接定义一个他就好，默认是BriefReport()

      1
      ceres::Solver::Summary summary;

  • 开始优化Solve

    • 这里我们直接把上面定义的那一堆东西全部放进来就好，没错，all in！！！！

      1
      ceres::Solve(options, &problem, &summary);

  • 输出结果Summary.BriefReport

案例详解

根据单线激光laser估计圆心的二维坐标

  • 做这种问题，希望各位能够明确一个问题：我们的目标是什么？

    • 简单来说：最小化损失函数！

    • 在这个问题里，我们要做的很简单

      argmin (x0,y0)∗ R2−[(x−x0)2+(y−y0)2]arg min \space (x_0,y_0)^* \space R^2 - [(x-x_0)^2 + (y-y_0)^2]

    • 解释一下：

      • 其实就是圆对应公式，圆上所有点到圆心距离都应该为RR
      • 也就是说我们只要找到一点(x0,y0,z0)(x_0, y_0, z_0)使得所有点到这个点的距离为 RR 就好了，对吧！
      • 进一步想，就是去计算这些点到球心的距离，然后令这个距离和半径的差值最小，最接近，对吧！
      • 这里为了方便，我们选择平方的形式，就不开方了，当然这里遗留一个问题：开方后没法计算

    • 既然明确了这个目标，那我们就要在程序里作第一件事情，就是定义这个损失函数！

    • 和各位啰嗦一句，只要是用ceres-solver 最好上来就直接写这个**(main函数外)**

      1
      2
      3
      4
      5
      6
      7
      8
      9
      10
      11
      12
      13
      14
      15
      16
      17
      18
      19
      20
      21
      22
      23
      24
      class circle_cost_function {
      private:
          // 在这里我们去定义需要加载的数据
          // 注意这个数据不是要估计的参数！
          double _x, _y;
      public:
          circle_cost_function(double x, double y): _x(x), _y(y) {}
          // ceres-solver的套路，用这个operator来定义残差计算公式
          /**
           * @brief 
           * 
           * @param abc       这个就是你要估计的参数
           *                  abc这里是一个三维数组 
           *                  abc[0] = x0, abc[1] = y0, abc[2] = radius 
           * @param residual  这个就是残差结果
           */
          template<typename T> bool operator() ( const T *const abc, T *residual) const {
              // 公式：R** - (x-x0)** -(y-y0)**
              residual[0] = abc[2] - 
                            pow( (T(_x)-abc[0]), 2 ) - 
                            pow( (T(_y)-abc[1]), 2 );
              return true;
          }
      };

  • 好了，现在已经定义好了cost function，接下来的事情就很简单了

    • 在main函数里，我们先生成一些数据

      • 圆心的真值

        1
        2
        3
        double op_x_gd = 3.2;
        double op_y_gd = 1.5;
        double radius = 4.0;

      • 圆上的一些点

        1
        2
        3
        4
        5
        6
        7
        8
        9
        vector<double> x_data, y_data;
        for ( int i = 0; i < 100; i++ ) {
            // sample from 0deg to 100deg
            // 注意：cmath的cos采用弧度制
            double x = op_x_gd + radius * cos( i * M_PI/180 );
            double y = op_y_gd + radius * sin( i * M_PI/180 );
            x_data.push_back ( x );
            y_data.push_back (y);
        }

    • 然后就开始求解！

      • 按照上面说的那个顺序：problem->options->summary->solver

      • 先是向problem里面添加残差块

        • 这里有三个需要注意的地方
          • 在new ceres::AutoDiffCostFunction<circle_cost_function, 1, 3>中，1是说circle_cost_function中residual的维度，3是说abc，即需要估计的变量维度
          • nullptr这里是选用的核函数，这个是什么东西我们放到下一个BA的例子中说
          • abc就是需要估计的参数变量
        1
        2
        3
        4
        5
        6
        7
        8
        9
        10
        ceres::Problem problem;
        for ( int i = 0; i < 100; i++ ) {
        problem.AddResidualBlock (
        new ceres::AutoDiffCostFunction<circle_cost_function, 1, 3> (
            new circle_cost_function ( x_data[i], y_data[i] )
            ),
            nullptr, 
            abc
            );
        }

      • 确定优化时的options

        1
        2
        3
        ceres::Solver::Options options;
        options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
        options.minimizer_progress_to_stdout = true;

      • 定义smmuary

        1
        2
        ceres::Solver::Summary summary;
        ceres::Solve ( options, &problem, &summary );

      • 开始solver并输出结果

        1
        2
        3
        4
        5
        6
        // 输出结果
        cout << summary.BriefReport() << endl;
        cout << "x0 =  " << abc[0] 
            	  << " y0 = " << abc[1] 
                  << " Radius = " << sqrt(abc[2]) 
            	  << endl;

    • 好了我们来展示一下这个代码的全部面貌吧！

      • CmakeLists.txt

        1
        2
        3
        4
        5
        6
        7
        8
        9
        10
        cmake_minimum_required(VERSION 3.0)
        project(ceres_solver_tutorial)

        include_directories("/usr/local/include/Eigen")

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

        add_executable(nonlinear_circle_fit nonlinear_circle_fit.cpp)
        target_link_libraries(nonlinear_circle_fit ${CERES_LIBRARIES})

      • nonlinear_circle_fit.cpp

        1
        2
        3
        4
        5
        6
        7
        8
        9
        10
        11
        12
        13
        14
        15
        16
        17
        18
        19
        20
        21
        22
        23
        24
        25
        26
        27
        28
        29
        30
        31
        32
        33
        34
        35
        36
        37
        38
        39
        40
        41
        42
        43
        44
        45
        46
        47
        48
        49
        50
        51
        52
        53
        54
        55
        56
        57
        58
        59
        60
        61
        62
        63
        64
        65
        66
        #include <iostream>
        #include <ceres/ceres.h>
        using namespace std;
        class circle_cost_function {
        private:
            // 在这里我们去定义需要加载的数据
            // 注意这个数据不是要估计的参数！
            double _x, _y;
        public:
            circle_cost_function(double x, double y): _x(x), _y(y) {}
            // ceres-solver的套路，用这个operator来定义残差计算公式
            /**
             * @brief 
             * 
             * @param abc       这个就是你要估计的参数
             *                  abc这里是一个三维数组 
             *                  abc[0] = x0, abc[1] = y0, abc[2] = radius 
             * @param residual  这个就是残差结果
             */
            template<typename T> bool operator() ( const T *const abc, T *residual) const {
                // 公式：R** - (x-x0)** -(y-y0)**
                residual[0] = abc[2] - 
                              pow( (T(_x)-abc[0]), 2 ) - 
                              pow( (T(_y)-abc[1]), 2 );
                return true;
            }
        };

        int main ( int argc, char** argv )
        {
            double op_x_gd = 3.2;
            double op_y_gd = 1.5;
            double radius = 4.0;
            double abc[3] = {0,0,0};
            vector<double> x_data, y_data;
            for ( int i = 0; i < 100; i++ ) {
                // sample from 0deg to 100deg
                // 注意：cmath的cos采用弧度制
                double x = op_x_gd + radius * cos( i * M_PI/180 );
                double y = op_y_gd + radius * sin( i * M_PI/180 );
                x_data.push_back ( x );
                y_data.push_back (y);
            }

            ceres::Problem problem;
            for ( int i = 0; i < 100; i++ ) {
                problem.AddResidualBlock (
                    new ceres::AutoDiffCostFunction<circle_cost_function, 1, 3> (
                        new circle_cost_function ( x_data[i], y_data[i] )
                    ),
                    nullptr, 
                    abc
                );
            }

            ceres::Solver::Options options;
            options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
            options.minimizer_progress_to_stdout = true;

            ceres::Solver::Summary summary;
            ceres::Solve ( options, &problem, &summary );
            // 输出结果
            cout << summary.BriefReport() << endl;
            cout << "x0 =  " << abc[0] << " y0 = " << abc[1] << " Radius = " << sqrt(abc[2]) << endl;
            return 0;
        }

      • 运行一下看看结果

Bundle Adjustment(Ceres-solver版本)！！！

我猜到大家肯定喜欢这个，那不如先看看我讲解的配套视频吧！

等待更新。。。