深入理解空間座標系的矩陣變換
空間中三維座標變換一般由三種方式實現，第一種是旋轉矩陣和旋轉向量；第二種是歐拉角；第三種是四元數。這裏先介紹旋轉矩陣(旋轉向量)與歐拉角實現三維空間座標變換的方法以及兩者之間的關係。
空間變換分析：
這裏以常見的世界座標系與相機座標系間的變換爲例。
從相機座標系轉換到世界座標系，也就是比較通用的body到世界座標系間的轉換。
那麼旋轉的歐拉角按從世界座標系轉換到相機座標系的過程，先按z軸旋轉、之後y軸旋轉、之後x軸旋轉，最終得到相機座標系，得到的角度分別是yaw、pitch、roll，那麼從相機座標系到世界座標系的旋轉矩陣按如下方式定義:
那麼得到的相機（body）座標系到世界座標系間的旋轉矩陣爲：
對應四元數變換矩陣：
Opencv實現代碼：
cv::Mat IMUReader::angleToRotation(COORDINATES& carrier)
{
// R
cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
R.at(0, 0) = cos(carrier.yaw)*cos(carrier.pitch);
R.at(0, 1) = cos(carrier.yaw)*sin(carrier.roll)*sin(carrier.pitch) - cos(carrier.roll)*sin(carrier.yaw);
R.at(0, 2) = sin(carrier.yaw)*sin(carrier.roll) + cos(carrier.yaw)*cos(carrier.roll)*sin(carrier.pitch);
R.at(1, 0) = cos(carrier.pitch)*sin(carrier.yaw);
R.at(1, 1) = cos(carrier.yaw)*cos(carrier.roll) + sin(carrier.yaw)*sin(carrier.roll)*sin(carrier.pitch);
R.at(1, 2) = cos(carrier.roll)*sin(carrier.yaw)*sin(carrier.pitch) - cos(carrier.yaw)*sin(carrier.roll);
R.at(2, 0) = - sin(carrier.pitch);
R.at(2, 1) = cos(carrier.pitch)*sin(carrier.roll);
R.at(2, 2) = cos(carrier.roll)*cos(carrier.pitch);
return R;
}
對於平移矩陣爲相機座標系原點在世界座標系下的座標T，最終得到的座標變換方程如下：
其理解過程爲從世界座標系經過z，y，x的旋轉以及平移T後得到相機座標系，那麼從相機座標轉換的世界座標實際上是反變換過程，最後一步是x旋轉，那麼通過定義Rx爲反旋轉將相機座標反旋轉過來，之後依次是y，z，旋轉後得到的值爲與世界座標系同方向，但是有一個平移的T的座標系下的座標，之後再加一個T座標就得到了在世界座標系下的座標。
齊次座標變換矩陣：
