1. 项目概述与核心价值最近在整理一个机器人轨迹优化的项目里面涉及到大量的传感器数据对齐问题。传感器数据点往往带有噪声直接拿来用效果很差这时候就需要一个“平滑器”来从一堆杂乱的点里找到那条最有可能的真实轨迹。这活儿本质上就是曲线拟合。而说到拟合最小二乘法绝对是绕不开的基石。它不追求每个点都完美落在曲线上而是追求所有点到曲线的“距离”平方和最小这个思想既直观又强大在工程和科研里应用太广了。为什么这次选择用C和g2o来实现原因很实在。C的效率和性能控制是处理大规模数据点、进行实时计算时的硬需求。而g2oGeneral Graph Optimization这个库别看名字里带着“图优化”它本质上是一个求解非线性最小二乘问题的通用框架。很多刚接触的同学会觉得g2o只用于SLAM同步定位与地图构建其实大材小用了。它的核心是把一个复杂的优化问题拆解成许多简单的“边”和“顶点”然后用图的方式组织起来求解这种思想用来做曲线拟合简直是降维打击——代码结构会变得异常清晰而且能轻松应对各种复杂的曲线模型比如高次多项式、样条曲线。所以这个项目不只是“用C写个拟合函数”。它的深层价值在于掌握一种用现代优化库解决经典数值计算问题的范式。你将学会如何把抽象的数学问题最小二乘映射到具体的编程框架g2o的图模型中这对于未来处理更复杂的优化问题比如捆绑调整、机器学习中的参数优化是一个极好的铺垫。无论你是做机器人、计算机视觉还是单纯想提升自己的C工程能力和数值计算功底跟着走一遍都会大有收获。2. 最小二乘与g2o核心原理拆解2.1 最小二乘曲线拟合的数学本质我们先抛开代码把最小二乘拟合的数学问题理清楚。假设我们有一组观测数据点(x_i, y_i), i1,...,n。我们相信这些点背后隐藏着一个函数关系y f(x, θ)其中θ是我们需要求解的参数向量。例如对于二次多项式拟合f(x, θ) θ_0 θ_1 * x θ_2 * x^2这里θ [θ_0, θ_1, θ_2]^T。由于观测存在噪声数据点不会完全落在理论曲线上。最小二乘法的目标就是找到一组参数θ*使得所有观测值y_i与理论值f(x_i, θ)之间的残差平方和最小。用公式表示就是θ* argmin_θ Σ_i [ y_i - f(x_i, θ) ]^2这个Σ [残差]^2就是我们的目标函数。求解这个最小值问题对于线性模型即f关于参数θ是线性的如多项式拟合我们可以通过求导等于零得到正规方程直接求解线性方程组。但对于非线性模型或者我们希望引入更鲁棒的损失函数时就需要迭代优化算法比如高斯-牛顿法、列文伯格-马夸尔特法。注意这里容易混淆“线性”。y a*x^2 b*x c关于变量x是非线性的但关于参数[a, b, c]是线性的所以仍可用线性最小二乘求解。如果模型是y a * exp(b*x)关于参数a和b都是非线性的就必须用非线性优化。2.2 g2o的图优化思想如何应用于拟合g2o将优化问题建模为一个图Graph。这个图由两种基本元素构成顶点Vertex代表需要优化的变量。在我们的曲线拟合问题里需要优化的参数就是顶点。比如拟合一个三次多项式y a b*x c*x^2 d*x^3那么参数向量[a, b, c, d]就构成一个顶点。边Edge代表约束或观测它连接一个或多个顶点并计算残差。每一个数据点都对应一条边。这条边会连接到一个顶点即参数顶点它的作用是计算给定当前的参数估计值我这个数据点产生的残差是多少。这样整个拟合问题就被构建成了一张图一个顶点参数块被许多条边数据点连接着。优化过程就是调整顶点的值即参数使得所有边计算的残差平方和最小。g2o内部会自动处理雅可比矩阵的计算或由我们提供、海森矩阵的近似构建并调用求解器如高斯牛顿、LM进行迭代优化。这种方式的巨大优势是模块化和可扩展性。如果想从二次拟合换成三次拟合我只需要改变顶点参数的维度。如果想给某个数据点加个权重或者改用Huber等鲁棒核函数来抑制离群点也只需要修改对应边的实现其他部分完全不用动。代码的复用性和可读性极高。3. 开发环境准备与g2o库安装3.1 基础工具链配置工欲善其事必先利其器。一个舒适的C开发环境能事半功倍。我的主力环境是Ubuntu 20.04/22.04Windows用户可以使用WSL2获得近乎一致的体验或者使用Visual Studio。编译器必须支持C11及以上标准。推荐GCC 7或Clang 5。在Ubuntu上可以通过sudo apt install build-essential安装GCC。构建系统强烈推荐使用CMake。它是管理C项目依赖和构建过程的事实标准。通过sudo apt install cmake安装。IDE/编辑器VSCode CMake Tools插件是跨平台且高效的选择。CLion是专业的C IDE对CMake支持极好。Vim/Emacs高手请自便。3.2 g2o库的安装与踩坑指南g2o的安装有一定门槛因为它依赖较多。以下是经过验证的可靠步骤# 1. 安装必备依赖库 sudo apt-get install libeigen3-dev libsuitesparse-dev qtdeclarative5-dev qt5-qmake libqglviewer-dev-qt5 # 2. 克隆g2o源代码建议使用较新的版本如2022年后的commit git clone https://github.com/RainerKuemmerle/g2o.git cd g2o # 3. 创建并进入构建目录 mkdir build cd build # 4. 配置CMake。关键选项关掉不必要的模块以减少依赖 cmake .. -DCMAKE_BUILD_TYPERelease -DG2O_BUILD_EXAMPLESOFF -DG2O_BUILD_APPSOFF # 如果你不需要GUI支持对于纯拟合不需要可以加上 -DBUILD_WITH_MARCH_NATIVEOFF (可选优化) # 5. 编译并安装 make -j$(nproc) # 使用所有CPU核心并行编译 sudo make install实操心得与常见坑点依赖缺失最常见的错误是find_package找不到Eigen3、SuiteSparse。请确保第一步的依赖安装成功。libqglviewer-dev如果找不到可以尝试libqglviewer-dev不带qt5。版本冲突如果你的系统有多个Eigen3版本可能导致编译错误。确保libeigen3-dev安装的路径在/usr/include/eigen3并在CMake时通过-DEigen3_DIR指定路径。安装路径默认安装路径是/usr/local/include和/usr/local/lib。编译自己的项目时CMake需要用find_package(g2o REQUIRED)来定位它。如果找不到可以尝试在CMakeLists.txt中手动设置g2o_DIR为/usr/local/lib/cmake/g2o/。简化安装对于仅用于曲线拟合我们其实只用到了g2o的核心优化部分。一个更轻量的方法是只编译g2o的核心库跳过所有examples和apps。上述CMake命令中的-DG2O_BUILD_EXAMPLESOFF -DG2O_BUILD_APPSOFF正是为此。验证安装创建一个简单的测试程序test_g2o.cpp包含头文件#include g2o/core/base_vertex.h用CMake能成功编译链接即说明安装成功。4. 项目实战使用g2o实现二次多项式拟合理论铺垫和环境准备就绪现在进入最核心的实战环节。我们将实现一个拟合二次函数y ax^2 bx c的完整程序。我会把代码拆解成几个部分并逐行解释。4.1 定义优化顶点Parameters首先我们需要定义待优化的参数顶点。在g2o中通过继承g2o::BaseVertex模板类来实现。// 文件名curve_fitting_vertex.h (或直接放在主cpp里) #include g2o/core/base_vertex.h #include g2o/core/hyper_graph_action.h #include Eigen/Core // 模板参数优化变量维度 变量类型 class CurveFittingVertex : public g2o::BaseVertex3, Eigen::Vector3d { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Eigen库固定大小对象需要的内存对齐宏 // 重置函数设定优化变量的初始值。这里我们初始化为0 virtual void setToOriginImpl() override { _estimate 0, 0, 0; // 对应 [a, b, c] } // 更新函数。非常重要的核心函数优化过程中求解器会提供一个增量向量v。 // 我们需要将这个增量加到当前的估计值上。 // 对于大多数参数在流形上是向量空间直接相加即可。 virtual void oplusImpl(const double* update) override { // update 是一个指向double数组的指针包含了3个增量值 _estimate Eigen::Vector3d(update[0], update[1], update[2]); } // 读和写函数留空即可除非需要保存/加载模型 virtual bool read(std::istream in) override { return false; } virtual bool write(std::ostream out) const override { return false; } };关键点解析BaseVertex3, Eigen::Vector3d第一个模板参数3表示顶点优化变量的维度这里是3维[a, b, c]。第二个是变量类型我们使用Eigen::Vector3d一个3维的双精度向量性能好且与Eigen生态无缝衔接。_estimate是基类中定义的成员变量代表该顶点当前的估计值。在setToOriginImpl和oplusImpl中我们操作的就是它。oplusImpl这个名字来源于李代数中的“⊕”运算符。对于曲线拟合这种在欧式空间中的参数更新就是简单的向量加法。如果优化变量是旋转如四元数、李群这里的更新会复杂得多需要用到流形上的扰动模型。这是g2o处理复杂状态估计的关键但在我们这个问题里暂时用不到。4.2 定义优化边Error / Residual接下来定义边也就是计算单个数据点残差的地方。继承g2o::BaseUnaryEdge一元边只连接一个顶点。// 文件名curve_fitting_edge.h #include g2o/core/base_unary_edge.h #include g2o/core/base_vertex.h #include Eigen/Core // 前向声明顶点类 class CurveFittingVertex; // 模板参数观测值维度 观测值类型 连接的顶点类型 class CurveFittingEdge : public g2o::BaseUnaryEdge1, double, CurveFittingVertex { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 构造函数需要传入数据点的x坐标 CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {} // 核心计算残差 virtual void computeError() override { // 1. 获取这条边所连接的顶点 const CurveFittingVertex* v static_castconst CurveFittingVertex*(_vertices[0]); // 2. 从顶点中获取当前的参数估计值 [a, b, c] const Eigen::Vector3d abc v-estimate(); // 3. 根据模型计算理论y值y_hat a*x^2 b*x c double y_hat abc[0] * _x * _x abc[1] * _x abc[2]; // 4. 残差 观测值y - 理论值y_hat。观测值_y在setMeasurement时设置。 _error[0] _measurement - y_hat; } // 核心计算雅可比矩阵即残差对顶点各参数的导数 virtual void linearizeOplus() override { const CurveFittingVertex* v static_castconst CurveFittingVertex*(_vertices[0]); const Eigen::Vector3d abc v-estimate(); double x _x; // 雅可比矩阵 J [∂e/∂a, ∂e/∂b, ∂e/∂c] // 因为 e y - (a*x^2 b*x c) // 所以 ∂e/∂a -x^2, ∂e/∂b -x, ∂e/∂c -1 _jacobianOplusXi[0] -x * x; // 对a的导数 _jacobianOplusXi[1] -x; // 对b的导数 _jacobianOplusXi[2] -1; // 对c的导数 } virtual bool read(std::istream in) override { return false; } virtual bool write(std::ostream out) const override { return false; } private: double _x; // 存储数据点的x坐标在构造边时传入 };关键点解析BaseUnaryEdge1, double, CurveFittingVertex第一个参数1表示观测值的维度。我们的观测是单个的y值所以是1维。第二个是观测值类型double。第三个是连接的顶点类型。_measurement继承自基类存储观测值即数据点的y坐标。我们会在主函数里通过edge-setMeasurement(y)来设置。_error继承自基类是一个Eigen向量存储计算出的残差。维度与观测值维度相同这里是1。_jacobianOplusXi继承自基类是一个Eigen矩阵存储雅可比矩阵。它的行数等于观测维度(1)列数等于顶点维度(3)。提供准确的雅可比矩阵能极大提升优化收敛速度和稳定性。g2o也支持数值求导但效率低且不精确。linearizeOplus这个函数在每次迭代中都会被调用用于计算当前估计值处的雅可比。对于简单的模型我们可以像上面一样手动推导。对于复杂模型可以使用自动微分库如Ceres内置的。4.3 构建问题与求解主流程现在我们把顶点和边组装起来构建完整的优化问题并求解。// 文件名main_curve_fitting.cpp #include iostream #include vector #include random #include g2o/core/g2o_core_api.h #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/solvers/dense/linear_solver_dense.h #include Eigen/Core // 包含我们自定义的顶点和边 #include curve_fitting_vertex.h #include curve_fitting_edge.h int main() { // -------------------- 1. 生成模拟数据 -------------------- double a 1.0, b 2.0, c 1.0; // 真实参数 int N 100; // 数据点数量 double w_sigma 1.0; // 噪声标准差 std::default_random_engine generator; std::normal_distributiondouble noise(0.0, w_sigma); std::vectordouble x_data, y_data; for (int i 0; i N; i) { double x i / 100.0; // x在[0, 1)区间 double y a * x * x b * x c noise(generator); x_data.push_back(x); y_data.push_back(y); } // -------------------- 2. 构建g2o优化器 -------------------- // 2.1 定义块求解器。我们优化的是3维参数误差是1维。 // BlockSolverTraits顶点维度 误差维度 using BlockSolverType g2o::BlockSolverg2o::BlockSolverTraits3, 1; // 使用稠密线性求解器对于小规模问题稠密求解器简单高效 using LinearSolverType g2o::LinearSolverDenseBlockSolverType::PoseMatrixType; // 2.2 创建算法求解器。这里使用列文伯格-马夸尔特法LM比高斯牛顿更鲁棒。 auto solver new g2o::OptimizationAlgorithmLevenberg( std::make_uniqueBlockSolverType(std::make_uniqueLinearSolverType())); // 2.3 构建稀疏优化器 g2o::SparseOptimizer optimizer; optimizer.setAlgorithm(solver); optimizer.setVerbose(true); // 打开调试输出可以看到迭代信息 // -------------------- 3. 添加顶点 -------------------- CurveFittingVertex* v new CurveFittingVertex(); v-setId(0); // 设置顶点ID通常从0开始 // 设置顶点的初始估计值这个值很重要会影响收敛。 // 如果初始值离真值太远非线性优化可能陷入局部最优或发散。 v-setEstimate(Eigen::Vector3d(0.0, 0.0, 0.0)); // 初始化为[0,0,0] optimizer.addVertex(v); // -------------------- 4. 添加边对应每个数据点 -------------------- for (int i 0; i N; i) { CurveFittingEdge* edge new CurveFittingEdge(x_data[i]); edge-setId(i); // 设置边ID edge-setVertex(0, v); // 设置边连接的顶点。0表示第一个也是唯一一个顶点 edge-setMeasurement(y_data[i]); // 设置观测值 y_i // 设置信息矩阵协方差矩阵的逆。这里我们认为每个观测的噪声独立同分布方差为 w_sigma^2。 // 所以信息矩阵就是 1 / (w_sigma^2) * Identity edge-setInformation(Eigen::Matrixdouble, 1, 1::Identity() * 1.0 / (w_sigma * w_sigma)); optimizer.addEdge(edge); } // -------------------- 5. 执行优化 -------------------- std::cout 开始优化... std::endl; optimizer.initializeOptimization(); optimizer.optimize(10); // 指定最大迭代次数为10 // -------------------- 6. 输出结果 -------------------- Eigen::Vector3d abc_estimate v-estimate(); std::cout \n优化结果 std::endl; std::cout 估计的参数 a, b, c abc_estimate.transpose() std::endl; std::cout 真实的参数 a, b, c a , b , c std::endl; // 计算均方根误差RMSE double total_error 0; for (int i 0; i N; i) { double y_hat abc_estimate[0] * x_data[i] * x_data[i] abc_estimate[1] * x_data[i] abc_estimate[2]; total_error (y_hat - y_data[i]) * (y_hat - y_data[i]); } double rmse std::sqrt(total_error / N); std::cout 拟合RMSE: rmse std::endl; return 0; }4.4 CMakeLists.txt 配置为了让项目顺利编译需要一个正确的CMakeLists.txt文件。cmake_minimum_required(VERSION 3.10) project(CurveFittingG2O) set(CMAKE_CXX_STANDARD 14) # 寻找必需的依赖包 find_package(Eigen3 REQUIRED) find_package(g2o REQUIRED) # 添加可执行文件 add_executable(curve_fitting_g2o main_curve_fitting.cpp curve_fitting_vertex.cpp # 如果顶点和边的实现分离到了cpp文件需要加上 curve_fitting_edge.cpp ) # 链接库 target_link_libraries(curve_fitting_g2o g2o::core g2o::stuff ${EIGEN3_LIBRARIES} # Eigen通常是头文件库但这样链接更规范 ) # 包含头文件目录 target_include_directories(curve_fitting_g2o PRIVATE ${EIGEN3_INCLUDE_DIRS})5. 编译、运行与结果分析在项目目录下执行以下命令mkdir build cd build cmake .. make -j4 ./curve_fitting_g2o如果一切顺利你会在终端看到类似以下的优化过程输出和结果开始优化... iteration0 chi2215.583333 time0.000144664 cumTime0.000144664 edges100 schur0 lambda79.000000 levenbergIter1 iteration1 chi296.751543 time0.000103215 cumTime0.000247879 edges100 schur0 lambda26.333333 levenbergIter1 iteration2 chi296.744751 time9.4172e-05 cumTime0.000342051 edges100 schur0 lambda8.777778 levenbergIter1 ... iteration8 chi296.744750 time7.7004e-05 cumTime0.000850678 edges100 schur0 lambda0.00478516 levenbergIter1 优化结果 估计的参数 a, b, c 0.925121 2.04672 0.971301 真实的参数 a, b, c 1.0, 2.0, 1.0 拟合RMSE: 0.984215结果解读迭代过程chi2就是所有边的残差平方和也就是我们的目标函数值。可以看到它随着迭代快速下降并趋于稳定说明优化在收敛。参数估计估计出的[a, b, c]接近真实值[1.0, 2.0, 1.0]。由于我们加入了标准差为1.0的高斯噪声估计值不可能完全等于真实值但非常接近。RMSE均方根误差约为0.984接近我们设定的噪声标准差1.0这说明拟合效果是合理的模型基本捕捉到了数据中的真实规律残差主要来自我们添加的噪声。6. 高级话题与性能调优6.1 鲁棒核函数应对“离群点”现实中的数据常有“离群点”Outliers即严重偏离主要趋势的噪声点。标准最小二乘对离群点非常敏感因为残差平方会放大这些大误差的影响。g2o提供了鲁棒核函数来抑制这种影响。常见的核函数有Huber、Cauchy等。使用起来非常简单只需要在添加边之后设置一下边的setRobustKernel方法。// 在添加边到优化器之前 #include g2o/core/robust_kernel_impl.h // ... for (int i 0; i N; i) { CurveFittingEdge* edge new CurveFittingEdge(x_data[i]); // ... 设置顶点、观测、信息矩阵 // 添加Huber鲁棒核函数delta是一个阈值参数需要根据问题调整 g2o::RobustKernelHuber* rk new g2o::RobustKernelHuber; rk-setDelta(1.0); // 例如设置delta为1.0 edge-setRobustKernel(rk); optimizer.addEdge(edge); }原理核函数作用于残差的平方项上。当残差很大时Huber核函数会将其增长从二次变为一次从而降低大残差对总目标函数的贡献使优化结果不被少数离群点“带偏”。6.2 自动求导与数值求导手动推导雅可比矩阵对于复杂模型如涉及三角函数、复杂变换容易出错且繁琐。g2o支持两种替代方案数值求导在自定义边中不重写linearizeOplus()函数或者使用g2o::BaseUnaryEdge的默认实现需要设置一个标志。g2o会用有限差分法自动计算雅可比。缺点速度慢精度低不推荐用于生产环境。自动求导g2o本身不直接提供但可以结合Ceres的自动微分或者使用Eigen的AutoDiff模块自己实现。思路是在computeError()函数中使用Eigen::AutoDiffScalar类型的变量来计算残差这个类型可以同时计算出导数值。这需要更深入的模板编程知识是性能与开发效率的折中方案。建议对于自己能轻松推导雅可比的简单模型如多项式手动实现。对于复杂模型优先考虑使用Ceres Solver它对自动微分和数值求导的支持更友好、更成熟。g2o的优势在于其对SLAM等特定问题的高度定制化和图结构的直观性。6.3 信息矩阵与协方差在edge-setInformation()中我们设置了一个“信息矩阵”。信息矩阵是协方差矩阵的逆。它代表了我们对这个观测的“信任程度”。如果某个观测非常精确噪声小其协方差小信息矩阵的值就大在优化中权重就大。如果某个观测噪声大信息矩阵的值就小权重就小。在我们的例子中假设所有观测独立且噪声方差相同w_sigma^2那么信息矩阵就是(1 / w_sigma^2) * I。这是一个非常重要的概念在多传感器融合等场景中不同传感器精度不同就是通过设置不同的信息矩阵来体现的。6.4 求解器选择与性能我们使用了LinearSolverDense和OptimizationAlgorithmLevenberg。对于不同规模的问题选择有讲究问题规模推荐线性求解器特点参数少 (100)LinearSolverDense简单直接利用Eigen的稠密矩阵运算对于小问题最快。参数多矩阵稀疏LinearSolverCholmod(需SuiteSparse)利用系数矩阵的稀疏性内存占用和计算速度有巨大优势。SLAM问题通常用这个。参数多带舒尔补LinearSolverPCG(预条件共轭梯度法)用于超大规模问题是迭代法不需要显式存储矩阵。算法选择高斯牛顿 (GN)收敛快但要求初始值好在残差大或非凸区域容易不稳定。列文伯格-马夸尔特 (LM)在GN基础上增加了阻尼因子更鲁棒能处理较差的初始值是默认推荐的选择。在我们的拟合例子中参数只有3个数据点100个属于极小规模问题稠密求解器是最佳选择。7. 常见问题排查与调试技巧在实际运行中你可能会遇到各种问题。这里记录几个典型的坑和解决方法。问题1编译错误找不到g2o/config.h或类似头文件。原因g2o安装不完整或者CMake找不到g2o的配置。解决确保sudo make install成功执行。在项目的CMakeLists.txt中在find_package(g2o)前可以尝试手动指定其路径set(g2o_DIR /usr/local/lib/cmake/g2o)。检查/usr/local/include/g2o目录是否存在。问题2运行时崩溃段错误 (Segmentation Fault)。原因最常见的原因是内存对齐问题。Eigen库的对象对内存对齐有严格要求特别是在使用固定大小的向量/矩阵时如Eigen::Vector3d。解决确保在自定义的顶点和边类中使用了EIGEN_MAKE_ALIGNED_OPERATOR_NEW宏。确保使用new关键字在堆上创建顶点和边对象而不是在栈上。如果类中有Eigen固定大小成员变量可能需要重写operator new或使用std::aligned_alloc但EIGEN_MAKE_ALIGNED_OPERATOR_NEW宏通常已处理。问题3优化不收敛chi2一直很大或变成NaN。原因初始值太差非线性优化严重依赖初始值。尝试给一个更合理的初始估计例如用最小二乘解析解或RANSAC粗估计一个。雅可比矩阵计算错误这是手动求导最容易出问题的地方。仔细检查linearizeOplus()中的导数公式。可以用数值求导临时替换对比结果。信息矩阵设置错误如果信息矩阵设置得极大或极小会导致数值不稳定。数据有严重离群点考虑使用鲁棒核函数。调试打开优化器的详细输出optimizer.setVerbose(true)观察每次迭代的chi2变化。在第一次迭代前手动调用optimizer.computeActiveErrors()并打印初始chi2看是否合理。简化问题先用2-3个无噪声的完美数据点测试看优化能否收敛到精确解。问题4优化结果明显不对但程序没报错。原因可能是局部最优。最小二乘问题对于非线性模型可能是非凸的存在多个局部极小值。解决尝试不同的初始值观察结果是否一致。如果可能尝试更简单的模型或者增加数据量。考虑使用全局优化算法如模拟退火、遗传算法先得到一个粗解再用g2o进行局部精优化。不过这在g2o中不是直接支持的。一个实用的调试流程单元测试顶点和边单独实例化一个顶点和一条边手动设置参数和观测调用computeError()和linearizeOplus()检查残差和雅可比计算是否正确。小数据测试用3-5个手工构造的、无噪声的精确数据点测试。优化结果应该能完美恢复参数。关闭核函数在排除离群点问题影响下调试核心逻辑。对比验证对于线性模型如多项式用g2o优化的结果应该与Eigen直接求解正规方程(X^T * X) * theta X^T * y的结果在数值上非常接近。这是一个很好的验证基准。通过这个完整的项目你不仅实现了一个曲线拟合工具更重要的是理解了如何将一个传统的数值计算问题用现代优化框架的思维进行建模和求解。这套“定义顶点-定义边-构建图-优化”的流程是解决许多更复杂估计问题的通用蓝图。下次当你遇到需要优化一堆参数来最小化某种误差的问题时不妨想想能不能用g2o的图模型把它优雅地表示出来。