三维空间刚体运动
一、 Eigen 矩阵运算
Eigen 是常用的 C++ 矩阵运算库,具有很高的运算效率。大部分需要在 C++ 中使用矩阵运算的库,都会选用 Eigen 作为基本代数库,例如 Google Tensorflow,Google Ceres,GTSAM 等。本次习题,你需要使用 Eigen 库,编写程序,求解一个线性方程组。为此,你需要先 了解一些有关线性方程组数值解法的原理。 设线性方程 \(Ax = b\),在 A 为方阵的前提下,请回答以下问题:
1)在什么条件下,x 有解且唯一?
\(A\) 是可逆的, 有\(AA^{-1}=E\)
2)高斯消元法的原理是什么?
原理是通过逐次消元后,再回代求解。
3)QR 分解的原理是什么?
QR(正交三角)分解法是求一般矩阵全部特征值的最有效并广泛应用的方法,一般矩阵先经过正交相似变化成为Hessenberg矩阵,然后再应用QR方法求特征值和特征向量。它是将矩阵分解成一个正规正交矩阵Q与上三角形矩阵R,所以称为QR分解法,与此正规正交矩阵的通用符号Q有关。
4)Cholesky 分解的原理是什么?
Cholesky 分解是把一个对称正定的矩阵表示成一个下三角矩阵L和其转置的乘积的分解。它要求矩阵的所有特征值必须大于零,故分解的下三角的对角元也是大于零的。Cholesky分解法又称平方根法,是当A为实对称正定矩阵时,LU三角分解法的变形。
5)编程实现 A 为 100×100 随机矩阵时,用 QR 和 Cholesky 分解求 x 的程序。你可以参考本次课 用到的 useEigen 例程。
#include <iostream>
using namespace std;
#include <ctime>
// Eigen 部分
#include <Eigen/Core>
// 稠密矩阵的代数运算(逆,特征值等)
#include <Eigen/Dense>
#define MATRIX_SIZE 100
int main( int argc, char** argv )
{
Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> matrix_100;//100*100的动态矩阵
// 设置数据长度
matrix_100.conservativeResize(MATRIX_SIZE, MATRIX_SIZE);
matrix_100 = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);//随机产生
matrix_100 = matrix_100.transpose() * matrix_100;//产生对称矩阵
cout<<"det[matrix_100] = " << matrix_100.determinant() <<endl;//计算行列式
// 求解 matrix_NN * x = v_Nd 这个方程
Eigen::Matrix<double,Eigen::Dynamic,1> v_Nd;
v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE,1);
Eigen::Matrix<double,Eigen::Dynamic,1> x;
x = matrix_100.colPivHouseholderQr().solve(v_Nd);//QR分解的结果
cout<<"QR: x = " <<x<<endl;
x = matrix_100.llt().solve(v_Nd); //Cholesky分解的结果
cout<<"Cholesky: x= "<< x <<endl;
return 0;
}
运行结果:
det[matrix_100] = 1.02855e+108
QR: x = 311.894
-262.248
34.83
-421.497
...
Cholesky: x= 311.894
-262.248
34.83
-421.497
...
二、几何运算练习
下面我们来练习如何使用 Eigen/Geometry 计算一个具体的例子。 设有小萝卜一号和小萝卜二号位于世界坐标系中。小萝卜一号的位姿为:\(q_1 = [0.55,0.3,0.2,0.2]\), \(t_1 = [0.7,1.1,0.2]^T\)(q 的第一项为实部)。这里的 q 和 t 表达的是 \(T_{cw}\),也就是世界到相机的变换关系。小萝卜二号的位姿为 \(q_2 = [−0.1,0.3,−0.7,0.2]\),\(t_2 = [−0.1,0.4,0.8]^T\)。现在,小萝卜一号看到某个点在自身的坐标系下,坐标为 \(p_1 = [0.5,−0.1,0.2]^T\),求该向量在小萝卜二号坐标系下的坐标。请编程实现此事,并提交你的程序。
提示: 1. 四元数在使用前需要归一化。 2. 请注意 Eigen 在使用四元数时的虚部和实部顺序。 3. 参考答案为 \(p_2 = [1.08228,0.663509,0.686957]^T\)。你可以用它验证程序是否正确
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
using namespace std;
int main( int argc, char** argv ) {
//小萝卜一号的位姿
Eigen::Quaterniond q1(0.55,0.3,0.2,0.2);
q1 = q1.normalized();
Eigen::Matrix<double,3,1> t1;
t1<<0.7,1.1,0.2;
//小萝卜二号的位姿
Eigen::Quaterniond q2(-0.1,0.3,-0.7,0.2);
q2 = q2.normalized();
Eigen::Matrix<double,3,1> t2;
t2<<-0.1,0.4,0.8;
//小萝卜一号在自身的坐标系中看到某个点p1
Eigen::Matrix<double,3,1> p1;
p1<<0.5,-0.1,0.2;
Eigen::Matrix<double,3,1> pw, p2; //p_w为p点的世界坐标点,p_2为小萝卜二号坐标系下的坐标
Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();
T1.rotate(q1.toRotationMatrix());
T1.pretranslate(t1);
pw = T1.colPivHouseholderQr().solve(p1);//QR分解的结果
Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();
T2.rotate(q2.toRotationMatrix());
T2.pretranslate(t2);
p2 = T2*p1;
cout<<"小萝卜二号坐标系下的坐标为,"<<p2<<endl;
cout<<"参考答案为[1.08228,0.663509,0.686957]^T"<<endl;
return 0;
}
运行结果
小萝卜二号坐标系下的坐标为, -0.298413
-0.0936508
0.669841
参考答案为[1.08228,0.663509,0.686957]^T
三、旋转的表达
1.三维空间变换
\(F_0\) 的坐标系,上有一点\(a=[x,y,z]^T\),则当\(F_0\) 坐标系开始旋转变化时,有如下的变换关系
-
绕X轴逆时针旋转θ角
经过变换后的新空间坐标点为\(a'=[x,y,z]^TR_x(\theta)\) -
绕Y轴逆时针旋转θ角
-
绕Z轴逆时针旋转θ角
则旋转矩阵为\(M=R_x(\theta)R_y(\theta)R_z(\theta)\)
2. 旋转矩阵、旋转向量与四元数表达
课程中提到了旋转可以用旋转矩阵、旋转向量与四元数表达,其中旋转矩阵与四元数是日常应用中常见的表达方式。请根据课件知识,完成下述内容的证明。
1) 设有旋转矩阵 \(R\),证明 \(R^TR = I\) 且 \(det R = +I\)
设旋转矩阵 \(R\) 为 \(\begin{bmatrix} \cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{bmatrix}\) , 则 \(R^T = \begin{bmatrix} -\sin \theta& \cos \theta \\ \cos \theta & \sin \theta \end{bmatrix}\),
那么\(R^TR = I\), 且 \(det R = +I\)
2) 设有四元数 \(q\),我们把虚部记为\(ε\),实部记为 \(η\),那么 \(q = (ε,η)\)。请说明 \(ε\) 和 \(η\) 的维度。
因为四元数\(q\)可以表示为\(s+xi+yj+zk\), 其中,实部为 \(η = [s]\) 维度为\(1 \times 1\); 而虚部为\(ε =[x,y,z]^T\) 维度为\(3 \times 1\) .
3) 定义运算 + 和 ⊕ 为:
\begin{equation} q+ =\begin{bmatrix}
ηI + ε^× & ε \
−ε^T & η
\end{bmatrix}\end{equation}
\begin{equation}q⊕ =\begin{bmatrix}
ηI - ε^× & ε \
−ε^T & η
\end{bmatrix}\end{equation}
其中运算 \(×\) 含义与 \(∧\) 相同,即取 \(ε\) 的反对称矩阵(它们都成叉积的矩阵运算形式),$ I$ 为单位矩 阵。请证明对任意单位四元数 \(q_1\), \(q_2\),四元数乘法可写成矩阵乘法:\(q_1q_2 = {q_1}^+ q_2\) 或者 \(q_1q_2 = {q_2}^⊕q_1\)
四、罗德里格斯公式的证明
罗德里格斯公式描述了从旋转向量到旋转矩阵的转换关系。设旋转向量长度为 \(θ\),方向为 \(n\),那么旋 转矩阵 \(R\) 为:
\begin{equation}R = \cos θI + (1−\cosθ)nn^T +\sinθn^∧\end{equation}
1)我们在课程中仅指出了该式成立,但没有给出证明。请你证明此式。提示:[参考](https://en.wikipedia. org/wiki/Rodrigues%27_rotation_formula)。
2)请使用此式证明 \(R^{−1} = R^T\)。
1)证:
由李群李代数可知\(R = exp(\phi^∧)\),其中\(\phi\) 是三维向量,我们可以定义它的模长为\(a\),方向为\(\theta\), 于是有 \(\phi = \theta a\), 这里\(a^∧\)有两个性质,分别为 \(a^∧a^∧ = aa^T - I\) 和 \(a^∧a^∧a^∧ = -a^∧\)。
根据泰勒展开有
\(exp(\phi ^{\wedge }) = exp(\theta a ^{\wedge}) = \sum^{\infty }_{n=0}(\theta a ^{\wedge})^n \\ \qquad \quad \; = I + \theta a ^{\wedge} + \frac{1}{2!}\theta ^{2} a^{\wedge}a^{\wedge}+\frac{1}{3!}\theta ^{3} (a^{\wedge})^3+\frac{1}{4!}\theta ^{4} (a^{\wedge})^4 + \cdots \\ \qquad \quad \; = aa^T - a^{\wedge}a^{\wedge}+ \theta a^{\wedge}+\frac{1}{2!}\theta ^{2} a^{\wedge}a^{\wedge} - \frac{1}{3!}\theta ^{3} a^{\wedge}- \frac{1}{4!}\theta ^{4} (a^{\wedge})^2+ \cdots \\ \qquad \quad \; = aa^T + (\theta - \frac{1}{3!}\theta^3 + \frac{1}{5!}\theta^5 - \cdots)a^{\wedge}-(1-\frac{1}{2!}\theta^2 + \frac{1}{4!}\theta^4 - \cdots)a^{\wedge}a^{\wedge} \\ \qquad \quad \; = a^{\wedge}a^{\wedge}+ I + \sin \theta a^{\wedge}- \cos \theta a^{\wedge}a^{\wedge} \\ \qquad \quad \; = (1- \cos \theta)a^{\wedge}a^{\wedge} + I + \sin \theta a^{\wedge} \\ \qquad \quad \; = \cos \theta I + (1-\cos \theta)aa^T + \sin \theta a^{\wedge}\)
则 \(exp(\phi^∧) = \cos θI + (1−\cosθ)aa^T +\sinθa^∧\),
所以 \(R = \cos θI + (1−\cosθ)nn^T +\sinθn^∧\) 得证。
2) 假设某个单位正交基 \((e_1,e_2,e_3)\) 经过一次旋转变成了 \((e_1',e_2', e_3')\) ,则对同一个向量\(\overrightarrow{a}\),在两个坐标系下的坐标分别为\([a_1,a_2,a_3]^T\)和\([a_1', a_2', a_3']^T\) ,则有:
\begin{equation} (e_1,e_2,e_3)[a_1,a_2,a_3]^T = (e_1',e_2', e_3')[a_1', a_2', a_3']^T\end{equation}
对上述等式的左右两边同时左乘\([e_1^T, e_2^T, e_3^T]^T\),则左边的系数就化为单位矩阵,即
\(\begin{bmatrix} a_1\\ a_2\\ a_3 \end{bmatrix} = \begin{bmatrix} 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{bmatrix}\begin{bmatrix} a_1'\\ a_2'\\ a_3' \end{bmatrix}= Ra'\)
所以可得 \(a'=R^{-1}a=R^Ta\), \(R^{−1} = R^T\) 得证。
五、四元数运算性质的验证
课程中介绍了单位四元数可以表达旋转。其中,在谈论用四元数 \(q\) 旋转点 \(p\) 时,结果为:
\(p′ = qpq−1\) .
我们说,此时 \(p′\) 必定为虚四元数(实部为零)。请你验证上述说法。
此外,上式亦可写成矩阵运算:\(p′ = Qp\)。请根据你的推导,给出矩阵 \(Q\)。注意此时 \(p\) 和 \(p′\) 都是四元数形式的变量,所以 \(Q\) 为 4×4 的矩阵。
提示:如果使用第 3 题结果,那么有:
\(p^′ = qpq^{−1} = q^{+}p^{+}q^{−1} = q^{+}q^{−1\bigoplus}p\)
从而可以导出四元数至旋转矩阵的转换方式:
\(R = Im(q^+q^{−1\bigoplus })\)
证:
假设
\(q =\begin{bmatrix} ε \\ η \end{bmatrix}\) , \(q^{-1} =\begin{bmatrix} -ε \\ η \end{bmatrix}\) , 令 \(p =\begin{bmatrix} r_b \\ 0 \end{bmatrix}\), \(p^{-1} =\begin{bmatrix} r_b^‘ \\ 0 \end{bmatrix}\)
\(p′ = qpq^{−1} = q^+p^+q^{−1} = q^+q^{−1⊕}p \\ \ \ \ =\begin{bmatrix} ηI + ε^× & ε \\ −εT & η \end{bmatrix}\begin{bmatrix} ηI - ε^× & ε \\ −ε^T & η \end{bmatrix}\begin{bmatrix} r_b \\ 0 \end{bmatrix} \\ \ \ \ =\begin{bmatrix} -(ηI - ε^×)^2 & 0 \\ 0 & -1 \end{bmatrix}\begin{bmatrix} r_b \\ 0 \end{bmatrix}\)
\(r_b' = (-(ηI - ε^×)^2 - εε^T)r_b\)
\(R = -(ηI - ε^×)^2 - εε^T\)
六、 熟悉 C++11
C++ 是一门古老的语言,但它的标准至今仍在不断发展。在 2011 年、2014 年和 2017 年,C++ 的标 准又进行了更新,被称为 C++11,C++14,C++17。其中,C++11 标准是最重要的一次更新,让 C++ 发生了重要的改变,也使得近年来的 C++ 程序与你在课本上(比如谭浩强)学到的 C++ 程序有很大的 不同。你甚至会惊叹这是一种全新的语言。C++14 和 C++17 则是对 11 标准的完善与扩充。
越来越多的程序开始使用 11 标准,它也会让你在写程序时更加得心应手。本题中,你将学习一些 11 标准下的新语法。请参考本次作业 books/目录下的两个 pdf,并回答下面的问题。
设有类 A,并有 A 类的一组对象,组成了一个 vector。现在希望对这个 vector 进行排序,但排序的 方式由 A.index 成员大小定义。那么,在 C++11 的语法下,程序写成: