SLAM学习——eigen库实现四元数与旋转矩阵的转换

任务

已知相机的位姿用四元数表示为q=[0.35,0.2,0.3,0.1],顺序为x,y,z,w，请编程实现：

输出四元数对应的旋转矩阵、旋转矩阵的转置，旋转矩阵的逆矩阵，旋转矩阵乘以自身的转置，验证旋转矩阵的正交性。

题目来源：从零开始一起学习SLAM | 学习SLAM到底需要学什么？－计算机视觉life

过程

配置eigen库

安装

sudo apt-get install libeigen3-dev

由于之前已经配置过ROS系统，而在eigen的官网可以看到ROS是使用Eigen的，所以系统中已经存在了Eigen库，但在编译时会报错，根据博文Ubuntu16.04安装eigen3并为其设置软链接，否则编译OpenCV库源码会提示出错提供的解决方案可以解决。

查看安装位置

locate eigen3

发现库文件夹在/usr/include中下的eigen3中，如下建立软链接，即可

建立软链接

软链接的方式则是产生一个特殊的文件，该文件的内容是指向另一个文件的位置。

sudo ln -s /usr/include/eigen3/Eigen /usr/include/Eigen
sudo ln -s /usr/include/eigen3/unsupported /usr/include/unsupported

eigen使用

四元数

根据文档，四元数和一些旋转有关的定义需要引入头文件#include<Eigen/Geometry>

定义

初始化的顺序为w,x,y,z

Quterniond q((0.1,0.35,0.2,0.3);

可以通过函数coeffs()按照x，y，z，w顺序打印成员，vec()打印虚数x,y,z部分

     cout<<"quaternion x y z w:\n "<<q.coeffs()<<endl;
     cout<<"x y z:\n "<<q.vec()<<endl;

eigen库已经集成有高度优化的算法，可以通过初始化的方式直接将四元数转化为旋转矩阵的形式，但首先要进行归一化处理。

归一化

q.normalize();

normalize()与normalized()区别：前者会改变当前变量的值，后者是临时的矩阵。

旋转矩阵

四元数和旋转矩阵都可以表示3维旋转，但四元数以其极小的数据量占有优势，四元数表示旋转的推导可参考之前的博文四元数旋转公式推导，今天又推导了四元数到旋转矩阵的转换，主要思路是利用公式：

$$
q\boldsymbol{x}q*=R\boldsymbol{x}
$$
可以推得旋转矩阵通过四元数的表示方法。

完整代码

#include<iostream>
#include<Eigen/Dense>
#include<Eigen/Geometry>
using namespace std;
using namespace Eigen;
int main(void)
{
        //输出四元数对应的旋转矩阵、旋转矩阵的转置，旋转矩阵的逆矩阵，旋转矩阵乘以自身的转置，验证旋转矩阵的正交性。
        //1.输出旋转矩阵
        Quaternionf q(0.1,0.35,0.2,0.3);
        cout<<"quaternion x y z w:\n "<<q.coeffs()<<endl;
        cout<<"x y z:\n "<<q.vec()<<endl;
        //单位化
        Quaternionf qu=q.normalized();
        cout<<"after normalized:\n"<<qu.coeffs()<<endl;
        cout<<"rotation matrix:\n"<<qu.matrix()<<endl;
        //2.输出矩阵转置
        Matrix3f rm=qu.matrix();
        Matrix3f rm_t=rm.transpose();
        cout<<"transpose of rotation matrix:\n"<<rm_t<<endl;
        //3.逆矩阵
        Matrix3f rm_i=rm.inverse();
        cout<<"inverse of rotation matrix:\n"<<rm_i<<endl;
        //4.验证正交性
        Matrix3f rm_o=rm_t*rm;
        cout<<"cross of transpose and itself\n"<<rm_o<<endl;
        return 0;
}

编译

可以直接通过命令行编译为可执行文件quaternion2matrix

g++ quaternion2matrix.cpp -o quaternion2matrix

cmake

学习使用cmake进行配置CMakeList.txt的编写

# CMake 最低版本号要求
cmake_minimum_required (VERSION 2.8)
# 项目信息
project (quaternion2matrix)
# 指定生成目标
add_executable(quaternion2matrix quaternion2matrix.cpp)

要得到可执行文件需要在命令行输入下列命令：

make .
cmake quaternion2matrix

结果展示