二维刚体变换
欢迎访问我的博客首页。
二维刚体变换
- 1. 二维旋转矩阵
-
- 1.1 向量旋转
- 1.2 坐标系旋转
- 1.3 使用 Eigen
- 2. 误差公式
-
- 2.1 SBA
- 2.2 SPA
- 3. 参考
1. 二维旋转矩阵
1.1 向量旋转
二维世界坐标系中任一向量 O P = ( x , y ) OP = (x, y) OP=(x,y) 绕原点逆时针旋转 θ \theta θ 度成为向量 O P ′ = ( x ′ , y ′ ) OP' = (x', y') OP′=(x′,y′)，这个旋转可以用一个二维矩阵表示
R 逆 向量 = [ c o s θ − s i n θ s i n θ c o s θ ] . (1.1.1) R_逆^{向量} = \begin{bmatrix} cos \theta & -sin \theta \\ sin \theta & cos \theta \end{bmatrix}. \tag{1.1.1} R逆向量=[cosθsinθ−sinθcosθ].(1.1.1)
证明：因为只考虑旋转不考虑平移，所以点 P P P 到原点的距离等于点 P ′ P' P′ 到原点的距离，我们设为 r r r。假设直线 O P OP OP 与 x x x 轴的夹角是 ϕ \phi ϕ。那么
{ x = r c o s ϕ y = r s i n ϕ (1.1.2) \left\{\begin{aligned} x &= r cos \phi \\ y &= r sin \phi \end{aligned}\right. \tag{1.1.2} {xy=rcosϕ=rsinϕ(1.1.2)
{ x ′ = r c o s ( ϕ + θ ) = r c o s ϕ c o s θ − r s i n ϕ s i n θ y ′ = r s i n ( ϕ + θ ) = r s i n ϕ c o s θ + r c o s ϕ s i n θ (1.1.3) \left\{\begin{aligned} x' &= r cos (\phi + \theta) = r cos \phi cos \theta - r sin \phi sin \theta\\ y' &= r sin (\phi + \theta) = r sin \phi cos \theta + r cos \phi sin \theta \end{aligned}\right. \tag{1.1.3} {x′y′=rcos(ϕ+θ)=rcosϕcosθ−rsinϕsinθ=rsin(ϕ+θ)=rsinϕcosθ+rcosϕsinθ(1.1.3)
把公式 (1.1.2) 代入公式 (1.1.3) 得
{ x ′ = x c o s θ − y s i n θ y ′ = y c o s θ + x s i n θ . (1.1.4a) \left\{\begin{aligned} x' &= x cos \theta - y sin \theta\\ y' &= y cos \theta + x sin \theta \end{aligned}\right.. \tag{1.1.4a} {x′y′=xcosθ−ysinθ=ycosθ+xsinθ.(1.1.4a)
把公式 (1.1.4a) 写成矩阵形式是
[ x ′ y ′ ] = [ c o s θ − s i n θ s i n θ c o s θ ] [ x y ] . (1.1.4b) \begin{bmatrix} x' \\ y' \end{bmatrix}= \begin{bmatrix} cos \theta & -sin \theta \\ sin \theta & cos \theta \end{bmatrix} \begin{bmatrix} x \\ y \end{bmatrix}. \tag{1.1.4b} [x′y′]=[cosθsinθ−sinθcosθ][xy].(1.1.4b)
把公式 (1.1.1) 中的 θ \theta θ 换成 − θ -\theta −θ 就可以得到顺时针旋转 θ \theta θ 度的旋转矩阵 R 顺 向量 R_顺^{向量} R顺向量。很容易验证， R 逆 向量 R_逆^{向量} R逆向量 和 R 顺 向量 R_顺^{向量} R顺向量 都是行列式为 1 的二阶矩阵，且互为逆矩阵。
1.2 坐标系旋转
更常见的情况是，求向量在旋转后的坐标系中的坐标。这种情况和向量旋转正好相反，即
{ R 顺 坐标系 = R 逆 向量 = [ c o s θ − s i n θ s i n θ c o s θ ] R 逆 坐标系 = R 顺 向量 = [ c o s θ s i n θ − s i n θ c o s θ ] . (1.2.1) \left\{\begin{aligned} R_顺^{坐标系} &= R_逆^{向量} = \begin{bmatrix} cos \theta & -sin \theta \\ sin \theta & cos \theta \end{bmatrix} \\ R_逆^{坐标系} &= R_顺^{向量} = \begin{bmatrix} cos \theta & sin \theta \\ -sin \theta & cos \theta \end{bmatrix} \end{aligned}\right.. \tag{1.2.1} ⎩ ⎨ ⎧R顺坐标系R逆坐标系=R逆向量=[cosθsinθ−sinθcosθ]=R顺向量=[cosθ−sinθsinθcosθ].(1.2.1)
如图 1 右图，坐标系 x ′ O y ′ x' O y' x′Oy′ 由坐标系 x O y x O y xOy 逆时针旋转 45° 得到。则 x O y x O y xOy 中的点 P ( 1 , 2 ) P(1, 2) P(1,2) 在 x ′ O y ′ x' O y' x′Oy′ 中的坐标
P ′ = R 逆 坐标系 P = [ c o s π 4 s i n π 4 − s i n π 4 c o s π 4 ] [ 1 2 ] = [ 3 2 1 2 ] . P' = R_逆^{坐标系} P = \begin{bmatrix} cos \frac{\pi}{4} & sin \frac{\pi}{4} \\ -sin \frac{\pi}{4} & cos \frac{\pi}{4} \end{bmatrix} \begin{bmatrix} 1 \\ 2 \end{bmatrix} = \begin{bmatrix} \frac{3}{\sqrt{2}} \\ \frac{1}{\sqrt{2}} \end{bmatrix}. P′=R逆坐标系P=[cos4π−sin4πsin4πcos4π][12]=[2321].
1.3 使用 Eigen
使用 Eigen 库实现二维欧式变换。
// 顺时针旋转 45 度。Eigen::Rotation2D 等价于 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;
2. 误差公式
2.1 SBA
2.2 SPA
相对 SBA，论文《Efficient Sparse Pose Adjustment for 2D mapping》提出了 SPA。假设全局位姿（在世界坐标系中的位姿）是
c = [ t , θ ] = [ x y θ ] . c = [t, \theta] = \begin{bmatrix} x \\ y \\ \theta \end{bmatrix}. c=[t,θ]= xyθ .
其中， ( x , y ) (x, y) (x,y) 表示二维坐标系的平移量， θ \theta θ 表示二维坐标系逆时针旋转的角度。
3. 参考
- 二维旋转矩阵，CSDN，2020。