二维刚体变换

欢迎访问我的博客首页。

---

1. 平面旋转

---

  计算机中通常使用右手三维坐标系，因此，如果给图 1 中的坐标系 $xOy$ 加上 $z$ 轴，则 $z$ 轴垂直纸面指向读者。 鉴于此，我们把图 1(左) 中绿色坐标系 $O_a$ 按箭头旋转到红色坐标系 $O_b$ 称为从 $z$ 轴负方向看向正方向时的顺时针旋转。

图 1 平面上的坐标系顺时针旋转(左)与向量顺时针旋转(右)

1.1 使用平面向量基本定理推导旋转

---

  向量基本定理：任意一个 n 维向量一定可以由 n 个相同维度且线性无关的向量来线性表示。这 n 个向量称为一组基底向量，简称基底。如果这 n 个向量两两正交，称为正交基。如果这 n 个向量都是单位向量，称为单位基底。

  假设坐标系 $O_a$ 顺时针旋转 $\theta$ 变成坐标系 $O_b$，如图 1(左)。坐标系 $O_a$ 的单位正交基在坐标系 $O_a$ 中的坐标表示是 $e_x^a=(1,0{)}^T$ 和 $e_y^a=(0,1{)}^T$。坐标系 $O_b$ 的单位正交基在坐标系 $O_a$ 中的坐标表示是 $e_x^b=(cos\theta ,sin\theta {)}^T$ 和 $e_y^b=(-sin\theta ,cos\theta {)}^T$。平面上任意一点 $P$ 在两个坐标系中的坐标是 $P_a$ 和 $P_b$。由向量基本定理知

$$[e_x^a,e_y^a]\cdot P_a=[e_x^b,e_y^b]\cdot P_b,$$

所以

$$\begin{array}{rl}{P}_a& =[e_x^a,e_y^a{]}^{-1}\cdot [e_x^b,e_y^b]\cdot P_b\\ & ={\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]}^{-1}\cdot \left[\begin{array}{cc}cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right]\cdot P_b\\ & =\left[\begin{array}{cc}cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right]\cdot P_b.\end{array}\text{\hspace{1em}(1.1)}$$

我们令

$$R_{ab}^{坐标系顺}=\left[\begin{array}{cc}cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right].\text{\hspace{1em}(1.2)}$$

  这个 $R_{ab}^{坐标系顺}$ 左乘平面上任意一点 $P$ 在坐标系 $O_b$ 中的坐标，得到 $P$ 在坐标系 $O_a$ 中的坐标。所以我们称它为坐标系顺时针旋转时 $O_b$ 坐标到 $O_a$ 坐标的变换。

1.2 使用几何关系推导旋转

---

  平面坐标系中任一长度为 $r$ 的向量 $\overrightarrow{O{P}_a}=(x_a,y_a)$ 绕原点顺时针旋转 $\theta$ 成为向量 $\overrightarrow{O{P}_b}=(x_b,y_b)$。假设向量 $\overrightarrow{O{P}_a}$ 与 $x$ 轴正方向的夹角是 $\varphi$，图 1(右)。则有

$$\{\begin{array}{rl}{x}_a& =rcos\varphi \\ y_a& =rsin\varphi \\ x_b& =rcos(\varphi +\theta )=rcos\varphi cos\theta -rsin\varphi sin\theta \\ y_b& =rsin(\varphi +\theta )=rsin\varphi cos\theta +rcos\varphi sin\theta ,\end{array}\text{\hspace{1em}(1.3)}$$

把前两个公式代入后两个公式得

$$\{\begin{array}{rl}{x}_b& =x_{a}cos\theta -y_{a}sin\theta \\ y_b& =y_{a}cos\theta +x_{a}sin\theta \end{array}.\text{\hspace{1em}(1.4a)}$$

把公式 $(1.4a)$ 写出矩阵相乘的形式

$$P_b=\left[\begin{array}{cc}cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right]P_a.\text{\hspace{1em}(1.4b)}$$

我们令

$$R_{ba}^{向量顺}=\left[\begin{array}{cc}cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right].\text{\hspace{1em}(1.5)}$$

  这个 $R_{ba}^{向量顺}$ 左乘平面上任意一点 $P$ 在坐标系 $O_a$ 中的坐标，得到 $P$ 在坐标系 $O_b$ 中的坐标。所以我们称它为向量顺时针旋转时 $O_a$ 坐标到 $O_b$ 坐标的变换。

1.3 总结

---

  由公式 $(1.2)$ 和公式 $(1.5)$ 可知：坐标系顺时针旋转时，把固定向量在旋转后的坐标系中的坐标变换到它在旋转前的坐标系中的坐标的变换，与坐标系不动时，把向量顺时针旋转后的坐标变换回旋转前的坐标的变换，互逆/互为转置。总结：

$$\{\begin{array}{rl}{R}_{ab}^{坐标系顺}& =R_{ab}^{向量逆}=\left[\begin{array}{cc}cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right]\\ R_{ab}^{坐标系逆}& =R_{ab}^{向量顺}=\left[\begin{array}{cc}cos\theta & sin\theta \\ -sin\theta & cos\theta \end{array}\right]\\ R_{ab}^{\ast }& =(R_{ba}^{\ast }{)}^{-1}=(R_{ba}^{\ast }{)}^T\end{array}.\text{\hspace{1em}(1.6)}$$

其中，下标 $ab$ 表示该变换矩阵把旋转后的坐标 $P_b$ 变换到旋转前的坐标 $P_a$，下标 $ba$ 表示该变换矩阵把旋转前的坐标 $P_a$ 变换到旋转后的坐标 $P_b$。

  以图 1(左) 为例，坐标系 $O_b$ 由坐标系 $O_a$ 顺时针旋转 45° 得到。则 $O_a$ 中的点 $P_a(1,2)$ 在 $O_b$ 中的坐标

$$P_b=(R_{ab}^{坐标系顺}{)}^{-1}{P}_a=\left[\begin{array}{cc}cos\frac{\pi }{4}& sin\frac{\pi }{4}\\ -sin\frac{\pi }{4}& cos\frac{\pi }{4}\end{array}\right]\left[\begin{array}{c}1\\ 2\end{array}\right]=\left[\begin{array}{c}\frac{3}{\sqrt{2}}\\ \frac{1}{\sqrt{2}}\end{array}\right].$$

2. 平面刚体变换

---

  假设坐标系 $O_a$ 平移 ${\vec{t}}_{ab}=(t_{ab}^x,t_{ab}^y{)}^T$ 且顺时针旋转 ${\theta }_{ab}$ 变成坐标系 $O_b$，如图 2(左)。平面上任意一点 $P$ 在两个坐标系中的坐标是 $P_a$ 和 $P_b$。

图 2 平面刚体变换

2.1 坐标变换

---

  由公式 $1.2$ 知向量 $\overrightarrow{O_{b}P}$ 在坐标系 $O_a$ 中的坐标是 $R_{ab}^{坐标系顺}\cdot P_b$。又因为向量 $\overrightarrow{O_a{O}_b}$ 在坐标系 $O_a$ 中的坐标是 ${\vec{t}}_{ab}$。所以向量 $\overrightarrow{O_{a}P}$ 在坐标系 $O_a$ 中的坐标

$$P_a=\overrightarrow{O_a{O}_b}+\overrightarrow{O_{b}P}={\vec{t}}_{ab}+R_{ab}^{坐标系顺}\cdot P_b=\left[\begin{array}{ccc}cos{\theta }_{ab}& -sin{\theta }_{ab}& t_{ab}^x\\ sin{\theta }_{ab}& cos{\theta }_{ab}& t_{ab}^y\\ 0& 0& 1\end{array}\right]\cdot P_b=T_{ab}^{坐标系顺}\cdot P_b.\text{\hspace{1em}(2.1)}$$

一般情况下，我们默认旋转指的是坐标系顺时针旋转，因此 $R_{ab}^{坐标系顺}$ 可以简写为 $R_{ab}$，$T_{ab}^{坐标系顺}$ 可以简写成 $T_{ab}$。其中 $R$ 称为旋转矩阵，$T$ 因包含旋转和平移而称为变换矩阵。

  下面是 gmapping 实现公式 $(2.1)$ 的代码。

Point phit = lp;
phit.x += *r * cos(lp.theta + *angle);
phit.y += *r * sin(lp.theta + *angle);

2.2 方向点

---

  坐标系 $O_a$ 与坐标系 $O_b$ 的关系可以用一个带方向的点表示。带方向的点是一个三元数 ${\xi }_{ab}=(t_{ab}^x,t_{ab}^y,{\theta }_{ab})$，分别表示坐标系 $O_b$ 的原点在坐标系 $O_a$ 中的横坐标、纵坐标、从坐标系 $O_a$ 到坐标系 $O_b$ 的旋转角度。

  方向点 $\xi$ 可以很容易地转换为变换矩阵 $T$。与变换矩阵相比，使用方向点表示二维变换有很多好处：维度更低，正好等于平面变换的自由度；两个方向点相加仍能表示二维变换，方便使用导数优化，而两个变换矩阵相加的结果不一定是变换矩阵。二维变换中的方向点类似三维变换中的李代数。

  假设坐标系 $O_b$ 平移 ${\vec{t}}_{bc}=(t_{bc}^x,t_{bc}^y{)}^T$ 且顺时针旋转 ${\theta }_{bc}$ 变成坐标系 $O_c$，如图 2(右)。那么，坐标系 $O_c$ 也可以看作由坐标系 $O_a$ 平移 ${\vec{t}}_{ac}={\vec{t}}_{ab}+R_{ab}\cdot {\vec{t}}_{bc}$ 且顺时针旋转 ${\theta }_{ac}={\theta }_{ab}+{\theta }_{bc}$ 而来。因此

$${\xi }_{ac}=\left[\begin{array}{c}{t}_{ac}^x\\ t_{ac}^y\\ {\theta }_{ac}\end{array}\right]=\left[\begin{array}{c}{t}_{ab}^x\\ t_{ab}^y\\ {\theta }_{ab}\end{array}\right]+\left[\begin{array}{ccc}cos{\theta }_{ab}& -sin{\theta }_{ab}& 0\\ sin{\theta }_{ab}& cos{\theta }_{ab}& 0\\ 0& 0& 1\end{array}\right]\cdot \left[\begin{array}{c}{t}_{bc}^x\\ t_{bc}^y\\ {\theta }_{bc}\end{array}\right].\text{\hspace{1em}(2.2)}$$

注意，${\vec{t}}_{ac}={\vec{t}}_{ab}+R_{ab}\cdot {\vec{t}}_{bc}$ 而不是 ${\vec{t}}_{ac}={\vec{t}}_{ab}+{\vec{t}}_{bc}$。这是因为 ${\vec{t}}_{bc}$ 是坐标系 $O_b$ 中的坐标，需要先使用 $R_{ab}$ 转换成坐标系 $O_a$ 中的坐标，再与 ${\vec{t}}_{ab}$ 相加。

  下面是 gmapping 实现公式 $(2.2)$ 的代码。

OrientedPoint lp = p;
lp.x += cos(p.theta) * m_laserPose.x - sin(p.theta) * m_laserPose.y;
lp.y += sin(p.theta) * m_laserPose.x + cos(p.theta) * m_laserPose.y;
lp.theta += m_laserPose.theta;

3. 使用 Eigen

---

  使用 Eigen 库实现二维欧式变换。

// 顺时针旋转 45 度。Eigen::Rotation2D<double> 等价于 Eigen::Rotation2Dd。
Eigen::Rotation2D<double> R = Eigen::Rotation2Dd(M_PI / 4);
cout << R.angle() << endl;	// 弧度值。
cout << R.matrix() << endl;	// 二阶旋转矩阵。
// 二维单位变换（三阶矩阵）。
Eigen::Isometry2d T;
T.setIdentity();
cout << T.matrix() << endl;
// 二维一般变换（三阶矩阵）。
T.rotate(R);
T.pretranslate(Eigen::Vector2d(2, 1));
cout << T.matrix() << endl;

4. 误差公式

---

4.1 SBA

---

4.2 SPA

---

  相对 SBA，论文《Efficient Sparse Pose Adjustment for 2D mapping》提出了 SPA。假设全局位姿（在世界坐标系中的位姿）是

$$c=[t,\theta ]=\left[\begin{array}{c}x\\ y\\ \theta \end{array}\right].$$

其中，$(x,y)$ 表示二维坐标系的平移量，$\theta$ 表示二维坐标系逆时针旋转的角度。

5. 参考

---

1. 二维旋转矩阵，CSDN，2020。