刚体变换

刚体变换 刚体变换及其连续合成
2020-04-12 18:06:14
目录

刚体变换及其连续合成
刚体（rigid body）的姿态：
旋转矩阵
基本旋转
向量的旋转
连续旋转变换矩阵的合成
欧拉角表示

连续齐次变换矩阵的合成
刚体变换及其连续合成

刚体（rigid body）的姿态：

位姿（pose）包括位置（position）和方向（orientation）。
- 为了表示位置和指向，考虑在刚体上建立一个固连在刚体上的标准正交坐标系。
- 位置，即坐标，用刚体坐标系原点相对参考坐标系基向量的分量表示，一般参考坐标系基向量是正交的单位向量。
- 方向，即指向，用刚体坐标系各轴相对参考坐标系基向量的分量表示，如三维空间，需要三个单位向量表示，每个单位向量都是刚体坐标系的轴相对参考坐标系的方向余弦（即在参考坐标系各个基向量上的投影）。
旋转矩阵

表示刚体坐标系方向的三个单位向量组成了3x3矩阵 $R$ ， $R$ 的每列为当前坐标系的轴在参考坐标系基向量下的分量。因为每列都是单位的，正交的，因此矩阵 $R$ 是正交阵。

右手坐标系下 $R$ 的行列式 $d e t (R) = 1$ ，左手坐标系下， $d e t (R) = - 1$ 。

$R$ 的逆矩阵为 $R$ 的转置矩阵, $R^{-1}=R^{T}$ ，即 $R^{-1}·R^{T}=I$ 。

基本旋转

不考虑平移的情况下，即坐标系原点重合时，通过上述定义的 $R$ 矩阵，其每列表示的是刚体坐标系基向量在参考坐标系下的余弦。

旋转矩阵 $R$ 的物理含义是：将参考坐标系旋转到与刚体坐标系重合所需要的旋转。

向量的旋转

向量，可以通过其在参考坐标系各轴基向量的分量来表达。同一个向量在不同参考坐标系下的坐标表达是不同的。

对于同一个点 $P$ ，我们已知 $P_{0}$ 为其在参考坐标系 $O_{0}$ 的坐标表示， $P_{1}$ 为其在刚体坐标系 $O_{1}$ 下的坐标表示， $R_{1}^{0}$ 表示坐标系 $O_{0}$ 到 $O_{1}$ 的旋转，即 $O_{0}$ 旋转到 $O_{1}$ 重合所需要的旋转。则两个向量之间的转换关系如下：

$P_{0}=R_{1}^{0} · P_{1}$

因此，至此可以总结一个旋转矩阵 $R$ 具有的三个等价的几何学意义：
1. $R$ 描述了两个坐标系之间的相对指向。其列向量为旋转后的坐标系的轴关于原来坐标系中各轴的方向余弦。
2. $R$ 表示了同一点 $P$ 在两个坐标系下的坐标转换矩阵。
3. $R$ 是在一个坐标系下，对向量进行旋转的算子。
连续旋转变换矩阵的合成

完整的旋转可以表示为若干次旋转的合成，且由于每次旋转之后当前坐标系已经不是初始坐标系了，因此多次旋转合成分为每次相对原始固定坐标系旋转，可称为绕静坐标系旋转，和每次都相对当前新坐标系旋转，称为绕动坐标系旋转。

若 $R_{1}^{0}$ 表示 $O_{0}$ 到 $O_{1}$ 的旋转， $R_{2}^{1}$ 表示 $O_{1}$ 到 $O_{2}$ 的旋转，则：
- 绕动坐标系旋转，合成原则为连续右乘，即：
$R_{2}^{0}=R_{1}^{0}·R_{2}^{1}$
- 绕静坐标系旋转，合成原则为连续左乘，即：
$\overline{R_{2}^{0}}=\overline{R_{2}^{1}}·R_{1}^{0}$

在第二次旋转中， $R_{2}^{1}$ 与 $\overline{R_{2}^{1}}$ 不同， $R_{2}^{1}$ 指的是相对当前坐标系（ $O_{1}$ ）将 $O_{1}$ 旋转到与 $O_{2}$ 重合需要的旋转矩阵，而 $\overline{R_{2}^{1}}$ 指的是相对静坐标系（ $O_{0}$ ）将 $O_{1}$ 旋转到与 $O_{2}$ 重合需要的旋转矩阵。

同理，最终得到的 $R_{2}^{0}$ 与 $\overline{R_{2}^{0}}$ 涵义亦不同。

连续左乘、右乘的左右是合成旋转矩阵的方向，从第一个矩阵 $R_{1}^{0}$ 开始算起。

值得注意的是，相对固定坐标系的旋转序列和相对当前（运动）坐标系的旋转序列，导致的最终刚体坐标系指向是不一致的（一般来说）。但是两者的影响是互换的,容易从旋转矩阵的合成公式看出：沿着固定坐标系沿着 $X - Y - Z$ 序列进行旋转，其结果与沿着运动坐标系进行 $Z - Y - X$ 反向序列旋转得到的结果是一致的。

欧拉角表示

旋转矩阵9个元素是冗余的，对于特殊正交群 $S O (m)$ ，最简表达需要m(m-1)/2个参数，一种通过3个参数表达旋转的方式是欧拉角，即沿着坐标系三个轴顺序转动的合成来表达一个旋转。

由于相邻两次旋转要求不是平行的轴，因此在27种顺序组合中，有效的顺序组合有12种，两种经典的欧拉角顺序是ZYZ和ZYX角。

以ZYX角为例：

$R=R_{z}·R_{y}·R_{x}$

可以从两个方向进行解读：
1. 沿着Z-Y-X顺序，相对动坐标系，从 $R_{z}$ 开始连续右乘；
2. 沿着X-Y-Z顺序，相对静坐标系，从 $R_{x}$ 开始连续左乘。
连续齐次变换矩阵的合成

因为刚体变换不仅涉及到上述旋转，还涉及到平移变换，因此在连续变换过程中，表达会变得越来越繁琐，不能直接合成，使用齐次变换可以直接对连续的若干次刚体变换进行合成。

$A_{j}^{i}$ 表示坐标系 $O_{i}$ 变换到和坐标系 $O_{j}$ 重合的齐次矩阵。则:

$A_{n}^{0}=A_{1}^{0} · A_{2}^{1} ··· A_{n}^{n-1}$

$P_{0}=A_{1}^{0} · A_{2}^{1} ··· A_{n}^{n-1} · P_{n}$

可以从两个方向进行解读：
1. 计算一系列齐次变换的等价变换时，理解为将坐标系从原始坐标系变换到终了坐标系，因此是从 $A_{1}^{0}$ 开始，是右乘，是动坐标系。比如知道机器人7个连杆两两之间的齐次变换，求末端工具坐标系位姿，则 $A_{6}^{0}=A_{1}^{0} · A_{2}^{1} · A_{3}^{2} · A_{4}^{3} · A_{5}^{4} · A_{6}^{5}$ ，从机器人基坐标系，连乘到最后一个连杆的齐次变换矩阵。
2. 计算一个向量的变换时，理解为针对一点 $P$ ，进行一系列齐次坐标变换，因此是从 $P_{n}$ 和 $A_{n}^{n-1}$ 开始，是对点 $P$ 不断左乘。比如将一个向量从末端坐标系转换到机器人基坐标系，则在计算的时候，从最后一个连杆的齐次变换开始，不断在向量上左乘，如：
$P_{0}=A_{6}^{0} · P_{6}=A_{1}^{0} · A_{2}^{1} · A_{3}^{2} · A_{4}^{3} · A_{5}^{4} · A_{6}^{5} · P_{6}$
1. 值得注意的是，以上 $R_{j}^{i}$ ，含义均为坐标系 $O_{i}$ 变换到与 $O_{j}$ 重合的齐次变换（旋转变换）。其可以用于左乘 $O_{j}$ 下的向量 $P_{j}$ 将其转换到 $O_{i}$ 坐标系下得到 $P_{i}$ 。其每列为 $O_{j}$ 各轴在 $O_{i}$ 基向量下的余弦值（亦即，基向量上的投影）。
齐次变换

更多相关内容
SVD解算对应点集的刚体变换矩阵
2021-03-11 23:08:33

SVD解算对应点集的刚体变换矩阵

收起
大数据集的刚体变换：大量uint8的刚体变换-matlab开发
2021-06-01 19:36:37

% Big_Transform：大数据集的刚体变换% % [Vi,M] = Big_Transform(V,p) % % V：要变换的体积，可以是整数 uint8。 % p: [ti tj tk ai aj ak si sj sk]; %（平移、旋转、缩放） % Vi：使用三线性插值的新插值体积% M...

收起

matlab
三维空间刚体变换：变换矩阵
2022-02-06 12:17:24
通过对视觉SLAM十四讲第三章及机器人学建模控制与视觉第三章的学习，在旋转矩阵的基础上，整理变换矩阵的表达、运算
变换矩阵

坐标系描述

用 $^Ap_{Bo}$ 表示在参考坐标系 ${A\}$ 下坐标系 ${B\}$ 的原点坐标，并用旋转矩阵 $^A_BR$ 描述坐标系 ${B\}$ 在参考坐标系 ${A\}$ 下的姿态，则坐标系 ${B\}$ 的姿态可表示为：
$\{B\}=\{\enspace^Ap_{Bo}\quad{}^A_BR\enspace\}$
坐标系的描述概括了刚体的位置和姿态的描述，当表示位置时，旋转矩阵 ${}^A_BR=I$ ；当表示姿态时，位移矢量 $^Ap_{Bo}=0$

平移映射

当两坐标系具有相同的方位，当坐标原点不重合时，可用位移矢量 $^Ap_{Bo}$ 描述坐标系 ${B\}$ 相对于坐标系 ${A\}$ 的位置：

称 $^Ap_{Bo}$ 为坐标系 ${B\}$ 相对于 ${A\}$ 的平移矢量，若点 $p$ 在坐标系 ${B\}$ 下的位置为 $^Bp$ ，则它相对于坐标系 ${A\}$ 的位置 $^Ap$ 可表示为：
${}^Ap={}^Bp+{}^Ap_{Bo}$

旋转映射

当两坐标系原点重合，但方位不同时，可用旋转矩阵 ${}^A_BR$ 描述坐标系 ${B\}$ 相对于坐标系 ${A\}$ 的姿态：

称 ${}^A_BR$ 为坐标系 ${B\}$ 相对于 ${A\}$ 的方位，若点 $p$ 在坐标系 ${B\}$ 下的位置为 $^Bp$ ，则它相对于坐标系 ${A\}$ 的位置 $^Ap$ 可表示为：
${}^Ap={}^A_BR\:{}^Bp$
同样，用 ${}^B_AR$ 表示坐标系 ${A\}$ 相对于 ${B\}$ 的方位，则可由旋转矩阵的正交约束得：
${}^B_AR={}^A_BR^{-1}={}^A_BR^T$

一般刚体变换

在三维空间中，最常见的情况如下：坐标系 ${B\}$ 与 ${A\}$ 不但原点不重合，同时姿态也不相同。此时采用位移矢量 $^Ap_{Bo}$ 描述坐标系 ${B\}$ 相对于坐标系 ${A\}$ 的位置，用旋转矩阵 ${}^A_BR$ 描述坐标系 ${B\}$ 相对于坐标系 ${A\}$ 的姿态：

则若点 $p$ 在坐标系 ${B\}$ 下的位置为 $^Bp$ ，则它相对于坐标系 ${A\}$ 的位置 $^Ap$ 可表示为：
${}^Ap={}^A_BR\:{}^Bp+{}^Ap_{Bo}$
也即，为旋转映射与平移映射的复合。先将坐标系 ${B\}$ 根据旋转矩阵进行旋转，在沿着位移矢量进行平移。

将上式进行齐次变换，得到变换矩阵 ${}^A_BT$ ：
$\begin{bmatrix}{}^Ap\\1\end{bmatrix}=\begin{bmatrix}{}^A_BR&{}^Ap_{Bo}\\0&1\end{bmatrix}\begin{bmatrix}{}^Bp\\1\end{bmatrix}={}^A_BT\:{}^Bp$
其中齐次变换矩阵 ${}^A_BT$ 为 $4\times4$ 的矩阵，它表示了由坐标系 ${B\}$ 到坐标系 ${A\}$ 的变换关系：
${}^A_BT=\begin{bmatrix}{}^A_BR&{}^Ap_{Bo}\\0&1\end{bmatrix}$

齐次坐标

若一点的 $p$ 的坐标为 $\begin{bmatrix}x&y&z\end{bmatrix}^T$ ，则它的齐次坐标表示如下，齐次坐标并不唯一，可将隔行同乘一个常数 $\omega$ 表示的仍为同一坐标：
$p=\begin{bmatrix}x\\y\\z\\1\end{bmatrix}=\begin{bmatrix}\omega x\\\omega y\\\omega z\\\omega\end{bmatrix}$
同时，规定坐标 $\begin{bmatrix}0&0&0&0\end{bmatrix}^T$ 无意义，各个轴的无穷远处如下表示：
$X轴:\begin{bmatrix}1\\0\\0\\0\end{bmatrix}\quad Y轴:\begin{bmatrix}0\\1\\0\\0\end{bmatrix}\quad Z轴:\begin{bmatrix}0\\0\\1\\0\end{bmatrix}\quad 任意无穷远处: \begin{bmatrix}a\\b\\c\\0\end{bmatrix}$

变换矩阵运算与刚体群

运动算子

变换矩阵 ${}^A_BT$ 可分解为两个矩阵相乘的形式，其中用 $Trans({}^Ap_{Bo})$ 表示平移变换矩阵，用 $Rot(k,\theta)$ 旋转变换矩阵：
$\begin{aligned} {}^A_BT =&\begin{bmatrix}{}^A_BR&{}^Ap_{Bo}\\0&1\end{bmatrix}\\ =&\begin{bmatrix}I_{3\times3}&{}^Ap_{Bo}\\0&1\end{bmatrix}\begin{bmatrix}{}^A_BR(k,\theta)&0\\0&1\end{bmatrix}\\ =&Trans({}^Ap_{Bo})Rot(k,\theta) \end{aligned}$
$Trans({}^Ap_{Bo})$ 表示沿着位移矢量 ${}^Ap_{Bo}$ 进行平移， $Rot(k,\theta)$ 则表示绕着过原点的轴 $k$ 旋转 $\theta$ 角。

平移算子

相对坐标系 ${A\}$ ，位移矢量 ${}^Ap_1$ 沿着 ${}^Ap$ 移动至位移矢量 ${}^Ap_2$ 可用矢量相加表示：
${}^Ap_2={}^Ap_1+{}^Ap$
将其写为算子形式如下：
${}^Ap_2=Trans({}^Ap)\:{}^Ap_1$
平移算子为 $Trans({}^Ap)$ 表示沿着位移矢量 ${}^Ap$ 的大小和方向进行平移。

旋转算子

相对坐标系 ${A\}$ ，某点由 ${}^Ap_1$ 旋转至 ${}^Ap_2$ 可表示为：
${}^Ap_2=R\:{}^Ap_1$
将其写为算子形式如下：
${}^Ap_2=Rot(k,\theta)\:{}^Ap_1$
旋转算子 $Rot(k,\theta)$ 表示沿着过原点的轴 $k$ 旋转角度 $\theta$ 。

运动算子一般形式

齐次变换矩阵可用作为运动算子，描述某点在坐标系内的运动（包括平移和旋转）。当变换矩阵作为运动算子使用时，不带上、下标。

例如，质点 $p$ 在坐标系 ${A\}$ 中的运动轨迹为时间 $t$ 的函数 ${}^Ap(t)$ ，其初始位置为 ${}^Ap(0)$ ，则用运动算子表示该质点的运动轨迹如下：
${}^Ap(t)=T(t)\:{}^Ap(0)$

变换矩阵的运算

变换矩阵乘法

给定变换矩阵 ${}^A_BT、{}^B_CT$ ，则可以得到变换矩阵 ${}^A_CT$ ：
${}^A_CT={}^A_BT\:{}^B_CT=\begin{bmatrix}{}^A_BR\:{}^B_CR&{}^A_BR\:{}^Bp_{Co}+{}^Ap_{Bo}\\0&1\end{bmatrix}$
也即，坐标系 ${C\}$ 在坐标系 ${A\}$ 中的位姿，可以拆为两步进行：先将坐标系 ${C\}$ 转换至坐标系 ${B\}$ 中的位姿；再将此位姿根据坐标系 ${B\}$ 至 ${A\}$ 的变换进行变换，从而得到最终变换矩阵。

变换矩阵乘法满足如下条件：
- 运动相对固定坐标系而言：满足左乘规则，即变换顺序从右至左
- 运动相对运动坐标系而言：满足右乘规则，即变换顺序从左至右
- 变换的顺序不能调换进行
例如，对于一个变换矩阵 ${}^A_BT=Trans(\begin{matrix}1&-3&4\end{matrix})\:Rot(y,90\degree)\:Rot(z,90\degree)$

若相对固定坐标系（坐标系 ${A\}$ ）而言，采用左乘规则进行：首先绕 $z_A$ 轴旋转 $90\degree$ ，再绕 $y_A$ 轴旋转 $90\degree$ ，最后相对 ${A\}$ 平移 $\begin{bmatrix}1&-3&4\end{bmatrix}^T$ 即可。

若相对于运动坐标系（坐标系 ${B\}$ ）而言，采用右乘规则进行：首先相对坐标系 ${A\}$ 平移 $\begin{bmatrix}1&-3&4\end{bmatrix}^T$ ，再绕 $y_B$ 轴旋转 $90\degree$ ，最后绕 $z_B$ 轴旋转 $90\degree$ 即可。

变换矩阵求逆

若已知变换矩阵 ${}^A_BT$ ，此时需要求解坐标系 ${A\}$ 相对于坐标系 ${B\}$ 的变换 ${}^B_AT$ ，则需要对变换矩阵进行求逆。

此处利用齐次变换矩阵特点，简化计算：已知 ${}^A_BT$ 求解 ${}^B_AT$ ，只需要由 ${}^A_BR$ 和 ${}^Ap_{Bo}$ 求解 ${}^B_AR$ 和 ${}^Bp_{Ao}$ 即可。

首先，通过旋转矩阵正交约束知：
${}^B_AR={}^A_BR^{-1}={}^A_BR^T$
随后，根据平移映射关系，可知在坐标系 ${A\}$ 下坐标系 ${B\}$ 的原点 ${}^Ap_{Bo}$ ，在坐标系 ${B\}$ 的坐标 ${}^B({}^Ap_{Bo})$ ：
${}^B({}^Ap_{Bo})={}^B_AR\:{}^Ap_{Bo}+{}^Bp_{Ao}$
已知，在坐标系 ${B\}$ 中，点 $B o$ 为原点，即上式右侧为零：
${}^Bp_{Ao}=-{}^B_AR\:{}^Ap_{Bo}=-{}^A_BR^T\:{}^Ap_{Bo}$
由此得到变换矩阵的逆矩阵：
${}^B_AT=\begin{bmatrix}{}^A_BR^T&-{}^A_BR^T\:{}^Ap_{Bo}\\0&1\end{bmatrix}$
易得： ${}^B_AT={}^A_BT^{-1}$

刚体变换群

变换矩阵 $T$ 由旋转矩阵 $R$ 和位移矢量 $t$ 决定，也即任意刚体的位姿由 $(t,R):t\in\Re^{3},R\in SO(3)$ 决定。

定义刚体变换群 $S E (3)$ 为乘积空间 $\Re^{3}\times SO(3)$ ：
$SE(3)=\bigl\{T=\begin{bmatrix}R&t\\0^T&1\end{bmatrix}\in \Re^{4\times4}\big\vert R\in SO(3),t\in\Re^{3}\bigr\}=\Re^{3}\times SO(3)$
$S E (3)$ 又称为三维空间的特殊欧式群（Special Euclidean Group ），推广到n维空间中可得：
$SE(n)=\bigl\{T=\begin{bmatrix}R&t\\0^T&1\end{bmatrix}\in \Re^{n+1\times n+1}\big\vert R\in SO(n),t\in\Re^{n}\bigr\}=\Re^{n}\times SO(n)$
$S E (n)$ 为李群，当n=2时， $S E (2)$ 表示刚体的平面运动，其单位元为 $I_3$ ；当n=3时， $S E (3)$ 表示刚体
收起
展开全文
刚体变换
万次阅读 2017-08-06 23:07:53

向量叉乘（Cross Product） ---------------------------------------------------------------------------------------------------------------------------------------------------------- ...刚体变换

向量叉乘（Cross Product）

----------------------------------------------------------------------------------------------------------------------------------------------------------

刚体变换（Rigid Transformation）

刚体变换变换前后两点间的距离依旧保持不变则被称为刚体变换（Rigid Transform）。刚体变换可分解为平移变换、旋转变换和反转（镜像）变换。

----------------------------------------------------------------------------------------------------------------------------------------------------------

（1）让g（x）是三维向量空间到三维空间的一个映射函数。如果该函数满足下列性质，则被称为刚体变换（Rigid Transformation）:

（2）刚体变化能够通过下式表示：

在该式中，矩阵，R，被称为旋转矩阵（Rotation Matrix），并且满足下列特殊性质：

（3）拥有下述性质的矩阵，连同矩阵乘法运算，构成一个组（Group）：SO(3)

上式是一个旋转矩阵。

性质：

组（Group）

----------------------------------------------------------------------------------------------------------------------------------------------------------

组（Group）是一种代数结构，由一个集合，G，和一组组上的操作组成，满足下列公理：

（1）封闭性（Closure）:



（2）结合性（Associativity）:



（3）同一性和可逆性（Identity and Inverse）:





罗德里格斯公式（Rodrigues Formula）

----------------------------------------------------------------------------------------------------------------------------------------------------------

表示物体的旋转有三种方式：转轴和转角，旋转矩阵，四元素。

罗德里格斯公式用来将转轴转角表示方式转换成旋转矩阵表示方式：

其中theta是转角，omega是转轴的单位向量。

四元素（Quaternion）

----------------------------------------------------------------------------------------------------------------------------------------------------------

四元素可以被看做是由一个标量和一个矢量构成：



四元素乘法计算如下：



单位四元素是四个分量平方和为一的四元素，对于转轴和转角表示方式也可以用单位四元素方式表示：



同样也可以从四元素得到旋转矩阵：



四元数共轭（Quaternion Conjugate）



值得注意的是：

（1）如果一个四元素对应一个旋转矩阵，则该四元素的共轭对应于同一旋转矩阵的转置；

（2）四元素乘法与对应旋转矩阵乘法的等价性：



收起

展开全文
OpenCV 之空间刚体变换
2021-04-29 19:45:09
刚体就是 "刚性物体"，它在运动过程中，内部各质点间的相对位置不会改变，也即每两个质点间的距离保持不变假设刚体内任意两个质点，坐标分别为 $(x_1, y_1, z_1)$ 和 $(x_2, y_2, z_2)$，则在刚体运动过程中，这...
刚体就是 "刚性物体"，它在运动过程中，内部各质点间的相对位置不会改变，也即每两个质点间的距离保持不变

假设刚体内任意两个质点，坐标分别为 $(x_1, y_1, z_1)$ 和 $(x_2, y_2, z_2)$ ，则在刚体运动过程中，这两个质点满足如下条件：

        $\quad \left( (x_1 - x_2)^2 + (y_1 - y_2)^2 + (z_1 - z_2)^2 \right) |_t = l^2$

例如：影视剧《西游记》中的法宝金刚琢、玉净瓶是刚体；而幌金绳、芭蕉扇等，则不是刚体

1 刚体变换

1.1 矩阵形式

  OpenCV 之图像几何变换中的等距变换，实际是二维刚体变换

$\quad \begin{bmatrix} x^{\prime} \\ y^{\prime} \\ 1 \end{bmatrix} = \begin{bmatrix} \cos \theta & -\sin \theta & t_x \\ \sin \theta & \cos \theta & t_y \\ 0&0&1 \end{bmatrix} \begin{bmatrix} x \\ y \\ 1\end{bmatrix}$

从平面推及空间，三维刚体变换的矩阵形式为

$\quad \begin{bmatrix}x' \\ y' \\ z' \\ 1 \end{bmatrix}=\begin{bmatrix} r_{11} & r_{12} & r_{13} & t_x \\ r_{21} & r_{22} & r_{23} & t_y \\ r_{31} & r_{32} & r_{33} &t_z \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\z \\ 1\end{bmatrix}$

例如：空间任一点，在相机坐标系中为 $P_{c}(x, y, z)$ ，世界坐标系中为 $P_{w}(X, Y, Z)$ ，则 $P_c$ -> $P_w$ 就是一个刚体变换



1.2 约束分析

R 和 T 共有 12 个未知数，但 R 是标准正交矩阵，自带 6 个约束方程，则刚体变换有 12 - 6 = 6 个自由度 (和直观的感受一致)

表面上看，似乎只需两组空间对应点，联立 6 个方程，便可求得 6 个未知数，但这 6 个方程是有冗余的 (因为这两组对应点，在各自的坐标系下，两点之间的距离是相等的)

因此，第二组对应点，只是提供了 2 个约束方程，加上第一组对应点的 3 个约束，共有 5 个独立的方程

    显然，还需要第三组对应点，提供 1 个独立的方程，才能求出 R 和 T

如图所示，两个刚体之间：1个点重合 => 3个自由度；2个点重合 => 1个自由度；3个点重合=> 0个自由度



OpenCV 中有一个函数 estimateAffine3D() 可求解刚体变换的矩阵

1.3 直观理解

一个单位立方体，可在 X-Y-Z 坐标系中自由运动，则二者之间的转换关系，可视为刚体变换

  单点重合：当立方体的角点 0 和 X-Y-Z 坐标系的原点 O 重合时，立方体还能自由的旋转 (X 轴 -> Y 轴 -> Z 轴)



  两点重合：除了立方体的角点 0 和坐标系的原点 O 重合外，再令角点 4 和 X 轴上的某点重合，则此时立方体只能绕 X 轴旋转



  三点重合：除了以上两个角点 0 和 4，如果再使角点 1 和 Z 轴上的某点重合，则立方体就会和 X-Y-Z 坐标系牢牢的连接在一起



因此，选取不共面的三组对应点，联立方程组，便可求得 R 和 T

2 旋转向量

任意的旋转，都可用一个旋转轴 (axis) 和绕轴旋转角 (angle) 来描述，简称“轴角”



因此，可用一个方向和旋转轴一致，长度等于旋转角的向量来表示旋转，这个向量称为旋转向量 (或“旋量”)

假定旋转轴 $\mathbf{n} = [r_{x}, r_{y}, r_{z}]$ ，旋转角为 $\theta$ ，则旋转向量可记为 $\theta \mathbf{n}$

旋转向量到旋转矩阵的转换，可由罗德里格斯公式来实现，如下：

$\quad R = cos(\theta) I + (1 - cos \theta) \mathbf{nn^T} + sin \theta \begin{bmatrix} 0&-r_z&r_y \\ r_z&0&-r_x \\ -r_y&r_x&0 \end{bmatrix}$

反之，从旋转矩阵到旋量，公式如下：

$\quad \sin \theta \begin{bmatrix}0 &-r_z&r_y \\r_z&0&-r_x \\-r_y & r_x &0 \end{bmatrix} = \dfrac{R - R^T}{2}$

OpenCV 中有一个 Rodrigues() 函数，可实现二者的相互转换
```
void  Rodrigues (
      InputArray   src,  // 输入旋转向量 n(3x1 或 1x3) 或 旋转矩阵 R(3x3)
      OutputArray  dst,  // 输出旋转矩阵 R 或 旋转向量 n
      OutputArray  jacobian = noArray()  // 可选，输出 Jacobian 矩阵(3x9 或 9x3)
)     
```
3 欧拉角

3.1 定义

假定 X-Y 平面内有一点 P，旋转 $\theta$ 角到 $P^{\prime}$ 位置，如下图：



取 $\angle$POX = $\theta_0$ ，列方程组得

$\quad x^{\prime} = r \cos (\theta_0 + \theta) = r \cos \theta_0 \cos\theta - r \sin \theta_0 \sin\theta = x \cos\theta - y \sin \theta$

$\quad y^{\prime} = r \sin(\theta_0+ \theta) = r \sin\theta_0 cos \theta + r \cos \theta_0 \sin\theta = x \sin \theta + y \cos \theta$

转化为矩阵形式   $\begin{bmatrix} x' \\ y' \end{bmatrix} = \begin{bmatrix} cos \theta & -sin \theta \\ sin \theta & cos \theta \end{bmatrix} \begin{bmatrix} x \\ y \end{bmatrix}$

3.2 欧拉角

将二维旋转矩阵 $R_{2 \times 2}$ 扩展到三维空间

1）绕 Z 轴旋转 roll，则添加 z 坐标 $\pmb{R_z} = \begin{bmatrix} cos \theta_z & -sin \theta_z & 0 \\ sin \theta_z & cos \theta_z & 0 \\ 0&0&1 \end{bmatrix}$

2）绕 Y 轴旋转 yaw，则添加 y 坐标 $\pmb{R_y} = \begin{bmatrix} cos \theta_y & 0 & \color{blue}{sin \theta_y} \\ 0 & 1 & 0 \\ \color{blue}{-sin \theta_y} & 0 & cos \theta_y \end{bmatrix}$

3）绕 X 轴旋转 pitch，则添加 x 坐标 $\pmb{R_x} = \begin{bmatrix} 1 & 0 & 0 \\ 0 &cos \theta_x & -sin \theta_x \\ 0 & sin \theta_x & cos \theta_x \end{bmatrix}$

因此，当按 Z-Y-X 的顺序旋转时，一个旋转矩阵就被分解成了绕不同轴的三次旋转，旋转角称为 "欧拉角"

$\quad R = R_z \cdot R_y \cdot R_x = \begin{bmatrix} cos \theta_y cos \theta_z & sin \theta_x sin \theta_y cos \theta_z - cos \theta_x sin \theta_z & sin \theta_x sin \theta_z + cos \theta_x sin \theta_y cos \theta_z \\ cos \theta_y sin \theta_z & cos \theta_x cos \theta_z + sin \theta_x sin \theta_y sin \theta_z & cos \theta_x sin \theta_y sin \theta_z - sin \theta_x cos \theta_z \\ -sin \theta_y & sin \theta_x cos \theta_y & cos \theta_x cos \theta_y \end{bmatrix}$

注意：在使用欧拉角时，要先指明旋转的顺序，因为绕不同的轴旋转时得到的欧拉角也不同

反之，由旋转矩阵求解欧拉角，则有：

$\quad \theta_x = tan^{-1} (\dfrac{r_{32}}{r_{33}})$

$\quad \theta_y = sin^{-1} (-r_{33})$

$\quad \theta_z = tan^{-1} (\dfrac{r_{21}}{r_{11}})$

3.3 代码实现

已知绕三个轴旋转的欧拉角，要转换为旋转矩阵，直接套用公式
```
Mat eulerAnglesToRotationMatrix(Vec3f &theta)
{
    // Calculate rotation about x axis
    Mat R_x = (Mat_<double>(3,3) <<
               1,       0,              0,
               0,       cos(theta[0]),   -sin(theta[0]),
               0,       sin(theta[0]),   cos(theta[0])  );

    // Calculate rotation about y axis
    Mat R_y = (Mat_<double>(3,3) <<
               cos(theta[1]),    0,      sin(theta[1]),
               0,                1,      0,
               -sin(theta[1]),   0,      cos(theta[1])  );

    // Calculate rotation about z axis
    Mat R_z = (Mat_<double>(3,3) <<
               cos(theta[2]),    -sin(theta[2]),      0,
               sin(theta[2]),    cos(theta[2]),       0,
               0,                0,                   1  );

    // Combined rotation matrix
    Mat R = R_z * R_y * R_x;

    return R;
}
```
旋转矩阵到欧拉角的转换，要指明旋转顺序 (Z-Y-X 或 X-Y-Z 等 6 种)，下面代码实现了和 MATLAB 中 rotm2euler 一样的功能，只是旋转顺序不同 (X-Y-Z)
```
// Checks if a matrix is a valid rotation matrix.
bool isRotationMatrix(Mat &R)
{
    Mat Rt;
    transpose(R, Rt);
    Mat shouldBeIdentity = Rt * R;
    Mat I = Mat::eye(3,3, shouldBeIdentity.type());

    return  norm(I, shouldBeIdentity) < 1e-6;
}

// The result is the same as MATLAB except the order of the euler angles ( x and z are swapped ).
Vec3f rotationMatrixToEulerAngles(Mat &R)
{
    assert(isRotationMatrix(R));
    float sy = sqrt(R.at<double>(0,0) * R.at<double>(0,0) +  R.at<double>(1,0) * R.at<double>(1,0) );
    bool singular = sy < 1e-6; // If

    float x, y, z;
    if (!singular) {
        x = atan2(R.at<double>(2,1) , R.at<double>(2,2));
        y = atan2(-R.at<double>(2,0), sy);
        z = atan2(R.at<double>(1,0), R.at<double>(0,0));
    } else {
        x = atan2(-R.at<double>(1,2), R.at<double>(1,1));
        y = atan2(-R.at<double>(2,0), sy);
        z = 0;
    }
    return Vec3f(x, y, z);
}  
```
4 四元数

4.1 定义

四元数本质是一种高阶的复数，普通复数有一个实部和一个虚部，而四元数有一个实部和三个虚部

$\quad q = s + x \mathbf{i} + y \mathbf{j} + z \mathbf{k}$，其中 $\mathbf{i}^2=\mathbf{j}^2=\mathbf{k}^2=\mathbf{ijk}=-1$



平面中任一点的旋转，可通过“左乘” 旋转向量来表示，如下：

$\quad p = a + b \mathbf{i} $，$q = cos \theta + \mathbf{i} sin \theta$

$\quad p^{\prime} = qp = a\cos\theta-b\sin\theta+(a\sin\theta+b\cos\theta)i$

推及空间中任一点的旋转，可通过“左乘”四元数来表示，如下：

$\quad p = [0, a \mathbf{i} + b \mathbf{j} + c \mathbf{k}]$，$q = [cos \theta, sin \theta \mathbf{v} ]$ ，其中 $\mathbf{v}$ 由 $\mathbf{i}, \mathbf{j}, \mathbf{k}$ 组合而成

$\quad p^{\prime} = qp$

4.2 实例

例1：当向量 p 围绕 k 轴在 i-j 平面内旋转 45°，表示该旋转的四元数为

$\quad q = \left[\dfrac{\sqrt{2}}{2}, \dfrac{\sqrt{2}}{2} \mathbf{k} \right]$

取 $p = [0, 2 \mathbf{i}]$，则 $p^{\prime} = qp = [0, \sqrt{2}\mathbf{i} + \sqrt{2} \mathbf{j}] $，如下图 p' 确实是 p 围绕 $\mathbf{k}$ 轴旋转 45° 得到的

例2：当向量 p 围绕 q 旋转 45°，且 q 中的向量 v 在 i-k 平面内和 p 成 45° 时，表示该旋转的四元数为

$\quad q = \left[ \dfrac{\sqrt{2}}{2}, \dfrac{\sqrt{2}}{2} \left(\dfrac{\sqrt{2}}{2} \mathbf{i} + \dfrac{\sqrt{2}}{2} \mathbf{k} \right) \right]$

取 $p = [0, 2 \mathbf{i}]$，则 $p^{\prime} = qp = [-1,\sqrt{2}\mathbf{i}+\mathbf{j}]$ ，可看出 $p^{\prime}$ 中向量模长为 $\sqrt{3}$，这不再是一个纯旋转的变换



但如果再“右乘” $q^{-1}$ ，则 $p^{\prime} = qpq^{-1} = [0,\mathbf{i}+\sqrt{2}\mathbf{j}+\mathbf{k} ]$ ，如下图，这又变成了一个纯旋转，但是旋转的角度是 90° 不是 45°



综上所述，向量 p 围绕任一轴 $\mathbf{v}$ 旋转 $\theta$ ，则表示该旋转的四元数形式为

$\quad q=\left[\cos\dfrac{\theta}{2},\sin\dfrac{\theta}{2} \mathbf{v}\right]$

4.3 转换关系

假定四元数 $q = s + x \mathbf{i} + y \mathbf{j} + z \mathbf{k}$ ，则旋转矩阵为

$\quad R = \begin{bmatrix}1-2(y^{2}+z^{2})&2(xy-sz)&2(xz+sy)\\2(xy+sz)&1-2(x^{2}+z^{2})&2(yz-sx)\\2(xz-sy)&2(yz+sx)&1-2(x^{2}+y^{2})\end{bmatrix}$

或另一种形式

$\quad R = \begin{bmatrix} s^2 + x^2 - y^2 -z^2 & 2(xy - sz) & 2(xz + sy) \\ 2(xy +sz) & s^2-x^2+y^2-z^2 & 2(yz - sx) \\ 2(xz-sy) & 2(yz+sx) & s^2-x^2-y^2+z^2 \end{bmatrix}$



附 - 欧拉角可视化

一个欧拉角的可视化链接 Euler Angle Visualization Tool，输入欧拉角可实时显示位姿变化



参考资料

《An Invitaton to 3D Vision》 ch2

《Robot Vision》ch13

  OpenCV - 3D rigid/afine transforamtion

  Understanding Quaternions

  Rotation Matrix To Euler Angles

  An Orge compatible class for euler angles

更多机器视觉文章请访问飞鸢逐浪的博客
收起
展开全文
几种图像变换 刚体变换 仿射变换投影变换
千次阅读 2020-10-09 10:35:24

... 变换模型是指根据待匹配图像与背景图像之间几何畸变的情况...可采用的变换模型有如下几种:刚性变换、仿射变换、透视变换和非线形变换等，如下图：参考：http://wenku.baidu.com/view/826a796027d3240c8447...

收起

数据增强
#1 SLAM_Rigid body transformation（刚体变换）
千次阅读 2022-02-02 13:39:35

对于刚体而言，它们的所有组件(零件、点、面或任何东西)都在一个自身的局部参考系中刚性地指定(它们在这个参考系中构成常数参数，即位置相对固定)。因此，刚性物体的运动可以完全由自身参照系的运动来表示。（参考系...

收起
刚体变换和仿射变换的区别
千次阅读 2020-10-07 10:54:57

Content（1）刚体变换（2）仿射变换结论（1）刚体变换 如果图像A只需要经过空间的平移和旋转就可以变换到图像B。这样的变换称为刚体变换。在变换中，图像可沿着x、y、z轴平移或旋转。因此刚性变换只需要6个自由度。...

收起
三维空间变换：刚体变换、仿射变换、线性变换、旋转变换
千次阅读 2021-03-30 09:51:34

本文简要介绍了三维空间变换的一些概念，重点针对旋转变换做了较为细致地描述，给出了不同的旋转矩阵

收起
三维刚体变换
2021-01-30 10:55:04

第三章：三维刚体变换 刚体：形状和大小不发生变化的物体，刚体不光有位置，还有自身的姿态，姿态是指物体的朝向。点：点是空间中的基本元素，没有长度没有体积，两个点连接起来，构成了向量。向量：可以看成从某...

收起

计算机视觉
刚体变换与视觉标定.pptx
2019-12-05 15:06:19

刚体变换与视觉标定是入门视觉三维重建，定位等基本问题。PPT总结了刚体变换的表示方法以及标定的原理。刚体变换与视觉标定是入门视觉三维重建，定位等基本问题。PPT总结了刚体变换的表示方法以及标定的原理。刚体...

收起
图像处理--------刚体变换
2021-12-06 16:12:30

参考: https://www.pianshen.com/article/1900518653/

收起

图像处理
视觉SLAM十四讲学习4 （1）刚体变换与李群，李代数
2021-12-04 14:26:01

视觉SLAM十四讲学习4 刚体变换与李群，李代数前言群定义李群与李代数的引出正交群与欧式群李代数的引出李代数so(3)so(3)so(3)的几何意义李群与李代数的转换后记前言本篇记录SLAM十四讲中的，李群与李代数。本篇与...

收起

矩阵
三维刚体变换公式
2021-07-29 21:25:59

最近在做刚体变换，查阅了许多相关的文献，发现许多文章中对于旋转矩阵的定义都不一致。笔者经过仔细比对终于发现了一些规律，现在总结如下：第一种：注意这里的 sin 角的正负号，使用这种方式定义的x,y,z旋转...

收起
三维空间刚体变换：旋转矩阵
2022-02-06 12:13:36

通过对视觉SLAM十四讲第三章前半部分内容以及机器人学建模控制与视觉第三章位姿描述与齐次变换两本书的学习，整理了三维空间中旋转矩阵的表示方法。

收起

自动驾驶
仿射变换+透视变换+欧式变换（刚体变换）+相似变换+剪切变换
2021-02-18 16:00:03

仿射变换（引入齐次坐标）齐次坐标：就是用n+1维来表示n维坐标。一个2D笛卡尔坐标末尾加上一个额外的变量w来形成2D齐次坐标，即一个点（X,Y）在齐次坐标里面变成了（x,y,w）,并且有X=x/w, Y=y/w 为什么叫齐次...

收起

计算机视觉
关于图像变换的总结（仿射变换，刚体变换等）
千次阅读 2019-07-10 21:14:13

如果一幅图像中的两点间的距离经变换到另一幅图像中后仍然保持不变，则这种变换称为刚体变换(Rigid Transform)。刚体变换仅局限于平移、旋转和反转(镜像)。 (2)仿射变换如果一幅图像中的直线经过变换到另一幅图像...

收起
点云配准（二）三维刚体变换
2022-05-07 21:02:52

刚体的欧式变换就相当于我们把一个刚体原封不动地进行了运动，而不改变它自身的形态，该变换过程可以由旋转和平移组合描述。 1.旋转变换推导 2.平移变换推导二.齐次坐标变换三.旋转变换的多种表示 1....

收起
刚体变换 相似变换仿射变换投影变换
2018-10-10 23:16:00

刚体变换即将图形进行平移旋转相似变换加入了放缩仿射变换则是保持二维图形的“平直性”（译注：straightness，即变换后直线还是直线不会打弯，圆弧还是圆弧）保持原有线段的比例关系不变。投影变换任意变换，...

收起
刚体变换，相似性变换，单应性变换，透视变换，投影变换，仿射变换
2020-05-29 09:11:56

转载自https://www.cnblogs.com/vincentcheng/p/7191014.html

收起

计算机视觉
估计刚体变换(similarity、euclidean、affine)
千次阅读 2019-09-04 19:27:01

欧式/仿射变换：已知点对应关系：基于svd的经典算法：https://igl.ethz.ch/projects/ARAP/svd_rot.pdf http://rci.rutgers.edu/~meer/TEACH/ADD/similaritymine.pdf 代码参考博客：...

收起

affine
刚体变换与ICP原理
千次阅读 2019-06-27 15:10:20

刚性变换矩阵点云配准的最终目的是通过一定的旋转和平移变换将不同坐标系下的两组或者多组点云数据统一到同一参考坐标系下。这个过程，可以通过一组映射来完成。假设映射变换为H，这H可以用以下的公式来表示。 ...

收起
一文读懂图像中点的坐标变换(刚体变换，相似变换，仿射变换，投影变换)
万次阅读 多人点赞 2019-01-16 16:32:44

现在的人脸图像识别流程中有一个步骤叫人脸对齐，现在的一般方法是采用人脸上的关键点坐标，进行相似变换来实现人脸校正。多次在人脸识别的论文中看到 similarity transform，由于在线代和矩阵分析的课上一直划水。...

收起
迭代最近点法：将一组数据点拟合到一组刚体变换下的模型点-matlab开发
2021-05-29 21:25:50

ICP（迭代最近点）算法找到一个刚体变换，使得一组数据点适合变换下的一组模型点。默认是使用最小二乘最小化，但也可以使用其他标准函数。该实现基于 [1] 中描述的 IRLS-ICP。参考： [1] Bergström, P. 和 ...

收起

matlab
sophus 欧几里得群 SE(2) 和 SE(3) 来表示刚体变换
2022-05-06 11:37:33

这是李群的 C++ 实现，通常用于 2d 和 3d 几何问题（即...其中，这个包包括特殊正交群 SO(2) 和 SO(3) 来表示 2d 和 3d 中的旋转，以及特殊的欧几里得群 SE(2) 和 SE(3) 来表示刚体变换（即旋转和翻译）在 2d 和 3d 中

收起

sophus
利用奇异值分解的刚体变换(MATLAB)
2019-09-10 15:31:15

%% 在三维空间中对给定两个点集，利用SVD方法实现从一个点集到另一个点集的刚体变换 clear; clc; d=3; %给定空间维数 n=10; %给定点对数 P=10*rand(d,n); %点集P Q=2*rand(d,n); %点集Q %%-----生成点对权重----- w=...

收起

奇异值分解 MATLAB
OpenCVSharp 三维点计算刚体变换的旋转平移矩阵(RT矩阵)
千次阅读 2021-09-23 18:27:21

OpenCVSharp 三维点计算刚体变换的旋转平移矩阵前言一、旋转平移矩阵是什么二、计算原理：1.计算源和目标点的平均中心2.计算质心3.SVD求解4.计算R矩阵5.计算t矩阵三、示例代码：四、结果比较：总结前言在3D相机...

收起

opencv
PCL学习笔记--利用矩阵对点云进行刚体变换(ICP)
万次阅读 2019-01-28 13:11:55

利用矩阵对点云进行刚体变换 在这篇教程中，我们会学习如何用一个4*4的矩阵对点云进行变换。我们会加载一个点云，然后对其进行刚体变换（旋转加平移），并可视化结果，最后通过ICP算法对点云进行配准。 **...

收起