diff --git a/paraview_wrapping/Slam.xml b/paraview_wrapping/Slam.xml
index 2bab6810df0827426833a045c992b4314817eaaf..83ea626d59c52d944b41045a5d318773a30e1cc0 100644
--- a/paraview_wrapping/Slam.xml
+++ b/paraview_wrapping/Slam.xml
@@ -234,7 +234,10 @@
- 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.
@@ -1030,6 +1033,28 @@
+
+
+
+
+ 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.
+
+
+
+
+
+
+
+
+
+
diff --git a/paraview_wrapping/vtkSlam.cxx b/paraview_wrapping/vtkSlam.cxx
index 14b8db8051f1ec75fd3226a6b9b282a62ccba9ee..3650f8f2b8b9e0bca5e0ce07f962d3613dfa117e 100644
--- a/paraview_wrapping/vtkSlam.cxx
+++ b/paraview_wrapping/vtkSlam.cxx
@@ -121,7 +121,12 @@ void vtkSlam::Reset()
{
this->Trajectory->GetPointData()->AddArray(Utils::CreateArray(it.first));
}
+ // Enable overlap computation
+ this->SlamAlgo->SetOverlapEnable(true);
}
+ else
+ // Disable overlap computation
+ this->SlamAlgo->SetOverlapEnable(false);
// Refresh view
this->Modified();
@@ -681,6 +686,8 @@ void vtkSlam::SetAdvancedReturnMode(bool _arg)
array->SetTuple1(i, 0.);
this->Trajectory->GetPointData()->AddArray(array);
}
+ // Enable overlap computation
+ this->SlamAlgo->SetOverlapEnable(true);
}
// If AdvancedReturnMode is being disabled
@@ -689,6 +696,8 @@ void vtkSlam::SetAdvancedReturnMode(bool _arg)
// Delete optional arrays
for (const auto& it : debugInfo)
this->Trajectory->GetPointData()->RemoveArray(it.first.c_str());
+ // Disable overlap computation
+ this->SlamAlgo->SetOverlapEnable(false);
}
this->AdvancedReturnMode = _arg;
diff --git a/paraview_wrapping/vtkSlam.h b/paraview_wrapping/vtkSlam.h
index 19e02e69cde9aec72d71d438b7693c16cd56a1b0..92935f0d1283e83acadeedbc10f09dd6d264b2c8 100644
--- a/paraview_wrapping/vtkSlam.h
+++ b/paraview_wrapping/vtkSlam.h
@@ -244,6 +244,13 @@ public:
vtkCustomSetMacroNoCheck(VoxelGridSize, int)
vtkCustomSetMacroNoCheck(VoxelGridResolution, double)
+ // ---------------------------------------------------------------------------
+ // Confidence estimator parameters
+ // ---------------------------------------------------------------------------
+
+ vtkCustomGetMacro(OverlapSamplingLeafSize, double)
+ vtkCustomSetMacro(OverlapSamplingLeafSize, double)
+
protected:
vtkSlam();
diff --git a/ros_wrapping/lidar_slam/params/slam_config.yaml b/ros_wrapping/lidar_slam/params/slam_config.yaml
index bc4a310f9a96de4e8dcc5667466e519a6153d790..1b059e08fe44136ca99b48bb601f2efe5eadd1ee 100644
--- a/ros_wrapping/lidar_slam/params/slam_config.yaml
+++ b/ros_wrapping/lidar_slam/params/slam_config.yaml
@@ -192,8 +192,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
+ 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).
# Keypoints maps, saved as downsampled voxel grids
voxel_grid:
diff --git a/slam_lib/include/LidarSlam/Slam.h b/slam_lib/include/LidarSlam/Slam.h
index 62b6d1c9add1f2955885dad3383e06d3c44a0040..4958f9eb4addaa048b43753b48daae972f8e3c9f 100644
--- a/slam_lib/include/LidarSlam/Slam.h
+++ b/slam_lib/include/LidarSlam/Slam.h
@@ -534,10 +534,6 @@ private:
// 6-DoF parameters (DoF order : X, Y, Z, rX, rY, rZ)
LocalOptimizer::RegistrationError LocalizationUncertainty;
- // Overlap estimation of the current registered scan on the keypoints map
- // It is contained between 0 and 1
- float OverlapEstimation = -1.f;
-
// Odometry manager
// Compute the residual with a weight, a measurements list and
// taking account of the acquisition time correspondance
@@ -623,8 +619,20 @@ private:
double LocalizationFinalSaturationDistance = 0.5;
// ---------------------------------------------------------------------------
- // Confidence estimator parameters
+ // Confidence estimation
// ---------------------------------------------------------------------------
+
+ // Data
+
+ // Overlap estimation of the current registered scan on the keypoints map
+ // It is contained between 0 and 1
+ float OverlapEstimation = -1.f;
+
+ // Number of matches for processed frame
+ unsigned int TotalMatchedKeypoints = 0;
+
+ // Parameters
+
// Boolean to choose whether to compute the estimated overlap or not
// at the end of the Localization step
bool OverlapEnable = true;
@@ -636,9 +644,6 @@ private:
// Notably, the continuity of the estimation could be slighlty corrupted
float OverlapSamplingLeafSize = 0.f;
- // Number of matches for processed frame
- unsigned int TotalMatchedKeypoints = 0;
-
// ---------------------------------------------------------------------------
// Main sub-problems and methods
// ---------------------------------------------------------------------------
@@ -670,9 +675,6 @@ private:
// Log current frame processing results : pose, covariance and keypoints.
void LogCurrentFrameState(double time, const std::string& frameId);
- // Estimate the overlap of the current scan onto the keypoint maps
- void EstimateOverlap(const std::map& mapKdTrees);
-
// ---------------------------------------------------------------------------
// Undistortion helpers
// ---------------------------------------------------------------------------
@@ -692,6 +694,13 @@ private:
// Update the undistortion interpolator poses bounds,
// and refine the undistortion of the current keypoints clouds.
void RefineUndistortion();
+
+ // ---------------------------------------------------------------------------
+ // Confidence estimator helpers
+ // ---------------------------------------------------------------------------
+
+ // Estimate the overlap of the current scan onto the keypoint maps
+ void EstimateOverlap(const std::map& mapKdTrees);
};
} // 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 a4a85a33e796b981e07200f18ad1240613279f09..a61a00cfe7adb37d852ead8b47ceaccd6883aee5 100644
--- a/slam_lib/src/ConfidenceEstimators.cxx
+++ b/slam_lib/src/ConfidenceEstimators.cxx
@@ -17,6 +17,7 @@
//==============================================================================
#include
+#include
namespace LidarSlam
{
@@ -28,27 +29,39 @@ float LCPEstimator(PointCloud::ConstPtr cloud, const std::map&
{
float LCP = 0.f;
int nbPoints = cloud->size();
- #pragma omp parallel for reduction(+:LCP) num_threads(nbThreads)
- for (int n = 0; n < nbPoints; ++n)
+ if (nbPoints > 0)
{
- for (auto& kdTree : kdTrees)
+ // Get only usable keypoint types
+ std::vector kpToUse;
+ for (const auto& kdTree : kdTrees)
{
- std::vector knnIndices;
- std::vector knnSqDist;
- kdTree.second.KnnSearch(cloud->at(n), 1, knnIndices, knnSqDist);
- float sqLCPThreshold = std::pow(leafSizes.at(kdTree.first), 2);
- if (!knnSqDist.empty())
+ if (!kdTree.second.GetInputCloud()->empty())
+ kpToUse.push_back(kdTree.first);
+ }
+
+ std::vector LCPvec(nbPoints, 0.f);
+ #pragma omp parallel for num_threads(nbThreads)
+ for (int n = 0; n < nbPoints; ++n)
+ {
+ for (const auto& k : kpToUse)
{
- // 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.6 if the distance is g.t. the leaf size
- LCP += std::exp( -knnSqDist[0] / (2.f * sqLCPThreshold) );
- break;
+ std::vector knnIndices;
+ std::vector knnSqDist;
+ if (kdTrees.at(k).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(leafSizes.at(k) / 3.f, 2);
+ float currentProba = std::exp( -knnSqDist[0] / (2.f * sqLCPThreshold) );
+ if (currentProba > LCPvec[n])
+ LCPvec[n] = currentProba;
+ }
}
}
+ LCP = std::accumulate(LCPvec.begin(), LCPvec.end(), 0.f) / nbPoints;
}
- LCP /= nbPoints;
return LCP;
}
diff --git a/slam_lib/src/Slam.cxx b/slam_lib/src/Slam.cxx
index 6e9a2d96f22418ca2caed95e4ba222e04daa920a..f07c2472c9c02e3bd185c8bbdca5145bfcfd3e87 100644
--- a/slam_lib/src/Slam.cxx
+++ b/slam_lib/src/Slam.cxx
@@ -602,7 +602,7 @@ std::unordered_map Slam::GetDebugInformation() const
map["Localization: position error"] = this->LocalizationUncertainty.PositionError;
map["Localization: orientation error"] = this->LocalizationUncertainty.OrientationError;
- map["Localization: overlap"] = this->OverlapEstimation;
+ map["Confidence: overlap"] = this->OverlapEstimation;
return map;
}
@@ -1172,75 +1172,6 @@ void Slam::Localization()
}
}
-//-----------------------------------------------------------------------------
-void Slam::EstimateOverlap(const std::map& mapKdTrees)
-{
- // Init transformed cloud
- PointCloud::Ptr transformedCloud(new PointCloud);
- int currentIdx = 0;
-
- // Transform each input point to BASE coordinates
- for (const PointCloud::Ptr& frame: this->CurrentFrames)
- {
- *transformedCloud += *frame;
- // Get LiDAR device id
- int lidarDevice = frame->front().device_id;
- // Get LiDAR to BASE transform
- Eigen::Isometry3d baseToLidar = this->GetBaseToLidarOffset(lidarDevice);
-
- // If distortion is enabled, distort all input points
- if (this->Undistortion)
- {
- // Extrapolate first and last poses with constant velocity model
- Eigen::Isometry3d worldToBaseBegin = this->InterpolateScanPose(this->WithinFrameMotion.GetTime0());
- Eigen::Isometry3d worldToBaseEnd = this->InterpolateScanPose(this->WithinFrameMotion.GetTime1());
- // Init the interpolator to transform all points
- auto transformInterpolator = this->WithinFrameMotion;
- transformInterpolator.SetTransforms(worldToBaseBegin, worldToBaseEnd);
- // Get time offset of current scan input relatively to device 0 scan
- double timeOffset = Utils::PclStampToSec(frame->header.stamp) - Utils::PclStampToSec(this->CurrentFrames[0]->header.stamp);
-
- // Transform all points taking into account the points' timestamps
- #pragma omp parallel for num_threads(this->NbThreads)
- for (int idxPt = currentIdx; idxPt < currentIdx + frame->size(); ++idxPt)
- Utils::TransformPoint(transformedCloud->at(idxPt), transformInterpolator(transformedCloud->at(idxPt).time + timeOffset) * baseToLidar);
- }
- else
- {
- // Transform all points without taking into account the points' timestamps
- // they are supposed to have been acquired at the same time
- #pragma omp parallel for num_threads(this->NbThreads)
- for (int idxPt = currentIdx; idxPt < currentIdx + frame->size(); ++idxPt)
- Utils::TransformPoint(transformedCloud->at(idxPt), this->Tworld * baseToLidar);
- }
- currentIdx += frame->size();
- }
-
- PointCloud::Ptr sampledTransformedCloud;
- // Uniform sampling cloud
- if (this->OverlapSamplingLeafSize > 1e-3)
- {
- sampledTransformedCloud.reset(new PointCloud);
- pcl::VoxelGrid uniFilter;
- uniFilter.setInputCloud(transformedCloud);
- uniFilter.setLeafSize(this->OverlapSamplingLeafSize, this->OverlapSamplingLeafSize, this->OverlapSamplingLeafSize);
- uniFilter.filter(*sampledTransformedCloud);
- }
- else
- sampledTransformedCloud = transformedCloud;
-
- std::map leafSizes;
- for (auto k : KeypointTypes)
- {
- if (this->UseKeypoints[k])
- leafSizes[k] = this->LocalMaps[k]->GetLeafSize();
- }
-
- // Compute LCP like estimator (see http://geometry.cs.ucl.ac.uk/projects/2014/super4PCS/ for more info)
- this->OverlapEstimation = Confidence::LCPEstimator(sampledTransformedCloud, mapKdTrees, leafSizes, this->NbThreads);
- PRINT_VERBOSE(3, "Overlap : " << this->OverlapEstimation << ", estimated on : " << sampledTransformedCloud->size() << " points.");
-}
-
//-----------------------------------------------------------------------------
void Slam::UpdateMapsUsingTworld()
{
@@ -1428,6 +1359,75 @@ void Slam::RefineUndistortion()
}
}
+//==============================================================================
+// Confidence estimators
+//==============================================================================
+
+//-----------------------------------------------------------------------------
+void Slam::EstimateOverlap(const std::map& mapKdTrees)
+{
+ // Init transformed cloud
+ PointCloud::Ptr transformedCloud(new PointCloud);
+ int currentIdx = 0;
+
+ // Transform each input point to BASE coordinates
+ for (const PointCloud::Ptr& frame: this->CurrentFrames)
+ {
+ *transformedCloud += *frame;
+ // Get LiDAR device id
+ int lidarDevice = frame->front().device_id;
+ // Get LiDAR to BASE transform
+ Eigen::Isometry3d baseToLidar = this->GetBaseToLidarOffset(lidarDevice);
+ int nbPoints = frame->size();
+ // If undistortion is enabled, undistort all input points
+ if (this->Undistortion)
+ {
+ // Extrapolate first and last poses with constant velocity model
+ Eigen::Isometry3d worldToBaseBegin = this->InterpolateScanPose(this->WithinFrameMotion.GetTime0());
+ Eigen::Isometry3d worldToBaseEnd = this->InterpolateScanPose(this->WithinFrameMotion.GetTime1());
+ // Init the interpolator to transform all points
+ auto transformInterpolator = this->WithinFrameMotion;
+ transformInterpolator.SetTransforms(worldToBaseBegin, worldToBaseEnd);
+ // Get time offset of current scan input relatively to device 0 scan
+ double timeOffset = Utils::PclStampToSec(frame->header.stamp) - Utils::PclStampToSec(this->CurrentFrames[0]->header.stamp);
+
+ // Transform all points taking into account the points' timestamps
+ #pragma omp parallel for num_threads(this->NbThreads)
+ for (int idxPt = currentIdx; idxPt < currentIdx + nbPoints; ++idxPt)
+ Utils::TransformPoint(transformedCloud->at(idxPt), transformInterpolator(transformedCloud->at(idxPt).time + timeOffset) * baseToLidar);
+ }
+ else
+ {
+ // Transform all points without taking into account the points' timestamps
+ // they are supposed to have been acquired at the same time
+ #pragma omp parallel for num_threads(this->NbThreads)
+ for (int idxPt = currentIdx; idxPt < currentIdx + nbPoints; ++idxPt)
+ Utils::TransformPoint(transformedCloud->at(idxPt), this->Tworld * baseToLidar);
+ }
+ currentIdx += nbPoints;
+ }
+
+ // Uniform sampling cloud
+ if (this->OverlapSamplingLeafSize > 1e-3)
+ {
+ pcl::VoxelGrid uniFilter;
+ uniFilter.setInputCloud(transformedCloud);
+ uniFilter.setLeafSize(this->OverlapSamplingLeafSize, this->OverlapSamplingLeafSize, this->OverlapSamplingLeafSize);
+ uniFilter.filter(*transformedCloud);
+ }
+
+ std::map leafSizes;
+ for (auto k : KeypointTypes)
+ {
+ if (this->UseKeypoints[k])
+ leafSizes[k] = this->LocalMaps[k]->GetLeafSize();
+ }
+
+ // Compute LCP like estimator (see http://geometry.cs.ucl.ac.uk/projects/2014/super4PCS/ for more info)
+ this->OverlapEstimation = Confidence::LCPEstimator(transformedCloud, mapKdTrees, leafSizes, this->NbThreads);
+ PRINT_VERBOSE(3, "Overlap : " << this->OverlapEstimation << ", estimated on : " << transformedCloud->size() << " points.");
+}
+
//==============================================================================
// Sensor data setting
//==============================================================================