1. Problem
用HDL做定位时会出现如下报错,然后就没办法继续run了,卡在这,然后出现如下报错:
Error: TF_DENORMALIZED_QUATERNION: Ignoring transform for
child_frame_id “odom” from authority “unknown_publisher” because of an
invalid quaternion in the transform (0.0 0.0 0.983641 0.122190)
2. Solution
从报错中可以看出,提示的是四元数出问题了,关键就是说没有归一化,发布TF有严格的要求,四元数必须归一化,也就是xyzw平方和等于1,如果不等于就报上述的错,简单算一下,上述报错提示的四元数是: (0.0 0.0 0.983641 0.122190), 即 0.98364 1 2 + 0.12219 0 2 = 0.982480012981 0.983641^2 + 0.122190 ^2 =0.982480012981 0.9836412+0.1221902=0.982480012981 ,明显不等于1,所以就会出现上述问题,解决方法也简单,在输出之前做个归一化处理即可:
Eigen::Quaterniond quatern = {0.0 0.0 0.983641 0.122190}; // w x y z
quatern.normalize();
odom_trans.transform.rotation.x = quatern.x();
odom_trans.transform.rotation.y = quatern.y();
odom_trans.transform.rotation.z = quatern.z();
odom_trans.transform.rotation.w = quatern.w();
odom_trans.header.stamp = stamp;
odom_trans.header.frame_id = "map";
odom_trans.child_frame_id = odom_child_frame_id;
tf_broadcaster.sendTransform(odom_trans);
用Eigen的库函数可以快速得到结果,也可以直接自己写个归一化公式处理,比如:
x
x
2
+
y
2
+
z
2
+
w
2
=
0
\frac{x}{ \sqrt{x^2+y^2+z^2+w^2}}=0
x2+y2+z2+w2x=0
y
x
2
+
y
2
+
z
2
+
w
2
=
0
\frac{y}{ \sqrt{x^2+y^2+z^2+w^2}}=0
x2+y2+z2+w2y=0
z
x
2
+
y
2
+
z
2
+
w
2
=
0.983641
0.982480012981
=
0.9923725907873522
\frac{z}{ \sqrt{x^2+y^2+z^2+w^2}}=\frac{0.983641}{0.982480012981}=0.9923725907873522
x2+y2+z2+w2z=0.9824800129810.983641=0.9923725907873522
w
x
2
+
y
2
+
z
2
+
w
2
=
0.122190
0.982480012981
=
0.12327465698187301
\frac{w}{ \sqrt{x^2+y^2+z^2+w^2}}=\frac{0.122190}{0.982480012981}=0.12327465698187301
x2+y2+z2+w2w=0.9824800129810.122190=0.12327465698187301
引起这个问题的原因是之前改了hdl里面的算法部分,修改了输出的四元数,默认不会出现roll和pitch的情况,所以四元数的x和y都设成0,但是修改了这个后,又忘了重新给修改后的四元数归一化再输出,就出现了上述的报错。
在这里感谢实验室里的好伙伴,yongji,发现了解决办法
困扰了挺久,原来是漏了归一化的原因。如果遇到类似报错,也应该从这个角度去检查,应该很快能定位到问题所在。