第一部分:SO(3)和so(3)
1、SO(3)对应的李代数
SO(3)对应的李代数是定义在上向量,我们记作。根据上一篇博客中的推导,可以知道,每个都可以生成一个反对称矩阵:
在此定义下,两个向量的李括号(运算)为:
由于和反对称矩阵关系很紧密,在不引起歧义的情况下,可以说的元素是三维向量或者三维反对称矩阵,即:
到此,我们已经清楚了SO(3)对应的李代数的内容,它们是一个由三维向量组成的集合,其中每个向量都可以对应到三维反对称矩阵,可以表达旋转矩阵的导数。它和SO(3)的关系由指数映射给定:
2、SO(3)上的指数映射
由上面的内容可以知道,SO(3)中的每个元素都有对应的李代数,它们之间的关系由指数映射给定: 。那么它具体应该怎么去计算呢?
(1)再进入真正的计算公式的推导之前,首先来看一下如何利用Sophus库来进行李群和李代数的运算,因为这比较简单。
Eigen提供了几何模块,但是没有提供对李代数的支持。Sophus是一个较好的李代数库,它是Strasdat维护的,基于Eigen开发的,能够较好的支持SO(3)、SE(3),此外还能够支持SO(2)、SE(2)。
关于Sophus库的配置在百度上可以找到很多教程,这里不记录了。下面是一个调用Sophus库进行李代数运算的例子。
#include <iostream>
//Eigen
#include <Eigen/Core>
#include <Eigen/Geometry>
//Sophus
#include "sophus/so3.h"
using namespace std;
int main(int argc, char **argv)
{
//定义一个沿Z轴转90度的旋转
Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
//可以直接从旋转矩阵构造SO(3)
Sophus::SO3 SO3_R(R);
//也可以直接从旋转向量构造SO(3)
Sophus::SO3 SO3_V(0, 0, M_PI/2);
//也可以从四元数构造SO(3)
Eigen::Quaterniond q(R);
Sophus::SO3 SO3_Q(q);
//输出SO(3)的时候,是以so(3)的形式显示的
cout << "SO(3) from rotation_matrix : " << SO3_R << endl;
cout << "SO(3) from rotation_vector : " << SO3_V << endl;
cout << "SO(3) from quaternion : " << SO3_Q << endl;
//SO(3)---> so(3) 对数映射
Eigen::Vector3d so3 = SO3_R.log();
cout << "SO3_R.log() = " << so3 << endl;
//hat ---> 从三维向量到三维反对称矩阵的运算
cout << "so3 hat = " << Sophus::SO3::hat(so3) << endl;
//vee ---> 从三维反对称矩阵到三维向量
cout << "so3 hat vee = "<< Sophus::SO3::vee(Sophus::SO3::hat(so3)).transpose() << endl;
return 0;
}
下面是CMakeLists.txt的内容:
cmake_minimum_required(VERSION 3.5)
project(use_sophus)
find_package(Sophus REQUIRED)
include_directories(
${Sophus_INCLUDE_DIRS}
)
add_executable(useSophus
useSophus.cpp
)
target_link_libraries(useSophus
${Sophus_LIBRARIES}
)
(2)数学上的计算推导
通过上面的推导得到了计算的公式:
说明:实际上,这个公式和第3讲中讲到的“罗德里格斯公式 --- 描述旋转矩阵和旋转向量的转换过程”形式相同,这表明,so(3)实际上就是由所谓的 旋转向量 组成的空间,而指数映射公式即罗德里格斯公式。
3、从SO(3) ---> so(3) :对数映射
第二部分:SE(3)和se(3)
1、SE(3)对应的李代数
与上面相似的是SE(3),也有对应的李代数。与上面最大的不同是位于空间中,位于空间中。
下面直接给出的内容,如下:
对上式的说明:
(1)是一个6维向量
(2)向量的前3维表示平移(注意和变换矩阵中平移不同,后面会解释)
(3)向量的后3维表示旋转,实际上是中的元素。
(4)在这里符号的含义不同了,在中是将3维向量映射成为3*3反对称矩阵,在这里是将一个6维向量映射称为一个4*4的矩阵,并且不再表示反对称了。
在中,也有类似的李括号运算:
2、SE(3)上的指数映射
(1)同样的,首先来看一下如何调用Sophus库来进行SE(3)和se(3)的相互运算。
#include <iostream>
//Eigen
#include <Eigen/Core>
#include <Eigen/Geometry>
//sophus
#include "sophus/se3.h"
using namespace std;
int main(int argc, char **argv)
{
//可以从旋转矩阵,平移向量构造SE3
Eigen::Matrix3d rotation_matrix = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0, 0, 1)).toRotationMatrix();
Eigen::Vector3d t(1, 0, 0);
Sophus::SE3 SE3_RT(rotation_matrix, t);
//也可以从四元数,平移向量构造SE3
Eigen::Quaterniond q(rotation_matrix);
Sophus::SE3 SE3_QT(q, t);
cout << "SE3_RT = " << SE3_RT << endl;
cout << "SE3_QT" << SE3_QT << endl;
//SE3 --- se(3) : 对数映射
typedef Eigen::Matrix<double, 6, 1> Vector6d;
Vector6d se3 = Sophus::SE3::log(SE3_RT);
cout << "se3 = " << se3.transpose() << endl;
//同样的,在SE3中也有 hat 和 vee 这两个运符
cout << "se3 hat = " << "\n" << Sophus::SE3::hat(se3) << endl;
cout << "SE3 vee = " << "\n" << Sophus::SE3::vee(Sophus::SE3::hat(se3)) << endl;
return 0;
}
(2)数学公式推导
这里直接给出se(3)上的指数映射公式:
下面说一下关于的整理:
我们可以这样理解:平移部分经过指数映射之后,还发生了一次以为系数矩阵的线性变换。
(3)对数映射
这里没有给出直接的公式,但是给出了对数映射的计算思路。
关于对数和指数映射就先到这里了。下一篇会开始李代数求导和扰动模型的学习。