Eigen中旋转矩阵和欧拉角的转换
Eigen中旋转矩阵和欧拉角的转换
在计算机视觉研究中常常用到物体的姿态转换,一般在计算中常用旋转矩阵和四元数,然而二者在表示物体位姿时并不直观,通常采用欧拉角去表示物体的姿态。所幸,Eigen中已经给出了现成的欧拉角与旋转矩阵的转换函数。
Eigen中欧拉角转换旋转矩阵
在计算中常常遇到已知两坐标系三个轴的角度转换关系,求解二者之间的旋转矩阵。下面举例来进行说明,已知坐标系A和坐标系B,其中B是由A先通过Z轴旋转
γ
\gamma
γ度,再在旋转后坐标的基础上沿Y轴旋转
β
\beta
β度,在此基础上再沿X轴旋转
α
\alpha
α度,则两坐标系之间的旋转矩阵
R
A
B
R_{AB}
RAB可由下式求得:
R
A
B
=
R
z
(
γ
)
∗
R
y
(
β
)
∗
R
x
(
α
)
R_{AB} =R_z(\gamma) * R_y(\beta) * R_x(\alpha)
RAB=Rz(γ)∗Ry(β)∗Rx(α)
上式用Eigen代码表示如下:
int main(int argc, char **argv)
{
Eigen::Vector3d rpy_raw, ypr;
rpy_raw << 30, 60, 150;
rpy_raw = rpy_raw * M_PI / 180;
Eigen::Matrix3d R_AB;
//输入部分为YPR,所以转换的输出也是YPR
R_AB = Eigen::AngleAxisd(rpy_raw[2], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(rpy_raw[1], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(rpy_raw[0], Eigen::Vector3d::UnitX());
ypr = R_AB.eulerAngles(2, 1, 0);
cout << ypr.transpose() * 180 / M_PI << endl;
return 0;
}