Sophus(李群、李代数库)

文章目录

介绍
代码实例
基本应用
- SO3
- SE3
- SIM3

介绍

下载：

模板类：
git clone https://github.com/strasdat/Sophus.git
非模板类：

git clone https://github.com/strasdat/Sophus.git
cd Sophus
git checkout a621ff

安装: cmake工程，

mkdir build && cd build
cmake ..
make -j 8
sudo make install

代码实例

#include <iostream>
using namespace std;#include <Eigen/Core>
#include <Eigen/Geometry>#include "../../source/Sophus/sophus/so3.hpp"
#include "../../source/Sophus/sophus/se3.hpp"int main()
{Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();Eigen::Quaterniond q(R);// --------------------------利群的初始化--------------------------Sophus::SO3d SO3d_R(R);//Sophus::SO3d SO3d_v(0.0, 0.0, M_PI/2); 非模板类提供这样的构造，模板类没有了Sophus::SO3d SO3d_q(q);cout << "SO(3) from matrix " << SO3d_R.matrix() << endl;//cout << "SO(3) from vector " << SO3d_v.matrix() << endl;cout << "SO(3) from  q" << SO3d_q.matrix() << endl;//----------------------李群到李代数----------------------------Eigen::Vector3d so3 = SO3d_R.log(); // 获取李群的李代数cout << "so(3) hat = " << endl;cout << Sophus::SO3d::hat(so3) << endl; //输出李代数的反对称矩阵cout << "so(3) hat vee = " << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl;// vee 为反对称 到向量// --------------增量扰动模型的跟新-------------------------------Eigen::Vector3d update_so3(1e-4, 0, 0); //新的增量Sophus::SO3d SO3_Update = Sophus::SO3d::exp(update_so3)*SO3d_R ;  //左乘模型cout << "SO3d updated = "<< endl;cout  << SO3_Update.log().transpose() << endl;// *******************************************************************************cout << "**********************************我是萌萌的分割线************************" << endl;Eigen::Vector3d t(1, 0, 0);// t 平移部分，沿着x轴平移1Sophus::SE3d SE3_Rt(R, t);   // 从R ,t 构造SE3Sophus::SE3d SE3_qt(q,t);  //从 q, t构造SE3cout << "SE3 = " << endl << SE3_Rt.matrix() << endl; // 变换矩阵Ttypedef Eigen::Matrix<double, 6, 1> Vector6d;Vector6d se3 = SE3_Rt.log();cout << "se3 = " << endl;cout << se3.transpose() << endl;  // 输出李代数se3cout << "se3 hat = " << endl << Sophus::SE3d::hat(se3) << endl;cout << "se3 hat vee = " << endl << Sophus::SE3d::vee(Sophus::SE3d::hat(se3)) << endl;// **********************跟新************************Vector6d update_se3; //跟新量update_se3.setZero();update_se3(0,0) = 1e-4d;Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3)*SE3_Rt;cout << "SE3d updated = " << endl << SE3_updated.matrix() << endl;  // 变换矩阵T的形式return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.10)project(sophus)find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})
include_directories("/usr/include/eigen3")add_executable(sophus main.cpp)
target_link_libraries(sophus ${Sophus_LIBRARIES})

基本应用

SO3

构造

 Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d(0, 0, 1)).toRotationMatrix();Eigen::Quaterniond q(R);// 1. 默认无参构造Sophus::SO3d so3d_I;   // 通过四元数(1, 0, 0, 0) 构造的，等于没有旋转// 2. 通过旋转矩阵构造Sophus::SO3d so3d_R(R);  // 3. 通过四元数构造()Sophus::SO3d so3d_q(q);

常用的函数

matrix() ：将群转成矩阵的形式
angleX()：得到Ｘ轴的旋转，弧度值，对应也有Y,Z
static hat() : from vector o skew-symmetric matrix, 静态函数，以类名来调用
static vee(): from skew-symmetric matrix to vector
unit_quaternion() 得到单位旋转四元数
static lieBracket() ：计算两向量的李括号的结果

SE3

构造

    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d(0, 0, 1)).toRotationMatrix();Eigen::Quaterniond q(R);Eigen::Vector3d t(1,2,3);//1. 从旋转矩阵和平移来构造Sophus::SE3d se3_Rt(R, t);　　// //2. 从四元数和平移来构造Sophus::SE3d se3_qt(q, t);　　//3. 从4*4的矩阵来构造，左上角rotaion矩阵必须的正交且determinant=1,最后一行为(0,0,0,1)Eigen::Matrix4d T;T << 1, 0, 0, 1,0, 1, 0, 2,0, 0, 1, 3,0, 0, 0, 1;Sophus::SE3d se3_4m(T);//4. 从SO3 和　t　构造Sophus::SO3d so3_R(R);Sophus::SE3d se3_Rt(so3_R, t);

常用的函数

matrix() 将群转成矩阵的形式
rotaionMatrix() : 取旋转矩阵
translation 取平移部分
static hat() ６维向量到４*４矩阵的操作，左上角是向量的旋转部分对应的反对称矩阵，右上角是平移部分
static vee()４*４矩阵到６维向量的操作
angleX()：得到Ｘ轴的旋转，弧度值，对应也有Y,Z

SIM3

构造

在这里插入代码片

Sophus(李群、李代数库)相关推荐

SLAM学习--------相机位姿表示-李群李代数
slam 求解相机的位姿求解核心思想:将有约束的李群问题转换成无约束的李代数问题,然后使用高斯牛顿算法或者LM(列文伯格-马夸尔特法)求解. 人们找了很多以相机位姿为变量的误差函数,比如光度误差,重投 ...
数学小抄: 李群李代数再回顾 [SLAM十四讲]
前言最近阅读了高翔老师的视觉SLAM十四讲, 也算是了解了当下机器人领域最炙热的机器人领域前沿研究问题大概是怎么样的数学问题. 不得不说视频与书籍真的是深入浅出. 所有第三库无痛安装教程: 知乎链接 ...
SLAM中相机位姿求解（李群李代数）
前言 slam中一个关键问题之一就是求解相机的位姿,人们找了很多以相机位姿为变量的误差函数,比如光度误差,重投影误差,3D几何误差等等,希望使得误差最小,进而求得比较准确的相机位姿.举一个重投影例子: ...
SLAM14讲学习笔记（一) 李群李代数基础
第二次翻看<视觉SLAM十四讲>,发现第一次看过的都忘记了,认真分析了一下,我认为是第一次学习没有弄清楚整体的脉络.因此这次做一个小小的总结,希望不要再过几天又忘掉.前面的内容就没有总结的 ...
SLAM基础——李群李代数
李群李代数&雅可比求导 1 李群李代数 1.1 李群李群是一种可微分流形的群,即可以在流形上进行微分操作. 群(G,∘)(\mathcal G,\circ)(G,∘)由一个集合G\mathc ...
多视图几何的数学基础知识的掌握（2）--李群李代数
文章目录前言多视图几何的数学基础知识的掌握(2)--李群李代数三维重建的起源三维欧式空间叉乘与反对称阵刚体运动刚体运动的表示旋转的指数坐标李群$SO(3)$ 李群与李代数李群定义 ...
SLAM本质剖析番外-李群李代数的微分和导数
0. 简介这几个月,博主已经从SLAM算法的使用向着算法的数学推导进行了记录和分享,之前也分享了李群李代数关注核心一文,从现象中解释了李群和李代数表达的含义.但是这还不够,所以这次作者作为SLAM本 ...
李群李代数简明教程
李群李代数简明教程 slam算法中往往需要用到李群李代数的一些知识,最近看到一个李群李代数的简明教程,就翻译下来,有些地方翻译的可能不准,主要是作为一个学习记录. 文章目录李群李代数简明教程 2 ...
李群李代数在计算机视觉中的应用,李群李代数在数据融合算法中的应用分析...
袁治晴摘要:数据融合是提升机器人.无人驾驶.无人机等应用能力的重要手段,一直是前沿技术中研究的一个热点,关于数据融合算法的分析设计,学术界和工程界对此方面进行了长期的研究与讨论,而数据融合算法结合李 ...

Sophus(李群、李代数库)

文章目录

介绍

代码实例

基本应用

SO3

SE3

SIM3

Sophus(李群、李代数库)相关推荐

最新文章

热门文章