Ceres(1)

转载地址：

http://www.cnblogs.com/xiaowangba/p/6313933.html

http://ceres-solver.org/tutorial.html

Ceres Solver [1] is an open source C++ library for modeling and solving large, complicated optimization problems. It can be used to solve (1) Non-linear Least Squares problems with bounds constraints and (2) general unconstrained optimization problems. 

目标函数的形式：

                             

示例1：求极值

首先我们以Ceres库官网中的Hello World例子来进行说明。这里例子的目的是为了计算方程取得最小值时x的值。从这个方程很容易看出来当x=10时，f(x)取得最小值0。这个方程虽然没有什么实际意义，但是为了演示Ceres库还是很不错的例子。

1、编写一个g(x)=10-x的残差方程。代码如下：

1. struct CostFunctor {  
2.    template <typename T>  
3.    bool operator()(const T* const x, T* residual) const {  
4.      residual[0] = T(10.0) - x[0];  
5.      return true;  
6.    }  
7. };  

这里值得注意的是，必须要编写一个重载()运算，而且必须使用模板类型，所有的输入参数和输出参数都要使用T类型。

2、当我们写完了上面的计算残差的方程，接下来就可以使用Ceres库来构造一个求解非线性最小二乘法的Problem来进行求解未知数了。代码如下：

1. int main(int argc, char** argv)   
2. {  
3.   google::InitGoogleLogging(argv[0]);  
4.   
5.   // 指定求解的未知数的初值，这里设置为5.0  
6.   double initial_x = 5.0;  
7.   double x = initial_x;  
8.   
9.   // 建立Problem  
10.   Problem problem;  
11.   
12.    // 建立CostFunction（残差方程）  
13.    CostFunction* cost_function =  
14.        new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);  
15.    problem.AddResidualBlock(cost_function, NULL, &x);  
16.   
17.    // 求解方程!  
18.    Solver::Options options;  
19.    options.linear_solver_type = ceres::DENSE_QR;  
20.    options.minimizer_progress_to_stdout = true;  
21.    Solver::Summary summary;  
22.    Solve(options, &problem, &summary);  
23.   
24.    std::cout << summary.BriefReport() << "\n";  
25.    std::cout << "x : " << initial_x  
26.              << " -> " << x << "\n";  
27.    return 0;  
28. }  

Derivatives

Ceres Solver provides a number of ways of doing so. You have already seen one of them in action – Automatic Differentiation in examples/helloworld.cc. We now consider the other two possibilities. Analytic and numeric derivatives.

Numeric Derivatives

struct CostFunctor {  bool operator()(const double* const x, double* residual) const {    residual[0] = 10.0 - x[0];    return true;  }};CostFunction* cost_function =      new NumericDiffCostFunction<CostFunctor, ceres::CENTRAL, 1, 1> (new CostFunctor); //NumericDiffCostFunction的模板参数里的< 1，1>好像是：  // 第一个参数指的是每个残差的维数，第二个参数是 定义的函数对象的第一个参数的维数problem.AddResidualBlock(cost_function, NULL, &x);

相比上面的  AutoDiffCostFunction  多了一个模板参数 ceres::CENTRAL，意思是用有限差分来计算导数，详见NumericDiffCostFunction。

Generally speaking we recommend automatic differentiation instead of numeric differentiation. 

The use of C++ templates makes automatic differentiation efficient, 

whereas numeric differentiation is expensive, prone to numeric errors, and leads to slower convergence.

Analytic Derivatives

In such cases, it is possible to supply your own residual and jacobian computation code.To do this, define a subclass of CostFunction or SizedCostFunction if you know the sizes of the parameters and residuals at compile time. 

class QuadraticCostFunction  : public SizedCostFunction<1 /* 残差的维数*/,                             1 /* size of first parameter */> { //parameters[0]的维数public:  virtual ~QuadraticCostFunction() {}  virtual bool Evaluate(double const* const* parameters,                        double* residuals,                        double** jacobians) const {   //这个函数好像是基类的函数，虚函数重载。ellipse_approximation.cc中也有一个类似的例子    const double x = parameters[0][0];    residuals[0] = 10 - x;    // Compute the Jacobian if asked for.    if (jacobians != NULL && jacobians[0] != NULL) {      jacobians[0][0] = -1;    }    return true;  }};CostFunction* cost_function = new QuadraticCostFunction;  problem.AddResidualBlock(cost_function, NULL, &x);   //不自己写函数对象，而是继承自ceres::CostFunction或者 ceres::SizedCostFunction时,problem.AddResidualBlock()里的 cost_function定义和 自己写函数对象时，不一样。

曲线拟合
下面的例子是指定一系列的点对来拟合一个曲线的系数。这一系列点对是通过曲线插值的点然后添加了标准差的高斯噪声。我们要拟合的曲线形式为：

struct ExponentialResidual {    ExponentialResidual(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:    // 观测值    const double x_;    const double y_;  };  

假设观测点对全部存在数组data中，data中共有2n个数，下面的代码用来演示如何将每个观测点对都加入到Problem中用于后续求解。

double m = 0.0;  double c = 0.0;    Problem problem;  for (int i = 0; i < kNumObservations; ++i)   {    CostFunction* cost_function =         new AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>(             new ExponentialResidual(data[2 * i], data[2 * i + 1]));    problem.AddResidualBlock(cost_function, NULL, &m, &c);  }  //AutoDiffCostFunction 的模板参数里的< 1，1，1>好像是：// 第一个参数指的是每个残差的维数，第二个参数是 定义的函数对象的第一个参数的维数，第三个参数是 定义的函数对象的第二个参数的维数，以此类推

Robust Curve Fitting
To deal with outliers, a standard technique is to use a LossFunction. Loss functions reduce the influence of residual blocks with high residuals, usually the ones corresponding to outliers.To associate a loss function with a residual block, we change

problem.AddResidualBlock(cost_function, NULL , &m, &c);toproblem.AddResidualBlock(cost_function, new CauchyLoss(0.5) , &m, &c);

CauchyLoss is one of the loss functions that ships with Ceres Solver. The argument 0.5specifies the scale of the loss function.


Bundle Adjustment
比如：
Given a set of measured image feature locations and correspondences, the goal of bundle adjustment is to find 3D point positions and camera parameters that minimize the reprojection error. This optimization problem is usually formulated as a non-linear least squares problem, where the error is the squared L2 norm of the difference between the observed feature location and the projection of the corresponding 3D point on the image plane of the camera.

The first step as usual is to define a templated functor that computes the reprojection error/residual. Let us solve a problem from the BAL dataset.

Each residual in a BAL problem depends on a three dimensional point and a nine parameter camera. The nine parameters defining the camera are: three for rotation as a Rodriques’ axis-angle vector, three for translation, one for focal length and two for radial distortion. The details of this camera model can be found the  BAL homepage.

struct SnavelyReprojectionError {  SnavelyReprojectionError(double observed_x, double observed_y)      : observed_x(observed_x), observed_y(observed_y) {}  template <typename T>  bool operator()(const T* const camera,                  const T* const point,                  T* residuals) const {    // camera[0,1,2] are the angle-axis rotation.    T p[3];    ceres::AngleAxisRotatePoint(camera, point, p);    // camera[3,4,5] are the translation.    p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5];    // Compute the center of distortion. The sign change comes from    // the camera model that Noah Snavely's Bundler assumes, whereby    // the camera coordinate system has a negative z axis.    T xp = - p[0] / p[2];    T yp = - p[1] / p[2];    // Apply second and fourth order radial distortion.    const T& l1 = camera[7];    const T& l2 = camera[8];    T r2 = xp*xp + yp*yp;    T distortion = T(1.0) + r2  * (l1 + l2  * r2);    // Compute final projected point position.    const T& focal = camera[6];    T predicted_x = focal * distortion * xp;    T predicted_y = focal * distortion * yp;    // The error is the difference between the predicted and observed position.    residuals[0] = predicted_x - T(observed_x);    residuals[1] = predicted_y - T(observed_y);    return true;  }   // Factory to hide the construction of the CostFunction object from   // the client code.   static ceres::CostFunction* Create(const double observed_x,                                      const double observed_y) {     return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(    // 2，9，3                 new SnavelyReprojectionError(observed_x, observed_y)));   }  double observed_x;  double observed_y;};ceres::Problem problem;for (int i = 0; i < bal_problem.num_observations(); ++i) {  ceres::CostFunction* cost_function =      SnavelyReprojectionError::Create(           bal_problem.observations()[2 * i + 0],           bal_problem.observations()[2 * i + 1]);  problem.AddResidualBlock(cost_function,                           NULL /* squared loss */,                           bal_problem.mutable_camera_for_observation(i),                           bal_problem.mutable_point_for_observation(i));}