传感器观测点集对齐：SVD求解刚体变换原理与实践

1. 传感器观测点集对齐问题概述

在计算机视觉和传感器融合领域，我们经常遇到这样的场景：两个不同的视觉传感器（如相机、激光雷达等）同时观测到同一组空间点，需要确定这两个传感器之间的相对位置和姿态关系。这个问题在机器人导航、三维重建、增强现实等领域有着广泛的应用价值。

假设我们有两个传感器A和B，它们同时观测到一组空间点P={P₁,P₂,...,Pₙ}。由于传感器安装位置和朝向不同，它们观测到的点坐标会有差异。我们的目标是通过这些共同观测点，计算出两个传感器之间的相对位姿关系，即旋转矩阵R和平移向量t，使得对于任意点Pᵢ，满足：

Pᵢᴮ = R·Pᵢᴬ + t

其中Pᵢᴬ和Pᵢᴮ分别表示点Pᵢ在传感器A和B坐标系下的坐标。这个变换关系就是我们要寻找的两个传感器之间的刚体变换。

2. 数学模型建立与问题分析

2.1 坐标系与观测关系

首先我们需要明确几个坐标系：

1. 世界坐标系(W)：通常选择其中一个传感器坐标系作为参考
2. 传感器A坐标系(A)
3. 传感器B坐标系(B)

每个点Pᵢ在A和B坐标系下的观测可以表示为：
Pᵢᴬ = [xᵢᴬ, yᵢᴬ, zᵢᴬ]ᵀ
Pᵢᴮ = [xᵢᴮ, yᵢᴮ, zᵢᴮ]ᵀ

2.2 刚体变换的数学表示

两个坐标系之间的刚体变换可以用4×4齐次变换矩阵表示：

T = [ R t ]
[ 0 1 ]

其中R是3×3旋转矩阵，t是3×1平移向量。旋转矩阵R具有以下性质：

1. 正交性：RᵀR = I
2. 行列式为1：det(R) = 1

2.3 问题转化为优化问题

我们的目标是找到R和t，使得变换后的A坐标系点与B坐标系点尽可能接近。这可以转化为最小化以下误差函数：

min Σ || (R·Pᵢᴬ + t) - Pᵢᴮ ||²

这是一个典型的非线性最小二乘问题，可以通过多种方法求解，其中最经典的就是基于SVD的方法。

3. 基于SVD的位姿求解方法

3.1 去中心化处理

为了分离旋转和平移的影响，我们首先对点集进行去中心化处理。计算两个点集的质心：

cᴬ = (1/n) Σ Pᵢᴬ
cᴮ = (1/n) Σ Pᵢᴮ

然后计算去中心化的点坐标：
Qᵢᴬ = Pᵢᴬ - cᴬ
Qᵢᴮ = Pᵢᴮ - cᴮ

3.2 构建相关矩阵