diff --git a/corelib/include/rtabmap/core/util3d_filtering.h b/corelib/include/rtabmap/core/util3d_filtering.h index 817719a4..2f2b8e81 100644 --- a/corelib/include/rtabmap/core/util3d_filtering.h +++ b/corelib/include/rtabmap/core/util3d_filtering.h @@ -466,38 +466,38 @@ pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering( const pcl::PointCloud::Ptr & cloud, const std::vector & viewpointIndices, const std::map & viewpoints, - float factor, - float neighborScale=1.0f); + float factor=0.01f, + float neighborScale=2.0f); pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering( const pcl::PointCloud::Ptr & cloud, const std::vector & viewpointIndices, const std::map & viewpoints, - float factor, - float neighborScale=1.0f); + float factor=0.01f, + float neighborScale=2.0f); pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering( const pcl::PointCloud::Ptr & cloud, const std::vector & viewpointIndices, const std::map & viewpoints, - float factor, - float neighborScale=1.0f); + float factor=0.01f, + float neighborScale=2.0f); pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering( const pcl::PointCloud::Ptr & cloud, const std::vector & viewpointIndices, const std::map & viewpoints, - float factor, - float neighborScale=1.0f); + float factor=0.01f, + float neighborScale=2.0f); pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering( const pcl::PointCloud::Ptr & cloud, const std::vector & viewpointIndices, const std::map & viewpoints, - float factor, - float neighborScale=1.0f); + float factor=0.01f, + float neighborScale=2.0f); pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering( const pcl::PointCloud::Ptr & cloud, const std::vector & viewpointIndices, const std::map & viewpoints, - float factor, - float neighborScale=1.0f); + float factor=0.01f, + 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 & viewpointIndices, const std::map & viewpoints, - float factor, - float neighborScale=1.0f); + float factor=0.01f, + float neighborScale=2.0f); pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const std::vector & viewpointIndices, const std::map & viewpoints, - float factor, - float neighborScale=1.0f); + float factor=0.01f, + float neighborScale=2.0f); pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const std::vector & viewpointIndices, const std::map & viewpoints, - float factor, - float neighborScale=1.0f); + float factor=0.01f, + float neighborScale=2.0f); pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const std::vector & viewpointIndices, const std::map & viewpoints, - float factor, - float neighborScale=1.0f); + float factor=0.01f, + float neighborScale=2.0f); pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const std::vector & viewpointIndices, const std::map & viewpoints, - float factor, - float neighborScale=1.0f); + float factor=0.01f, + float neighborScale=2.0f); pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, const std::vector & viewpointIndices, const std::map & viewpoints, - float factor, - float neighborScale=1.0f); + float factor=0.01f, + float neighborScale=2.0f); /** * For convenience. diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 13e71d22..19f799f9 100644 --- a/corelib/src/Rtabmap.cpp +++ b/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; + } } } } diff --git a/corelib/src/util3d_filtering.cpp b/corelib/src/util3d_filtering.cpp index 00218604..e6bc9923 100644 --- a/corelib/src/util3d_filtering.cpp +++ b/corelib/src/util3d_filtering.cpp @@ -1123,7 +1123,7 @@ pcl::IndicesPtr radiusFilteringImpl( { std::vector kIndices; std::vector 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 kIndices; std::vector 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; diff --git a/tools/Export/main.cpp b/tools/Export/main.cpp index b0c59a6d..9269f97b 100644 --- a/tools/Export/main.cpp +++ b/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=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;