1. Ceres Solver
1.1 简介
- 定位：一个功能强大、通用的非线性最小二乘问题求解器。
- 哲学：提供一套丰富的API，让用户能够轻松地定义和构建残差项，并自动或手动指定微分方式，最终求解这些残差的平方和的最小值。它更像一个“数学工具”。
- 由Google维护，非常活跃。
- 主要开源项目：VINS、OB-GINS
1.2 使用示例
// 在 problem 中构建最小二乘问题
ceres::Problem problem;
// 在 problem 中加入残差块
// cost_function 里面构造残差及雅克比矩阵
// loss_function 抗差函数，不用就传 nullptr
// parameter_blocks 待估状态量
problem.AddResidualBlock(CostFunction* cost_function, LossFunction* loss_function, const std::vector<double*>& parameter_blocks);
// 通过 options 配置最小二乘求解器，没特殊情况可以都默认，具体情况可查阅官网
ceres::Solver::Options options;
// 通过 summary 接收求解结果
ceres::Solver::Summary summary;
// 进行求解器求解
ceres::Solve(options, &problem, &summary);
非线性约束最小二乘按照求导方式可以分为：自动求导（Automatic Derivatives），数值求导（Numeric derivatives），解析求导（Analytic Derivatives）。
具体选哪种求导方式进行计算呢？ceres solver官方推荐：
（1）优先选择用Automatic Derivatives；
（2）如果Analytic Derivatives比Automatic Derivatives计算效率或精度上有明显优势，才用Analytic Derivatives
（3）如果Automatic Derivatives和Analytic Derivatives都不能解决问题，再考虑用Numeric derivatives
这里主要介绍Automatic Derivatives和Analytic Derivatives的用法，Numeric derivatives的具体用法可以在Ceres Solver官网查看
1.3 自动求导
需要在结构体中重载operator()函数，函数里面只需要构建残差，然后调用ceres::AutoDiffCostFunction()函数进行自动求导。
operator()形参中是（参数块[0]， 参数块[1]，...， 参数块[n]，残差块维数），其需与ceres::AutoDiffCostFunction<自己定义的结构体名称，残差块维数，参数块[0]维数， 参数块[1]维数，...， 参数块[n]维数>()对应，其还需与ceres::Problem::AddResidualBlock()形参中的参数块对应。如果参数块分开写最多可以写10个参数块，大于10个就需要组成std::vector的形式。
ResidualBlockId AddResidualBlock(CostFunction* cost_function,LossFunction* loss_function,const std::vector<double*>& parameter_blocks);ResidualBlockId AddResidualBlock(CostFunction* cost_function,LossFunction* loss_function,double* x0, double* x1, double* x2,double* x3, double* x4, double* x5,double* x6, double* x7, double* x8,double* x9);
#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>using namespace std;struct AutoCostFunctor
{AutoCostFunctor(double x, double y): m_x(x), m_y(y) {};template <typename T_>bool operator() (const T_* const a, const T_* const b, const T_* const c, T_* residual) const{residual[0] = m_y - exp(a[0] * m_x * m_x + b[0] * m_x + c[0]);return true;}const double m_x;const double m_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};cout << "true ae: " << ar << ",\tbe: " << br << ",\tce: " << cr << endl;cout << "initial ae: " << ae << ",\tbe: " << be << ",\tce: " << ce << endl;ceres::LossFunction* loss_function = new ceres::HuberLoss(1.0);ceres::Problem problem;for (int i = 0; i < N; i++){ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<AutoCostFunctor, 1, 1, 1, 1>(new AutoCostFunctor(x_data[i], y_data[i]));//problem.AddResidualBlock(cost_function, loss_function, &ae, &be, &ce);problem.AddResidualBlock(cost_function, nullptr, &ae, &be, &ce);}ceres::Solver::Options options;options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;options.minimizer_progress_to_stdout = true;ceres::Solver::Summary summary;ceres::Solve(options, &problem, &summary);cout << "estimate ae: " << ae << ",\tbe: " << be << ",\tce: " << ce << endl;summary.BriefReport();return 0;
}
可以将new ceres::AutoDiffCostFunction<AutoCostFunctor, 1, 1, 1, 1>(new AutoCostFunctor(x_data[i], y_data[i]));部分写成函数封装在struct AutoCostFunctor中，俗称工厂方法或工厂函数
static ceres::CostFunction* Create(double x, double y)
{return new ceres::AutoDiffCostFunction<AutoCostFunctor, 1, 1, 1, 1>(new AutoCostFunctor(x, y));
}
1.4 解析求导
需要重载一个CostFunction::Evaluate()函数，函数里面需要构建残差和雅克比矩阵。
'ceres::SizedCostFunction<残差块维数， 参数块[0]维数， 参数块[1]维数，...， 参数块[n]维数> 其需与ceres::Problem::AddResidualBlock()形参中的参数块对应。理论上多个参数块可以写成一个大的一维参数块，但是ceres solver会根据参数块对雅克比矩阵进行分块，如果写成一个大的一维参数块，将导致雅克比矩阵也是一整块，不能进行一些分块计算影响性能，所以最好还是根据参数的具体含义对参数进行分块。
#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>using namespace std;class ManualCostFunctor : public ceres::SizedCostFunction<1, 3>
{
public:ManualCostFunctor(double x, double y): m_x(x), m_y(y) {}virtual ~ManualCostFunctor() {};virtual bool Evaluate(double const* const* parameters, double *residuals, double **jacobians) const{const double a = parameters[0][0], b = parameters[0][1], c = parameters[0][2];const double f = exp(a*m_x*m_x +b*m_x +c);residuals[0] = m_y - f;if ((jacobians == nullptr)|| (jacobians[0] == nullptr)){return true;}else{jacobians[0][0] = -f * m_x * m_x;jacobians[0][1] = -f * m_x;jacobians[0][2] = -f;return true;}}private:const double m_x;const double m_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));}double abc[3] = {ae, be, ce};cout << "true a: " << ar << ",\tb: " << br << ",\tc: " << cr << endl;cout << "initial a: " << ae << ",\tb: " << be << ",\tc: " << ce << endl;ceres::LossFunction* loss_function = new ceres::HuberLoss(1.0);ceres::Problem problem;for (int i = 0; i < N; i++){ceres::CostFunction* cost_function = new ManualCostFunctor(x_data[i], y_data[i]);//problem.AddResidualBlock(cost_function, loss_function, &ae, &be, &ce);problem.AddResidualBlock(cost_function, nullptr, abc);}ceres::Solver::Options options;options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;options.minimizer_progress_to_stdout = true;ceres::Solver::Summary summary;ceres::Solve(options, &problem, &summary);cout << "estimate a: " << abc[0] << ",\tb: " << abc[1] << ",\tc: " << abc[2] << endl;summary.BriefReport();return 0;
}