Ceres使用（三）

Powell’s Function

现在，考虑更复杂一点的情况，最小化Powell方程。令x=[x1,x2,x3,x4]，并且

f1(x)f2(x)f3(x)f4(x)F(x)=x1+10x2=5√(x3−x4)=(x2−2x3)2=10−−√(x1−x4)2=[f1(x),f2(x),f3(x),f4(x)]

F(x)是一个有四个参数的方程，有四个残差，我们想要找到一个x，使得12∥F(x)∥2最小。
第一步，定义评估目标函数项的函数。这里是评估f4(x1,x4)的代码：

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

类似的，我们可以定义类F1，F2，F3来分别评估f1(x1,x2)，f2(x3,x4)和f3(x2,x3)。使用这些，可以构造problem：

double x1 =  3.0; double x2 = -1.0; double x3 =  0.0; double x4 = 1.0;Problem problem;// Add residual terms to the problem using the using the autodiff// wrapper to get the derivatives automatically.problem.AddResidualBlock(  new AutoDiffCostFunction<F1, 1, 1, 1>(new F1), NULL, &x1, &x2);problem.AddResidualBlock(  new AutoDiffCostFunction<F2, 1, 1, 1>(new F2), NULL, &x3, &x4);problem.AddResidualBlock(  new AutoDiffCostFunction<F3, 1, 1, 1>(new F3), NULL, &x2, &x3)problem.AddResidualBlock(  new AutoDiffCostFunction<F4, 1, 1, 1>(new F4), NULL, &x1, &x4);

注意，每个ResidualBlock只依赖于残差对应的两个参数，而不是全部四个参数。完整代码：

#include <iostream>#include <ceres/ceres.h>#include <vector>#include <glog/logging.h>#include <gflags/gflags.h>using namespace std;using ceres::AutoDiffCostFunction;using ceres::CostFunction;using ceres::Problem;using ceres::Solver;using ceres::Solve;struct F1 {    template <typename T>            bool operator()(const T* const x1, const T* const x2, T* residual) const {        residual[0] = x1[0] + T(10.0)*x2[0];        return true;    }};struct F2 {    template <typename T>            bool operator()(const T* const x3, const T* const x4, T* residual) const {        residual[0] = T(sqrt(5.0))*(x3[0]-x4[0]);        return true;    }};struct F3 {    template <typename T>            bool operator()(const T* const x2, const T* const x3, T* residual) const {        residual[0] = (x2[0]-T(2.0)*x3[0])*(x2[0]-T(2.0)*x3[0]);        return true;    }};struct F4 {    template <typename T>            bool operator()(const T* const x1, const T* const x4, T* residual) const {        residual[0] = T(sqrt(10.0))*(x1[0]-x4[0])*(x1[0]-x4[0]);        return true;    }};DEFINE_string(minimizer, "trust_region",              "Minimizer type to use, choices are: line_search & trust_region");int main(int argc, char** argv) {//    CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);    google::InitGoogleLogging(argv[0]);    double x1 =  3.0;    double x2 = -1.0;    double x3 = 0.0;    double x4 = 1.0;    // 建立Problem    Problem problem;    // 设置四个残差方程Fx，用自动求导获得导数（或者雅克比矩阵）    // 自动求导的模板参数<误差类型， 输出维度， 输入维度>，如果输入有多个参数，就依次展开    CostFunction* cost_function1 = new AutoDiffCostFunction<F1, 1, 1, 1>(new F1);    CostFunction* cost_function2 = new AutoDiffCostFunction<F2, 1, 1, 1>(new F2);    CostFunction* cost_function3 = new AutoDiffCostFunction<F3, 1, 1, 1>(new F3);    CostFunction* cost_function4 = new AutoDiffCostFunction<F4, 1, 1, 1>(new F4);    // 设置残差模块，参数（代价函数，核函数，待估计参数）    problem.AddResidualBlock(cost_function1, NULL, &x1, &x2);    problem.AddResidualBlock(cost_function2, NULL, &x3, &x4);    problem.AddResidualBlock(cost_function3, NULL, &x2, &x3);    problem.AddResidualBlock(cost_function4, NULL, &x1, &x4);    // 设置求解器选项    Solver::Options options;    options.max_num_iterations = 100;    options.linear_solver_type = ceres::DENSE_QR;   //增量方程的求解方式    options.minimizer_progress_to_stdout = true;    //是否把最小二乘的优化过程输出到cout    cout << "Initial x1 = "<< x1 << ", x2 = " << x2 << ", x3 = " << x3 << ", x4 = " << x4 << endl;    Solver::Summary summary;    //优化信息    Solve(options, &problem, &summary); //开始优化    cout << summary.BriefReport() << endl;  //简要报告    cout << summary.FullReport() << endl;   //完整报告    cout << "Final x1 = " << x1 << ", x2 = " << x2 << ", x3 = " << x3 << ", x4 = " << x4 << endl;    return 0;}

对应的CMakeLists.txt文件：

cmake_minimum_required(VERSION 3.8)project(ceres_study)set(CMAKE_CXX_STANDARD 11)find_package(Ceres REQUIRED)include_directories( ${CERES_INCLUDE_DIRS} )set(SOURCE_FILES powell.cpp)add_executable(ceres_study ${SOURCE_FILES})target_link_libraries(ceres_study ${CERES_LIBRARIES})

编译运行结果：

Initial x1 = 3, x2 = -1, x3 = 0, x4 = 1iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time   0  1.075000e+02    0.00e+00    1.55e+02   0.00e+00   0.00e+00  1.00e+04        0    5.84e-05    1.25e-04   1  5.036190e+00    1.02e+02    2.00e+01   2.16e+00   9.53e-01  3.00e+04        1    7.50e-05    2.31e-04   2  3.148168e-01    4.72e+00    2.50e+00   6.23e-01   9.37e-01  9.00e+04        1    4.01e-05    2.84e-04   3  1.967760e-02    2.95e-01    3.13e-01   3.08e-01   9.37e-01  2.70e+05        1    3.73e-05    3.31e-04   4  1.229900e-03    1.84e-02    3.91e-02   1.54e-01   9.37e-01  8.10e+05        1    3.58e-05    3.76e-04   5  7.687123e-05    1.15e-03    4.89e-03   7.69e-02   9.37e-01  2.43e+06        1    3.09e-05    4.16e-04   6  4.804625e-06    7.21e-05    6.11e-04   3.85e-02   9.37e-01  7.29e+06        1    2.85e-05    4.51e-04   7  3.003028e-07    4.50e-06    7.64e-05   1.92e-02   9.37e-01  2.19e+07        1    2.82e-05    4.85e-04   8  1.877006e-08    2.82e-07    9.54e-06   9.62e-03   9.37e-01  6.56e+07        1    2.82e-05    5.19e-04   9  1.173223e-09    1.76e-08    1.19e-06   4.81e-03   9.37e-01  1.97e+08        1    2.93e-05    5.57e-04  10  7.333425e-11    1.10e-09    1.49e-07   2.40e-03   9.37e-01  5.90e+08        1    2.81e-05    5.91e-04  11  4.584044e-12    6.88e-11    1.86e-08   1.20e-03   9.37e-01  1.77e+09        1    3.06e-05    6.28e-04  12  2.865573e-13    4.30e-12    2.33e-09   6.02e-04   9.37e-01  5.31e+09        1    3.53e-05    6.73e-04  13  1.791438e-14    2.69e-13    2.91e-10   3.01e-04   9.37e-01  1.59e+10        1    3.40e-05    7.17e-04  14  1.120029e-15    1.68e-14    3.64e-11   1.51e-04   9.37e-01  4.78e+10        1    3.90e-05    7.66e-04Ceres Solver Report: Iterations: 15, Initial cost: 1.075000e+02, Final cost: 1.120029e-15, Termination: CONVERGENCESolver Summary (v 1.13.0-eigen-(3.2.0)-lapack-suitesparse-(4.2.1)-cxsparse-(3.1.2)-openmp)                                     Original                  ReducedParameter blocks                            4                        4Parameters                                  4                        4Residual blocks                             4                        4Residual                                    4                        4Minimizer                        TRUST_REGIONDense linear algebra library            EIGENTrust region strategy     LEVENBERG_MARQUARDT                                        Given                     UsedLinear solver                        DENSE_QR                 DENSE_QRThreads                                     1                        1Linear solver threads                       1                        1Linear solver ordering              AUTOMATIC                        4Cost:Initial                          1.075000e+02Final                            1.120029e-15Change                           1.075000e+02Minimizer iterations                       15Successful steps                           15Unsuccessful steps                          0Time (in seconds):Preprocessor                         0.000067  Residual evaluation                0.000023  Jacobian evaluation                0.000394  Linear solver                      0.000071Minimizer                            0.000715Postprocessor                        0.000005Total                                0.000787Termination:                      CONVERGENCE (Gradient tolerance reached. Gradient max norm: 3.642190e-11 <= 1.000000e-10)Final x1 = 0.000146222, x2 = -1.46222e-05, x3 = 2.40957e-05, x4 = 2.40957e-05

可以看到，该问题的最优结果在x1=0,x2=0,x3=0,x4=0，此时目标方程结果值为0。在第十次迭代中，Ceres计算得到的结果是4×10−12。

Curve Fitting

直到现在，我们看到的例子都是没有数据的简单优化问题。最小二乘法和非线性最小二乘法分析的最初目的是拟合数据曲线。现在就来考虑一个这样的例子，数据是由曲线y=e0.3x+0.1抽样生成的，再添加标准差为σ=0.2的高斯误差。让我们将一些数据拟合到曲线上：

y=emx+c

首先，定义一个模板对象来评估残差，对每一个观测值都会有一个残差。

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:  // Observations for a sample.  const double x_;  const double y_;};

假设观测值是一个2n大小的数组，那么问题的构建就是为每个观测创建一个Cost函数。构建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);}

整个曲线拟合优化代码为：

#include <iostream>#include <ceres/ceres.h>#include <vector>#include <glog/logging.h>#include <gflags/gflags.h>#include <opencv2/core.hpp>using namespace std;using ceres::AutoDiffCostFunction;using ceres::CostFunction;using ceres::Problem;using ceres::Solver;using ceres::Solve;struct ExponentialResidual {    ExponentialResidual(double x, double y):x_(x), y_(y) {}    template <typename T>            bool operator()(const T* const m,   //待估参数m                            const T* const c,   //待估参数c                            T* residual) const {        residual[0] = y_ - exp(m[0]*x_ + c[0]);        return true;    }private:    const double x_;  //x_, y_已知    const double y_;};int main(int argc, char** argv){    double m = 0.3, c = 0.1;    //生成数据集的准确值    int N = 5;                  //x的最大值    double sigma = 0.2;         //高斯噪声的标准差    cv::RNG rng;                //OpenCV随机数生成器    vector<double> x_data, y_data;    // 生成随机数    for(double i=0; i<=N ; i=i+0.075)    {        x_data.push_back(i);        double yi = exp(m*i+c)+rng.gaussian(sigma);        y_data.push_back(yi);    }    m = 0.0, c = 0.0;    //待估计值的初始值    // 建立Problem    Problem problem;    // 对于每一对(x, y)都要添加残差模块    for ( int i=0; i < x_data.size(); i++)    {        problem.AddResidualBlock(                new AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>(new ExponentialResidual(x_data[i], y_data[i])),                NULL,                &m, &c);    }    cout << "Intial m = " << m << ", n = " << c << endl;    // 设定求解选项    Solver::Options options;    options.max_num_iterations = 25;    options.minimizer_progress_to_stdout = true;    options.linear_solver_type = ceres::DENSE_QR;    // 运行求解器    Solver::Summary summary;    Solve(options, &problem, &summary);    cout << summary.BriefReport() << endl;    cout << "Final m = " << m << ", n = " << c << endl;    return 0;}

对应的CMakelists.txt为：

cmake_minimum_required(VERSION 3.8)project(ceres_study)set(CMAKE_CXX_STANDARD 11)# ceresfind_package(Ceres REQUIRED)include_directories( ${CERES_INCLUDE_DIRS} )# OpenCVfind_package(OpenCV REQUIRED)include_directories(${OpenCV_DIRS})set(SOURCE_FILES curve_fitting.cpp)add_executable(ceres_study ${SOURCE_FILES})target_link_libraries(ceres_study ${CERES_LIBRARIES} ${OpenCV_LIBS})

编译运行结果：

Intial m = 0, n = 0iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time   0  1.204244e+02    0.00e+00    3.60e+02   0.00e+00   0.00e+00  1.00e+04        0    3.24e-04    3.74e-04   1  2.425961e+03   -2.31e+03    0.00e+00   8.02e-01  -1.95e+01  5.00e+03        1    3.96e-05    4.64e-04   2  2.422258e+03   -2.30e+03    0.00e+00   8.02e-01  -1.95e+01  1.25e+03        1    1.83e-05    5.08e-04   3  2.400245e+03   -2.28e+03    0.00e+00   7.98e-01  -1.93e+01  1.56e+02        1    1.49e-05    5.34e-04   4  2.210383e+03   -2.09e+03    0.00e+00   7.66e-01  -1.77e+01  9.77e+00        1    1.47e-05    5.59e-04   5  8.483095e+02   -7.28e+02    0.00e+00   5.71e-01  -6.32e+00  3.05e-01        1    1.43e-05    5.84e-04   6  3.404435e+01    8.64e+01    4.10e+02   3.12e-01   1.37e+00  9.16e-01        1    6.54e-04    1.25e-03   7  7.242644e+00    2.68e+01    1.84e+02   1.27e-01   1.11e+00  2.75e+00        1    6.11e-04    1.97e-03   8  3.933925e+00    3.31e+00    5.81e+01   3.45e-02   1.03e+00  8.24e+00        1    5.71e-04    2.64e-03   9  2.333679e+00    1.60e+00    2.52e+01   9.77e-02   9.90e-01  2.47e+01        1    3.54e-04    3.02e-03  10  1.419436e+00    9.14e-01    8.73e+00   1.16e-01   9.83e-01  7.42e+01        1    3.44e-04    3.38e-03  11  1.245322e+00    1.74e-01    1.43e+00   6.69e-02   9.89e-01  2.22e+02        1    3.45e-04    3.74e-03  12  1.237976e+00    7.35e-03    9.21e-02   1.61e-02   9.91e-01  6.67e+02        1    3.42e-04    4.10e-03  13  1.237935e+00    4.11e-05    9.96e-04   1.28e-03   9.90e-01  2.00e+03        1    3.40e-04    4.46e-03Ceres Solver Report: Iterations: 14, Initial cost: 1.204244e+02, Final cost: 1.237935e+00, Termination: CONVERGENCEFinal m = 0.301681, n = 0.0907709

从参数m=0，c=0开始，目标方程值的结果最开始为120.424，Ceres找到的结果是m=0.301681,n=0.0907709，此时目标方程值为1.2379。参数值的优化结果和原来模型的m=0.3,c=0.1有一点不同，但这是意料之中的。当从噪声数据中重建曲线时，我们期望看到这样的偏差。事实上，如果要评估m=0.3,c=0.1的目标函数，那么目标函数值为1.237935。最后的拟合曲线如下图所示：

Robust Curve Fitting

现在，假设给出的数据中含有异常点，也即有一些点并不遵从噪声模型。如果我们使用上面的代码区拟合曲线，我们将会得到下面的曲线，拟合曲线会偏离真值曲线。

为了处理异常点，标准做法是使用一个损失函数LossFunction。损失函数减少了较大残差值对残差块的影响，通常是对应于异常点。为了在残差块中使用损失函数，我们修改以下代码：

problem.AddResidualBlock(cost_function, NULL , &m, &c);

为

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

CauchyLoss是Ceres求解器的损失函数之一。0.5指定了损失函数的大小。结果，我们可以得到下面的拟合图。注意，拟合曲线和真值曲线已经很接近了。