视觉SLAM十四讲笔记---03三维空间刚体运动1

1、旋转矩阵

（1）点和向量，坐标系

• 对于a,b∈R³
  • 内积
    
  • 外积（只对三维向量存在定义）
    
    外积的方向垂直于这两个向量，大小为|a||b|sin<a,b>

^ 记成一个反对称符号，则a^可表示把一个向量写成了一个反对称矩阵。可以用外积来表示旋转。

（2）坐标系间的欧式变换

• 问题描述
  世界坐标系：x_W，y_W，z_W
  相机坐标系：x_C，y_C，z_C
  相机视野中某一个向量p，在两个坐标系中的坐标分别为p_W，p_C
• 欧式变换 = 旋转R + 平移t
  考虑世界坐标系中的向量a，经过一次旋转R和一次平移t后，得到a’，则有：a’ = Ra + t
  • 旋转矩阵R
    旋转矩阵的引入：
    
    旋转矩阵特性：正交矩阵，行列式为1，证明如下：
    
    定义旋转矩阵的集合为SO(n)（即特殊正交群）：
    

（3）变换矩阵与齐次坐标

考虑到（2）中提到的变换是非线性的，故引入齐次坐标和变换矩阵T重写可有：

由此，可以用b=Ta的形式来表达旋转和平移。

• 齐次坐标
  在原有向量后添加最后一维增加一个自由度
  ==>向量的每个分量同乘非零常数k，表示的是同一个点
  ==>当最后一项不为0时，强制最后一下为1进行归一化（次此时忽略到最后一项即同欧式空间坐标一致）
• 变换矩阵T
  

两种表达形式均可，但默认在出现R或T时对应的坐标分别为非齐次和齐次。

（4）Eigen库的使用

• 安装eigen

sudo apt-get install libeigen3-dev

• 练习使用

CMakeLists.txt

#声明要求的cmake最低版本
cmake_minimum_required(VERSION 2.6)
#声明一个cmake工程
project(eigenmatrix)

#设置编译模式
set(CMAKE_BUILD_TYPE "Debug")
#设置C++11标准
#后面的-O3是用来调节编译时的优化程度的，最高为-O3，最低为-O0
set(CMAKE_CXX_FLAGS "-O3")

#添加Eigen头文件
include_directories("/usr/include/eigen3")
#添加一个可执行程序$add_executable(程序名 源代码文件）
add_executable(eigenmatrix main.cpp)

install(TARGETS eigenmatrix RUNTIME DESTINATION bin)

eigenMatrix.cpp

#include 
#include 
using namespace std;

#include 
#include 

#define MATRIX_SIZE 50

int main(int argc, char **argv) {
    /*****矩阵的声明*****/
    //声明一个2*3的float矩阵
    Eigen::Matrix<float,2,3>matrix_23;
    
    //声明一个3*1的double矩阵
    //方法1：
    Eigen::Matrix<double,3,1>matrix_31;
    //方法2：
    Eigen::Vector3d v_3d;
    
    //声明一个3*3的double矩阵
    //方法1：
    Eigen::Matrix<double,3,3>matrix_33;
    //方法2,并初始化为零：
    Eigen::Matrix3d matrix_33_1 = Eigen::Matrix3d::Zero();
    
    //声明一个动态矩阵
    //方法1：
    Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> matrix_dynamic;
    //方法2:
    Eigen::MatrixXd matrix_x;

    
    /*****矩阵的操作*****/
    //(1)输入数据
    matrix_23 << 1,2,3,4,5,6;
    //(2)输出数据
    cout << matrix_23 << endl;
    //(3)访问元素
    for(int i = 0; i < 2; i++)
      for(int j = 0; j < 3; j++)
 cout << matrix_23(i,j) << endl;
    //(4)矩阵和向量相乘(强制转换类型）
    v_3d << 3,2,1;
    Eigen::Matrix<double,2,1>result = matrix_23.cast<double>() * v_3d;
    cout << result << endl;
    
    /*****矩阵的运算*****/
    Eigen::Matrix<double,3,4> matrix_34;
    matrix_34 = Eigen::MatrixXd::Random(3,4);
    cout << matrix_34 << endl << endl;
    
    matrix_33 = Eigen::Matrix3d::Random();
    cout << matrix_33 << endl << endl;
    
    //(1)转置
    cout << matrix_34.transpose() << endl << endl;
    //(2)各元素和
    cout << matrix_34.sum() << endl << endl;
    //(3)迹
    cout << matrix_34.trace() << endl << endl;
    //(4)数乘
    cout << matrix_34 * 10 << endl << endl;
    //(5)逆(只有n×n的矩阵才有逆）
    cout << matrix_33.inverse() << endl << endl;
    //(6)行列式(只有n×n的矩阵才有行列式）
    cout << matrix_33.determinant() << endl << endl;
    
    //(7)特征值
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d>eigen_solver(matrix_33);
    cout << "Eigen values = " << eigen_solver.eigenvalues() << endl << endl;
    cout << "Eigen vectors = " << eigen_solver.eigenvectors() << endl << endl;
    
    //(8)解方程：matrix_NN * x = v_Nd
    Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN;
    matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
    Eigen::Matrix<double,MATRIX_SIZE,1> v_Nd;
    v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE,1);
    
    clock_t time_stt = clock();//计时
    //①直接求逆
    Eigen::Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse() * v_Nd;
    cout << "time use in normal inverse is " << 1000 * (clock() - time_stt)/(double)CLOCKS_PER_SEC << "ms" << endl << endl;
    //②利用矩阵分解，如QR分解
    time_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout << "time use in normal inverse is " << 1000 * (clock() - time_stt)/(double)CLOCKS_PER_SEC << "ms" << endl << endl;
    
    return 0;
}