Generating multiple maps with rosbags

[Constantine]

Hi everyone,

I'm trying to generate point cloud maps from rosbag data using rtabmap.
The thing is they're all different maps and they're not interconnected. Is there a way to automate making individual maps without having to keep shutting down and restarting rtabmap?

I know there's the /reset, /reset_odom and /trigger_new_map services but I feel like I'm missing something since the /cloud_map topic doesn't seem to be generating a proper map after the first map.

Cheers.

EDIT:
I just found this out but for some reason Rtabmapviz isn't building the 3D map after updating it to the latest version. Does this have anything to do with using an outdated launch file?

[matlabbe]

Hi,

Here some info about how new maps are created:

/reset: delete the whole database (delete all maps) and start a new one

/reset_odom: if you are using rgbd_odometry, that will make odometry restart to 0 (Identity transform). When rtabmap node detects an Identity transform, it triggers a new map.

/trigger_new_map: Trigger a new map. rtabmap will start a new map. The previous maps should disappear, only publishing the latest one. If a loop closure is detected with a previous map, then related point clouds will be republished again (the maps are merged together).

rtabmap triggers a new map automatically when an Identity odometry is detected. In your rosbag, if the odometry is reset between the sessions, rtabmap would create multiple maps automatically. If the odometry is not reset between the sessions, you should do it manually by calling /trigger_new_map service.

Indeed, if you want a single database/session and not one database for multiple sessions, you would have to
1- kill rtabmap, save ~/.ros/rtabmap.db somewhere then restart rtabmap.
2- or call /backup service, copy the backup database (~/.ros/rtabmap.db.back) then call /reset service to flush the previous map.

For rtabmapviz problem, is the basic mapping demo working? http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping

cheers

[Constantine]

Hi Mat,

Thanks for the reply.

Here's some idea on what I'm trying to do:
I'm trying to create a map of lets say an area. I'll do a rosbag record of the two image topics /camera/depth_registered/image_raw and /camera/rgb/image_rect_color and move in a linear direction (no odometry). Every time I encounter a corner to go around on, I'll close the bag and start one of just going around the corner and close it after I've gone around it. This process repeats 12 times.

So my bags are a series of recordings of going in a straight line and around corners. I've decided to do it this way because we've found that it has trouble going around things while mapping so we'll just assemble the linear parts in some other fashion later to build the full 3D map of the area.

So far my pipeline's like this:
Launch Rtabmap.

For each bag: {
Play rosbags of images with a static tf for the camera.
Save pointcloud generated by /cloud_map (a node I've written as a workaround)
Call /reset, /reset_odom and /trigger_new_map (in that order)
}

Am I on the right track? I don't need the database file, just the stitched together XYZRGB PointCloud.

So far I've encountered a couple of issues:
Like I said, Rtabmapviz doesn't show the full 3D map that its built. It works when I'm using the live handheld mapping but when I'm using the rosbags it only shows the active scene and the feature points (green and yellow dots) but not the whole map it generated.

Also, this is a weird one. The pipeline seems to work when I'm running rosbags for a single area (each area has 12 bags). Once I start processing a new area, /cloud_map seems to stop publishing.

EDIT:
Another issue, the pipeline seems to lock up when it tries to do the /reset service call after a couple of cycles. It happens once in a couple of sets so I'm not sure what causes it.

Cheers,

[matlabbe]

Hi,

Make sure that just after you start a roscore, you set "$ rosparam set use_sim_time true" and that you run the bags with "--clock" argument. Otherwise, the timestamp in the recorded topics and the one of the TFs published by your static transform publisher won't match together.

I assume that in your bags you are recording /camera/rgb/camera_info too.

I am not sure of this, but you may have some issues to play a bag with timestamps older than the previous bag.

You said you have written a node to save point cloud, did you know about pcl_ros/pointcloud_to_pcd node?

$ rosrun pcl_ros pointcloud_to_pcd input:=/rtabmap/cloud_map

In rtabmapviz, if there is only the active point cloud shown, that means the map is not published (look at the terminal to see if rtabmap is logging info each second) or /rtabmap/mapData topic is not connected to rtabmapviz. Are you using rgbd_mapping.launch directly with your bags?
 

cheers

[Constantine]

matlabbe wrote

Make sure that just after you start a roscore, you set "$ rosparam set use_sim_time true" and that you run the bags with "--clock" argument. Otherwise, the timestamp in the recorded topics and the one of the TFs published by your static transform publisher won't match together.

I assume that in your bags you are recording /camera/rgb/camera_info too.

I am not sure of this, but you may have some issues to play a bag with timestamps older than the previous bag.

Yep, I have those. I'll try and play them in the order that I recorded them and see what happens.
EDIT: Looks like that was the case.

matlabbe wrote

You said you have written a node to save point cloud, did you know about pcl_ros/pointcloud_to_pcd node?

$ rosrun pcl_ros pointcloud_to_pcd input:=/rtabmap/cloud_map

I didn't know about that one. Mine does something similar but instead of saving every pointcloud, I have the latest callback saved into memory until I do a service call to save it.

matlabbe wrote

In rtabmapviz, if there is only the active point cloud shown, that means the map is not published (look at the terminal to see if rtabmap is logging info each second) or /rtabmap/mapData topic is not connected to rtabmapviz. Are you using rgbd_mapping.launch directly with your bags?

Yeah, I used rgbd_mapping.launch as a base when I wrote my launch file. rqt shows that rtabmapviz is subscribed to /rtabmap/mapData and the topic is publishing. My "3D Map" window shows the current scene with feature highlights and the odometry but not the whole map.

I'm also getting this weird lockup every once in a while when I do a service call to /reset. --udebug doesn't show any details on what's happening. Do you have any ideas on how I can look further into it?

[matlabbe]

Hi,

Well, I don't have any clues of what are the problems. I will try to get some rosbags (rgb+depth+camera_info) to test on my side and see if I can reproduce the rtabmapviz and /reset lockup problems.

cheers

[matlabbe]

Hi,

I fixed a bug (see this commit) that when odometry was reset, the last image of the previous bag was still in the ROS message synchronizer's queue list. So when a new bag was played, odometry started with the last image of the previous bag even after calling /reset_odom service, causing an odometry lost at start (if the first image of the new bag is not correlated with the previous bag). To have this fix, you will have to build rtabmap_ros from source. Now the message queue list is correctly flushed when /reset_odom is called.

The following steps are now working. Make 10 bags:

$ roslaunch freenect_launch freenect.launch depth_registration:=true data_skip:=2
$ mkdir bags
$ cd bags
$ rosbag record /tf /camera/rgb/image_rect_color /camera/rgb/camera_info /camera/depth_registered/image_raw -O 1.bag
...
$ rosbag record /tf /camera/rgb/image_rect_color /camera/rgb/camera_info /camera/depth_registered/image_raw -O 10.bag

Create a cloud for each bag:

$ roscore
$ rosparam set use_sim_time true
$ roslaunch rtabmap_ros rtabmap.launch

$ rosbag play --clock 1.bag
### wait to finish, then in rtabmapviz: 
 1- click Pause 
 2- File->Export 3D clouds (uncheck Meshing) -> Save to "cloud1.pcd"
 3- click Pause again to resume
 4- Edit-> Delete Memory (if you use service /rtabmap/reset instead, make sure to do Edit->"Clear the cache" in rtabmapviz)
 5- Detection -> Reset odometry (same as service /rtabmap/reset_odom)

$ rosbag play --clock 2.bag
## Do the 5 steps above and so on for each bag
...

Note that the bags should be played chronologically, as TF is used.

cheers

[Constantine]

Hi mat,

Thanks for the heads up. The only thing left I'm struggling with is the random lockup when calling /rtabmap/reset.

EDIT: I'm not sure if its any help or not but whenever I terminate rtabmap because it locks up I'm getting kind of a last error message along the lines of:

[ERROR] (*timestamp*) Rtabmap.cpp:880::process() RGB-D SLAM mode is enabled and no odometry is provided. Image 461 is ignored!

Cheers.

[matlabbe]

Hi,

I cannot reproduce the lockup problem using live stream and rgbd_mapping.launch. Is it only happen when switching rosbags? Can you reproduce with a live stream and calling often /rtabmap/reset?

The log error means that odometry is lost. Do you still have odometry published?

$ rostopic hz /rtabmap/odom

cheers

[Constantine]

Hi mat,

I can't reproduce it on the live stream either. Its only happening between rosbags as far as I know.

Cheers

[Constantine]

I think I've managed to reproduce the problem.

I've tried adding printouts to the service call callback to see which step its locking up and I've found that its not even going into the callback at all. So the problem seems to be happening on the ROS side of things.

During the data gathering process, one of my cameras was failing and was dropping a lot of data on the rgb camera side. The depth data however is fine publishing @ 30fps. I'm consistently getting lockups on this bag where I have a large gap of missing data on the rgb stream at the end of the bag.

Does rtabmap wait for an rgb callback when it receives a depth image?

Cheers

[matlabbe]

Yes, rtabmap uses an approximate synchronizer on input topics. If one of the topic is not published, the callback will never be called.

cheers

[Constantine]

After a couple of hours of ROS_INFO statements I think I've managed to figure out where exactly my rtabmap is locking up.

Turns out the reason calling /reset didn't even call the service callback was that it wasn't coming out of commonDepthCallback() to begin with.

Going further into that it gets stuck inside getTransform() on this line:

if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_)))

After this point I'm not sure what to do now. Should I just be able to comment this out?
Cheers

[matlabbe]

Hi,

Can you show up the stamp before the waitForTransform() call?

ROS_INFO("stamp=%fs (max wait time=%fs)",stamp, waitForTransformDuration_);

Maybe between rosbags, the new stamp required is too far in the future.You could try this to just use the latest transform:

if(!tfListener_.waitForTransform(fromFrameId, toFrameId, ros::Time(0), ros::Duration(waitForTransformDuration_))) 
...
tfListener_.lookupTransform(fromFrameId, toFrameId,ros::Time(0), tmp);

cheers

[Constantine]

Did you mean stamp.toSec()?

[ INFO] [1467680678.535417497, 1464611902.874841582]: Enter depthScan3dCallback
[ INFO] [1467680678.535481293, 1464611902.874841582]: depthScan3dCallback : sensor_msgs::LaserScanConstPtr scan2dMsg
[ INFO] [1467680678.535535865, 1464611902.874841582]: getTransform()
[ INFO] [1467680678.535562268, 1464611902.874841582]: stamp=1464611902.733460s (max wait time=1.000000s)

I'll try that and see how it goes.

cheers

[matlabbe]

Yeah toSec()