I am thinking about using rtabmap to localize a robot, sourcing odometry / pose data for a robot that as a vertical laser. That is, to use it along with something like laser_assembler to create a point cloud from the laser only.
I never tested this kind of setup. Reading laser_assembler, this could be integrated with rtabmap. laser_assembler could assemble a small window of scans, then trigger the assembling to generate a local point cloud that would be used as input to rtabmap on /rtabmap/scan_cloud topic (along with parameter subscribe_scan_cloud:=true).
We could set Rtabmap/DetectionRate to 0 Hz, then trigger the assembling service at each second (at 1 Hz or 2 Hz). This assumes that odometry has very low drift during the 500 ms/1 second of capture.
On loop closure, each assembled laser point cloud will be corrected. The individual laser scans in the assembled clouds won't be modified (rtabmap sees the assembled scans like a single point cloud). It is why while the scans are assembled (by laser_assembler), odometry should not drift too much.
I am struggling a bit with the TF tree I think :
A few other issues with this setup:
- the white line in the graph visible in the image is "disappearing" over time, that is the white line, vanishes after a few seconds.
- although I do see the subscribed cloud (assembled), BUT it is always the same cloud, and it does not increment either. I have verified in rviz that it does update.
However i notice 2 thinks:
- the cloud is only added when a new graph node is triggered; my intention was that there was a continuous cloud. Can this be done?
- when the subscribe_cloud option is true the odometry windows does not display the detections as when the cloud is not subscribed. I am not sure this is a problem because as can be seen the map is still built as the robot moves forward
Images to explain what I mean:
Without odometry detections (scan_cloud subscribed)
With odometry detections (scan_cloud unsubscribed)
- The white line disappears automatically after some time. It is the odometry trajectory. You can do on 3D Map view: right-click->Trajectory->Set Trajectory size to 0 if you don't want it to disappear.
- What do you mean by continuous cloud? You could increase the map update rate to add more nodes. I generally don't recommend this because it increases processing time a lot more faster on large maps. If you want to subscribe to a single merged cloud, try /rtabmap/cloud_map topic.
- Yes, it is because the interface is not implemented. In this commit, I implemented the callback supporting odom_info + scan_cloud subscription at the same time. Tell me if it works, as I didn't test it yet.
I tried the map rate and even add intermediate node but didn't seem to make any difference. My idea is to create a 3D point cloud from each 2D scan line, by adding each line as the odometry information is added.
The kinect is facing forward and the laser facing up profiling the sides and the top.
I don't want to keep the cloud sourced from the kinect. The continuous cloud that i mentioned, is in fact made of all the 2D profiles; so going back to my previous post, we would get a continuous stream of blue 2D lines, instead of one line at each graph node.
The /rtabmap/cloud_map doesn't display anything.
The commit didn't seem to make any difference to me.
I tested the new interface and subscribing to odom_info + scan/scan_cloud at the same time should work. In your example, it is like that your laser_assembler is not assembling any scans, it just sends a single scan. Try setting laser_assembler to assemble scans for 1 second, then publish the assembled cloud at 1 Hz. If the cloud is published at 1 Hz, you can set "Rtabmap/DetectionRate=0" to add all clouds. Setting "RGBD/LinearUpdate=0" and "RGBD/AngularUpdate=0" is also important to avoid holes. Set "RGBD/ProximityPathMaxNeighbors=0" to avoid long proximity detection with laser scans. I verified also /rtabmap/cloud_map, and it should work:
<!-- Build laser_assembler package from source to have periodic_snapshotter node -->
<!-- Also use assemble_scans2 service in periodic_snapshotter, publish PointCloud2 and set duration to 1 second -->
$ roslaunch rtabmap_ros test_laser_assembler.launch
If you do post-processing in rtabmapviz, /rtabmap/cloud_map topic will not be updated, only in the map in rtabmapviz will be updated. You may open the database in Database Viewer to do some post-processing, then when saved, if you open again the database under ros, /rtabmap/cloud_map will have the changes.
I have this working but it seems the subscribed cloud doesn't change once it is subscribed: the cloud from the kinect seems fine but the subscribed has clear problems visible where there is an overlap. For example the final loop closure (start and end at approximately the same point): the cloud of the kinect is adjusted once the loop is detected but the subscribed cloud still shows a double line:
The cloud from Kinect is red in the bellow:
My snapshootter is running every 0.05 s / max scans in laser assembler is 100; --Rtabmap/DetectionRate is 0. Rtabmap/CreateIntermediateNodes is "true"