【视觉slam十四讲笔记】Sophus库的使用

Sophus库的使用

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/se3.hpp"

using namespace std;
using namespace Eigen;

/// 本程序演示sophus的基本用法

int main(int argc, char **argv) {
  Matrix3d R=AngleAxisd(M_PI/2,Vector3d(0,0,1)).toRotationMatrix();
  //角轴定义矩阵
  Quaterniond q(R);
  Sophus::SO3d SO3_R(R);//定义SO3
  Sophus::SO3d SO3_Q(q);
 
 Vector3d so3=SO3_R.log();//对数映射得到李代数
 cout<<"李代数为："<<endl;
 cout<<so3.matrix()<<endl;
 cout<<"上三角为："<<endl;
 cout<<Sophus::SO3d::hat(so3)<<endl;
 cout<<"下三角为："<<endl;
 cout<<Sophus::SO3d::vee(Sophus::SO3d::hat(so3))<<endl;

 //定义SE3
 Vector3d t(1,0,0);
 Sophus::SE3d SE3(R,t);
 

 //当用到其他维度的向量可以用以下命令
 typedef Eigen::Matrix<double,6,1> Vector6d;
 Vector6d se3=SE3.log();
 cout<<se3<<endl;
  return 0;
}

CmakeLists文件

cmake_minimum_required(VERSION 3.0)
project(useSophus)

# 为使用 sophus，需要使用find_package命令找到它
find_package(Sophus REQUIRED)

# Eigen
include_directories("/usr/include/eigen3")
add_executable(useSophus useSophus.cpp)
target_link_libraries(useSophus Sophus::Sophus)