RGBD/NeighborLinkRefining: do one proximity detection by time if intermediate nodes are added. Optimized radiusFiltering(). Updated default parameters of proportionalRadiusFiltering().

corelib/include/rtabmap/core/util3d_filtering.h

@@ -466,38 +466,38 @@ pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const std::vector<int> & viewpointIndices,
 		const std::map<int, Transform> & viewpoints,
-		float factor,
+		float factor=0.01f,
-		float neighborScale=1.0f);
+		float neighborScale=2.0f);
 pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
 		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
 		const std::vector<int> & viewpointIndices,
 		const std::map<int, Transform> & viewpoints,
-		float factor,
+		float factor=0.01f,
-		float neighborScale=1.0f);
+		float neighborScale=2.0f);
 pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const std::vector<int> & viewpointIndices,
 		const std::map<int, Transform> & viewpoints,
-		float factor,
+		float factor=0.01f,
-		float neighborScale=1.0f);
+		float neighborScale=2.0f);
 pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const std::vector<int> & viewpointIndices,
 		const std::map<int, Transform> & viewpoints,
-		float factor,
+		float factor=0.01f,
-		float neighborScale=1.0f);
+		float neighborScale=2.0f);
 pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
 		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
 		const std::vector<int> & viewpointIndices,
 		const std::map<int, Transform> & viewpoints,
-		float factor,
+		float factor=0.01f,
-		float neighborScale=1.0f);
+		float neighborScale=2.0f);
 pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
 		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
 		const std::vector<int> & viewpointIndices,
 		const std::map<int, Transform> & viewpoints,
-		float factor,
+		float factor=0.01f,
-		float neighborScale=1.0f);
+		float neighborScale=2.0f);
 
 /**
  * @brief Filter points based on distance from their viewpoint.
@@ -516,43 +516,43 @@ pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
 		const pcl::IndicesPtr & indices,
 		const std::vector<int> & viewpointIndices,
 		const std::map<int, Transform> & viewpoints,
-		float factor,
+		float factor=0.01f,
-		float neighborScale=1.0f);
+		float neighborScale=2.0f);
 pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
 		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		const std::vector<int> & viewpointIndices,
 		const std::map<int, Transform> & viewpoints,
-		float factor,
+		float factor=0.01f,
-		float neighborScale=1.0f);
+		float neighborScale=2.0f);
 pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		const std::vector<int> & viewpointIndices,
 		const std::map<int, Transform> & viewpoints,
-		float factor,
+		float factor=0.01f,
-		float neighborScale=1.0f);
+		float neighborScale=2.0f);
 pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		const std::vector<int> & viewpointIndices,
 		const std::map<int, Transform> & viewpoints,
-		float factor,
+		float factor=0.01f,
-		float neighborScale=1.0f);
+		float neighborScale=2.0f);
 pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
 		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		const std::vector<int> & viewpointIndices,
 		const std::map<int, Transform> & viewpoints,
-		float factor,
+		float factor=0.01f,
-		float neighborScale=1.0f);
+		float neighborScale=2.0f);
 pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
 		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		const std::vector<int> & viewpointIndices,
 		const std::map<int, Transform> & viewpoints,
-		float factor,
+		float factor=0.01f,
-		float neighborScale=1.0f);
+		float neighborScale=2.0f);
 
 /**
  * For convenience.

corelib/src/Rtabmap.cpp

@@ -1407,6 +1407,7 @@ bool Rtabmap::process(
 
 		// Update optimizedPoses with the newly added node
 		Transform newPose;
+		bool intermediateNodeRefining = false;
 		if(_neighborLinkRefining &&
 			signature->getLinks().size() &&
 			signature->getLinks().begin()->second.type() == Link::kNeighbor &&
@@ -1507,9 +1508,8 @@ bool Rtabmap::process(
 			}
 			else
 			{
-				UWARN("Neighbor link refining is activated but there are intermediate nodes (%d=%d %d=%d), aborting refining...",
-						signature->id(), signature->getWeight(), oldS->id(), oldS->getWeight());
 				newPose = _mapCorrection * signature->getPose();
+				intermediateNodeRefining = true;
 			}
 		}
 		else
@@ -1633,7 +1633,7 @@ bool Rtabmap::process(
 		//============================================================
 		// Local loop closure in TIME
 		//============================================================
-		if(_proximityByTime &&
+		if((_proximityByTime || intermediateNodeRefining) &&
 		   rehearsedId == 0 && // don't do it if rehearsal happened
 		   _memory->isIncremental() && // don't do it in localization mode
 		   signature->getWeight()>=0)
@@ -1684,6 +1684,12 @@ bool Rtabmap::process(
 						UINFO("Local loop closure (time) between %d and %d rejected: %s",
 								*iter, signature->id(), rejectedMsg.c_str());
 					}
+
+					if(!_proximityByTime && intermediateNodeRefining)
+					{
+						// Do it only with the latest non-intermediate node
+						break;
+					}
 				}
 			}
 		}

corelib/src/util3d_filtering.cpp

@@ -1123,7 +1123,7 @@ pcl::IndicesPtr radiusFilteringImpl(
 		{
 			std::vector<int> kIndices;
 			std::vector<float> kDistances;
-			int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
+			int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances, minNeighborsInRadius+1);
 			if(k > minNeighborsInRadius)
 			{
 				output->at(oi++) = indices->at(i);
@@ -1141,7 +1141,7 @@ pcl::IndicesPtr radiusFilteringImpl(
 		{
 			std::vector<int> kIndices;
 			std::vector<float> kDistances;
-			int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
+			int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances, minNeighborsInRadius+1);
 			if(k > minNeighborsInRadius)
 			{
 				output->at(oi++) = i;

tools/Export/main.cpp

@@ -110,7 +110,7 @@ void showUsage()
 			"    --noise_radius  #     Noise filtering search radius (default 0, 0=disabled).\n"
 			"    --noise_k       #     Noise filtering minimum neighbors in search radius (default 5, 0=disabled).\n"
 			"    --prop_radius_factor #  Proportional radius filter factor (default 0, 0=disabled). Start tuning from 0.01.\n"
-			"    --prop_radius_scale  #  Proportional radius filter neighbor scale (default 1).\n"
+			"    --prop_radius_scale  #  Proportional radius filter neighbor scale (default 2).\n"
 			"    --random_samples #    Number of output samples using a random filter (default 0, 0=disabled).\n"
 			"    --color_radius  #     Radius used to colorize polygons (default 0.05 m, 0 m with --scan). Set 0 for nearest color.\n"
 			"    --scan                Use laser scan for the point cloud.\n"
@@ -168,7 +168,7 @@ int main(int argc, char * argv[])
 	float noiseRadius = 0.0f;
 	int noiseMinNeighbors = 5;
 	float proportionalRadiusFactor = 0.0f;
-	float proportionalRadiusScale = 1.0f;
+	float proportionalRadiusScale = 2.0f;
 	int randomSamples = 0;
 	int textureSize = 8192;
 	int textureCount = 1;
@@ -626,6 +626,7 @@ int main(int argc, char * argv[])
 			if(i<argc-1)
 			{
 				proportionalRadiusScale = uStr2Float(argv[i]);
+				UASSERT_MSG(proportionalRadiusScale>=1.0f, "--prop_radius_scale should be >= 1.0");
 			}
 			else
 			{
@@ -1225,7 +1226,7 @@ int main(int argc, char * argv[])
 			}
 		}
 
-		if(proportionalRadiusFactor>0.0f)
+		if(proportionalRadiusFactor>0.0f && proportionalRadiusScale>=1.0f)
 		{
 			printf("Proportional radius filtering of the assembled cloud... (factor=%f scale=%f, %d points)\n", proportionalRadiusFactor, proportionalRadiusScale, !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
 			pcl::IndicesPtr indices;