ceres使用方式

CMakeLists写的方式

ceres安装和CMakeLists教程

cmake_minimum_required( VERSION 2.8 )
project( ceres_curve_fitting )

set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

# 添加cmake模块以使用ceres库
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )

# 寻找Ceres库并添加它的头文件
find_package( Ceres REQUIRED )
include_directories( ${CERES_INCLUDE_DIRS} )

# OpenCV
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_DIRS} )

add_executable( curve_fitting main.cpp )
# 与Ceres和OpenCV链接
target_link_libraries( curve_fitting ${CERES_LIBRARIES} ${OpenCV_LIBS} )

使用ceres进行BA

BAL数据集
BAL数据集
参考链接

Ceres Solver提供了三种求导方式：自动求导、数值求导和解析求导。

自动求导方式

1. 自动求导是通过定义一个仿函数，然后传给AutoDiffCostFunction，就可以让Ceres自己去求导。
2. 所谓仿函数，其实是一个类，只不过这个类的作用像函数，所以叫仿函数。原理就是类实现了operator()函数。
3. 自动求导仿函数实现的operator()函数必须是模板函数，因为Ceres内部求导要用到。
   可直接理解T为double。

\\构造仿函数
struct AutoCostFunctor {
  AutoCostFunctor(const double x, const double y) : x_(x), y_(y) {}
  template <typename T> bool operator()(const T *const abc, T *residual) const {
    residual[0] = T(y_) - ceres::exp(abc[0] * x_ * x_ + abc[1] * x_ + abc[2]);
    return true;
  }

private:
  double x_, y_;
};

\\为仿函数加边
Problem problem;
for (int i = 0; i < N; i++) {
  AutoDiffCostFunction<AutoCostFunctor,1,3> *costFunction = new AutoDiffCostFunction<AutoCostFunctor,1,3>
      (new AutoCostFunctor(x_data[i],y_data[i]));
  problem.AddResidualBlock(costFunction, nullptr,abc);
}

数值求导方式

1. 有时，无法定义自动求导的模板仿函数，比如参数的估计调用了无法控制的库函数或外部函数。
   这种情况无法使用自动求导了，数值求导便可以派上用场了。

数值求导用法类似，先定义仿函数，然后传递给NumericDiffCostFunction，然后去构造问题求解。

//数值求导，构造仿函数
struct NumericCostFunctor {
  NumericCostFunctor(double x, double y) : x_(x), y_(y) {}
  bool operator()(const double *const abc, double *residual) const {
    residual[0] = y_ - ceres::exp(abc[0] * x_ * x_ + abc[1] * x_ + abc[2]);
    return true;
  }

private:
  double x_, y_;
};

\\为仿函数加边
Problem problem;
for (int i = 0; i < N; i++) {
  auto costFunction =
      new NumericDiffCostFunction<NumericCostFunctor, ceres::CENTRAL, 1, 3>(
          new NumericCostFunctor(x_data[i], y_data[i]));
  problem.AddResidualBlock(costFunction, nullptr, abc);
}

解析求导

1. 有些情况，自己写求导解析式，计算效率会更高一些。

如果使用解析求导的方式，就要自行计算残差和雅克比。

//继承解析求导的源类
class AnalyticCostFunction : public ceres::SizedCostFunction<1, 3> {
public:
  AnalyticCostFunction(const double x, const double y) : x_(x), y_(y) {}

  virtual ~AnalyticCostFunction() {}
  virtual bool Evaluate(double const *const *parameters, double *residuals,
                        double **jacobians) const {
    const double a = parameters[0][0];
    const double b = parameters[0][1];
    const double c = parameters[0][2];
    residuals[0] = ceres::exp(a * x_ * x_ + b * x_ + c) - y_;

    if (!jacobians)
      return true;
    double *jacobian = jacobians[0];
    if (!jacobian)
      return true;
    jacobian[0] = x_ * x_ * ceres::exp(a * x_ * x_ + b * x_ + c);
    jacobian[1] = x_ * ceres::exp(a * x_ * x_ + b * x_ + c);
    jacobian[2] = ceres::exp(a * x_ * x_ + b * x_ + c);
    return true;
  }

private:
  double x_, y_;
};

//添加边
Problem problem;
for (int i = 0; i < N; i++) {
  auto costFunction = new AnalyticCostFunction(x_data[i],y_data[i]);
  problem.AddResidualBlock(costFunction, nullptr, abc);
}

造数据

  vector<double> x_data, y_data;
  double a = 1.0, b = 2.0, c = 3.0;
  int N = 100;
  double sigma = 1.0;
  cv::RNG rng;
  for (int i = 0; i < N; i++) {
    double x = static_cast<double>(i / 100.0);
    x_data.push_back(x);
    y_data.push_back(std::exp(a * x * x + b * x + c) + rng(sigma));
  }
  double abc[3] = {0., 0., 0.};

完整的代码

• Cmakelist

cmake_minimum_required(VERSION 3.17)
project(Ceres_test)

set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

# 添加cmake模块以使用ceres库
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )

# 寻找Ceres库并添加它的头文件
find_package( Ceres REQUIRED )

# OpenCV
find_package( OpenCV REQUIRED )
include_directories(${PROJECT_SOURCE_DIR}/include ${CERES_INCLUDE_DIRS} ${OpenCV_DIRS} )

aux_source_directory(${PROJECT_SOURCE_DIR}/src SRC_LIST)

add_executable(${PROJECT_NAME} ${SRC_LIST})
# 与Ceres和OpenCV链接
target_link_libraries(${PROJECT_NAME} ${CERES_LIBRARIES} ${OpenCV_LIBS} )

• 代码

#include <ceres/ceres.h>
#include <cmath>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <string>
#include <vector>

using namespace ceres;
using namespace std;

struct AutoCostFunctor {
  AutoCostFunctor(const double x, const double y) : x_(x), y_(y) {}
  template <typename T> 
  bool operator()(const T *const abc, T *residual) const {
    residual[0] = T(y_) - ceres::exp(abc[0] * x_ * x_ + abc[1] * x_ + abc[2]);
    return true;
  }

private:
  double x_, y_;
};

//数值求导
struct NumericCostFunctor {
  NumericCostFunctor(double x, double y) : x_(x), y_(y) {}
  bool operator()(const double *const abc, double *residual) const {
    residual[0] = y_ - ceres::exp(abc[0] * x_ * x_ + abc[1] * x_ + abc[2]);
    return true;
  }

private:
  double x_, y_;
};

//解析求导
class AnalyticCostFunction : public ceres::SizedCostFunction<1, 3> {
public:
  AnalyticCostFunction(const double x, const double y) : x_(x), y_(y) {}

  virtual ~AnalyticCostFunction() {}
  virtual bool Evaluate(double const *const *parameters, double *residuals,
                        double **jacobians) const {
    const double a = parameters[0][0];
    const double b = parameters[0][1];
    const double c = parameters[0][2];
    residuals[0] = ceres::exp(a * x_ * x_ + b * x_ + c) - y_;

    if (!jacobians)
      return true;
    double *jacobian = jacobians[0];
    if (!jacobian)
      return true;
    jacobian[0] = x_ * x_ * ceres::exp(a * x_ * x_ + b * x_ + c);
    jacobian[1] = x_ * ceres::exp(a * x_ * x_ + b * x_ + c);
    jacobian[2] = ceres::exp(a * x_ * x_ + b * x_ + c);
    return true;
  }

private:
  double x_, y_;
};

int main() {
  vector<double> x_data, y_data;
  double a = 1.0, b = 2.0, c = 3.0;
  int N = 100;
  double sigma = 1.0;
  cv::RNG rng;
  for (int i = 0; i < N; i++) {
    double x = static_cast<double>(i / 100.0);
    x_data.push_back(x);
    y_data.push_back(std::exp(a * x * x + b * x + c) + rng(sigma));
  }
  double abc[3] = {0., 0., 0.};

  Problem problem;
  for (int i = 0; i < N; i++) {
    auto costFunction = new AnalyticCostFunction(x_data[i],y_data[i]);
    problem.AddResidualBlock(costFunction, nullptr, abc);
  }

  Solver::Options options;
  options.linear_solver_type = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = true;

  Solver::Summary summary;
  Solve(options, &problem, &summary);
  cout << summary.BriefReport() << endl;
  for (auto a : abc) {
    cout << a << " , ";
  }
  return 0;
}