李群 | SO(3) | SE(3) |
---|---|---|
旋转矩阵构建 | Sophus::SO3d SO3® | Sophus::SE3d SE3(R,t) |
四元数构建 | Sophus::SO3d SO3(q) | Sophus::SO3d SO3(q,t) |
输出 | SO3.matrix() | SE3.matrix() |
对数映射 | Vector3d so3=SO3.log() | Vecotr6d se3=SE3.log() |
指数映射 | SO3d::exp(so3) | SE3d::exp(se3) |
向量到反对称矩阵 | SO3d::hat(so3) | SE3d::hat(se3) |
反对称矩阵到向量 | SO3d::vee(hat) | SE3d::vee(hat) |
李代数so(3):Sophus::Vector3d //因为so(3)仅仅只是一个普通的3维向量
李代数se(3):Sophus::Vector6d //因为se(3)仅仅只是一个普通的6维向量
李群的构造
SO3 ();
SO3 (const SO3 & other); // 复制构造
explicit SO3 (const Matrix3d & _R); // 由旋转矩阵构造
explicit SO3 (const Quaterniond & unit_quaternion); // 由四元数构造
SO3 (double rot_x, double rot_y, double rot_z); // 由欧拉角构造
Sophus::SO3 SO3_V2 = Sophus::SO3::exp(so3_V1); // 通过转换构造 使用指数映射将李代数转化为李群
//通过李群构造李代数
Eigen::Vector3d so3_V1 = SO3_V1.log();
//李群构造
SE3 ();
SE3 (const SO3 & so3,const Vector3d & translation);
SE3 (const Matrix3d & rotation_matrix,const Vector3d & translation);
SE3 (const Quaterniond & unit_quaternion,const Vector3d & translation_);
SE3 (const SE3 & other);
// 李代数
// se(3) 是一个六维向量,方便起见先typedef一下
typedef Eigen::Matrix<double, 6, 1> Vector6d;
//通过李群构造李代数
Sophus::Vector6d se3_Rt=SE_Rt.log();//se(3)在Sophus中用Vector6d表示,使用对数映射获得李群对应的
//李代数
Sophus::Vector6d update_se3_=Sophus::Vector6d::Zero();update_se3_(0)=1e-4;
特别注意
// 注意:尽管SO(3)是对应一个矩阵,但是输出SO(3)时,实际上是以so(3)形式输出,从输出的结果可以看到,
// 其输出的值与旋转角对应的值相同,这也证证实了SO(3)对应的李代数so(3)就是旋转角。
cout << "SO(3) SO3_RR from Matrix" << SO3_RR << endl << endl;
// 输出 SO(3) SO3_RR from Matrix 0 0 1.5708
构造实例
Eigen::AngleAxisd A1(M_PI / 2, Eigen::Vector3d(0, 0, 1));//以(0,0,1)为旋转轴,旋转180度
Eigen::Matrix3d R1 = A1.matrix();
Eigen::Quaterniond Q1(A1);
//1.使用旋转矩阵初始化李群
Sophus::SO3 SO3_RR(R1);
//2.使用四元数初始化李群
Sophus::SO3 SO3_QQ(Q1);
cout << "SO(3) SO3_QQ from Quaterion" << SO3_QQ << 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_RR;
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;
构造实例
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 trans(1,0,0);
//1. 使用旋转矩阵和平移向量来初始化SE3
Sophus::SE3 SE_Rt(R2,trans);
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. 使用四元数和平移向量来初始化SE3
Sophus::SE3 SE_Qt(Q2,trans);
cout<<"SE3 SE_Qt from Quaterion and Transform=\n"<<SE_Qt<<endl<<endl;
//3. 使用SO3和平移向量来初始化SE3
Sophus::SE3 SE_St(SO3_2,trans);
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-4; // 能够直接赋值
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;
cout<<"被增矩阵为 SE(3) SO3_Rt2 from se3_Rt"<<SE3_Rt2<<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;