右手系转左手系、旋转矩阵转四元数、四元数的两种表达（Hamilton/JPL)

最近一个项目需要使用unity做可视化，由于unity使用左手系而我的代码中都是使用右手系，在转化的过程中踩了很多坑，简单记录一下。

右手系转左手系

此处参考这篇博文：从左手坐标系到右手坐标系的变换

左手系转右手系，简单来说就是反转其中一个轴即可。对于坐标点直接取反，以 z 轴取反为例：

式中S_z为：

而对于旋转的处理稍有不同：假设左手系中旋转矩阵为R，转换到右手系中为S_z ·R·S_z。

右手系转左手系同理。将位姿 T 拆分为旋转矩阵 R 和位置向量 t 分别处理即可。

旋转矩阵转四元数

直接在网上查找将旋转矩阵转化为四元数的代码，却发现了两种不同的代码，使用网上的代码和MATLAB计算结果进行比较也发现了一些差异，于是我就困惑了：难道左手系的旋转矩阵和右手系的旋转矩阵有什么区别？或者左右手系下旋转矩阵转四元数的定义不同？

花了很多时间查阅资料后进一步了解到，其实两种计算方法的差异是由四元数的两种表达形式造成的。

四元数的两种表达：Hamilton/JPL

此处参考文章：
四元数旋转表达（Hamilton notation & JPL notation）
四元数的两种 notation：Hamilton 和 JPL

在Hamilton 表达中，$ij=k,i^2+j^2+k^2=ijk=-1$，在这种定义下，单位四元数转化为旋转矩阵的公式为：

在JPL表达中，$ij=-k,ijk=1$，在这种定义下，单位四元数转化为旋转矩阵是上式的转置：

两种表达的区别：

（两种定义应该是对应左右手系的差异，Hamilton对应右手系，JPL对应左手系）

定义的差异直接导致计算公式的不同，但实际应用中只要统一标准应该问题不大。

Eigen ，Matlab ，ROS、Google Ceres Solver等都使用Hamilton四元数，而JPL则多用于航空航天领域）。

（实测Unity应该也是Hamilton，奇怪的是unity使用左手系却使用右手系定义的四元数？此处存疑）

Matlab中的四元数定义是Hamilton四元数，但是应用转换函数quat2dcm(）时却发现是以JPL四元数形式处理的，实在有点混乱。。。这里有个解释把左手法则和右手法则以及两种四元数分开处理了：
StackOverflow

两种转换代码

Hamilton：
基于OpenCV的四元数、旋转矩阵和欧拉角互相转换(一)

void getQuaternion(cv::Mat R, double Q[])//Hamilton
{
	double trace = R.at<double>(0, 0) + R.at<double>(1, 1) + R.at<double>(2, 2);

	if (trace > 0.0)
	{
		double s = sqrt(trace + 1.0);
		Q[3] = (s * 0.5);
		s = 0.5 / s;

		//主要区别在此，即减数与被减数顺序
		Q[0] = (((R.at<double>(2, 1) - R.at<double>(1, 2))) * s);
		Q[1] = (((R.at<double>(0, 2) - R.at<double>(2, 0))) * s);
		Q[2] = (((R.at<double>(1, 0) - R.at<double>(0, 1))) * s);
	}

	else
	{
		int i = R.at<double>(0, 0) < R.at<double>(1, 1) ? (R.at<double>(1, 1) < R.at<double>(2, 2) ? 2 : 1) : (R.at<double>(0, 0) < R.at<double>(2, 2) ? 2 : 0);
		int j = (i + 1) % 3;
		int k = (i + 2) % 3;

		double s = sqrt(R.at<double>(i, i) - R.at<double>(j, j) - R.at<double>(k, k) + 1.0);
		Q[i] = s * 0.5;
		s = 0.5 / s;

		Q[3] = ((R.at<double>(k, j) - R.at<double>(j, k))) * s;
		Q[j] = ((R.at<double>(j, i) + R.at<double>(i, j))) * s;
		Q[k] = ((R.at<double>(k, i) + R.at<double>(i, k))) * s;
	}
}

JPL：
从旋转矩阵到四元数

void getQuaternion(cv::Mat R, double Q[])//JPL
{
	double m11=R.at<double>(0,0), m12 = R.at<double>(0, 1), m13 = R.at<double>(0, 2);
	double m21 = R.at<double>(1, 0), m22 = R.at<double>(1, 1), m23 = R.at<double>(1, 2);
	double m31 = R.at<double>(2, 0), m32 = R.at<double>(2, 1), m33 = R.at<double>(2, 2);

	double w, x, y, z;

	//探测四元数中最大的项 
	double fourWSquaredMinusl = m11 + m22 + m33;
	double fourXSquaredMinusl = m11 - m22 - m33;
	double fourYSquaredMinusl = m22 - m11 - m33;
	double fourZSquaredMinusl = m33 - m11 - m22;

	int biggestIndex = 0;
	double fourBiggestSqureMinus1 = fourWSquaredMinusl;
	if (fourXSquaredMinusl > fourBiggestSqureMinus1) {
		fourBiggestSqureMinus1 = fourXSquaredMinusl;
		biggestIndex = 1;
	}
	if (fourYSquaredMinusl > fourBiggestSqureMinus1) {
		fourBiggestSqureMinus1 = fourYSquaredMinusl;
		biggestIndex = 2;
	}
	if (fourZSquaredMinusl > fourBiggestSqureMinus1) {
		fourBiggestSqureMinus1 = fourZSquaredMinusl;
		biggestIndex = 3;
	}

	//计算平方根和除法 
	double biggestVal = sqrt(fourBiggestSqureMinus1 + 1.0f) * 0.5f;
	double mult = 0.25f / biggestVal;

	//计算四元数的值
	switch (biggestIndex) {
	case 0:
		w = biggestVal;
		x = (m23 - m32) * mult;
		y = (m31 - m13) * mult;
		z = (m12 - m21) * mult;
		break;
	case 1:
		x = biggestVal;
		w = (m23 - m32) * mult;
		y = (m12 + m21) * mult;
		z = (m31 + m13) * mult;
		break;
	case 2:
		y = biggestVal;
		w = (m31 - m13) * mult;
		x = (m12 + m21) * mult;
		z = (m23 + m32) * mult;
		break;
	case 3:
		z = biggestVal;
		w = (m12 - m21) * mult;
		x = (m31 + m13) * mult;
		y = (m23 + m32) * mult;
		break;
	}
	Q[0] = x;
	Q[1] = y;
	Q[2] = z;
	Q[3] = w;
}