Official RTAB-Map Forum

Painting a pointcloud.

Classic

List

Threaded

16 messages Options

Mikor

Painting a pointcloud.

Hi Mathieu,

I noticed that there is an option to project the pointcloud points back to an image and take the rgb info that corresponds to each xyz and I have some questions.

First of all, does this technique paint each scan independently and afterwards assembles the pointcloud or does it paint the already assembled cloud ?

Also, In case I want to paint the pointcloud I got as result, how should I do this ? Should I subscribe to the rgb topics? Probably not, can you tell me a bit more how this works ?

best regards,
Anthony

matlabbe

Re: Painting a pointcloud.

Administrator

Hi Anthony,

There are two ways to add color to scans. In both approaches, you need subscribe_rgb to be true and feed a color image.
1) enable gen_depth parameter, this will project the latest scan received into the RGB image, creating a depth image. If the lidar scan is dense enough, the RGB-D generated image could be used for loop closure detection. However, generating the map point cloud from those RGB-D images, you will lose lidar field of view.
2) During export, enable camera projection option. The point cloud of the map will be assembled from the lidar, then afterwards the full point cloud will be projected on RGB images, coloring each scan points. Camera projection is done offline in File->Export Clouds... or with rtabmap-export command line tool (see -h).

cheers,
Mathieu

Mikor

Re: Painting a pointcloud.

This post was updated on .

Hello Mathieu,

From the 2 options you gave me I tried the second one as the lidar scan is not dense enough and I do not want the lidar field of view affected. It seems that I get some colored points although they are too sparse and I wonder if I am doing something wrong.

First of all, when the lidar points are getting projected back on the images I get the following warning:

[ WARN] (2021-06-07 15:42:24.961) ExportCloudsDialog.cpp:2635::getExportedClouds() 7 node has invalid camera models,

The camera model as it comes from the camera_info topic is

plumb_bob

and taking it one step back, the images come from the D435's rgb sensor.

The point cloud I am getting is the following:

But the colored points are too sparse, in contrast to painting the cloud with another package for example autoware's pixel cloud fusion But at this case I lose lidar's field of view and odometry drifts.

Does the sparsity of the colored points on rtabmap occur because it keeps the non colored points on the map?
Thanks in advance,
Anthony

Edit: I noticed something strange when I change the voxel filter size. It seems like the density of the colored points is decreased when the voxel filter size is also decreased. Why is this happening?

Voxel filter size 50cm

Mikor

Re: Painting a pointcloud.

Hi again Mathieu,

sorry for bombarding you with questions about this but I want to know how to utilize this tool. This time I am curious about the input topics that can be used for the coloring. More specifically:

[1] Can multiple cameras be used? If yes, then how and how many? Should I subscribe each camera to rtabmap and their topics too?
[2] What kind of images can be supported for painting ? For example, can a 360 camera be used ?

Thanks in advance,
Anthony.

P.S. I am looking forward to try your new iphone app !! So exciting!

Mikor

Re: Painting a pointcloud.

Any updates? I post this so the topic won't be lost.

matlabbe

Re: Painting a pointcloud.

Administrator

This post was updated on .

Hi Mikor,

Sorry I am super late in my emails, got a lot of projects in parallel to debug.

With option 2, if the point cloud is too sparse, enable regeneration option and decrease the voxel size. For example, if the voxel size is 0 and you regenerate scans without decimation, you will have all point cloud of the scans (cannot be more dense than that, except if you assemble all lidar scans prior to rtabmap node with point_cloud_assembler, like in this velodyne example or this example, or that you increase Rtabmap/DetectionRate to add more nodes to graph). You can also set "Keep points not seen by the cameras" to keep all scan points.

The camera projection also works with a multi-cameras setup. You should synchronize each camera with rgbd_sync nodelet, then use subscribe_rgbd=true for rtabmap node, like this example. To use nodelet without a nodelet manager and without depth image, we can do (using rgb_sync instead of rgbd_sync):

   <group ns="camera1">
    <node pkg="nodelet" type="nodelet" name="rgb_sync" args="standalone rtabmap_ros/rgb_sync">
      <remap from="rgb/image"         to="rgb/image_rect_color"/>
      <remap from="rgb/camera_info"   to="rgb/camera_info"/>
    </node>
   </group>
   <group ns="camera2">
    <node pkg="nodelet" type="nodelet" name="rgb_sync" args="standalone rtabmap_ros/rgb_sync">
      <remap from="rgb/image"         to="rgb/image_rect_color"/>
      <remap from="depth/image"       to="depth_registered/image_raw"/>
      <remap from="rgb/camera_info"   to="rgb/camera_info"/>
    </node>
   </group>

Only fisheye and pinhole cameras can be used in rtabmap. 360 cameras are not yet supported. Maximum cameras are limited by the implementation, currently supporting maximum 6 cameras.

For the issue that the colored clouds is smaller as the voxel size is smaller, are you also regenerating the clouds with normals? You may compare with and without normals (setting normal estimation to 0). I cannot reproduce the problem. IF you can share the database, I could try it.

cheers,
Mathieu

Mikor

Re: Painting a pointcloud.

Hi Mathieu,

Unfortunately, I do not have the bag available.

But now I want to give this another try with multiple cameras this time, I have a VLP16 lidar and 4 non-overlapping & calibrated cameras producing 4 rgb topics and 4 camera info topics. The logic again is to create a 3D point cloud through lidar-based slam and then paint the map from the images.

I get the part that you use the nodelets to synchronize them but it is not exactly clear to me how to make this work. For example, at the experiment I did before when I was using a singe rgb topic and the lidar all I did was simply to set "rgb_topic:=/image_topic and camera_info_topic:=/camera_info_topic" and this was enough. Now what should I twick? As always, any recommendation is highly appreciated.

With respect,
Antonis.

Mikor

Re: Painting a pointcloud.

This post was updated on .

In reply to this post by matlabbe

I will try to provide more Information, because I think (probably I should say that I am almost sure) that I am missing something about the use of namespaces, and how to differentiate the instance of using the two kinects and my case:

My tf tree is the following:

And here is a visualization:

And those coordinate frames are referring to the following data:

So my question is how should I structure the launch file to make this work. So far I've removed some visual odometry related parts from the rtambap.launch file and now it is like this.

<!-- -->
<launch>
  <!-- Convenience launch file to launch odometry, rtabmap and rtabmapviz nodes at once -->

  <!-- For stereo:=false
        Your RGB-D sensor should be already started with "depth_registration:=true".
        Examples:
           $ roslaunch freenect_launch freenect.launch depth_registration:=true 
           $ roslaunch openni2_launch openni2.launch depth_registration:=true -->
           
  <!-- For stereo:=true
        Your camera should be calibrated and publishing rectified left and right 
        images + corresponding camera_info msgs. You can use stereo_image_proc for image rectification.
        Example:
           $ roslaunch rtabmap_ros bumblebee.launch -->
     
  <!-- Choose between depth and stereo, set both to false to do only scan -->      
  <arg name="stereo"                    default="false"/>
  <arg     if="$(arg stereo)" name="depth"  default="false"/>
  <arg unless="$(arg stereo)" name="depth"  default="true"/>
 
  <!-- Choose visualization -->
  <arg name="rtabmapviz"              default="true" /> 
  <arg name="rviz"                    default="false" />
  
  <!-- Localization-only mode -->
  <arg name="localization"            default="false"/>
  
  <!-- sim time for convenience, if playing a rosbag -->
  <arg name="use_sim_time"            default="false"/>
  <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
  
  <!-- Corresponding config files -->
  <arg name="cfg"                     default="" /> <!-- To change RTAB-Map's parameters, set the path of config file (*.ini) generated by the standalone app -->
  <arg name="gui_cfg"                 default="~/.ros/rtabmap_gui.ini" />
  <arg name="rviz_cfg"                default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
  
  <arg name="frame_id"                default="camera_link"/>     <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
  <arg name="odom_frame_id"           default=""/>                <!-- If set, TF is used to get odometry instead of the topic -->
  <arg name="odom_frame_id_init"      default=""/>                <!-- If set, TF map->odom is published even if no odometry topic has been received yet. The frame id should match the one in the topic. -->
  <arg name="map_frame_id"            default="map"/>
  <arg name="ground_truth_frame_id"   default=""/>     <!-- e.g., "world" -->
  <arg name="ground_truth_base_frame_id" default=""/>  <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) -->
  <arg name="namespace"               default="rtabmap"/>
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="queue_size"              default="10"/>
  <arg name="wait_for_transform"      default="0.2"/>
  <arg name="args"                    default=""/>              <!-- delete_db_on_start, udebug -->
  <arg name="rtabmap_args"            default="$(arg args)"/>   <!-- deprecated, use "args" argument -->
  <arg name="gdb"                     default="false"/>         <!-- Launch nodes in gdb for debugging (apt install xterm gdb) -->
  <arg     if="$(arg gdb)" name="launch_prefix" default="xterm -e gdb -q -ex run --args"/>
  <arg unless="$(arg gdb)" name="launch_prefix" default=""/>
  <arg name="output"                  default="screen"/>        <!-- Control node output (screen or log) -->
  <arg name="publish_tf_map"          default="true"/>
  
  <!-- stereo related topics -->
  <arg name="stereo_namespace"        default="/stereo_camera"/>
  <arg name="left_image_topic"        default="$(arg stereo_namespace)/left/image_rect_color" />
  <arg name="right_image_topic"       default="$(arg stereo_namespace)/right/image_rect" />      <!-- using grayscale image for efficiency -->
  <arg name="left_camera_info_topic"  default="$(arg stereo_namespace)/left/camera_info" />
  <arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />

  <!-- Already synchronized RGB-D related topic, with rtabmap_ros/rgbd_sync nodelet -->
  <arg name="rgbd_sync"               default="false"/>         <!-- pre-sync rgb_topic, depth_topic, camera_info_topic -->
  <arg name="approx_rgbd_sync"        default="true"/>          <!-- false=exact synchronization -->
  <arg name="subscribe_rgbd"          default="$(arg rgbd_sync)"/>
  <arg name="rgbd_topic"              default="rgbd_image" />
  <arg name="depth_scale"             default="1.0" />         <!-- Deprecated, use rgbd_depth_scale instead -->
  <arg name="rgbd_depth_scale"        default="$(arg depth_scale)" />
  <arg name="rgbd_decimation"         default="1" />

  <arg name="compressed"              default="false"/>         <!-- If you want to subscribe to compressed image topics -->
  <arg name="rgb_image_transport"     default="compressed"/>    <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
  <arg name="depth_image_transport"   default="compressedDepth"/>  <!-- Depth compatible types: compressedDepth (see "rosrun image_transport list_transports") -->
   
  <arg name="subscribe_scan"          default="false"/>
  <arg name="scan_topic"              default="/scan"/>
  <arg name="subscribe_scan_cloud"    default="false"/>
  <arg name="scan_cloud_topic"        default="/scan_cloud"/>
  <arg name="subscribe_scan_descriptor" default="false"/>
  <arg name="scan_descriptor_topic"   default="/scan_descriptor"/>
  <arg name="scan_cloud_max_points"   default="0"/>
  <arg name="scan_cloud_filtered"     default="false"/> <!-- use filtered cloud from icp_odometry for mapping -->
  <arg name="gen_scan"                default="false"/> <!-- only works with depth image and if not subscribing to scan topic-->
   
  <arg name="visual_odometry"          default="true"/>          <!-- Launch rtabmap visual odometry node -->
  <arg name="icp_odometry"             default="false"/>         <!-- Launch rtabmap icp odometry node -->
  <arg name="odom_topic"               default="odom"/>          <!-- Odometry topic name -->
  <arg name="vo_frame_id"              default="$(arg odom_topic)"/> <!-- Visual/Icp odometry frame ID for TF -->
  <arg name="publish_tf_odom"          default="true"/>
  <arg name="odom_tf_angular_variance" default="1"/>             <!-- If TF is used to get odometry, this is the default angular variance -->
  <arg name="odom_tf_linear_variance"  default="1"/>             <!-- If TF is used to get odometry, this is the default linear variance -->
  <arg name="odom_args"                default=""/>              <!-- More arguments for odometry (overwrite same parameters in rtabmap_args) -->
  <arg name="odom_sensor_sync"         default="false"/>
  <arg name="odom_guess_frame_id"        default=""/>
  <arg name="odom_guess_min_translation" default="0"/>
  <arg name="odom_guess_min_rotation"    default="0"/>
  <arg name="odom_max_rate"            default="0"/>
  <arg name="odom_expected_rate"       default="0"/>
  <arg name="imu_topic"                default="/imu/data"/>          <!-- only used with VIO approaches -->
  <arg name="wait_imu_to_init"         default="false"/>
  <arg name="use_odom_features"        default="false"/>
  
  <arg name="scan_cloud_assembling"              default="false"/>
  <arg name="scan_cloud_assembling_time"         default="1"/>
  <arg name="scan_cloud_assembling_fixed_frame"  default=""/>
  <arg name="scan_cloud_assembling_voxel_size"   default="0.05"/>
  <arg name="scan_cloud_assembling_noise_radius"   default="0.0"/>    <!-- 0=disabled -->
  <arg name="scan_cloud_assembling_noise_min_neighbors"   default="5"/>
  
  <arg name="subscribe_user_data"      default="false"/>             <!-- user data synchronized subscription -->
  <arg name="user_data_topic"          default="/user_data"/>
  <arg name="user_data_async_topic"    default="/user_data_async" /> <!-- user data async subscription (rate should be lower than map update rate) -->
  
  <arg name="gps_topic"                default="/gps/fix" />         <!-- gps async subscription -->
  
  <arg name="tag_topic"                default="/tag_detections" />  <!-- apriltags async subscription -->
  <arg name="tag_linear_variance"      default="0.0001" />
  <arg name="tag_angular_variance"     default="9999" />             <!-- >=9999 means ignore rotation in optimization, when rotation estimation of the tag is not reliable -->

    <!-- RGB-D related topics -->
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
  <arg name="depth_camera_info_topic" default="$(arg camera_info_topic)" />

  <!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
  <arg if="$(arg compressed)"     name="rgb_topic_relay"           default="$(arg rgb_topic)_relay"/>
  <arg unless="$(arg compressed)" name="rgb_topic_relay"           default="$(arg rgb_topic)"/>
  <arg if="$(arg compressed)"     name="depth_topic_relay"         default="$(arg depth_topic)_relay"/>
  <arg unless="$(arg compressed)" name="depth_topic_relay"         default="$(arg depth_topic)"/>
  <arg if="$(arg rgbd_sync)"      name="rgbd_topic_relay"          default="$(arg rgbd_topic)"/>
  <arg unless="$(arg rgbd_sync)"  name="rgbd_topic_relay"          default="$(arg rgbd_topic)_relay"/>
  <arg if="$(arg compressed)"     name="left_image_topic_relay"    default="$(arg left_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="left_image_topic_relay"    default="$(arg left_image_topic)"/>
  <arg if="$(arg compressed)"     name="right_image_topic_relay"   default="$(arg right_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="right_image_topic_relay"   default="$(arg right_image_topic)"/>

  <!-- Nodes -->
  <group ns="$(arg namespace)">
     
    <!-- relays -->
   <group ns="cam/0">
    <node pkg="nodelet" type="nodelet" name="rgb_sync" args="standalone rtabmap_ros/rgb_sync">
      <remap from="/image_raw"         to="rgb/image_rect_color"/>
      <remap from="/camera_info"   to="rgb/camera_info"/>
    </node>
   </group>
   <group ns="cam/1">
    <node pkg="nodelet" type="nodelet" name="rgb_sync" args="standalone rtabmap_ros/rgb_sync">
      <remap from="/image_raw"         to="rgb/image_rect_color"/>
      <remap from="/camera_info"   to="rgb/camera_info"/>
    </node>
   </group>   
   <group ns="cam/2">
    <node pkg="nodelet" type="nodelet" name="rgb_sync" args="standalone rtabmap_ros/rgb_sync">
      <remap from="/image_raw"         to="rgb/image_rect_color"/>
      <remap from="/camera_info"   to="rgb/camera_info"/>
    </node>
   </group>
   <group ns="cam/3">
    <node pkg="nodelet" type="nodelet" name="rgb_sync" args="standalone rtabmap_ros/rgb_sync">
      <remap from="/image_raw"         to="rgb/image_rect_color"/>ρ
      <remap from="/camera_info"   to="rgb/camera_info"/>
    </node>
   </group>
    
    <!-- ICP Odometry -->
    <node if="$(arg icp_odometry)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
      <remap from="imu"                    to="$(arg imu_topic)"/>
      
      <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
      <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
      <param name="publish_tf"                  type="bool"   value="$(arg publish_tf_odom)"/>
      <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
      <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
      <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
      <param name="wait_imu_to_init"            type="bool"   value="$(arg wait_imu_to_init)"/>
      <param name="config_path"                 type="string" value="$(arg cfg)"/>
      <param name="queue_size"                  type="int"    value="$(arg queue_size)"/>
      <param name="guess_frame_id"              type="string" value="$(arg odom_guess_frame_id)"/>
      <param name="guess_min_translation"       type="double" value="$(arg odom_guess_min_translation)"/>
      <param name="guess_min_rotation"          type="double" value="$(arg odom_guess_min_rotation)"/>
      <param name="scan_cloud_max_points"       type="int"    value="$(arg scan_cloud_max_points)"/>
      <param name="expected_update_rate"        type="double" value="$(arg odom_expected_rate)"/>
      <param name="max_update_rate"             type="double" value="$(arg odom_max_rate)"/>
    </node>
    
    <node if="$(arg scan_cloud_assembling)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" output="screen">
      <remap     if="$(arg scan_cloud_filtered)" from="cloud" to="odom_filtered_input_scan"/>
      <remap unless="$(arg scan_cloud_filtered)" from="cloud" to="$(arg scan_cloud_topic)"/>

      <remap from="odom"            to="$(arg odom_topic)"/>
      <param name="assembling_time" type="double" value="$(arg scan_cloud_assembling_time)"/>
      <param name="fixed_frame_id"  type="string" value="$(arg scan_cloud_assembling_fixed_frame)"/>
      <param name="voxel_size"      type="double" value="$(arg scan_cloud_assembling_voxel_size)"/>
      <param name="noise_radius"        type="double" value="$(arg scan_cloud_assembling_noise_radius)"/>
      <param name="noise_min_neighbors" type="int"    value="$(arg scan_cloud_assembling_noise_min_neighbors)"/>
    </node>
  
    <!-- Visual SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
      <param     if="$(arg stereo)" name="subscribe_depth"  type="bool" value="false"/>
      <param unless="$(arg stereo)" name="subscribe_depth"  type="bool" value="$(arg depth)"/>
      <param name="subscribe_rgb"        type="bool"   value="$(arg depth)"/>
      <param name="subscribe_rgbd"       type="bool"   value="$(eval subscribe_rgbd or use_odom_features)"/>
      <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
      <param name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
      <param name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
      <param name="subscribe_scan_descriptor" type="bool" value="$(arg subscribe_scan_descriptor)"/>
      <param name="subscribe_user_data"  type="bool"   value="$(arg subscribe_user_data)"/>
      <param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
      <param if="$(arg icp_odometry)"    name="subscribe_odom_info" type="bool" value="true"/>
      <param name="frame_id"             type="string" value="$(arg frame_id)"/>
      <param name="map_frame_id"         type="string" value="$(arg map_frame_id)"/>
      <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
      <param name="odom_frame_id_init"   type="string" value="$(arg odom_frame_id_init)"/>
      <param name="publish_tf"           type="bool"   value="$(arg publish_tf_map)"/>
      <param name="gen_scan"             type="bool"   value="$(arg gen_scan)"/>
      <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
      <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
      <param name="odom_tf_angular_variance" type="double" value="$(arg odom_tf_angular_variance)"/>
      <param name="odom_tf_linear_variance"  type="double" value="$(arg odom_tf_linear_variance)"/>
      <param name="odom_sensor_sync"         type="bool"   value="$(arg odom_sensor_sync)"/>
      <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
      <param name="database_path"        type="string" value="$(arg database_path)"/>
      <param name="approx_sync"          type="bool"   value="$(eval approx_sync and not use_odom_features)"/>
      <param name="config_path"          type="string" value="$(arg cfg)"/>
      <param name="queue_size"           type="int" value="$(arg queue_size)"/>
      <param name="scan_cloud_max_points"     type="int" value="$(arg scan_cloud_max_points)"/>
      <param name="landmark_linear_variance"   type="double" value="$(arg tag_linear_variance)"/>
      <param name="landmark_angular_variance"  type="double" value="$(arg tag_angular_variance)"/>      
      
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      
      <remap     if="$(arg use_odom_features)" from="rgbd_image" to="odom_rgbd_image"/>
      <remap unless="$(arg use_odom_features)" from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
	
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
      
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap if="$(eval scan_cloud_assembling)" from="scan_cloud" to="assembled_cloud"/>
      <remap if="$(eval scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="odom_filtered_input_scan"/>
      <remap if="$(eval not scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
      <remap from="scan_descriptor"        to="$(arg scan_descriptor_topic)"/>
      <remap from="user_data"              to="$(arg user_data_topic)"/>
      <remap from="user_data_async"        to="$(arg user_data_async_topic)"/>
      <remap from="gps/fix"                to="$(arg gps_topic)"/>
      <remap from="tag_detections"         to="$(arg tag_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
      <remap from="imu"                    to="$(arg imu_topic)"/>
      
      <!-- localization mode -->
      <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
      <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
      <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
    </node>
  
    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
      <param     if="$(arg stereo)" name="subscribe_depth"  type="bool" value="false"/>
      <param unless="$(arg stereo)" name="subscribe_depth"  type="bool" value="$(arg depth)"/>
      <param name="subscribe_rgbd"       type="bool"   value="$(eval subscribe_rgbd or use_odom_features)"/>
      <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
      <param unless="$(arg icp_odometry)" name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
      <param     if="$(arg icp_odometry)" name="subscribe_scan_cloud" type="bool"   value="$(eval subscribe_scan or subscribe_scan_cloud)"/>
      <param unless="$(arg icp_odometry)" name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
      <param name="subscribe_scan_descriptor" type="bool" value="$(arg subscribe_scan_descriptor)"/>
      <param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
      <param if="$(arg icp_odometry)"    name="subscribe_odom_info" type="bool" value="true"/>
      <param name="frame_id"             type="string" value="$(arg frame_id)"/>
      <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
      <param name="wait_for_transform_duration" type="double"   value="$(arg wait_for_transform)"/>
      <param name="queue_size"           type="int"    value="$(arg queue_size)"/>
      <param name="approx_sync"          type="bool"   value="$(eval approx_sync and not use_odom_features)"/>
    
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      
      <remap     if="$(arg use_odom_features)" from="rgbd_image" to="odom_rgbd_image"/>
      <remap unless="$(arg use_odom_features)" from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
      
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
      
      <remap unless="$(arg icp_odometry)" from="scan" to="$(arg scan_topic)"/>
      <remap     if="$(arg icp_odometry)" from="scan_cloud" to="odom_filtered_input_scan"/>
      <remap unless="$(arg icp_odometry)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
      <remap from="scan_descriptor"        to="$(arg scan_descriptor_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
    </node>
  
  </group>
  
  <!-- Visualization RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb" output="$(arg output)">
    <remap if="$(arg stereo)" from="left/image"        to="$(arg left_image_topic_relay)"/>
    <remap if="$(arg stereo)" from="right/image"       to="$(arg right_image_topic_relay)"/>
    <remap if="$(arg stereo)" from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
    <remap if="$(arg stereo)" from="right/camera_info" to="$(arg right_camera_info_topic)"/>
    <remap unless="$(arg subscribe_rgbd)" from="rgb/image"         to="$(arg rgb_topic_relay)"/>
    <remap unless="$(arg subscribe_rgbd)" from="depth/image"       to="$(arg depth_topic_relay)"/>
    <remap unless="$(arg subscribe_rgbd)" from="rgb/camera_info"   to="$(arg camera_info_topic)"/>
    <remap from="rgbd_image"        to="$(arg rgbd_topic_relay)"/>
    <remap from="cloud"             to="voxel_cloud" />

    <param name="decimation"  type="double" value="4"/>
    <param name="voxel_size"  type="double" value="0.0"/>
    <param name="approx_sync" type="bool"   value="$(arg approx_sync)"/>
  </node>

</launch>

Near the middle part you can see the the relays I've added.

Now I am using this command to open up the nodes:

roslaunch rtabmap_ros rtabmap_multicamera_test.launch    \
   use_sim_time:=true \
   depth:=false \
   subscribe_rgb:=true \
   subscribe_scan_cloud:=true \
   rgb_topic:=/cam/0/image_raw \
   rgb_topic1:=/cam/1/image_raw \
   rgb_topic2:=/cam/2/image_raw \
   rgb_topic3:=/cam/3/image_raw \
   camera_info0:=/cam/0/camera_info \
   camera_info1:=/cam/1/camera_info \
   camera_info2:=/cam/2/camera_info \
   camera_info3:=/cam/3/camera_info \
   frame_id:=/cam_2 \
   scan_cloud_topic:=/velodyne_points \
   scan_cloud_max_points:=15000  \
   icp_odometry:=true \
   approx_sync:=false \
   scan_cloud_assembling:=true \
   scan_cloud_assembling_voxel_size:=0 \
   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 \
      --Icp/PMMatcherKnn 3 \
      --Icp/PMMatcherEpsilon 1 \
      --Icp/Epsilon 0.0001 \
      --Icp/PointToPlaneK 10 \
      --Icp/PointToPlaneRadius 0 \
      --Icp/CorrespondenceRatio 0.01 \
      --Icp/PointToPlaneGroundNormalsUp 0.3"

But I get this error:
[ WARN] [1629986688.137274147, 1629829809.902043191]: /rtabmap/cam/2/rgb_sync: 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. Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
/rtabmap/cam/2/rgb_sync subscribed to (exact sync):
/rtabmap/cam/2/rgb/image_rect \
/rtabmap/cam/2/rgb/camera_info
[ WARN] [1629986688.137434161, 1629829809.902043191]: /rtabmap/cam/3/rgb_sync: 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. Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
/rtabmap/cam/3/rgb_sync subscribed to (exact sync):
/rtabmap/cam/3/rgb/image_rect \
/rtabmap/cam/3/rgb/camera_info
[ WARN] [1629986688.137445931, 1629829809.902043191]: /rtabmap/cam/0/rgb_sync: 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. Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
/rtabmap/cam/0/rgb_sync subscribed to (exact sync):
/rtabmap/cam/0/rgb/image_rect \
/rtabmap/cam/0/rgb/camera_info
[ WARN] [1629986688.137989218, 1629829809.902043191]: /rtabmap/cam/1/rgb_sync: 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. Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
/rtabmap/cam/1/rgb_sync subscribed to (exact sync):
/rtabmap/cam/1/rgb/image_rect \
/rtabmap/cam/1/rgb/camera_info

And now it is not exactly clear to me how should I tackle this. Any ideas?

matlabbe

Re: Painting a pointcloud.

Administrator

You don't need relays, the warnings tell you that image and their camera_info cannot be syncrhonized with exact time policy. Make sure for each image published, there is a camera_info with exactly the same time stamp in the header. EDIT check first if they are published: "rostopic hz /rtabmap/cam/2/rgb/image_rect /rtabmap/cam/2/rgb/camera_info"

Afterwards, for rtabmap node, subscribe_rgbd should be always true. You also need to set rgbd_cameras parameter to tell how many cameras you want to synchronize in rtabmap node (example here). Then, remap rgbd_image0, rgbd_image1... topic of rtabmap node to rgbd_image published by the rgb_sync nodelets (similar to this).

cheers,
Mathieu

Mikor

Re: Painting a pointcloud.

Hi Mathieu, those topics are not being published. Camera_infos and image topics are in sync and also every camera is in sync.

Here is my launch command:

roslaunch rtabmap_ros rtabmap_multicamera_test.launch    \
    use_sim_time:=true \
    depth:=false \
    subscribe_scan_cloud:=true \
    subscribe_rgbd:=false \
    rgbd_image0:=/cam/0/image_raw \
    rgbd_image1:=/cam/1/image_raw \
    rgbd_image2:=/cam/2/image_raw \
    rgbd_image3:=/cam/3/image_raw \
    camera_info0:=/cam/0/camera_info \
    camera_info1:=/cam/1/camera_info \
    camera_info2:=/cam/2/camera_info \
    camera_info3:=/cam/3/camera_info \
    frame_id:=/cam_2\
    scan_cloud_topic:=/velodyne_points \
    scan_cloud_max_points:=15000  \
    icp_odometry:=true \
    approx_sync:=false \
    scan_cloud_assembling:=true \
    scan_cloud_assembling_voxel_size:=0 \
    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 \
       --Icp/PMMatcherKnn 3 \
       --Icp/PMMatcherEpsilon 1 \
       --Icp/Epsilon 0.0001 \
       --Icp/PointToPlaneK 10 \
       --Icp/PointToPlaneRadius 0 \
       --Icp/CorrespondenceRatio 0.01 \
       --Icp/PointToPlaneGroundNormalsUp 0.3"

For this launch file:

<!-- -->
<launch>
  <!-- Convenience launch file to launch odometry, rtabmap and rtabmapviz nodes at once -->

  <!-- For stereo:=false
        Your RGB-D sensor should be already started with "depth_registration:=true".
        Examples:
           $ roslaunch freenect_launch freenect.launch depth_registration:=true 
           $ roslaunch openni2_launch openni2.launch depth_registration:=true -->
           
  <!-- For stereo:=true
        Your camera should be calibrated and publishing rectified left and right 
        images + corresponding camera_info msgs. You can use stereo_image_proc for image rectification.
        Example:
           $ roslaunch rtabmap_ros bumblebee.launch -->
     
  <!-- Choose between depth and stereo, set both to false to do only scan -->      
  <arg name="stereo"                    default="false"/>
  <arg     if="$(arg stereo)" name="depth"  default="false"/>
  <arg unless="$(arg stereo)" name="depth"  default="true"/>
 
  <!-- Choose visualization -->
  <arg name="rtabmapviz"              default="true" /> 
  <arg name="rviz"                    default="false" />
  
  <!-- Localization-only mode -->
  <arg name="localization"            default="false"/>
  
  <!-- sim time for convenience, if playing a rosbag -->
  <arg name="use_sim_time"            default="false"/>
  <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
  
  <!-- Corresponding config files -->
  <arg name="cfg"                     default="" /> <!-- To change RTAB-Map's parameters, set the path of config file (*.ini) generated by the standalone app -->
  <arg name="gui_cfg"                 default="~/.ros/rtabmap_gui.ini" />
  <arg name="rviz_cfg"                default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
  
  <arg name="frame_id"                default="camera_link"/>     <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
  <arg name="odom_frame_id"           default=""/>                <!-- If set, TF is used to get odometry instead of the topic -->
  <arg name="odom_frame_id_init"      default=""/>                <!-- If set, TF map->odom is published even if no odometry topic has been received yet. The frame id should match the one in the topic. -->
  <arg name="map_frame_id"            default="map"/>
  <arg name="ground_truth_frame_id"   default=""/>     <!-- e.g., "world" -->
  <arg name="ground_truth_base_frame_id" default=""/>  <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) -->
  <arg name="namespace"               default="rtabmap"/>
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="queue_size"              default="10"/>
  <arg name="wait_for_transform"      default="0.2"/>
  <arg name="args"                    default=""/>              <!-- delete_db_on_start, udebug -->
  <arg name="rtabmap_args"            default="$(arg args)"/>   <!-- deprecated, use "args" argument -->
  <arg name="gdb"                     default="false"/>         <!-- Launch nodes in gdb for debugging (apt install xterm gdb) -->
  <arg     if="$(arg gdb)" name="launch_prefix" default="xterm -e gdb -q -ex run --args"/>
  <arg unless="$(arg gdb)" name="launch_prefix" default=""/>
  <arg name="output"                  default="screen"/>        <!-- Control node output (screen or log) -->
  <arg name="publish_tf_map"          default="true"/>
  
  <!-- stereo related topics -->
  <arg name="stereo_namespace"        default="/stereo_camera"/>
  <arg name="left_image_topic"        default="$(arg stereo_namespace)/left/image_rect_color" />
  <arg name="right_image_topic"       default="$(arg stereo_namespace)/right/image_rect" />      <!-- using grayscale image for efficiency -->
  <arg name="left_camera_info_topic"  default="$(arg stereo_namespace)/left/camera_info" />
  <arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />

  <!-- Already synchronized RGB-D related topic, with rtabmap_ros/rgbd_sync nodelet -->
  <arg name="rgbd_sync"               default="false"/>         <!-- pre-sync rgb_topic, depth_topic, camera_info_topic -->
  <arg name="approx_rgbd_sync"        default="true"/>          <!-- false=exact synchronization -->
  <arg name="subscribe_rgbd"          default="$(arg rgbd_sync)"/>
  <arg name="rgbd_topic"              default="rgbd_image" />
  <arg name="depth_scale"             default="1.0" />         <!-- Deprecated, use rgbd_depth_scale instead -->
  <arg name="rgbd_depth_scale"        default="$(arg depth_scale)" />
  <arg name="rgbd_decimation"         default="1" />

  <arg name="compressed"              default="false"/>         <!-- If you want to subscribe to compressed image topics -->
  <arg name="rgb_image_transport"     default="compressed"/>    <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
  <arg name="depth_image_transport"   default="compressedDepth"/>  <!-- Depth compatible types: compressedDepth (see "rosrun image_transport list_transports") -->
   
  <arg name="subscribe_scan"          default="false"/>
  <arg name="scan_topic"              default="/scan"/>
  <arg name="subscribe_scan_cloud"    default="false"/>
  <arg name="scan_cloud_topic"        default="/scan_cloud"/>
  <arg name="subscribe_scan_descriptor" default="false"/>
  <arg name="scan_descriptor_topic"   default="/scan_descriptor"/>
  <arg name="scan_cloud_max_points"   default="0"/>
  <arg name="scan_cloud_filtered"     default="false"/> <!-- use filtered cloud from icp_odometry for mapping -->
  <arg name="gen_scan"                default="false"/> <!-- only works with depth image and if not subscribing to scan topic-->
   
  <arg name="visual_odometry"          default="true"/>          <!-- Launch rtabmap visual odometry node -->
  <arg name="icp_odometry"             default="false"/>         <!-- Launch rtabmap icp odometry node -->
  <arg name="odom_topic"               default="odom"/>          <!-- Odometry topic name -->
  <arg name="vo_frame_id"              default="$(arg odom_topic)"/> <!-- Visual/Icp odometry frame ID for TF -->
  <arg name="publish_tf_odom"          default="true"/>
  <arg name="odom_tf_angular_variance" default="1"/>             <!-- If TF is used to get odometry, this is the default angular variance -->
  <arg name="odom_tf_linear_variance"  default="1"/>             <!-- If TF is used to get odometry, this is the default linear variance -->
  <arg name="odom_args"                default=""/>              <!-- More arguments for odometry (overwrite same parameters in rtabmap_args) -->
  <arg name="odom_sensor_sync"         default="false"/>
  <arg name="odom_guess_frame_id"        default=""/>
  <arg name="odom_guess_min_translation" default="0"/>
  <arg name="odom_guess_min_rotation"    default="0"/>
  <arg name="odom_max_rate"            default="0"/>
  <arg name="odom_expected_rate"       default="0"/>
  <arg name="imu_topic"                default="/imu/data"/>          <!-- only used with VIO approaches -->
  <arg name="wait_imu_to_init"         default="false"/>
  <arg name="use_odom_features"        default="false"/>
  
  <arg name="scan_cloud_assembling"              default="false"/>
  <arg name="scan_cloud_assembling_time"         default="1"/>
  <arg name="scan_cloud_assembling_fixed_frame"  default=""/>
  <arg name="scan_cloud_assembling_voxel_size"   default="0.05"/>
  <arg name="scan_cloud_assembling_noise_radius"   default="0.0"/>    <!-- 0=disabled -->
  <arg name="scan_cloud_assembling_noise_min_neighbors"   default="5"/>
  
  <arg name="subscribe_user_data"      default="false"/>             <!-- user data synchronized subscription -->
  <arg name="user_data_topic"          default="/user_data"/>
  <arg name="user_data_async_topic"    default="/user_data_async" /> <!-- user data async subscription (rate should be lower than map update rate) -->
  
  <arg name="gps_topic"                default="/gps/fix" />         <!-- gps async subscription -->
  
  <arg name="tag_topic"                default="/tag_detections" />  <!-- apriltags async subscription -->
  <arg name="tag_linear_variance"      default="0.0001" />
  <arg name="tag_angular_variance"     default="9999" />             <!-- >=9999 means ignore rotation in optimization, when rotation estimation of the tag is not reliable -->

    <!-- RGB-D related topics -->
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
  <arg name="depth_camera_info_topic" default="$(arg camera_info_topic)" />

  <!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
  <arg if="$(arg compressed)"     name="rgb_topic_relay"           default="$(arg rgb_topic)_relay"/>
  <arg unless="$(arg compressed)" name="rgb_topic_relay"           default="$(arg rgb_topic)"/>
  <arg if="$(arg compressed)"     name="depth_topic_relay"         default="$(arg depth_topic)_relay"/>
  <arg unless="$(arg compressed)" name="depth_topic_relay"         default="$(arg depth_topic)"/>
  <arg if="$(arg rgbd_sync)"      name="rgbd_topic_relay"          default="$(arg rgbd_topic)"/>
  <arg unless="$(arg rgbd_sync)"  name="rgbd_topic_relay"          default="$(arg rgbd_topic)_relay"/>
  <arg if="$(arg compressed)"     name="left_image_topic_relay"    default="$(arg left_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="left_image_topic_relay"    default="$(arg left_image_topic)"/>
  <arg if="$(arg compressed)"     name="right_image_topic_relay"   default="$(arg right_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="right_image_topic_relay"   default="$(arg right_image_topic)"/>



  <!-- Nodes -->
  <group ns="$(arg namespace)">
    
    <!-- ICP Odometry -->
    <node if="$(arg icp_odometry)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
      <remap from="imu"                    to="$(arg imu_topic)"/>
      
      <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
      <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
      <param name="publish_tf"                  type="bool"   value="$(arg publish_tf_odom)"/>
      <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
      <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
      <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
      <param name="wait_imu_to_init"            type="bool"   value="$(arg wait_imu_to_init)"/>
      <param name="config_path"                 type="string" value="$(arg cfg)"/>
      <param name="queue_size"                  type="int"    value="$(arg queue_size)"/>
      <param name="guess_frame_id"              type="string" value="$(arg odom_guess_frame_id)"/>
      <param name="guess_min_translation"       type="double" value="$(arg odom_guess_min_translation)"/>
      <param name="guess_min_rotation"          type="double" value="$(arg odom_guess_min_rotation)"/>
      <param name="scan_cloud_max_points"       type="int"    value="$(arg scan_cloud_max_points)"/>
      <param name="expected_update_rate"        type="double" value="$(arg odom_expected_rate)"/>
      <param name="max_update_rate"             type="double" value="$(arg odom_max_rate)"/>
    </node>
    
    <node if="$(arg scan_cloud_assembling)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" output="screen">
      <remap     if="$(arg scan_cloud_filtered)" from="cloud" to="odom_filtered_input_scan"/>
      <remap unless="$(arg scan_cloud_filtered)" from="cloud" to="$(arg scan_cloud_topic)"/>

      <remap from="odom"            to="$(arg odom_topic)"/>
      <param name="assembling_time" type="double" value="$(arg scan_cloud_assembling_time)"/>
      <param name="fixed_frame_id"  type="string" value="$(arg scan_cloud_assembling_fixed_frame)"/>
      <param name="voxel_size"      type="double" value="$(arg scan_cloud_assembling_voxel_size)"/>
      <param name="noise_radius"        type="double" value="$(arg scan_cloud_assembling_noise_radius)"/>
      <param name="noise_min_neighbors" type="int"    value="$(arg scan_cloud_assembling_noise_min_neighbors)"/>
    </node>
  
    <!-- Visual SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
      <param     if="$(arg stereo)" name="subscribe_depth"  type="bool" value="false"/>
      <param unless="$(arg stereo)" name="subscribe_depth"  type="bool" value="$(arg depth)"/>
      <param name="subscribe_rgb"        type="bool"   value="$(arg depth)"/>
      <param name="subscribe_rgbd"       type="bool"   value="$(eval subscribe_rgbd or use_odom_features)"/>
      <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
      <param name="rgbd_cameras"    	 type="int"    value="4"/>
      <param name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
      <param name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
      <param name="subscribe_scan_descriptor" type="bool" value="$(arg subscribe_scan_descriptor)"/>
      <param name="subscribe_user_data"  type="bool"   value="$(arg subscribe_user_data)"/>
      <param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
      <param if="$(arg icp_odometry)"    name="subscribe_odom_info" type="bool" value="true"/>
      <param name="frame_id"             type="string" value="$(arg frame_id)"/>
      <param name="map_frame_id"         type="string" value="$(arg map_frame_id)"/>
      <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
      <param name="odom_frame_id_init"   type="string" value="$(arg odom_frame_id_init)"/>
      <param name="publish_tf"           type="bool"   value="$(arg publish_tf_map)"/>
      <param name="gen_scan"             type="bool"   value="$(arg gen_scan)"/>
      <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
      <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
      <param name="odom_tf_angular_variance" type="double" value="$(arg odom_tf_angular_variance)"/>
      <param name="odom_tf_linear_variance"  type="double" value="$(arg odom_tf_linear_variance)"/>
      <param name="odom_sensor_sync"         type="bool"   value="$(arg odom_sensor_sync)"/>
      <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
      <param name="database_path"        type="string" value="$(arg database_path)"/>
      <param name="approx_sync"          type="bool"   value="$(eval approx_sync and not use_odom_features)"/>
      <param name="config_path"          type="string" value="$(arg cfg)"/>
      <param name="queue_size"           type="int" value="$(arg queue_size)"/>
      <param name="scan_cloud_max_points"     type="int" value="$(arg scan_cloud_max_points)"/>
      <param name="landmark_linear_variance"   type="double" value="$(arg tag_linear_variance)"/>
      <param name="landmark_angular_variance"  type="double" value="$(arg tag_angular_variance)"/>      
      
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      
      <remap     if="$(arg use_odom_features)" from="rgbd_image" to="odom_rgbd_image"/>
      <remap unless="$(arg use_odom_features)" from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
	
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
      
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap if="$(eval scan_cloud_assembling)" from="scan_cloud" to="assembled_cloud"/>
      <remap if="$(eval scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="odom_filtered_input_scan"/>
      <remap if="$(eval not scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
      <remap from="scan_descriptor"        to="$(arg scan_descriptor_topic)"/>
      <remap from="user_data"              to="$(arg user_data_topic)"/>
      <remap from="user_data_async"        to="$(arg user_data_async_topic)"/>
      <remap from="gps/fix"                to="$(arg gps_topic)"/>
      <remap from="tag_detections"         to="$(arg tag_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
      <remap from="imu"                    to="$(arg imu_topic)"/>
      
      <!-- localization mode -->
      <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
      <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
      <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
    </node>
  
    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
      <param     if="$(arg stereo)" name="subscribe_depth"  type="bool" value="false"/>
      <param unless="$(arg stereo)" name="subscribe_depth"  type="bool" value="$(arg depth)"/>
      <param name="subscribe_rgbd"       type="bool"   value="$(eval subscribe_rgbd or use_odom_features)"/>
      <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
      <param unless="$(arg icp_odometry)" name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
      <param     if="$(arg icp_odometry)" name="subscribe_scan_cloud" type="bool"   value="$(eval subscribe_scan or subscribe_scan_cloud)"/>
      <param unless="$(arg icp_odometry)" name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
      <param name="subscribe_scan_descriptor" type="bool" value="$(arg subscribe_scan_descriptor)"/>
      <param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
      <param if="$(arg icp_odometry)"    name="subscribe_odom_info" type="bool" value="true"/>
      <param name="frame_id"             type="string" value="$(arg frame_id)"/>
      <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
      <param name="wait_for_transform_duration" type="double"   value="$(arg wait_for_transform)"/>
      <param name="queue_size"           type="int"    value="$(arg queue_size)"/>
      <param name="approx_sync"          type="bool"   value="$(eval approx_sync and not use_odom_features)"/>
    
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      
      <remap     if="$(arg use_odom_features)" from="rgbd_image" to="odom_rgbd_image"/>
      <remap unless="$(arg use_odom_features)" from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
      
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
      
      <remap unless="$(arg icp_odometry)" from="scan" to="$(arg scan_topic)"/>
      <remap     if="$(arg icp_odometry)" from="scan_cloud" to="odom_filtered_input_scan"/>
      <remap unless="$(arg icp_odometry)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
      <remap from="scan_descriptor"        to="$(arg scan_descriptor_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
    </node>
  
  </group>
  
  <!-- Visualization RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb" output="$(arg output)">
    <remap if="$(arg stereo)" from="left/image"        to="$(arg left_image_topic_relay)"/>
    <remap if="$(arg stereo)" from="right/image"       to="$(arg right_image_topic_relay)"/>
    <remap if="$(arg stereo)" from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
    <remap if="$(arg stereo)" from="right/camera_info" to="$(arg right_camera_info_topic)"/>
    <remap unless="$(arg subscribe_rgbd)" from="rgb/image"         to="$(arg rgb_topic_relay)"/>
    <remap unless="$(arg subscribe_rgbd)" from="depth/image"       to="$(arg depth_topic_relay)"/>
    <remap unless="$(arg subscribe_rgbd)" from="rgb/camera_info"   to="$(arg camera_info_topic)"/>
    <remap from="rgbd_image"        to="$(arg rgbd_topic_relay)"/>
    <remap from="cloud"             to="voxel_cloud" />

    <param name="decimation"  type="double" value="4"/>
    <param name="voxel_size"  type="double" value="0.0"/>
    <param name="approx_sync" type="bool"   value="$(arg approx_sync)"/>
  </node>

</launch>

And I take a pointcloud like this when I select camera projection at point cloud export which is similar at I showed at the begin of this thread:

If I set subscribe_rgbd:=true I am getting first of all this which is expected:

Cannot synchronize more than 1 rgbd topic (rtabmap_ros has been built without RTABMAP_SYNC_MULTI_RGBD option)

But they are already in sync, and also this output:

[ WARN] [1630079164.045092642, 1629829839.932245252]: /rtabmap/rtabmapviz: 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"). Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
/rtabmap/rtabmapviz subscribed to (exact sync):
   /rtabmap/odom \
   /rtabmap/rgbd_image_relay \
   /rtabmap/odom_filtered_input_scan
[ WARN] [1630079164.045470381, 1629829839.932245252]: /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"). Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.

For easier debugging I attach the BAG of the data but if you want to post images please post only areas with no people like zoomed in walls and small details, thanks in advance!

Here is the link to the bag

Here are the static publishers for the tf tree:

rosrun tf2_ros static_transform_publisher 0 -0.125 -0.0375 1.5708 -1.5708 0 cam_2 velodyne

rosrun tf2_ros static_transform_publisher 0.0375 0 -0.0375 0 1.5708 0 cam_2 cam_0

rosrun tf2_ros static_transform_publisher 0.0375 0 -0.0375 0 1.5708 0 cam_0 cam_3

rosrun tf2_ros static_transform_publisher 0.0375 0 -0.0375 0 1.5708 0 cam_3 cam_1

matlabbe

Re: Painting a pointcloud.

Administrator

This post was updated on .

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

Mikor

Re: Painting a pointcloud.

Hello Mathieu,

Thanks for taking the time!! I have some questions.

[1] First of all I try to replicate what you did but it seems that only the odometry is being computed and the mapping does not take place, here is the output when I run launch the command https://pastebin.pl/view/dc7c1892

Any ideas what's wrong or different than yours?

[2] I see that you set scan_cloud_max_points to 0 is this because you generated depth?

Thanks in advance.

Wishing all the best,
Anthony.

matlabbe

Re: Painting a pointcloud.

Administrator

Hi,

The log is just showing the initialization logs, is the bag started?

scan_cloud_max_points could be set to real max points of the lidar, it is because I assemble the point clouds prior to rtabmap node and before this commit, the scan_cloud_max_points was wrongly set to rtabmap node when we subscribed to assembled cloud. Now scan_cloud_max_points is disabled if assemble cloud is enabled, but scan_cloud_max_points should still be correctly used for icp_odometry.

EDIT: I've put scan_cloud_max_points back to right place, thx for the observation.

cheers,
Mathieu

Mikor

Re: Painting a pointcloud.

Hello Mathieu,

here is what I mean it only computes the odometry, and it got me confused because I run exactly the same launch file with this command from the terminal:

roslaunch rtabmap_ros 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 \
       Icp/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"

Do you believe that there is something more I could check and get back to you so we can sort it out?

With respect,
Anthony

Mikor

Re: Painting a pointcloud.

This post was updated on .

I've also updated rtabmap and rtabmap_ros and build it with sync multiple rgbd = on but still the problem persists.

Edit: I've used rqt_graph and noticed that the velodyne_points are not used anywhere and thought that probably that's why the rtabmap node does not do the mapping because it does not get any pointcloud information.

So, I "unsubscribed" the assembled_cloud from rtabmap and put the velodyne points instead and then the mapping took place for a while and then it went back to calculating only the odometry.

[ INFO] [1630570233.393540313, 1629829804.850763082]: Odom: ratio=0.000000, std dev=99.995000m|99.995000rad, update time=0.154839s
[ INFO] [1630570233.409803086, 1629829804.850763082]: Odom: ratio=0.836938, std dev=0.005718m|0.001808rad, update time=0.013644s
[ INFO] [1630570233.449052252, 1629829804.850763082]: Odom: ratio=0.845547, std dev=0.008236m|0.002604rad, update time=0.013834s
[ INFO] [1630570233.537438915, 1629829804.850763082]: Odom: ratio=0.836528, std dev=0.008828m|0.002792rad, update time=0.019863s
[ INFO] [1630570233.637143535, 1629829804.850763082]: Odom: ratio=0.833708, std dev=0.008222m|0.002600rad, update time=0.014642s
[ INFO] [1630570233.770131524, 1629829804.850763082]: Odom: ratio=0.841512, std dev=0.007842m|0.002480rad, update time=0.026171s
[ INFO] [1630570233.866934699, 1629829804.850763082]: Odom: ratio=0.847419, std dev=0.007552m|0.002388rad, update time=0.015904s
[ INFO] [1630570233.974825271, 1629829804.850763082]: Odom: ratio=0.840180, std dev=0.007928m|0.002507rad, update time=0.011775s
beignet-opencl-icd: no supported GPU found, this is probably the wrong opencl-icd package for this hardware
(If you have multiple ICDs installed and OpenCL works, you can ignore this message)
[ INFO] [1630570234.059638595, 1629829804.850763082]: Odom: ratio=0.833239, std dev=0.008410m|0.002659rad, update time=0.020498s
[ WARN] (2021-09-02 11:10:34.069) Memory.cpp:4223::createSignature() Initializing rectification maps for camera 0 (only done for the first image received)...
beignet-opencl-icd: no supported GPU found, this is probably the wrong opencl-icd package for this hardware
(If you have multiple ICDs installed and OpenCL works, you can ignore this message)
[ INFO] [1630570234.122715822, 1629829804.850763082]: Odom: ratio=0.843346, std dev=0.007070m|0.002236rad, update time=0.019018s
[ WARN] (2021-09-02 11:10:34.171) Memory.cpp:4225::createSignature() Initializing rectification maps for camera 0 (only done for the first image received)... done!
[ WARN] (2021-09-02 11:10:34.175) Memory.cpp:4223::createSignature() Initializing rectification maps for camera 1 (only done for the first image received)...
[ WARN] (2021-09-02 11:10:34.184) Memory.cpp:4225::createSignature() Initializing rectification maps for camera 1 (only done for the first image received)... done!
[ WARN] (2021-09-02 11:10:34.185) Memory.cpp:4223::createSignature() Initializing rectification maps for camera 2 (only done for the first image received)...
[ WARN] (2021-09-02 11:10:34.193) Memory.cpp:4225::createSignature() Initializing rectification maps for camera 2 (only done for the first image received)... done!
[ WARN] (2021-09-02 11:10:34.194) Memory.cpp:4223::createSignature() Initializing rectification maps for camera 3 (only done for the first image received)...
[ WARN] (2021-09-02 11:10:34.203) Memory.cpp:4225::createSignature() Initializing rectification maps for camera 3 (only done for the first image received)... done!
[ INFO] [1630570234.243331662, 1629829804.850763082]: Odom: ratio=0.855942, std dev=0.005289m|0.001672rad, update time=0.017438s
[ INFO] [1630570234.284673870, 1629829804.850763082]: Odom: ratio=0.830242, std dev=0.008273m|0.002616rad, update time=0.013766s
[ INFO] [1630570234.308315016, 1629829804.850763082]: rtabmap (1): Rate=0.00s, Limit=0.000s, Conversion=0.0047s, RTAB-Map=0.2292s, Maps update=0.0093s pub=0.0002s (local map=1, WM=1)
[ INFO] [1630570234.388091117, 1629829804.850763082]: Odom: ratio=0.855693, std dev=0.005471m|0.001730rad, update time=0.018222s
[ INFO] [1630570234.393628169, 1629829804.850763082]: rtabmap (2): Rate=0.00s, Limit=0.000s, Conversion=0.0021s, RTAB-Map=0.0789s, Maps update=0.0016s pub=0.0000s (local map=1, WM=1)
[ INFO] [1630570234.461723141, 1629829804.850763082]: Odom: ratio=0.837366, std dev=0.008453m|0.002673rad, update time=0.017704s
[ INFO] [1630570234.486379878, 1629829804.850763082]: rtabmap (3): Rate=0.00s, Limit=0.000s, Conversion=0.0019s, RTAB-Map=0.0873s, Maps update=0.0014s pub=0.0000s (local map=1, WM=1)
[ INFO] [1630570234.571137654, 1629829804.850763082]: rtabmap (4): Rate=0.00s, Limit=0.000s, Conversion=0.0033s, RTAB-Map=0.0769s, Maps update=0.0015s pub=0.0000s (local map=1, WM=1)
[ INFO] [1630570234.638850669, 1629829804.850763082]: Odom: ratio=0.857788, std dev=0.006186m|0.001956rad, update time=0.017014s
[ INFO] [1630570234.656285376, 1629829804.850763082]: rtabmap (5): Rate=0.00s, Limit=0.000s, Conversion=0.0022s, RTAB-Map=0.0791s, Maps update=0.0015s pub=0.0000s (local map=1, WM=1)
[ INFO] [1630570234.708512711, 1629829804.850763082]: Odom: ratio=0.833146, std dev=0.008494m|0.002686rad, update time=0.022176s
[ INFO] [1630570234.744209751, 1629829804.850763082]: rtabmap (6): Rate=0.00s, Limit=0.000s, Conversion=0.0027s, RTAB-Map=0.0809s, Maps update=0.0019s pub=0.0000s (local map=1, WM=1)
[ INFO] [1630570234.824585715, 1629829804.850763082]: rtabmap (7): Rate=0.00s, Limit=0.000s, Conversion=0.0015s, RTAB-Map=0.0759s, Maps update=0.0016s pub=0.0000s (local map=1, WM=1)
[ INFO] [1630570234.847605279, 1629829804.850763082]: Odom: ratio=0.863045, std dev=0.006005m|0.001899rad, update time=0.014721s
[ INFO] [1630570234.917815855, 1629829804.850763082]: Odom: ratio=0.837458, std dev=0.008762m|0.002771rad, update time=0.014627s
[ INFO] [1630570235.051295212, 1629829804.850763082]: Odom: ratio=0.859718, std dev=0.004743m|0.001500rad, update time=0.012098s
[ INFO] [1630570235.146625721, 1629829804.850763082]: Odom: ratio=0.835779, std dev=0.007285m|0.002304rad, update time=0.012475s
[ INFO] [1630570235.301368228, 1629829804.850763082]: Odom: ratio=0.834463, std dev=0.008471m|0.002679rad, update time=0.045941s
[ INFO] [1630570235.352021758, 1629829804.850763082]: Odom: ratio=0.834452, std dev=0.008562m|0.002707rad, update time=0.035442s
[ INFO] [1630570235.537794234, 1629829804.850763082]: Odom: ratio=0.840832, std dev=0.008360m|0.002644rad, update time=0.041472s
[ INFO] [1630570235.601066469, 1629829804.850763082]: Odom: ratio=0.843414, std dev=0.008277m|0.002617rad, update time=0.053597s
[ INFO] [1630570235.635360268, 1629829804.850763082]: Odom: ratio=0.832961, std dev=0.008578m|0.002713rad, update time=0.023475s
[ INFO] [1630570235.783174970, 1629829804.850763082]: Odom: ratio=0.830306, std dev=0.008905m|0.002816rad, update time=0.020224s
[ INFO] [1630570236.020323382, 1629829804.850763082]: Odom: ratio=0.835500, std dev=0.008567m|0.002709rad, update time=0.037034s

Edit: It turns out it was a performance issue, I played the bag at 0.1 publishing rate and rtab could keep up then. By the way how did you create that mesh??

matlabbe

Re: Painting a pointcloud.

Administrator

Hi,

Did you play the bag with --clock?

point_cloud_assembler is subscribing to /velodyne_points, assemble the scans for 1 second, then publish /assembled_cloud to rtabmap (so rtabmap should be updated at 1 Hz). In you previous message, there was a fatal error killing rtabmap node, which is solved by rebuilding rtabmap with RTABMAP_SYNC_MULTI_RGBD option enabled:

[FATAL] [1630480482.426505016]: Cannot synchronize more than 1 rgbd topic (rtabmap_ros has been built without RTABMAP_SYNC_MULTI_RGBD option)

Make sure you decompress the bag before replaying it locally.

cheers,
Mathieu

EDIT: For the mesh, it is created With Export/View 3D clouds menu options in rtabmapviz, or by reopening the database in rtabmap-databaseViewer. In export dialog, there are options to generate a mesh.