视觉SLAM十四讲 第3讲 三维空间刚体运动

主要目标:
1、理解三维空间的刚体运动描述方式：旋转矩阵、变换矩阵、四元数和欧拉角
2、掌握eigen库的矩阵、几何模块使用方法

3.1 旋转矩阵

3.1.1 内外积与左右手坐标系

（1）$内积$：描述向量间的投影关系。

（2）$外积$：可以表示向量的旋转。

(3) $坐标系$：

3.1.2 欧式变换

（1）$欧式变换矩阵T=旋转矩阵R+平移矩阵t$

用正交基的方式进行表示同一向量：


旋转矩阵R的性质如下：

最后欧式变换表示为：

3.1.3 $欧式变换中的问题以及新的表示方法$

（1）连续变换问题

（2）齐次矩阵与非齐次矩阵

（3）$旋转矩阵R（SO(3)）--->变换矩阵T(SE(3))$

3.2 实践eigen

(1) $eigen是一个c++开源线性代数库，提供矩阵的线性代数计算$

$在CMakelists加入库includedirectories("usr/include/eigen3")$

ch3 高翔

3.3 旋转向量和欧拉角

3.3.1 $为甚恶魔从旋转矩阵到使用旋转向量？$

（1）旋转矩阵缺点

• 旋转矩阵R 有9个量，表示的是3个自由度；变换矩阵T 有16个堂，表示的是6个自由度$（冗余）$
• 旋转矩阵自身的约束性，（必须为正交）

（2）引出旋转向量

• 任何旋转都可以用一个旋转轴和旋转角刻画。 （旋转向量）

（3）$罗德里格斯公式：从旋转向量到矩阵的转换$

3.3.2 $欧拉角$

（1） RPY

• yaw 偏航角 （绕z轴旋转）
• pitch 俯仰角（绕旋转之后的y轴旋转）
• roll 翻滚角 （绕旋转之后的x轴旋转）

（2）欧拉角

• 万向锁的问题
• 
• 

3.4 四元数

3.4.1 $slam中使用四元数表达姿态，为什么?$

• 因为旋转矩阵是冗余的，（9个表示3自由度）
• 旋转向量和欧拉角具有奇异性。
• 因此使用四元数。其缺点是：$不够直观，运算复杂一些$
  
  

3.4.2 四元数的运算

• 加减法：
  
• 乘法：
  
• 共轭、模长、逆：
  
  
• 点乘、数乘
  

3.4.3 四元数表示旋转

3.4.3 $四元数转旋转矩阵$

3.5 相似、放射、射影变换

3.6 eigen几何模块

演示几种旋转变换

3.7 $坐标系转换$

3.8 $总结：旋转矩阵、四元数、欧拉角的转换$

（1）transform.cpp
attention!
转换出来的欧拉角是弧度
直接用的roll是角度值 ,利用roll/180*pi转成弧度
角度和弧度转换： 1度=π/180≈0.01745弧度，1弧度=180/π≈57.3度。

#include <iostream>
#include <math.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;

#define PI 3.1415926535897932346f

int main(int argc,char **argv){
   //3D旋转矩阵 定义为单位阵    即 绕xyz旋转0,0,0度
   Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
   //定义旋转向量  沿着z轴旋转45度
   Eigen::AngleAxisd rotation_vector(PI/4,Eigen::Vector3d(0,0,1)); 
   //  隐含了 一个变换  angleaxisd
   
   /*
    * 旋转向量  -->  旋转矩阵 
    * 旋转向量  -->  旋转矩阵  --->   欧拉角
    * 旋转向量  -->  四元数
    */
   cout<<"旋转矩阵："<<endl;
   cout<<rotation_vector.matrix()<<endl;   //..............................rotation_vector   to   rotation_matrix(1)
                                                                          //旋转向量--->旋转矩阵
                                                                          
   rotation_matrix = rotation_vector.toRotationMatrix();
   cout<<"旋转矩阵："<<endl;
   cout<<rotation_matrix<<endl;            //...............................rotation_vector to rotation_matrix(2)
                                                                          //旋转向量--->旋转矩阵
   
   Eigen::Quaterniond q = Eigen::Quaterniond (rotation_vector);  //....................rotation_vector  to Quaterniond
   cout<<"四元数："<<endl;
   cout<<q.coeffs()<<endl;  // (x,y,z,w)
                                                                          //旋转向量--->四元数
   
   /*
    * 旋转矩阵  -->  旋转向量
    * 旋转矩阵  --->   欧拉角
    * 旋转矩阵   --->   四元数
    */
   rotation_vector=rotation_matrix;   // ..................................rotation_matrix  to rotation_vector
                                                                          //旋转矩阵--->旋转向量  
                                                                          
   Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0);  //.......rotation_matrix  to euler_angles .........attention:  ypr
   cout<<"欧拉角："<<endl;
   cout<<euler_angles.transpose()<<endl;                                   //  ypr
                                                                          //旋转矩阵--->欧拉角
     
   q = Eigen::Quaterniond(rotation_matrix);  // .......................................rotation_matrix to Quaterniond
   q = {1,0,0,0};//Quaternion::IDENTITY(1.0, 0.0, 0.0, 0.0)  单位四元数  和eigen中不同的是q.coeff是（x,y,z,w）
   cout<<"四元数："<<endl;
   cout<<q.coeffs()<<endl;  // (x,y,z,w)
                                                                           //旋转矩阵--->四元数  
                                                                           
   /*
    * 四元数--->旋转矩阵
    * 四元数--->旋转矩阵 --->欧拉角
    * 四元数--->旋转向量 
    */                
   cout<<"旋转矩阵："<<endl;                                                        
   cout <<q.toRotationMatrix()<<endl;  // .............................................Quaterniond  to rotatin_matrix
                                                                          //四元数--->旋转矩阵   
                 
   cout<<"欧拉角："<<endl;                                                         
   cout<<q.toRotationMatrix().eulerAngles(2,1,0); //......................................Quaterniond  to rotatin_matrix  to euler_angles  ....attention:  ypr
                                                                          //四元数--->旋转矩阵--->欧拉角
   
   cout<<"旋转向量："<<endl;
   rotation_vector = q ;             // ..................................Quaterniond  to rotation_vector
                                                                          //四元数--->旋转向量  
      
   /*
    * 都是因为定义的不同，存在隐式转换，
    * Eigen::Matrix3d rotation_matrix, Eigen::AngleAxisd rotation_vector,Eigen::Quaterniond q
    * 欧拉角 --->  旋转向量
    * 欧拉角 --->  旋转矩阵
    * 欧拉角 --->  四元数
    * 
    * 按照rpy旋转
    * tran.rotate(Eigen::AngleAxisf(FLAGS_roll / 180.0 * M_PI, Eigen::Vector3f::UnitX()) *
                Eigen::AngleAxisf(FLAGS_pitch / 180.0 * M_PI, Eigen::Vector3f::UnitY()) *
                Eigen::AngleAxisf(FLAGS_yaw / 180.0 * M_PI, Eigen::Vector3f::UnitZ()));
    */
   Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(euler_angles(2),Eigen::Vector3d::UnitX()));
   Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(euler_angles(1),Eigen::Vector3d::UnitY()));
   Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(euler_angles(0),Eigen::Vector3d::UnitZ()));

   rotation_matrix = rollAngle * pitchAngle * yawAngle;//................euler_angles to rotation_matrix
   cout<<"旋转矩阵："<<endl;
   cout<< rotation_matrix << endl;
   rotation_vector = rollAngle * pitchAngle * yawAngle;//................euler_angles to rotation_vector
   cout<<"旋转矩阵："<<endl;
   cout<< rotation_vector.matrix() << endl;
   q               = rollAngle * pitchAngle * yawAngle;//................euler_angles to Quaterniond
   cout<<"四元数："<<endl;
   cout<< q.coeffs()<<endl;
  
}

(2) cmakelist.txt

cmake_minimum_required(VERSION 3.1)
project(transform)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++14)
set(CMAKE_BUILD_TYPE "Debug")
set(CMAKE_CXX_STANDARD 14)
#set(CMAKE_BUILD_TYPE "Debug")
#set(CMAKE_BUILD_TYPE "Release")
#set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
include_directories("/usr/include/eigen3")
add_compile_options(-std=c++11 -Wall)


include_directories(
  ${catkin_INCLUDE_DIRS}
)

add_executable(transform src/transform.cpp )
target_link_libraries(transform ${LIBS})