Disclaimer:
I have not used the Hector libraries, but I have heard of them in the past.
- Have you looked at homogeneous transformation matrices? Assuming that you only have a change in Yaw to keep track of, you can use the Z-axis rotation matrix to rotate the point to the correct location.
[ cos(theta) -sin(theta) 0 0 ]
[ sin(theta) cos(theta) 0 0 ]
[ 0 0 1 0 ]
[ 0 0 0 1 ]
Multiply this matrix with the 3D point vector
[ x ]
[ y ]
[ z ]
[ 1 ]
And then you can rectify for the point for lidar Z-axis rotation.
- You can take the RPY values from an IMU and then check to see if the
R
and P
values pass a threshold. If the magnitudes of those R
and P
values are too large, then you can simply not count that point in the reading. You will be dropping data points, but that isn't necessarily a bad thing.
The smarter way (IMHO) to use the data is to use 3D SLAM
to create a full-on point cloud. That is probably too computationally expensive, but I think these methods are good enough for a start, and it looks like you want a simpler methods anyways.