Re: Painting a pointcloud.

Posted by matlabbe on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Painting-a-pointcloud-tp8130p8473.html

Hi Anthony,

try this:

roslaunch 4cam.launch args:="-d  \
       RGBD/CreateOccupancyGrid false \
       Rtabmap/DetectionRate 0 \
       Odom/ScanKeyFrameThr 0.5 \
       OdomF2M/ScanMaxSize 30000  \
       OdomF2M/ScanSubtractRadius 0.3   \
       Icp/PM true \
       Icp/VoxelSize 0.3   \
       Icp/MaxTranslation 2   \
       Icp/MaxCorrespondenceDistance 1 \
       Icp/PMOutlierRatio 0.7 \
       Icp/Iterations 10 \
       Icp/PointToPlane true \
       cp/PMMatcherKnn 3 \
       Icp/PMMatcherEpsilon 1 \
       Icp/Epsilon 0.0001 \
       Icp/PointToPlaneK 10 \
       Icp/PointToPlaneRadius 0 \
       Icp/CorrespondenceRatio 0.01 \
       Icp/PointToPlaneGroundNormalsUp 0.3 \
       Rtabmap/ImagesAlreadyRectified false"

with:

<launch>

  <arg name="args" default="-d"/>

  <param name="use_sim_time" type="bool" value="true"/>
  <arg name="pi/2" value="1.5707963267948966" />
  
  <!-- tf tree -->
  <node pkg="tf2_ros" type="static_transform_publisher" name="static_transform_publisher0" args="0 -0.125 -0.0375 -$(arg pi/2) 0 -$(arg pi/2) velodyne cam_2"/>
  <node pkg="tf2_ros" type="static_transform_publisher" name="static_transform_publisher1" args="0.0375 0 -0.0375 0 $(arg pi/2) 0 cam_2 cam_0"/>
  <node pkg="tf2_ros" type="static_transform_publisher" name="static_transform_publisher2" args="0.0375 0 -0.0375 0 $(arg pi/2) 0 cam_0 cam_3"/>
  <node pkg="tf2_ros" type="static_transform_publisher" name="static_transform_publisher3" args="0.0375 0 -0.0375 0 $(arg pi/2) 0 cam_3 cam_1"/>

  <!-- sync camera_info and image_raw -->
  <group ns="cam/0">
    <node pkg="rtabmap_ros" type="rgb_sync" name="rgb_sync" output="screen">
      <remap from="rgb/camera_info" to="camera_info"/>
      <remap from="rgb/image_rect"  to="image_raw"/>
    </node>
  </group>
  <group ns="cam/1">
    <node pkg="rtabmap_ros" type="rgb_sync" name="rgb_sync" output="screen">
      <remap from="rgb/camera_info" to="camera_info"/>
      <remap from="rgb/image_rect"  to="image_raw"/>
    </node>
  </group>
  <group ns="cam/2">
    <node pkg="rtabmap_ros" type="rgb_sync" name="rgb_sync" output="screen">
      <remap from="rgb/camera_info" to="camera_info"/>
      <remap from="rgb/image_rect"  to="image_raw"/>
    </node>
  </group>
  <group ns="cam/3">
    <node pkg="rtabmap_ros" type="rgb_sync" name="rgb_sync" output="screen">
      <remap from="rgb/camera_info" to="camera_info"/>
      <remap from="rgb/image_rect"  to="image_raw"/>
    </node>
  </group>

  <!-- Nodes -->
  <group ns="rtabmap">
    
    <!-- ICP Odometry -->
    <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen" args="$(arg args)">
      <remap from="scan_cloud" to="/velodyne_points"/>
      
      <param name="frame_id" type="string" value="velodyne"/>
      <param name="scan_cloud_max_points"     type="int" value="15000"/>   
    </node>
    
    <node pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" output="screen">
      <remap from="cloud" to="/velodyne_points"/>

      <param name="assembling_time" type="double" value="1"/>
      <param name="voxel_size"      type="double" value="0"/>
      <param name="wait_for_transform_duration"      type="double" value="1"/>
    </node>
  
    <!-- SLAM -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg args)">
      <param name="subscribe_rgbd"       type="bool"   value="true"/>
      <param name="rgbd_cameras"    	 type="int"    value="4"/>
      <param name="subscribe_scan_cloud" type="bool"   value="true"/>
      <param name="frame_id"             type="string" value="velodyne"/>
      <param name="odom_sensor_sync" type="bool"   value="true"/>
      <param name="gen_depth" type="bool"   value="true"/>
      <param name="gen_depth_decimation" type="int"   value="8"/>
      <param name="gen_depth_fill_holes_size" type="int"   value="1"/>

      <remap from="rgbd_image0" to="/cam/0/rgbd_image"/>
      <remap from="rgbd_image1" to="/cam/1/rgbd_image"/>
      <remap from="rgbd_image2" to="/cam/2/rgbd_image"/>
      <remap from="rgbd_image3" to="/cam/3/rgbd_image"/>
      
      <remap from="scan_cloud" to="assembled_cloud"/>
     
    </node>
  
    <!-- Visualisation RTAB-Map -->
    <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
      <param name="frame_id"             type="string" value="velodyne"/>
      <param name="subscribe_scan_cloud" type="bool"   value="true"/>
      <remap from="scan_cloud" to="/velodyne_points"/>
      <param name="subscribe_odom_info" type="bool" value="true"/>
    </node>
  
  </group>

</launch>

Issue: The static transforms of the cameras are not accurate, so coloring or texturing the assembled scans has some color registration errors. I enabled gen_depth option to debug this. This will generate an image like this where we can see that depth values are not correctly registered to color cameras. Adjusting correctly the static transform would reduce the registration error.

In DatabaseViewer, with 3D view, open Gui parameters and check Create RGB-D point cloud from RGB projection on scan". You can also show the frustum of the cameras accordingly to base frame (in this case the velodyne) by right-click on 3D View and show the frustum (move the main slider to refresh the view):


Here some outputs that can be generated from the export panel:
Scan:

Scan colored with cameras (note that because of the registration issue, colors don't match exactly the correct structures):

Mesh+Texture:


Regards,
Mathieu