0.11.4: updated with API changes

CMakeLists.txt

@@ -17,7 +17,7 @@ find_package(octomap_ros)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-find_package(RTABMap 0.11.0 REQUIRED)
+find_package(RTABMap 0.11.4 REQUIRED)
 
 find_package(OpenCV REQUIRED)
 

src/CoreWrapper.cpp

@@ -89,6 +89,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
 		useActionForGoal_(false),
 		genScan_(false),
 		genScanMaxDepth_(4.0),
+		genScanMinDepth_(0.0),
 		mapToOdom_(rtabmap::Transform::getIdentity()),
 		mapsManager_(true),
 		depthSync_(0),
@@ -170,6 +171,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
 	pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_);
 	pnh.param("gen_scan",            genScan_, genScan_);
 	pnh.param("gen_scan_max_depth",  genScanMaxDepth_, genScanMaxDepth_);
+	pnh.param("gen_scan_min_depth",  genScanMinDepth_, genScanMinDepth_);
 
 	if(!tfPrefix.empty())
 	{
@@ -900,6 +902,7 @@ void CoreWrapper::commonDepthCallback(
 					cameraModels.back().cx(),
 					cameraModels.back().cy(),
 					genScanMaxDepth_,
+					genScanMinDepth_,
 					localTransform);
 			genMaxScanPts += subDepth.cols;
 		}

src/CoreWrapper.h

@@ -275,6 +275,7 @@ private:
 	bool useActionForGoal_;
 	bool genScan_;
 	double genScanMaxDepth_;
+	double genScanMinDepth_;
 
 	rtabmap::Transform mapToOdom_;
 	boost::mutex mapToOdomMutex_;

src/MapsManager.cpp

@@ -32,6 +32,7 @@ using namespace rtabmap;
 MapsManager::MapsManager(bool usePublicNamespace) :
 		cloudDecimation_(4),
 		cloudMaxDepth_(4.0), // meters
+		cloudMinDepth_(0.0), // meters
 		cloudVoxelSize_(0.05), // meters
 		cloudFloorCullingHeight_(0.0),
 		cloudCeilingCullingHeight_(0.0),
@@ -62,6 +63,7 @@ MapsManager::MapsManager(bool usePublicNamespace) :
 	// cloud map stuff
 	pnh.param("cloud_decimation", cloudDecimation_, cloudDecimation_);
 	pnh.param("cloud_max_depth", cloudMaxDepth_, cloudMaxDepth_);
+	pnh.param("cloud_min_depth", cloudMinDepth_, cloudMinDepth_);
 	pnh.param("cloud_voxel_size", cloudVoxelSize_, cloudVoxelSize_);
 	pnh.param("cloud_floor_culling_height", cloudFloorCullingHeight_, cloudFloorCullingHeight_);
 	pnh.param("cloud_ceiling_culling_height", cloudCeilingCullingHeight_, cloudCeilingCullingHeight_);
@@ -268,11 +270,17 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 						{
 							if(!image.empty() && !depth.empty())
 							{
+								pcl::IndicesPtr validIndices(new std::vector<int>);
 								cloudRGB = util3d::cloudRGBFromSensorData(
 										data,
 										cloudDecimation_,
 										cloudMaxDepth_,
-										cloudVoxelSize_);
+										cloudMinDepth_,
+										validIndices.get());
+								if(cloudVoxelSize_)
+								{
+									cloudRGB = util3d::voxelize(cloudRGB, validIndices, cloudVoxelSize_);
+								}
 								if(cloudRGB->size() && cloudNoiseFilteringRadius_ > 0.0 && cloudNoiseFilteringMinNeighbors_ > 0)
 								{
 									pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloudRGB, cloudNoiseFilteringRadius_, cloudNoiseFilteringMinNeighbors_);
@@ -290,11 +298,17 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 						{
 							if(	!depth.empty())
 							{
+								pcl::IndicesPtr validIndices(new std::vector<int>);
 								cloudXYZ = util3d::cloudFromSensorData(
 										data,
 										cloudDecimation_,
 										cloudMaxDepth_,
-										gridCellSize_); // use gridCellSize since this cloud is only for the projection map
+										cloudMinDepth_,
+										validIndices.get()); // use gridCellSize since this cloud is only for the projection map
+								if(gridCellSize_)
+								{
+									cloudXYZ = util3d::voxelize(cloudXYZ, validIndices, gridCellSize_);
+								}
 								if(cloudXYZ->size() && cloudNoiseFilteringRadius_ > 0.0 && cloudNoiseFilteringMinNeighbors_ > 0)
 								{
 									pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloudXYZ, cloudNoiseFilteringRadius_, cloudNoiseFilteringMinNeighbors_);

src/MapsManager.h

@@ -68,6 +68,7 @@ private:
 	// mapping stuff
 	int cloudDecimation_;
 	double cloudMaxDepth_;
+	double cloudMinDepth_;
 	double cloudVoxelSize_;
 	double cloudFloorCullingHeight_;
 	double cloudCeilingCullingHeight_;

src/rviz/MapCloudDisplay.cpp

@@ -143,6 +143,12 @@ MapCloudDisplay::MapCloudDisplay()
 	cloud_max_depth_->setMin( 0.0f );
 	cloud_max_depth_->setMax( 999.0f );
 
+	cloud_min_depth_ = new rviz::FloatProperty( "Cloud min depth (m)", 0.0f,
+											 "Minimum depth of the generated clouds.",
+											 this, SLOT( updateCloudParameters() ), this );
+	cloud_min_depth_->setMin( 0.0f );
+	cloud_min_depth_->setMax( 999.0f );
+
 	cloud_voxel_size_ = new rviz::FloatProperty( "Cloud voxel size (m)", 0.01f,
 										 "Voxel size of the generated clouds.",
 										 this, SLOT( updateCloudParameters() ), this );
@@ -282,11 +288,18 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
 				if(!s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty())
 				{
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+					pcl::IndicesPtr validIndices(new std::vector<int>);
 					cloud = rtabmap::util3d::cloudRGBFromSensorData(
 							s.sensorData(),
 							cloud_decimation_->getInt(),
 							cloud_max_depth_->getFloat(),
-							cloud_voxel_size_->getFloat());
+							cloud_min_depth_->getFloat(),
+							validIndices.get());
+
+					if(cloud_voxel_size_->getFloat())
+					{
+						cloud = rtabmap::util3d::voxelize(cloud, validIndices, cloud_voxel_size_->getFloat());
+					}
 
 					if(cloud->size())
 					{

src/rviz/MapCloudDisplay.h

@@ -106,6 +106,7 @@ public:
 	rviz::EnumProperty* style_property_;
 	rviz::IntProperty* cloud_decimation_;
 	rviz::FloatProperty* cloud_max_depth_;
+	rviz::FloatProperty* cloud_min_depth_;
 	rviz::FloatProperty* cloud_voxel_size_;
 	rviz::FloatProperty* cloud_filter_floor_height_;
 	rviz::FloatProperty* cloud_filter_ceiling_height_;