时间戳

首页 > 技术文章 > 三维空间刚体运动

三维空间刚体运动

yujingxiang 2021-02-28 15:36 原文

一、 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 })\)

其中 $Im$ 指取出虚部的内容。

证：

假设

\(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 的语法下，程序写成：

推荐阅读