视觉SLAM十四讲2nd—学习笔记（一）

3.1旋转矩阵

点、向量、坐标系

向量$a$：$a=e{a}_e={\Sigma }_1^3{a}_i{e}_i$，$e=[e_1,e_2,e_3]$为三维空间的一组基（是一个矩阵，每个元素$e_i$是一个列向量），$a_e$为$a$在这组基下的坐标（是一个列向量）。
内积：$a\cdot b=a_e^T{b}_e=\Sigma a_i{b}_i=|a||b|cos<a,b>$，向量的坐标与基有关，所以右小角有一个${\cdot }_e$，但向量的长度、夹角都与基无关，所以不加${\cdot }_e$，$a_i,b_i$分别是$a_e,b_e$的元素。
外积：$a\times b=det(\left[\begin{array}{c}e\\ a_e^T\\ b_e^T\end{array}\right])=\left[\begin{array}{c}{a}_2{b}_3-a_3{b}_2\\ a_3{b}_1-a_1{b}_3\\ a_1{b}_2-a_2{b}_1\end{array}\right]=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]b_e$，
$\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]$是一个由$a_e$改写成的反对称矩阵。
注：这么写有点麻烦，后面无特殊说明，$a,b$就指用同一组基表示的向量。
注：反对称矩阵满足$A^T=-A$

坐标系间的欧式变换

世界坐标系或惯性坐标系是一个静坐标系，相机坐标系是一个动坐标系。
刚体运动：如果相机运动是一个刚体运动，则两个坐标系间的运动由一个旋转加一个平移组成。或者说两个坐标系间相差一个欧式变换（Euclidean Transform）。

向量$a$在两个坐标系下的坐标为$a_e,a_{e^{\prime }}$，两个坐标系是经过一次旋转的关系，则有：
$a_e=R{a}_{e^{\prime }}$，其中$R=e^T{e}^{\prime }$称为旋转矩阵或方向余弦矩阵（direction cosine）。
旋转矩阵是行列式为1的正交矩阵；行列式为1的正交矩阵是旋转矩阵。
注：正交矩阵满足$A^T=A^{-1}$，则有$a_{e^{\prime }}=R^{-1}{a}_e$，矩阵求逆描述了反向旋转
注：行列式为-1的正交矩阵是瑕旋转矩阵（一次旋转加一次反射）。

$n$维旋转矩阵的集合：SO(n)={R∈Rn×n∣RRT=I,det(R)=I}SO(n) = \{R \in \mathbb R^{n\times n} | RR^T = I, det(R)=I \}SO(n)={R∈Rn×n∣RRT=I,det(R)=I}，$SO(n)$是特殊正交群的意思。

欧式变换：两个坐标系是一次旋转加一次平移的关系，$a_{e^{\prime }}=R{a}_e+t$，$t$为平移向量
注：将$a_{e^{\prime }}=R{a}_e+t$简写为$a^{\prime }=Ra+t$

变换矩阵与齐次坐标

如果向量$a$至$c$经过两次欧式变换，则有：$c=R_2(R_{1}a+t_1)+t_2$
为了简便表示，引入齐次坐标，将$a^{\prime }=Ra+t$表示为：
$\left[\begin{array}{c}{a}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]\left[\begin{array}{c}a\\ 1\end{array}\right]=T\left[\begin{array}{c}a\\ 1\end{array}\right]$
用变换矩阵$T$表示欧式变换，$a$用齐次坐标$\left[\begin{array}{c}a\\ 1\end{array}\right]$代替（为简便表示，$a$就是齐次坐标），则两次欧式变换可写作：$c=T_2{T}_{1}a$。

特殊欧式群：SE(3)={T=[Rt01]∈R4×4∣R∈SO(3),t∈R3}SE(3) = \{T = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix} \in \mathbb{R}^{4\times 4} | R \in SO(3), t\in \mathbb{R}^3 \}SE(3)={T=[R0​t1​]∈R4×4∣R∈SO(3),t∈R3}。
欧式变换的逆：$T^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}t\\ 0& 1\end{array}\right]$

3.3 旋转向量与欧拉角

旋转向量

旋转矩阵有9个分量，但一次旋转只有3个自由度，是否有更紧凑的表达？
答：罗德里格斯公式(Rodrigues)，用一个向量$n$和一个标量$\theta$表示

任意旋转都可以用一个旋转轴和一个旋转角表示，转轴是单位向量$n$，转角为$\theta$，则有：
$R=cos\theta I+(1-cos\theta )n{n}^T+sin\theta n^{ssm}$
其中，${\cdot }^{ssm}$表示向量的反对称矩阵
缺点：具有奇异性，$\theta =0$时，$R=I$，$n$可以是任意向量，即转轴不唯一。

由$R$计算$n,\theta$：
$\theta =arccos(\frac{tr(R)-1}{2})$
因为$Rn=n$，所以$n$是$R$的特征值为1对应的特征向量。

欧拉角

欧拉角：将一个旋转分解为三次绕不同轴的旋转，最常见的是采用偏航-俯仰-滚转描述旋转。
偏航-俯仰-滚转：绕Z轴旋转$\alpha$，绕旋转后的Y轴旋转$\beta$，绕旋转后的X轴旋转$\gamma$。
缺点：用三个标量表示旋转会出现奇异性问题（万向锁问题）。

万向锁：如果绕Y轴旋转90度，那么系统丢失一个自由度（绕X轴的旋转）。绕Z轴旋转$\alpha$再绕旋转后的Y轴旋转$90$再绕旋转后的X轴旋转$\beta$ 等于 绕Z轴旋转$\alpha \pm \beta$再绕旋转后的Y轴旋转$90$。
欧拉角演示网站 https://danceswithcode.net/engineeringnotes/rotations_in_3d/demo3D/rotations_in_3d_tool.html

3.4 四元数

四元数即紧凑也没有奇异性
平面旋转可由单位复数描述$e^{i\theta }=cos\theta +isin\theta$，三维旋转可由单位四元数描述。

四元数$q=[s,v{]}^T,s=q_0\in \mathbb{R},v=[q_1,q_2,q_3{]}^T\in {\mathbb{R}}^3$，四元数有三个虚部一个实部。
四元数的加减、乘、逆、数乘和复数类似。

用四元数描述旋转：$p^{\prime }=qp{q}^{-1}$，等效为$p^{\prime }=Rp$
其中，三维空间点$p=[0,x,y,z{]}^T$（用虚四元数表示），单位四元数$q$。
注：用四元数表示旋转，则$p$为虚四元数；用旋转矩阵表示旋转，则$p$为三维向量。需要读者带入当前语境中自行理解。

旋转即可用单位四元数$q=[s,v{]}^T$表示，也可用旋转矩阵表示，则前者转换为后者的公式为：
$R=v{v}^T+s^{2}I+2s{v}^{ssm}+(v^{ssm}{)}^2$
反过来的转换公式：
$\theta =2arccos(s),n=[q_1,q_2,q_3]/sin(\theta /2)$

3.5 相似、仿射、射影变换

相似变换比欧式变换多了一个自由度：缩放，变换矩阵为：
$T_S=\left[\begin{array}{cc}sR& t\\ 0& 1\end{array}\right]$
用$Sim(3)$表示三维相似变换的集合（相似变换群）

仿射变换（正交投影）仅要求$A$是一个可逆矩阵，不是必须为一个正交矩阵，变换矩阵为：
$T_A=\left[\begin{array}{cc}A& t\\ 0& 1\end{array}\right]$

射影变换是最一般的变换，变换矩阵为：
$T_A=\left[\begin{array}{cc}A& t\\ a^T& v\end{array}\right]$
其中，$A$是一个可逆矩阵，左下角为缩放，右上角为平移

真实世界到相机照片的变换，如果焦距有限是射影变换，焦距无限是仿射变换。

欧式变换：保持长度、夹角、体积，6自由度；
相似变换：保持体积比，7自由度；
仿射变换：保持平行性、体积比，12自由度；
射影变换：保持接触平面的相交和相切，15自由度。
注：欧式变换的旋转矩阵有正交性，将其自由度缩减为3，再加上平移向量，共6个自由度。
注：射影变换右下角的$v=0$或$1$，共15个自由度

Python代码

对矩形的变换

import matplotlib.pyplot as plt
import numpy as np

# 绘制矩阵
#rect_coords=[rect_coord1, rect_coord2, ...]
#rect_coord的一列为矩形的一个顶点的齐次坐标
def display_rect(rect_coords): 
    for rect_coord in rect_coords:
        markers = ['o', 's', '^', 'D']
        for i in range(4):
            plt.scatter(rect_coord[:, i][0], rect_coord[:, i][1], marker=markers[i], s=100)

            next_i = (i + 1) % 4
            plt.plot([rect_coord[0, i], rect_coord[0, next_i]],
                     [rect_coord[1, i], rect_coord[1, next_i]],
                     'k-')
    plt.show()

# 欧式变换
angle = 20 / 180 * np.pi
Euclid = np.array([[np.cos(angle), -np.sin(angle), 1],
                   [np.sin(angle), np.cos(angle), -1],
                   [0, 0, 1]])
# print(Euclid)

# 相似变换
scale = 0.5
Sim = Euclid.copy()
Sim[0:2, 0:2] = Euclid[0:2, 0:2] * scale

# 仿射变换
Affine = Euclid.copy()
Affine[0:2, 0:2] = np.array([[0.5, 0.3], [0, 2]])
# print(np.linalg.det(Affine[0:2,0:2])) #判断矩阵是否可逆

# 射影变换
Project = Affine.copy()
Project[2, :] = np.array([2, 0, 1])
# Project[2, :] = np.array([0, 2, 1])

# 矩形
rect_coord = np.array([[0, 0, 1, 1],
                       [0, 1, 1, 0],
                       [1, 1, 1, 1]]) #坐标按顺序排序 才可以正确画出矩形

## 显示1
# display_rect([rect_coord, Euclid @ rect_coord, Sim @ rect_coord])
## 显示2
# display_rect([rect_coord, Affine @ rect_coord])
# 显示3
proj_rect_coord = Project @ rect_coord
proj_rect_coord = proj_rect_coord/proj_rect_coord[2,:]
print(proj_rect_coord)
display_rect([rect_coord, Affine @ rect_coord, proj_rect_coord])

对图像的变换

from scipy import interpolate
from skimage import data
import matplotlib.pyplot as plt
import numpy as np

# 显示图像
def display(img):
    plt.imshow(img, cmap='gray')
    plt.axis('off')
    plt.subplots_adjust(0, 0, 1, 1)
    plt.tight_layout()
    plt.show()

# 对原图像坐标执行变换 #遍历的效率较低应该用矩阵乘法
def transform(X, Y, A):  # A为变换矩阵
    new_X, new_Y = X.copy(), Y.copy()
    for i in range(img.shape[1]):
        for j in range(img.shape[0]):
            new_coord = A @ np.array([X[i, j], Y[i, j], 1])
            new_X[i, j] = new_coord[0]
            new_Y[i, j] = new_coord[1]
    return new_X, new_Y

# 图像
img = data.camera()
display(img)

# 对图像执行变换
# 原图像坐标
x, y = (np.arange(img.shape[1]) - img.shape[1]/2 + 0.5,
        np.arange(img.shape[0]) - img.shape[0]/2 + 0.5)
Y, X = np.meshgrid(y, x)
xy = np.stack([X.ravel(), Y.ravel()]).T
# print(xy)

# 对图像执行变换
new_X, new_Y = transform(X, Y, Euclid)
# display(new_X)
new_img = interpolate.griddata(xy, img.ravel(),
                               (new_X.ravel(), new_Y.ravel()), method='linear')
new_img = new_img.reshape(img.shape)
display(new_img)