Hector_Slam orientation probelm for drone

Question

We are using Hector slam(2-D Mapping) for our drone in which we are facing some problems

1) as the orientation(axis) of the lidar changes the orientation of map changes

2) as the plane of the lidar changes the map order distorts.

Please give tips for these problems

Answer 1

Disclaimer: I have not used the Hector libraries, but I have heard of them in the past.

1. 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.

2. 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.