ceresCurveFitting代码笔记

ceres使用基本流程
1 构建代价函数
2 通过代价函数构建待求解的问题
3 配置求解器并求解问题

从简单的开始，比如现在需要求解下面这个函数的最小值：
\((10-x)^2 \over 2\)

1 构建代价函数

struct CostFunctor{

    template<typename T>
    bool operator()(const T* const x,T*residual)const{
        residual[0]=10.0-x[0];
        return true;
    }
};  

2 通过代价函数构建待求解问题

//Build the problem
  ceres::Problem problem;

  ceres::CostFunction* cost_function =
          new ceres::AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
  problem.AddResidualBlock(cost_function, nullptr, &x);

3配置求解器求解问题

    // Run the solver!
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = true;
    ceres::Solver::Summary summary;
    Solve(options, &problem, &summary);

完整代码：

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

using namespace std;

struct CostFunctor{
    template<typename T>
    bool operator()(const T* const x,T*residual)const{
        residual[0]=10.0-x[0];
        return true;
    }
};

int main() {
    // The variable to solve for with its initial value.(初值)
    double initial_x = 5.0;
    double x = initial_x;

    //Build the problem
    ceres::Problem problem;

    ceres::CostFunction* cost_function =
            new ceres::AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
    problem.AddResidualBlock(cost_function, nullptr, &x);

    // Run the solver!
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = true;
    ceres::Solver::Summary summary;
    Solve(options, &problem, &summary);

    std::cout << summary.BriefReport() << "\n";
    std::cout << "x : " << initial_x
              << " -> " << x << "\n";
    return 0;
}

---

一个比较复杂的例子：求解y=exp(ax^2+bx+c)
其中abc为待优化参数，x为随机产生噪声数据

1构建代价函数

struct CURVE_FITTING_COST {
  //构造函数：要求传入x和y
  CURVE_FITTING_COST(double x, double y) : _x(x), _y(y) {}

  // 残差的计算
  //abc为待优化参数，residual为残差变量
  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]); // y-exp(ax^2+bx+c)
    return true;
  }
  const double _x, _y;    // x,y数据
}; 

• 构造函数(可选)
  误差函数中的参数包括： 已知参数和待优化参数两部分，其中已知参数则在仿函数创建时通过构造函数传入，若优化问题没有已知参数，则不需要编写构造函数。而待优化参数由Problem::AddResidualBlock() 统一添加和管理

• 重载操作符()（必有）
  操作符()是一个模板方法，返回值为bool型，接受参数为待优化变量和残差变量。待优化变量的传入方式应和 Probelm::AddResidualBlock() 一致，即若Probelm::AddResidualBlock()中一次性传入变量数组指针，此处亦应该一次性传入变量数组指针；若Probelm::AddResidualBlock()变量是依次传入，此处亦应该依次传入，且保证变量传入顺序一致。由于在优化过程中，我们不希望因为程序的误操作导致操作符()重载的内容被修改，因此需要为函数体加上const关键字修饰。同理，在残差的计算过程中，为了避免除ceres优化之外的误操作引起待优化变量的改变，需要同时使用const关键字修饰参数类型和参数名保证类型和内容均不变；而residual只需要保证类型不变，参数每次都是可变的，因此只需要使用const修饰类型T即可。

2 通过代价函数构建待求解的问题

ceres::Problem problem;
  for (int i = 0; i < N; i++) {
//写法一：
//    problem.AddResidualBlock(     // 向问题中添加误差项
//      // 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
//      new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(new CURVE_FITTING_COST(x_data[i], y_data[i])),
//      nullptr,            // 核函数，这里不使用，为空
//      abc                 // 待估计参数（传入的是指针）
//    );

//写法二：
//模板参数依次为仿函数（functor）类型CostFunctor，残差维数residualDim和参数维数paramDim，接受参数类型为仿函数指针CostFunctor*
  ceres::CostFunction* costFunction=
            new ceres::AutoDiffCostFunction<CURVE_FITTING_COST,1,3>(new CURVE_FITTING_COST(x_data[i],y_data[i]));   //构造函数传入数据
    problem.AddResidualBlock(costFunction, nullptr,abc);//通过仿函数传入待优化参数

  }

AutoDiffCostFunction()

ceres::AutoDiffCostFunction<CostFunctor, int residualDim, int paramDim>(CostFunctor* functor);

模板参数依次为仿函数（functor）类型CostFunctor，残差维数residualDim和参数维数paramDim，接受参数类型为仿函数指针CostFunctor*

AddResidualBlock()

AddResidualBlock() 顾名思义主要用于向 Problem 类传递残差模块的信息，函数原型如下，
传递的参数主要包括：代价函数模块、损失函数模块 和 参数模块；

ResidualBlockId Problem::AddResidualBlock(CostFunction *cost_function, 
                                          LossFunction *loss_function,  
                                          const vector<double *> parameter_blocks)
										  
ResidualBlockId Problem::AddResidualBlock(CostFunction *cost_function, 
                                          LossFunction *loss_function, 
                                          double *x0, double *x1, ...)

• 代价函数：包含了参数模块的维度信息，内部使用仿函数定义误差函数的计算方式。AddResidualBlock() 函数会检测传入的参数模块是否和代价函数模块中定义的维数一致，维度不一致时程序会强制退出。

• 损失函数：用于处理参数中含有野值的情况，避免错误量测对估计的影响，常用参数包括 HuberLoss、CauchyLoss 等（完整的参数列表参见Ceres API文档）；该参数可以取 NULL或 nullptr，此时损失函数为单位函数。

• 参数模块：待优化的参数，可一次性传入所有参数的 指针容器 vector<double> 或 依次传入所有参数的指针double

3 配置求解器求解问题

ceres::Solver::Options options;     // 这里有很多配置项可以填
options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;  // 增量方程如何求解
options.minimizer_progress_to_stdout = true;   // 输出到cout
ceres::Solver::Summary summary;                // 优化信息
ceres::Solve(options, &problem, &summary);  // 开始优化

4 输出结果

cout << summary.BriefReport() << endl;
cout << "estimated a,b,c = ";
  for (auto a:abc) cout << a << " ";