SLAM学习笔记(Code4)——李群李代数-Sophus库

Sophus库：支持三维SO(3),SE(3),二维SO(2),SE(2),相似变换Sim(3)

3.1头文件

/*****通用*****/
#include <iostream>
#include <cmath>
/*****Sophus以Eigen库为基础开发*****/
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <sophus/so3.h>
#include <sophus/se3.h>

3.2命名空间

/*****通用*****/
using namespace std;
/*****其他*****/
using namespace Eigen;
using namespace Sophus;

3.3定义

//李群
/*****特殊正交群(3x3)*****/
SO3 SO3_R;//直接输出是行向量so3.transpose()
SO3d SO3_R;//double型
/*****特殊欧式群(4x4)*****/
SE3 SE3_Rt;//直接输出是so3.transpose()\n t.transpose()
//李代数
/*****so3(3维列向量)*****/
Vector3d so3;
/*****se3(6维列向量)*****/
Matrix<double,6,1> se3;
typedef Matrix<double,6,1> Vector6d;//可以先加入新定义
Vector6d se3;

3.4赋值

3.4.1定义时赋值

Matrix3d R = AngleAxisd(M_PI/2,Vector3d(0,0,1)).toRotationMatrix();//先定义一个旋转矩阵
Quaterniond q(R);
/*****SO3*****/
SO3 SO3_R(R);
SO3 SO3_q(q);
/*****so3*****/
Vector3d so3 = SO3_R.log();
/*****SE3*****/
SE3 SE3_Rt(R,t);
SE3 SE3_qt(q,t);
/*****se3*****/
Matrix<double,6,1> se3 = SE3.log();

3.4.2先定义后赋值

/*****SO3*****/
SO3 SO3_R;
SO3_R = SO3(R);
/*****so3*****/
Vector3d so3;
so3 = SO3.log();
/*****SE3*****/
SE3 SE3_Rt;
SE3_Rt = SE3(R,t);
/*****se3*****/
typedef Matrix<double,6,1> Vector6d;
Vector6d se3;
se3 = SE3_Rt.log();

3.5反对称阵的转化(用于叉乘)

/*****转化为反对称阵*****/
Matrix3d so3_hat = SO3::hat(so3);
cout<<so3_hat<<endl;
cout<<SO3::hat(so3)<<endl;
Matrix4d se3_hat = SE3::hat(se3);
cout<<se3_hat<<endl;
cout<<SE3::hat(se3)<<endl;
/*****反对称阵转化为李代数*****/
Vector3d so3_hat_vee = SO3::vee(so3_hat);
Vector6d se3_hat_vee = SE3::vee(se3_hat);
/*****李代数到李群*****/
SO3_R = SO3::exp(so3_hat);//error!!!
SO3_R = SO3::exp(so3);//True
SE3_Rt = SE3::exp(se3_hat);//error!!
SE3_Rt = SE3::exp(se3);//True

3.6输出

cout<<SO3_R<<endl;//输出李代数so3的转置
cout<<SO3_R.matrix()<<endl;//输出李群形式
cout<<SE3_Rt<<endl;//第一行输出so3.transpose(),第二行输出t.transpose()
cout<<SE3_Rt.matrix()<<endl;

3.7变换矩阵的更新(左乘!)

Vector6d updata_se3;
updata_se3.setZero();//初始化
updata_se3(0,0) = 1e-4d;//给第一个元素赋值
SE3 SE3_updated = SE3::exp(updata_se3)*SE3_Rt;//左乘以更新