最小二乘问题总结 | 定位与建图中的数学与优化问题 | SLAM必修课 | 最小二乘问题的分类、公式推导与 C++17/Eigen 实现:从线性拟合到 SLAM 点到平面优化
摘要:最小二乘不仅是一个求解公式,更是一套将观测、噪声、先验和约束组织成优化问题的方法。本文从普通线性最小二乘出发,讨论超定、欠定与秩亏,加权与广义最小二乘,非线性最小二乘中的高斯—牛顿与 LM,鲁棒估计、正则化、等式约束、整体最小二乘与递归最小二乘,最后以 SLAM 点到平面位姿优化串联残差建模、雅可比推导和可观测性分析。各部分提供 C++17/Eigen 实现,文末附完整工程源码及确定性数值测试结果。
关键词:最小二乘;Eigen;高斯—牛顿;Levenberg–Marquardt;Huber;白化;点云配准;SLAM。

1. 先建立分类框架:问题类型不等于求解算法
学习最小二乘时,很容易把“线性最小二乘、加权最小二乘、SVD、高斯—牛顿、LM”并列记忆。但这些概念回答的不是同一个问题。
建模层面关心残差如何定义、各观测是否同等可信、有没有异常值和先验;解的性质关心方程是否独立、参数能否唯一确定;求解层面才讨论 QR、SVD、高斯—牛顿和 LM。
| 分类维度 | 常见情况 | 需要回答的问题 |
|---|---|---|
| 残差对参数的关系 | 线性、非线性 | 能否直接组织成固定的矩阵方程? |
| 噪声模型 | 普通、加权、广义 | 观测精度是否不同?误差是否相关? |
| 异常值处理 | 平方损失、鲁棒损失 | 大残差是否应该持续受到平方惩罚? |
| 额外信息 | 无先验、正则化、带约束 | 参数是否有先验,或必须满足条件? |
| 输入数据误差 | 普通、整体最小二乘 | 只有观测值有误差,还是输入坐标也有误差? |
| 数据到达方式 | 批量、递归 | 数据一次给全,还是逐条到来? |
| 解的性质 | 超定、欠定、满秩、秩亏 | 独立约束是否足够,解是否唯一? |
| 数值求解 | QR、SVD、Cholesky、GN、LM | 如何求解线性子问题,如何组织非线性迭代? |
因此,一个 SLAM 问题完全可以同时是“加权、鲁棒、带先验、定义在流形上的非线性最小二乘”。Ceres 的接口也将残差块、损失函数、参数块和流形分别建模,而不是把它们视作互斥选项。
2. 统一符号与基本目标
考虑观测模型:
其中, 是已知输入,
是观测值,
是待估计参数。全文统一使用“预测减观测”的残差约定:
将 个标量残差堆叠为向量
,普通最小二乘为:
系数 1/2 只是让求导更简洁,不改变最优解。主要符号如下。
| 符号 | 含义 |
|---|---|
| 线性模型的设计矩阵 | |
| 线性模型中的观测向量 | |
| 残差对参数或局部增量的雅可比 | |
| 观测噪声协方差 | |
| 正定协方差对应的信息矩阵 | |
| 第 i 个标量观测的标准差,不是方差 | |
| 当前迭代的参数增量 |
若误差独立、零均值、服从固定方差的高斯分布,负对数似然中与参数相关的部分为:
因此,固定高斯噪声模型下的最大似然估计可以写成最小二乘;方差不同时得到加权形式。这里“固定”很重要:若协方差也随待估计参数变化,通常不能忽略似然中的对数行列式等项。[5]
2.1 代码组织约定
正文中的函数共用以下头文件、类型别名和检查函数;调用示例写在 main() 中。非线性迭代和位姿优化部分先展示核心步骤,完整的迭代控制、输入检查和数值测试在文末源码附录中。
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <algorithm>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <stdexcept>
#include <string>
#include <vector>
using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::Matrix3d;
using Eigen::Vector3d;
using Eigen::Vector2d;
using Vector6d = Eigen::Matrix<double, 6, 1>;
void require(bool ok, const std::string& message) {
if (!ok) throw std::runtime_error(message);
}
void checkAB(const MatrixXd& A, const VectorXd& b) {
require(A.rows() > 0 && A.cols() > 0 && A.rows() == b.size(),
"Invalid A/b dimensions");
require(A.allFinite() && b.allFinite(), "A/b contain NaN or Inf");
}
代码中的
x或x_表示待估计参数向量,不要与数学表达式中的已知采样输入 x_i 混淆。
3. 普通线性最小二乘:从直线拟合开始
3.1 “线性”指的是对未知参数线性
对于直线模型:
未知参数 以线性方式出现,因此属于线性最小二乘。
对于二次多项式:
虽然图像是曲线,但 和
都是已知数,模型对 a,b,c 仍然是线性的。
判断标准是未知参数如何进入模型,而不是拟合曲线的外形。
3.2 矩阵形式与正规方程
以直线拟合为例:
最小化:
梯度与 Hessian 为:
令梯度为零得到正规方程:
若 A 列满秩,则 正定,目标严格凸,解唯一;若列秩亏,最小二乘目标仍是凸的,但参数一般不能唯一确定。
3.3 C++:使用列主元 QR
VectorXd solveLS(const MatrixXd& A, const VectorXd& b) {
checkAB(A, b);
Eigen::ColPivHouseholderQR<MatrixXd> qr(A);
require(qr.rank() == A.cols(), "Rank-deficient A; use COD/SVD or add a justified prior");
VectorXd x = qr.solve(b);
require(x.allFinite(), "Non-finite LS solution");
return x;
}
调用示例:
MatrixXd A(5, 2);
VectorXd b(5);
A << 0, 1,
1, 1,
2, 1,
3, 1,
4, 1;
b << 1.1, 2.9, 5.2, 6.8, 9.1;
VectorXd theta = solveLS(A, b);
std::cout << theta.transpose() << '\n';
// a = 1.99, b = 1.04
3.4 为什么不直接计算逆矩阵?
满列秩时,可以在纸上写:
但这不意味着代码应该先构造逆矩阵。工程中应当使用分解来求解线性系统。对于二范数条件数,满列秩矩阵满足:
因此,形成正规方程可能明显放大数值敏感性。Eigen 官方文档提供 QR、SVD 和正规方程三种路线,并明确提醒正规方程的条件数问题。
// QR:一般的小型稠密最小二乘。
VectorXd x_qr = A.colPivHouseholderQr().solve(b);
// SVD:分析奇异值、秩和最小范数解。
Eigen::JacobiSVD<MatrixXd> svd(
A, Eigen::ComputeThinU | Eigen::ComputeThinV);
VectorXd x_svd = svd.solve(b);
// 正规方程:应先确认秩、条件数和问题规模适合。
VectorXd x_normal =
(A.transpose() * A).ldlt().solve(A.transpose() * b);
这不是“QR 永远比 Cholesky 好”的结论,而是速度、稳定性、稀疏性和规模之间的选择。
4. 超定、欠定与秩亏:方程很多,为什么仍然解不出来?
设 ,m 为方程数,n 为参数数。
| 情况 | 条件 | 对无约束线性最小二乘的影响 |
|---|---|---|
| 超定 | m>n | 可能不一致,也可能精确一致;是否唯一还要看列秩 |
| 方阵系统 | m=n | 满秩时唯一,秩亏时不能唯一确定全部参数 |
| 欠定 | m<n | 不可能列满秩,因此最小二乘参数不唯一 |
| 列秩亏 | 存在非零参数方向不改变预测值 |
4.1 一个欠定例子
只有一个方程:
(0,2)、(1,1) 和 (2,0) 都是精确解。
若规定在所有最小二乘解中选择欧氏范数最小者:
得到:
Eigen 的完整正交分解 COD 支持直接求最小范数最小二乘解。
VectorXd solveMinimumNormLS(const MatrixXd& A, const VectorXd& b) {
checkAB(A, b);
Eigen::CompleteOrthogonalDecomposition<MatrixXd> cod(A);
VectorXd x = cod.solve(b);
require(x.allFinite(), "Non-finite minimum-norm solution");
return x;
}
// main() 中:
MatrixXd A(1, 2);
VectorXd b(1);
A << 1, 1;
b << 2;
std::cout << solveMinimumNormLS(A, b).transpose() << '\n';
// 1 1
4.2 超定也可能秩亏
下面有三个方程、两个参数:
但三行只提供一个独立约束,矩阵秩仍为 1。
若 ,则对任意
:
因此,数据无法区分沿 方向变化的参数。SVD 或 COD 能选择一个解,却不能凭空补上观测信息。
在实际计算中,还要区分严格秩亏与近似秩亏:非常小但非零的奇异值意味着参数对噪声高度敏感。矩阵的数值秩阈值也不等于应用中的可观测性阈值;单位、尺度和噪声模型都必须考虑。
5. 加权与广义最小二乘:先把误差放到可比较的尺度上
5.1 加权最小二乘
若第 条标量观测的标准差为
:
对线性残差:
更方便的实现是白化残差:
VectorXd solveWeightedLS(const MatrixXd& A, const VectorXd& b,
const VectorXd& sigma) {
checkAB(A, b);
require(sigma.size() == b.size() && sigma.allFinite() &&
(sigma.array() > 0.0).all(), "sigma must be finite and positive");
MatrixXd Aw = A;
VectorXd bw = b;
for (Eigen::Index i = 0; i < A.rows(); ++i) {
Aw.row(i) /= sigma(i);
bw(i) /= sigma(i);
}
return solveLS(Aw, bw);
}
损失中的权重是 ,但残差与雅可比应乘
。 不能给残差再乘一次完整权重,否则平方后会使用错误的
。
在直线数据中,把最后一项改成 20,并事先声明它的标准差为 10,其余为 1:
VectorXd noisy_b = b;
noisy_b(4) = 20.0;
VectorXd sigma = VectorXd::Ones(5);
sigma(4) = 10.0;
VectorXd ordinary = solveLS(A, noisy_b);
VectorXd weighted = solveWeightedLS(A, noisy_b, sigma);
结果:
普通最小二乘:a = 4.17000000,b = -1.14000000
加权最小二乘:a = 1.99492611,b = 1.03507389
这里的改善来自正确的先验精度信息,不是加权最小二乘自动发现了异常点。
5.2 广义最小二乘:处理相关误差
当不同观测的误差相关,使用完整协方差:
若 对称正定,并有:
则:
因此通过三角求解进行白化:
VectorXd solveGeneralizedLS(const MatrixXd& A, const VectorXd& b,
const MatrixXd& Sigma) {
checkAB(A, b);
require(Sigma.rows() == b.size() && Sigma.cols() == b.size() &&
Sigma.allFinite() && Sigma.isApprox(Sigma.transpose(), 1e-12),
"Invalid covariance dimensions/symmetry");
Eigen::LLT<MatrixXd> llt(Sigma);
require(llt.info() == Eigen::Success, "Covariance must be positive definite");
MatrixXd Aw = llt.matrixL().solve(A);
VectorXd bw = llt.matrixL().solve(b);
return solveLS(Aw, bw);
}
这里必须使用 ,不是随意改成
。等价地,若直接从信息矩阵构造白化因子
,应满足
。
Ceres 不会替用户推断各观测的噪声单位;残差的协方差不是单位阵时,需要在建模时正确缩放。[5]
6. 非线性最小二乘:反复求解局部线性问题
6.1 指数模型与雅可比
考虑:
参数出现在指数函数内部,问题是非线性的。雅可比为:
void evaluateExp(const VectorXd& xs, const VectorXd& ys, const Vector2d& theta,
VectorXd& r, MatrixXd& J) {
r.resize(xs.size()); J.resize(xs.size(), 2);
for (Eigen::Index i = 0; i < xs.size(); ++i) {
const double prediction = std::exp(theta(0) * xs(i) + theta(1));
r(i) = prediction - ys(i);
J(i, 0) = xs(i) * prediction;
J(i, 1) = prediction;
}
}
一个容易忽略的问题是:对正观测取对数后,虽然 可线性求解,但优化的是对数域残差,不再是原来的
。变量变换可能有助于初始化,却不能在不改变噪声假设和目标函数的情况下宣称两者等价。
6.2 高斯—牛顿推导
在当前估计处做一阶展开:
于是求解局部问题:
得到:
更新参数:
是完整步长;为避免线性化不准确导致目标上升,可以通过回溯线搜索缩小
。完整工程使用 Armijo 条件,而不是不检查就接受完整步长。
// 当前 r、J 已由 evaluateExp() 计算。
Vector2d step = solveLS(J, VectorXd(-r));
// 之后进行回溯线搜索,再更新 theta。
6.3 高斯—牛顿与牛顿法的区别
对于 :
牛顿法使用完整 Hessian;高斯—牛顿忽略第二项,以 近似 Hessian。当残差较小、且残差的二阶导没有异常增大时,或残差局部接近线性时,这一近似更合理。
6.4 LM:用阻尼控制局部更新
令:
LM 的一种常见形式为:
其中 D 是正对角矩阵。本文代码采用 ,这是示例中的数值保护,不是所有应用通用的物理阈值。代码局部变量
lambda 对应这里的阻尼 。
Eigen::Matrix2d H = J.transpose() * J;
Vector2d g = J.transpose() * r;
Vector2d diagonal = H.diagonal().cwiseMax(1e-12);
Eigen::Matrix2d Hd = H;
Hd.diagonal() += lambda * diagonal;
Vector2d step = Hd.ldlt().solve(-g);
当阻尼较小时,它接近高斯—牛顿;当阻尼较大时:
即尺度调整后的负梯度方向。
完整的 LM 不只是往 Hessian 上加一个常数。它还需要评估候选更新是否可信。以原始局部二次模型定义预测下降:
以及下降比:
只有在预测下降有效时才计算此比值。若实际下降与预测一致,可以接受更新并减小阻尼;若实际下降很差甚至目标上升,应拒绝更新并增加阻尼。这是 LM 信赖域思想的核心。[4]
7. 鲁棒最小二乘:不知道哪些观测有问题怎么办?
加权最小二乘利用已知的噪声模型;鲁棒估计则尝试抑制与当前模型明显不一致的观测。两者可以组合使用。
7.1 Huber 损失
先标准化:
Huber 损失为:
小残差保持平方惩罚,大残差转为线性增长。
7.2 IRLS 推导与实现
令 ,冻结当前迭代的权重:
时按连续延拓取
。每轮求解:
所以每一行实际乘上的因子是 :
VectorXd solveHuberIRLS(const MatrixXd& A, const VectorXd& b,
const VectorXd& sigma, double delta = 1.345) {
require(std::isfinite(delta) && delta > 0.0, "Invalid Huber delta");
VectorXd x = solveWeightedLS(A, b, sigma);
for (int iter = 0; iter < 200; ++iter) {
VectorXd u = (A * x - b).cwiseQuotient(sigma);
MatrixXd Aw = A;
VectorXd bw = b;
for (Eigen::Index i = 0; i < A.rows(); ++i) {
const double a = std::abs(u(i));
const double w = (a <= delta) ? 1.0 : delta / a;
const double scale = std::sqrt(w) / sigma(i);
Aw.row(i) *= scale;
bw(i) *= scale;
}
VectorXd next = solveLS(Aw, bw);
if ((next - x).norm() <= 1e-10 * (1.0 + x.norm())) return next;
x = next;
}
throw std::runtime_error("IRLS did not converge within 200 iterations");
}
生成 ,原本令
,再给 x=3 对应的观测增加 20。取
、Huber 阈值
:
普通最小二乘:a = 2.54545455,b = 2.81818182
Huber IRLS: a = 2.03296703,b = 1.10989011
IRLS 的内层求解把本轮权重视作常量,不是在内层继续对权重求导。
7.3 与 Ceres 损失函数接口的对应
Ceres 使用 的约定。对标量标准化残差:
恰好有 。向量残差块通常按整个块的平方范数施加损失,不一定等于逐分量独立使用 Huber。[6]
鲁棒核不能补回缺失的可观测性,也不能保证所有错误匹配都被识别。 对错误对应很多或初值很差的问题,仍需几何验证、对应筛选等机制。
8. 正则化与先验:最小范数解不等于加一个惩罚项
8.1 从先验信息到正则项
考虑:
正规方程:
用增广矩阵可避免额外显式构造正规方程:
VectorXd solveRidge(const MatrixXd& A, const VectorXd& b,
double lambda, const VectorXd& x0) {
checkAB(A, b);
require(std::isfinite(lambda) && lambda > 0.0 &&
x0.size() == A.cols() && x0.allFinite(), "Invalid ridge prior");
const Eigen::Index m = A.rows(), n = A.cols();
MatrixXd Aa(m + n, n);
VectorXd ba(m + n);
Aa.topRows(m) = A;
Aa.bottomRows(n) = std::sqrt(lambda) * MatrixXd::Identity(n, n);
ba.head(m) = b;
ba.tail(n) = std::sqrt(lambda) * x0;
return solveLS(Aa, ba);
}
8.2 对同一个欠定问题比较两种答案
对于 ,最小范数最小二乘解是
。
加入零均值先验并取 :
得到:
原因不是求解器不同,而是目标已经不同:最小范数是在原问题的最优解集合里选择;正则化则允许牺牲数据拟合,换取参数更接近先验。
8.3 真实先验、LM 阻尼和固定规范不能混为一谈
一般高斯先验应写作:
各参数单位和精度不同时,应使用合理的信息矩阵,而不是不加区分地惩罚所有分量。
LM 阻尼主要约束当前增量,随迭代调整;真实先验表达对参数本身的额外知识。SLAM 中固定参考位姿还可能只是选择全局坐标规范,不等同于解决所有场景退化。Ceres 文档将规范不确定性与其他秩亏来源专门区分。[5]
9. 约束最小二乘:参数必须满足条件
9.1 等式约束与 KKT 系统
考虑:
拉格朗日函数为:
对 求导并令其为零:
下面是用于小型问题的教学实现,要求 KKT 系统非奇异:
VectorXd solveEqualityLS(const MatrixXd& A, const VectorXd& b,
const MatrixXd& C, const VectorXd& d) {
checkAB(A, b);
require(C.rows() > 0 && C.cols() == A.cols() && C.rows() == d.size() &&
C.allFinite() && d.allFinite(), "Invalid equality constraints");
const Eigen::Index n = A.cols(), k = C.rows();
MatrixXd K = MatrixXd::Zero(n + k, n + k);
K.topLeftCorner(n, n) = A.transpose() * A;
K.topRightCorner(n, k) = C.transpose();
K.bottomLeftCorner(k, n) = C;
VectorXd rhs(n + k);
rhs.head(n) = A.transpose() * b;
rhs.tail(k) = d;
Eigen::FullPivLU<MatrixXd> lu(K);
require(lu.rank() == n + k, "Singular KKT system; inspect constraints/nullspace");
VectorXd solution = lu.solve(rhs);
require(solution.allFinite() &&
(K * solution - rhs).norm() <= 1e-9 * (1.0 + rhs.norm()),
"KKT solve failed");
return solution.head(n);
}
9.3 不等式与流形约束
线性最小二乘还可能带有 或参数上下界,通常应采用针对约束设计的求解方法;求无约束解后直接裁剪,一般不保证得到原约束问题的最优解。
旋转又是另一类约束:,或四元数满足单位范数。使用与流形相容的局部更新,可以避免将旋转的表示维度误当作实际自由度。
10. 整体最小二乘与正交拟合:点云平面怎么估计?
10.1 普通直线拟合与几何距离的区别
普通的 拟合最小化纵向残差
,将输入
视为已知。
而点云的各坐标都可能有误差。若坐标噪声独立、等方差且各向同性,最小化点到几何对象的正交距离更自然。这是整体最小二乘在几何拟合中的典型特例;任意异方差或各向异性噪声并不自动满足这一假设。
10.2 平面拟合推导
使用隐式平面:
最小化:
对 d 求导得到:
代回并定义:
问题变成:
因此:
PCL 的局部表面法向估计教程采用这一协方差矩阵特征分析思路。[7]
11. 递归最小二乘:观测逐条到达时如何更新?
11.1 从批量目标到递归更新
设线性观测为:
无遗忘因子时,递归方法对应的批量目标为:
其中 是参数先验协方差。
引入遗忘因子 ,先设置:
再计算:
用 Joseph 形式更新协方差:
保留全部历史权重;
则逐渐降低历史信息的影响,适用于跟踪缓慢变化的参数。MathWorks 的官方递归最小二乘资料也区分了这一遗忘机制。[8]
11.3 为什么 RLS 不一定等于无先验批量最小二乘?
对于直线拟合,取 。设置:
对应先验信息 。因此应与
的零均值正则化批量解比较,而不是直接要求它与无先验 OLS 完全相同。
测试得到:
RLS:a = 1.99000895,b = 1.03977414
它与使用同一先验的批量最小二乘一致。有限的“大协方差”仍然是有限先验,不等于严格无先验。
14. 常见误区与排查顺序
| 常见说法或做法 | 更准确的理解 |
|---|---|
| 多项式拟合是非线性最小二乘 | 要看是否对待估计参数线性 |
| 超定问题一定没有精确解 | 超定只描述行列数量,也可能完全一致 |
| 方程数多于参数数,解就一定唯一 | 关键是独立约束,也就是列秩 |
| 使用逆矩阵公式就是最直接的实现 | 应优先求解线性系统,避免不必要的显式求逆 |
| 加权时给残差乘 1/\sigma^2 | 残差应乘 1/\sigma |
| Huber 权重能代替传感器协方差 | 两者来源不同,通常先白化再进行鲁棒化 |
| SVD 或 LM 解决了不可观测性 | 它们能选择解或稳定计算,但不会补充观测信息 |
| 最小范数与有限正则化是同一问题 | 前者选择原最优解,后者改变目标 |
| RLS 与 OLS 应当无条件完全一致 | 必须比较相同先验、权重和遗忘机制 |
| 点到平面雅可比可以直接照搬 | 必须匹配坐标系、残差符号、增量顺序和扰动约定 |
15. 总结
最小二乘的共同出发点,是寻找能够解释观测的参数;不同变体则处理观测质量、异常值、参数先验、几何约束和数据组织方式的差异。
普通线性最小二乘通常直接求解;非线性最小二乘通过局部线性化反复求解。加权与白化负责尺度和噪声,鲁棒损失控制异常残差,先验与约束引入额外知识,而秩与零空间决定哪些方向真正被数据约束。
对于 SLAM、点云配准和标定,最重要的不只是记住正规方程,而是把下列关系建立起来:
求解器能把一个定义正确的问题算出来,但不能替代正确的建模,也不能创造原本不存在的信息。
更多推荐
所有评论(0)