1 简单介绍

目的：为了计算变换矩阵的更新，李群和李代数之间的相互转换代码实现；
- 为什么引入李代：因为李群（一个集合 + 一种运算）中只定义了乘法运算，而在优化相机位姿时需要求导，而求导、求极限这样的运算需要用到加法运算，而李代（一个集合 + 一种运算 + 一个数域）中有加法运算，所以引入了李代数。
- 一种李群对应一种李代。
Sophus库基于Eigen；
github地址：
视觉SLAM十四讲地址：

SO(3) 与 so(3)、SE(3) 与 se(3) 相互转换关系：

(对应Sophus中函数 .log() .exp(...) )

重要的求导公式：

扰动模型（左乘）：对旋转矩阵 R 进行一次扰动 ∆R。这个扰动可以乘在左边也可以乘在右

边，最后结果会有一点儿微小的差异，我们以左扰动为例。设左扰动 ∆R 对应的李代数为

φ。然后，对 φ 求导，结果为：（p为空间中的一个3D点, ^ 表示取反对称矩阵）

SE(3) 上的扰动模型：假设某空间点 p 经过一次变换 T（对应李代数为 ξ），得到 Tp。现在，给 T 左乘一个扰动

，我们设扰动项的李代数为

，那么求导结果为：

2 代码演示

2.1 目录结构

2.2 代码

CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( useSophus )#当找不到Sophus时，添加下面其build目录，如下所示
#SET(Sophus_DIR "path-to-/Sophus/build")# 为使用 sophus，您需要使用find_package命令找到它
find_package( Sophus REQUIRED )
include_directories( ${Sophus_INCLUDE_DIRS} )add_executable( useSophus useSophus.cpp )
target_link_libraries( useSophus ${Sophus_LIBRARIES} )

useSophus.cpp

#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 )
{// 1. 使用矩阵、旋转向量、四元数构造李群形式的旋转SO3(Sophus中的SO3类维护了一个四元数)// 沿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( 0, 0, M_PI/2 );  // 亦可从旋转向量构造Eigen::Quaterniond q(R);            // 或者四元数Sophus::SO3 SO3_q( q );// 上述表达方式都是等价的// 输出SO(3)时，以so(3)形式输出cout<<"SO(3) from matrix: "<<SO3_R<<endl;cout<<"SO(3) from vector: "<<SO3_v<<endl;cout<<"SO(3) from quaternion :"<<SO3_q<<endl;// 2. 使用对数映射获得它的李代数so3Eigen::Vector3d so3 = SO3_R.log();  //《======cout<<"so3 = "<<so3.transpose()<<endl;// 2.1 使用hat、vee函数，实现 so3形式的3x1向量 与 3x3反对称矩阵 的相互转换cout<<"so3 hat=\n"<<Sophus::SO3::hat(so3)<<endl;// 相对的，vee为反对称到向量cout<<"so3 hat vee= "<<Sophus::SO3::vee( Sophus::SO3::hat(so3) ).transpose()<<endl; // transpose纯粹是为了输出美观一些// 3. 增量扰动模型的更新Eigen::Vector3d update_so3(1e-4, 0, 0); //假设更新量为这么多Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3)*SO3_R; //《====== 使用 扰动模型, 左乘扰动量 cout<<"SO3 updated = "<<SO3_updated<<endl;/********************萌萌的分割线*****************************/cout<<"************我是分割线*************"<<endl;// 4. 对SE(3)操作大同小异 SE3类内维护了一个 SO3 和 一个3x1的旋转向量Eigen::Vector3d t(1,0,0);           // 沿X轴平移1Sophus::SE3 SE3_Rt(R, t);           // 从R,t构造SE(3)Sophus::SE3 SE3_qt(q,t);            // 从q,t构造SE(3)cout<<"SE3 from R,t= "<<endl<<SE3_Rt<<endl;cout<<"SE3 from q,t= "<<endl<<SE3_qt<<endl;// 5. 李代数se(3) 是一个六维向量，方便起见先typedef一下typedef Eigen::Matrix<double,6,1> Vector6d;Vector6d se3 = SE3_Rt.log();                    //《======cout<<"se3 = "<<se3.transpose()<<endl;// 观察输出，会发现在Sophus中，se(3)的平移在前，旋转在后.// 同样的，有hat和vee两个算符cout<<"se3 hat = "<<endl<<Sophus::SE3::hat(se3)<<endl;cout<<"se3 hat vee = "<<Sophus::SE3::vee( Sophus::SE3::hat(se3) ).transpose()<<endl;// 6 .最后，演示一下更新Vector6d update_se3; //更新量update_se3.setZero();update_se3(0,0) = 1e-4d;Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3)*SE3_Rt; //《======cout<<"SE3 updated = "<<endl<<SE3_updated.matrix()<<endl;return 0;
}

运行结果

参考：视觉SLAM十四讲李群与李代数视频去哪了呢？_哔哩哔哩_bilibili

矩阵运算_Sophus库的使用相关推荐

三阶矩阵的lu分解详细步骤_快速入门矩阵运算——开源库Eigen
矩阵是数学中一个重要的工具,广泛应用于各种场景下的数值分析,例如,数字信号处理,图像处理等.我们如何在程序中使用矩阵进行运算呢?本文将为大家介绍一个开源的矩阵运算工具--Eigen. Eigen is ...
eigen 列向量转矩阵_快速入门矩阵运算——开源库Eigen
矩阵是数学中一个重要的工具,广泛应用于各种场景下的数值分析,例如,数字信号处理,图像处理等.我们如何在程序中使用矩阵进行运算呢?本文将为大家介绍一个开源的矩阵运算工具--Eigen. Eigen is ...
OpenGL矩阵运算——GLM库的使用
GLM库简介 OpenGL没有内建矩阵运算方法,常用的第三方库为GLM.GLM是OpenGL Mathematics的缩写.作为一个header only库,GLM只要包括了相应的头文件就可以使用它提 ...
3 Spark机器学习 spark MLlib 矩阵向量、矩阵运算Breeze库-1
机器学习里矩阵是必不可少的,无论Python.Java能做机器学习的语言,都会提供比较优质的矩阵库. spark mllib中提供的矩阵库是Breeze,可以简单看看Breeze库的情况. Scala ...
3 Spark机器学习 spark MLlib 矩阵向量、矩阵运算Breeze库-2
上一篇是矩阵的创建.连接等相关操作. 这一篇主要是矩阵的数值计算. 1 基本运算元素加法 a+b 元素乘法 *:* 元素除法 /:/ 元素比较 <:< 元素相等 :== 元素追加 :+= ...
armadillo matlab,科学网—C++下媲美MATLAB矩阵运算的Armadillo 库 - 吴泓润的博文
Matlab在矩阵运算上确实无敌,但是如果涉及高频的for/while循环就彻底完蛋了.此时我们希望用C或C++跑仿真,但又希望使用Maltab下的矩阵运算.现在给出几个关于在VS下使用Armadil ...
Python中矩阵库Numpy基本操作
NumPy是一个关于矩阵运算的库,熟悉Matlab的都应该清楚,这个库就是让python能够进行矩阵话的操作,而不用去写循环操作. 下面对numpy中的操作进行总结. numpy包含两种基本的数据类 ...
python中math函数库矩阵_Python中矩阵库Numpy基本操作详解
NumPy是一个关于矩阵运算的库,熟悉Matlab的都应该清楚,这个库就是让python能够进行矩阵话的操作,而不用去写循环操作. 下面对numpy中的操作进行总结. numpy包含两种基本的数据类型 ...
机器学习的实现（语言及库的选择）
Machine learning and Programming Languages Python 下的 TensorFlow 以及 torch 底层都是通过 C++ 实现的,考虑到性能的需要: Ja ...
SLAM相关学习资料：综述/激光/视觉/数据集/常用库
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者丨菠萝包包包@知乎来源丨https://zhuanlan.zhihu.com/p/4348743 ...

矩阵运算_Sophus库的使用