search

cartographer-project / cartographer

Cartographer is a system that provides real-time simultaneous localization and mapping (SLAM) in 2D and 3D across multiple platforms and sensor configurations.
Apache License 2.0
7.04k stars 2.24k forks source link

Fix raytracing origin #1850

Open ValerioMagnago opened 2 years ago

ValerioMagnago commented 2 years ago

Adds the laser ray origin to RangefinderPoint to enable correct raytracing of range data misses into a 2D probability grid also after scan accumulation.

Previously, all rays were casted from their hit point to the tracking frame. This is wrong for any setup where the tracking frame is not the sensor frame and leads to artifacts in the map.

Fixes: #947

Below an example of a map generated from a multiple 2D laser scanners configuration, which are not centered in the tracking frame, before and after the proposed fix. It can be seen clearly that the previous version of the software was cutting and cleaning the corners of the map due to the imprecise origin used for raytracing. This caused the exclusion of large structures from the map. image image

tulku commented 2 years ago

@ValerioMa this is great to see! One question though, do you have a matching version of cartographer-ros that works with these changes? Maybe I did something wrong, but I failed to build the assets_writer.cc while trying this change.

ValerioMagnago commented 2 years ago

@tulku yes we have it. I have created the related PR on cartographer_ros (here), let me know if works for you.

tulku commented 2 years ago

Thanks @ValerioMa I was using a very similar fix.

I am not very familiar with cartographer code, however, I was wondering why you choose to not add the origin to TimedRangefinderPoint also, and then keep the current idea of the Timed one being a RangefinderPoint + time. Instead you are adding the origin when converting from Timed to a non timed one in sensor::ToRangefinderPoint, which then needs to look for the origin of that point somewhere else.

Also, it looks like you should updated inline bool operator==(const RangefinderPoint& lhs, const RangefinderPoint& rhs) to also check for the origin.

ValerioMagnago commented 2 years ago

@tulku thanks for your suggestion and sorry delay! Now the origin is added to the operator == and I also some tests that were not initializing the origin are now fixed.

Regarding the TimedPointCloud I left it without the origin because for that purpose we have the TimedPointCloudOridinData.