Visual Odometry

[consoante]

Hi,

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.

As this been tested before?

Thanks

//consoante

[matlabbe]

Hi,

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.

cheers,
Mathieu

[consoante]

Hi Mathieu

the last part of your answer, how can we trigger / repeat the service at each second so that the cloud increments as the platform moves forward?

<node type="periodic_snapshotter" pkg="laser_assembler" name="periodic_snaphotter"/>  is how far I got :|

Thanks

[consoante]

Made a bit of progress with laser_assembler and get a cloud (Type: sensor_msgs/PointCloud)

Trying to subscribe the cloud as suggested with  
param name="subscribe_scan_cloud" type="bool" value="true"
remap from="scan_cloud" to="/assembled_cloud"

But I guess rtabmap wants sensor_msgs/PointCloud2 (error is Client [/rtabmap/rtabmap] wants topic /assembled_cloud to have datatype/md5sum )

Any work around?

I am correct in thinking that the laser clouds will be shifted as well when a loop closure as detected?

Thanks

[consoante]

Modified the assembler to publish pointcloud2 and now and sort of see something.

Can you let me know the answer to: I am correct in thinking that the laser clouds will be shifted as well when a loop closure as detected?

thanks

[matlabbe]

Hi,

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.

cheers,
Mathieu

[consoante]

Hi Matthieu,

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.



My launch file is

https://pastebin.com/raw/MZFgymaP

I am running, kinect2_bridge, urg_node and laser_assembler separately.

TF Tree:



Any pointers?

Thanks once again
//consoante

[consoante]

Hi again,

After restarting it seems the same launch file runs almost as intended...maybe ctrl+c the launch file is actually not killing all the instances.

I have modified the launch file to:  https://pastebin.com/PkE6J1nw

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)



Cheers!

[matlabbe]

Hi,

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

cheers,
Mathieu

[consoante]

Hi,

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.

Cheers

[matlabbe]

Hi,

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:

<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap">
   ...
   <param name="Rtabmap/DetectionRate"          type="string" value="0"/>
   <param name="RGBD/LinearUpdate"              type="string" value="0"/>
   <param name="RGBD/AngularUpdate"             type="string" value="0"/>
   <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="0"/>
</node>

For convenience, I did a small demo with laser_assembler. The launch file is test_laser_assembler.launch (I've set periodic_snapshotter's rate to 1 Hz and used AssembleScans2 service ("assemble_scans2") with publishing PointCloud2 msg (assembled_cloud2)):

<!-- 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

cheers,
Mathieu

[consoante]

I tried again and it does work  . It seems my launch files were poorly constructed.

I think you meant "RGBD/LinearUpdate" and "RGBD/AngularUpdate"

If I end up doing pos-processing / bundle adjustment, will the /rtabmap/cloud_map be updated?

Cheers

[matlabbe]

Hi,

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.

cheers,
Mathieu

[consoante]

Hi again,

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:



Any pointers?

My snapshootter is running every 0.05 s / max scans in laser assembler is 100; --Rtabmap/DetectionRate is 0. Rtabmap/CreateIntermediateNodes is "true"

Cheers!

[matlabbe]

Hi,

I re-tested test_laser_assembler.launch to see if the scans are corrected on loop closure, and they should:

Corresponding point cloud for reference:


Laser scans before and after loop closure:


Do you have a database to share?

cheers,
Mathieu