如何将四元数转化为三轴旋转坐标（Rodrigues旋转公式）

在日常编程中，不同的地方使用的角度表示方式不一定相同。例如hector中输出的位姿使用的是直角坐标系的形式，而ROB_SLAM中位姿通常以四元数形式保存。如果我们同时需要使用到这两者进行运算。就需要进行两者之间的转换。

Rodrigues旋转公式提供了一个非常方便的转换。

具体推导公式参考百度百科：

https://baike.baidu.com/item/%E7%BD%97%E5%BE%B7%E9%87%8C%E6%A0%BC%E6%97%8B%E8%BD%AC%E5%85%AC%E5%BC%8F/18878562?fr=aladdin

使用方式如下：
1、将直角坐标系转化为四元数：

#include <iostream> //这里是需要的两个头文件
#include <opencv2/opencv.hpp>

	//A代表直角坐标系。B代表四元数
	Mat A = (Mat_<double>(3, 1) << 0, 1 , 0);
	Mat B;
	//转化成旋转矩阵
	Rodrigues(A, B);
 

2、将四元数转化为直角坐标系：

#include <iostream> //这里是需要的两个头文件
#include <opencv2/opencv.hpp>

	//A代表四元数。B代表直角坐标系
	Mat A = (Mat_<double>(3, 1) << 1, 0 , 0, 0 ,1, 0 , 0 ,0, 0 ,1);
	Mat B;
	//转化成旋转矩阵
	Rodrigues(A, B);
 

可以看出，其实不管是从四元数转换为直角坐标还是从直角坐标转变为四元数。其只需要使用到Rodrigues(A, B)一个函数就可以实现了。当前面为直角坐标的时候它会自动将其转变为四元数。当前面是四元数的时候，它会自动将其转变为直角坐标。这大大方便了数据的转化。