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

这篇具有很好参考价值的文章主要介绍了【数理知识】求刚体旋转矩阵和平移矩阵,已知 N>=3 个点在前后时刻的坐标,且这 N>=3 点间距离始终不变代表一个刚体。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

序号 内容
1 【数理知识】自由度 degree of freedom 及自由度的计算方法
2 【数理知识】刚体 rigid body 及刚体的运动
3 【数理知识】刚体基本运动,平动,转动
4 【数理知识】向量数乘,内积,外积,matlab代码实现
5 【数理知识】最小二乘法,从线性回归出发,数值举例并用最小二乘法求解回归模型
6 【数理知识】最小二乘法,一般线性情况,矩阵化表示过程,最佳参数的求解公式过程
7 【数理知识】协方差,随机变量的的协方差,随机变量分别是单个数字和向量时的协方差
8 【数理知识】奇异值分解,从数据的线性变换角度来理解
9 【数理知识】旋转矩阵的推导过程,基于向量的旋转来实现,同时解决欧式变换的非线性局限
10 【数理知识】三维空间旋转矩阵的欧拉角表示法,四元数表示法,两者之间的转换,Matlab 代码实现
11 【数理知识】已知 N>=3 个点在前后时刻的坐标,求刚体平移矩阵,旋转矩阵,且这 N>=3 点间距离始终不变代表一个刚体

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

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

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

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

接下来的描述中,我们为了和图中情况保持一致,我们都假设 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=13Ak=31k=13Bk

其中 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 US V V V 的大小也相同。

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

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

【数理知识】求刚体旋转矩阵和平移矩阵,已知 N>=3 个点在前后时刻的坐标,且这 N>=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=(ACentroidA)(BCentroidB)T=SVD(H)=VUT

其中, H H H 是我们熟悉的协方差矩阵。 A − Centroid A A - \text{Centroid}_{A} ACentroidA 是用 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=CentroidBR×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=n1kn=3(xkixˉi)(ykjyˉ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.57740.57740.57740.81650.40820.40820.00000.70710.7071 ,S= 900000000 ,V= 0.57740.57740.57740.81650.40820.408200.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=CentroidBR×CentroidA=(1,1,1)文章来源地址https://www.toymoban.com/news/detail-643517.html


Ref

  1. FINDING OPTIMAL ROTATION AND TRANSLATION BETWEEN CORRESPONDING 3D POINTS
  2. 从3组对应点中求得最佳的旋转和平移变换

到了这里,关于【数理知识】求刚体旋转矩阵和平移矩阵,已知 N>=3 个点在前后时刻的坐标,且这 N>=3 点间距离始终不变代表一个刚体的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • webgl-矩阵、旋转、平移、缩放 glMatrix组件

    引入新组建glMatrix glMatrix.js /*! @fileoverview gl-matrix - High performance matrix and vector operations @author Brandon Jones @author Colin MacKenzie IV @version 3.4.3 Copyright (c) 2015-2021, Brandon Jones, Colin MacKenzie IV. Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation f

    2024年02月10日
    浏览(45)
  • Halcon用矩阵实现图像变换(平移,旋转,缩放,镜像等)

    目录 图像变换介绍  用Halcon自带的算子实现图像变换 使用矩阵来实现相关算子的功能 一、平移 二、旋转 三、缩放 四、镜像 完整代码         在halcon中经常会用到图像变换的操作,然后这次作业是用矩阵来实现相关算子的功能,学到了挺多的所以就记录下来方便复习。

    2024年04月17日
    浏览(40)
  • 【KITTI】Kitti数据集 Tr(旋转平移)矩阵求逆

            本节详细介绍KITTI从图像坐标系到激光雷达坐标系的变换原理、过程和代码。         Kitti数据集的Tr_velo_to_cam矩阵是将激光雷达点云坐标变换到图像坐标系。Tr是一个3x4的矩阵,直接左乘激光雷达坐标即可得到图像坐标系中的坐标。         Tr可以认为是由

    2023年04月24日
    浏览(47)
  • 旋转矩阵R、平移向量t以及变换矩阵T的定义及其下标的含义

    首先,只考虑旋转。 假设坐标系1: { X 1 , Y 1 , Z 1 } {X_1, Y_1, Z_1} { X 1 ​ , Y 1 ​ , Z 1 ​ } 经过 纯旋转 之后得到坐标系2: { X 2 , Y 2 , Z 2 } {X_2, Y_2, Z_2} { X 2 ​ , Y 2 ​ , Z 2 ​ } (如上图),其中坐标系1对应的单位正交基为 ( e 1 , e 2 , e 3 ) left(e_{1}, e_{2}, e_{3}right) ( e 1 ​ , e

    2023年04月23日
    浏览(48)
  • 2D坐标系下的点的转换矩阵(平移、缩放、旋转、错切)

    1. 平移 (Translation) 在2D空间中,我们经常需要将一个点平移到另一个位置。假设空间中的一点 P ( x , y ) P(x,y) P ( x , y ) ;将其向 x , y x, y x , y 方向分别平移 t x t_x t x ​ , t y t_y t y ​ , 假设平移后点的坐标为 ( x ′ , y ′ ) (x\\\',y\\\') ( x ′ , y ′ ) ,则上述点的平移操作可以归纳为

    2024年02月15日
    浏览(38)
  • Unity矩阵平移旋转缩放Matrix4x4

    最近在研究帧同步定点数物理系统中需要自定义定点数矩阵,所以在这里分享下基础的矩阵案例旋转、平移、缩放。(注意这里本文中的transform组件式基于unity浮点数的教程并非帧同步定点数)参考原文 参数可以参考我上图的参数,这里注意三个顶点是一个面,这里我上述的

    2024年01月17日
    浏览(40)
  • 三维变换矩阵实战——三维点云的旋转、缩放、镜像、错切、平移、正交投影

    旋转矩阵:右边矩阵是点云的原始坐标,左边的是旋转矩阵     可视化:绕x轴旋转90度 代码: 旋转矩阵:    可视化:绕y轴旋转180度 代码: 旋转矩阵:    可视化:绕z轴旋转90度 代码: 旋转矩阵:  线绕哪个轴转,xyz矩阵就和哪和轴的旋转矩阵先计算      可视化:先

    2024年02月04日
    浏览(89)
  • Android中矩阵Matrix实现平移,旋转,缩放和翻转的用法详细介绍

    一,矩阵Matrix的数学原理 矩阵的数学原理涉及到矩阵的运算和变换,是高等代数学中的重要概念。在图形变换中,矩阵起到关键作用,通过矩阵的变换可以改变图形的位置、形状和大小。矩阵的运算是数值分析领域的重要问题,对矩阵进行分解和简化可以简化计算过程。对于

    2024年01月22日
    浏览(53)
  • Web3D数学基础(平移、旋转、缩放矩阵)—WebGL、WebGPU、Threejs

    参考资料:threejs中文网 threejs qq交流群:814702116 本下节课给大家介绍下矩阵的概念,以及用于几何变换的矩阵,比如平移矩阵、缩放矩阵、旋转矩阵。 如果你对这些几何变换的矩阵概念比较熟悉,可以跳过本节课。 线性代数、图形学 如果你有《线性代数》、《计算机图形学

    2024年02月03日
    浏览(50)
  • 【数理知识】矩阵普通乘积,哈达玛积,克罗内克积,点乘,点积,叉乘,matlab代码实现

    序号 内容 1 【数理知识】向量数乘,内积,外积,matlab代码实现 2 【数理知识】矩阵普通乘积,哈达玛积,克罗内克积,点乘,点积,叉乘,matlab代码实现 首先介绍矩阵 在数学中,矩阵是一个按照长方阵列排列的复数或实数集合。由 m × n m times n m × n 个数 a i j a_{ij} a ij ​

    2024年02月04日
    浏览(72)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包