在Eigen裡面的quaterniond要注意W 實部的位置，在初始化的時候是放在第一個元素。
然而在輸出的時候，他的順序卻是xyzw把實部放在最後儲存。

而這邊示範用Eigen裡面的軸角表示法來初始化quanterniond

Eigen::AngleAxisd rotation_vector(CV_PI / 4, Eigen::Vector3d(0, 0, 1));//沿 Z 轴旋轉 45 度
Eigen::Quaterniond tmp = Eigen::Quaterniond(rotation_vector);
std::cout << "quaternion = \n" << tmp.coeffs() << std::endl;   // 請注意coeffs的順序是(x,y,z,w),w為實部，前三者為虚部

std::cout << "acos() = \n" << acos(tmp.w()) << std::endl;      //四元數轉角是原本的1/2
std::cout << "original angle = \n" << (CV_PI / 4) << std::endl;

Eigen::Quaterniond test_normalize(10,1,2,3);
std::cout << "test_normalize = \n" << test_normalize.coeffs() << std::endl;
test_normalize.normalize();
std::cout << "test_normalize = \n" << test_normalize.coeffs() << std::endl;

Eigen::Quaterniond tmp1;
tmp1= tmp1.UnitRandom();
std::cout << "tmp1 = \n" << tmp1.coeffs() << std::endl;

其中 四元數很方便的是當我們要知道旋轉幾度的時候，我們只要對w實部的部分取acos()就能得到旋轉角度的一半，
將acos()的回傳值乘以2就是旋轉的角度。
而如果你初始化的四元數不是單位四元數，也可以透過normalize()來將四元數標準化，而UnitRandom()也可以直接獲得一個隨機的單位四元數。

而在四元數的旋轉中，你可以將要被旋轉的點視為實部為0的特殊四元數，虛部xyz分別對應3D點的xyz座標。
而在四元數的旋轉中，只要將這個特殊的四元數分別左乘單位四元數與右乘單位四元數的共軛即可。
而四元數的共軛也是相當容易，只需要在虛部的地方乘一個-1即可。
寫起來有點像下面的樣子。

(Eigen::Quaterniond(testQ.w(), testQ.x(), testQ.y(), testQ.z()))*
Eigen::Quaterniond(0, point_x, point_y, point_z)) * 
(Eigen::Quaterniond(testQ.w(), -testQ.x(), -testQ.y(), -testQ.z()));

最後我們也可以將四元數轉換成比較熟悉的3X3矩陣透過矩陣的運算來驗證四元數的旋轉結果是不是如同你所想的一樣。

	Eigen::Matrix3d tmp_mat = testQ.toRotationMatrix();
	Eigen::Vector3d v(point_x, point_y, point_z);
	std::cout << "rotation = \n" << tmp_mat << std::endl;
	Eigen::Vector3d restult = tmp_mat* v;
	std::cout << "after rotation: [" <<restult << " ]" << std::endl;