Eigen so3 and se3

最新推荐文章于 2024-08-09 09:22:42 发布

F-study

最新推荐文章于 2024-08-09 09:22:42 发布

阅读量3k

点赞数 2

分类专栏： C++11 代码

本文链接：https://blog.csdn.net/weixin_40353209/article/details/81359244

版权

该代码源于《视觉SLAM十四讲》 Eigen 中Geometry （几何模块）
主要操作有 so3 se3 旋转向量和旋转矩阵转换以及增量扰动模型更新
该页代码用于日后查找方便（添加了部分笔记）

#include <iostream>
#include <cmath>
using namespace std; 

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "sophus/so3.h"
#include "sophus/se3.h"

int main( int argc, char** argv )
{
    // 沿Z轴转90度的旋转矩阵
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();

    Sophus::SO3 SO3_R(R);               // Sophus::SO(3)可以直接从旋转矩阵构造
    Sophus::SO3 SO3_v(