From 56f8ccb63f46fd1b96474853379e816acbf5e477 Mon Sep 17 00:00:00 2001 From: "julia.sanchez" Date: Thu, 3 Jun 2021 19:56:43 +0200 Subject: [PATCH 1/8] [fix] Fix SegFault in overlap estimation * The blobs kdtree is created but not filled, this causes a problem when accessing leafSizes[blob] which does not exist --- slam_lib/src/ConfidenceEstimators.cxx | 37 ++++++++++++++++----------- 1 file changed, 22 insertions(+), 15 deletions(-) diff --git a/slam_lib/src/ConfidenceEstimators.cxx b/slam_lib/src/ConfidenceEstimators.cxx index a4a85a33..11e931fd 100644 --- a/slam_lib/src/ConfidenceEstimators.cxx +++ b/slam_lib/src/ConfidenceEstimators.cxx @@ -28,27 +28,34 @@ 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) + #pragma omp parallel for reduction(+:LCP) num_threads(nbThreads) + for (int n = 0; n < nbPoints; ++n) { - 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()) + for (auto& kdTree : kdTrees) { - // 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; + // Check if a kdtree was filled for this keypoint type + if (kdTree.second.GetInputCloud()->size() > 0) + { + 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()) + { + // 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; + } + } } } + LCP /= nbPoints; } - LCP /= nbPoints; return LCP; } -- GitLab From 4cc784324236b65ea899facabccf13e66f560c5a Mon Sep 17 00:00:00 2001 From: "julia.sanchez" Date: Thu, 3 Jun 2021 19:58:21 +0200 Subject: [PATCH 2/8] [fix] Remove warnings --- slam_lib/src/Slam.cxx | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/slam_lib/src/Slam.cxx b/slam_lib/src/Slam.cxx index 6e9a2d96..35805050 100644 --- a/slam_lib/src/Slam.cxx +++ b/slam_lib/src/Slam.cxx @@ -1187,8 +1187,8 @@ void Slam::EstimateOverlap(const std::map& mapKdTrees) 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 + int nbPoints = frame->size(); + // If undistortion is enabled, undistort all input points if (this->Undistortion) { // Extrapolate first and last poses with constant velocity model @@ -1202,7 +1202,7 @@ void Slam::EstimateOverlap(const std::map& mapKdTrees) // 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) + for (int idxPt = currentIdx; idxPt < currentIdx + nbPoints; ++idxPt) Utils::TransformPoint(transformedCloud->at(idxPt), transformInterpolator(transformedCloud->at(idxPt).time + timeOffset) * baseToLidar); } else @@ -1210,10 +1210,10 @@ void Slam::EstimateOverlap(const std::map& mapKdTrees) // 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) + for (int idxPt = currentIdx; idxPt < currentIdx + nbPoints; ++idxPt) Utils::TransformPoint(transformedCloud->at(idxPt), this->Tworld * baseToLidar); } - currentIdx += frame->size(); + currentIdx += nbPoints; } PointCloud::Ptr sampledTransformedCloud; -- GitLab From e5026566ab91f276debe7979c21b5e996b35e6b3 Mon Sep 17 00:00:00 2001 From: "julia.sanchez" Date: Thu, 3 Jun 2021 20:01:39 +0200 Subject: [PATCH 3/8] [refact] Gather confidence estimations in code * Overlap + keypoints number --- slam_lib/include/LidarSlam/Slam.h | 31 ++++--- slam_lib/src/Slam.cxx | 142 +++++++++++++++--------------- 2 files changed, 93 insertions(+), 80 deletions(-) diff --git a/slam_lib/include/LidarSlam/Slam.h b/slam_lib/include/LidarSlam/Slam.h index 62b6d1c9..4958f9eb 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/Slam.cxx b/slam_lib/src/Slam.cxx index 35805050..5e1b694d 100644 --- a/slam_lib/src/Slam.cxx +++ b/slam_lib/src/Slam.cxx @@ -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); - 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; - } - - 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,79 @@ 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; + } + + 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."); +} + //============================================================================== // Sensor data setting //============================================================================== -- GitLab From f0721056b6ab6595f4594671bac6da12681dcd02 Mon Sep 17 00:00:00 2001 From: "julia.sanchez" Date: Thu, 3 Jun 2021 20:22:34 +0200 Subject: [PATCH 4/8] [feat][PV] Add overlap estimation parameters to PV wrapping * Make them visible and active overlap computation if advanced arrays are enabled * Rename PV overlap output --- paraview_wrapping/Slam.xml | 25 +++++++++++++++++++++++++ paraview_wrapping/vtkSlam.cxx | 9 +++++++++ paraview_wrapping/vtkSlam.h | 7 +++++++ slam_lib/src/Slam.cxx | 2 +- 4 files changed, 42 insertions(+), 1 deletion(-) diff --git a/paraview_wrapping/Slam.xml b/paraview_wrapping/Slam.xml index 2bab6810..83ea626d 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 14b8db80..3650f8f2 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 19e02e69..92935f0d 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/slam_lib/src/Slam.cxx b/slam_lib/src/Slam.cxx index 5e1b694d..79363434 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; } -- GitLab From c69ceeb4fb63041ea41e2cb25bc397cb79de7dad Mon Sep 17 00:00:00 2001 From: "julia.sanchez" Date: Fri, 4 Jun 2021 16:49:09 +0200 Subject: [PATCH 5/8] [fix] Fix major issue in overlap computation * Computation was only performed for the edges map * The points lying farther than the leafsize from their nearest neighbor are considered not to have a match * Good values are >0.5, bad values are <0.4 --- slam_lib/src/ConfidenceEstimators.cxx | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/slam_lib/src/ConfidenceEstimators.cxx b/slam_lib/src/ConfidenceEstimators.cxx index 11e931fd..cf23c1b1 100644 --- a/slam_lib/src/ConfidenceEstimators.cxx +++ b/slam_lib/src/ConfidenceEstimators.cxx @@ -17,6 +17,7 @@ //============================================================================== #include +#include namespace LidarSlam { @@ -30,7 +31,8 @@ float LCPEstimator(PointCloud::ConstPtr cloud, const std::map& int nbPoints = cloud->size(); if (nbPoints > 0) { - #pragma omp parallel for reduction(+:LCP) num_threads(nbThreads) + std::vector LCPvec(nbPoints, 0.f); + #pragma omp parallel for num_threads(nbThreads) for (int n = 0; n < nbPoints; ++n) { for (auto& kdTree : kdTrees) @@ -40,21 +42,21 @@ float LCPEstimator(PointCloud::ConstPtr cloud, const std::map& { 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.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.6 if the distance is g.t. the leaf size - LCP += std::exp( -knnSqDist[0] / (2.f * sqLCPThreshold) ); - break; + // Probability < 0.011 if the distance is g.t. the leaf size + float sqLCPThreshold = std::pow(leafSizes.at(kdTree.first) / 3.f, 2); + float currentProba = std::exp( -knnSqDist[0] / (2.f * sqLCPThreshold) ); + if (currentProba > LCPvec[n]) + LCPvec[n] = currentProba; } } } } - LCP /= nbPoints; + LCP = std::accumulate(LCPvec.begin(), LCPvec.end(), 0.f) / nbPoints; } return LCP; } -- GitLab From fcd83c38ae9a7bdd530f8247dac5b23aefad8f83 Mon Sep 17 00:00:00 2001 From: "julia.sanchez" Date: Fri, 4 Jun 2021 17:16:51 +0200 Subject: [PATCH 6/8] [refact] Overlap -> check if kpt type is usable outside loop --- slam_lib/src/ConfidenceEstimators.cxx | 36 +++++++++++++++------------ 1 file changed, 20 insertions(+), 16 deletions(-) diff --git a/slam_lib/src/ConfidenceEstimators.cxx b/slam_lib/src/ConfidenceEstimators.cxx index cf23c1b1..a61a00cf 100644 --- a/slam_lib/src/ConfidenceEstimators.cxx +++ b/slam_lib/src/ConfidenceEstimators.cxx @@ -31,28 +31,32 @@ float LCPEstimator(PointCloud::ConstPtr cloud, const std::map& int nbPoints = cloud->size(); if (nbPoints > 0) { + // Get only usable keypoint types + std::vector kpToUse; + for (const auto& kdTree : kdTrees) + { + 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 (auto& kdTree : kdTrees) + for (const auto& k : kpToUse) { - // Check if a kdtree was filled for this keypoint type - if (kdTree.second.GetInputCloud()->size() > 0) + std::vector knnIndices; + std::vector knnSqDist; + if (kdTrees.at(k).KnnSearch(cloud->at(n), 1, knnIndices, knnSqDist) > 0) { - std::vector knnIndices; - std::vector knnSqDist; - if (kdTree.second.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(kdTree.first) / 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(leafSizes.at(k) / 3.f, 2); + float currentProba = std::exp( -knnSqDist[0] / (2.f * sqLCPThreshold) ); + if (currentProba > LCPvec[n]) + LCPvec[n] = currentProba; } } } -- GitLab From bac90420416148144157b348492e8cdf4c612750 Mon Sep 17 00:00:00 2001 From: "julia.sanchez" Date: Fri, 4 Jun 2021 17:23:06 +0200 Subject: [PATCH 7/8] [refact][style] Overlap -> remove intermediate useless cloud --- slam_lib/src/Slam.cxx | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/slam_lib/src/Slam.cxx b/slam_lib/src/Slam.cxx index 79363434..f07c2472 100644 --- a/slam_lib/src/Slam.cxx +++ b/slam_lib/src/Slam.cxx @@ -1407,18 +1407,14 @@ void Slam::EstimateOverlap(const std::map& mapKdTrees) currentIdx += nbPoints; } - 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); + uniFilter.filter(*transformedCloud); } - else - sampledTransformedCloud = transformedCloud; std::map leafSizes; for (auto k : KeypointTypes) @@ -1428,8 +1424,8 @@ void Slam::EstimateOverlap(const std::map& mapKdTrees) } // 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."); + this->OverlapEstimation = Confidence::LCPEstimator(transformedCloud, mapKdTrees, leafSizes, this->NbThreads); + PRINT_VERBOSE(3, "Overlap : " << this->OverlapEstimation << ", estimated on : " << transformedCloud->size() << " points."); } //============================================================================== -- GitLab From 155fd35055746675bff4b4d7f13fb7a7e5dad870 Mon Sep 17 00:00:00 2001 From: "julia.sanchez" Date: Mon, 7 Jun 2021 15:17:44 +0200 Subject: [PATCH 8/8] [doc][ROS] Update description of overlap sampling --- ros_wrapping/lidar_slam/params/slam_config.yaml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ros_wrapping/lidar_slam/params/slam_config.yaml b/ros_wrapping/lidar_slam/params/slam_config.yaml index bc4a310f..1b059e08 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: -- GitLab