3D laser data and stereo images to build a map

[Rohit]

Hi matlabee,

we collected our own data using zed stereo camera and 3D Quanergy lidar.
can we use both the stereo images and the lidar data at the same time to generate a map in rtabmap-ros?

[Rohit]

it is giving a good map using the stereo images. but unable to load scan data.






[ WARN] (2019-08-30 05:04:56.834) OccupancyGrid.cpp:357::createLocalMap() Cannot create local map, scan is empty (node=291).
[ WARN] (2019-08-30 05:04:56.835) SensorData.cpp:718::uncompressDataConst() Requested laser scan data, but the sensor data (290) doesn't have laser scan.
[ WARN] (2019-08-30 05:04:56.835) SensorData.cpp:718::uncompressDataConst() Requested laser scan data, but the sensor data (291) doesn't have laser scan.
[ERROR] (2019-08-30 05:04:56.835) RegistrationIcp.cpp:1206::computeTransformationImpl() Laser scans empty?!? (new[291]=0 old[290]=0)
[ WARN] (2019-08-30 05:04:56.836) SensorData.cpp:718::uncompressDataConst() Requested laser scan data, but the sensor data (256) doesn't have laser scan.
[ WARN] (2019-08-30 05:04:56.836) SensorData.cpp:718::uncompressDataConst() Requested laser scan data, but the sensor data (291) doesn't have laser scan.
[ INFO] [1567121696.847268983, 1561635504.084158446]: Odom: quality=217, std dev=0.375386m|0.072825rad, update time=0.069387s
[ERROR] (2019-08-30 05:04:56.887) RegistrationIcp.cpp:1206::computeTransformationImpl() Laser scans empty?!? (new[291]=0 old[256]=0)
[ WARN] (2019-08-30 05:04:56.887) Rtabmap.cpp:2144::process() Rejected loop closure 256 -> 291: Laser scans empty?!? (new[291]=0 old[256]=0)
[ INFO] [1567121696.891971315, 1561635504.134495573]: rtabmap (291): Rate=1.00s, Limit=0.000s, RTAB-Map=0.1120s, Maps update=0.0023s pub=0.0000s (local map=284, WM=290)
[ INFO] [1567121696.927992446, 1561635504.164711637]: Odom: quality=444, std dev=0.233987m|0.062885rad, update time=0.079350s

[matlabbe]

Hi,

/rtabmap/cloud_map can be constructed with a point cloud from stereo OR the lidar, but not both at the same time. To choose between then, set Grid/FromDepth parameter to false to make the map from lidar.

I looks like there are graph nodes without any lidar valid data. Is it happening just for some nodes or all nodes in the map? For some nodes, was the sensor obstructed at that moment? For all nodes, does the lidar return valid values?

cheers,
Mathieu

[Rohit]

Hi Matlabbe,

I want to generate map given the left and right stereo images and the point cloud from lidar.
Though the ros bag has scan data and rostopic echo /sensor/data return me the point cloud values but still it gives me the same error for all nodes.

[matlabbe]

Can you share a sample of the rosbag (like 10/15 sec) to see the data is actually compatible with rtabmap?

[Rohit]

https://drive.google.com/open?id=1rOQ2Vwg2IVJ36GlEwbjMiwK8pY4aKfHW

link of the bag file is shared..

[Rohit]

did u got the bag file..

[matlabbe]

Hi,

sorry for the delay, I've been quite busy the past days. I am able to run correctly the rosbag, so not sure what was the problem on your side (I am using latest rtabmap version). Here is what I did:
1) removed map -> zed_left_camera_optical_frame using this script (output.bag -> output_without_map_frame.bag)
2) tf_static is missing from the bag, I had to add them manually. I guessed the tilt of the camera. The position between the lidar and camera is 0,0,0, but should be probably something else.
3) run:

roslaunch rtabmap_ros rtabmap.launch \
   args:="-d" \
   stereo:=true \
   left_image_topic:=/zed/zed_node/rgb/image_rect_color \
   left_camera_info_topic:=/zed/zed_node/rgb/camera_info \
   right_image_topic:=/zed/zed_node/right/image_rect_color \
   right_camera_info_topic:=/zed/zed_node/right/camera_info \
   use_sim_time:=true \
   frame_id:=base_link \
   subscribe_scan_cloud:=true \
   rgbd_sync:=true \
   scan_cloud_topic:=/Sensor/points \
   approx_rgbd_sync:=false \
   approx_sync:=true

# base to lidar
$ rosrun tf static_transform_publisher 0 0 0 0 0 0 base_link Sensor 100

# base to camera
$ rosrun tf static_transform_publisher 0 0 0 0 -0.2 0 base_link camera_link 100

# optical rotation
$ rosrun tf static_transform_publisher 0 0 0 -1.5707963267948966 0 -1.5707963267948966 camera_link zed_left_camera_optical_frame 100

# run the bag from 8 sec (before that there are no scans)
rosbag play --clock -s 8 output_without_map_frame.bag

We can see the trajectory in cyan, the red are the scans.

UPDATE
If you want to record all lidar scan in the map without updating the map too fast (default 1 Hz), we can use rtabmap_ros/point_cloud_assembler nodelet to assemble 10 lidar scans, then make rtabmap subscribe to assembled cloud. As the assembled cloud is published at 1 Hz (scans are 10 Hz), we can set Rtabmap/DetectionRate to 0. We will still have the 1 Hz map update but with all scans.

roslaunch rtabmap_ros rtabmap.launch \
   args:="-d --Rtabmap/DetectionRate 0" \
   stereo:=true \
   left_image_topic:=/zed/zed_node/rgb/image_rect_color \
   left_camera_info_topic:=/zed/zed_node/rgb/camera_info \
   right_image_topic:=/zed/zed_node/right/image_rect_color \
   right_camera_info_topic:=/zed/zed_node/right/camera_info \
   use_sim_time:=true \
   frame_id:=base_link \
   subscribe_scan_cloud:=true \
   rgbd_sync:=true \
   scan_cloud_topic:=/assembled_cloud \
   approx_rgbd_sync:=false \
   approx_sync:=true

# base to lidar
$ rosrun tf static_transform_publisher 0 0 0 0 0 0 base_link Sensor 100

# base to camera
$ rosrun tf static_transform_publisher 0 0 0 0 -0.2 0 base_link camera_link 100

# optical rotation
$ rosrun tf static_transform_publisher 0 0 0 -1.5707963267948966 0 -1.5707963267948966 camera_link zed_left_camera_optical_frame 100

# assemble scans
$ rosrun nodelet nodelet standalone rtabmap_ros/point_cloud_assembler _max_clouds:=10 _fixed_frame_id:=odom cloud:=/Sensor/points _wait_for_transformation:=1

# run the bag from 8 sec (before that there are no scans)
rosbag play --clock -s 8 output_without_map_frame.bag

cheers,
Mathieu

[Rohit]

Thank you ... its taking the laser data now. maybe it was due to removing the map from the bag file.
Though the tracking fails after sometime due to less no of inliers. Need to to change some parameters i guess.

can you please tell me where in the source code the pose values for plotting graph are stored, i guess it is somewhere in graph.cpp . I am not very good in c++ and i need to store the values for the graph generated to manually compare it with that of the google maps.

Thank you once again.

[matlabbe]

For convenience, you can do File -> "Export poses..." to export the poses in a TXT file.