RGB-D Mapping with Laser scanner

[You Li]

Here is the information:

app16@app16:/media/app16/Data/youli/data/test_rtabmap_bag$ roslaunch rtabmap_ros demo_hector_mapping_new_v2.launch  resolution:=hd
... logging to /home/app16/.ros/log/a5620a92-98a3-11e6-8be1-b88198d39eb0/roslaunch-app16-17522.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://app16:42493/

SUMMARY
========

PARAMETERS
 * /hector_mapping/base_frame: base_footprint
 * /hector_mapping/map_frame: hector_map
 * /hector_mapping/map_multi_res_levels: 2
 * /hector_mapping/map_resolution: 0.05
 * /hector_mapping/map_size: 2048
 * /hector_mapping/map_update_angle_thresh: 0.06
 * /hector_mapping/odom_frame: odom
 * /hector_mapping/pub_map_odom_transform: False
 * /hector_mapping/pub_map_scanmatch_transform: True
 * /hector_mapping/pub_odometry: True
 * /hector_mapping/scan_topic: /scan
 * /points_xyzrgb/voxel_size: 0.01
 * /rosdistro: kinetic
 * /rosversion: 1.12.5
 * /rtabmap/rtabmap/Grid/FromDepth: False
 * /rtabmap/rtabmap/Optimizer/Slam2D: true
 * /rtabmap/rtabmap/Reg/Force3DoF: true
 * /rtabmap/rtabmap/Reg/Strategy: 1
 * /rtabmap/rtabmap/Vis/InlierDistance: 0.1
 * /rtabmap/rtabmap/Vis/MaxDepth: 10.0
 * /rtabmap/rtabmap/frame_id: base_footprint
 * /rtabmap/rtabmap/map_frame_id: map
 * /rtabmap/rtabmap/subscribe_depth: True
 * /rtabmap/rtabmap/subscribe_scan: True
 * /rtabmap/rtabmapviz/frame_id: base_footprint
 * /rtabmap/rtabmapviz/subscribe_depth: True
 * /rtabmap/rtabmapviz/subscribe_laserScan: True

NODES
  /rtabmap/
    rtabmap (rtabmap_ros/rtabmap)
    rtabmapviz (rtabmap_ros/rtabmapviz)
  /
    hector_mapping (hector_mapping/hector_mapping)
    points_xyzrgb (nodelet/nodelet)
    scanmatcher_to_base_footprint (tf/static_transform_publisher)

ROS_MASTER_URI=http://localhost:11311

core service [/rosout] found
process[scanmatcher_to_base_footprint-1]: started with pid [17540]
process[hector_mapping-2]: started with pid [17541]
process[rtabmap/rtabmap-3]: started with pid [17552]
process[rtabmap/rtabmapviz-4]: started with pid [17572]
process[points_xyzrgb-5]: started with pid [17583]
HectorSM map lvl 0: cellLength: 0.05 res x:2048 res y: 2048
HectorSM map lvl 1: cellLength: 0.1 res x:1024 res y: 1024
[ INFO] [1477173938.226816643]: HectorSM p_base_frame_: base_footprint
[ INFO] [1477173938.226878484]: HectorSM p_map_frame_: hector_map
[ INFO] [1477173938.226896346]: HectorSM p_odom_frame_: odom
[ INFO] [1477173938.226915017]: HectorSM p_scan_topic_: /scan
[ INFO] [1477173938.226939991]: HectorSM p_use_tf_scan_transformation_: true
[ INFO] [1477173938.226957398]: HectorSM p_pub_map_odom_transform_: false
[ INFO] [1477173938.226974896]: HectorSM p_scan_subscriber_queue_size_: 5
[ INFO] [1477173938.226995544]: HectorSM p_map_pub_period_: 2.000000
[ INFO] [1477173938.227012631]: HectorSM p_update_factor_free_: 0.400000
[ INFO] [1477173938.227031648]: HectorSM p_update_factor_occupied_: 0.900000
[ INFO] [1477173938.227048541]: HectorSM p_map_update_distance_threshold_: 0.400000
[ INFO] [1477173938.227065840]: HectorSM p_map_update_angle_threshold_: 0.060000
[ INFO] [1477173938.227082390]: HectorSM p_laser_z_min_value_: -1.000000
[ INFO] [1477173938.227100989]: HectorSM p_laser_z_max_value_: 1.000000
[ INFO] [1477173938.272086695]: Starting node...
[ INFO] [1477173938.306314535]: Initializing nodelet with 8 worker threads.
[ INFO] [1477173938.454978972]: Starting node...
[ INFO] [1477173938.580601951]: /rtabmap/rtabmap(maps): grid_cell_size             = 0.050000
[ INFO] [1477173938.580631842]: /rtabmap/rtabmap(maps): grid_incremental           = false
[ INFO] [1477173938.580650072]: /rtabmap/rtabmap(maps): grid_size                  = 0.000000
[ INFO] [1477173938.580668683]: /rtabmap/rtabmap(maps): grid_eroded                = false
[ INFO] [1477173938.580686015]: /rtabmap/rtabmap(maps): grid_footprint_radius      = 0.000000
[ INFO] [1477173938.580702203]: /rtabmap/rtabmap(maps): map_filter_radius          = 0.000000
[ INFO] [1477173938.580723147]: /rtabmap/rtabmap(maps): map_filter_angle           = 30.000000
[ INFO] [1477173938.580740509]: /rtabmap/rtabmap(maps): map_cleanup                = true
[ INFO] [1477173938.580757266]: /rtabmap/rtabmap(maps): map_negative_poses_ignored = false
[ INFO] [1477173938.580775147]: /rtabmap/rtabmap(maps): cloud_output_voxelized     = true
[ INFO] [1477173938.580792526]: /rtabmap/rtabmap(maps): cloud_subtract_filtering   = false
[ INFO] [1477173938.580810179]: /rtabmap/rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[ INFO] [1477173938.583323626]: /rtabmap/rtabmap(maps): octomap_tree_depth         = 16
[ INFO] [1477173938.583357093]: /rtabmap/rtabmap(maps): octomap_occupancy_thr      = 0.500000
[ INFO] [1477173938.589328326]: rtabmapviz: Using configuration from "/home/app16/catkin_ws/src/rtabmap_ros/launch/config/rgbd_gui.ini"
[ INFO] [1477173938.604272714]: rtabmap: frame_id      = base_footprint
[ INFO] [1477173938.604306391]: rtabmap: map_frame_id  = map
[ INFO] [1477173938.604331164]: rtabmap: tf_delay      = 0.050000
[ INFO] [1477173938.604350402]: rtabmap: tf_tolerance  = 0.100000
[ INFO] [1477173938.604372321]: rtabmap: odom_sensor_sync   = false
[ INFO] [1477173938.694795615]: Setting RTAB-Map parameter "Grid/FromDepth"="false"
[ INFO] [1477173938.860405172]: Setting RTAB-Map parameter "Optimizer/Slam2D"="true"
[ INFO] [1477173938.903852863]: Setting RTAB-Map parameter "Reg/Force3DoF"="true"
[ INFO] [1477173938.904237129]: Setting RTAB-Map parameter "Reg/Strategy"="1"
[ INFO] [1477173939.002088226]: Setting RTAB-Map parameter "Vis/InlierDistance"="0.1"
[ INFO] [1477173939.004244373]: Setting RTAB-Map parameter "Vis/MaxDepth"="10.0"
[ INFO] [1477173939.019457157]: lookupTransform base_footprint to laser timed out. Could not transform laser scan into base_frame.
[ INFO] [1477173939.195764969]: RTAB-Map detection rate = 1.000000 Hz
[ INFO] [1477173939.195854326]: rtabmap: Deleted database "/home/app16/.ros/rtabmap.db" (--delete_db_on_start is set).
[ INFO] [1477173939.195873116]: rtabmap: Using database from "/home/app16/.ros/rtabmap.db".
[ INFO] [1477173939.415047962]: rtabmap: Database version = "0.11.11".
[ INFO] [1477173939.445831350]: /rtabmap/rtabmap: queue_size    = 10
[ INFO] [1477173939.445872849]: /rtabmap/rtabmap: rgbd_cameras = 1
[ INFO] [1477173939.445886115]: /rtabmap/rtabmap: approx_sync   = true
[ INFO] [1477173939.445953977]: Setup depth callback
[ INFO] [1477173939.526128442]: lookupTransform base_footprint to laser timed out. Could not transform laser scan into base_frame.
[ INFO] [1477173939.571902033]:
/rtabmap/rtabmap subscribed to (approx sync):
   /scanmatch_odom,
   /kinect2/hd/image_color_rect,
   /kinect2/hd/image_depth_rect,
   /kinect2/hd/camera_info,
   /scan
[ INFO] [1477173939.571995524]: rtabmap 0.11.11 started...
[ INFO] [1477173939.663702310]: Reading parameters from the ROS server...
[ INFO] [1477173939.844335096]: Parameters read = 253
[ INFO] [1477173939.844365882]: Parameters successfully read.
[ WARN] [1477173939.947098411]: rtabmap: "subscribe_laserScan" parameter is deprecated, use "subscribe_scan" instead. The scan topic is still subscribed.
[ INFO] [1477173939.954686503]: /rtabmap/rtabmapviz: queue_size    = 10
[ INFO] [1477173939.954726958]: /rtabmap/rtabmapviz: rgbd_cameras = 1
[ INFO] [1477173939.954741183]: /rtabmap/rtabmapviz: approx_sync   = true
[ INFO] [1477173939.954769102]: Setup depth callback
[ INFO] [1477173940.030891757]: lookupTransform base_footprint to laser timed out. Could not transform laser scan into base_frame.
[ INFO] [1477173940.119361708]:
/rtabmap/rtabmapviz subscribed to (approx sync):
   /scanmatch_odom,
   /kinect2/hd/image_color_rect,
   /kinect2/hd/image_depth_rect,
   /kinect2/hd/camera_info,
   /scan
[ INFO] [1477173940.119431793]: rtabmapviz started.
[ INFO] [1477173940.531675128]: lookupTransform base_footprint to laser timed out. Could not transform laser scan into base_frame.
[ INFO] [1477173941.038691351]: lookupTransform base_footprint to laser timed out. Could not transform laser scan into base_frame.
[ INFO] [1477173941.548088080]: lookupTransform base_footprint to laser timed out. Could not transform laser scan into base_frame.
[ INFO] [1477173942.054068437]: lookupTransform base_footprint to laser timed out. Could not transform laser scan into base_frame.
[ INFO] [1477173942.560830980]: lookupTransform base_footprint to laser timed out. Could not transform laser scan into base_frame.
[ INFO] [1477173943.064624911]: lookupTransform base_footprint to laser timed out. Could not transform laser scan into base_frame.
[ INFO] [1477173943.572524715]: lookupTransform base_footprint to laser timed out. Could not transform laser scan into base_frame.
[ INFO] [1477173944.073147652]: lookupTransform base_footprint to laser timed out. Could not transform laser scan into base_frame.
^C[ WARN] [1477173944.572000888]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10).
/rtabmap/rtabmap subscribed to (approx sync):
   /scanmatch_odom,
   /kinect2/hd/image_color_rect,
   /kinect2/hd/image_depth_rect,
   /kinect2/hd/camera_info,
   /scan



When run $ rostopic hz, there were messages for /kinect2/hd/image_color_rect,   /kinect2/hd/image_depth_rect, /kinect2/hd/camera_info, and /scan.

However, there is "no new message" for /scanmatch_odom

[You Li]

Thanks consoante,

I updated the .launch file:
demo_hector_mapping_new_v3.launch

Now the frames becomes:


Now I can see the Kinect and lidar data in real-time with the new launch file.

The new problem is that there seems to be misalignment between the Kinect and lidar images.

[consoante]

yes so in your tf the 0.0 0.0 0.0 0.0 0.0 0.0 needs to be updated
accordingly. You will have them once you establish the relationship
between the sensors

you are nearly there

[You Li]

   
Thanks. I have a question. Why is there an angle transformation in the rgbd_mapping_kinect2.launch file?


   <arg name="pi/2" value="1.5707963267948966"/>
   <arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
   <node pkg="tf" type="static_transform_publisher" name="kinect2_base_link"
        args="$(arg optical_rotate) kinect2_base_link kinect2_link 100" /> 
 
Is it essential whenever we use kinect2?

[You Li]

Problem solved. Many thanks for the help from you guys.

From my experience, one needs to set the rotation of Kinect 2, as well as the pose of lidar. Here is my final launch file.
demo_hector_mapping_new_v5.launch