[toc]

第二讲 三维空间的刚体运动

1、熟悉 Eigen 矩阵运算

设线性⽅程 ，在 A 为⽅阵的前提下，请回答以下问题：

1.1 在什么条件下，x 有解且唯⼀？

答案：非齐次线性方程组有唯一解的充要条件是 rank(A)=n。

1.2 ⾼斯消元法的原理是什么？

答案：最基本的那种求方程解的方法，就是对矩阵进行行变换。

1.3 QR 分解的原理是什么？

答案：在了解分解之前，先了解一下Gram-Schmidt正交化：

存在可逆矩阵的列向量组经过正交化之后可以得到：

把上述式子使用矩阵表示就可以得到下面的定义

对于物理意义，有时候不是那么重要，知道是这种数学表达就可以了。

定义：对于n阶方阵A，若存在正交矩阵Q和上三角矩阵R，使得，则该式称为矩阵A的完全QR分解或正交三角分解。(对于可逆矩阵A存在完全QR分解)。

基于我们熟悉的Gram-Schmidt正交化，令:

由此有若记，则此时。其中是由Gram-Schmidt正交化得到的标准的正交基。

1.4 Cholesky 分解的原理是什么？

可以阅读Cholesky分解维基百科获取更加详细的解释，下面只是为了便于理解而简单的进行说明。

直接先说Cholesky 分解是用于求解这个方程，然后具有等式：

从式子可以看到就和代数的平方一样的效果，常用在优化里面作为误差；另外这个分解方法的优点是提高代数运算效率（矩阵求逆）、蒙特卡罗方法等场合中十分有用。

其中是一个阶厄米特正定矩阵(Hermitian positive-definite matrix)，是下三角矩阵。下面介绍推导过程：

所以我们的目的是为了求矩阵，算法由开始，令：

在步骤中，矩阵的形式如下：

其中表示的共轭转置，若是实数矩阵， 就是转置；代表维的单位矩阵。此时定义为：

只有可以将矩阵改写为：

我们发现这个和我们熟悉的的特征值分解结构相似,接下来可以得到的形式：

需要注意的是这里的是一个外积。

重复此步骤，直到从1到。步之后，我们可以得到。因此下三角矩阵为：

1.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 )

{

    // 解方程
    // 我们求解 matrix_NN * x = v_Nd 这个方程
    // N的大小在前边的宏里定义，它由随机数生成
    // 直接求逆自然是最直接的，但是求逆运算量大
    
    cout <<"---解方程---"<<endl;
    clock_t time_stt = clock(); // 计时
    Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > matrix_Dy;
    Eigen::Matrix< double, Eigen::Dynamic,  1> v_Nd;
    Eigen::Matrix< double, Eigen::Dynamic,  1> x;
    matrix_Dy = Eigen::MatrixXd::Random( MATRIX_SIZE, MATRIX_SIZE );
    matrix_Dy=matrix_Dy.transpose()*matrix_Dy;       //乔利斯基分解需要正定矩阵
    
    // 求逆
    
    v_Nd = Eigen::MatrixXd::Random( MATRIX_SIZE, 1 );
    x = matrix_Dy.inverse()*v_Nd;
    cout<<x[0]<<endl;
    cout <<"time use in normal inverse is " << 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC << "ms"<< endl;


    // Cholesky 

    time_stt = clock();
    x = matrix_Dy.ldlt().solve(v_Nd);
    cout << x[0] << endl;
    cout <<"time use in Cholesky decomposition is " <<1000*  (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

    // QR/Lu
    
    time_stt = clock();
    x = matrix_Dy.colPivHouseholderQr().solve(v_Nd);
    cout << x[0] << endl;
    x = matrix_Dy.fullPivLu().solve(v_Nd);
    cout << x[0] << endl;
    cout <<"time use in Qr decomposition is " <<1000*  (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

    return 0;
}

2、几何运算练习

设有⼩萝⼘1⼀号和⼩萝⼘⼆号位于世界坐标系中。⼩萝⼘⼀号的位姿为： ,（ 的第⼀项为实部）。这⾥的 和表达的是 ，也就是世界到相机的变换关系。⼩萝⼘ ⼆号的位姿为 ,。现在，⼩萝⼘⼀号看到某个点在⾃⾝的坐 标系下，坐标为，求该向量在⼩萝⼘⼆号坐标系下的坐标。请编程实现此事，并提交 你的程序。

#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

int main ( int argc, char** argv )

{
    Eigen::Isometry3d Tcw1= Eigen::Isometry3d::Identity();                
    Eigen::Isometry3d Tcw2= Eigen::Isometry3d::Identity();              

    Eigen::Quaterniond q1(0.55,0.3,0.2,0.2);//定义四元数Q1
    Eigen::Quaterniond q2(-0.1,0.3,-0.7,0.2);

    cout<<"quaternion q1 = \n"<<q1.coeffs() <<endl;  // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部

    cout<<"quaternion q2 = \n"<<q2.coeffs() <<endl;   

    Eigen::Matrix<double, 3, 1> t1(0.7, 1.1, 0.2 );
    Eigen::Matrix<double, 3, 1> t2(-0.1, 0.4, 0.8 );

    Eigen::Matrix< double, 3, 1 > p1c(0.5, -0.1, 0.2);
    Eigen::Matrix< double, 3, 1 > p1w;
    Eigen::Matrix< double, 3, 1 > p2c;


    q1 = q1.normalized();
    q2 = q2.normalized();

    Tcw1.rotate ( q1.toRotationMatrix() );       // 按照rotation_vector进行旋转
    Tcw1.pretranslate (t1);                     // 平移向量
    cout << " Tcw1 Transform matrix = \n" << Tcw1.matrix() <<endl;

    Tcw2.rotate ( q2.toRotationMatrix() );     // 按照rotation_vector进行旋转
    Tcw2.pretranslate (t2);                     // 平移向量
    cout << "Tcw2 Transform matrix = \n" << Tcw2.matrix() <<endl;

    p1w = Tcw1.inverse()*p1c;
    p2c = Tcw2*p1w;
    cout << "p2c = \n" << p2c.matrix() <<endl;

    return 0;
}

3、旋转的表达

课程中提到了旋转可以⽤旋转矩阵、旋转向量与四元数表达，其中旋转矩阵与四元数是⽇常应⽤中常见的表达⽅式。请根据课件知识，完成下述内容的证明。

3.1 设有旋转矩阵 ，证明 且

3.1.1 正交矩阵性质：

• 正交矩阵的每一个列向量都是单位向量，且向量之间两两正交。
• 正交矩阵的行列式为1或者-1.
• (充要条件)

3.1.2 证明旋转矩阵是正交矩阵且行列式为1

首先我们令空间的一个位置经过旋转和一次平移之后得到新的位置:

对于每一个有：

需要注意的是旋转之后平移与平移之后旋转是不同的例如：

由于旋转矩阵可以对任意形式的（列）向量组和进行变换，这里我们考虑沿着轴变换的情况，也就是笛卡尔坐标系上进行变换：

因为一开始的（列）向量组是正交的，并且是单位向量；所以最后的结果一定也是正交的单位向量组，然后一起组成了旋转向量。

之后考虑的转置:

因为都是单位向量并且相互正交，也就是,因此：

由此可以得到 ，即证旋转矩阵是正交的。

对于行列式，我们知道矩阵的行列式是向量的混合积: 。这也表示以这些向量为边构成的平行六面体的体积。

<img src="https://i.loli.net/2020/04/03/ufa6XKNnlh7cxDI.png" alt="image-20200403184551535" />

由前面的结果可以得到是正交矩阵，因此行列式为 +1或者-1；

emmm 看了很多解释，我觉得都不是很清除[Rotations and rotation matrices][ 2 ]，所以还是来算一下吧，具体看下面三种形式的旋转矩阵：

image-20200403185756164

简单的计算一下，例如按照第一行展开：

3.2 设有四元数 ，我们把虚部记为，实部记为 ，那么 。请说明 和 的维度[gaoxiang-cnblogs][3]

3.3 四元数运算总结：

这里可以简单的扩展一下四元数相关的知识:

一个四元数可以写成如下形式：

其中：

这样子我们就可以得到四元数的一些性质：

对于四元数的运算（加法、乘法、共轭、模）有：

所以对于 是指四元数每个位置上的数值分别相乘经过一顿操作可以写成上面的式子的简洁形式注意与点乘区分。

其中：

4、罗德里格斯公式的证明

参考： https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula

罗德里格斯公式提供了一种算法，可以计算从（的李代数）到的指数映射，而无需实际计算完整矩阵指数。令然后是一个单位向量，描述了根据右手定则围绕其旋转角度θ的旋转轴，例如，那么旋转向量的罗德里格斯公式的形式如下：

首先，使用点积和叉积，向量可以分为与轴平行和垂直的分量：

其中与平行的分量为：

垂直于的分量是在上的向量投影：

向量可以看作是的副本，逆时针绕旋转了，因此它们的大小相等，但方向垂直。

同理可得，向量 是将的副本绕逆时针旋转到180°，因此和的大小相等，但方向相反，所以是负号。

展开向量三乘积（ vector triple product ）可建立平行分量和垂直分量之间的连接，给定任何三个向量公式的公式为 。

平行于轴的分量在旋转下不会改变大小或方向:

只有垂直分量会改变方向，但保持其大小:

由于 是平行的，因此他们的叉积为0：

旋转分量的形式类似于笛卡尔基础上二维平面极坐标中的径向矢量:

其中是其指示方向上的单位向量。

现在完整的旋转向量为：

通过将和的定义代入等式，得出:

参考链接：

• https://www.cnblogs.com/indulge-code/p/10492209.html "东电逸仙-cnblog"
• https://onlinelibrary.wiley.com/doi/pdf/10.1107/S0907444901012410 "Rotations and rotation matrices"
• https://www.cnblogs.com/gaoxiang12/p/5120175.html " gaoxiang-cnblogs"