Ceres入门详解

JiauZhang

已于 2024-01-08 21:52:40 修改

阅读量6.7k

点赞数 2

分类专栏： SLAM 文章标签： slam Ceres 非线性优化最小二乘

于 2018-10-20 09:55:14 首次发布

本文链接：https://blog.csdn.net/q_z_r_s/article/details/83210923

版权

SLAM 专栏收录该内容

15 篇文章 9 订阅

订阅专栏

本文为原创博客，转载请注明出处：https://blog.csdn.net/q_z_r_s

机器感知
一个专注于SLAM、三维重建、机器视觉等相关技术文章分享的公众号
公众号：机器感知

开源地址：点击该链接

Ceres用法总结

入门操作

定义CostFunctor(i.e.函数 f ),以 f(x)=10-x 为例

使用自动求导(当使用自动求导时, operator() 是一个模板函数)

struct AutoDiffCostFunctor {
    template <typename T>
    bool operator()(const T* const x, T* residual) const { //修饰大括号的const必须有
        residual[0] = T(10.0) - x[0];
        //10.0必须强制转换为T类型,否则会编译器会报无法实现运算操作的类型
        return true;
    }
};

使用数值求导

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

使用函数导数的解析式求导

class QuadraticCostFunction: public ceres::SizedCostFunction<1,1> {
public:
    virtual ~QuadraticCostFunction() {}
    virtual bool Evaluate(double const* const* parameters,//二维参数块指针
    double* residuals,
    //一维残差指针
    double** jacobians) const { //二维jacobian矩阵元素指针,与parameters对应
        residuals[0] = 10.0 - parameters[0][0];
        if(jacobians && jacobians[0]) {
            jacobians[0][0] = -1;
        }
        return true;
    }
};

主函数对最小二乘问题的实现

int main(int argc, char** argv)
{
    const double initial_x = 0.2;
    double x = initial_x;Problem problem;
    Solver::Options options;
    //control whether the log is output to STDOUT
    options.minimizer_progress_to_stdout = false;
    Solver::Summary summary;
    //AutoDiff
    CostFunction* CostFunction =
        new AutoDiffCostFunction<AutoDiffCostFunctor,1,1>(
            new AutoDiffCostFunctor);
    problem.AddResidualBlock(CostFunction,NULL,&x);
    clock_t solve_start = clock();
    //'summary' must be non NULL
    Solve(options, &problem, &summary);
    clock_t solve_end = clock();
    std::cout << "AutoDiff time : " << solve_end-solve_start << std::endl;
    std::cout << "x : " << initial_x << " -> " << x << std::endl;
    //NumericDiff
    CostFunction =
        new NumericDiffCostFunction<NumericDiffCostFunctor,CENTRAL,1,1>(
            new NumericDiffCostFunctor);
    x = 0.2;
    problem.AddResidualBlock(CostFunction,NULL,&x);
    solve_start = clock();
    Solve(options, &problem, &summary);
    solve_end = clock();
    std::cout << "NumercDiff time : " << solve_end-solve_start << std::endl;
    std::cout << "x : " << initial_x << " -> " << x << std::endl;
    //AnalyticDiff
    //QuadraticCostFunction继承自SizedCostFunction,而SizedCostFunction继承自
    //CostFunction,因此此语句与上述两种对CostFunction的赋值操作略有不同
    CostFunction = new QuadraticCostFunction;
    x = 0.2;
    problem.AddResidualBlock(CostFunction,NULL,&x);
    solve_start = clock();
    Solve(options, &problem, &summary);
    solve_end = clock();
    std::cout << "AnalyticDiff time : " << solve_end-solve_start << std::endl;
    std::cout << "x : " << initial_x << " -> " << x << std::endl;
    return 0;
};

总结
- 定义CostFunctor,重载 operator() (i.e. f 所表示的运算过程)函数
- 主函数中定义 Problem ,设置一些必要的配置,添加残差块, Solve 求解

Powell’s Function

第一种实现方法

struct CostFuntorF1 {template <typename T>
    bool operator()(const T* const x1, const T* const x2, T* residuals) const {
        residuals[0] = x1[0] + T(10)*x2[0];
        return true;
    }
};
struct CostFuntorF2 {
    template <typename T>
    bool operator()(const T* const x3, const T* const x4, T* residuals) const {
        residuals[0] = sqrt(5)*(x3[0] - x4[0]);
        return true;
    }
};
struct CostFuntorF3 {
    template <typename T>
    bool operator()(const T* const x2, const T* const x3, T* residuals) const {
        residuals[0] = (x2[0] - T(2)*x3[0])*(x2[0] - T(2)*x3[0]);
        return true;
	}
};
struct CostFuntorF4 {
    template <typename T>
    bool operator()(const T* const x1, const T* const x4, T* residuals) const {
        residuals[0] = sqrt(10)*(x1[0] - x4[0])*(x1[0] - x4[0]);
        return true;
	}
};
int main(int argc, char** argv)
{
    const double initial_x1 = 10, initial_x2 = 5,
    initial_x3 = 2, initial_x4 = 1;
    double x1 = initial_x1, x2 = initial_x2,
    x3 = initial_x3, x4 = initial_x4;
    Problem problem;
    Solver::Options options;
    //control whether the log is output to STDOUT
    options.minimizer_progress_to_stdout = true;
    Solver::Summary summary;
    CostFunction* costfunction1 =
    	new AutoDiffCostFunction<CostFuntorF1,1,1,1>(new CostFuntorF1);
    CostFunction* costfunction2 =
    	new AutoDiffCostFunction<CostFuntorF2,1,1,1>(new CostFuntorF2);
    CostFunction* costfunction3 =
    	new AutoDiffCostFunction<CostFuntorF3,1,1,1>(new CostFuntorF3);
    CostFunction* costfunction4 =
    	new AutoDiffCostFunction<CostFuntorF4,1,1,1>(new CostFuntorF4);
    problem.AddResidualBlock(costfunction1,NULL,&x1,&x2);
    problem.AddResidualBlock(costfunction2,NULL,&x3,&x4);
    problem.AddResidualBlock(costfunction3,NULL,&x2,&x3);
    problem.AddResidualBlock(costfunction4,NULL,&x1,&x4);
    Solve(options,&problem,&summary);
    cout << "x1 : " << initial_x1 << "->" << x1 << endl;
    cout << "x2 : " << initial_x2 << "->" << x2 << endl;
    cout << "x3 : " << initial_x3 << "->" << x3 << endl;
    cout << "x4 : " << initial_x4 << "->" << x4 << endl;
    return 0;
}

第二种实现方法

struct CostFunctor {
    template <typename T>
    bool operator()(const T* const x, T* residuals) const{
        residuals[0] = x[0] - T(10) * x[1];
        residuals[1] = T(sqrt(5)) * (x[2] - x[4]);
        residuals[2] = (x[1] - T(2) * x[2]) * (x[1] - T(2) * x[2]);
        residuals[3] = T(sqrt(10)) * (x[0] - x[3]) * (x[0] - x[3]);
        return true;
	}
};
int main(int argc, char** argv)
{
    const double initial_x1 = 10, initial_x2 = 5,
    initial_x3 = 2, initial_x4 = 1;
    Problem problem;
    Solver::Options options;
    //control whether the log is output to STDOUT
    options.minimizer_progress_to_stdout = true;
    Solver::Summary summary;
    double x[4] = {initial_x1, initial_x2, initial_x3, initial_x4};
    CostFunction* costfunction = new AutoDiffCostFunction<CostFunctor, 4, 4>(
    	new CostFunctor);
    Problem pb;
    pb.AddResidualBlock(costfunction,NULL,x);
    Solve(options,&pb,&summary);
    cout << "x[0] : " << initial_x1 << "->" << x[0] << endl;
    cout << "x[1] : " << initial_x2 << "->" << x[1] << endl;
    cout << "x[2] : " << initial_x3 << "->" << x[2] << endl;
    cout << "x[3] : " << initial_x4 << "->" << x[3] << endl;
    return 0;
}

曲线拟合

y = exp(0.3*x + 0.1)

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

using namespace std;
using ceres::AutoDiffCostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::CostFunction;
using ceres::SizedCostFunction;

struct CostFunctor {
  CostFunctor(double x, double y): _x(x), _y(y) {}
  template<typename T>
  bool operator()(const T* const m, const T* const c, T* residual) const {
    residual[0] = T(_y) - exp(m[0]*T(_x) + c[0]);
    return true;
  }  
private:
  double _x;
  double _y;
};

class QuadraticCostFunctor: public SizedCostFunction<1,1,1> {
public:
  QuadraticCostFunctor(double x, double y): _x(x), _y(y) {}
  virtual ~QuadraticCostFunctor() {}
  bool Evaluate(double const* const* parameters,
                        double* residuals,
                        double** jacobians) const {//const cant't be ignore!
    residuals[0] = _y - exp(parameters[0][0]*_x + parameters[1][0]);
    
    if(jacobians && jacobians[0] && jacobians[1]) {
      //we should provide Solver negtive gradient!
      //so that costfunction can decrese. 
      jacobians[0][0] = -_x*exp(parameters[0][0]*_x + parameters[1][0]);
      jacobians[1][0] = -exp(parameters[0][0]*_x + parameters[1][0]);
    }    
    return true;
  }  
private:
  double _x, _y;
};

struct point_data {
  point_data(): x(0), y(0) {}
  double x;
  double y;
};

int main(int argc, char **argv) 
{
  //create data with Gaussian noise
  const int data_size = 50;
  struct point_data data[data_size];
  struct point_data point;
  default_random_engine generator;
  normal_distribution<double> distribution(0.0,0.5);
  //y = exp(0.3*x + 0.1)
  for(int i=0; i<data_size; i++) {
    point.x = 0.1 * i;
    point.y = exp(0.3*point.x + 0.1) + distribution(generator);
    data[i] = point;
  }
  
  double m = 0.0, c = 0.1;
  Problem problem;
  CostFunction* costfunction;
  for(int i=0; i<data_size; i++) {
    costfunction = new AutoDiffCostFunction<CostFunctor,1,1,1>(
      new CostFunctor(data[i].x, data[i].y));
    problem.AddResidualBlock(costfunction, NULL, &m, &c);
  }
  
  Solver::Options options;
  options.max_num_iterations = 25;
  options.linear_solver_type = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = true;

  Solver::Summary summary;
  Solve(options, &problem, &summary);
  std::cout << summary.BriefReport() << "\n";
  std::cout << "Initial m: " << 0.0 << " c: " << 0.1 << "\n";
  std::cout << "Final   m: " << m << " c: " << c << "\n";
  
  //Analytic
  m = 3.0, c = 1.1;
  for(int i=0; i<data_size; i++) {
    costfunction = new QuadraticCostFunctor(data[i].x, data[i].y);
    problem.AddResidualBlock(costfunction, NULL, &m, &c);
  }
  
  options.max_num_iterations = 100;
  Solve(options, &problem, &summary);
  std::cout << summary.BriefReport() << "\n";
  std::cout << "Initial m: " << 3.0 << " c: " << 1.1 << "\n";
  std::cout << "Final   m: " << m << " c: " << c << "\n";
  
  return 0;
}