Using Time of Flight Sensors

[camasmie]

Hello @matlabbe,

I have added four T.O.F. sensors to our robot—two front and two rear. I am using them for additional collision avoidance, but I also wanted to use their information to improve the SLAM performance.

These are amazing little sensors from ST (https://www.st.com/en/imaging-and-photonics-solutions/vl53l8cx.html), and they have an 8x8 multi-zone.

I was wondering how to integrate that information into rtabmap. Can I use your standard LiDar implementation and feed in their information with some parameter modification? I look at them as four separate sparse three-dimensional LiDar; do you agree?

Otherwise, I am considering running a 'scan matching' algorithm at each node, sending that 'odometry' over our CAN Bus, and fusing it (EKF) it with the visual and wheel odometry. I implemented scan matching in grad school and found it very elegant, simple, and powerful.

Do you have any recommendations?

Thank you in advance,

Paulo

[matlabbe]

Hello Paulo,

With enough of them, you could indeed do some lidar scan matching with them. They have 45 deg horizontal FOV, so with 4 you can get 180 deg coverage with 32 points, or 360 deg with 4x45 deg blindspots.

One way to combine those scans for icp_odometry would be to use rtabmap_util/point_cloud_aggregator node. Ideally using wheel odometry/visual odometry as fixed frame when assembling them, then use that odometry frame as guess_frame_id for icp_odometry.

Not sure if only 32 points on horizontal (with limited range <4 meters) would be enough to robustly do scan matching. For cluttered spaces it may work, but probably won't in large empty spaces. For example, with rplidar 4 meters, it has 360 coverage with low resolution of points, it works in an house or apartment, but for large spaces the scan matching may fail (not enough correspondences between consecutive frames).

cheers,
Mathieu

[camasmie]

Hello Mathieu,

Thank so much for your reply, for looking into the sensor, and suggestions. I will look into the point_cloud_aggregator.

Reading your answer, it got me thinking that a simple, effective solution might be to run four independent 'scan matches' for each sensor and micro-controller, locally at the low level (I could write that in my firmware). I would simply compute the transform between two time steps and send that information over the data bus. That information would be aggregated and fused with other odometry.

I can continue to use rtabmap for the actual 3D map and the occupancy grid. So that I would not have to agregate the scan matching data at a high-level to build an occupancy grid off of that as the whole algorithm implies.

I could also try to use all the sensor information from the 8 x 8 grid, by stacking the data into a single 1 x 64 vector. Since I am just getting the transform, that might work.

I guess I have to try and see. Thank you again!

Cheers,

Paulo