C++轻量级非线性最小二乘库least-squares-cpp：从原理到SLAM实战

1. 项目概述与核心价值

最近在折腾一个机器人定位相关的项目，需要处理一堆带噪声的传感器数据，核心任务就是通过非线性优化来估计机器人的位姿。这玩意儿说白了，就是给你一堆观测方程，让你找到一个最优的参数（比如位置、姿态），使得预测值和实际观测值之间的误差平方和最小。这不就是经典的非线性最小二乘问题嘛。一开始我图省事，直接上Eigen的Levenberg-Marquardt（LM）模块，但很快就遇到了瓶颈：问题规模稍大，迭代速度就慢得感人；想自定义损失函数或者添加一些特殊的参数约束（比如流形上的优化），更是得自己从头造轮子，调试起来极其痛苦。

就在我准备硬着头皮去啃Ceres Solver或者g2o（这俩确实是工业级重器）那略显庞大的代码和依赖时，偶然在GitHub上发现了这个叫least-squares-cpp的库。名字起得直白，就是“C++最小二乘”。抱着试试看的心态clone下来，编译、跑例程，一套流程下来，给我的第一印象是：轻量、清晰、够用。它没有Ceres那么庞大的生态系统和复杂的配置，但恰恰把非线性最小二乘最核心的LM算法实现得干净利落，接口设计得也非常C++，用起来有种“刚刚好”的感觉。最关键的是，它完全免费开源，对于很多学术研究、课程作业或者中小型项目来说，避免了引入重型依赖的负担，是一个性价比极高的选择。

这个库的核心价值，我认为在于它精准地填补了一个市场空白：对于那些需要比纯手写LM算法更稳健、比Eigen内置模块功能更灵活，但又远未达到需要动用Ceres/g2o这种“核武器”级别的项目，least-squares-cpp提供了一个优雅、高效的折中方案。它让你能快速上手解决实际问题，同时代码结构清晰到足以让你理解优化过程的每一个细节，这对于学习和调试来说是无价之宝。

2. 核心设计思路与方案选型

2.1 为什么选择Levenberg-Marquardt算法？

非线性最小二乘问题的求解，本质上是一个迭代寻优的过程。库的作者选择了Levenberg-Marquardt（LM）算法作为核心，这是一个非常经典且明智的选择。我们可以简单理解一下它的思路：

想象你在一个复杂的地形（即损失函数曲面）上寻找最低点（最优解）。最速下降法就像蒙着眼，只根据脚下最陡的方向一小步一小步往下走，在接近谷底时效率很低。高斯-牛顿法则像有了一个局部地图（基于当前点的二阶近似），试图预测最低点并一大步跳过去，但在地形很“弯曲”或初始点不好时容易跳飞。

LM算法巧妙地将两者结合。它引入了一个“阻尼因子”λ。当λ很大时，算法行为接近最速下降法，步伐小但稳健，保证能下山；当λ很小时，算法行为接近高斯-牛顿法，步伐大且高效，能快速收敛。在每次迭代中，算法会根据本次迭代的效果动态调整λ：如果误差下降了，就减小λ，更信任高斯-牛顿的预测，加速收敛；如果误差上升了，就增大λ，回归稳健的最速下降模式，重新寻找方向。

least-squares-cpp库实现了这个自适应的LM过程。它的设计没有追求支持所有种类的优化问题（比如带复杂边界约束的），而是聚焦于解决无约束或仅带简单参数上下界约束的非线性最小二乘问题，这使得其内部结构可以非常精简高效。

2.2 库的架构与接口哲学

浏览库的源代码，你会发现它的架构非常清晰，主要包含以下几个核心组件：

1. Problem类：这是用户交互的主要入口。你通过它来定义优化问题：添加参数块（要优化的变量）、添加残差块（误差项）。它负责管理所有数据和计算图。
2. LossFunction类（可选）：用于定义鲁棒核函数，例如Huber损失、Cauchy损失。这是处理数据中异常值（Outliers）的关键。库内置了几种常见的核函数。
3. Solver类及其Options：这是求解器核心。Options结构体让你可以精细控制LM算法的行为，比如最大迭代次数、函数/梯度容忍度、初始阻尼因子λ等。Solver类则根据配置执行优化流程。
4. LocalParameterization接口（可选但重要）：这是库的一个亮点。它允许你定义参数的局部参数化方式。最常见的应用场景就是在优化旋转（如四元数、旋转矩阵）时。旋转本身存在于非欧几里得空间（流形）上，简单的加减法更新会破坏其约束（如四元数需要保持模长为1）。通过实现LocalParameterization，你可以告诉求解器如何在流形的切空间（一个欧氏空间）中进行增量更新，然后再映射回流形本身。这保证了优化过程中旋转参数始终合法。

这种接口设计体现了“约定优于配置”和“显式优于隐式”的C++哲学。你需要什么功能，就实现对应的接口；库只提供算法框架，不做过多的魔法。这带来的好处是极高的透明度和可控性，你清楚地知道每一步计算是如何发生的，出了bug也容易定位。

注意：虽然Ceres Solver也提供类似甚至更丰富的接口，但least-squares-cpp的实现更加轻量化，没有Ceres中自动求导、多线程优化等高级（同时也更复杂）的特性。这既是它的局限，也是其简洁性的来源。

3. 从零开始：环境配置与第一个例子

理论说再多不如跑个例子。我们来看看如何快速把这个库用起来。

3.1 获取与编译

库的源码通常托管在GitHub上。获取和编译它非常直接，因为它几乎没有外部依赖（除了标准的C++11编译器和Eigen3）。

# 1. 克隆仓库 git clone https://github.com/YourUserName/least-squares-cpp.git cd least-squares-cpp # 2. 创建构建目录并编译 mkdir build && cd build cmake .. make -j4

这里的关键是确保你的系统已安装Eigen3。在Ubuntu上可以通过apt-get install libeigen3-dev安装。CMake脚本会自动查找Eigen。编译完成后，你会得到静态库文件（如libleast_squares.a）和一系列示例程序。

3.2 一个简单的曲线拟合示例

假设我们有一组观测数据(x_i, y_i)，我们想用模型y = a * exp(b * x) + c来拟合它们。这里[a, b, c]就是我们要优化的参数。

#include <iostream> #include <vector> #include <least_squares/least_squares.h> // 假设头文件路径已包含 // 1. 定义残差计算器 class ExponentialResidual { public: ExponentialResidual(double x, double y) : x_(x), y_(y) {} // 这个运算符重载是核心：计算单个数据点的残差 template <typename T> bool operator()(const T* const abc, T* residual) const { // abc 是指向参数数组的指针，这里 abc[0]=a, abc[1]=b, abc[2]=c T predicted_y = abc[0] * exp(abc[1] * T(x_)) + abc[2]; residual[0] = predicted_y - T(y_); // 残差 = 预测值 - 观测值 return true; // 始终返回true，除非计算失败 } private: const double x_; const double y_; }; int main() { // 2. 伪造一些观测数据 (实际中从文件或传感器读取) std::vector<double> x_data = {0.0, 1.0, 2.0, 3.0, 4.0}; std::vector<double> y_data = {1.0, 1.5, 2.5, 3.5, 5.0}; // 大致符合 y ~ 1.0*exp(0.3*x) + 0.5 // 3. 构建优化问题 least_squares::Problem problem; // 4. 初始化参数块。我们估计 a, b, c 的初始值。 double abc[3] = {0.5, 0.1, 0.0}; // 初始猜测，可以离真值较远 // 5. 添加残差块（即观测项） for (int i = 0; i < x_data.size(); ++i) { // 每个数据点对应一个残差块 // CostFunction* 这里由库内部通过模板自动创建 problem.AddResidualBlock( new least_squares::AutoDiffCostFunction<ExponentialResidual, 1, 3>( new ExponentialResidual(x_data[i], y_data[i])), nullptr, // 不使用鲁棒核函数 abc // 指向要优化的参数块 ); } // 6. 配置求解器选项 least_squares::Solver::Options options; options.max_num_iterations = 100; // 最大迭代次数 options.function_tolerance = 1e-6; // 函数值变化容忍度 options.gradient_tolerance = 1e-10; // 梯度容忍度 options.parameter_tolerance = 1e-8; // 参数变化容忍度 options.minimizer_progress_to_stdout = true; // 将迭代信息输出到控制台 // 7. 运行优化 least_squares::Solver::Summary summary; Solve(options, &problem, &summary); // 8. 输出结果 std::cout << summary.BriefReport() << "\n"; std::cout << "优化后的参数 a, b, c: \n"; std::cout << abc[0] << ", " << abc[1] << ", " << abc[2] << std::endl; return 0; }

编译并运行这个程序，你会在终端看到LM算法的迭代过程，最终输出优化后的参数值。通过对比初始猜测和最终结果，你能直观感受到LM算法是如何将参数从“盲猜”调整到“最佳拟合”状态的。

实操心得：初始值的选择对非线性优化至关重要。虽然LM算法对初始值有一定鲁棒性，但一个糟糕的初始值仍可能导致收敛到局部最优或迭代缓慢。对于指数拟合，参数b（增长率）的初始值符号如果搞反了，优化可能完全失败。在实践中，往往需要根据问题背景给一个合理的初始估计。

4. 核心功能深度解析与高级用法

4.1 鲁棒核函数：让优化对抗异常值

现实中的数据总是不完美的，常常混入一些“离谱”的观测值，即异常值。如果使用标准的平方损失（L2范数），这些异常值会因为残差很大而对总损失产生巨大影响，从而把优化结果“拉偏”。

least-squares-cpp提供了LossFunction接口来解决这个问题。其思想是，对大的残差进行“压制”，降低其对整体损失的影响。例如，Huber损失函数：

// 在添加残差块时使用鲁棒核函数 problem.AddResidualBlock( new AutoDiffCostFunction<MyCostFunctor, 1, 3>(new MyCostFunctor(...)), new HuberLoss(1.0), // delta 参数设为 1.0 parameters );

Huber损失的行为是：当残差绝对值小于阈值delta时，使用平方损失（保持二阶收敛性）；当大于delta时，使用线性损失（降低异常值影响）。库内置的损失函数通常包括：

• TrivialLoss: 标准平方损失。
• HuberLoss: Huber损失。
• CauchyLoss: 柯西损失，对异常值压制更强烈。
• SoftLOneLoss: 另一种常用的鲁棒损失。

选择哪种损失函数以及设置其参数（如Huber的delta）需要根据具体问题的噪声特性来调整。一个实用的技巧是，先不用核函数跑一次优化，观察残差的分布，再根据分布情况选择合适的核函数和参数。

4.2 局部参数化：处理流形上的优化

这是least-squares-cpp库中一个非常强大且必要的特性，尤其在SLAM、三维重建等领域。很多参数并不生活在普通的欧氏空间中。

典型场景：优化旋转（四元数）一个单位四元数q = [w, x, y, z]需要满足模长约束w^2 + x^2 + y^2 + z^2 = 1。如果你直接把它当作一个4维向量用q = q + delta来更新，新的q很可能不再是单位四元数，破坏了物理意义。

解决方案是使用LocalParameterization。你需要告诉求解器两件事：

1. 全局参数大小：例如，四元数是4维的。
2. 局部参数大小：在其流形（切空间）中，增量只需要3维（因为单位四元数空间是3维流形）。
3. 如何将局部增量delta（3维）加到全局参数x（4维）上，并保持流形约束。
4. 如何计算全局参数之间的差值在局部空间的表示（可选，用于某些情况）。

库可能已经内置了EigenQuaternionParameterization。使用方式如下：

// 假设 parameters 中有一个四元数参数块，起始地址是 quat double quat[4] = {1, 0, 0, 0}; // 单位四元数 problem.AddParameterBlock(quat, 4); problem.SetParameterization(quat, new EigenQuaternionParameterization); // 现在，求解器会在其内部使用3维的局部增量来更新这个4维的四元数，始终保持其单位性。

除了旋转，其他常见的流形还有二维圆环（角度，局部参数1维）、三维球面等。实现自己的LocalParameterization是使用该库进行高级应用的关键一步。

踩坑记录：我曾经在优化相机位姿（包含旋转和平移）时，忘记给旋转部分设置LocalParameterization。结果优化过程极不稳定，经常发散，或者收敛到一个物理上无意义的解。加上四元数的局部参数化后，问题立刻变得稳定且快速收敛。这是一个必须检查的项。

4.3 求解器选项调优：加速收敛与确保稳定

Solver::Options提供了丰富的控制参数。理解它们对高效使用求解器很重要：

一个实用的调试流程：

1. 首次运行时，打开minimizer_progress_to_stdout，观察输出。
2. 如果成本函数值持续快速下降直至收敛，恭喜你，问题配置良好。
3. 如果成本在最初几次迭代后就不再下降，可能是function_tolerance或parameter_tolerance设得太松，或者梯度已经很小（接近最优），可以检查最终梯度范数。
4. 如果成本下降非常缓慢，或者阻尼因子λ变得非常大，说明算法在艰难地“下山”，可能是初始值太差，或者问题本身ill-conditioned（雅可比矩阵病态）。这时需要重新审视初始值，或者考虑对参数进行缩放（使其量级接近1）。

5. 实战：一个完整的SLAM前端位姿优化实例

让我们结合一个更贴近实际的例子：在视觉SLAM中，通过匹配到的3D-2D点对（PnP问题）来优化相机位姿。假设我们已经通过特征匹配得到了n个世界坐标系下的3D点P_i和它们在当前相机图像上的2D投影p_i，相机内参K已知。我们需要优化相机的旋转R（用四元数q表示）和平移t。

5.1 定义重投影误差残差

这是最核心的残差计算器。对于每一对3D-2D匹配点，计算其重投影误差。

class PnPCostFunctor { public: PnPCostFunctor(const Eigen::Vector3d& P_3d, const Eigen::Vector2d& p_2d, const Eigen::Matrix3d& K) : P_3d_(P_3d), p_2d_(p_2d), K_(K) {} template <typename T> bool operator()(const T* const q_vec, const T* const t_vec, T* residual) const { // q_vec: 四元数 [w, x, y, z] // t_vec: 平移向量 [tx, ty, tz] Eigen::Quaternion<T> q(q_vec[0], q_vec[1], q_vec[2], q_vec[3]); Eigen::Matrix<T, 3, 1> t(t_vec[0], t_vec[1], t_vec[2]); // 将世界点转换到相机坐标系 Eigen::Matrix<T, 3, 1> P_cam = q * P_3d_.cast<T>() + t; // 投影到归一化平面 T xp = P_cam[0] / P_cam[2]; T yp = P_cam[1] / P_cam[2]; // 应用内参，得到像素坐标预测值 T u_pred = K_(0,0) * xp + K_(0,2); T v_pred = K_(1,1) * yp + K_(1,2); // 计算残差：预测像素坐标 - 观测像素坐标 residual[0] = u_pred - T(p_2d_.x()); residual[1] = v_pred - T(p_2d_.y()); return true; } static ceres::CostFunction* Create(const Eigen::Vector3d& P_3d, const Eigen::Vector2d& p_2d, const Eigen::Matrix3d& K) { // 残差维度2，第一个参数块（旋转四元数）大小4，第二个参数块（平移）大小3 return new ceres::AutoDiffCostFunction<PnPCostFunctor, 2, 4, 3>( new PnPCostFunctor(P_3d, p_2d, K)); } private: const Eigen::Vector3d P_3d_; const Eigen::Vector2d p_2d_; const Eigen::Matrix3d K_; };

5.2 构建并求解问题

void OptimizeCameraPose(const std::vector<Eigen::Vector3d>& points_3d, const std::vector<Eigen::Vector2d>& points_2d, const Eigen::Matrix3d& K, Eigen::Quaterniond& q_est, // 输入初始估计，输出优化结果 Eigen::Vector3d& t_est) { least_squares::Problem problem; // 准备参数块 double q_coeffs[4] = {q_est.w(), q_est.x(), q_est.y(), q_est.z()}; double t_vec[3] = {t_est.x(), t_est.y(), t_est.z()}; problem.AddParameterBlock(q_coeffs, 4); problem.AddParameterBlock(t_vec, 3); // **关键步骤**：为四元数参数块设置局部参数化 problem.SetParameterization(q_coeffs, new EigenQuaternionParameterization); // 添加所有观测到的点对作为残差块 for (size_t i = 0; i < points_3d.size(); ++i) { ceres::CostFunction* cost_function = PnPCostFunctor::Create(points_3d[i], points_2d[i], K); problem.AddResidualBlock(cost_function, nullptr, // 这里可以先不用鲁棒核 q_coeffs, t_vec); } // 配置求解器 least_squares::Solver::Options options; options.max_num_iterations = 50; options.minimizer_progress_to_stdout = true; // 调试时打开 options.function_tolerance = 1e-6; least_squares::Solver::Summary summary; Solve(options, &problem, &summary); std::cout << summary.BriefReport() << std::endl; // 更新输出位姿 q_est = Eigen::Quaterniond(q_coeffs[0], q_coeffs[1], q_coeffs[2], q_coeffs[3]); t_est = Eigen::Vector3d(t_vec[0], t_vec[1], t_vec[2]); }

5.3 引入鲁棒核函数处理误匹配

在实际SLAM中，特征匹配必然存在误匹配（Outliers）。我们需要使用鲁棒核函数来抑制它们的影响。修改添加残差块的部分：

// 在循环内部添加残差块时 ceres::CostFunction* cost_function = ...; // 使用Huber损失，delta值需要根据像素误差的分布来设定，例如1.0个像素（与重投影误差尺度相关） problem.AddResidualBlock(cost_function, new HuberLoss(1.0), // 使用Huber核函数 q_coeffs, t_vec);

通过对比使用和不使用Huber损失的结果，你会发现优化后的位姿对误匹配的鲁棒性显著增强。

6. 性能对比、常见问题与排查技巧

6.1 与Eigen和Ceres的简单对比

为了让你对least-squares-cpp的定位有更直观的认识，这里做一个非常粗略的定性对比：

选择建议：

• 如果你在做课程作业、算法原型验证，或者你的问题规模不大但需要比Eigen更友好的接口和流形支持，least-squares-cpp是绝佳选择。
• 如果你在构建一个大型的、生产级的SLAM或三维重建系统，需要处理成千上万个参数和观测，并且对速度有极致要求，那么Ceres Solver或g2o是更稳妥的选择。
• 如果你只是快速验证一个数学想法，且问题维度很低，Eigen内置的LM可能就足够了。

6.2 常见问题排查表

在实际使用中，你可能会遇到以下问题。这里提供一个快速排查指南：

6.3 调试技巧与心得

1. 从小开始，逐步验证：不要一开始就用成百上千个残差块。先用一个或几个简单的残差块构建一个最小可工作示例（MWE），确保残差计算、参数更新逻辑正确。
2. 善用输出信息：将options.minimizer_progress_to_stdout设为true。观察每次迭代的成本下降情况、阻尼因子λ的变化以及最终收敛报告。这是诊断问题最直接的窗口。
3. 验证雅可比矩阵：如果你手动提供了雅可比矩阵（该库主要用自动微分，但接口支持手动），或者怀疑自动微分有问题，可以实现一个数值差分（有限差分）的版本进行对比。在残差函数附近用微小的参数扰动来计算残差变化，与自动微分的结果对比。
4. 可视化中间结果：对于像曲线拟合、位姿优化这类问题，将每次迭代后的结果（如拟合曲线、相机位姿）实时画出来，能非常直观地看到优化过程是否在向正确的方向前进。
5. 理解“黑盒”：虽然least-squares-cpp封装了LM算法，但不要把它当成完全的黑盒。花点时间阅读其核心源码（尤其是solver.cc和levenberg_marquardt_strategy.cc），理解信任域策略、阻尼因子更新规则，这能让你在调参时更有把握。

least-squares-cpp这个库给我的感觉，就像一个设计精良的瑞士军刀。它没有重型机床（Ceres）那么强大，但比随手捡的石片（手写LM）要锋利和顺手得多。对于大多数非极端性能要求的非线性优化场景，它提供了恰到好处的抽象和性能。其清晰的代码结构和接口设计，也让它成为一个学习非线性优化原理的绝佳范本。如果你正在寻找一个轻量、免费且功能足够的C++非线性最小二乘库，它绝对值得你花一个下午的时间尝试一下。