1、 TLS 的核心思想
残差小的观测正常用最小二乘；残差超过阈值的观测直接"截断"，不再继续惩罚。
它不是"减弱"异常值，而是直接拒绝它们的影响。
2、 从最小二乘到 TLS
2.1 经典最小二乘（LS）
给定残差 ( r i ( x ) r_i(\mathbf{x}) ri(x))：
min x ∑ i = 1 N r i 2 ( x ) \min_{\mathbf{x}} \sum_{i=1}^N r_i^2(\mathbf{x}) xmini=1∑Nri2(x)
问题：
一个巨大外点就能拖垮整个解
2.2 鲁棒核的改进思路
一般鲁棒核：
min x ∑ i ρ ( r i 2 ) \min_{\mathbf{x}} \sum_i \rho(r_i^2) xmini∑ρ(ri2)
- Huber：线性增长
- Cauchy / Geman--McClure：饱和
- Tukey：直接归零（红降）
TLS 是最极端的一种饱和形式
3、 TLS 的数学定义
3.1 TLS 损失函数
设阈值 ( τ > 0 \tau > 0 τ>0 )，则：
ρ TLS ( r 2 ) = { r 2 , r 2 ≤ τ 2 τ 2 , r 2 > τ 2 \rho_{\text{TLS}}(r^2) = \begin{cases} r^2, & r^2 \le \tau^2 \\ \tau^2, & r^2 > \tau^2 \end{cases} ρTLS(r2)={r2,τ2,r2≤τ2r2>τ2
特点：
- 小残差：标准二次惩罚
- 大残差：损失函数饱和
3.2 梯度行为（极其重要）
4 ∂ ρ ∂ r = { 2 r , ∣ r ∣ ≤ τ 0 , ∣ r ∣ > τ 4 \frac{\partial \rho}{\partial r} = \begin{cases} 2r, & |r| \le \tau \\ 0, & |r| > \tau \end{cases} 4∂r∂ρ={2r,0,∣r∣≤τ∣r∣>τ
超过阈值 → 梯度为 0 → 该观测不再影响优化
这就是 TLS "硬拒绝"的本质。
4、 TLS vs 常见鲁棒核
| 方法 | 大残差影响 | 可导性 | 工程风险 |
|---|---|---|---|
| LS | 无限增长 | 光滑 | 极差 |
| Huber | 线性增长 | 光滑 | 有外点偏置 |
| Cauchy | 饱和 | 光滑 | 收敛慢 |
| Tukey | 归零 | 非光滑 | 局部最优 |
| TLS | 完全截断 | 非光滑 | 强非凸 |
TLS = 最强鲁棒性 + 最强非凸性
5、 TLS 的优化难点
5.1 非凸问题
TLS 的代价函数是：
- 平坦区域
- 不连续导数
- 大量局部极小值
不能直接用标准 Gauss-Newton
6、 TLS 的三种主流优化方式
6.1 方式一：Binary Weight / Inlier Selection（最直观）
引入隐变量 ( w i ∈ 0 , 1 w_i \in {0,1} wi∈0,1)：
min x , w ∑ i w i r i 2 + ( 1 − w i ) τ 2 \min_{\mathbf{x}, \mathbf{w}} \sum_i w_i r_i^2 + (1-w_i)\tau^2 x,wmini∑wiri2+(1−wi)τ2
等价于：
text
if |r_i| < τ → w_i = 1
else → w_i = 0常见于 RANSAC / ICP inlier 选择
6.2 方式二：Graduated Non-Convexity (GNC-TLS)
相关文献内容：SLAM文献之-Outlier-Robust Estimation: Hardness,Minimally Tuned Algorithms, and Applications(1)
这是 SLAM / GTSAM / TEASER++ 中最重要的 TLS 解法
思想：
- 用平滑近似替代 TLS
- 逐渐减小平滑参数
- 最终收敛到真正的 TLS
常用 surrogate：
ρ ( r 2 ; μ ) = μ r 2 μ + r 2 \rho(r^2; \mu) = \frac{\mu r^2}{\mu + r^2} ρ(r2;μ)=μ+r2μr2
当 ( μ → 0 \mu \to 0 μ→0 )：
ρ → min ( r 2 , τ 2 ) \rho \to \min(r^2, \tau^2) ρ→min(r2,τ2)
GNC-TLS = 工程可用的 TLS
6.3 方式三：Black--Rangarajan Duality
将 TLS 转化为：
min x , w ∑ i ( w i r i 2 + ϕ ( w i ) ) \min_{\mathbf{x}, \mathbf{w}} \sum_i \left( w_i r_i^2 + \phi(w_i) \right) x,wmini∑(wiri2+ϕ(wi))
其中：
w i = { 1 , r 2 ≤ τ 2 0 , r 2 > τ 2 w_i = \begin{cases} 1, & r^2 \le \tau^2 \\ 0, & r^2 > \tau^2 \end{cases} wi={1,0,r2≤τ2r2>τ2
本质：交替优化权重 + 状态
7、 TLS 在 SLAM / 点云中的典型应用
7.1 位姿图优化（Loop Closure）
- 错误闭环边
- 使用 TLS 截断错误约束
- 防止整张图"拉崩"
GTSAM 的 noiseModel::Robust::Create(TLS, ...)
7.2 点云配准（ICP / 特征匹配）
- 错误最近邻
- 特征误匹配（FPFH / SHOT）
TLS = 比 Huber 更狠的 outlier rejector
7.3 TEASER++ / GNC 配准
TLS 是 理论保证全局最优的关键假设
8、 数值示例
残差集合：
r = [ 0.1 , 0.2 , 0.15 , 5.0 ] τ = 0.5 r = [0.1, 0.2, 0.15, 5.0] \quad \tau = 0.5 r=[0.1,0.2,0.15,5.0]τ=0.5
| 残差 | LS | TLS |
|---|---|---|
| 0.1 | 0.01 | 0.01 |
| 0.2 | 0.04 | 0.04 |
| 0.15 | 0.0225 | 0.0225 |
| 5.0 | 25.0 | 0.25（截断） |
外点被彻底"剪掉"
9、 C++ 示例
9.1 TLS 残差评估函数
cpp
double TLSLoss(double r2, double tau2)
{
return std::min(r2, tau2);
}9.2 ICP 中的 TLS inlier 判定
cpp
if (residual_norm < tau)
{
// normal least squares update
H += J.transpose() * J;
b += J.transpose() * r;
}
else
{
// truncated → ignore
}9.3 GNC-TLS 权重更新（简化）
cpp
double UpdateWeight(double r2, double mu)
{
return mu / (mu + r2);
}10、示例展示
示例 1：一维均值估计（最直观理解 TLS）
1. 问题描述
观测一组标量数据（含外点）：
text
z = [1.0, 1.1, 0.9, 1.05, 10.0]目标：估计真实值 ( x )
2. LS vs TLS 建模
LS：
min x ∑ i ( x − z i ) 2 \min_x \sum_i (x - z_i)^2 xmini∑(x−zi)2
TLS：
min x ∑ i min ( ( x − z i ) 2 , τ 2 ) \min_x \sum_i \min \left( (x - z_i)^2, \tau^2 \right) xmini∑min((x−zi)2,τ2)
设：
τ = 0.5 \tau = 0.5 τ=0.5
3. TLS 优化流程（inlier selection）
- 初始化 ( x )（用中值）
- 判断每个残差是否 ( |x-z_i| < \tau )
- 仅用 inliers 更新 ( x )
- 重复直到收敛
4. C++ 代码示例
cpp
#include <vector>
#include <cmath>
#include <iostream>
int main()
{
std::vector<double> z = {1.0, 1.1, 0.9, 1.05, 10.0};
double tau = 0.5;
double x = 1.0; // initial guess
for (int iter = 0; iter < 10; ++iter)
{
double sum = 0.0;
int cnt = 0;
for (double zi : z)
{
if (std::abs(x - zi) < tau)
{
sum += zi;
cnt++;
}
}
if (cnt > 0)
x = sum / cnt;
}
std::cout << "TLS estimate: " << x << std::endl;
}** 5. 结果解释**
- LS → ( x ≈ 2.8 x \approx 2.8 x≈2.8 )（被 10 拉走）
- TLS → ( x ≈ 1.01 x \approx 1.01 x≈1.01 )（外点被截断）
TLS = 坚决拒绝异常值
示例 2：2D 点云 ICP + TLS
1. 问题描述
- 两帧 2D 点云
- 存在错误最近邻匹配
- 目标：估计刚体变换 ( T \in SE(2) )
2. 残差定义
r i ( T ) = ∣ p i − T q i ∣ r_i(\mathbf{T}) = | \mathbf{p}_i - \mathbf{T}\mathbf{q}_i | ri(T)=∣pi−Tqi∣
TLS 代价：
min T ∑ i min ( r i 2 , τ 2 ) \min_{\mathbf{T}} \sum_i \min(r_i^2, \tau^2) Tmini∑min(ri2,τ2)
3. 优化流程（ICP + TLS）
text
repeat
1. 建立最近邻匹配
2. 计算残差
3. residual < τ → inlier
4. 用 inliers 做 SVD 求解 T
until convergence4. 关键代码（核心逻辑）
cpp
for (int iter = 0; iter < max_iter; ++iter)
{
std::vector<Eigen::Vector2d> src_in, tgt_in;
for (size_t i = 0; i < src_pts.size(); ++i)
{
double r = (tgt_pts[i] - (R * src_pts[i] + t)).norm();
if (r < tau)
{
src_in.push_back(src_pts[i]);
tgt_in.push_back(tgt_pts[i]);
}
}
// SVD solve for SE(2)
ComputeRigidTransform2D(src_in, tgt_in, R, t);
}5. 工程意义
- 错误匹配点 → 自动剔除
- 比 Huber 更强鲁棒性
- 非凸，但 ICP 本身已是迭代式
TLS 是 ICP 中最"干脆"的 outlier rejection
示例 3：位姿图优化（简单版本）
** 1. 问题描述**
- Pose Graph
- 部分 loop closure 是错误的
- 目标：估计所有位姿 ( {T_i} )
2. TLS 位姿图建模
min T i ∑ ( i , j ) ∈ E min ( ∣ log ( T i j − 1 T i − 1 T j ) ∣ Ω i j 2 , τ 2 ) \min_{{T_i}} \sum_{(i,j)\in \mathcal{E}} \min \left( | \log(T_{ij}^{-1} T_i^{-1} T_j) |^2_{\Omega_{ij}}, \tau^2 \right) Timin(i,j)∈E∑min(∣log(Tij−1Ti−1Tj)∣Ωij2,τ2)
3. GNC-TLS 优化流程
text
μ = μ0 (large)
repeat
1. compute weights: w_ij = μ / (μ + r_ij^2)
2. weighted least squares solve
3. μ ← μ / κ
until μ small4. C++ 伪代码
cpp
double mu = 10.0;
for (int outer = 0; outer < 10; ++outer)
{
for (auto& edge : edges)
{
double r2 = edge.Residual2();
edge.weight = mu / (mu + r2);
}
SolveWeightedPoseGraph(edges);
mu *= 0.5;
}5. 效果对比
| 方法 | 错误闭环 |
|---|---|
| LS | 图崩 |
| Huber | 偏移 |
| GNC-TLS | 稳定 |
这是 TLS 在 SLAM 中的"终极形态"
三个示例的层级关系
| 示例 | 核心理解 |
|---|---|
| 示例 1 | TLS = inlier / outlier |
| 示例 2 | TLS = ICP 的硬筛选 |
| 示例 3 | TLS = 非凸全局优化工具 |
11、 TLS vs RANSAC
| 特性 | TLS | RANSAC |
|---|---|---|
| 是否随机 | ❌ | ✅ |
| 是否确定性 | ✅ | ❌ |
| 可处理外点比例 | 99% | ~50% |
| 是否连续优化 | ✅ | ❌ |
| 是否可并行 | ✅ | ⚠️ |
TLS 是"连续型 RANSAC"
12 、TLS 的工程使用建议
何时用 TLS
- 外点比例 > 30%
- 闭环检测不稳定
- 特征匹配噪声极大
何时不用
- 噪声近似高斯
- 对连续性/光滑性要求高
最佳实践
GNC-TLS + 好初值 + 逐步收紧阈值
13、 总结
TLS 是"最狠"的鲁棒损失函数，它用非凸性换取对外点的绝对免疫；GNC-TLS 是它真正能落地的工程形态。