C++如何使用Ceres或g2o进行非线性优化_C++ 非线性优化方法-C++-PHP中文网

C++如何使用Ceres或g2o进行非线性优化_C++ 非线性优化方法

裘德小鎮的故事

发布： 2025-10-09 08:55:01

原创

240人浏览过

Ceres Solver适用于通用非线性最小二乘问题，支持自动微分与灵活残差定义，适合曲线拟合等任务；g2o专注稀疏图优化，如SLAM与位姿估计，基于图模型高效处理大规模问题；选择取决于是否为标准图结构及性能需求。

c++如何使用ceres或g2o进行非线性优化_c++ 非线性优化方法

在C++中进行非线性优化，Ceres Solver 和 g2o（General Graph Optimization） 是两个广泛使用的开源库，尤其在SLAM、Bundle Adjustment、机器人状态估计等领域表现优异。它们都基于图优化理论，但接口设计和适用场景略有不同。

1. Ceres Solver：通用非线性最小二乘优化

Ceres 由 Google 开发，适合解决一般的非线性最小二乘问题，使用自动微分，配置灵活，代码清晰。

基本使用步骤：

定义残差函数：通过仿函数（functor）或lambda表达式实现成本项。
构建问题：向 Problem 添加残差块和待优化参数。
求解：调用 Solve 并传入配置选项。

示例：拟合曲线 y = exp(ax + b)

立即学习“C++免费学习笔记（深入）”；

假设有一组数据点 (x, y)，想拟合指数函数的参数 a 和 b。

#include <ceres/ceres.h>
#include <iostream>
<p>struct ExponentialResidual {
ExponentialResidual(double x, double y) : x<em>(x), y</em>(y) {}</p><p>template <typename T>
bool operator()(const T<em> const a, const T</em> const b, T<em> residual) const {
residual[0] = T(y_) - ceres::exp(a[0] </em> T(x_) + b[0]);
return true;
}</p><p>double x<em>, y</em>;
};</p><p>int main() {
double a = 1.0, b = 0.5; // 初始值
std::vector<double> xs = {0.0, 1.0, 2.0, 3.0};
std::vector<double> ys = {1.0, 2.7, 7.4, 20.1}; // 近似 exp(x)</p><p>ceres::Problem problem;
for (int i = 0; i < xs.size(); ++i) {
ceres::CostFunction* cost_function =
new ceres::AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>(
new ExponentialResidual(xs[i], ys[i])
);
problem.AddResidualBlock(cost_function, nullptr, &a, &b);
}</p><p>ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = true;</p><p>ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);</p><p>std::cout << summary.BriefReport() << "\n";
std::cout << "Estimated a: " << a << ", b: " << b << "\n";</p><p>return 0;
}</p>

登录后复制

Ceres 支持自动微分、解析导数、鲁棒核函数（如 Huber）、边界约束等，非常适合复杂但结构不固定的优化问题。

2. g2o：面向图优化的稀疏非线性优化

g2o 更专注于稀疏图结构的优化，比如位姿图（pose graph）或BA问题。它采用图模型，节点是待优化变量（如SE(3)位姿），边是观测残差。

基本流程：

如知AI笔记

如知笔记——支持markdown的在线笔记，支持ai智能写作、AI搜索，支持DeepseekR1满血大模型

查看详情

定义图类型：选择合适的顶点和边类型（如SE3Quat, VertexSBAPointXYZ）。
构建图结构：添加顶点和边。
初始化求解器：设置线性求解器、迭代方法（如Gauss-Newton, Levenberg-Marquardt）。
执行优化：调用 optimize()。

示例：简单2D位姿图优化

假设有三个2D位姿节点，带有一些相对位姿观测。

#include <g2o/core/g2o_core_api.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/types/slam2d/types_slam2d.h>
#include <iostream>
<p>int main() {
g2o::SparseOptimizer optimizer;
auto linearSolver = std::make_unique<g2o::LinearSolverDense<
g2o::BlockSolverX::PoseMatrixType>>();
auto blockSolver = std::make_unique<g2o::BlockSolverX>(std::move(linearSolver));
g2o::OptimizationAlgorithmLevenberg* solver =
new g2o::OptimizationAlgorithmLevenberg(std::move(blockSolver));
optimizer.setAlgorithm(solver);</p><p>// 添加顶点
g2o::VertexSE2* v1 = new g2o::VertexSE2();
v1->setId(0);
v1->setEstimate(g2o::SE2(0, 0, 0));
optimizer.addVertex(v1);</p><p>g2o::VertexSE2* v2 = new g2o::VertexSE2();
v2->setId(1);
v2->setEstimate(g2o::SE2(2, 0, 0));
optimizer.addVertex(v2);</p><p>// 添加边（v1到v2的理想观测为 (2,0,0)）
g2o::EdgeSE2* e12 = new g2o::EdgeSE2();
e12->setMeasurement(g2o::SE2(2, 0, 0)); // 观测值
e12->setInformation(Eigen::Matrix3d::Identity());
e12->setVertex(0, v1);
e12->setVertex(1, v2);
optimizer.addEdge(e12);</p><p>optimizer.initializeOptimization();
optimizer.optimize(20);</p><p>std::cout << "Optimized pose 2: " << v2->estimate().translation().x() << ", "
<< v2->estimate().translation().y() << "\n";</p><p>optimizer.deleteSurface();
return 0;
}</p>

登录后复制

g2o 的优势在于对大规模稀疏系统高效，支持多种李群类型（SE3、SO3等），常用于视觉SLAM前端后端。