视觉SLAM实践入门——（4）Sophus库的基本用法

101 阅读 0 评论 67 点赞

我是靠谱客的博主现实外套，最近开发中收集的这篇文章主要介绍视觉SLAM实践入门——（4）Sophus库的基本用法，觉得挺不错的，现在分享给大家，希望可以做个参考。

概述

安装Sophus库

前面已经通过下面命令获取了资源包，在slambook2/3rdparty中有Sophus库

git clone https://github.com/gaoxiang12/slambook2.git

方法1：将库（包括整个cmake工程）拷贝到工程的根目录（如useSophus）后，配置cmake工程

cd Sophus && cmake .

然后在执行程序的CMakeLists.txt中添加以下几行，即可在这个工程中使用

set( Sophus_DIR ${PROJECT_SOURCE_DIR}/Sophus)    #设置库目录
find_package( Sophus REQUIRED )    #查找库
include_directories( ${Sophus_INCLUDE_DIRS} )    #包含头文件目录

使用方法2和方法3一劳永逸，不需要每次建立工程都拷贝库

方法2：将库的头文件所在目录拷贝到/usr/local/include后可以直接使用

sudo cp slambook2/3rdparty/Sophus/sophus /usr/include -r

方法3：输入编译安装命令，库会被安装到/usr/local/include

make && sudo make install

另外，无论用哪种方法，使用时注意要在CMakeLists文件中添加C++11标准

add_definitions( -std=c++11 )

Sophus库基本用法

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

using namespace std;
using namespace Eigen;
using namespace Sophus;



int main(int argc, char **argv) {

  //定义绕Z轴旋转90°的旋转矩阵和四元数
  Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();	
  Quaterniond q(R);
  Vector3d t(1, 0, 0);	//平移向量
  
  cout << "旋转矩阵 = n" << R << endl;
  cout << "四元数 = " << q.coeffs().transpose() << endl;
  cout << "欧拉角ZYX = " << (180 * R.eulerAngles(2,1,0) / M_PI).transpose() << endl;
  cout << "平移向量 = " << t.transpose() << endl;
  
  /*******对SO（3）*******/
  cout << endl << "**************SO（3）***************" << endl;
  //定义李群SO（3）（下面两行是等价的）
  SO3d SO3_R(R);
  SO3d SO3_q(q);  
  cout << "用旋转矩阵构造的SO(3) = n" << SO3_R.matrix() << endl;
  cout << "用四元数构造的SO(3) = n" << SO3_q.matrix() << endl;
  
  //通过对数映射到李代数
  Vector3d so3 = SO3_R.log();
  cout << "so(3) = " << so3.transpose() << endl;
  
  //映射反对称矩阵
  cout << "so(3)的反对称矩阵 = n" << SO3d::hat(so3) << endl;
  cout << "反对称矩阵映射的向量 = " << SO3d::vee(SO3d::hat(so3)).transpose() << endl;
  
  //增量扰动模型
  Vector3d update_so3(1e-4, 0, 0);	//扰动量
  SO3d SO3_updated = SO3d::exp(update_so3) * SO3_R;
  cout << "SO3 updated = n" << SO3_updated.matrix() << endl;
  
  /******对SE（3）*******/
  cout << endl << "**************SE（3）***************" << endl;
  //定义李群SE（3）
  SE3d SE3_Rt(R, t);
  SE3d SE3_qt(q, t);
  cout << "用旋转矩阵和平移向量构造的SE(3) = n" << SE3_Rt.matrix() << endl;
  cout << "用四元数和平移向量构造的SE(3) = n" << SE3_qt.matrix() << endl;

  //定义李代数，前三维是（间接，相差一个J）平移，后三维是旋转
  Vector6d se3 = SE3_Rt.log();
  cout << "se(3) = " << se3.transpose() << endl;

  //映射反对称矩阵
  cout << "se(3)的反对称矩阵 = n" << SE3d::hat(se3) << endl;
  cout << "反对称矩阵映射的向量 = " << SE3d::vee(SE3d::hat(se3)).transpose() << endl;

  //增量扰动模型，update_se3(1,0) = 1e-4d，也就是将平移向量第二维增加1e-4d
  Vector6d update_se3;	//扰动量
  update_se3.setZero();	//清0
  update_se3(1, 0) = 1e-4d;	//update_se3是列向量，（0，0）是0行0列，（1，0）是1行0列；列参数填非0数都会出错
  //cout << update_se3.transpose() << endl;	
  SE3d SE3_updated = SE3d::exp(update_se3) * SE3_Rt;
  cout << "SE3 updated = n" << SE3_updated.matrix() << endl;
  
  
  return 0;
}