PointCloudAssembler: added noise filtering options. Added test_ouster_gen2.launch example

launch/rtabmap.launch

@@ -110,6 +110,8 @@
   <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"/>
@@ -281,6 +283,8 @@
       <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) -->

launch/tests/test_ouster_gen2.launch

@@ -0,0 +1,136 @@
+
+<launch>
+  
+    <!-- 
+    Hand-held 3D lidar mapping example using only a Ouster GEN2 (no camera).
+    Prerequisities: rtabmap should be built with libpointmatcher
+    Example:
+     $ roslaunch rtabmap_ros test_ouster_gen2.launch sensor_hostname:=os-XXXXXXXXXXXX.local udp_dest:=192.168.1.XXX
+     $ rosrun rviz rviz -f map
+     $ Show TF and /rtabmap/cloud_map topics
+    ISSUE: You may have to reset odometry after receiving the first cloud if the map looks tilted. The problem seems 
+           coming from the first cloud sent by os_cloud_node, which may be poorly synchronized with IMU data.
+    -->
+
+    <!-- Required: -->
+    <arg name="sensor_hostname"/>
+    <arg name="udp_dest"/>
+
+    <arg name="frame_id" default="os_sensor"/>
+    <arg name="rtabmapviz"    default="true"/>
+    <arg name="scan_20_hz"    default="true"/>
+    <arg name="use_sim_time"  default="false"/>
+    <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
+    
+    <!-- Ouster -->
+    <remap unless="$(arg use_sim_time)" from="/os_cloud_node/imu" to="/os_cloud_node/imu/data_raw"/>
+    <include unless="$(arg use_sim_time)" file="$(find ouster_ros)/ouster.launch">
+      <arg name="sensor_hostname" value="$(arg sensor_hostname)"/>
+      <arg name="udp_dest" value="$(arg udp_dest)"/>
+      <arg name="image" value="true"/>
+      <arg     if="$(arg scan_20_hz)" name="lidar_mode" value="1024x20"/>
+      <arg unless="$(arg scan_20_hz)" name="lidar_mode" value="1024x10"/>
+    </include>
+
+    <!-- IMU orientation estimation and publish tf accordingly to os_sensor frame -->
+    <node pkg="nodelet" type="nodelet" name="imu_nodelet_manager" args="manager">
+      <remap from="imu" to="/os_cloud_node/imu"/>
+    </node>
+    <node pkg="nodelet" type="nodelet" name="imu_filter" args="load imu_filter_madgwick/ImuFilterNodelet imu_nodelet_manager">
+      <param name="use_mag" value="false"/>
+      <param name="world_frame" value="enu"/>
+      <param name="publish_tf" value="false"/>
+    </node> 
+    <node pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_ros/imu_to_tf imu_nodelet_manager">
+      <remap from="imu/data" to="/os_cloud_node/imu/data"/>
+      <param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
+      <param name="base_frame_id" value="$(arg frame_id)"/>
+    </node> 
+
+    <group ns="rtabmap">
+      <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
+        <remap from="scan_cloud" to="/os_cloud_node/points"/>
+        <param name="frame_id"        type="string" value="$(arg frame_id)"/>  
+        <param name="odom_frame_id"   type="string" value="odom"/>
+        <param     if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
+        <param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
+
+        <remap from="imu" to="/os_cloud_node/imu/data"/>
+        <param name="guess_frame_id"   type="string" value="$(arg frame_id)_stabilized"/>
+        <param name="wait_imu_to_init" type="bool" value="true"/>
+     
+        <!-- ICP parameters -->
+        <param name="Icp/PointToPlane"        type="string" value="true"/>
+        <param name="Icp/Iterations"          type="string" value="10"/>
+        <param name="Icp/VoxelSize"           type="string" value="0.2"/>
+        <param name="Icp/DownsamplingStep"    type="string" value="1"/> <!-- cannot be increased with ring-like lidar -->
+        <param name="Icp/Epsilon"             type="string" value="0.001"/>
+        <param name="Icp/PointToPlaneK"       type="string" value="20"/>
+        <param name="Icp/PointToPlaneRadius"  type="string" value="0"/>
+        <param name="Icp/MaxTranslation"      type="string" value="2"/>
+        <param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
+        <param name="Icp/PM"                  type="string" value="true"/> 
+        <param name="Icp/PMOutlierRatio"      type="string" value="0.1"/>
+        <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>  
+
+        <!-- Odom parameters -->       
+        <param name="Odom/ScanKeyFrameThr"       type="string" value="0.95"/>
+        <param name="Odom/Strategy"              type="string" value="0"/>
+        <param name="OdomF2M/ScanSubtractRadius" type="string" value="0.2"/>
+        <param name="OdomF2M/ScanMaxSize"        type="string" value="15000"/>      
+      </node>
+
+      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">	  
+        <param name="frame_id"             type="string" value="$(arg frame_id)"/>  
+        <param name="subscribe_depth"      type="bool" value="false"/>
+        <param name="subscribe_rgb"        type="bool" value="false"/>
+        <param name="subscribe_scan_cloud" type="bool" value="true"/>
+        <param name="approx_sync"          type="bool" value="false"/>
+        
+        <remap from="scan_cloud" to="/os_cloud_node/points"/>
+     
+        <!-- RTAB-Map's parameters -->
+        <param name="Rtabmap/DetectionRate"          type="string" value="1"/>  
+        <param name="RGBD/NeighborLinkRefining"      type="string" value="false"/>
+        <param name="RGBD/ProximityBySpace"          type="string" value="true"/>
+        <param name="RGBD/ProximityMaxGraphDepth"    type="string" value="0"/>
+        <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="1"/>
+        <param name="RGBD/AngularUpdate"             type="string" value="0.05"/>
+        <param name="RGBD/LinearUpdate"              type="string" value="0.05"/>
+        <param name="Mem/NotLinkedNodesKept"         type="string" value="false"/>
+        <param name="Mem/STMSize"                    type="string" value="30"/>
+        <!-- param name="Mem/LaserScanVoxelSize"     type="string" value="0.1"/ -->
+        <!-- param name="Mem/LaserScanNormalK"       type="string" value="10"/ -->
+        <!-- param name="Mem/LaserScanRadius"        type="string" value="0"/ -->
+        
+        <param name="Reg/Strategy"                   type="string" value="1"/> 
+        <param name="Grid/CellSize"                  type="string" value="0.1"/>
+        <param name="Grid/RangeMax"                  type="string" value="20"/>
+        <param name="Grid/ClusterRadius"             type="string" value="1"/>
+        <param name="Grid/GroundIsObstacle"          type="string" value="true"/>
+
+        <!-- ICP parameters -->
+        <param name="Icp/VoxelSize"                  type="string" value="0.3"/>
+        <param name="Icp/PointToPlaneK"              type="string" value="20"/>
+        <param name="Icp/PointToPlaneRadius"         type="string" value="0"/>
+        <param name="Icp/PointToPlane"               type="string" value="false"/>
+        <param name="Icp/Iterations"                 type="string" value="10"/>
+        <param name="Icp/Epsilon"                    type="string" value="0.001"/>
+        <param name="Icp/MaxTranslation"             type="string" value="3"/>
+        <param name="Icp/MaxCorrespondenceDistance"  type="string" value="1"/>
+        <param name="Icp/PM"                         type="string" value="true"/> 
+        <param name="Icp/PMOutlierRatio"             type="string" value="0.7"/>
+        <param name="Icp/CorrespondenceRatio"        type="string" value="0.4"/>
+      </node>
+
+      <node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
+        <param name="frame_id" type="string" value="$(arg frame_id)"/>
+        <param name="odom_frame_id" type="string" value="odom"/>
+        <param name="subscribe_odom_info" type="bool" value="true"/>
+        <param name="subscribe_scan_cloud" type="bool" value="true"/>
+        <param name="approx_sync" type="bool" value="false"/>
+        <remap from="scan_cloud" to="/os_cloud_node/points"/>
+      </node>
+  </group>
+
+</launch>

src/nodelets/point_cloud_assembler.cpp

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl_conversions/pcl_conversions.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/radius_outlier_removal.h>
 
 #include <pcl_ros/transforms.h>
 
@@ -78,6 +79,8 @@ public:
 		rangeMin_(0),
 		rangeMax_(0),
 		voxelSize_(0),
+		noiseRadius_(0),
+		noiseMinNeighbors_(5),
 		fixedFrameId_("odom")
 	{}
 
@@ -113,6 +116,8 @@ private:
 		pnh.param("range_min", rangeMin_, rangeMin_);
 		pnh.param("range_max", rangeMax_, rangeMax_);
 		pnh.param("voxel_size", voxelSize_, voxelSize_);
+		pnh.param("noise_radius", noiseRadius_, noiseRadius_);
+		pnh.param("noise_min_neighbors", noiseMinNeighbors_, noiseMinNeighbors_);
 		pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
 		ROS_ASSERT(maxClouds_>0 || assemblingTime_ >0.0);
 
@@ -126,6 +131,8 @@ private:
 		ROS_INFO("%s: range_min=%f", getName().c_str(), rangeMin_);
 		ROS_INFO("%s: range_max=%f", getName().c_str(), rangeMax_);
 		ROS_INFO("%s: voxel_size=%fm", getName().c_str(), voxelSize_);
+		ROS_INFO("%s: noise_radius=%fm", getName().c_str(), noiseRadius_);
+		ROS_INFO("%s: noise_min_neighbors=%d", getName().c_str(), noiseMinNeighbors_);
 
 		cloudsSkipped_ = skipClouds_;
 
@@ -303,6 +310,16 @@ private:
 						filter.filter(*output);
 						assembled = output;
 					}
+					if(noiseRadius_>0.0 && noiseMinNeighbors_>0)
+					{
+						pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2> filter;
+						filter.setRadiusSearch(noiseRadius_);
+						filter.setMinNeighborsInRadius(noiseMinNeighbors_);
+						filter.setInputCloud(assembled);
+						pcl::PCLPointCloud2Ptr output(new pcl::PCLPointCloud2);
+						filter.filter(*output);
+						assembled = output;
+					}
 
 					pcl_conversions::moveFromPCL(*assembled, rosCloud);
 					pcl_ros::transformPointCloud(t.toEigen4f().inverse(), rosCloud, rosCloud);
@@ -370,6 +387,8 @@ private:
 	double rangeMin_;
 	double rangeMax_;
 	double voxelSize_;
+	double noiseRadius_;
+	int noiseMinNeighbors_;
 	std::string fixedFrameId_;
 	tf::TransformListener tfListener_;