search

ANYbotics / elevation_mapping

Robot-centric elevation mapping for rough terrain navigation
BSD 3-Clause "New" or "Revised" License
1.31k stars 445 forks source link

NaN values in Elevation map #138

Closed doterkuile closed 3 years ago

doterkuile commented 4 years ago

Hi!

I am using the elevation mapping package in combination with a custom robot. I noticed that when I used the perfectSensorProcessor I got a lot of NaN values in the grid map during updating of the map. This results in holes in the map (see figure). Their location and the amount of holes vary after every update.

I tried to find what the problem was and I traced the problem to the calculation of the mahalanobisDistance in ElevationMap::Add(). When the variance of the new point of the pointcloud is zero and the difference between point.z and elevation is zero as well it results in a NaN. The mahalanobisDistanceThreshold check returns false and the point will be fused with the current elevation value of the map. However here another zero dividing by zero occurs resulting in the new elevation and variance value to be NaN, creating the holes in the map.

To fix it I set all NaN values for the mahalanobisDistance to infinity so it will do the mahalanobisDistanceThreshold check. However I found also that the variance of all points is zero because my robotPoseCovariance is matrix of zeros (it is published by tf_to_pose_publisher.py), in addition the perfectSensorProcesser which does not have any variance.

I would like to know if I did something wrong with the robotPoseCovariance or is this a bug in the mahalanobisdistance calculation?

Regards, David

elevation_map_holes

maximilianwulf commented 4 years ago

Hey @doterkuile, cool robot, which robot is that?

So in general if you use a perfect sensor the variance should be zero since it is perfect data. It should result in NaN since we divide by zero, https://github.com/ANYbotics/elevation_mapping/blob/master/elevation_mapping/src/ElevationMap.cpp#L114

For the moment I would propose you stick with your solution. We will look into it in more detail, but given the current work load I expect results earliest in 1 month.

doterkuile commented 4 years ago

Thanks for looking into it! For now I will stick to my current solution.

The robot is Talos from PAL Robotics, where I did an internship.

maximilianwulf commented 4 years ago

Okay, good to know.