Implementing an Ouster-based 3D Lidar via rtabmap_ros

classic Classic list List threaded Threaded
4 messages Options
Reply | Threaded
Open this post in threaded view
|

Implementing an Ouster-based 3D Lidar via rtabmap_ros

slessard
I would like to integrate my 3D lidar (an Ouster-16) into rtabmap via ROS similar to how it was done in this post. However, my current rtabmap launch file (below) does not work as intended:
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
<launch>
  <arg name="gui_cfg"             	default="~/.ros/rtabmap_gui.ini" />
  <arg name="launch_prefix"       	default=""/>  
  <arg name="output"              	default="screen"/>

  <group ns="rtabmap">
     <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      	<param name="frame_id" type="string" value="base_link"/>
      	<param name="subscribe_depth" type="bool" value="true"/>
      	<param name="subscribe_scan" type="bool" value="true"/>
      	<param name="queue_size" type="int" value="10"/>

      	<remap from="odom" to="/odom"/>
      	<remap from="scan" to="/os1_cloud_node/points"/>
        <remap from="rgb/image" to="/camera/color/image_raw"/>
        <remap from="depth/image" to="/camera/depth/image_rect_raw"/>
        <remap from="rgb/camera_info" to="/camera/color/camera_info"/>

      	<!-- RTAB-Map's parameters -->
      	<param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
      	<param name="RGBD/ProximityBySpace" 	type="string" value="true"/>
      	<param name="RGBD/AngularUpdate"    	type="string" value="0.01"/>
      	<param name="RGBD/LinearUpdate"     	type="string" value="0.01"/>
      	<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
      	<param name="Reg/Force3DoF"      	type="string" value="true"/> <!--same as optimizer/slam2D -->
      	<param name="Reg/Strategy"          	type="string" value="1"/> <!-- 1=ICP -->
      	<param name="Reg/Force3DoF"         	type="string" value="true"/>
      	<param name="Vis/MinInliers"        	type="string" value="5"/>
      	<param name="Vis/InlierDistance"    	type="string" value="0.1"/>
      	<param name="Rtabmap/TimeThr"       	type="string" value="700"/>
      	<param name="Mem/RehearsalSimilarity"   type="string" value="0.45"/>
      	<param name="Grid/CellSize" type="string" value="0.05"/> <!-- 5cm voxel default-->
      	<param name="Kp/MaxFeatures" type="string" value="400"/>
        <param name="Optimizer/Strategy" type="string" value="0"/> <!-- 0 is TORO, Default G20-->
        <param name="RGBD/OptimizeMaxError" type="string" value="0"/> <!--When using TORO, seto to 0,otherwise set to 1-->
        <param name="Kp/DetectorStrategy" type="string" value="6"/> <!--0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF(default) 7=BRISK 8=GFTT/ORB 9=KAZE-->
        <param name="RGBD/LoopClosureReextractFeatures" type="string" value="true"/> <!--extract features even if there are some already in the node-->
        <param name="Grid/FromDepth" type="string" value="true"/> <!--suppress warning (subscribe_scan=true)-->
        <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="10"/> <!--suppress warning (subscribe_scan = true and Reg/Strategy=1-->
        <param name="Vis/FeatureType" type="string" value="6"/> <!--suppress warning,want to be same as KP/detectorstrategy -->
      </node>

      <!-- Visualisation RTAB-Map -->
      <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">

  	<param name="subscribe_rgbd"   	type="bool"   value="false"/>
  	<param name="subscribe_scan"   	type="bool"   value="false"/>
  	<param name="subscribe_scan_cloud" type="bool"   value="true"/>
        <param name="scan_cloud_max_points" value="130000"/> <!-- number from Velodyne HDL-64E -->
  	<param name="frame_id"         	type="string" value="base_link"/>
  	<param name="odom_frame_id"    	type="string" value=""/>
  	<param name="wait_for_transform_duration" type="double"   value="0.2"/>
  	<param name="queue_size"       	type="int"	value="10"/>
  	<param name="approx_sync"      	type="bool"   value="true"/>
        <param name="scan_cloud_topic" value="/os1_cloud_node/points"/>

       <remap from="rgb/image" to="/camera/color/image_raw"/>
       <remap from="depth/image" to="/camera/depth/image_rect_raw"/>
       <remap from="rgb/camera_info" to="/camera/color/camera_info"/>
  	<remap from="scan"               	to="/os1_cloud_node/points"/>
  	<remap from="odom"               	to="/odom"/>
      </node>

  <node pkg="tf" type="static_transform_publisher"  name="base_to_laser"
      args="0.1 0.0 0.0 0 0 0 /base_link /laser 100" />

  <node pkg="tf" type="static_transform_publisher"  name="base_to_cam"
  	args="0.18 0.0 0.0 0 0 0 /base_link /camera_link 100" />
  </group>
</launch>
I am using this lidar in conjunction with an Intel D435i realsense camera and odometry native to my base robot. Am I incorrectly publishing point cloud data? Thank you very much, Steve
Reply | Threaded
Open this post in threaded view
|

Re: Implementing an Ouster-based 3D Lidar via rtabmap_ros

matlabbe
Administrator
Hi,

set subscribe_scan_cloud instead of subscribe_scan for rtabmap node. Also remap scan_cloud instead of scan to point cloud from os1 (same for rtabmapviz):
<param name="subscribe_scan_cloud" type="bool"   value="true"/>
<remap from="scan_cloud" to="/os1_cloud_node/points"/>

Based on this tutorial with D435, the rgb and depth images to subscribe would be more like:
<remap from="rgb/image" to="/camera/color/image_raw"/>
<remap from="depth/image" to="/camera/aligned_depth_to_color/image_raw"/>
<remap from="rgb/camera_info" to="/camera/color/camera_info"/>

RGBD/LoopClosureReextractFeatures is deprecated, it can be false in recent versions. Set Grid/FromDepth to false if you want the occupancy grid map created from os1 instead of the camera. For 360 3D lidar, set RGBD/ProximityPathMaxNeighbors to 1 to save computation time. Increase Vis/MinInliers >= 15 to reject most bad loop closures. Vis/InlierDistance is not needed as Vis/EstimationType is 1 by default (PnP). As you are using a lidar, there would be some "Icp/*****" parameters to tune.

As you are synchronizing a lidar with camera topics, I also suggest to use rtabmap_ros/rgdb_sync nodelet to pre-sync the image topics before rtabmap node (see figure 6 of this paper):

<launch>
  <arg name="gui_cfg"          default="~/.ros/rtabmap_gui.ini" />
  <arg name="launch_prefix"    default=""/>  
  <arg name="output"           default="screen"/>

  <arg name="rgb_topic"         default="/camera/color/image_raw"/>
  <arg name="depth_topic"       default="/camera/aligned_depth_to_color/image_raw"/>
  <arg name="camera_info_topic" default="/camera/color/camera_info"/>

  <group ns="rtabmap">

     <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">
          <remap from="rgb/image"       to="$(arg rgb_topic)"/>
          <remap from="depth/image"     to="$(arg depth_topic)"/>
          <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
          <remap from="rgbd_image"      to="rgbd_image"/> <!-- output -->
          <param name="approx_sync"     type="bool"   value="false"/> <!-- false for realsense -->
     </node>

     <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      	<param name="frame_id" type="string" value="base_link"/>
      	<param name="subscribe_depth" type="bool" value="false"/>
        <param name="subscribe_rgbd" type="bool" value="true"/>
      	<param name="subscribe_scan_cloud" type="bool" value="true"/>
      	<param name="queue_size" type="int" value="10"/>
        <param name="scan_cloud_max_points" value="32768"/> <!-- based on os-1 spec: 327,680 pts/second for 10 Hz -->

      	<remap from="odom" to="/odom"/>
      	<remap from="scan_cloud" to="/os1_cloud_node/points"/>
        <remap from="rgbd_image" to="rgbd_image"/>

      	<!-- RTAB-Map's parameters -->
      	<param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
      	<param name="RGBD/ProximityBySpace" 	type="string" value="true"/>
      	<param name="RGBD/AngularUpdate"    	type="string" value="0.01"/>
      	<param name="RGBD/LinearUpdate"     	type="string" value="0.01"/>
      	<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
      	<param name="Reg/Strategy"          	type="string" value="1"/> <!-- 1=ICP -->
      	<param name="Reg/Force3DoF"         	type="string" value="true"/>
      	<param name="Vis/MinInliers"        	type="string" value="15"/>
      	<param name="Rtabmap/TimeThr"       	type="string" value="0"/> <!-- set to 0 to disable memory management -->
      	<param name="Mem/RehearsalSimilarity"   type="string" value="0.45"/>
      	<param name="Grid/CellSize" type="string" value="0.05"/> <!-- 5cm voxel default-->
      	<param name="Kp/MaxFeatures" type="string" value="400"/>
        <param name="Optimizer/Strategy" type="string" value="0"/> <!-- 0 is TORO, Default G20-->
        <param name="RGBD/OptimizeMaxError" type="string" value="0"/> <!--When using TORO, seto to 0,otherwise set to 1-->
        <param name="Kp/DetectorStrategy" type="string" value="6"/> <!--0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF(default) 7=BRISK 8=GFTT/ORB 9=KAZE-->
        <param name="Grid/FromDepth" type="string" value="true"/> <!--suppress warning (subscribe_scan=true)-->
        <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="1"/> <!--suppress warning (subscribe_scan = true and Reg/Strategy=1-->
        <param name="Vis/FeatureType" type="string" value="6"/> <!--suppress warning,want to be same as KP/detectorstrategy -->

        <param name="Icp/VoxelSize"                  type="string" value="0.1"/>
        <param name="Icp/MaxCorrespondenceDistance"  type="string" value="0.15"/>
        <param name="Icp/Iterations"                 type="string" value="10"/>
        <param name="Icp/Epsilon"                    type="string" value="0.001"/>
      </node>

      <!-- Visualisation RTAB-Map -->
      <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">

  	<param name="subscribe_rgbd"   	type="bool"   value="true"/>
  	<param name="subscribe_scan_cloud" type="bool"   value="true"/>
  	<param name="frame_id"         	type="string" value="base_link"/>
  	<param name="odom_frame_id"    	type="string" value=""/>
  	<param name="wait_for_transform_duration" type="double"   value="0.2"/>
  	<param name="queue_size"       	type="int"	value="10"/>
  	<param name="approx_sync"      	type="bool"   value="true"/>

        <remap from="rgbd_image"   to="rgbd_image"/>
        <remap from="scan_cloud"  to="/os1_cloud_node/points"/>
        <remap from="odom"        to="/odom"/>
      </node>

  <node pkg="tf" type="static_transform_publisher"  name="base_to_laser"
      args="0.1 0.0 0.0 0 0 0 /base_link /laser 100" />

  <node pkg="tf" type="static_transform_publisher"  name="base_to_cam"
  	args="0.18 0.0 0.0 0 0 0 /base_link /camera_link 100" />
  </group>
</launch>

cheers,
Mathieu
Reply | Threaded
Open this post in threaded view
|

Re: Implementing an Ouster-based 3D Lidar via rtabmap_ros

slessard
Hi Mathieu,

Thank you for the response.

How would I remove any mention of 2D Lidar? My application only has a realsense and the Ouster Lidar (and on-board imu/odom). I think the inclusion of 2D Lidar references is creating a mixup in the launch file.

Also, do I need to modify my tf for the base_link_laser to a different keyword if my Lidar is 3D, not 2D?

Thanks!
Reply | Threaded
Open this post in threaded view
|

Re: Implementing an Ouster-based 3D Lidar via rtabmap_ros

matlabbe
Administrator
Hi,

In the example in my previous post, there is no reference to 2D lidar. Not sure what you are referring.

For 2D lidar, we use:
<param name="subscribe_scan" value="true"/>
<remap from="scan" to="/scan"/> <!-- sensor_msgs/LaserScan, assuming horizontal orientation -->
For 3D lidar, we use:
<param name="subscribe_scan_cloud" value="true"/>
<remap from="scan_cloud" to="/os1_cloud_node/points"/> <!-- sensor_msgs/PointCloud2 -->

For the last question, the TF name used for the lidar frame can be anything, there is no difference between 2d or 3d there.

cheers,
Mathieu