Ceres入门详解

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

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

---

开源地址：点击该链接

---

Ceres用法总结

入门操作

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

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

  struct AutoDiffCostFunctor {
      template 
      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(
              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(
              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 
    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 
    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 
    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 
    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(new CostFuntorF1);
    CostFunction* costfunction2 =
    	new AutoDiffCostFunction(new CostFuntorF2);
    CostFunction* costfunction3 =
    	new AutoDiffCostFunction(new CostFuntorF3);
    CostFunction* costfunction4 =
    	new AutoDiffCostFunction(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 
    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(
    	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 
#include 
#include 
#include 

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
  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 distribution(0.0,0.5);
  //y = exp(0.3*x + 0.1)
  for(int i=0; i(
      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