文章目录
ch3-主要内容推导 + 课后习题 [十四讲]
图片大都摘自网上大佬的。
主要目标：
三维空间刚体运动描述方式：旋转矩阵R、变换矩阵T、四元数q和欧拉角
掌握Eigen库的矩阵、几何模块的用法。
1. 主要内容：
1.1 点、向量和坐标系：
内 积 : a ∗ b = a T b = ∑ i = 1 3 a i b i = ∣ a ∣ ∣ b ∣ c o s < a , b > 外 积 : a × b = [ e 1 e 2 e 3 a 1 a 2 a 3 b 1 b 2 b 3 ] = [ a 2 b 3 − a 3 b 2 a 3 b 1 − a 1 b 3 a 1 b 2 − a 2 b 1 ] = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] b = a ∧ b 内积: a * b = a^Tb = \sum_{i=1}^3a_ib_i = |a||b|cos<a,b> \\ 外积: a\times b = \left[\begin{array}{cc} e_1 & e_2 & e_3 \\ a_1& a_2&a_3 \\ b_1&b_2&b_3 \end{array} \right] = \left[\begin{array}{cc} a_2b_3 - a_3b_2 \\ a_3b_1- a_1b_3 \\ a_1b_2-a_2b_1 \end{array} \right] =\left[\begin{array}{cc} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 &0 \end{array} \right]b=a^\wedge b 内积:a∗b=aTb=i=1∑3aibi=∣a∣∣b∣cos<a,b>外积:a×b=⎣⎡e1a1b1e2a2b2e3a3b3⎦⎤=⎣⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤b=a∧b
对外积（叉乘）这块，理解透彻，后面会经常用到。
^ 反对称符号，反对称： A T = − A A^T = -A AT=−A ,对角线元素为零， a i j = − a j i a_{ij} = -a_{ji} aij=−aji.
1.2 坐标间的欧式变换：
旋转矩阵R的引出:设某个正交基
(
e
1
,
e
2
,
e
3
)
(e_1,e_2,e_3)
(e1,e2,e3) 经过一次旋转变成
(
e
1
′
,
e
2
′
,
e
3
′
)
(e'_1 ,e'_2 ,e'_3)
(e1′,e2′,e3′) 。那么对于向量
a
a
a ，它在两个坐标系下的坐标为
[
a
1
,
a
2
,
a
3
]
T
[a_1,a_2,a_3]^T
[a1,a2,a3]T 和
[
a
1
′
,
a
2
′
,
a
3
′
]
T
[a'_1,a'_2,a'_3]^T
[a1′,a2′,a3′]T。所以坐标定义：
[
e
1
,
e
2
,
e
3
]
[
a
1
a
2
a
3
]
=
[
e
1
′
,
e
2
′
,
e
3
′
]
[
a
1
′
a
2
′
a
3
′
]
[e_1,e_2,e_3] \left[\begin{array}{cc} a_1\\a_2\\a_3 \end{array} \right] = [e'_1,e'_2,e'_3] \left[\begin{array}{cc} a'_1\\a'_2\\a'_3 \end{array} \right]
[e1,e2,e3]⎣⎡a1a2a3⎦⎤=[e1′,e2′,e3′]⎣⎡a1′a2′a3′⎦⎤
左右两边同乘
[
e
1
T
e
2
T
e
3
T
]
\left[\begin{array}{cc} e_1^T\\e_2^T\\e_3^T \end{array} \right]
⎣⎡e1Te2Te3T⎦⎤ , 【矩阵显示有问题，所以不得不公式写了】
[
a
1
a
2
a
3
]
=
[
e
1
T
e
1
′
e
1
T
e
2
′
e
1
T
e
3
′
e
2
T
e
1
′
e
2
T
e
2
′
e
2
T
e
3
′
e
3
T
e
1
′
e
3
T
e
2
′
e
3
T
e
3
′
]
[
a
1
′
a
2
′
a
3
′
]
=
R
a
′
\left[\begin{array}{cc} a_1\\a_2\\a_3 \end{array} \right] = \left[\begin{array}{cc} e_1^Te_1' &e_1^Te_2' &e_1^Te_3' \\e_2^Te_1' &e_2^Te_2' &e_2^Te_3'\\e_3^Te_1' &e_3^Te_2' &e_3^Te_3' \end{array} \right] \left[\begin{array}{cc} a'_1\\a'_2\\a'_3 \end{array} \right] = Ra'
⎣⎡a1a2a3⎦⎤=⎣⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦⎤⎣⎡a1′a2′a3′⎦⎤=Ra′
所以定义一个矩阵R，该矩阵由两组基之间的内积组成，描述了旋转前后坐标变换关系。
可以将n维旋转矩阵的集合定义如下：
S
O
(
n
)
=
{
R
∈
R
n
×
n
∣
R
R
T
=
I
,
d
e
t
(
R
)
=
1
}
SO(n) = \{R\in \mathbb{R}^{n\times n} |RR^T=I,det(R)=1\}
SO(n)={R∈Rn×n∣RRT=I,det(R)=1}
性质：1.正交 2. 行列式=1
矩
阵
正
交
：
A
A
T
=
I
;
A
T
=
A
−
1
;
A
的
列
向
量
两
两
正
交
，
长
度
为
1
A
的
行
向
量
两
两
正
交
，
长
度
为
1
矩阵正交：AA^T = I ; A^T=A^{-1}; \\A的列向量两两正交，长度为1 \\A的行向量两两正交，长度为1
矩阵正交：AAT=I;AT=A−1;A的列向量两两正交，长度为1A的行向量两两正交，长度为1
于是， 1到2的旋转可表达为：
a
1
=
R
12
a
2
反
之
：
a
2
=
R
21
a
1
矩
阵
关
系
：
R
21
=
R
12
−
1
=
R
12
T
a_1 = R_{12}a_2\\ 反之：a_2 =R_{21}a_1 \\ 矩阵关系：R_{21} = R_{12}^{-1}=R_{12}^T
a1=R12a2反之：a2=R21a1矩阵关系：R21=R12−1=R12T
1.3 变换矩阵T与齐次坐标
欧拉定理（ Euler’s rotation theorem）：刚体在三维空间里的一般运动，可分解为刚体上方某一点的平移，以及绕经过此点的旋转轴的转动。
如果用R，t来描述多次变换，则公式会拖很长，影响观看。 引入齐次坐标和变换矩阵T，以解决这个问题。
下面是从网上找的对两者的解释，可以借鉴参考理解。
S
E
(
3
)
=
{
T
=
[
R
t
0
T
1
]
∈
R
4
×
4
∣
R
∈
S
O
(
3
)
,
t
∈
R
3
}
SE(3) = \{ T =\left[\begin{array}{cc} R& t \\0^T&1\end{array} \right] \in \mathbb{R}^{4\times 4} | R\in SO(3), t\in \mathbb{R}^3 \}
SE(3)={T=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3}
注意：
T
12
T_{12}
T12表示2到1的变换。 默认使用符合运算法则的那一种。
1.5 旋转向量
其他的表示旋转的方式？旋转矩阵R用9个元素表达3个自由度，能否用更少的元素表达？
引出旋转向量：一种向量,方向-旋转轴，长度-转过的角度。 仅用三个量；且无无约束。
罗德里格斯公式[旋转向量->旋转矩阵]
R
=
c
o
s
θ
I
+
(
1
−
c
o
s
θ
)
n
n
T
+
s
i
n
θ
n
∧
R = cos\theta I + (1-cos\theta)nn^T + sin\theta n^\wedge
R=cosθI+(1−cosθ)nnT+sinθn∧
旋转矩阵->旋转向量
角
度
：
θ
=
a
r
c
c
o
s
(
t
r
(
R
)
−
1
2
)
轴
：
R
n
=
n
角度： \theta = arccos(\frac{tr(R)-1}{2}) \\ 轴： Rn = n
角度：θ=arccos(2tr(R)−1)轴：Rn=n
- 罗德里格斯证明：[参考wiki，网上大佬等]
图片摘自网上。
旋转前后表示：
-
旋转前： v = v ⊥ + v / / v = v_{\bot} + v_{//} v=v⊥+v//
-
旋转后： v ′ = v ⊥ ′ + v / / ′ v' = v'_{\bot} + v'_{//} v′=v⊥′+v//′
v ⊥ v_{\bot} v⊥ 表示v在正交与k轴上的投影 ； v / / v_{//} v//表示v在k轴上的投影 ； v ′ v' v′带撇表示浅蓝色部分，即旋转后的向量。 $v_{//} = (k\cdot v)k $ [括号里面的k,v为标量，外面的k为方向] , v ⊥ = v − v / / = v − ( k ⋅ v ) k v_{\bot} = v-v_{//}=v-(k \cdot v)k v⊥=v−v//=v−(k⋅v)k [点乘-表示v在k上的投影]
需要计算
v
′
v'
v′,即
v
⊥
′
,
v
/
/
′
v'_{\bot} , v'_{//}
v⊥′,v//′ 又因为
v
/
/
′
=
v
/
/
v'_{//} = v_{//}
v//′=v// ，所以只需求
v
⊥
v_{\bot}
v⊥
∵
w
=
k
×
v
⊥
=
k
×
v
[
方
向
−
右
手
定
则
]
v
⊥
′
=
v
⊥
w
′
+
v
⊥
v
⊥
′
=
sin
θ
w
+
cos
θ
v
⊥
=
sin
θ
(
k
×
v
)
+
cos
θ
v
⊥
\\ \because w = k \times v_{\bot} = k\times v [方向-右手定则] \\ v'_{\bot} =v'_{\bot w} + v'_{\bot v_{\bot}} \\ = \sin \theta w + \cos \theta v_{\bot} \\ = \sin \theta(k \times v) + \cos \theta v_{\bot}
∵w=k×v⊥=k×v[方向−右手定则]v⊥′=v⊥w′+v⊥v⊥′=sinθw+cosθv⊥=sinθ(k×v)+cosθv⊥
综上：
v
′
=
v
/
/
′
+
v
⊥
′
=
(
k
⋅
v
)
k
+
sin
θ
(
k
×
v
)
+
cos
θ
v
⊥
=
(
k
⋅
v
)
k
+
sin
θ
(
k
×
v
)
+
cos
θ
(
v
−
(
k
⋅
v
)
k
)
=
cos
θ
v
+
(
1
−
cos
θ
)
(
k
⋅
v
)
k
+
sin
θ
(
k
×
v
)
=
cos
θ
v
+
(
1
−
cos
θ
)
k
k
T
v
+
sin
θ
(
k
∧
v
)
等
价
于
R
=
c
o
s
θ
I
+
(
1
−
c
o
s
θ
)
n
n
T
+
s
i
n
θ
n
∧
v' = v'_{//} + v'_{\bot} \\ =(k\cdot v)k + \sin \theta(k \times v) + \cos \theta v_{\bot} \\ =(k\cdot v)k + \sin \theta(k \times v) + \cos \theta(v-(k \cdot v)k) \\ = \cos \theta v + (1 - \cos \theta)(k \cdot v)k + \sin \theta(k \times v) \\ = \cos \theta v + (1 - \cos \theta)kk^Tv + \sin \theta(k^\wedge v) \\等价于R = cos\theta I + (1-cos\theta)nn^T + sin\theta n^\wedge
v′=v//′+v⊥′=(k⋅v)k+sinθ(k×v)+cosθv⊥=(k⋅v)k+sinθ(k×v)+cosθ(v−(k⋅v)k)=cosθv+(1−cosθ)(k⋅v)k+sinθ(k×v)=cosθv+(1−cosθ)kkTv+sinθ(k∧v)等价于R=cosθI+(1−cosθ)nnT+sinθn∧
1.6 欧拉角[SLAM中用的少]
1.将旋转分解成三个方向上的转动。 eg：Z(偏航角yaw)-Y(俯仰角pitch)-X(滚转角roll) 。
2.轴可以使固定的，也可以是旋转的，顺序会有不同。
3.不同的领域习惯不同。
**问题：**万向锁:第一次绕Z旋转任意角度 ，当第二次绕Y旋转±90°时(X轴与初始Z轴在一条线上)，第三次旋转与第一次旋转使用同一个轴，即丢失一个自由度。
解决：[参考wiki] 1.使用四个万向节来克服 2. 检测到万向锁时，重置三个云台 3.避免使用欧拉角，改用四元数等。
1.7 四元数
1.7.1 四元数的定义：
- 二维： z = x + i y = r e i θ z=x+iy = r e^{i\theta} z=x+iy=reiθ ,i代表旋转90°，-i即-90°。
- 三维：四元数作为对复数的补充。 q = q 0 + q 1 i + q 2 j + q 3 k q = q_0+q_1 i+q_2j+q_3k q=q0+q1i+q2j+q3k
虚部满足以下关系：
i
2
=
j
2
=
k
2
=
−
1
[
复
数
运
算
]
i
j
=
k
,
j
i
=
−
k
j
k
=
i
,
k
j
=
−
i
k
i
=
j
,
i
j
=
−
j
[
类
似
叉
乘
]
i^2 = j^2 = k^2 = -1 [复数运算] \\ ij = k, ji = -k \\ jk=i,kj=-i \\ki=j,ij=-j \\ [类似叉乘]
i2=j2=k2=−1[复数运算]ij=k,ji=−kjk=i,kj=−iki=j,ij=−j[类似叉乘]
单位四元数能够表示旋转：
q
=
[
s
,
v
]
T
,
s
=
q
0
∈
R
,
v
=
[
q
1
,
q
2
,
q
3
]
T
∈
R
3
q = [s,v]^T, \ \ \ s=q_0\in \mathbb{R}, v=[q_1,q_2,q_3]^T\in \mathbb{R}^3
q=[s,v]T, s=q0∈R,v=[q1,q2,q3]T∈R3
s表示实部，v表示虚部
1.7.2 四元数间的运算
q
a
±
q
b
=
[
s
a
±
s
b
,
v
a
±
v
b
]
T
q
a
q
b
=
s
a
s
b
−
x
a
x
b
−
y
a
y
b
−
z
a
z
b
=
(
s
a
x
b
+
x
a
s
b
+
y
a
z
b
−
z
a
y
b
)
i
=
(
s
a
y
b
−
x
a
z
b
+
y
a
s
b
+
z
a
x
b
)
j
=
(
s
a
z
b
+
x
a
y
b
−
y
b
x
a
+
z
a
s
b
)
k
q
a
q
b
=
[
s
a
s
b
−
v
a
T
v
b
,
s
a
v
b
+
s
b
v
a
+
v
a
×
v
b
]
T
\mathbf{q_a ±q_b} = [s_a±sb, v_a±v_b]^T \\ \mathbf{q_aq_b} = s_as_b - x_ax_b-y_ay_b-z_az_b \\ = (s_ax_b + x_as_b+y_az_b-z_ay_b)i \\=(s_ay_b-x_az_b+y_as_b+z_ax_b)j \\=(s_az_b+x_ay_b-y_bx_a+z_as_b)k \\ \mathbf{q_aq_b} =[s_as_b-\mathbf{v_a}^T\mathbf{v_b}, s_a\mathbf{v_b}+s_b\mathbf{v_a} + \mathbf{v_a\times v_b}]^T
qa±qb=[sa±sb,va±vb]Tqaqb=sasb−xaxb−yayb−zazb=(saxb+xasb+yazb−zayb)i=(sayb−xazb+yasb+zaxb)j=(sazb+xayb−ybxa+zasb)kqaqb=[sasb−vaTvb,savb+sbva+va×vb]T
∣ ∣ q a ∣ ∣ = s a 2 + x a 2 + y z 2 + z a 2 q a ∗ = s a − x a i − y Z j − z a k = [ s a , − v a ] T q ∗ q = q q ∗ = [ s a 2 + v T v , 0 ] T q − 1 = q ∗ / ∣ ∣ q ∣ ∣ 2 q = [ k s , k v ] ||\mathbf{q_a}|| = \sqrt{s_a^2 + x_a^2 + y_z^2+z_a^2} \\ \mathbf{q_a}^* = s_a-x_ai-y_Zj-z_ak=[s_a,\mathbf{-v_a}]^T \\ \mathbf{q^*q} = \mathbf{qq^*} = [s_a^2+\mathbf{v^Tv},0]^T \\\mathbf{q}^{-1} = \mathbf{q}^* / ||\mathbf{q}||^2 \\ \mathbf{q} = [ks,k\mathbf{v}] ∣∣qa∣∣=sa2+xa2+yz2+za2qa∗=sa−xai−yZj−zak=[sa,−va]Tq∗q=qq∗=[sa2+vTv,0]Tq−1=q∗/∣∣q∣∣2q=[ks,kv]
q a ⋅ q b = s a s b + x a x b i + y a y b j + z a z b k \mathbf{q_a} \cdot \mathbf{q_b} = s_as_b + x_ax_bi + y_ay_bj + z_az_bk qa⋅qb=sasb+xaxbi+yaybj+zazbk
1.7.3 四元数与其他旋转表示的变换
- 角轴到四元数： q = [ cos θ 2 , n x sin θ 2 , n y sin θ 2 , n z sin θ 2 ] T q=[\cos \frac{\theta}2, n_x\sin \frac\theta2, n_y\sin \frac\theta2,n_z\sin \frac\theta2]^T q=[cos2θ,nxsin2θ,nysin2θ,nzsin2θ]T
- 四元数到角轴: { θ = 2 arccos q 0 [ n x , n y , n z ] T = [ q 1 , q 2 , q 3 ] T / sin θ 2 \left \{ \begin{array}{cc} \theta = 2\arccos q_0 \\ [n_x,n_y,n_z]^T = [q_1,q_2,q_3]^T/\sin \frac\theta2 \end{array} \right. {θ=2arccosq0[nx,ny,nz]T=[q1,q2,q3]T/sin2θ
1.7.4 用四元数表示旋转
设p经过一次以q表示的旋转后，得到了p‘ ，他们关系表达如下：
-
将p用四元数（虚四元数）表示： p = [ 0 , x , y , z ] T = [ 0 , v ] T \mathbf{p} = [0,x,y,z]^T = [0,\mathbf{v}]^T p=[0,x,y,z]T=[0,v]T
-
旋转后，的关系：(结果实部为0，把p‘虚部取出=旋转之后的坐标)
p ′ = q p q − 1 p' = qpq^{-1} p′=qpq−1
1.4 实践：Eigen
eigen矩阵的一些常见用法：
类型转换
// Eigen // Matlab // 注释
A.cast<double>(); // double(A) //变成双精度类型
A.cast<float>(); // single(A) //变成单精度类型
A.cast<int>(); // int32(A) //编程整型
A.real(); // real(A) //实部
A.imag(); // imag(A) //虚部
// 1.2 一些矩阵的基本运算
matrix_33 = Eigen::Matrix3d::Random();
std::cout << "random matrix: \n" << matrix_33 <<std::endl;
std::cout;std::cout << "transpose: \n" << matrix_33.transpose() << std::endl; // 转置
std::cout << "sum: " << matrix_33.sum() << std::endl; // 各元素和
std::cout << "trace: " << matrix_33.trace() << std::endl; // 迹
std::cout << "times 10: \n" << 10 * matrix_33 << std::endl; // 数乘
std::cout << "inverse: \n" << matrix_33.inverse() << std::endl; // 逆
std::cout << "det: " << matrix_33.determinant() << std::endl; // 行列式
// 特征值 : A^T*A 实对称矩阵
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solve(matrix_33.transpose() * matrix_33); // 计算矩阵的特征值和特征向量
std::cout << "Eigen values = \n" << eigen_solve.eigenvalues() << std::endl;
std::cout << "Eigen vectors = \n" << eigen_solve.eigenvectors() << std::endl;
// 求解方程
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
x = matrix_NN.ldlt().solve(v_Nd);
x = matrix_NN.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(v_Nd);
1.8 实践：Eigen几何模块
| 1.旋转矩阵 | 2.旋转向量 | 3.欧拉角 | 4.四元数 | |
|---|---|---|---|---|
| 1.旋转矩阵 | toRotationMatrix() | [3-1] | [4-1] | |
| 2.旋转向量 | [1-2] | |||
| 3.欧拉角 | rotation_matrix. eulerAngles(2,1,0);ZYX | |||
| 4.四元数 | Quaterniond(rotation_vector); |
Eigen::Matrix3d::Identity();// Identity()单位矩阵
std::cout.precision(3); //precision(n)精确度，到小数点后n位
T.rotate(rotation_matrix); //变换矩阵T- 旋转信息输入
T.pretranslate(Eigen::Vector3d(1,3,4));//变换矩阵T- 平移信息输入
Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);// w x y z，实部在前
q.coeffs().transpose() // coeffs()显示的是 x y z w ，即实部在后
// 下面两种方式等价。 q直接乘v即可(使用重载的乘法)
v_rotated = q * v; // qpq^{-1}
(q * Eigen::Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose()
以下参考网上大佬
1. 旋转向量
// 1.1 初始化
Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z))
// 1.2 旋转向量->旋转矩阵
Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.matrix()；//法1
rotation_matrix=rotation_vector.toRotationMatrix()；//法2
//1.3 旋转向量->旋转矩阵->欧拉角 ZYX
Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(2,1,0);
//1.4 旋转向量->四元数
Eigen::Quaterniond quaternion(rotation_vector);
2.旋转矩阵
//2.1 初始化
Eigen::Matrix3d rotation_matrix;
rotation_matrix<<x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22;
//2.2 旋转矩阵->旋转向量
Eigen::AngleAxisd rotation_vector(rotation_matrix); //法1
rotation_vector.fromRotationMatrix(rotation_matrix); //法2
//2.3 旋转矩阵->欧拉角ZYX
Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(2,1,0);
//2.4 旋转矩阵->四元数
Eigen::Quaterniond quaternion(rotation_matrix);
3.欧拉角
//3.1 初始化 ZYX
Eigen::Vector3d eulerAngle(2,1,0); //yaw,pitch,roll
//3.2 欧拉角->旋转向量
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));
Eigen::AngleAxisd rotation_vector;
rotation_vector=rollAngle*pitchAngle*yawAngle; // XYZ*x 等价于旋转顺序ZYX
//3.3 欧拉角->旋转矩阵
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));
Eigen::Matrix3d rotation_matrix;
rotation_matrix=rollAngle*pitchAngle*yawAngle; // XYZ*x 等价于旋转顺序ZYX
//3.4 欧拉角->四元数
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));
Eigen::Quaterniond quaternion;
quaternion=rollAngle*pitchAngle*yawAngle; // XYZ*x 等价于旋转顺序ZYX
4. 四元数
//4.1 初始化
Eigen::Quaterniond quaternion(w,x,y,z); //w实部在前
//4.2 四元数->旋转向量
Eigen::AngleAxisd rotation_vector(quaternion);
rotation_vector=quaternion;
// 4.3 四元数->旋转矩阵
Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.matrix(); //法1
Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.toRotationMatrix(); //法2
// 4.4 四元数->矩阵 -> 欧拉角ZYX
Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);
1.9 可视化演示
2.课后习题：
1.验证旋转矩阵是正交矩阵
(
e
1
,
e
2
,
e
3
)
(e_1 ,e_2 ,e_3)
(e1,e2,e3) 基底-行向量。
R
=
e
1
T
e
1
′
R
R
T
=
e
1
T
e
1
′
e
1
′
T
e
1
=
I
R = e_1^Te_1' \\ RR^T = e_1^Te_1'e_1'^Te_1 = I
R=e1Te1′RRT=e1Te1′e1′Te1=I
2. 罗德里格斯公式证明：
在主要内容中已经证明
3.验证四元数旋转
书中已给出证明。这里做下复述。
- 条件：定义两种符号,该符号将四元数映射成4x4的矩阵。
q + = [ s − v T v s I + v ∧ ] , q ⊕ = [ s − v T v s I − v ∧ ] q^+ = \left[\begin{array}{cc} s & \mathbf{-v}^T \\ \mathbf{v} & s\mathbf{I}+\mathbf{v}^\wedge \end{array}\right] , \ \ \ q^\oplus = \left[\begin{array}{cc} s & \mathbf{-v}^T \\ \mathbf{v} & s\mathbf{I}-\mathbf{v}^\wedge \end{array}\right] q+=[sv−vTsI+v∧], q⊕=[sv−vTsI−v∧]
于是，利用矩阵的形式可证明：
[
叉
乘
]
q
1
+
q
2
=
[
s
1
−
v
1
T
v
1
s
I
+
v
1
∧
]
[
s
2
v
2
]
=
[
−
v
1
T
v
2
+
s
1
s
2
s
1
v
2
+
s
2
v
1
+
v
1
∧
v
2
]
=
q
1
q
2
[
同
理
]
q
1
q
2
=
q
1
+
q
2
=
q
2
⊕
q
1
[叉乘]q_1^+q_2 =\left[\begin{array}{cc} s_1 & \mathbf{-v}_1^T \\ \mathbf{v}_1 & s\mathbf{I}+\mathbf{v}_1^\wedge \end{array}\right] \left[\begin{array}{cc} s_2\\ \mathbf{v}_2 \end{array}\right] = \left[\begin{array}{cc} -\mathbf{v}_1^T\mathbf{v}_2 + s_1s_2 \\ s_1\mathbf{v}_2 + s_2\mathbf{v}_1 + \mathbf{v}_1^\wedge\mathbf{v}_2 \end{array}\right] = q_1q_2 \\ [同理]q_1q_2 = q_1^+q_2 = q_2^\oplus q_1
[叉乘]q1+q2=[s1v1−v1TsI+v1∧][s2v2]=[−v1Tv2+s1s2s1v2+s2v1+v1∧v2]=q1q2[同理]q1q2=q1+q2=q2⊕q1
- 验证如下：
p ′ = q p q − 1 = q + p + q − 1 = q + q − 1 ⊕ p p' = qpq^{-1} = q^+p^+q^{-1} = q^+q^{-1\oplus}p p′=qpq−1=q+p+q−1=q+q−1⊕p
上面的符号代入：[注意：q为单位四元数时，
q
−
1
=
q
∗
q^{-1} = q^*
q−1=q∗ 其逆和共轭相等。
q
∗
=
[
s
,
−
v
]
T
\mathbf{q}^*=[s,\mathbf{-v}]^T
q∗=[s,−v]T || 模长：$s^2 + v^Tv = 1 $]
q
+
q
−
1
⊕
=
[
s
−
v
T
v
s
I
+
v
∧
]
[
s
v
T
−
v
s
I
+
v
∧
]
=
[
1
0
0
v
v
T
+
s
2
I
+
2
s
v
T
+
(
v
∧
)
2
]
q^+q^{-1\oplus} =\left[\begin{array}{cc} s & \mathbf{-v}^T \\ \mathbf{v} & s\mathbf{I}+\mathbf{v}^\wedge \end{array}\right] \left[\begin{array}{cc} s & \mathbf{v}^T \\ \mathbf{-v} & s\mathbf{I}+\mathbf{v}^\wedge \end{array}\right] = \left[\begin{array}{cc} 1 & 0 \\ 0 & \mathbf{vv}^T + s^2\mathbf{I} + 2s\mathbf{v}^T + (\mathbf{v^\wedge})^2 \end{array}\right]
q+q−1⊕=[sv−vTsI+v∧][s−vvTsI+v∧]=[100vvT+s2I+2svT+(v∧)2]
p是虚四元数，所以p’最终也是虚四元数。得证
p
′
=
q
p
q
−
1
=
q
+
q
−
1
⊕
p
=
[
1
0
0
v
v
T
+
s
2
I
+
2
s
v
T
+
(
v
∧
)
2
]
[
0
v
p
]
p' = qpq^{-1} =q^+q^{-1\oplus}p =\left[\begin{array}{cc} 1 & 0 \\ 0 & \mathbf{vv}^T + s^2\mathbf{I} + 2s\mathbf{v}^T + (\mathbf{v^\wedge})^2 \end{array}\right] \left[\begin{array}{cc} 0 \\ \mathbf{v}_p \end{array}\right]
p′=qpq−1=q+q−1⊕p=[100vvT+s2I+2svT+(v∧)2][0vp]
因为p和p’都是虚四元数，所以该矩阵右下角即四元数到旋转矩阵的变换关系。
4.画表总结旋转矩阵R，轴角，欧拉角，四元数转换关系。
默认由列到行的转换。|| 以旋转矩阵为“枢纽”，它们每个之间关系可以依次推出。
| 1.旋转矩阵 | 2.旋转向量 | 3.欧拉角 | 4.四元数 | |
|---|---|---|---|---|
| 1.旋转矩阵 | [2-1] | [3-1] | [4-1] | |
| 2.旋转向量 | [1-2] | |||
| 3.欧拉角 | [1-3] | |||
| 4.四元数 | [1-4] |
- [2-1] 罗德里格斯公式: R = c o s θ I + ( 1 − c o s θ ) n n T + s i n θ n ∧ R = cos\theta I + (1-cos\theta)nn^T + sin\theta n^\wedge R=cosθI+(1−cosθ)nnT+sinθn∧
- [1-2] 旋转向量为： 角 度 ： θ = a r c c o s ( t r ( R ) − 1 2 ) 轴 ： R n = n 角度： \theta = arccos(\frac{tr(R)-1}{2}) \\ 轴： Rn = n 角度：θ=arccos(2tr(R)−1)轴：Rn=n
- [3-1]欧拉角->旋转矩阵:按照ZYX的顺序，转角依次为 γ , β , α \gamma, \beta, \alpha γ,β,α .
R z ( γ ) = [ cos γ sin γ 0 − sin γ cos γ 0 0 0 1 ] R y ( β ) = [ cos β 0 − sin β 0 1 0 sin β 0 cos β ] R x ( α ) = [ 1 0 0 0 cos α sin α 0 − sin α cos α ] R_z(\gamma) = \left[\begin{array}{cc} \cos \gamma & \sin \gamma & 0 \\ -\sin \gamma & \cos \gamma & 0 \\ 0&0&1\end{array}\right] \\ R_y(\beta) = \left[\begin{array}{cc} \cos \beta & 0 & -\sin \beta \\ 0 & 1 & 0 \\ \sin \beta&0&\cos \beta\end{array}\right] \\ R_x(\alpha) = \left[\begin{array}{cc} 1 & 0 &0 \\ 0 & \cos \alpha & \sin \alpha \\ 0&-\sin \alpha &\cos \alpha \end{array}\right] Rz(γ)=⎣⎡cosγ−sinγ0sinγcosγ0001⎦⎤Ry(β)=⎣⎡cosβ0sinβ010−sinβ0cosβ⎦⎤Rx(α)=⎣⎡1000cosα−sinα0sinαcosα⎦⎤
x b = R x ( α ) R y ( β ) R z ( γ ) ⋅ x = [ cos β cos γ cos β sin γ − sin β − cos α sin γ + sin α sin β cos γ − cos α cos γ + sin α sin β sin γ sin α cos β sin α sin γ + cos α sin β cos γ − sin α cos γ + cos α sin β sin γ cos α cos β ] x = R ( α , β , γ ) x x_b =R_x(\alpha)R_y(\beta)R_z(\gamma)\cdot x \\ =\left[\begin{array}{cc} \cos \beta\cos\gamma & \cos \beta\sin\gamma & -\sin \beta \\ -\cos \alpha \sin \gamma +\sin\alpha \sin\beta \cos\gamma & -\cos \alpha \cos \gamma +\sin\alpha \sin\beta \sin\gamma & \sin\alpha \cos\beta \\ \sin\alpha \sin\gamma +\cos\alpha \sin\beta \cos\gamma & -\sin\alpha \cos\gamma +\cos\alpha \sin\beta \sin\gamma & \cos\alpha \cos\beta \end{array}\right]x \\=R(\alpha,\beta,\gamma) x xb=Rx(α)Ry(β)Rz(γ)⋅x=⎣⎡cosβcosγ−cosαsinγ+sinαsinβcosγsinαsinγ+cosαsinβcosγcosβsinγ−cosαcosγ+sinαsinβsinγ−sinαcosγ+cosαsinβsinγ−sinβsinαcosβcosαcosβ⎦⎤x=R(α,β,γ)x
- [1-3] 略
- [4-1] 旋转矩阵R 为: R = v v T + s 2 I + 2 s v T + ( v ∧ ) 2 R = \mathbf{vv}^T + s^2\mathbf{I} + 2s\mathbf{v}^T + (\mathbf{v^\wedge})^2 R=vvT+s2I+2svT+(v∧)2
- [1-4] 先[1-2]，后利用 q = [ cos θ 2 , n x sin θ 2 , n y sin θ 2 , n z sin θ 2 ] T q=[\cos \frac{\theta}2, n_x\sin \frac\theta2, n_y\sin \frac\theta2,n_z\sin \frac\theta2]^T q=[cos2θ,nxsin2θ,nysin2θ,nzsin2θ]T
5. Eigen矩阵取左上角元素
Eigen中矩阵的基本用法。
block块 等方式
6. 一般线性方程Ax=b有哪几种做法？Eigen的实现
Eigen中矩阵的基本用法:QR、cholesky、svd分解等方法。
568
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
