SLAM知识点——Sophus库的学习

Eigen库提供了几何模块，但是没有提供李代数的支持。一个较好的 李代数库是由Strasdat维护的Sophus库。Sophus库支持三维运动的 SO(3)、 SE(3)，此外还支持二维运动的 SO(2)、 SE(2)和相似变换 Sim(3)等内容。它是直接在Eigen库基础上开发的，因此我们不需要安装额外的依赖库。

下载地址：
https://github.com/strasdat/Sophus
https://download.csdn.net/download/weixin_41874898/40235940

下载并解压

tar -xzvf Sophus.tar.gz
cd Sophus
mkdir build
cd build
cmake …
make
sudo make install （可做可不做）

CMakeLists.txt:

cmake_minimum_required(VERSION 2.8)
project(sophus_demo)f
find_package( Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})
add_executable(sophus_demo sophus_demo.cpp)
target_link_libraries(sophus_demo ${Sophus_LIBRARIES})

将sophus的头文件和lilb加入后，就不用加这句include_directories("/usr/include/eigen3")，就可以使用#include<Eigen/Core>Eigen的头文件。

demo.cpp

#include<iostream>
#include<Eigen/Core>
#include<Eigen/Dense>
#include<Eigen/Geometry>
#include"sophus/so3.h"  //引号而不是<>
#include"sophus/se3.h"

using namespace std;
using namespace Eigen;

int main(int argc, char** argv)
{
	Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Vector3d(0,0,1)).matrix();
	cout<<R<<endl;	

	Sophus::SO3 SO3_1(R);    // Sophus::SO(3)可以直接从旋转矩阵构造
	Sophus::SO3 SO3_2(0,0,M_PI/2); // 亦可从旋转向量构造
	Eigen::Quaterniond q(R);   // 或者四元数
	Sophus::SO3 SO3_3(q);
	// 上述表达方式都是等价的
	cout<<"SO3_1: "<<SO3_1<<endl;    // 输出SO(3)时，以so(3)形式输出
	cout<<"SO3_2: "<<SO3_2<<endl;
	cout<<"SO3_3: "<<SO3_3<<endl;
	cout<<"SO3_3: "<<SO3_3.matrix()<<endl;  //旋转矩阵的形式
 
	//
	Eigen::Vector3d so3_1= SO3_1.log();  //不会生成带^
	cout<<"so3_1: "<<so3_1.transpose()<<endl;
	// hat 为向量到反对称矩阵
	Eigen::Matrix3d so3_1_hat = Sophus::SO3::hat(so3_1);  //NOT SO3(3)
	cout<<"so3_1_hat: "<< so3_1_hat<< endl;

    // 相对的，vee为反对称到向量
	Eigen::Vector3d so3_1_hat_vee = Sophus::SO3::vee(so3_1_hat);
	cout<<"so3_1_hat_vee:"<<so3_1_hat_vee.transpose()<<endl;
	
	Eigen::Vector3d update_so3(1e-4,0,0);
	//Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3)*SO3_1;//不会生成带^
	Sophus::SO3 SO3_updated = Sophus::SO3::exp(Sophus::SO3::hat(update_so3))*SO3_1;//这样带上^再做指数映射也是可以的。
	cout<<"SO3_updated: "<<SO3_updated.matrix()<<endl;;


	Eigen::Vector3d t(1,0,0); // 沿X轴平移1
	Sophus::SE3 SE3_Rt(R, t); // 从R,t构造SE(3)
	Sophus::SE3 SE3_qt(q, t); // 从q,t构造SE(3)
	cout<<"SE3_Rt: "<<SE3_Rt<<endl;
	cout<<"SE3_Rt.matrix: "<<SE3_Rt.matrix()<<endl;//4*4
	cout<<"SE3_qt: "<<SE3_qt<<endl;

	typedef Eigen::Matrix<double, 6, 1> Vector6d;	
	Vector6d se3 = SE3_Rt.log();  //直接生成ξ
	cout<< "se3: "<<se3.transpose()<<endl;  //六维，平移在前旋转在后0.785398 -0.785398 0 0 0 1.5708和SE3六维不同
	
	cout<<"-------: "<<Sophus::SE3::hat(se3)<<endl;
	Eigen::Matrix<double,4,4> se3_hat = Sophus::SE3::hat(se3);

	cout<<"se3_hat: "<< se3_hat <<endl;
	Vector6d se3_hat_vee = Sophus::SE3::vee(se3_hat);
	cout<<"se3_hat_vee: "<<se3_hat_vee.transpose()<<endl;

	Vector6d updated_se3;
	updated_se3.setZero();
	updated_se3(0,0)=1e-4;
	cout<<"updated_se3: "<<updated_se3<<endl;
	Sophus::SE3 SE3_updated = Sophus::SE3::exp(updated_se3)*SE3_Rt;
	

	cout<<"hello"<<endl;
	return 0;
}

指数映射和对数映射不需要对向量做^（反对称操作），只需要使用旋转向量和ξ。