VINS中欧拉角转换问题和解决方案【.eulerAngles和Utility::R2ypr不一致的问题】

我在VINS中想把Utility::R2ypr 替换成 .eulerAngles(2,1,0),出现了结果不一致的情况。

转载：https://zhuanlan.zhihu.com/p/55790406?utm_source=wechat_session

1 四元数转欧拉角

普通的方法是，用Eigen，把四元数转成旋转矩阵，再从旋转矩阵转到欧拉角：
::Eigen::Quaterniond q(w, x, y, z);
::Eigen::Matrix3d rx = q.toRotationMatrix();
::Eigen::Vector3d ea = rx.eulerAngles(2,1,0);
但这个方法存在问题，即可能转出来的欧拉角，不是想要的，因为因为同一个四元数，可以用2个欧拉角来表示，而这个方法得到的结果有可能是用转角大于2PI的方式表达的。例如，四元数(0.00392036, -0.00511095, -0.613622, 0.789573)应当转为欧拉角(-1.32133, -0.00325971, 0.0124636)，但用Eigen却被转成了(1.82026, -3.13833, -3.12913)。为了避免这个问题，可以使用 Conversion between quaternions and Euler angles 中给出的一个算法（如下），这个算法可以保证出来的欧拉角不会超过2PI。

static void toEulerAngle(const Quaterniond& q, double& roll, double& pitch, double& yaw)
{
// roll (x-axis rotation)
double sinr_cosp = +2.0 * (q.w() * q.x() + q.y() * q.z());
double cosr_cosp = +1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y());
roll = atan2(sinr_cosp, cosr_cosp);

// pitch (y-axis rotation)
double sinp = +2.0 * (q.w() * q.y() - q.z() * q.x());
if (fabs(sinp) >= 1)
pitch = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
else
pitch = asin(sinp);

// yaw (z-axis rotation)
double siny_cosp = +2.0 * (q.w() * q.z() + q.x() * q.y());
double cosy_cosp = +1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z());
yaw = atan2(siny_cosp, cosy_cosp);
}

 

2 欧拉角转四元数

这就非常容易了：
tf::Quaternion q;
q.setRPY(roll, pitch, yaw);

Eigen的方式是：
Eigen::Vector3d ea0(yaw,pitch,roll);
Eigen::Matrix3d R;
R = Eigen::AngleAxisd(ea0[0], ::Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(ea0[1], ::Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(ea0[2], ::Eigen::Vector3d::UnitX());
Eigen::Quaterniond q;
q = R;

 

 

最终结论如下：

0、推荐用Utility::R2ypr ，因为车体yaw角是允许有负值的。【同时保险起见 ypr2R 和 R2ypr同时使用】

1. 两种转换可以转成功！！！实测
2. Utility::R2ypr得到的yaw是±180之间，.eulerAngles(2,1,0)是在（0deg～180deg），故yaw为正值（0~180deg）时，两者相同。Yaw为负值，两者不一致【后者为保证正值，在前者上加了180度】。做了很多组实验结果亦表明：yaw为正值（0～180度）时两者相同，yaw为（0～负180度）时两者不同
3. 从代码层面，证据如下：

 

 

从上面可知：

Utility::R2ypr得到的yaw，pitch，roll均利用atan2函数故只能输出±180之间，（注意atan2不同atan，后者只能±90）

而.eulerAngles得到的是[0:pi] [-pi:pi] [-pi:pi]，

注：从.eulerAngles代码具体实现可知：

 

该.eulerAngles代码中若yaw小于0，则+=PI，故yaw不会小于0!!! 同时会把pitch的atan2中x乘以负号，也就是意味着把pitch转了个180度！！！

正在上传…重新上传取消

注：做了很多组实验结果亦表明：yaw为正值（0～180度）时两者相同，yaw为（0～负180度）时两者不同

测试时：都先转成四元数，再转成旋转矩阵，再对同一旋转矩阵采用如下两种方法：【跟R怎么来的没关系，因为中间经过了四元数】【但保险起见 ypr2R 和 R2ypr同时使用】

Utility::R2ypr	.eulerAngles（2，1，0）	是否一致
（0 0 0 ）	（0 0 0 ）	一致
（0.01 0 0 ）	（0.01 0 0 ）	一致
（0.8762 -0.25 89.60 ）	（0.8762 -0.25 89.60）	一致
（80 0 90 ）	（80 0 90）	一致
（90 0 90 ）	（90 0 90）	一致
（100 0 90 ）	（100 0 90）	一致
（-0.01 0 0 ）	（179.99 180 180 ）	不一致
（-0.004 -0.1 89.56 ）	（179.99 -179.9 -90）	不一致
（-90 0 90 ）	（90 -180 -90）	不一致
（-100 0 90 ）	（80 -180 -90）	不一致

 

 

 

 

问题如下：

过程代码如下：

转存失败重新上传取消

结果如下：

 

第一组结果对比 前者结果是（-0.004 -0.1 89.56 ）后者是 （179.99 -179.9 -90）

转存失败重新上传取消

第二组结果对比 前者结果是（0.8762 -0.25 89.60 ）后者是 （0.8762 -0.25 89.60）

转存失败重新上传取消

从上面结果可知：

第一组结果【即：在小车没动的时候，yaw理论上应该为0deg】，但方法一和方法二结果不同；（0,0,90）和（180,180，,90）都能实现相同转换！

第二组结果【即：在小车没动的时候，yaw理论上不为0deg】，方法一和方法二结果就相同了。

我现在困惑的是：小车没动的时候，方法一和方法二的欧拉角分解结果尽管不同，但都是对的。 

如果一个旋转矩阵有两种欧拉角分解，那么在4DOF优化时，如果只对yaw优化，会不会出问题？？？

 

另一组实验：两种转换都可以

前者结果是（-90 0 90 ）后者是 （90 -180 -90）不一致

前者结果是（-100 0 90 ）后者是 （80 -180 -90）不一致

前者结果是（90 0 90 ）后者是 （90 0 90） 一致

前者结果是（100 0 90 ）后者是 （100 0 90） 一致

前者结果是（80 0 90 ）后者是 （80 0 90） 一致

转存失败重新上传取消转存失败重新上传取消转存失败重新上传取消

 

 

 

转存失败重新上传取消

转存失败重新上传取消转存失败重新上传取消

转存失败重新上传取消转存失败重新上传取消

 

转存失败重新上传取消