视觉slam第三讲笔记：矩阵运算Eigen库

视觉SLAM第三讲笔记

一.数学符号说明

内积：$a\cdot b$
外积：$a\times b=\hat{a}b$，其中 $\hat{a}$为$a$的反对称矩阵表示
$$a\times b=\left(\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right)b$$ 旋转矩阵：单位正交基{$e_i$}经旋转为{$e_i^{\prime }$}，某向量原坐标$a$在新基下的坐标为$a^{\prime }$,则$$a=R{a}^{\prime }$$旋转矩阵$R$：标准正交单位矩阵， 反向旋转$R^T$

欧氏变换：旋转+平移：旋转矩阵$R$，平移向量$t$
$$a^{\prime }=Ra+t$$ 变换矩阵T：a–>b，b–>c，则a–>c?$b=R_{1}a+t_1$,$c=R_{2}b+t_2$,则$$c=R_2(R_{1}a+t_1)+t_2$$ 齐次坐标的作用：将旋转平移写在一个矩阵内，使整个关系为线性
所以上式变换可写作，变换矩阵形式$T$：$$\left(\begin{array}{c}{a}^{\prime }\\ 1\end{array}\right)=\left(\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right)\left(\begin{array}{c}a\\ 1\end{array}\right)=T\left(\begin{array}{c}a\\ 1\end{array}\right)$$
记为：$\tilde{b}=T_1\tilde{a},\tilde{c}=T_2\tilde{b}$,则
$$\tilde{c}=T_2{T}_1\tilde{a}$$

特殊正交群：旋转矩阵的集合：$SO(n)$= { $R\in {\mathbb{R}}^{n\times n}|R{R}^{{}^T}=I,det(R)=1$ }
特殊欧氏群：变换矩阵的集合：$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$}

二.Eigen矩阵线性代数运算

eigen:提供矩阵线性代数运算的库，只用头文件搭建，只需要引入头文件即可使用

cmake_minimum_required(VERSION 2.8)
project(threeeigen)
set(CMAKE_CXX_STANDARD 14)
add_executable(threeeigen main.cpp)
# 加上下一行就可以用eigen了
include_directories(/usr/include/eigen3)

#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>

using namespace std;

int main() {
    //2*3声明
    Eigen::Matrix<float, 2, 3> matrix_23;
    Eigen::Vector3d v_3d = Eigen::Vector3d::Random();
    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic;
    Eigen::MatrixXd matrix_x;


    matrix_23<<1,2,3,4,5,6;
    cout<< matrix_23<<endl;
    cout<< matrix_23(1,2)<<endl;
    cout<< matrix_23.transpose()<<endl;
    cout<< matrix_23.sum()<<endl;
    matrix_33 = Eigen::Matrix3d::Random();
    cout<< matrix_33.inverse()<<endl;
    cout<< matrix_33.determinant()<<endl;

    cout<< "特征运算"<<endl;
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(matrix_33.transpose()*matrix_33);
    cout<< eigen_solver.eigenvalues()<<endl;
    cout<< eigen_solver.eigenvectors()<<endl;

    cout<< "QR分解求逆"<<endl;
    Eigen::Matrix<double,3,1> x = matrix_33.colPivHouseholderQr().solve(v_3d);
    cout<< x<<endl;

    return 0;
}

三. clock计时

#include <ctime>
int main() {
    clock_t time_stt = clock();//start
    CLOCKS_PER_SEC<<"MS"<<ENDL;
    return 0;
}

四.旋转的多种表示方式

• 欧拉角和万向锁
• Rodrigues变换：从旋转向量到旋转矩阵的转换过程，推导见中文版《计算机视觉算法与应用》P34，以单位向量$n$为旋转轴做 $\theta$旋转，对应的旋转向量为 $\theta n$,则 Rodrigues变换为：
  $$R(n,\theta )=\cos \theta I+(1-\cos \theta )n{n}^T+\sin \theta \hat{n}$$

其中$\hat{n}$表示向量的叉积矩阵

• 四元数：一标量和一个三元向量表示
  旋转向量、旋转矩阵、四元数的转换关系，见书本P55

五.Eigen中的旋转变化演示

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

using namespace std;

int main() {
    //设旋转矩阵为单位矩阵
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    //设旋转向量绕Z轴转45度
    Eigen::AngleAxisd rotation_vector (M_PI/4, Eigen::Vector3d(0,0,1));
    cout.precision(3);
    //旋转向量->旋转矩阵
    cout<< "rotation_matrix\n:"<<rotation_vector.matrix()<<endl;

    rotation_matrix = rotation_vector.toRotationMatrix();
    cout<< "rotation_matrix\n:"<<rotation_matrix<<endl;

    //用AngleAxies旋转向量进行坐标变换
    Eigen::Vector3d v(1,0,0);
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout << "(1,0,0) after  angleaxies rotation:" << v_rotated.transpose()<<endl;

    //用旋转矩阵进行坐标变换
    v_rotated = rotation_matrix * v;
    cout << "(1,0,0) after  matrix rotation:" << v_rotated.transpose()<<endl;

    //旋转矩阵->欧拉角,210表示用ZYX顺序
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0);
    cout << "yaw pitch roll = "<<euler_angles.transpose()<<endl;

   //四元数，可以直接将AngleAxis旋转向量和旋转矩阵赋值给四元
    Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
    cout <<"quaternion = "<< q.coeffs()<<endl;//此处输出为x,y,z,w 即向量在前，标量在后
    q = Eigen::Quaterniond(rotation_matrix);
    cout <<"quaternion = "<< q.coeffs()<<endl;//此处输出为x,y,z,w 即向量在前，标量在后
    //使用四元数旋转向量
    v_rotated = q*v; //注意此处在数学上为q*v*q-1
    cout << "(1,0,0) after  quaternion rotation:" << v_rotated.transpose()<<endl;

    //放射和摄影变换：Eigen：：Affine3d Eigen::Projectived3d

    //欧氏变换矩阵T,设置旋转和平移
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); //注意此处实际是4-4为矩阵
    T.rotate(rotation_vector);
    T.pretranslate(Eigen::Vector3d(1,3,4));
    cout << "Transfor matrix = \n"<< T.matrix()<<endl;
    //用欧氏矩阵进行坐标变换
    Eigen::Vector3d v_transformed = T*v;  //注意此处进行了齐次和非其次变换两次，结果是R*V+T
    cout << v_transformed <<endl;

}

六.小结

旋转矩阵（33）：Eigen::Matrix3d
旋转向量（31）：Eigen::AngleAxisd
欧拉角（31）：Eigen::Vector3d
四元数（41）：Eigen::Quaterniond
欧氏变换（44）：Eigen::Isometry3d
仿射变换（44）：Eigen::Affine3d
射影变换（4*4）：Eigen::Projective3d

[1]: Xiang Gao, Tao Zhang, Yi Liu, Qinrui Yan, 14 Lectures on Visual SLAM: From Theory to Practice, Publishing House of Electronics Industry, 2017.

强烈安利《视觉slam十四讲》，这是极好的一本书