diff --git a/paraview_wrapping/Slam.xml b/paraview_wrapping/Slam.xml
index f76f9b53aa18176a8e95595a20b44f272f1580db..2125f3e7d340f93572a01ab0e4514d8a93def274 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.
@@ -1036,15 +1034,21 @@
- 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 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.
@@ -1088,7 +1092,7 @@
-
+
diff --git a/paraview_wrapping/vtkSlam.cxx b/paraview_wrapping/vtkSlam.cxx
index b1e188e4b4546a56e4149be35dbb2c6298593c23..55ce57b291abdb1e2c4d4005f57256ad3eba061b 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 52e7f8c27de4b8a81504669ec89f61c37764ee7b..6d75b0fb702926e5d57b454bfbff67525303c5b1 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)
+ 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 bb10048e19b52e6af6117d554be64c410a5d6fd8..34575efea3f9948ebd3e64691ab640afc0f86909 100644
--- a/ros_wrapping/lidar_slam/params/slam_config.yaml
+++ b/ros_wrapping/lidar_slam/params/slam_config.yaml
@@ -191,10 +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_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).
+ 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 34e5ce3d79c7532aaf450cc106ac3426bcd6784c..44dfba63e889cd618dff74592a03702a20a3cf95 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_leaf_size", OverlapSamplingLeafSize)
-
- // Keyframes
- SetSlamParam(double, "slam/keyframes/distance_threshold", KfDistanceThreshold)
- SetSlamParam(double, "slam/keyframes/angle_threshold", KfAngleThreshold)
-
+ SetSlamParam(float, "slam/confidence/overlap/sampling_ratio", OverlapSamplingRatio)
// 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/ConfidenceEstimators.h b/slam_lib/include/LidarSlam/ConfidenceEstimators.h
index 5044ac66bd1ec69034abd5ec1541299541eaa703..a0f0d61c28f0f768d9f98c297fc76f0909a64fdd 100644
--- a/slam_lib/include/LidarSlam/ConfidenceEstimators.h
+++ b/slam_lib/include/LidarSlam/ConfidenceEstimators.h
@@ -28,22 +28,27 @@
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)
-// 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.
+// 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.,
+ int nbThreads = 1);
} // enf of Confidence namespace
-
} // end of LidarSlam namespace
\ No newline at end of file
diff --git a/slam_lib/include/LidarSlam/Slam.h b/slam_lib/include/LidarSlam/Slam.h
index 22888576bd6e20f498d878c92580b0cae1a2eef2..c4c068d573608b4dc70a4a672b43f53c3501c531 100644
--- a/slam_lib/include/LidarSlam/Slam.h
+++ b/slam_lib/include/LidarSlam/Slam.h
@@ -364,12 +364,10 @@ public:
// ---------------------------------------------------------------------------
// Confidence estimation
// ---------------------------------------------------------------------------
- // Overlap
- GetMacro(OverlapEnable, bool)
- SetMacro(OverlapEnable, bool)
- GetMacro(OverlapSamplingLeafSize, float)
- SetMacro(OverlapSamplingLeafSize, float)
+ // Overlap
+ GetMacro(OverlapSamplingRatio, float)
+ void SetOverlapSamplingRatio(float _arg);
GetMacro(OverlapEstimation, float)
@@ -639,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
@@ -653,16 +652,10 @@ private:
// Parameters
- // Boolean to choose whether to compute the estimated overlap or not
- // 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.
+ // If 0, overlap won't be computed.
+ float OverlapSamplingRatio = 0.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 f9c7e9cd85be3cd358d2ccc129363b7f25816aec..7a8ff212cbe58b03ca24894a5092572114bef732 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 c8dbc5cf1416494864cff1ccb7af7afce6fda5b5..72d4b542726d188e6de1ecdbf89a8b4e151b2117 100644
--- a/slam_lib/src/ConfidenceEstimators.cxx
+++ b/slam_lib/src/ConfidenceEstimators.cxx
@@ -17,46 +17,52 @@
//==============================================================================
#include
-#include
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();
- if (nbPoints > 0)
+ // Number of points to process
+ int nbPoints = cloud->size() * subsamplingRatio;
+ if (nbPoints == 0 || maps.empty())
+ return -1.;
+
+ // 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)
{
- 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(cloud->at(n), 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
diff --git a/slam_lib/src/Slam.cxx b/slam_lib/src/Slam.cxx
index 1368fcb94da7939848cfb605afd6e2422e26d03c..c89935db77ad92fa00b73b5d1d6c5ef01a622882 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"));
@@ -1327,23 +1327,23 @@ void Slam::RefineUndistortion()
// Confidence estimators
//==============================================================================
+//-----------------------------------------------------------------------------
+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;
+}
+
//-----------------------------------------------------------------------------
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 +1354,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.");
}
//-----------------------------------------------------------------------------