TF报错及解决方法 Error: TF_DENORMALIZED_QUATERNION

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.983641^2+0.122190^2=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的库函数可以快速得到结果，也可以直接自己写个归一化公式处理，比如：
$$\frac{x}{\sqrt{x^2+y^2+z^2+w^2}}=0$$
$$\frac{y}{\sqrt{x^2+y^2+z^2+w^2}}=0$$
$$\frac{z}{\sqrt{x^2+y^2+z^2+w^2}}=\frac{0.983641}{0.982480012981}=0.9923725907873522$$
$$\frac{w}{\sqrt{x^2+y^2+z^2+w^2}}=\frac{0.122190}{0.982480012981}=0.12327465698187301$$

引起这个问题的原因是之前改了hdl里面的算法部分，修改了输出的四元数，默认不会出现roll和pitch的情况，所以四元数的x和y都设成0，但是修改了这个后，又忘了重新给修改后的四元数归一化再输出，就出现了上述的报错。

在这里感谢实验室里的好伙伴，yongji，发现了解决办法
困扰了挺久，原来是漏了归一化的原因。如果遇到类似报错，也应该从这个角度去检查，应该很快能定位到问题所在。