From 5d11f5cb65aa97dde197ee5e8c22bf4a1a840320 Mon Sep 17 00:00:00 2001 From: Nicolas Cadart Date: Tue, 15 Jun 2021 12:10:55 +0200 Subject: [PATCH 1/4] [feat] Change susbsampling strategy in overlap computation VoxelGrid sampling is quite slow, and leads to smaller overlap values. Now, the idea is to use only a subset of points to compute overlap on. The user can provide a ratio of points to use, between 0 and 1. --- paraview_wrapping/Slam.xml | 13 +++++------ paraview_wrapping/vtkSlam.h | 4 ++-- .../lidar_slam/params/slam_config.yaml | 3 +-- ros_wrapping/lidar_slam/src/LidarSlamNode.cxx | 2 +- .../include/LidarSlam/ConfidenceEstimators.h | 10 +++++++-- slam_lib/include/LidarSlam/Slam.h | 13 +++++------ slam_lib/include/LidarSlam/Utilities.h | 15 +++++++++++++ slam_lib/src/ConfidenceEstimators.cxx | 10 ++++++--- slam_lib/src/Slam.cxx | 22 ++++++++----------- 9 files changed, 54 insertions(+), 38 deletions(-) diff --git a/paraview_wrapping/Slam.xml b/paraview_wrapping/Slam.xml index f76f9b53..b5b1e3f5 100644 --- a/paraview_wrapping/Slam.xml +++ b/paraview_wrapping/Slam.xml @@ -1036,15 +1036,14 @@ - Leaf size of the voxel grid to sample uniformly before evaluating overlap (in meters). - If g.t. 0, the estimation step will be faster but the value will be lower. - If 0, no sampling is applied. + Ratio of points (between 0 and 1) from the input cloud to compute overlap on. + Downsampling accelerates the overlap computation, but may be less precise. @@ -1088,7 +1087,7 @@ - + diff --git a/paraview_wrapping/vtkSlam.h b/paraview_wrapping/vtkSlam.h index 52e7f8c2..7b724202 100644 --- a/paraview_wrapping/vtkSlam.h +++ b/paraview_wrapping/vtkSlam.h @@ -248,8 +248,8 @@ public: // Confidence estimator parameters // --------------------------------------------------------------------------- - vtkCustomGetMacro(OverlapSamplingLeafSize, double) - vtkCustomSetMacro(OverlapSamplingLeafSize, double) + vtkCustomGetMacro(OverlapSamplingRatio, double) + vtkCustomSetMacro(OverlapSamplingRatio, double) // Motion constraints virtual void SetAccelerationLimits(float linearAcc, float angularAcc); diff --git a/ros_wrapping/lidar_slam/params/slam_config.yaml b/ros_wrapping/lidar_slam/params/slam_config.yaml index bb10048e..7409a96d 100644 --- a/ros_wrapping/lidar_slam/params/slam_config.yaml +++ b/ros_wrapping/lidar_slam/params/slam_config.yaml @@ -193,8 +193,7 @@ slam: confidence: overlap: enable: false # Compute estimation of the overlap between the current scan and the keypoint maps. - sampling_leaf_size: 0. # [m] Leaf size of the voxel grid to sample uniformly before evaluating overlap. - # If g.t. 0, the estimation step will be faster but the value will be lower (due to sampling grids effects). + sampling_ratio: 1. # [0-1] Ratio of points from the input cloud to compute overlap on to save some time. motion_limits: # Physical constraints on motion to check pose credibility acceleration: [.inf, .inf] # [linear_acc (m/s2), angular_acc (°/s2)] Acceleration limits velocity: [.inf, .inf] # [linear_vel (m/s ), angular_vel (°/s )] Velocity limits diff --git a/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx b/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx index 34e5ce3d..be243b38 100644 --- a/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx +++ b/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx @@ -681,7 +681,7 @@ void LidarSlamNode::SetSlamParameters() // Confidence estimators // Overlap SetSlamParam(bool, "slam/confidence/overlap/enable", OverlapEnable) - SetSlamParam(float, "slam/confidence/overlap/sampling_leaf_size", OverlapSamplingLeafSize) + SetSlamParam(float, "slam/confidence/overlap/sampling_ratio", OverlapSamplingRatio) // Keyframes SetSlamParam(double, "slam/keyframes/distance_threshold", KfDistanceThreshold) diff --git a/slam_lib/include/LidarSlam/ConfidenceEstimators.h b/slam_lib/include/LidarSlam/ConfidenceEstimators.h index 5044ac66..d00aa0ca 100644 --- a/slam_lib/include/LidarSlam/ConfidenceEstimators.h +++ b/slam_lib/include/LidarSlam/ConfidenceEstimators.h @@ -41,8 +41,14 @@ using KDTree = KDTreePCLAdaptor; // It corresponds to the number of points from cloud which have a neighbor in // the submaps relatively to the resolution of the maps. // (see http://geometry.cs.ucl.ac.uk/projects/2014/super4PCS/ for more info) -// In this LCP extension, we also check the distance between nearest neighbors to make a smooth estimator -float LCPEstimator(PointCloud::ConstPtr cloud, const std::map>& maps, int nbThreads = 1); +// In this LCP extension, we also check the distance between nearest neighbors +// to make a smooth estimator. +// To accelerate the process, the ratio of points (between 0 and 1) from the +// input cloud to compute overlap on can be specified. +float LCPEstimator(PointCloud::ConstPtr cloud, + const std::map>& maps, + float subsamplingRatio = 1., + int nbThreads = 1); } // enf of Confidence namespace diff --git a/slam_lib/include/LidarSlam/Slam.h b/slam_lib/include/LidarSlam/Slam.h index 22888576..58d0a2ad 100644 --- a/slam_lib/include/LidarSlam/Slam.h +++ b/slam_lib/include/LidarSlam/Slam.h @@ -368,8 +368,8 @@ public: GetMacro(OverlapEnable, bool) SetMacro(OverlapEnable, bool) - GetMacro(OverlapSamplingLeafSize, float) - SetMacro(OverlapSamplingLeafSize, float) + GetMacro(OverlapSamplingRatio, float) + void SetOverlapSamplingRatio(float _arg); GetMacro(OverlapEstimation, float) @@ -657,12 +657,9 @@ private: // at the end of the Localization step bool OverlapEnable = true; - // Leaf size of the voxel grid used to filter input scans - // Remaining points are used to estimate the overlap - // If increased, computation time may be reduced but the accuracy - // of the overlap estimation may be impacted - // Notably, the continuity of the estimation could be slighlty corrupted - float OverlapSamplingLeafSize = 0.f; + // [0-1] Ratio of points from the input cloud to compute overlap on. + // Downsampling accelerates the overlap computation, but may be less precise. + float OverlapSamplingRatio = 1.f; // Motion limitations // Local velocity thresholds in BASE diff --git a/slam_lib/include/LidarSlam/Utilities.h b/slam_lib/include/LidarSlam/Utilities.h index f9c7e9cd..7a8ff212 100644 --- a/slam_lib/include/LidarSlam/Utilities.h +++ b/slam_lib/include/LidarSlam/Utilities.h @@ -109,6 +109,21 @@ std::vector SortIdx(const std::vector& v, bool ascending=true) return idx; } +//------------------------------------------------------------------------------ +/*! + * @brief Clamp a value between min and max saturation thresholds. + * @param val The value to clamp + * @param min The lower saturation threshold + * @param max The higher saturation threshold + * @return The value clamped between min and max: min <= retval <= max + */ +//---------------------------------------------------------------------------- +template +T Clamp(const T& val, const T& min, const T& max) +{ + return (val < min) ? min : (max < val) ? max : val; +} + //------------------------------------------------------------------------------ /*! * @brief Convert first char to upper case diff --git a/slam_lib/src/ConfidenceEstimators.cxx b/slam_lib/src/ConfidenceEstimators.cxx index c8dbc5cf..c9255a83 100644 --- a/slam_lib/src/ConfidenceEstimators.cxx +++ b/slam_lib/src/ConfidenceEstimators.cxx @@ -25,21 +25,25 @@ namespace LidarSlam namespace Confidence { -float LCPEstimator(PointCloud::ConstPtr cloud, const std::map>& maps, int nbThreads) +float LCPEstimator(PointCloud::ConstPtr cloud, + const std::map>& maps, + float subsamplingRatio, + int nbThreads) { float LCP = 0.f; - int nbPoints = cloud->size(); + int nbPoints = cloud->size() * subsamplingRatio; if (nbPoints > 0) { std::vector LCPvec(nbPoints, 0.f); #pragma omp parallel for num_threads(nbThreads) for (int n = 0; n < nbPoints; ++n) { + const auto& point = cloud->at(n / subsamplingRatio); for (const auto& map : maps) { std::vector knnIndices; std::vector knnSqDist; - if (map.second->GetSubMapKdTree().KnnSearch(cloud->at(n), 1, knnIndices, knnSqDist) > 0) + if (map.second->GetSubMapKdTree().KnnSearch(point, 1, knnIndices, knnSqDist) > 0) { // We use a Gaussian like estimation for each point fitted in target leaf space // to check the probability that one cloud point has a neighbor in the target diff --git a/slam_lib/src/Slam.cxx b/slam_lib/src/Slam.cxx index 1368fcb9..6ae64be6 100644 --- a/slam_lib/src/Slam.cxx +++ b/slam_lib/src/Slam.cxx @@ -1327,23 +1327,18 @@ void Slam::RefineUndistortion() // Confidence estimators //============================================================================== +//----------------------------------------------------------------------------- +void Slam::SetOverlapSamplingRatio (float _arg) +{ + this->OverlapSamplingRatio = Utils::Clamp(_arg, 0.f, 1.f); +} + //----------------------------------------------------------------------------- void Slam::EstimateOverlap() { // Aggregate all input points into WORLD coordinates PointCloud::Ptr aggregatedPoints = this->GetRegisteredFrame(); - // Uniform sampling cloud - PointCloud::Ptr sampledCloud = aggregatedPoints; - if (this->OverlapSamplingLeafSize > 1e-3) - { - sampledCloud.reset(new PointCloud); - pcl::VoxelGrid uniFilter; - uniFilter.setInputCloud(aggregatedPoints); - uniFilter.setLeafSize(this->OverlapSamplingLeafSize, this->OverlapSamplingLeafSize, this->OverlapSamplingLeafSize); - uniFilter.filter(*sampledCloud); - } - // Keep only the maps to use std::map> mapsToUse; for (auto k : KeypointTypes) @@ -1354,8 +1349,9 @@ void Slam::EstimateOverlap() // Compute LCP like estimator // (see http://geometry.cs.ucl.ac.uk/projects/2014/super4PCS/ for more info) - this->OverlapEstimation = Confidence::LCPEstimator(sampledCloud, mapsToUse, this->NbThreads); - PRINT_VERBOSE(3, "Overlap : " << this->OverlapEstimation << ", estimated on : " << sampledCloud->size() << " points."); + this->OverlapEstimation = Confidence::LCPEstimator(aggregatedPoints, mapsToUse, this->OverlapSamplingRatio, this->NbThreads); + PRINT_VERBOSE(3, "Overlap : " << this->OverlapEstimation << ", estimated on : " + << static_cast(aggregatedPoints->size() * this->OverlapSamplingRatio) << " points."); } //----------------------------------------------------------------------------- -- GitLab From c8597a5c8f0192bef84e63188ecd1a7edbac3b51 Mon Sep 17 00:00:00 2001 From: Nicolas Cadart Date: Tue, 15 Jun 2021 13:24:22 +0200 Subject: [PATCH 2/4] [refactor][perf] Clean and accelerate LCPEstimator Use OpenMP reduction instead of shared vector. Avoid using std::vector to store only a single value. Reorder checks. Add comments. This brings a perf improvement of 20% for the LCPEstimator function. --- .../include/LidarSlam/ConfidenceEstimators.h | 7 +-- slam_lib/src/ConfidenceEstimators.cxx | 56 ++++++++++--------- 2 files changed, 31 insertions(+), 32 deletions(-) diff --git a/slam_lib/include/LidarSlam/ConfidenceEstimators.h b/slam_lib/include/LidarSlam/ConfidenceEstimators.h index d00aa0ca..58afd6bd 100644 --- a/slam_lib/include/LidarSlam/ConfidenceEstimators.h +++ b/slam_lib/include/LidarSlam/ConfidenceEstimators.h @@ -28,16 +28,14 @@ namespace LidarSlam { - namespace Confidence { using Point = LidarPoint; using PointCloud = pcl::PointCloud; -using KDTree = KDTreePCLAdaptor; -// Compute the LCP estimator (overlap estimator) for the registration of the -// points of cloud onto some prebuilt submaps of maps. +// Compute the LCP estimator (overlap estimator) for the registration of a +// pointcloud onto some prebuilt maps. // It corresponds to the number of points from cloud which have a neighbor in // the submaps relatively to the resolution of the maps. // (see http://geometry.cs.ucl.ac.uk/projects/2014/super4PCS/ for more info) @@ -51,5 +49,4 @@ float LCPEstimator(PointCloud::ConstPtr cloud, int nbThreads = 1); } // enf of Confidence namespace - } // end of LidarSlam namespace \ No newline at end of file diff --git a/slam_lib/src/ConfidenceEstimators.cxx b/slam_lib/src/ConfidenceEstimators.cxx index c9255a83..75d13e0b 100644 --- a/slam_lib/src/ConfidenceEstimators.cxx +++ b/slam_lib/src/ConfidenceEstimators.cxx @@ -17,50 +17,52 @@ //============================================================================== #include -#include namespace LidarSlam { - namespace Confidence { +//----------------------------------------------------------------------------- float LCPEstimator(PointCloud::ConstPtr cloud, const std::map>& maps, float subsamplingRatio, int nbThreads) { - float LCP = 0.f; + // Number of points to process int nbPoints = cloud->size() * subsamplingRatio; - if (nbPoints > 0) + if (nbPoints == 0 || maps.empty()) + return 0.; + + // Iterate on all points of input cloud to process + float lcp = 0.; + #pragma omp parallel for num_threads(nbThreads) reduction(+:lcp) + for (int n = 0; n < nbPoints; ++n) { - std::vector LCPvec(nbPoints, 0.f); - #pragma omp parallel for num_threads(nbThreads) - for (int n = 0; n < nbPoints; ++n) + // Compute the LCP contribution of the current point + const auto& point = cloud->at(n / subsamplingRatio); + float bestProba = 0.; + for (const auto& map : maps) { - const auto& point = cloud->at(n / subsamplingRatio); - for (const auto& map : maps) + // Get nearest neighbor + int nnIndex; + float nnSqDist; + if (map.second->GetSubMapKdTree().KnnSearch(point.data, 1, &nnIndex, &nnSqDist)) { - std::vector knnIndices; - std::vector knnSqDist; - if (map.second->GetSubMapKdTree().KnnSearch(point, 1, knnIndices, knnSqDist) > 0) - { - // We use a Gaussian like estimation for each point fitted in target leaf space - // to check the probability that one cloud point has a neighbor in the target - // Probability = 1 if the two points are superimposed - // Probability < 0.011 if the distance is g.t. the leaf size - float sqLCPThreshold = std::pow(map.second->GetLeafSize() / 3.f, 2); - float currentProba = std::exp( -knnSqDist[0] / (2.f * sqLCPThreshold) ); - if (currentProba > LCPvec[n]) - LCPvec[n] = currentProba; - } + // We use a Gaussian like estimation for each point fitted in target leaf space + // to check the probability that one cloud point has a neighbor in the target + // Probability = 1 if the two points are superimposed + // Probability < 0.011 if the distance is g.t. the leaf size + float sqLCPThreshold = std::pow(map.second->GetLeafSize() / 3.f, 2); + float currentProba = std::exp( -nnSqDist / (2.f * sqLCPThreshold) ); + if (currentProba > bestProba) + bestProba = currentProba; } } - LCP = std::accumulate(LCPvec.begin(), LCPvec.end(), 0.f) / nbPoints; + lcp += bestProba; } - return LCP; -} - + return lcp / nbPoints; } -} \ No newline at end of file +} // end of Confidence namespace +} // end of LidarSlam namespace \ No newline at end of file -- GitLab From 9002c80e236884d55de853aaa9e8da5a64ff3f94 Mon Sep 17 00:00:00 2001 From: Nicolas Cadart Date: Wed, 16 Jun 2021 09:50:02 +0200 Subject: [PATCH 3/4] [defaults][api] Remove OverlapEnable and disable overlab by default Use OverlapSamplingRatio = 0 to disable overlap. Set default to OverlapSamplingRatio = 0, automatically disabling overlap computation to save time. Update wrappings to change this behavior. --- paraview_wrapping/Slam.xml | 17 +++++++---- paraview_wrapping/vtkSlam.cxx | 29 +++++++++++++------ paraview_wrapping/vtkSlam.h | 5 +++- .../lidar_slam/params/slam_config.yaml | 6 ++-- ros_wrapping/lidar_slam/src/LidarSlamNode.cxx | 10 +++---- slam_lib/include/LidarSlam/Slam.h | 11 ++----- slam_lib/src/Slam.cxx | 2 +- 7 files changed, 46 insertions(+), 34 deletions(-) diff --git a/paraview_wrapping/Slam.xml b/paraview_wrapping/Slam.xml index b5b1e3f5..2125f3e7 100644 --- a/paraview_wrapping/Slam.xml +++ b/paraview_wrapping/Slam.xml @@ -228,14 +228,12 @@ If advanced return mode is enabled, extra arrays are added to the outputs to display some of the SLAM internal variables : - - Trajectory : matching summary, localization error summary + - Trajectory : matching summary, localization error summary, confidence estimators - Output transformed frame : saliency, planarity, intensity gap, keypoint validity - Extracted keypoints : ICP matching results - - Confidence estimation : overlap - It is very useful when debugging or checking the SLAM behavior. If disabled, the overlap estimation is not computed. @@ -1038,12 +1036,19 @@ - Ratio of points (between 0 and 1) from the input cloud to compute overlap on. - Downsampling accelerates the overlap computation, but may be less precise. + Ratio of points (between 0 and 1) from the current frame to compute + overlap on. + + The overlap estimates how much the current scan is well registered on + the current maps. + + Downsampling accelerates the overlap computation, but may be less + precise. A ratio of 1 uses all points, 0.5 uses 1 point over 2, etc., + 0 disables overlap computation. diff --git a/paraview_wrapping/vtkSlam.cxx b/paraview_wrapping/vtkSlam.cxx index b1e188e4..55ce57b2 100644 --- a/paraview_wrapping/vtkSlam.cxx +++ b/paraview_wrapping/vtkSlam.cxx @@ -118,15 +118,10 @@ void vtkSlam::Reset() { auto debugInfo = this->SlamAlgo->GetDebugInformation(); for (const auto& it : debugInfo) - { this->Trajectory->GetPointData()->AddArray(Utils::CreateArray(it.first)); - } - // Enable overlap computation - this->SlamAlgo->SetOverlapEnable(true); } - else - // Disable overlap computation - this->SlamAlgo->SetOverlapEnable(false); + // Enable overlap computation only if advanced return mode is activated + this->SlamAlgo->SetOverlapSamplingRatio(this->AdvancedReturnMode ? this->OverlapSamplingRatio : 0.); // Refresh view this->Modified(); @@ -687,7 +682,7 @@ void vtkSlam::SetAdvancedReturnMode(bool _arg) this->Trajectory->GetPointData()->AddArray(array); } // Enable overlap computation - this->SlamAlgo->SetOverlapEnable(true); + this->SlamAlgo->SetOverlapSamplingRatio(this->OverlapSamplingRatio); } // If AdvancedReturnMode is being disabled @@ -697,7 +692,7 @@ void vtkSlam::SetAdvancedReturnMode(bool _arg) for (const auto& it : debugInfo) this->Trajectory->GetPointData()->RemoveArray(it.first.c_str()); // Disable overlap computation - this->SlamAlgo->SetOverlapEnable(false); + this->SlamAlgo->SetOverlapSamplingRatio(0.); } this->AdvancedReturnMode = _arg; @@ -794,6 +789,22 @@ void vtkSlam::SetVoxelGridLeafSize(LidarSlam::Keypoint k, double s) this->ParametersModificationTime.Modified(); } +//----------------------------------------------------------------------------- +void vtkSlam::SetOverlapSamplingRatio(double ratio) +{ + // Change parameter value if it is modified + vtkDebugMacro(<< this->GetClassName() << " (" << this << "): setting OverlapSamplingRatio to " << ratio); + if (this->OverlapSamplingRatio != ratio) + { + this->OverlapSamplingRatio = ratio; + this->ParametersModificationTime.Modified(); + } + + // Forward this parameter change to SLAM if Advanced Return Mode is enabled + if (this->AdvancedReturnMode) + this->SlamAlgo->SetOverlapSamplingRatio(this->OverlapSamplingRatio); +} + //----------------------------------------------------------------------------- void vtkSlam::SetAccelerationLimits(float linearAcc, float angularAcc) { diff --git a/paraview_wrapping/vtkSlam.h b/paraview_wrapping/vtkSlam.h index 7b724202..6d75b0fb 100644 --- a/paraview_wrapping/vtkSlam.h +++ b/paraview_wrapping/vtkSlam.h @@ -249,7 +249,7 @@ public: // --------------------------------------------------------------------------- vtkCustomGetMacro(OverlapSamplingRatio, double) - vtkCustomSetMacro(OverlapSamplingRatio, double) + virtual void SetOverlapSamplingRatio (double ratio); // Motion constraints virtual void SetAccelerationLimits(float linearAcc, float angularAcc); @@ -339,6 +339,9 @@ private: std::string VerticalCalibArrayName; ///< Calibration column used to sort laser rings by elevation angle double TimeToSecondsFactor; ///< Coef to apply to TimeArray values to express time in seconds double TimeToSecondsFactorSetting; ///< Duplicated parameter used to store the value set by user + + // Internal variable to store overlap sampling ratio when advanced return mode is disabled. + float OverlapSamplingRatio = 0.25; }; #endif // VTK_SLAM_H \ No newline at end of file diff --git a/ros_wrapping/lidar_slam/params/slam_config.yaml b/ros_wrapping/lidar_slam/params/slam_config.yaml index 7409a96d..34575efe 100644 --- a/ros_wrapping/lidar_slam/params/slam_config.yaml +++ b/ros_wrapping/lidar_slam/params/slam_config.yaml @@ -191,9 +191,9 @@ slam: # Confidence estimators on pose output confidence: - overlap: - enable: false # Compute estimation of the overlap between the current scan and the keypoint maps. - sampling_ratio: 1. # [0-1] Ratio of points from the input cloud to compute overlap on to save some time. + overlap: # Estimate how much the current scan is well registered on the current maps. + sampling_ratio: 0. # [0-1] Ratio of points to compute overlap on to save some time. + # 1 uses all points, 0.5 uses 1 point over 2, etc., 0 disables overlap computation. motion_limits: # Physical constraints on motion to check pose credibility acceleration: [.inf, .inf] # [linear_acc (m/s2), angular_acc (°/s2)] Acceleration limits velocity: [.inf, .inf] # [linear_vel (m/s ), angular_vel (°/s )] Velocity limits diff --git a/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx b/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx index be243b38..44dfba63 100644 --- a/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx +++ b/ros_wrapping/lidar_slam/src/LidarSlamNode.cxx @@ -680,13 +680,7 @@ void LidarSlamNode::SetSlamParameters() // Confidence estimators // Overlap - SetSlamParam(bool, "slam/confidence/overlap/enable", OverlapEnable) SetSlamParam(float, "slam/confidence/overlap/sampling_ratio", OverlapSamplingRatio) - - // Keyframes - SetSlamParam(double, "slam/keyframes/distance_threshold", KfDistanceThreshold) - SetSlamParam(double, "slam/keyframes/angle_threshold", KfAngleThreshold) - // Motion limitations (hard constraints to detect failure) std::vector acc; if (this->PrivNh.getParam("slam/confidence/motion_limits/acceleration", acc) && acc.size() == 2) @@ -695,6 +689,10 @@ void LidarSlamNode::SetSlamParameters() if (this->PrivNh.getParam("slam/confidence/motion_limits/velocity", vel) && vel.size() == 2) this->LidarSlam.SetVelocityLimits(Eigen::Map(vel.data())); + // Keyframes + SetSlamParam(double, "slam/keyframes/distance_threshold", KfDistanceThreshold) + SetSlamParam(double, "slam/keyframes/angle_threshold", KfAngleThreshold) + // Rolling grids double size; if (this->PrivNh.getParam("slam/voxel_grid/leaf_size_edges", size)) diff --git a/slam_lib/include/LidarSlam/Slam.h b/slam_lib/include/LidarSlam/Slam.h index 58d0a2ad..12fde6ee 100644 --- a/slam_lib/include/LidarSlam/Slam.h +++ b/slam_lib/include/LidarSlam/Slam.h @@ -364,10 +364,8 @@ public: // --------------------------------------------------------------------------- // Confidence estimation // --------------------------------------------------------------------------- - // Overlap - GetMacro(OverlapEnable, bool) - SetMacro(OverlapEnable, bool) + // Overlap GetMacro(OverlapSamplingRatio, float) void SetOverlapSamplingRatio(float _arg); @@ -653,13 +651,10 @@ private: // Parameters - // Boolean to choose whether to compute the estimated overlap or not - // at the end of the Localization step - bool OverlapEnable = true; - // [0-1] Ratio of points from the input cloud to compute overlap on. // Downsampling accelerates the overlap computation, but may be less precise. - float OverlapSamplingRatio = 1.f; + // If 0, overlap won't be computed. + float OverlapSamplingRatio = 0.f; // Motion limitations // Local velocity thresholds in BASE diff --git a/slam_lib/src/Slam.cxx b/slam_lib/src/Slam.cxx index 6ae64be6..165290f4 100644 --- a/slam_lib/src/Slam.cxx +++ b/slam_lib/src/Slam.cxx @@ -263,7 +263,7 @@ void Slam::AddFrames(const std::vector& frames) // Compute and check pose confidence estimators IF_VERBOSE(3, Utils::Timer::Init("Confidence estimators computation")); - if (this->OverlapEnable) + if (this->OverlapSamplingRatio > 0) this->EstimateOverlap(); this->CheckMotionLimits(); IF_VERBOSE(3, Utils::Timer::StopAndDisplay("Confidence estimators computation")); -- GitLab From 7177a1b3ddfca8d1b237cc4637ca4552d351624f Mon Sep 17 00:00:00 2001 From: Nicolas Cadart Date: Wed, 16 Jun 2021 15:38:56 +0200 Subject: [PATCH 4/4] [fix] Return -1 overlap value if not enough points are available --- slam_lib/include/LidarSlam/ConfidenceEstimators.h | 2 ++ slam_lib/include/LidarSlam/Slam.h | 3 ++- slam_lib/src/ConfidenceEstimators.cxx | 2 +- slam_lib/src/Slam.cxx | 5 +++++ 4 files changed, 10 insertions(+), 2 deletions(-) diff --git a/slam_lib/include/LidarSlam/ConfidenceEstimators.h b/slam_lib/include/LidarSlam/ConfidenceEstimators.h index 58afd6bd..a0f0d61c 100644 --- a/slam_lib/include/LidarSlam/ConfidenceEstimators.h +++ b/slam_lib/include/LidarSlam/ConfidenceEstimators.h @@ -43,6 +43,8 @@ using PointCloud = pcl::PointCloud; // to make a smooth estimator. // To accelerate the process, the ratio of points (between 0 and 1) from the // input cloud to compute overlap on can be specified. +// It returns a valid overlap value between 0 and 1, or -1 if the overlap could +// not be computed (not enough points). float LCPEstimator(PointCloud::ConstPtr cloud, const std::map>& maps, float subsamplingRatio = 1., diff --git a/slam_lib/include/LidarSlam/Slam.h b/slam_lib/include/LidarSlam/Slam.h index 12fde6ee..c4c068d5 100644 --- a/slam_lib/include/LidarSlam/Slam.h +++ b/slam_lib/include/LidarSlam/Slam.h @@ -637,7 +637,8 @@ private: // Data // Overlap estimation of the current registered scan on the keypoints map - // It is contained between 0 and 1 + // A valid value lies in range [0-1]. + // It is set to -1 if overlap has not been evaluated (disabled or not enough points). float OverlapEstimation = -1.f; // Number of matches for processed frame diff --git a/slam_lib/src/ConfidenceEstimators.cxx b/slam_lib/src/ConfidenceEstimators.cxx index 75d13e0b..72d4b542 100644 --- a/slam_lib/src/ConfidenceEstimators.cxx +++ b/slam_lib/src/ConfidenceEstimators.cxx @@ -32,7 +32,7 @@ float LCPEstimator(PointCloud::ConstPtr cloud, // Number of points to process int nbPoints = cloud->size() * subsamplingRatio; if (nbPoints == 0 || maps.empty()) - return 0.; + return -1.; // Iterate on all points of input cloud to process float lcp = 0.; diff --git a/slam_lib/src/Slam.cxx b/slam_lib/src/Slam.cxx index 165290f4..c89935db 100644 --- a/slam_lib/src/Slam.cxx +++ b/slam_lib/src/Slam.cxx @@ -1330,7 +1330,12 @@ void Slam::RefineUndistortion() //----------------------------------------------------------------------------- void Slam::SetOverlapSamplingRatio (float _arg) { + // Clamp ratio beteen 0 and 1 this->OverlapSamplingRatio = Utils::Clamp(_arg, 0.f, 1.f); + + // Reset overlap value if overlap computation is disabled + if (this->OverlapSamplingRatio == 0.) + this->OverlapEstimation = -1.f; } //----------------------------------------------------------------------------- -- GitLab