Re: Problem fusing LiDAR and depth (hector_slam)

Posted by matlabbe on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Problem-fusing-LiDAR-and-depth-hector-slam-tp4115p4133.html

Hi,

It depends on what you want as output. Is there a particular output cloud topic that you want to know how it is assembled form the actual ones published by rtabmap node? Output map topics are assembled in MapsManager class. They don't use raw laser scans directly though, but local occupancy grids (which can be created from laser scans when "Grid/FromDepth" is false) for efficiency reasons.

If you are referring to cyan laser scans shown in rtabmapviz, they are not concatenated but added separately (each node of the map's graph) to OpenGL renderer from MainWindow::createAndAddScanToMap().

If you want to generate the raw concatenated laser scans after mapping, this can be done with "File->Export 3D clouds..." from rtabmapviz or standalone RTAB-Map application (by opening the generated database), then check top laser scan checkbox to use laser scans as point cloud instead of depth images. Clouds are actually assembled here.

If you want to get the assembled raw laser scans (like the File->Export above), you should do it yourself by subscribing to /rtabmap/mapData topic. Convert node data from the ROS msg to Signature class (Signature is a node in the graph) like here, look for nodes with laser scan set (normally the last node should always have data), then instead of using MapsManager, you can convert the laser scan field of Signature.sensorData() (make sure to do Signature.sensorData().uncompress() before to uncompress laser scans) to a point cloud by using rtabmap::util3d::laserScanToPointCloud() with transform set to Signature.sensorData().laserScanInfo().localTransform(). You will have clouds in "frame_id" frame, now you should transform the point clouds according to corresponding node's pose (poses from MapData ROS msg can be converted like this).

cheers,
Mathieu