Truncated Least Squares（TLS 截断最小二乘）算法原理

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

等价于：

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 解法

思想：

1. 用平滑近似替代 TLS
2. 逐渐减小平滑参数
3. 最终收敛到真正的 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 残差评估函数

double TLSLoss(double r2, double tau2)
{
    return std::min(r2, tau2);
}

---

9.2 ICP 中的 TLS inlier 判定

if (residual_norm < tau)
{
    // normal least squares update
    H += J.transpose() * J;
    b += J.transpose() * r;
}
else
{
    // truncated → ignore
}

---

9.3 GNC-TLS 权重更新（简化）

double UpdateWeight(double r2, double mu)
{
    return mu / (mu + r2);
}

---

10、示例展示

示例 1：一维均值估计（最直观理解 TLS）

1. 问题描述

观测一组标量数据（含外点）：

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）

1. 初始化 ( x )（用中值）
2. 判断每个残差是否 ( |x-z_i| < \tau )
3. 仅用 inliers 更新 ( x )
4. 重复直到收敛

---

4. C++ 代码示例

#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）

repeat
  1. 建立最近邻匹配
  2. 计算残差
  3. residual < τ → inlier
  4. 用 inliers 做 SVD 求解 T
until convergence

---

4. 关键代码（核心逻辑）

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 优化流程

μ = μ0 (large)
repeat
  1. compute weights: w_ij = μ / (μ + r_ij^2)
  2. weighted least squares solve
  3. μ ← μ / κ
until μ small

---

4. C++ 伪代码

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 是它真正能落地的工程形态。

---