坐标系、刚体运动、李群李代数

1 欧式坐标系和刚体姿态表示

body frame：本体坐标系，不进行旋转平移等（本体坐标系定义了旋转平移等动作）
world frame：世界坐标系
例：

无人机坐标系是本体坐标系，vision frame是相机视野的地图坐标系（即world frame）

通过相机看到的vision frame中的地图信息，通过相机视野得到相机的位姿（即相机的外参：相机看到的世界和其自身的转换关系），再通过相机与无人机之间的位置关系转换成无人机在世界坐标系下的位姿

world frame：xyz
camera frame：uvw

初始时（无人机开机时），world frame和camera frame重合：

旋转：

若已知Px和Pu、Pv、Pw之间的关系，Py和Pu、Pv、Pw之间的关系、Pz和Pu、Pv、Pw之间的关系，就可得到如何旋转

Pxyz = R * Puvw

二维：

（1，0）=>（cosa，sina）
（0，1）=>（-sina，cosa）

Rotation matrix：

三维：
绕x轴旋转：

Rotation matrix：

绕y轴、绕z轴：

绕xyz轴旋转：
旋转可表示为各个坐标轴依次旋转

平移：

t：平移量
Pxyz = R * Puvw + t，即

通过矩阵的增广，得

R：Rotation matrix
P：translation matrix

若p点在world frame和camera frame中坐标已知，则只要多去几个p点，就可以解出旋转矩阵和平移量

特殊情况：
若只有旋转，没有平移：

缺少scale信息，无法解出深度，无法求解

若只有平移，没有旋转：

旋转平移构成了一个特殊的欧式群（李群）

2 四元数和刚体姿态表示

欧拉角表示旋转的缺陷：

1. 三个轴都旋转时Rotation matrix为三条轴各自旋转时Rotation matrix的乘积，matrix乘积有顺序，顺序不同，结果不同
2. Gimbal Lock（万向锁）：两个坐标轴旋转到重合，三个维度降成两个维度

四元数：q = a + bi +cj+dk

四元数用向量表示，类似于极坐标系，是向量之间的旋转问题，不需要考虑轴

欧拉旋转表达式：

推广到三维：
单位向量：

旋转表示：
一个向量绕单位向量旋转角度：

四元数的表示方法把三条轴的旋转表示成一个向量的旋转，大大简化了计算

角速度：[Wx Wy Wz]

四元数旋转：
Pk+1 = R * Pk

欧拉旋转：

欧式旋转和四元数旋转变化关系：

3 李群和李代数

旋转构成了一个特殊的正交群：SO(3)

旋转平移构成了一个特殊的欧式群：SE(3)
记李群为G，满足一些性质：
斜对称矩阵:

4 实例：Eigen Sophus

Eigen：

安装eigen3：
方法1：

 sudo apt install libeigen3-dev

方法2：

git clone https://github.com/eigenteam/eigen-git-mirror.git

在CMakeLists.txt文件中添加下句令eigen包和工程连接在一起

SET( EIGEN3_INCLUDE ${yourproject_SOURCE_DIR}/eigen3 CACHE PATH "Directory of Eigen3")

在本例中，在工程目录(eigenexample)下建立文件夹eigenLibrary，放eigen文件，工程目录下的CmakeLists.txt添加

include_directories（${PROJECT_SOURCE_DIR}/eigenLibrary）

CMakeLists.txt:

文件位置：eigenexample / CMakeLists.txt:

CMAKE_MINIMUN_REQUIRED(VERSION 2.8)
PROJECT(EIGENDEMO)SET(EXECUTABLE_OUTPUT_PSTH ${PROJECT_SOURCE_DIR}/bin)//可执行文件放在工程目下的bin文件中INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/eigenLIarary)//eigen从eigenLIarary中寻找ADD_EXECUTABLE(eigendemo src/eigendemo.c)

eigendemo.c：
文件位置：eigenexample / src / eigendemo.c

#include<stdio.h>
#include<ctime>
#include<Eigen/Dense>
#include<Eigen/Core>#include<iostream>
#include<ctime>
#include<Eigen/Core>
#include<Eigen/Dense>int main(int argc,char** argv)
{Eigen::Vector3d pos;pos << 0.0,-2.0,1.0;Eigen::Vector3d pos2 = Eigen::Vector3d(1,-3,0.5);pos = pos+pos2;Eigen::Vector3d pos3 = Eigen::Vector3d::Zero();pos.normalize();pos.inverse();Eigen::Vector3d v(1,2,3);Eigen::Vector3d w(0,1,2);double vDotw = v.dot(w);Eigen::Vector3d vCrossw = v.cross(w);Eigen::Quaterniond rot;rot.setFromTwoVectors(Eigen::Vector3d(0,1,0),pos);Eigen::Matrix<double,3,3> rotationMatrix;rotationMatrix = rot.toRotationMatrix();Eigen::Quaterniond q(2, 0, 1, -3);std::cout<<"This Quaterniond consists of a scalar "<<q.w()<<" and a vector "<<std::endl <<q.vec()<< std::endl;q.normalize();std::cout << "To represent rotation, we need to normalize it such that its length is "<<q.norm() << std::endl;Eigen::Vector3d vec(1,2,-1);Eigen::Quaterniond p;p.w() = 0;p.vec() = vec;Eigen::Quaterniond rotatedP = q*p*q.inverse();Eigen::Vector3d rotatedV = rotatedP.vec();std::cout << "We can now use it to rotate a vector " << std::endl <<vec<<"to"<< std::endl <<rotatedV<< std::endl;Eigen::Quaterniond a = Eigen::Quaterniond::Identity();Eigen::Quaterniond b = Eigen::Quaterniond::Identity();Eigen::Quaterniond c;c.w() = a.w() + b.w();c.x() = a.x() + b.x();c.y() = a.y() + b.y();c.z() = a.z() + b.z();Eigen::MatrixXd A(3,2);A<<	1,2,2,3,3,4;Eigen::MatrixXd B = A.transpose();Eigen::MatrixXd C = (B*A).inverse();C.determinant();Eigen::Matrix3d D = Eigen::Matrix3d::Identity();Eigen::Matrix3d m = Eigen::Matrix3d::Random(7,9);m = (m + Eigen::Matrix3d::Constant(1.2))*50;Eigen::Vector3d v2(1,2,3);std::cout << "m =" << std::endl << m << std::endl;std::cout << "m * v2 =" << std::endl<<m*v2<< std::endl;Eigen::MatrixXd A2 = Eigen::MatrixXd::Random(7,9);std::cout << "The fourth row and 7th column element is " <<A2(3,6)<<std::endl;Eigen::MatrixXd B2 = A2.block(1,2,3,3);std::cout << "Take sub-matrix whose upper left corner is A(1, 2)" << std::endl << B2 << std::endl;Eigen::VectorXd a2 = A2.col(1);Eigen::VectorXd b2 = B2.row(0);Eigen::VectorXd c2 = a2.head(3);Eigen::Vector3d d2 = b2.tail(2);return 0;
}

Eigen总结：

初始化向量：
1.Eigen::Vector3d vv<<1,2,3;
2.Eigen::Vector3d v2 = Eigen::Vector3d(1,2,3);
3.Eigen::Vector3d v3 = Eigen::Vector3d::Zero();向量的运算：
1.加法：v = v+v2
2.乘法：double vDot = v.dot(v2)
3.向量叉乘：Eigen::Vector3d vCross = v.cross(v2)
4.归一化：v.normalize()
5.逆运算v.inverse()初始化四元数：
1.Eigen::Quaterniond rotrot.setFromTwoVectors(v3,v4) 四元数rot由v3绕v4旋转而得
2.Eigen::Quaterniond q(1,2,3,4)
3.Eigen::Quaterniond pp.w() = 0p.vec = v2
4.Eigen::Quaternion a = Eigen::Quaterniond::Identity()  四元数的运算：
1.四元数转换为矩阵：Eigen::Matrix<double,3,3> rotationMatrixrotationMatrix = rot.torotationMatrix() 
2.归一化：  q.normalize()q.norm() 获得四元数的长
3.逆运算q.inverse()  
4.乘法：Eigen::Quaterniond p1 = q*p
5.加法：a.w() = q.w()+p.w()a.x() = q.x()+p.x()a.y() = q.y()+p.y()a.z() = q.z()+p.z()初始化矩阵：
1.Eigen::MatrixXd A(3,2)A <<   	1,2,2,3,3,4;
2.Eigen::Matrix D = Eigen::Matrix3d::Identity()
3.Eigen::Matrix M = Eigen::Matrix3d::Random(7,9)     
4.Eigen::Matrix con  = Eigen::Matrix3d::Constant(1.2)矩阵的运算：
1.转置：EIgen::MatrixXd B = A.transpose()
2.逆运算A.inverse()
3.乘法：Eigen::MatrixXd C = B*A
4.行列式：C.determinant() 
5.加法：M = M + con 
6.向量矩阵相乘：v2*M    子矩阵：
Eigen::MatrixXd E = A.block(1,2,3,3)	从(1,2)开始，(3,3)是sizeEigen::VectorXd a = A.col(1)	取第二列
Eigen::VectorXd b = A.row(0) 	取第一行
Eigen::VectorXd c = a.head(3)	取前三个元素 
Eigen::Vector3d d = b.tail(2)	取后两个元素

Sophus：

安装Sophus：

git clone https://github.com/strasdat/Sophus.git
cd Sophus
cmake ..
git checkout a621ff
make install

新建工程：

mkdir sophusexample
cd sophusexample
mkdir src eigenLibrary #将eigen包放入eigenLibrary
touch CmakeLists.txt
cd src
touch sophusdemo.cpp

CmakeLists.txt:

CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
PROJECT(SOPHUSDEMO)# Sophus source dir
set( Sophus_SOURCE_DIR "/home/ze/ROS_WORKSPACE/courseRepo/2_class/sophus_example/Sophus")################################################################################
# Sophus build dir
set( Sophus_DIR "/home/ze/ROS_WORKSPACE/courseRepo/2_class/sophus_example/Sophus/build")################################################################################
set( Sophus_INCLUDE_DIR  "/home/ze/ROS_WORKSPACE/courseRepo/2_class/sophus_example/Sophus;/usr/include/eigen3" )
set( Sophus_INCLUDE_DIRS  "/home/ze/ROS_WORKSPACE/courseRepo/2_class/sophus_example/Sophus;/usr/include/eigen3" )set( Sophus_LIBRARIES    "/home/ze/ROS_WORKSPACE/courseRepo/2_class/sophus_example/Sophus/build/libSophus.so" )
set( Sophus_LIBRARY      "/home/ze/ROS_WORKSPACE/courseRepo/2_class/sophus_example/Sophus/build/libSophus.so" )set( Sophus_LIBRARY_DIR  "/home/ze/ROS_WORKSPACE/courseRepo/2_class/sophus_example/Sophus/build" )
set( Sophus_LIBRARY_DIRS "/home/ze/ROS_WORKSPACE/courseRepo/2_class/sophus_example/Sophus/build" )
//SET()部分不同工程不一样，由Sophus中的CMakeConfig文件得来
FIND_PACKAGE(Sophus REQUIRED)
INCLUDE_DIRECTORIES(BEFORE ${Sophus_INCLUDE_DIR} ${PROJECT_SOURCE_DIR}/eigenLibrary)
ADD_EXECUTABLE(sophusdemo src/sophusdemo.cpp)
TARGET_LINK_LIBRARIES(sophusdemo ${Sophus_LIBRARIES})

sophusdemo.cpp:
文件位置：sophusdemo/src/sophusdemo.cpp

#include <iostream>
#include <Eigen/Core>
#include <sophus/so3.h>
#include <sophus/se3.h>using namespace std;
//using namespace Eigen;
//using namespace Sophus;int main(int argc, char **argv) {//沿着Z轴旋转90度的旋转矩阵Eigen::AngleAxisd A1(M_PI / 2, Eigen::Vector3d(0, 0, 1));//以（0,0,1）为旋转轴，旋转180度Eigen::Matrix3d R1 = A1.matrix();Eigen::Quaterniond Q1(A1);//一、初始化的李群（SO3）的几种方式//1.使用旋转矩阵初始化李群Sophus::SO3 SO3_R(R1);//注意：尽管SO(3)是对应一个矩阵,但是输出SO(3)时,实际上是以so(3)形式输出,从输出的结果可以看到,其输出的值与旋转角对应的值相同,这也证证实了SO(3)对应的李代数so(3)就是旋转角。cout << "SO(3) SO3_R from Matrix" << SO3_R << endl << endl;//2.使用四元数初始化李群Sophus::SO3 SO3_Q(Q1);cout << "SO(3) SO3_Q from Quaterion" << SO3_Q << endl << endl;/****************************************************************************3.1 使用旋转角（轴角）的各个元素对应的代数值来初始化李群注意：直接使用旋转角AngleAxis或是旋转角度对应的向量(Vector3d=AngleAxis.axis()*AngleAxis.angle())对李群进行初始化是不行的，因为SO3李群没有对应的构造函数。也即是使用下列方法是错误的：Sophus::SO3 SO3_A(A1);//直接使用旋转角对李群初始化Sophus::SO3 SO3_A(A1.axis()*A1.angle());//直接使用旋转角度对应的向量(Vector3d=AngleAxis.axis()*AngleAxis.angle())对李群进行初始化只能使用旋转角对应的向量的每一个维度进行赋值，对应于SO3的这样一个构造函数SO3(double rot_x, double rot_y, double rot_z);*******************************************************************************///3.1.1 使用旋转角度对应的向量(Vector3d=AngleAxis.axis()*AngleAxis.angle())中的各个元素对李群进行初始化Sophus::SO3 SO3_A1((A1.axis() * A1.angle())(0), (A1.axis() * A1.angle())(1), (A1.axis() * A1.angle())(2));cout << "SO(3) SO3_A1 from AngelAxis1" << SO3_A1 << endl << endl;//3.1.2 使用旋转角度对应的向量(Vector3d=AngleAxis.axis()*AngleAxis.angle())中的各个元素对李群进行初始化Sophus::SO3 SO3_A2(M_PI / 2 * 0, M_PI / 2 * 0, M_PI / 2 * 1);cout << "SO(3) SO3_A2 from AngleAixs2" << SO3_A2 << endl << endl;//3.2 由于旋转角（轴角）与李代数so(3)对应,所以直接使用旋转角的值获得se(3),进而再通过Sophus::SO3::exp()获得对应的SO(3)Eigen::Vector3d V1(0, 0, M_PI / 2);//so3在Eigen中用Vector3d表示Sophus::SO3 SO3_V1 = Sophus::SO3::exp(V1);cout << "SO(3) SO3_V1 from SO3::exp()" << SO3_V1 << endl << endl;//二、SO(3)与so(3)的相互转换，以及so3对应的hat和vee操作Eigen::Vector3d so3_V1 = SO3_V1.log();//so(3)在Sophus(Eigen)中用vector3d表示,使用对数映射获得李群对应的李代数cout << "so(3) so3_V1 from SO3_V1" << so3_V1.transpose() << endl << endl;Sophus::SO3 SO3_V2 = Sophus::SO3::exp(so3_V1);//使用指数映射将李代数转化为李群cout << "SO(3) so3_V2 from so3_V1" <<SO3_V2 << endl << endl;Eigen::Matrix3d M_so3_V1 = Sophus::SO3::hat(so3_V1);//hat为向量到其对应的反对称矩阵cout << "so3 hat=\n" << M_so3_V1 << endl << endl;Eigen::Vector3d V_M = Sophus::SO3::vee(M_so3_V1);//vee为反对称矩阵对应的向量cout << "so3 vee=\n" << V_M << endl << endl;//三、增量扰动模型Eigen::Vector3d update_so3(1e-4,0,0);//假设更新量为这么多Eigen::Matrix3d update_matrix=Sophus::SO3::exp(update_so3).matrix();//将李群转换为旋转矩阵cout<<"SO3 update Matrix=\n"<<update_matrix<<endl<<endl;Sophus::SO3 SO3_updated=Sophus::SO3::exp(update_so3)*SO3_R;cout<<"SO3 updated = \n"<<SO3_updated<<endl;Eigen::Matrix3d SO3_updated_matrix=SO3_updated.matrix();//将李群转换为旋转矩阵cout<<"SO3 updated Matrix = \n"<<SO3_updated_matrix<<endl<<endl;//******************************************************************分割线***********************************************************************************cout<<"************************************分割线*************************************************"<<endl<<endl;Eigen::AngleAxisd A2(M_PI/2,Eigen::Vector3d(0,0,1));Eigen::Matrix3d R2=A2.matrix();Eigen::Quaterniond Q2(A2);Sophus::SO3 SO3_2(R2);//一、初始化李代数的几种方式Eigen::Vector3d t(1,0,0);//1. 使用旋转矩阵和平移向量来初始化SE3Sophus::SE3 SE_Rt(R2,t);cout<<"SE3 SE_Rt from  Rotation_Matrix and Transform=\n"<<SE_Rt<<endl<<endl;//注意尽管SE(3)是对应一个4*4的矩阵,但是输出SE(3)时是以一个六维向量输出的,其中前前三位为对应的so3,后3维度为实际的平移量t，而不是se3中的平移分量//2. 使用四元数和平移向量来初始化SE3Sophus::SE3 SE_Qt(Q2,t);cout<<"SE3 SE_Qt from  Quaterion and Transform=\n"<<SE_Qt<<endl<<endl;//3. 使用SO3和平移向量来初始化SE3Sophus::SE3 SE_St(SO3_2,t);cout<<"SE3 SE_St from  SO3 and Transform=\n"<<SE_St<<endl<<endl;//二、SE(3)与se(3)的相互转换，以及se3对应的hat和vee操作Sophus::Vector6d se3_Rt=SE_Rt.log();//se(3)在Sophus中用Vector6d表示,使用对数映射获得李群对应的李代数cout<<"se(3) se3_Rt from SE3_Rt\n"<<se3_Rt<<endl<<endl;//se3输出的是一个六维度向量,其中前3维是平移分量,后3维度是旋转分量Sophus::SE3 SE3_Rt2=Sophus::SE3::exp(se3_Rt);//使用指数映射将李代数转化为李群cout<<"SE(3) SO3_Rt2 from se3_Rt"<<SE3_Rt2<<endl<<endl;Sophus::Matrix4d M_se3_Rt=Sophus::SE3::hat(se3_Rt);cout<<"se(3) hat=\n"<<M_se3_Rt<<endl<<endl;Sophus::Vector6d V_M_se3=Sophus::SE3::vee(M_se3_Rt);cout<<"se(3) vee=\n"<<V_M_se3<<endl<<endl;//三、增量扰动模型Sophus::Vector6d update_se3=Sophus::Vector6d::Zero();update_se3(0)=1e-4d;cout<<"update_se3\n"<<update_se3.transpose()<<endl<<endl;Eigen::Matrix4d update_matrix2=Sophus::SE3::exp(update_se3).matrix();//将李群转换为旋转矩阵cout<<"update matrix=\n"<<update_matrix2<<endl<<endl;Sophus::SE3 SE3_updated=Sophus::SE3::exp(update_se3)*SE3_Rt2;cout<<"SE3 updated=\n"<<SE3_updated<<endl<<endl;Eigen::Matrix4d SE3_updated_matrix=SE3_updated.matrix();//将李群转换为旋转矩阵cout<<"SE3 updated Matrix=\n"<<SE3_updated_matrix<<endl<<endl;return 0;}

Sophus总结：

  R1：旋转矩阵Q1：四元数R2：旋转矩阵Q2：四元数1.初始化李群
Sophus::SO3 SO3_R1(R1)	//使用旋转矩阵初始化李群Sophus::SO3 SO3_Q1 (Q1)	//使用四元数初始化李群
//尽管SO(3)是对应一个矩阵,但是输出SO(3)时,实际上是以so(3)形式输出,从输出的结果可以看到,其输出的值与旋转角对应的值相同,这也证证实了SO(3)对应的李代数so(3)就是旋转角。Sophus::SO3 SO3_A1((A1.axis() * A1.angle())(0), (A1.axis() * A1.angle())(1), (A1.axis() * A1.angle())(2) )	使用旋转角度对应的向量(Vector3d=AngleAxis.axis()*AngleAxis.angle())中的各个元素对李群进行初始化Sophus::SO3 SO3_A2(M_PI/2*0, M_PI*0, M_PI*1)	//使用旋转角度对应的向量(Vector3d=AngleAxis.axis()*AngleAxis.angle())中的各个元素对李群进行初始化使用下列方法是错误的：
Sophus::SO3 SO3_A(A1);//直接使用旋转角对李群初始化
Sophus::SO3 SO3_A(A1.axis()*A1.angle());//直接使用旋转角度对应的向量(Vector3d=AngleAxis.axis()*AngleAxis.angle())对李群进行初始化2.SO3和so3相互转化：
Eigen::Vector3d V1(0,0,M_PI/2)	//so3在Eigen中用Vector3d表示
Sophus::SO3 SO3_V1 = Sophus::SO3::exp(V1)	//由于旋转角（轴角）与李代数so(3)对应,所以直接使用旋转角的值获得so(3),进而再通过Sophus::SO3::exp()获得对应的SO(3)Eigen::Vector3d so3_V1 = SO3_V1.log()	///so(3)在Sophus(Eigen)中用vector3d表示,使用对数映射获得李群对应的李代数Sophus::SO3 SO3_V2 = Sophus::SO3::exp(so3_V1)	//使用指数映射将李代数转化为李群3.so3对应的hat和vee操作
Eigen::Matrix3d M_so3_V1 = Sophus::SO3::hat(so3_V1)	//hat为向量到其对应的反对称矩阵
Eigen::Vector3d V_M = Sophus::SO3::vee(M_so3_V1)	//vee为反对称矩阵对应的向量4.增量扰动模型
1.得到更新量：Eigen::Vector3d update_so3(1e-4,0,0)
2.将李群转换为旋转矩阵
Eigen::Matrix3d update_matrix = Sophus::SO3::exp(update_so3).matrix()
//等于：Sophus::SO3 SO3_update = Sophus::SO3::exp(update_so3)Eigen::Matrix3d SO3_update_matrix = SO3_update.matrix()SE3同SO3