Re: Rtabmap mapping with two cameras

Posted by matlabbe on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Rtabmap-mapping-with-two-cameras-tp2662p2693.html

Hi,

There was a bug in the laser scan generation from the depth images (in rtabmap node). It is fixed in this commit.

Before:


With the fix:


For convenience, I added "map_negative_scan_empty_ray_tracing" to avoid filling space between the two kinects (as in the second screenshot above), while being able to keep "map_negative_poses_ignored" to update grid map with latest data even if we are not moving. Note that demo_two_kinects.launch example assumes that the cameras are parallel to ground, like in this post: http://official-rtab-map-forum.206.s1.nabble.com/demo-two-kinect-data-error-td1231.html#a1251, so the middle lines in the cameras simulate directly a laser scan.

In your setup, the kinect is tilted toward the ground. With "gen_scan" (which works exactly like depthimage_to_laserscan), the scan is generated like in the image below (middle line of the depth image):


Can you show TF in RVIZ to better visualize how the cameras are configured? Here is an example with pointcloud_to_laserscan and a single camera tilted 45 degrees toward the ground:

<launch>

<!-- The fake scan will be 20 cm over /base_link -->
<node pkg="tf" type="static_transform_publisher" name="base_link"
        args="0 0 0.2 0 0 0 base_link base_scan_link 100" />

<!-- Camera is tilted 45 degrees toward the ground, 80 cm over base_scan_link -->
<node pkg="tf" type="static_transform_publisher" name="base_scan_link"
        args="0 0 0.8 0 0.785398163 0 base_scan_link camera_link 100" />

<!-- Camera -->
<include file="$(find freenect_launch)/launch/freenect.launch">
    <arg name="depth_registration" value="True" />
</include>

<group ns="camera">

  <!-- Run a VoxelGrid filter to clean NaNs and downsample the data -->
  <node pkg="nodelet" type="nodelet" name="voxel_grid" args="load pcl/VoxelGrid camera_nodelet_manager" output="screen">
    <remap from="~input" to="/camera/depth_registered/points" />
    <remap from="~output" to="/camera/depth_registered/points_downsampled" />
    <rosparam>
      filter_field_name: z
      filter_limit_min: 0.01
      filter_limit_max: 10
      filter_limit_negative: False
      leaf_size: 0.01
    </rosparam>
  </node>

<node pkg="nodelet" type="nodelet" name="pointcloud_to_laserscan" args="load pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet camera_nodelet_manager" output="screen">
      <remap from="cloud_in"     to="/camera/depth_registered/points_downsampled"/>

      <!-- in base_scan_link referential -->
      <param name="min_height" type="double" value="-0.05"/> 
      <param name="max_height" type="double" value="0.05"/>
      <param name="target_frame" type="string" value="base_scan_link"/>
      <param name="angle_min" type="double" value="-0.65"/>
      <param name="angle_max" type="double" value="0.65"/>
      <param name="angle_increment" type="double" value="0.006"/>
      <param name="range_min" type="double" value="0"/>
      <param name="range_max" type="double" value="4"/>
   </node>
</group>
</launch>

For rtabmap node:

<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap">
   ...
   <param name="subscribe_scan" type="bool"   value="true"/>
   <remap from="scan" to="/camera/scan"/> <!-- output of pointcloud_to_laserscan -->
</node>

In red the computed laser scan with the corresponding grid map:


cheers,
Mathieu