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} )
#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 a=1.0, b=2.0, c=1.0; // 真实参数值int N=100; // 数据点double w_sigma=1.0; // 噪声Sigma值cv::RNG rng; // OpenCV随机数产生器double abc[3] = {0,0,0}; // abc参数的估计值vector<double> x_data, y_data; // 数据cout<<"generating data: "<<endl;for ( int i=0; i<N; i++ ){double x = i/100.0;x_data.push_back ( x );y_data.push_back (exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma ));cout<<x_data[i]<<" "<<y_data[i]<<endl;}// 构建最小二乘问题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_QR; // 增量方程如何求解options.minimizer_progress_to_stdout = true; // 输出到coutceres::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;
}没问题！Ceres 是一个非常强大的非线性优化库（由 Google 开发），虽然初看有点吓人，但它的逻辑非常严谨。
这段代码的目标是拟合一条指数曲线：
我们通过 4 个核心步骤来拆解这段代码。
1. 定义代价函数 (Cost Function)
这是 Ceres 最灵魂的部分。你需要告诉它：“我的误差是怎么算的？”
struct CURVE_FITTING_COST {CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}// 这里的 operator() 是一个模板，为了方便 Ceres 自动求导template <typename T>bool operator() (const T* const abc, // 待优化的模型参数 [a, b, c]T* residual ) const // 残差（预测值与真实值的差）{// 公式：y - exp(ax^2 + bx + c)// 注意：这里必须用 ceres::exp 而不是 std::exp，因为 T 可能是 Ceres 自定义的类型（用于求导）residual[0] = T ( _y ) - ceres::exp ( abc[0]*T ( _x ) *T ( _x ) + abc[1]*T ( _x ) + abc[2] );return true;}const double _x, _y;
};template <typename T>: Ceres 使用“自动求导”（AutoDiff），它会传入一种特殊类型Jet来计算导数。所以你的公式必须写成模板。- 残差 (Residual): 优化的目标就是让
residual的平方和最小。
2. 生成模拟数据
在 main 函数开头，代码模拟了一些带噪声的数据点：
double a=1.0, b=2.0, c=1.0; // 真实的参数（我们要努力去猜的目标）
double abc[3] = {0,0,0}; // 初始猜测值（从 0 开始猜）// ... 循环生成 100 个点 ...
y_data.push_back (exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma ) // 真实公式 + 高斯噪声
);- 注意: 即使我们知道答案是
(1, 2, 1)，但在优化开始前，我们设定初始值为(0, 0, 0)。
3. 构建优化问题 (The Problem)
这是把数据和公式“喂”给 Ceres 的过程：
ceres::Problem problem;
for ( int i=0; i<N; i++ ) {problem.AddResidualBlock ( // 使用自动求导：<代价函数类型, 输出维度, 输入参数维度>new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3> ( new CURVE_FITTING_COST ( x_data[i], y_data[i] )),nullptr, // 核函数 (Loss Function)，这里填空表示不处理离群点abc // 我们要优化的参数块);
}AddResidualBlock: 每一对 数据都会产生一个残差块。1, 3: 这里的1代表残差是 1 维的（一个标量误差），3代表我们要优化的参数abc有 3 个元素。
4. 配置并求解
最后一步，设置求解器的“脾气”并启动：
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR; // 求解增量方程的方法，小规模问题常用 DENSE_QR
options.minimizer_progress_to_stdout = true; // 把优化过程打印出来看ceres::Solver::Summary summary;
ceres::Solve ( options, &problem, &summary ); // 咔哒一声，开始计算！DENSE_QR: 就像解方程组的算法，对于参数很少的问题，这种方法又快又稳。summary: 记录了优化是否成功、迭代了几次、最终误差是多少。
总结 Ceres 的使用套路：
- 写个 Struct: 里面定义残差公式（记得用模板
T）。 - 实例化 Problem: 循环
AddResidualBlock把所有观测数据塞进去。 - 选个 Options: 决定用什么算法解。
- Solve: 等待结果。
小建议：
如果你在运行这个代码，可以尝试把 abc 的初始值改得离真实值远一点（比如 100, 100, 100），看看 Ceres 能不能找回来。对于非线性问题，初始值非常关键，如果给得太离谱，优化可能会掉进“坑里”（局部最优解）。
想尝试把这个模型改成简单的直线拟合 来练练手吗？我可以帮你改写一下。
#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <memory>
#include <chrono>
using namespace std; // 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void setToOriginImpl() // 重置
{
_estimate << 0,0,0;
}
virtual void oplusImpl( const double* update ) // 更新
{
_estimate += Eigen::Vector3d(update);
}
// 存盘和读盘：留空
virtual bool read( istream& in ) {}
virtual bool write( ostream& out ) const {}
};// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
// 计算曲线模型误差
void computeError()
{
const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
const Eigen::Vector3d abc = v->estimate();
_error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
}
virtual bool read( istream& in ) {}
virtual bool write( ostream& out ) const {}
public:
double _x; // x 值， y 值为 _measurement
};int main( int argc, char** argv )
{
double a=1.0, b=2.0, c=1.0; // 真实参数值
int N=100; // 数据点
double w_sigma=1.0; // 噪声Sigma值
cv::RNG rng; // OpenCV随机数产生器
double abc[3] = {0,0,0}; // abc参数的估计值 vector<double> x_data, y_data; // 数据
cout<<"generating data: "<<endl;
for ( int i=0; i<N; i++ )
{
double x = i/100.0;
x_data.push_back ( x );
y_data.push_back (
exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
);
cout<<x_data[i]<<" "<<y_data[i]<<endl;
}
// 构建图优化，先设定g2o
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block; // 每个误差项优化变量维度为3，误差值维度为1
auto linearSolver = std::make_unique<g2o::LinearSolverDense<Block::PoseMatrixType>>(); // 线性方程求解器
auto solver_ptr = std::make_unique<Block>( std::move(linearSolver) ); // 矩阵块求解器
// 梯度下降方法，从GN, LM, DogLeg 中选
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( std::move(solver_ptr) );
// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
// g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm( solver ); // 设置求解器
optimizer.setVerbose( true ); // 打开调试输出
// 往图中增加顶点
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate( Eigen::Vector3d(0,0,0) );
v->setId(0);
optimizer.addVertex( v );
// 往图中增加边
for ( int i=0; i<N; i++ )
{
CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
edge->setId(i);
edge->setVertex( 0, v ); // 设置连接的顶点
edge->setMeasurement( y_data[i] ); // 观测数值
edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵：协方差矩阵之逆
optimizer.addEdge( edge );
}
// 执行优化
cout<<"start optimization"<<endl;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.initializeOptimization();
optimizer.optimize(100);
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;
// 输出优化值
Eigen::Vector3d abc_estimate = v->estimate();
cout<<"estimated model: "<<abc_estimate.transpose()<<endl;
return 0;
}这段代码是使用g20 来实现曲线拟合 也同样的帮我逐行讲解，让我也学会
学完了 Ceres，再看 g2o (General Graph Optimization)，你会发现它的逻辑稍微“绕”了一点，但结构感极强。
如果说 Ceres 是把优化问题看作一堆等式，那么 g2o 就是把优化问题看作一张图 (Graph)：
- 顶点 (Vertex)：我们要优化的变量（比如 ）。
- 边 (Edge)：观测值与变量之间的约束关系（即残差项）。
第一部分：定义顶点 (Vertex)
在 g2o 中，你必须定义一个类来代表你要优化的变量。
// 模板参数：变量维度（3维：a,b,c），变量数据类型（Eigen::Vector3d）
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d> {
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Eigen 内存对齐，老规矩了// 重置变量virtual void setToOriginImpl() {_estimate << 0,0,0;}// 更新变量：当优化算法算出一个增量 update 时，如何加到当前的变量上virtual void oplusImpl( const double* update ) {_estimate += Eigen::Vector3d(update); // 简单的向量加法}// 存盘读盘（这里不涉及，留空即可）virtual bool read( istream& in ) {}virtual bool write( ostream& out ) const {}
};_estimate: 这是父类定义的成员变量，用来存当前的 。oplusImpl: 这个函数非常重要。在复杂问题（如旋转）中，更新不是简单的加法。但在曲线拟合中，加法就够了。
第二部分：定义边 (Edge)
边代表了“观测”。每多一个数据点 ，就多一条边。
// 模板参数：观测值维度（1维：y值），类型（double），连接的顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWCurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}// 计算误差：这是核心！void computeError() {// 取出这条边连接的顶点const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);const Eigen::Vector3d abc = v->estimate(); // 获取当前估计的 a,b,c// _error(0,0) = 实际观测值 - 预测值// _measurement 是基类自带的变量，存放 setMeasurement 进去的值_error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) );}virtual bool read( istream& in ) {}virtual bool write( ostream& out ) const {}
public:double _x; // 观测值 y 已经存在 _measurement 里的，这里只存 x
};_vertices[0]: 因为是“一元边”（UnaryEdge），只连一个顶点。_measurement: 这是 g2o 存储 y 值的地方。
第三部分：配置求解器 (Solver Boilerplate)
g2o 的初始化代码通常比较长，像是在搭积木：
// 1. 每个误差项优化变量维度为3，误差值维度为1
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;// 2. 线性方程求解器：处理 Hx = -b
auto linearSolver = std::make_unique<g2o::LinearSolverDense<Block::PoseMatrixType>>();// 3. 矩阵块求解器：把线性求解器包装一层
auto solver_ptr = std::make_unique<Block>( std::move(linearSolver) );// 4. 选择优化算法（GN, LM, 还是 DogLeg）
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( std::move(solver_ptr) );// 5. 最后定义总的图优化器
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm( solver );这部分基本是模版代码。对于新手，你只需要记住改变 BlockSolverTraits<变量维度, 误差维度> 即可。
第四部分：向图中添加数据
// 增加一个顶点
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate( Eigen::Vector3d(0,0,0) ); // 初始值
v->setId(0); // 顶点的唯一编号
optimizer.addVertex( v );// 增加 100 条边
for ( int i=0; i<N; i++ ) {CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );edge->setId(i);edge->setVertex( 0, v ); // 连向编号为 0 的顶点edge->setMeasurement( y_data[i] ); // 观测值// 信息矩阵：这里是权重，1/sigma^2 表示对这个点信任程度edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) );optimizer.addEdge( edge );
}- 信息矩阵 (Information Matrix)：在 g2o 中，它是协方差矩阵的逆。简单理解就是：如果这个点噪声很大，信息矩阵的值就小，优化时就不太听它的。
第五部分：执行
optimizer.initializeOptimization(); // 初始化
optimizer.optimize(100); // 最多迭代 100 次总结：g2o vs Ceres
| 特性 | Ceres | g2o |
|---|---|---|
| 编程模型 | 函数式 (Functional) | 结构式 (Graph-based) |
| 自动求导 | 极其强大 | 支持，但通常需要手动指定或写得更复杂 |
| 直观程度 | 像在写数学公式 | 像在搭电路/建网络 |
| 擅长领域 | 通用优化 | SLAM（视觉里程计等图优化） |
你会发现：g2o 的代码量虽然多，但它的顶点和边的概念在处理那种“成千上万个点互相连接”的复杂问题（比如几千张照片的定位）时，比 Ceres 更好管理。
想试试看把 Ceres 和 g2o 的结果对比一下吗？或者你想尝试手动给 g2o 的边增加一个雅可比矩阵 (Jacobian) 计算来提高性能？我可以教你具体的求导写法。
