【数理知识】求刚体旋转矩阵和平移矩阵,已知 N>=3 个点在前后时刻的坐标,且这 N>=3 点间距离始终不变代表一个刚体

序号 内容
1 【数理知识】自由度 degree of freedom 及自由度的计算方法
2 【数理知识】刚体 rigid body 及刚体的运动
3 【数理知识】刚体基本运动,平动,转动
4 【数理知识】向量数乘,内积,外积,matlab代码实现
5 【数理知识】协方差,随机变量的的协方差,随机变量分别是单个数字和向量时的协方差
6 【数理知识】已知 N>=3 个点在前后时刻的坐标,求刚体平移矩阵,旋转矩阵,且这 N>=3 点间距离始终不变代表一个刚体

文章目录

  • [1 解决流程](#1 解决流程)
    • [1. 找寻质心(Centroid)](#1. 找寻质心(Centroid))
    • [2. 奇异值分解(SVD)](#2. 奇异值分解(SVD))
    • [3. 通过协方差矩阵得到旋转矩阵](#3. 通过协方差矩阵得到旋转矩阵)
    • [4. 计算平移矩阵](#4. 计算平移矩阵)
  • [2 举例验证 1](#2 举例验证 1)
    • [1. 找寻质心(Centroid)](#1. 找寻质心(Centroid))
    • [2. 计算协方差矩阵](#2. 计算协方差矩阵)
    • [3. 奇异值分解](#3. 奇异值分解)
    • [4. 计算平移矩阵](#4. 计算平移矩阵)
  • Ref

存在有 N ≥ 3 N\ge 3 N≥3 个点,它们两两之间距离始终不变,这就满足了可代表一个刚体的条件。同时,已知这 N ≥ 3 N\ge 3 N≥3 个点在前后时刻的坐标,如何求对应刚体的平移矩阵,旋转矩阵?

如下图所示,对应点的颜色相同, R R R 是旋转, t t t 是平移。我们希望找到能将数据集 A A A 中的点对齐到数据集 B B B 的最佳旋转和平移。这种变换有时被称为欧几里得变换(Euclidean)或刚性变换(Rigid transform),因为它保留了形状和大小。这与仿射变换不同,后者包括缩放和剪切。

这个问题尤其出现在三维点云数据注册等任务中,因为这些数据是从三维激光扫描仪或流行的 Kinect 设备等硬件中获取的。

接下来的描述中,我们为了和图中情况保持一致,我们都假设 N = 3 N = 3 N=3。

1 解决流程

旋转和平移的方程式可以表示为如下形式:

R A + t = B \begin{aligned} RA + t = B \end{aligned} RA+t=B

最终目的是求取最合适的 R R R 和 t t t。

至于为什么可以这么表示,请参考文章开头所提及到的其他文章。

1. 找寻质心(Centroid)

这一步也比较简单,质心就是 N = 3 N = 3 N=3 个数据点的平均值

Centroid A = 1 3 ∑ k = 1 3 A k Centroid B = 1 3 ∑ k = 1 3 B k \begin{aligned} \text{Centroid}{A} &= \frac{1}{3} \sum{k=1}^{3} A_k \\ \text{Centroid}{B} &= \frac{1}{3} \sum{k=1}^{3} B_k \end{aligned} CentroidACentroidB=31k=1∑3Ak=31k=1∑3Bk

其中 A k A_k Ak 和 B k B_k Bk 分别表示在数据集 A A A 和 B B B 中第 k k k 个数据点的坐标。


2. 奇异值分解(SVD)

有几种方法可以找到点之间的最佳旋转。最简单的方法是使用奇异值分解(SVD),因为许多编程语言(Matlab、Octave、使用 LAPACK 的 C 语言、使用 OpenCV 的 C++ 语言...)都可以使用这个函数。SVD 就像线性代数中的一根神奇魔杖,可以解决各种数值问题。这里不会详细介绍它的工作原理,而会介绍如何使用它。你只需要知道,SVD 可以将一个矩阵(称作 E E E)分解/因式分解为另外 3 个矩阵,即

[ U , S , V ] = SVD ( E ) E = U S V T \begin{aligned} [U, S, V] &= \text{SVD} (E) \\ E &= U S V^\text{T} \end{aligned} [U,S,V]E=SVD(E)=USVT

如果 E E E 是方阵,那么 U 、 S U、S U、S 和 V V V 的大小也相同。

3. 通过协方差矩阵得到旋转矩阵

要找到最佳旋转方式,我们首先要重新调整两个数据集的中心,使两个中心点都位于原点,如下图所示。

这样就去除了平移部分,只剩下旋转部分需要处理。下一步是累加一个矩阵(称为 H H H),然后使用 SVD 求出旋转,如下所示:

H = ( A − Centroid A ) ( B − Centroid B ) T [ U , S , V ] = SVD ( H ) R = V U T \begin{aligned} H &= (A - \text{Centroid}{A})(B - \text{Centroid}{B})^\text{T} \\ [U, S, V] &= \text{SVD} (H) \\ R &= V U^\text{T} \end{aligned} H[U,S,V]R=(A−CentroidA)(B−CentroidB)T=SVD(H)=VUT

其中, H H H 是我们熟悉的协方差矩阵。 A − Centroid A A - \text{Centroid}{A} A−CentroidA 是用 A A A 减去 Centroid A \text{Centroid}{A} CentroidA 中的每一列的操作。

需要注意的一点是,要正确计算 H H H。它最终应该是一个 3 × 3 3 \times 3 3×3 矩阵,而不是一个 N × N N \times N N×N 矩阵(这里 N N N 是指点的数量,而 3 3 3 是指数据的坐标 [ x , y , z ] [x,y,z] [x,y,z] 维度是 3 3 3)。注意转置符号。它是在两个矩阵之间进行乘法运算,这两个矩阵的实际维数分别是 3 × N 3 \times N 3×N 和 N × 3 N \times 3 N×3。乘法的顺序也很重要,如果换一种方法,就会变成是从 B B B 到 A A A 的旋转。

4. 计算平移矩阵

得到旋转矩阵 R R R 后,平移矩阵 t t t 也就变得简单了。把质心代入开篇咱们提到的方程,那么有

R A + t = B R × Centroid A + t = Centroid B t = Centroid B − R × Centroid A \begin{aligned} RA + t &= B \\ R \times \text{Centroid}_A + t &= \text{Centroid}_B \\ t &= \text{Centroid}_B - R \times \text{Centroid}_A \end{aligned} RA+tR×CentroidA+tt=B=CentroidB=CentroidB−R×CentroidA


2 举例验证 1

假设有 3 3 3 个相对位置保持不变的点,已知它们在数据集合 A A A 和数据集合 B B B 中的位置,然后计算旋转矩阵 R R R 和平动矩阵 t t t。

在数据集合 A A A 中:

点 1 1 1 的位置为: A 1 = ( 1 , 2 , 3 ) A_1 = (1, 2, 3) A1=(1,2,3);

点 2 2 2 的位置为: A 2 = ( 4 , 5 , 6 ) A_2 = (4, 5, 6) A2=(4,5,6);

点 3 3 3 的位置为: A 3 = ( 7 , 8 , 9 ) A_3 = (7, 8, 9) A3=(7,8,9)。

在数据集合 B B B 中:

点 1 1 1 的位置为: B 1 = ( 2 , 3 , 4 ) B_1 = (2, 3, 4) B1=(2,3,4);

点 2 2 2 的位置为: B 2 = ( 5 , 6 , 7 ) B_2 = (5, 6, 7) B2=(5,6,7);

点 3 3 3 的位置为: B 3 = ( 8 , 9 , 10 ) B_3 = (8, 9, 10) B3=(8,9,10)。

计算思路为:

  • 先计算平移:通过求取这些点在两个时刻的质心位置,然后求差来得到平移矩阵

1. 找寻质心(Centroid)

这一步也比较简单,直接代入样本数据

Centroid A = ( 1 + 4 + 7 , 2 + 5 + 8 , 3 + 6 + 9 ) 3 = ( 1 + 4 + 7 3 , 2 + 5 + 8 3 , 3 + 6 + 9 3 ) = ( 4 , 5 , 6 ) Centroid B = ( 2 + 5 + 8 , 3 + 6 + 9 , 4 + 7 + 10 ) 3 = ( 2 + 5 + 8 3 , 3 + 6 + 9 3 , 4 + 7 + 10 3 ) = ( 5 , 6 , 7 ) \begin{aligned} \text{Centroid}{A} &= \frac{(1+4+7, 2+5+8, 3+6+9)}{3} = (\frac{1 + 4 + 7}{3}, \frac{2 + 5 + 8}{3}, \frac{3 + 6 + 9}{3}) = (4, 5, 6) \\ \text{Centroid}{B} &= \frac{(2+5+8, 3+6+9, 4+7+10)}{3} = (\frac{2 + 5 + 8}{3}, \frac{3 + 6 + 9}{3}, \frac{4 + 7 + 10}{3}) = (5, 6, 7) \end{aligned} CentroidACentroidB=3(1+4+7,2+5+8,3+6+9)=(31+4+7,32+5+8,33+6+9)=(4,5,6)=3(2+5+8,3+6+9,4+7+10)=(32+5+8,33+6+9,34+7+10)=(5,6,7)

2. 计算协方差矩阵

根据公式

Cov ( X , Y ) i j = ∑ k n = 3 ( x k i − x ˉ i ) ( y k j − y ˉ i ) n − 1 \begin{aligned} \text{Cov} (X,Y){ij} &= \frac{\sum_k^{n=3} (x{ki} - \bar{x}i)(y{kj} - \bar{y}_i)}{n-1} \end{aligned} Cov(X,Y)ij=n−1∑kn=3(xki−xˉi)(ykj−yˉi)

可以得到协方差矩阵

Cov ( X , Y ) = [ 3 3 3 3 3 3 3 3 3 ] \begin{aligned} \text{Cov} (X,Y) &= \left[\begin{matrix} 3 & 3 & 3 \\ 3 & 3 & 3 \\ 3 & 3 & 3 \\ \end{matrix}\right] \end{aligned} Cov(X,Y)= 333333333

关于协方差矩阵的原理和求解方法,可参考文章:【数理知识】协方差,随机变量的的协方差,随机变量分别是单个数字和向量时的协方差

3. 奇异值分解

可以直接使用 Matlab 进行奇异值分解,可以得到

U = [ − 0.5774 0.8165 − 0.0000 − 0.5774 − 0.4082 − 0.7071 − 0.5774 − 0.4082 0.7071 ] , S = [ 9 0 0 0 0 0 0 0 0 ] , V = [ − 0.5774 0.8165 0 − 0.5774 − 0.4082 − 0.7071 − 0.5774 − 0.4082 0.7071 ] U = \left[\begin{matrix} -0.5774 & 0.8165 & -0.0000 \\ -0.5774 & -0.4082 & -0.7071 \\ -0.5774 & -0.4082 & 0.7071 \\ \end{matrix}\right], S = \left[\begin{matrix} 9 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ \end{matrix}\right], V = \left[\begin{matrix} -0.5774 & 0.8165 & 0 \\ -0.5774 & -0.4082 & -0.7071 \\ -0.5774 & -0.4082 & 0.7071 \\ \end{matrix}\right] U= −0.5774−0.5774−0.57740.8165−0.4082−0.4082−0.0000−0.70710.7071 ,S= 900000000 ,V= −0.5774−0.5774−0.57740.8165−0.4082−0.40820−0.70710.7071

R = V U T = [ 1 0 0 0 1 0 0 0 1 ] R = V U^\text{T} = \left[\begin{matrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \\ \end{matrix}\right] R=VUT= 100010001

至此得到了旋转矩阵。

4. 计算平移矩阵

t = Centroid B − R × Centroid A = ( 1 , 1 , 1 ) \begin{aligned} t &= \text{Centroid}_B - R \times \text{Centroid}_A &= (1, 1, 1) \end{aligned} t=CentroidB−R×CentroidA=(1,1,1)


Ref

  1. FINDING OPTIMAL ROTATION AND TRANSLATION BETWEEN CORRESPONDING 3D POINTS
  2. 从3组对应点中求得最佳的旋转和平移变换
相关推荐
MarkHD5 小时前
第十一天 线性代数基础
线性代数·决策树·机器学习
星沁城9 小时前
240. 搜索二维矩阵 II
java·线性代数·算法·leetcode·矩阵
幼儿园园霸柒柒1 天前
第七章: 7.3求一个3*3的整型矩阵对角线元素之和
c语言·c++·算法·矩阵·c#·1024程序员节
星沁城1 天前
73. 矩阵置零
java·算法·矩阵
jndingxin1 天前
OpenCV视觉分析之目标跟踪(11)计算两个图像之间的最佳变换矩阵函数findTransformECC的使用
opencv·目标跟踪·矩阵
pen-ai2 天前
【机器学习】21. Transformer: 最通俗易懂讲解
人工智能·神经网络·机器学习·矩阵·数据挖掘
会写代码的饭桶2 天前
【C++刷题】力扣-#566-重塑矩阵
c++·leetcode·矩阵
君臣Andy2 天前
【矩阵的大小和方向的分解】
线性代数·矩阵
勤劳的进取家2 天前
利用矩阵函数的导数公式求解一阶常系数微分方程组的解
线性代数
武子康2 天前
大数据-207 数据挖掘 机器学习理论 - 多重共线性 矩阵满秩 线性回归算法
大数据·人工智能·算法·决策树·机器学习·矩阵·数据挖掘