視覺十四講：第六講_ceres非線性優化

使用Ceres求解非線性優化問題，一共分為三個部分：
1、 第一部分：構建cost fuction，即代價函數，也就是尋優的目標式。這個部分需要使用仿函數（functor）這一技巧來實現，做法是定義一個cost function的結構體，在結構體內重載（）運算符。
2、 第二部分：通過代價函數構建待求解的優化問題。
3、 第三部分：配置求解器參數並求解問題，這個步驟就是設置方程怎么求解、求解過程是否輸出等，然后調用一下Solve方法

#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>

using namespace std;

// 代價函數的計算模型
struct CURVE_FITTING_COST {
  CURVE_FITTING_COST(double x, double y) : _x(x), _y(y) {}

  // 殘差的計算
  template<typename T>
  bool operator()(
    const T *const abc, // 模型參數，待優化的參數，有3維
    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數據
};

int main(int argc, char **argv) {
  double ar = 1.0, br = 2.0, cr = 1.0;         // 真實參數值
  double ae = 2.0, be = -1.0, ce = 5.0;        // 估計參數值
  int N = 100;                                 // 數據點
  double w_sigma = 1.0;                        // 噪聲Sigma值
  double inv_sigma = 1.0 / w_sigma;
  cv::RNG rng;                                 // OpenCV隨機數產生器

  vector<double> x_data, y_data;      // 數據
  for (int i = 0; i < N; i++) {
    double x = i / 100.0;
    x_data.push_back(x);
    y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
  }

  double abc[3] = {ae, be, ce};

  // 構建最小二乘問題
  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                 // 待估計參數
    );
  }

  // 配置求解器
  ceres::Solver::Options options;     // 這里有很多配置項可以填
  options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;  // 增量方程如何求解
  //options.linear_solver_type = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = true;   // 輸出到cout

  ceres::Solver::Summary summary;                // 優化信息
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  ceres::Solve(options, &problem, &summary);  // 開始優化，求解
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

  // 輸出結果
  cout << summary.BriefReport() << endl;   //輸出優化的簡要信息
  cout << "estimated a,b,c = ";
  for (auto a:abc) cout << a << " ";
  cout << endl;

  return 0;
}

cmakelists.txt:

cmake_minimum_required(VERSION 2.8)
project(gaussnewton)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})
include_directories("/usr/include/eigen3")
set(SOURCE_FILES main.cpp)
add_executable(gaussnewton ${SOURCE_FILES})
target_link_libraries(gaussnewton ${OpenCV_LIBS})
target_link_libraries(gaussnewton ${CERES_LIBRARIES})