刚性运动 python

[视觉SLAM十四讲]学习笔记1-刚体运动之旋转矩阵与变换矩阵

• 1点、向量和坐标系
• 2 坐标系间的欧式变换

• 2.1 欧式变换之旋转
• 2.2 欧式变换之平移

• 3 变换矩阵与齐次坐标
• 4 Eigen库的简单应用

• 4.1 Eigen库的介绍与安装
• 4.2 Eigen库应矩阵运算实例代码

1点、向量和坐标系

• 刚体： 形状和大小不发生变化的物体被称为刚体，在我们生活的三维空间中，一个空间点的位置可以由3个坐标指定，而刚体不光有位置，还有自身的姿态(姿态是指物体的朝向)。
• 点： 点是空间中的基本元素，没有长度没有体积。
• 向量： 可以看成从某点指向另一点的箭头，它是空间中的一样东西，向量在坐标系中表示为坐标，同一向量在不同坐标系中的坐标不同。
• 坐标： 在一个三维线性空间中，我们可以找到一组基$(\bm{e}_1,\bm{e}_2,\bm{e}_3)$来表示任意向量$\bm{a}$在线性空间下的坐标，称为向量$\bm{a}$在这组基下的坐标：
  $\bm{a}= \begin{bmatrix} \bm{e}_1,\bm{e}_2,\bm{e}_3 \end{bmatrix} \begin{bmatrix} a_1 \\ a_2 \\ a_3 \end{bmatrix}=a_1\bm{e}_1+a_2\bm{e}_2+a_3\bm{e}_3.$
• 坐标系： 通常来说，坐标系由3个正交的坐标轴组成，当给定$x$和$y$轴后，通过右手(或左手)法则由$x{\times}y$定义出来$z$轴。根据定义方式不同可以分为左手系和右手系。右手系的定义如图所示：
  
  在大部分3D程序库中会使用右手系(如OpenGL、3D Max等)，也有部分库使用左手系(如Unity、Direct3D等)，左手系的第3个轴与右手系方向相反。
  相信阅读本教程的同学都学习过线性代数的知识，对于向量和向量之间及向量和数之间的运算都比较熟悉，这里数乘和加减法由于比较简单不做赘述，我们着重介绍一下内积(点乘)和外积(叉乘)。
• 内积： 表示为$a{\cdot}b$，在三维线性空间，定义为$a{\cdot}b=a^Tb=\sum_{i=1}^3a_ib_i=|a||b|cos\langle a,b\rangle$，$\langle a,b\rangle$指向量a，b的夹角。
• 外积： 表示为$a{\times}b$，$a{\times}b$定义为垂直于(正交)a和b的向量c ，方向由右手法则给出，大小等于向量所在的平行四边形的面积跨度。外积公式为：$a{\times}b=|a||b|sin({\theta})n$。
  θ是包含它们的平面中a和b之间的角度(因此，它介于 0° 和 180° 之间);
  |a|和|b|是向量a和b的大小;
  n是垂直于包含a和b的平面的单位向量，其方向使得有序集 (a,b,n)为正向的。
  下图为示意图
  
  外积的计算公式及形式有很多，感兴趣的同学可以点击文章末尾的链接进行学习，这里为了和视觉SLAM十四讲这本书中的例子相同，我们给出了下面的形式表示外积。
  
  标准基向量($i , j , k$，也表示为$e_1 , e_2 , e_3$)和$a$的向量分量($a_x , a_y , a_z$，也表示为$a_1 , a_2 , a_3$)
  每个向量都可以定义为平行于标准基向量的三个正交分量的总和：
  $a=a_1i+a_2j+a_3k,b=b_1i+b_2j+b_3k$它们的外积$a{\times}b$可以使用分配性展开：
  $a{\times}b=(a_1i+a_2j+a_3k){\times}(b_1i+b_2j+b_3k)$
  $=a_1b_1(i{\times}i)+a_1b_2(i{\times}j)+a_1b_3(i{\times}k)+a_2b_1(j{\times}i)+a_2b_2(j{\times}j)+a_2b_3(j{\times}k)+a_3b_1(k{\times}i)+a_3b_2(k{\times}j)+a_3b_3(k{\times}k)=a_1b_10+a_1b_2k-a_1b_3j-a_2b_1k+a_2b_20+a_2b_3i+a_3b_1j-a_3b_2i+a_3b_30$
  $=(a_2b_3-a_3b_2)i+(a_3b_1-a_1b_3)j+(a_1b_2-a_2b_1)k$
  则结果向量$s=s_1i+s_2j+s_3k=a{\times}b$的三个标量分量为$s_1=a_2b_3-a_3b_2,s_2=a_3b_1-a_1b_3,s_3=a_1b_2-a_2b_1$列向量形式为
  $\begin{bmatrix} s_1 \\ s_2 \\ s_3 \end{bmatrix}=\begin{bmatrix} a_2b_3-a_3b_2 \\ a_3b_1-a_1b_3 \\ a_1b_2-a_2b_1 \end{bmatrix}$这对应了书上形式的第二项。而书上第一项的形式为外积的行列式形式，
  $a{\times}b=\begin{vmatrix} i&j&k \\ a_1&a_2&a_3 \\ b_1&b_2&b_3 \end{vmatrix}=(a_2b_3i+a_3b_1j+a_1b_2k)-(a_3b_2i+a_1b_3j+a_2b_1k)$$=(a_2b_3-a_3b_2)i+(a_3b_1-a_1b_3)j+(a_1b_2-a_2b_1)k$可以看出，结果同第一种形式相同，所以书上的外积可以表示为：
  $a{\times}b=\begin{vmatrix} i&j&k \\ a_1&a_2&a_3 \\ b_1&b_2&b_3 \end{vmatrix}=\begin{bmatrix} a_2b_3-a_3b_2 \\ a_3b_1-a_1b_3 \\ a_1b_2-a_2b_1 \end{bmatrix}=\begin{bmatrix} 0&-a_3&a_2 \\ a_3&0&-a_1 \\ -a_2&a_1&0 \end{bmatrix} \xlongequal{\rm{def}} a^{\land}b.$通过上述等式，我们引入符号$^\land$，把$a$写成矩阵，$a^{\land}$实际上是一个反对称矩阵，可以把$a{\times}b$写成矩阵与向量的乘法$a^{\land}b$，变成线性运算。此符号是一个一一映射，意味着任意向量都对应着唯一的一个反对称矩阵，反之亦然：
  $a^{\land}=\begin{bmatrix} 0&-a_3&a_2 \\ a_3&0&-a_1 \\ -a_2&a_1&0 \end{bmatrix}$

2 坐标系间的欧式变换

书中给了我们这样的介绍： 我们经常在实际场景中定义各种各样的坐标系，如果考虑运动的机器人(即相机)，那么常见的做法是设定一个惯性坐标系(或者叫世界坐标系)，可以认为它是固定不动的。这时就会有这样的疑问：相机视野中某个向量$p$，它在相机坐标系下的坐标为$p_c$，而在世界坐标系下看其坐标为$p_w$，那么，这两个坐标之间是如何转换的呢？这时，需要先得到该点针对机器人坐标系的坐标值，再根据机器人位姿变换到世界坐标系中，可以通过数学手段的变换矩阵$T$来描述它。
下面给出刚体运动的定义

• 刚体运动： 两个坐标系之间的运动变换由一个旋转加上一个平移组成，这种运动就是刚体运动。
  相机运动就是一个刚体运动。刚体运动过程中，同一个向量在各个坐标系下的长度和夹角都不会发生变化。此时，我们说手机坐标系和世界坐标系之间，相差了一个欧氏变换(Euclidean Transform)。

2.1 欧式变换之旋转

我们假设某个单位正交基$(e_1,e_2,e_3)$经过一次旋转变成了$(e^{\prime}_1,e^{\prime}_2,e^{\prime}_3)$，这时，对于同一个向量$a$，它在两个基坐标系下的坐标分别为$[a_1,a_2,a_3]^T$和$[a^{\prime}_1,a^{\prime}_2,a^{\prime}_3]^T$。因为向量本身并没有发生变化，所以下列等式成立：
$[e_1,e_2,e_3]\begin{bmatrix}a_1\\a_2\\a_3\end{bmatrix}=[e^{\prime}_1,e^{\prime}_2,e^{\prime}_3]\begin{bmatrix}a^{\prime}_1\\a^{\prime}_2\\a^{\prime}_3\end{bmatrix}$
为了让两个坐标之间的关系看起来更清晰，我们将上述等式两侧同时左乘$[e^T_1,e^T_2,e^T_3]$,此时左侧第一项将变为单位矩阵：
$\begin{bmatrix}a_1\\a_2\\a_3\end{bmatrix}=\begin{bmatrix}e^T_1e^{\prime}_1&e^T_1e^{\prime}_2&e^T_1e^{\prime}_3\\ e^T_2e^{\prime}_1&e^T_2e^{\prime}_2&e^T_2e^{\prime}_3 \\ e^T_3e^{\prime}_1&e^T_3e^{\prime}_2&e^T_3e^{\prime}_3 \end{bmatrix}\begin{bmatrix}a^{\prime}_1\\a^{\prime}_2\\a^{\prime}_3\end{bmatrix}\xlongequal{\rm{def}}\bm{R}a^{\prime}$
矩阵$\bm{R}$描述了不同坐标系下同一向量的坐标变换关系。可以说，矩阵$\bm{R}$描述了旋转本身。所以矩阵$\bm{R}$称为旋转矩阵(Rotation Matrix)。显然，我们定义的矩阵$\bm{R}$是由两组基之间的内积组成，实际上是各基向量夹角的余弦值，所以我们也可以称矩阵$\bm{R}$为方向余弦矩阵(Direction Cosine Matrix)。
同时，我们可以看出，矩阵$\bm{R}$为正交矩阵，根据正交矩阵的性质，我们可以得到下面的关系：
$a^{\prime}=\bm{R}^{-1}a=\bm{R}^Ta$很明显，$\bm{R}^{-1}(\bm{R}^T)$刻画了一个相反的旋转。
除此之外，旋转矩阵也有一些特别的性质，我们通过矩阵$R$的行列式可以看出，它是一个行列式为1的正交矩阵。反过来说，行列式为1的正交矩阵也是一个旋转矩阵。
根据这些性质，我们可以推广到$n$维旋转矩阵，可以将$n$维旋转矩阵的集合定义如下：
$\rm{SO(n)}=\{\bm{R}\in \mathbb{R}^{n\times n}|\bm{RR}^{T}=I,\rm{det}(\bm{R})=1\}.$在这里，$SO(n)$表示的是特殊正交群(Special Orthogonal Group)的意思。通过上式我们可以看出，这个集合由$n$维空间的旋转矩阵组成，所以我们可以用$SO(3)$表示三维空间的旋转。我们之后可以通过旋转矩阵直接谈论两个坐标系之间的旋转变换，而不再通过基表述。

2.2 欧式变换之平移

在欧式变换中，我们可以通过直接在旋转后的坐标后加上平移向量，这可以非常简单的表示欧式变换中的平移。我们首先考虑一个世界坐标系中的向量$a$，经过一次旋转和一次平移后，得到了$a^{\prime}$，通过开头的描述，我们将旋转和平移合到一起，有$a^{\prime}=\bm{R}a+\bm{t}$这里的$\bm{t}$即为平移向量。我们可以通过上式用一个旋转矩阵$\bm{R}$和一个平移向量$\bm{t}$来完整的描述一个欧式空间的坐标变换。
这里我们通过一个例子，来规定一下角标。在现实中，我们会定义坐标系1和坐标系2，同时向量$a$在两个坐标系下的坐标分别为$a_1$和$a_2$，根据上式，他们之间的关系应该这样表示：$a_1=\bm{R}_{12}a_2+\bm{t}_{12}.$这里的$\bm{R}_{12}$是指“把坐标系2的向量变换到坐标系1”中，即为“从2到1的旋转矩阵”。由于向量乘在矩阵的右边(右乘)，所以它的下标是从右往左读的。而对于平移向量$\bm{t}_{12}$,它实际对应的是坐标系1原点指向坐标系2原点的向量，即在坐标系1下取的坐标，这里建议把它记作“从1到2的向量”，它的下标是从左读到右的，由于两坐标系旋转的关系，它并不等于$-\bm{t}_{12}$。

3 变换矩阵与齐次坐标

根据上一节的介绍，通过式子$a^{\prime}=\bm{R}a+\bm{t}$可以完整的表达欧式空间的旋转与平移。下面假设我们进行两次变换：$\bm{R}_1,\bm{t}_1$和$\bm{R}_2,\bm{t}_2$：$b=\bm{R}_1a+\bm{t}_1,c=\bm{R}_2b+\bm{t}_2$$c=\bm{R}_2(\bm{R}_1a+\bm{t}_1)+\bm{t}_2$这里我们可以明显看出，连续两次变换的关系并不是一个线性关系，这也是欧式变换的一个问题，这样的形式在变换多次后会变得非常啰嗦和复杂。
在这里，我们引入齐次坐标和变换矩阵的概念，对式$a^{\prime}=\bm{R}a+\bm{t}$进行重新表示

• 齐次坐标： 齐次坐标简单来说就是将一个原本是$n$维的向量用一个$n+1$维向量来表示。我们在一个三维向量$a^{\prime}$的末尾添加1，将其变为四维向量$\begin{bmatrix}a^{\prime} \\ 1\end{bmatrix}$，称为齐次坐标。
  关于齐次坐标引入的意义，可以参阅这三篇文章：
  关于齐次坐标的理解(经典)为什么要引入齐次坐标，齐次坐标的意义(一)为什么要引入齐次坐标，齐次坐标的意义(二)
• 变换矩阵： 通过引入齐次坐标，我们可以把旋转和平移写在一个矩阵里，使得整个关系变成线性关系：$\tilde{a}=\begin{bmatrix}a^{\prime} \\ 1\end{bmatrix}=\begin{bmatrix}\bm{R}a+\bm{t} \\ 1\end{bmatrix}=\begin{bmatrix}\bm{R}&\bm{t} \\ 0^T&1\end{bmatrix}\begin{bmatrix}a \\ 1\end{bmatrix}\xlongequal{\rm{def}}\bm{T}\begin{bmatrix}a \\ 1\end{bmatrix}.$
  在上式中，$\tilde{a}$为$a$的齐次坐标，矩阵$\bm{T}$称为变换矩阵(Transform Matrix)。
  通过齐次坐标和变换矩阵，上述两次变换的叠加就可以以下更好的形式：$\tilde{b}=\bm{T}_1\tilde{a},\tilde{c}=\bm{T}_2\tilde{b}\Rightarrow \bm{T}_2\bm{T}_1\tilde{a}$在视觉SLAM十四讲中，为了防止区分齐次和非齐次坐标的符号带来的复杂性和麻烦，在不引起歧义的情况下，以后直接把它写成$b=\bm{T}a$的样子，默认其中进行了齐次坐标的转换。
  对于变换矩阵$\bm{T}$，我们可以看出，矩阵的左上角是旋转矩阵$\bm{R}$，右上角是平移向量$\bm{t}$，左下角为0向量，右下角为1。我们称这种矩阵为特殊欧式群(Special Euclidean Group)，三维特殊欧式群形式如下：$\rm{SE(3)}=\Bigg\{\bm{T}=\begin{bmatrix}\bm{R}&\bm{t} \\ \bm{0}^T&1\end{bmatrix}\in \mathbb{R}^{4\times 4}|\bm{R}\in SO(3),\bm{t}\in \mathbb{R}^3\Bigg\}.$类似于特殊正交群$SO(3)$，该矩阵的逆矩阵表示的是一个反向的变换：$\bm{T}^{-1}=\begin{bmatrix}\bm{R}^T&-\bm{R}^T\bm{t} \\ \bm{0}^T&1\end{bmatrix}$注意： 不是常规的反向变换，由于是旋转+平移的变换，且平移在旋转后，受到旋转的影响。例如，对于方程"$y=ax+b$，若想进行平移，首先进行变形，$y=a(x+b/a)$，从而调整参数$b/a$而不是参数$a$"。
  在视觉SLAM十四讲中，同样用$T_{12}$的形式表示从2到1的变换，在后续的内容中，在不引起歧义的情况下，为了让符号看起来更简介，不再刻意区分其次坐标和普通坐标的符号，默认使用的符号符合运算法则。

4 Eigen库的简单应用

4.1 Eigen库的介绍与安装

Eigen是一个C++开源线性代数库，它提供了快速的有关矩阵的线性代数运算，还包括解方程等功能。许多上层的软件库也使用Eigen进行矩阵运算，包括g2o、Sophus等。Eigen库的优势在于，它是一个纯用头文件搭建起来的库，这意味着你只能找到它的头文件，而没有类似.so或.a的二进制文件。在使用时，只需引入头文件即可，不需要链接库文件。
通过Eigen官网教程，我们可以学习更多关于Eigen库的知识。
Eigen库需要我们自己进行安装才可以使用，我们可以通过在终端命令行输入以下指令来安装Eigen库。

sudo apt-get install libeigen3-dev

4.2 Eigen库应矩阵运算实例代码

示例代码地址：https://github.com/gaoxiang12/slambook2
我们可以通过以下指令，克隆SLAM十四讲的示例代码到个人电脑上

git clone https://github.com/gaoxiang12/slambook2.git

然后在终端输入下面的命令，打开对应的实例程序即可

cd slambook2/ch3/useEigen/
code .

下面是操作演示

这里给出使用Eigen库表示矩阵和向量并进行基本运算和调用的示例代码

#include <iostream>

using namespace std;

#include <ctime>
// Eigen 核心部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>

using namespace Eigen;

#define MATRIX_SIZE 50

/****************************
 * 本程序演示了 Eigen 基本类型的使用
 ****************************/

int main(int argc, char **argv)
{
    // Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列
    // 声明一个2*3的float矩阵
    Matrix<float, 2, 3> matrix_23;

    // 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix
    // 例如 Vector3d 实质上是 Eigen::Matrix<double, 3, 1>，即三维向量
    Vector3d v_3d;
    // 这是一样的
    Matrix<float, 3, 1> vd_3d;

    // Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>
    Matrix3d matrix_33 = Matrix3d::Zero(); // 初始化为零
    // 如果不确定矩阵大小，可以使用动态大小的矩阵
    Matrix<double, Dynamic, Dynamic> matrix_dynamic;
    // 更简单的
    MatrixXd matrix_x;
    // 这种类型还有很多，我们不一一列举

    // 下面是对Eigen阵的操作
    // 输入数据（初始化）
    matrix_23 << 1, 2, 3, 4, 5, 6;
    // 输出
    cout << "matrix 2x3 from 1 to 6: \n"
         << matrix_23 << endl;

    // 用()访问矩阵中的元素
    cout << "print matrix 2x3: " << endl;
    for (int i = 0; i < 2; i++)
    {
        for (int j = 0; j < 3; j++)
            cout << matrix_23(i, j) << "\t";
        cout << endl;
    }

    // 矩阵和向量相乘（实际上仍是矩阵和矩阵）
    v_3d << 3, 2, 1;
    vd_3d << 4, 5, 6;

    // 但是在Eigen里你不能混合两种不同类型的矩阵，像这样是错的
    // Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;
    // 应该显式转换
    Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
    cout << "[1,2,3;4,5,6]*[3,2,1]=" << result.transpose() << endl;

    Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;
    cout << "[1,2,3;4,5,6]*[4,5,6]: " << result2.transpose() << endl;

    // 同样你不能搞错矩阵的维度
    // 试着取消下面的注释，看看Eigen会报什么错
    // Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d;

    // 一些矩阵运算
    // 四则运算就不演示了，直接用+-*/即可。
    matrix_33 = Matrix3d::Random(); // 随机数矩阵
    cout << "random matrix: \n"
         << matrix_33 << endl;
    cout << "transpose: \n"
         << matrix_33.transpose() << endl;          // 转置
    cout << "sum: " << matrix_33.sum() << endl;     // 各元素和
    cout << "trace: " << matrix_33.trace() << endl; // 迹
    cout << "times 10: \n"
         << 10 * matrix_33 << endl; // 数乘
    cout << "inverse: \n"
         << matrix_33.inverse() << endl;                // 逆
    cout << "det: " << matrix_33.determinant() << endl; // 行列式

    // 特征值
    // 实对称矩阵可以保证对角化成功
    SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33);
    cout << "Eigen values = \n"
         << eigen_solver.eigenvalues() << endl;
    cout << "Eigen vectors = \n"
         << eigen_solver.eigenvectors() << endl;

    // 解方程
    // 我们求解 matrix_NN * x = v_Nd 这个方程
    // N的大小在前边的宏里定义，它由随机数生成
    // 直接求逆自然是最直接的，但是求逆运算量大

    Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
    matrix_NN = matrix_NN * matrix_NN.transpose(); // 保证半正定
    Matrix<double, MATRIX_SIZE, 1> v_Nd = MatrixXd::Random(MATRIX_SIZE, 1);

    clock_t time_stt = clock(); // 计时
    // 直接求逆
    Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;
    cout << "time of normal inverse is "
         << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;
    cout << "x = " << x.transpose() << endl;

    // 通常用矩阵分解来求，例如 QR 分解，速度会快很多
    time_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout << "time of Qr decomposition is "
         << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;
    cout << "x = " << x.transpose() << endl;

    // 对于正定矩阵，还可以用cholesky分解来解方程
    time_stt = clock();
    x = matrix_NN.ldlt().solve(v_Nd);
    cout << "time of ldlt decomposition is "
         << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;
    cout << "x = " << x.transpose() << endl;

    return 0;
}

CMakelists配置文件

cmake_minimum_required(VERSION 2.8) # 最低版本声明
project(useEigen) # 项目名称

set(CMAKE_BUILD_TYPE "Release")

# CMake预定义的内建变量，且他们是全局的。该变量可用于设置编译选项。直接使用set修改其值即可。
set(CMAKE_CXX_FLAGS "-O3") # '-O3'是一个优化选项，告诉编译器优化我们的代码。

# 添加Eigen头文件
# 方式一
# include_directories("/usr/include/eigen3")

# 方式二
find_package(Eigen3 REQUIRED)
include_directories( ${EIGEN3_INCLUDE_DIRS})

# 声明一个 C++ 可执行文件
add_executable(eigenMatrix eigenMatrix.cpp)

通过下面的指令进行程序的编译和执行

mkdir build
cd build
cmake ..
make
./eigenMatrix

下面是操作演示

程序结果如下：

matrix 2x3 from 1 to 6: 
1 2 3
4 5 6
print matrix 2x3: 
1       2       3
4       5       6
[1,2,3;4,5,6]*[3,2,1]=10 28
[1,2,3;4,5,6]*[4,5,6]: 32 77
random matrix: 
 0.680375   0.59688 -0.329554
-0.211234  0.823295  0.536459
 0.566198 -0.604897 -0.444451
transpose: 
 0.680375 -0.211234  0.566198
  0.59688  0.823295 -0.604897
-0.329554  0.536459 -0.444451
sum: 1.61307
trace: 1.05922
times 10: 
 6.80375   5.9688 -3.29554
-2.11234  8.23295  5.36459
 5.66198 -6.04897 -4.44451
inverse: 
-0.198521   2.22739    2.8357
  1.00605 -0.555135  -1.41603
 -1.62213   3.59308   3.28973
det: 0.208598
Eigen values = 
0.0242899
 0.992154
  1.80558
Eigen vectors = 
-0.549013 -0.735943  0.396198
 0.253452 -0.598296 -0.760134
-0.796459  0.316906 -0.514998
time of normal inverse is 0.262ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734
time of Qr decomposition is 0.05ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734
time of ldlt decomposition is 0.02ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734