Fix raytracing origin

[ValerioMagnago]

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.

[tulku]

@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]

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

[tulku]

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]

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