point_cloud_assembler: use rtabmap voxel filter (at cost of having to convert 2 times the assembled cloud) if there is an integer overflow

launch/rtabmap.launch

@@ -119,9 +119,12 @@
   <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_time"         default="1"/>      <!-- max_clouds and time should not be set at the same time -->
+  <arg name="scan_cloud_assembling_max_clouds"   default="0"/>      <!-- max_clouds and time should not be set at the same time -->
   <arg name="scan_cloud_assembling_fixed_frame"  default=""/>
   <arg name="scan_cloud_assembling_voxel_size"   default="0.05"/>
+  <arg name="scan_cloud_assembling_range_min"    default="0.0"/>    <!-- 0=disabled -->
+  <arg name="scan_cloud_assembling_range_max"    default="0.0"/>    <!-- 0=disabled -->
   <arg name="scan_cloud_assembling_noise_radius"   default="0.0"/>    <!-- 0=disabled -->
   <arg name="scan_cloud_assembling_noise_min_neighbors"   default="5"/>
   
@@ -289,14 +292,17 @@
       <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">
+    <node if="$(arg scan_cloud_assembling)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" output="$(arg output)">
       <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="max_clouds"      type="int"    value="$(arg scan_cloud_assembling_max_clouds)"/>
       <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="range_min"       type="double" value="$(arg scan_cloud_assembling_range_min)"/>
+      <param name="range_max"       type="double" value="$(arg scan_cloud_assembling_range_max)"/>
       <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>
@@ -331,7 +337,7 @@
       <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 if="$(eval not scan_cloud_filtered and not scan_cloud_assembling)" 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)"/> 
       <param name="gen_depth"                  type="bool"   value="$(arg gen_depth)" />

src/nodelets/icp_odometry.cpp

@@ -535,7 +535,7 @@ private:
 					"cloud is not dense, for convenience it will be set to %d (%dx%d)",
 					scanCloudMaxPoints_, cloudMsg.width, cloudMsg.height);
 		}
-		else if(cloudMsg.height > 1 && scanCloudMaxPoints_ != cloudMsg.height * cloudMsg.width)
+		else if(cloudMsg.height > 1 && scanCloudMaxPoints_ < cloudMsg.height * cloudMsg.width)
 		{
 			NODELET_WARN("IcpOdometry: \"scan_cloud_max_points\" is set to %d but input "
 					"cloud is not dense and has a size of %d (%dx%d), setting to this later size.",

src/nodelets/point_cloud_assembler.cpp

@@ -51,6 +51,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_ros/OdomInfo.h>
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/Version.h>
 
 namespace rtabmap_ros
 {
@@ -391,12 +392,52 @@ private:
 					sensor_msgs::PointCloud2 rosCloud;
 					if(voxelSize_>0.0)
 					{
-						pcl::VoxelGrid<pcl::PCLPointCloud2> filter;
+						// estimate if there would be an overflow
-						filter.setLeafSize(voxelSize_, voxelSize_, voxelSize_);
+						int x_idx=-1, y_idx=-1, z_idx=-1;
-						filter.setInputCloud(assembled);
+						for (std::size_t d = 0; d < assembled->fields.size (); ++d)
-						pcl::PCLPointCloud2Ptr output(new pcl::PCLPointCloud2);
+						{
-						filter.filter(*output);
+							if (assembled->fields[d].name.compare("x")==0)
-						assembled = output;
+								x_idx = d;
+							if (assembled->fields[d].name.compare("y")==0)
+								y_idx = d;
+							if (assembled->fields[d].name.compare("z")==0)
+								z_idx = d;
+						}
+						bool overflow = false;
+						if(x_idx>=0 && y_idx>=0 && z_idx>=0) {
+							Eigen::Vector4f min_p, max_p;
+							pcl::getMinMax3D(assembled, x_idx, y_idx, z_idx, min_p, max_p);
+							float inverseVoxelSize = 1.0f/voxelSize_;
+							std::int64_t dx = static_cast<std::int64_t>((max_p[0] - min_p[0]) * inverseVoxelSize)+1;
+							std::int64_t dy = static_cast<std::int64_t>((max_p[1] - min_p[1]) * inverseVoxelSize)+1;
+							std::int64_t dz = static_cast<std::int64_t>((max_p[2] - min_p[2]) * inverseVoxelSize)+1;
+
+							if ((dx*dy*dz) > static_cast<std::int64_t>(std::numeric_limits<std::int32_t>::max()))
+							{
+								overflow = true;
+							}
+						}
+						if(overflow)
+						{
+							rtabmap::LaserScan scan = rtabmap::util3d::laserScanFromPointCloud(*assembled);
+							scan = rtabmap::util3d::commonFiltering(scan, 1, 0, 0, voxelSize_);
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+							std::uint64_t stamp = assembled->header.stamp;
+#else
+							pcl::uint64_t stamp = assembled->header.stamp;
+#endif
+							assembled = rtabmap::util3d::laserScanToPointCloud2(scan);
+							assembled->header.stamp = stamp;
+						}
+						else
+						{
+							pcl::VoxelGrid<pcl::PCLPointCloud2> filter;
+							filter.setLeafSize(voxelSize_, voxelSize_, voxelSize_);
+							filter.setInputCloud(assembled);
+							pcl::PCLPointCloud2Ptr output(new pcl::PCLPointCloud2);
+							filter.filter(*output);
+							assembled = output;
+						}
 					}
 					if(noiseRadius_>0.0 && noiseMinNeighbors_>0)
 					{