Inaccuracies in tracking Z for drone SLAM

[luca-della-vedova]

Hi and thanks for the great package!

I'm trying to run a gazebo simulation with a drone with a 16 beam lidar in a simple environment like the one shown below (there actually is a floor!):

Now what I see is excellent performance on the XY plane, but poor tracking in the Z direction. I had to apply the solution in #74 to avoid the drift so I don't know if that might have caused any issue. This is an example of the pointcloud I'm currently working with:

And this the result with a quadcopter takeoff and doing circles, the /mavros is the ground truth while the /odom is the one published by the slam, again excellent in XY but loses tracking of Z quite fast:

I'm mostly using default parameters from the launch file, I only applied #74 and added floor detection just to experiment, as well as tuning the sensor height to my application but it didn't make any difference. Of course I also added IMU. Any idea what might cause such behavior? Is there a specific tuning I should be looking at for drones? I'm guessing the standard 2D application doesn't expect large shifts in Z and hence might be more conservative in that degree of freedom, but this is just a wild guess.

Cheers, and again great package! /Luca

[koide3]

Hi @luca-della-vedova , Hmmm, the odometry estimation is a pure 6DoF algorithm, and it doesn't have a special care of Z-direction shift. I guess the drift is caused by the sparsity of the point cloud (The floor plane points look fewer than the wall points). Could you try to:

• increase/decrease "ndt_resolution" (it drastically changes the odometry accuracy)
• use "GICP" instead of "NDT_OMP" (GICP is more robust to sparse points)

[luca-della-vedova]

Hi @koide3

Thanks for the support! From a quick test (both double / half the parameter and with the GICP algorithm) I haven't seen much improvement. I'll keep trying to tune the algorithm and add more features to the environment. If I don't get any result would you be interested in adding a feature to fuse a Z sensor, especially useful for UAV applications? (i.e. barometer / laser). I think I have an idea of how to integrate it and if you think it might be useful I'll try to work on it and open a PR.

Cheers! /Luca

[koide3]

Hi @luca-della-vedova , Yes, it would be very interesting to have a new altitude constraint. I would be grateful if you could open a PR about this.

FYI: You can create a new graph constraint by defining a g2o edge class. The following code for XYZ positional constraints could be helpful to know how you can define a new edge.

https://github.com/koide3/hdl_graph_slam/blob/master/include/g2o/edge_se3_priorxyz.hpp