void matrix2angle(Eigen::Matrix4f &result_trans, Eigen::Vector3f &result_angle)
{
double ax, ay, az;
if (result_trans(2, 0) == 1 || result_trans(2, 0) == -1)
{
az = 0;
double dlta;
dlta = atan2(result_trans(0, 1), result_trans(0, 2));
if (result_trans(2, 0) == -1)
{
ay = M_PI / 2;
ax = az + dlta;
}
else
{
ay = -M_PI / 2;
ax = -az + dlta;
}
}
else
{
ay = -asin(result_trans(2, 0));
ax = atan2(result_trans(2, 1) / cos(ay), result_trans(2, 2) / cos(ay));
az = atan2(result_trans(1, 0) / cos(ay), result_trans(0, 0) / cos(ay));
}
result_angle << ax, ay, az;
}
08-31
1232
![](https://csdnimg.cn/release/blogv2/dist/pc/img/readCountWhite.png)
05-10
2858
![](https://csdnimg.cn/release/blogv2/dist/pc/img/readCountWhite.png)