sophus 使用

最新推荐文章于 2024-07-30 18:02:49 发布

磊磊哈哈

最新推荐文章于 2024-07-30 18:02:49 发布

阅读量1.9k

点赞数 3

原文链接：https://blog.csdn.net/robinhjwy/article/details/77334189

版权

SLAM十四讲专栏收录该内容

37 篇文章 8 订阅

订阅专栏

非模板类使用
转换关系图如下
在这里插入图片描述

SO

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <sophus/geometry.hpp>
#include <sophus/se3.hpp>
#include <sophus/so3.hpp>
#include <iostream>
#include <cmath>
using namespace std;
using namespace Eigen;
int main(int argc,char **argv)
{
  AngleAxisd A1=AngleAxisd(M_PI/2,Vector3d(0,0,1));//等效旋转矢量
  Matrix3d R=A1.matrix();//构造旋转矩阵 R
  //AngleAxisd 旋转矢量
  Quaterniond q(R);//转化为4元数
  //SO(3)初始化
  Sophus::SO3d SO_R(R);//使用矩阵构造初始化
  Sophus::SO3d SO_Q(q);//使用4元数构造初始化

  Sophus::SO3<double> R1=Sophus::SO3d::rotX(M_PI/4);//绕x周旋转的旋转矩阵
  
  // cout<<q<<endl;

  //查看值
  cout<<"SO_R"<<endl<<SO_R.matrix()<<endl;
  cout<<"SO_Q"<<endl<<SO_Q.matrix()<<endl;
  cout<<"R1"<<endl<<R1.matrix()<<endl;
  //获取李代数
  Sophus::Vector3d so3=SO_R.log();
  //Sophus::Vector3d 与 Eigen::Vector3d 相同

  cout<<"so3"<<endl<<SO_R.log()<<endl;//SO(3)的对数就是其李代数
  cout<<"so3×"<<endl<<Sophus::SO3d::hat(so3)<<endl;//获得so3 反对称矩阵
  cout<<"v"<<endl<<Sophus::SO3d::vee(Sophus::SO3d::hat(so3))<<endl;//求反对称矩阵对应向量

  //扰动模型
  Vector3d update_so(1e-4,0,0);//更新量
  Sophus::SO3d SO_update=Sophus::SO3d::exp(update_so)*SO_R;//SO（3）求得导数
  cout<<"SO_update"<<endl<<SO_update.matrix()<<endl;//
  return 0;
}

SE

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <sophus/geometry.hpp>
#include <sophus/se3.hpp>
using namespace std;
using namespace Eigen;
 int main(int argc, const char** argv) {
  Matrix3d R=AngleAxisd(M_PI/2,Vector3d(0,0,1)).matrix();
  Quaterniond q(R);
  Vector3d T(1,1,1);
  Sophus::SE3d SE3_R(R,T);//矩阵赋值
  Sophus::SE3d SE3_Q(q,T);//4元组赋值
  cout<<"SE3_R"<<endl<<SE3_R.matrix()<<endl;
  cout<<"SE3_Q"<<endl<<SE3_Q.matrix()<<endl;

  //李代数
  Sophus::Vector6d se3=SE3_Q.log();
  cout<<"se3"<<endl<<se3.matrix()<<endl;//平移在前旋转在后
  cout<<"se3 ×"<<endl<<Sophus::SE3d::hat(se3)<<endl;//se对应反对陈矩阵
  cout<<" × se3"<<endl<<Sophus::SE3d::vee(Sophus::SE3d::hat(se3))<<endl;

  //扰动模型更新  
  Sophus::Vector6d update_se3;//更新量
  update_se3.setZero();//设为0
  update_se3(0,0)=1e-4d;
  Sophus::SE3d se_update=Sophus::SE3d::exp(update_se3)*SE3_Q;
  cout<<"SE3_Q"<<endl<<SE3_Q.matrix()<<endl;
    return 0;
}