From fa29daad6b94e2062ce02e37d9d3e87d51050b36 Mon Sep 17 00:00:00 2001 From: Nicolas Cadart Date: Wed, 22 Jul 2020 16:22:08 +0200 Subject: [PATCH 1/7] Templatize KDTreePCLAdaptor to wrap any Point type --- slam_lib/include/LidarSlam/KDTreePCLAdaptor.h | 27 +++++++++---------- slam_lib/include/LidarSlam/Slam.h | 14 +++++----- slam_lib/src/Slam.cxx | 20 +++++++------- 3 files changed, 29 insertions(+), 32 deletions(-) diff --git a/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h b/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h index 497ca202..ac35450a 100644 --- a/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h +++ b/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h @@ -19,32 +19,29 @@ #ifndef KDTREE_PCL_ADAPTOR_H #define KDTREE_PCL_ADAPTOR_H -// LOCAL -#include "LidarSlam/LidarPoint.h" -// NANOFLANN #include -// PCL #include -#include -// BOOST -#include +template class KDTreePCLAdaptor { - using Point = PointXYZTIId; - typedef typename nanoflann::metric_L2::template traits::distance_t metric_t; - typedef nanoflann::KDTreeSingleIndexAdaptor index_t; + using Point = PointT; + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + + using metric_t = typename nanoflann::metric_L2::template traits>::distance_t; + using index_t = nanoflann::KDTreeSingleIndexAdaptor, 3, int>; public: KDTreePCLAdaptor() = default; - KDTreePCLAdaptor(pcl::PointCloud::Ptr cloud) + KDTreePCLAdaptor(PointCloudPtr cloud) { this->Reset(cloud); } - void Reset(pcl::PointCloud::Ptr cloud) + void Reset(PointCloudPtr cloud) { // copy the input cloud this->Cloud = cloud; @@ -101,7 +98,7 @@ public: return this->Cloud->points[idx].z; } - inline pcl::PointCloud::Ptr getInputCloud() const + inline PointCloudPtr getInputCloud() const { return this->Cloud; } @@ -120,8 +117,8 @@ protected: //! The kd-tree index for the user to call its methods as usual with any other FLANN index. std::unique_ptr Index; - //! the inputed data - pcl::PointCloud::Ptr Cloud; + //! The input data + PointCloudPtr Cloud; }; #endif // KDTREE_PCL_ADAPTOR_H \ No newline at end of file diff --git a/slam_lib/include/LidarSlam/Slam.h b/slam_lib/include/LidarSlam/Slam.h index bb88b497..3d4d9c9e 100644 --- a/slam_lib/include/LidarSlam/Slam.h +++ b/slam_lib/include/LidarSlam/Slam.h @@ -688,19 +688,19 @@ private: // (R * X + T - P).t * A * (R * X + T - P) // Where P is the mean point of the neighborhood and A is the symmetric // variance-covariance matrix encoding the shape of the neighborhood - MatchingResult ComputeLineDistanceParameters(KDTreePCLAdaptor& kdtreePreviousEdges, const Point& p, MatchingMode matchingMode); - MatchingResult ComputePlaneDistanceParameters(KDTreePCLAdaptor& kdtreePreviousPlanes, const Point& p, MatchingMode matchingMode); - MatchingResult ComputeBlobsDistanceParameters(KDTreePCLAdaptor& kdtreePreviousBlobs, const Point& p, MatchingMode matchingMode); + MatchingResult ComputeLineDistanceParameters(const KDTreePCLAdaptor& kdtreePreviousEdges, const Point& p, MatchingMode matchingMode); + MatchingResult ComputePlaneDistanceParameters(const KDTreePCLAdaptor& kdtreePreviousPlanes, const Point& p, MatchingMode matchingMode); + MatchingResult ComputeBlobsDistanceParameters(const KDTreePCLAdaptor& kdtreePreviousBlobs, const Point& p, MatchingMode matchingMode); // Instead of taking the k-nearest neigbors in the odometry step we will take // specific neighbor using the particularities of the lidar sensor - void GetEgoMotionLineSpecificNeighbor(std::vector& nearestValid, std::vector& nearestValidDist, - unsigned int nearestSearch, KDTreePCLAdaptor& kdtreePreviousEdges, const double pos[3]) const; + void GetEgoMotionLineSpecificNeighbor(std::vector& nearestValid, std::vector& nearestValidDist, unsigned int nearestSearch, + const KDTreePCLAdaptor& kdtreePreviousEdges, const double pos[3]) const; // Instead of taking the k-nearest neighbors in the localization // step we will take specific neighbor using a sample consensus model - void GetLocalizationLineSpecificNeighbor(std::vector& nearestValid, std::vector& nearestValidDist, double maxDistInlier, - unsigned int nearestSearch, KDTreePCLAdaptor& kdtreePreviousEdges, const double pos[3]) const; + void GetLocalizationLineSpecificNeighbor(std::vector& nearestValid, std::vector& nearestValidDist, double maxDistInlier, unsigned int nearestSearch, + const KDTreePCLAdaptor& kdtreePreviousEdges, const double pos[3]) const; void ResetDistanceParameters(); diff --git a/slam_lib/src/Slam.cxx b/slam_lib/src/Slam.cxx index 5013b593..0aca526b 100644 --- a/slam_lib/src/Slam.cxx +++ b/slam_lib/src/Slam.cxx @@ -771,7 +771,7 @@ void Slam::ComputeEgoMotion() // 4. with OpenMP used in other parts but removing here parallel section : time = 2. ??? // => Even if we don't use OpenMP, it is slower ! We expect (4) to behaves at similarly as (1) or (2)... IF_VERBOSE(3, InitTime("EgoMotion : build KD tree")); - KDTreePCLAdaptor kdtreePreviousEdges, kdtreePreviousPlanes; + KDTreePCLAdaptor kdtreePreviousEdges, kdtreePreviousPlanes; #pragma omp parallel sections num_threads(std::min(this->NbThreads, 2)) { #pragma omp section @@ -921,9 +921,9 @@ void Slam::Localization() // Get keypoints from maps and build kd-trees for fast nearest neighbors search IF_VERBOSE(3, InitTime("Localization : keypoints extraction")); PointCloud::Ptr subEdgesPointsLocalMap, subPlanarPointsLocalMap, subBlobPointsLocalMap(new PointCloud); - KDTreePCLAdaptor kdtreeEdges, kdtreePlanes, kdtreeBlobs; + KDTreePCLAdaptor kdtreeEdges, kdtreePlanes, kdtreeBlobs; - auto extractMapKeypointsAndBuildKdTree = [this](const RollingGrid& map, PointCloud::Ptr& keypoints, KDTreePCLAdaptor& kdTree) + auto extractMapKeypointsAndBuildKdTree = [this](const RollingGrid& map, PointCloud::Ptr& keypoints, KDTreePCLAdaptor& kdTree) { keypoints = map.Get(this->Tworld.translation()); kdTree.Reset(keypoints); @@ -1262,7 +1262,7 @@ void Slam::ComputePointInitAndFinalPose(MatchingMode matchingMode, const Point& } //----------------------------------------------------------------------------- -Slam::MatchingResult Slam::ComputeLineDistanceParameters(KDTreePCLAdaptor& kdtreePreviousEdges, const Point& p, MatchingMode matchingMode) +Slam::MatchingResult Slam::ComputeLineDistanceParameters(const KDTreePCLAdaptor& kdtreePreviousEdges, const Point& p, MatchingMode matchingMode) { // ===================================================== // Transform the point using the current pose estimation @@ -1403,7 +1403,7 @@ Slam::MatchingResult Slam::ComputeLineDistanceParameters(KDTreePCLAdaptor& kdtre } //----------------------------------------------------------------------------- -Slam::MatchingResult Slam::ComputePlaneDistanceParameters(KDTreePCLAdaptor& kdtreePreviousPlanes, const Point& p, MatchingMode matchingMode) +Slam::MatchingResult Slam::ComputePlaneDistanceParameters(const KDTreePCLAdaptor& kdtreePreviousPlanes, const Point& p, MatchingMode matchingMode) { // ===================================================== // Transform the point using the current pose estimation @@ -1535,7 +1535,7 @@ Slam::MatchingResult Slam::ComputePlaneDistanceParameters(KDTreePCLAdaptor& kdtr } //----------------------------------------------------------------------------- -Slam::MatchingResult Slam::ComputeBlobsDistanceParameters(KDTreePCLAdaptor& kdtreePreviousBlobs, const Point& p, MatchingMode matchingMode) +Slam::MatchingResult Slam::ComputeBlobsDistanceParameters(const KDTreePCLAdaptor& kdtreePreviousBlobs, const Point& p, MatchingMode matchingMode) { // ===================================================== // Transform the point using the current pose estimation @@ -1652,8 +1652,8 @@ Slam::MatchingResult Slam::ComputeBlobsDistanceParameters(KDTreePCLAdaptor& kdtr } //----------------------------------------------------------------------------- -void Slam::GetEgoMotionLineSpecificNeighbor(std::vector& nearestValid, std::vector& nearestValidDist, - unsigned int nearestSearch, KDTreePCLAdaptor& kdtreePreviousEdges, const double pos[3]) const +void Slam::GetEgoMotionLineSpecificNeighbor(std::vector& nearestValid, std::vector& nearestValidDist, unsigned int nearestSearch, + const KDTreePCLAdaptor& kdtreePreviousEdges, const double pos[3]) const { // Clear vector nearestValid.clear(); @@ -1704,8 +1704,8 @@ void Slam::GetEgoMotionLineSpecificNeighbor(std::vector& nearestValid, std: } //----------------------------------------------------------------------------- -void Slam::GetLocalizationLineSpecificNeighbor(std::vector& nearestValid, std::vector& nearestValidDist, double maxDistInlier, - unsigned int nearestSearch, KDTreePCLAdaptor& kdtreePreviousEdges, const double pos[3]) const +void Slam::GetLocalizationLineSpecificNeighbor(std::vector& nearestValid, std::vector& nearestValidDist, double maxDistInlier, unsigned int nearestSearch, + const KDTreePCLAdaptor& kdtreePreviousEdges, const double pos[3]) const { // reset vectors nearestValid.clear(); -- GitLab From 5afad1f3e800e2b84767743c0f18f303fafdfb73 Mon Sep 17 00:00:00 2001 From: Nicolas Cadart Date: Wed, 22 Jul 2020 17:14:03 +0200 Subject: [PATCH 2/7] Replace KDTreePCLAdaptor::query() by knnSearch for easier use Use index_t::knnSearch method which already wraps the NN search. Return the number of neighbors found. Reserve arrays instead of resizing them to avoid unnecessary init. --- slam_lib/include/LidarSlam/KDTreePCLAdaptor.h | 36 ++++++++-------- slam_lib/src/Slam.cxx | 42 +++++++------------ 2 files changed, 33 insertions(+), 45 deletions(-) diff --git a/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h b/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h index ac35450a..d343c8c7 100644 --- a/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h +++ b/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h @@ -1,6 +1,7 @@ //============================================================================== // Copyright 2019-2020 Kitware, Inc., Kitware SAS // Author: Guilbert Pierre (Kitware SAS) +// Nicolas Cadart (Kitware SAS) // Creation date: 2018-04-19 // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -52,23 +53,29 @@ public: Index->buildIndex(); } - /** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]). - * Note that this is a short-cut method for index->findNeighbors(). - * The user can also call index->... methods as desired. + /** + * Find the \a knearest closest neighbors points to the \a query_point[0:dim-1]. + * Their indices are stored inside the result object. + * \sa radiusSearch, findNeighbors + * + * \return Number `N` of valid points in the result set. + * Only the first `N` entries in `out_indices` and `out_distances_sq` will be valid. + * Return may be less than `num_closest` only if the number of elements in + * the tree is less than `num_closest`. + * * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface. + * + * \note Note that this is a short-cut method for index->findNeighbors(). + * The user can also call index->... methods as desired. */ - inline void query(const Point& query_point, int knearest, int* out_indices, double* out_distances_sq/*, const int nChecks_IGNORED = 10*/) const + inline size_t knnSearch(const Point& query_point, int knearest, int* out_indices, double* out_distances_sq/*, const int nChecks_IGNORED = 10*/) const { double pt[3] = {query_point.x, query_point.y, query_point.z}; - nanoflann::KNNResultSet resultSet(knearest); - resultSet.init(out_indices, out_distances_sq); - this->Index->findNeighbors(resultSet, pt, nanoflann::SearchParams()); + return this->Index->knnSearch(pt, knearest, out_indices, out_distances_sq); } - inline void query(const double query_point[3], int knearest, int* out_indices, double* out_distances_sq/*, const int nChecks_IGNORED = 10*/) const + inline size_t knnSearch(const double query_point[3], int knearest, int* out_indices, double* out_distances_sq/*, const int nChecks_IGNORED = 10*/) const { - nanoflann::KNNResultSet resultSet(knearest); - resultSet.init(out_indices, out_distances_sq); - this->Index->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); + return this->Index->knnSearch(query_point, knearest, out_indices, out_distances_sq); } inline const KDTreePCLAdaptor& derived() const @@ -90,12 +97,7 @@ public: // Returns the dim'th component of the idx'th point in the class: inline double kdtree_get_pt(const int idx, const int dim) const { - if (dim == 0) - return this->Cloud->points[idx].x; - else if (dim == 1) - return this->Cloud->points[idx].y; - else - return this->Cloud->points[idx].z; + return this->Cloud->points[idx].data[dim]; } inline PointCloudPtr getInputCloud() const diff --git a/slam_lib/src/Slam.cxx b/slam_lib/src/Slam.cxx index 0aca526b..af22be59 100644 --- a/slam_lib/src/Slam.cxx +++ b/slam_lib/src/Slam.cxx @@ -1439,12 +1439,12 @@ Slam::MatchingResult Slam::ComputePlaneDistanceParameters(const KDTreePCLAdaptor throw "ComputeLineDistanceParameters function got invalide step parameter"; } - std::vector nearestIndex(requiredNearest, -1); - std::vector nearestDist(requiredNearest, -1.0); - kdtreePreviousPlanes.query(pFinal.data(), requiredNearest, nearestIndex.data(), nearestDist.data()); + std::vector nearestIndex(requiredNearest); + std::vector nearestDist(requiredNearest); + unsigned int neighborhoodSize = kdtreePreviousPlanes.knnSearch(pFinal.data(), requiredNearest, nearestIndex.data(), nearestDist.data()); // It means that there is not enough keypoints in the neighborhood - if (nearestIndex.back() == -1) + if (neighborhoodSize < requiredNearest) { return MatchingResult::NOT_ENOUGH_NEIGHBORS; } @@ -1552,12 +1552,12 @@ Slam::MatchingResult Slam::ComputeBlobsDistanceParameters(const KDTreePCLAdaptor double maxDist = this->MaxDistanceForICPMatching; //< maximum distance between keypoints and its neighbors float maxDiameter = 4.; - std::vector nearestIndex(requiredNearest, -1); - std::vector nearestDist(requiredNearest, -1.0); - kdtreePreviousBlobs.query(pFinal.data(), requiredNearest, nearestIndex.data(), nearestDist.data()); + std::vector nearestIndex(requiredNearest); + std::vector nearestDist(requiredNearest); + unsigned int neighborhoodSize = kdtreePreviousBlobs.knnSearch(pFinal.data(), requiredNearest, nearestIndex.data(), nearestDist.data()); // It means that there is not enough keypoints in the neighborhood - if (nearestIndex.back() == -1) + if (neighborhoodSize < requiredNearest) { return MatchingResult::NOT_ENOUGH_NEIGHBORS; } @@ -1660,16 +1660,9 @@ void Slam::GetEgoMotionLineSpecificNeighbor(std::vector& nearestValid, std: nearestValidDist.clear(); // Get nearest neighbors of the query point - std::vector nearestIndex(nearestSearch, -1); - std::vector nearestDist(nearestSearch, -1.0); - kdtreePreviousEdges.query(pos, nearestSearch, nearestIndex.data(), nearestDist.data()); - - // Check neighborhood validity - unsigned int neighborhoodSize = nearestIndex.size(); - while (nearestIndex[neighborhoodSize - 1] == -1 && neighborhoodSize > 1) - { - --neighborhoodSize; - } + std::vector nearestIndex; nearestIndex.reserve(nearestSearch); + std::vector nearestDist; nearestDist.reserve(nearestSearch); + unsigned int neighborhoodSize = kdtreePreviousEdges.knnSearch(pos, nearestSearch, nearestIndex.data(), nearestDist.data()); // Shortcut to keypoints cloud const PointCloud& previousEdgesPoints = *kdtreePreviousEdges.getInputCloud(); @@ -1712,16 +1705,9 @@ void Slam::GetLocalizationLineSpecificNeighbor(std::vector& nearestValid, s nearestValidDist.clear(); // Get nearest neighbors of the query point - std::vector nearestIndex(nearestSearch, -1); - std::vector nearestDist(nearestSearch, -1.0); - kdtreePreviousEdges.query(pos, nearestSearch, nearestIndex.data(), nearestDist.data()); - - // Check neighborhood validity - unsigned int neighborhoodSize = nearestIndex.size(); - while (nearestIndex[neighborhoodSize - 1] == -1 && neighborhoodSize > 1) - { - --neighborhoodSize; - } + std::vector nearestIndex; nearestIndex.reserve(nearestSearch); + std::vector nearestDist; nearestDist.reserve(nearestSearch); + unsigned int neighborhoodSize = kdtreePreviousEdges.knnSearch(pos, nearestSearch, nearestIndex.data(), nearestDist.data()); // Shortcut to keypoints cloud const PointCloud& previousEdgesPoints = *kdtreePreviousEdges.getInputCloud(); -- GitLab From d61f089e705bcc12980b4123924aeef992050dda Mon Sep 17 00:00:00 2001 From: Nicolas Cadart Date: Wed, 22 Jul 2020 17:49:49 +0200 Subject: [PATCH 3/7] Use nanoflann::metric_L2_Simple instead of metric_L2 metric_L2_Simple is optimized for small dimensions datasets, such as 2D or 3D pointclouds. metric_L2's behavior is much more complex and thus slower. --- slam_lib/include/LidarSlam/KDTreePCLAdaptor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h b/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h index d343c8c7..01937f6e 100644 --- a/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h +++ b/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h @@ -30,7 +30,7 @@ class KDTreePCLAdaptor using PointCloud = pcl::PointCloud; using PointCloudPtr = typename PointCloud::Ptr; - using metric_t = typename nanoflann::metric_L2::template traits>::distance_t; + using metric_t = typename nanoflann::metric_L2_Simple::template traits>::distance_t; using index_t = nanoflann::KDTreeSingleIndexAdaptor, 3, int>; public: -- GitLab From ed9cd4948783014edaa7247fb0696289db226ff4 Mon Sep 17 00:00:00 2001 From: Nicolas Cadart Date: Thu, 23 Jul 2020 10:28:33 +0200 Subject: [PATCH 4/7] Add optional leafMaxSize setting, and set default to 16 leafMaxSize can be usefuly tuned to fit application (depending on tree building / NN queries tradeoff). Setting default leafMaxSize value from 25 to 16 speeds up NN queries, but slows down a bit KD tree build duration. As for most cases NN queries is more important than KD tree building, this lead to global improvement. --- slam_lib/include/LidarSlam/KDTreePCLAdaptor.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h b/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h index 01937f6e..50d5f5aa 100644 --- a/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h +++ b/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h @@ -37,18 +37,17 @@ public: KDTreePCLAdaptor() = default; - KDTreePCLAdaptor(PointCloudPtr cloud) + KDTreePCLAdaptor(PointCloudPtr cloud, int leafMaxSize = 16) { - this->Reset(cloud); + this->Reset(cloud, leafMaxSize); } - void Reset(PointCloudPtr cloud) + void Reset(PointCloudPtr cloud, int leafMaxSize = 16) { // copy the input cloud this->Cloud = cloud; // Build KD-tree - int leafMaxSize = 25; Index = std::make_unique(3, *this, nanoflann::KDTreeSingleIndexAdaptorParams(leafMaxSize)); Index->buildIndex(); } -- GitLab From 52ae1623ce1a8714e6964ea5f3777c69dd45ac97 Mon Sep 17 00:00:00 2001 From: Nicolas Cadart Date: Thu, 23 Jul 2020 17:01:20 +0200 Subject: [PATCH 5/7] Use float representation instead of double for KDTreePCLAdaptor PCL usesonly floats to encode points coordinates. Therefore, double precision is useless in KDTree, as points are read from PCL structures. Using float increase speed performace and reduce memory usage. --- slam_lib/include/LidarSlam/KDTreePCLAdaptor.h | 17 +++++++++++------ slam_lib/include/LidarSlam/Slam.h | 4 ++-- slam_lib/src/Slam.cxx | 14 +++++++------- 3 files changed, 20 insertions(+), 15 deletions(-) diff --git a/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h b/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h index 50d5f5aa..747bd50e 100644 --- a/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h +++ b/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h @@ -30,7 +30,7 @@ class KDTreePCLAdaptor using PointCloud = pcl::PointCloud; using PointCloudPtr = typename PointCloud::Ptr; - using metric_t = typename nanoflann::metric_L2_Simple::template traits>::distance_t; + using metric_t = typename nanoflann::metric_L2_Simple::template traits>::distance_t; using index_t = nanoflann::KDTreeSingleIndexAdaptor, 3, int>; public: @@ -67,15 +67,20 @@ public: * \note Note that this is a short-cut method for index->findNeighbors(). * The user can also call index->... methods as desired. */ - inline size_t knnSearch(const Point& query_point, int knearest, int* out_indices, double* out_distances_sq/*, const int nChecks_IGNORED = 10*/) const + inline size_t knnSearch(const Point& query_point, int knearest, int* out_indices, float* out_distances_sq/*, const int nChecks_IGNORED = 10*/) const { - double pt[3] = {query_point.x, query_point.y, query_point.z}; - return this->Index->knnSearch(pt, knearest, out_indices, out_distances_sq); + return this->Index->knnSearch(query_point.data, knearest, out_indices, out_distances_sq); } - inline size_t knnSearch(const double query_point[3], int knearest, int* out_indices, double* out_distances_sq/*, const int nChecks_IGNORED = 10*/) const + inline size_t knnSearch(const float query_point[3], int knearest, int* out_indices, float* out_distances_sq/*, const int nChecks_IGNORED = 10*/) const { return this->Index->knnSearch(query_point, knearest, out_indices, out_distances_sq); } + inline size_t knnSearch(const double query_point[3], int knearest, int* out_indices, float* out_distances_sq/*, const int nChecks_IGNORED = 10*/) const + { + float pt[3]; + std::copy(query_point, query_point + 3, pt); + return this->Index->knnSearch(pt, knearest, out_indices, out_distances_sq); + } inline const KDTreePCLAdaptor& derived() const { @@ -94,7 +99,7 @@ public: } // Returns the dim'th component of the idx'th point in the class: - inline double kdtree_get_pt(const int idx, const int dim) const + inline float kdtree_get_pt(const int idx, const int dim) const { return this->Cloud->points[idx].data[dim]; } diff --git a/slam_lib/include/LidarSlam/Slam.h b/slam_lib/include/LidarSlam/Slam.h index 3d4d9c9e..47c57850 100644 --- a/slam_lib/include/LidarSlam/Slam.h +++ b/slam_lib/include/LidarSlam/Slam.h @@ -694,12 +694,12 @@ private: // Instead of taking the k-nearest neigbors in the odometry step we will take // specific neighbor using the particularities of the lidar sensor - void GetEgoMotionLineSpecificNeighbor(std::vector& nearestValid, std::vector& nearestValidDist, unsigned int nearestSearch, + void GetEgoMotionLineSpecificNeighbor(std::vector& nearestValid, std::vector& nearestValidDist, unsigned int nearestSearch, const KDTreePCLAdaptor& kdtreePreviousEdges, const double pos[3]) const; // Instead of taking the k-nearest neighbors in the localization // step we will take specific neighbor using a sample consensus model - void GetLocalizationLineSpecificNeighbor(std::vector& nearestValid, std::vector& nearestValidDist, double maxDistInlier, unsigned int nearestSearch, + void GetLocalizationLineSpecificNeighbor(std::vector& nearestValid, std::vector& nearestValidDist, double maxDistInlier, unsigned int nearestSearch, const KDTreePCLAdaptor& kdtreePreviousEdges, const double pos[3]) const; void ResetDistanceParameters(); diff --git a/slam_lib/src/Slam.cxx b/slam_lib/src/Slam.cxx index af22be59..1b13f813 100644 --- a/slam_lib/src/Slam.cxx +++ b/slam_lib/src/Slam.cxx @@ -1280,7 +1280,7 @@ Slam::MatchingResult Slam::ComputeLineDistanceParameters(const KDTreePCLAdaptor< double eigenValuesRatio; //< min eigen values ratio to conbsider a neighborhood as flat double squaredMaxDist; //< maximum distance between keypoints and their computed line std::vector nearestIndex; - std::vector nearestDist; + std::vector nearestDist; if (matchingMode == MatchingMode::EGO_MOTION) { requiredNearest = this->EgoMotionLineDistanceNbrNeighbors; @@ -1440,7 +1440,7 @@ Slam::MatchingResult Slam::ComputePlaneDistanceParameters(const KDTreePCLAdaptor } std::vector nearestIndex(requiredNearest); - std::vector nearestDist(requiredNearest); + std::vector nearestDist(requiredNearest); unsigned int neighborhoodSize = kdtreePreviousPlanes.knnSearch(pFinal.data(), requiredNearest, nearestIndex.data(), nearestDist.data()); // It means that there is not enough keypoints in the neighborhood @@ -1553,7 +1553,7 @@ Slam::MatchingResult Slam::ComputeBlobsDistanceParameters(const KDTreePCLAdaptor float maxDiameter = 4.; std::vector nearestIndex(requiredNearest); - std::vector nearestDist(requiredNearest); + std::vector nearestDist(requiredNearest); unsigned int neighborhoodSize = kdtreePreviousBlobs.knnSearch(pFinal.data(), requiredNearest, nearestIndex.data(), nearestDist.data()); // It means that there is not enough keypoints in the neighborhood @@ -1652,7 +1652,7 @@ Slam::MatchingResult Slam::ComputeBlobsDistanceParameters(const KDTreePCLAdaptor } //----------------------------------------------------------------------------- -void Slam::GetEgoMotionLineSpecificNeighbor(std::vector& nearestValid, std::vector& nearestValidDist, unsigned int nearestSearch, +void Slam::GetEgoMotionLineSpecificNeighbor(std::vector& nearestValid, std::vector& nearestValidDist, unsigned int nearestSearch, const KDTreePCLAdaptor& kdtreePreviousEdges, const double pos[3]) const { // Clear vector @@ -1661,7 +1661,7 @@ void Slam::GetEgoMotionLineSpecificNeighbor(std::vector& nearestValid, std: // Get nearest neighbors of the query point std::vector nearestIndex; nearestIndex.reserve(nearestSearch); - std::vector nearestDist; nearestDist.reserve(nearestSearch); + std::vector nearestDist; nearestDist.reserve(nearestSearch); unsigned int neighborhoodSize = kdtreePreviousEdges.knnSearch(pos, nearestSearch, nearestIndex.data(), nearestDist.data()); // Shortcut to keypoints cloud @@ -1697,7 +1697,7 @@ void Slam::GetEgoMotionLineSpecificNeighbor(std::vector& nearestValid, std: } //----------------------------------------------------------------------------- -void Slam::GetLocalizationLineSpecificNeighbor(std::vector& nearestValid, std::vector& nearestValidDist, double maxDistInlier, unsigned int nearestSearch, +void Slam::GetLocalizationLineSpecificNeighbor(std::vector& nearestValid, std::vector& nearestValidDist, double maxDistInlier, unsigned int nearestSearch, const KDTreePCLAdaptor& kdtreePreviousEdges, const double pos[3]) const { // reset vectors @@ -1706,7 +1706,7 @@ void Slam::GetLocalizationLineSpecificNeighbor(std::vector& nearestValid, s // Get nearest neighbors of the query point std::vector nearestIndex; nearestIndex.reserve(nearestSearch); - std::vector nearestDist; nearestDist.reserve(nearestSearch); + std::vector nearestDist; nearestDist.reserve(nearestSearch); unsigned int neighborhoodSize = kdtreePreviousEdges.knnSearch(pos, nearestSearch, nearestIndex.data(), nearestDist.data()); // Shortcut to keypoints cloud -- GitLab From 18f1e79f778006bd0b8359aeb06a2a4ac67dacad Mon Sep 17 00:00:00 2001 From: Nicolas Cadart Date: Wed, 12 Aug 2020 16:00:20 +0200 Subject: [PATCH 6/7] Use std::vector to store NN search results --- slam_lib/include/LidarSlam/KDTreePCLAdaptor.h | 25 ++++++++++++++----- slam_lib/src/Slam.cxx | 24 +++++++++--------- 2 files changed, 31 insertions(+), 18 deletions(-) diff --git a/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h b/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h index 747bd50e..fd7de0ad 100644 --- a/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h +++ b/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h @@ -67,19 +67,32 @@ public: * \note Note that this is a short-cut method for index->findNeighbors(). * The user can also call index->... methods as desired. */ - inline size_t knnSearch(const Point& query_point, int knearest, int* out_indices, float* out_distances_sq/*, const int nChecks_IGNORED = 10*/) const - { - return this->Index->knnSearch(query_point.data, knearest, out_indices, out_distances_sq); - } inline size_t knnSearch(const float query_point[3], int knearest, int* out_indices, float* out_distances_sq/*, const int nChecks_IGNORED = 10*/) const { return this->Index->knnSearch(query_point, knearest, out_indices, out_distances_sq); } - inline size_t knnSearch(const double query_point[3], int knearest, int* out_indices, float* out_distances_sq/*, const int nChecks_IGNORED = 10*/) const + inline size_t knnSearch(const float query_point[3], int knearest, std::vector& out_indices, std::vector& out_distances_sq/*, const int nChecks_IGNORED = 10*/) const + { + // Init result to have large enough buffers that will be filled by knnSearch + out_indices.resize(knearest); + out_distances_sq.resize(knearest); + // Find nearest neighbors + size_t kneighbors = this->knnSearch(query_point, knearest, out_indices.data(), out_distances_sq.data()); + // If less than 'knearest' NN have been found, the last neighbors values are + // wrong, therefore we need to ignore them + out_indices.resize(kneighbors); + out_distances_sq.resize(kneighbors); + return kneighbors; + } + inline size_t knnSearch(const double query_point[3], int knearest, std::vector& out_indices, std::vector& out_distances_sq/*, const int nChecks_IGNORED = 10*/) const { float pt[3]; std::copy(query_point, query_point + 3, pt); - return this->Index->knnSearch(pt, knearest, out_indices, out_distances_sq); + return this->knnSearch(pt, knearest, out_indices, out_distances_sq); + } + inline size_t knnSearch(const Point& query_point, int knearest, std::vector& out_indices, std::vector& out_distances_sq/*, const int nChecks_IGNORED = 10*/) const + { + return this->knnSearch(query_point.data, knearest, out_indices, out_distances_sq); } inline const KDTreePCLAdaptor& derived() const diff --git a/slam_lib/src/Slam.cxx b/slam_lib/src/Slam.cxx index 1b13f813..5827279d 100644 --- a/slam_lib/src/Slam.cxx +++ b/slam_lib/src/Slam.cxx @@ -1439,9 +1439,9 @@ Slam::MatchingResult Slam::ComputePlaneDistanceParameters(const KDTreePCLAdaptor throw "ComputeLineDistanceParameters function got invalide step parameter"; } - std::vector nearestIndex(requiredNearest); - std::vector nearestDist(requiredNearest); - unsigned int neighborhoodSize = kdtreePreviousPlanes.knnSearch(pFinal.data(), requiredNearest, nearestIndex.data(), nearestDist.data()); + std::vector nearestIndex; + std::vector nearestDist; + unsigned int neighborhoodSize = kdtreePreviousPlanes.knnSearch(pFinal.data(), requiredNearest, nearestIndex, nearestDist); // It means that there is not enough keypoints in the neighborhood if (neighborhoodSize < requiredNearest) @@ -1552,9 +1552,9 @@ Slam::MatchingResult Slam::ComputeBlobsDistanceParameters(const KDTreePCLAdaptor double maxDist = this->MaxDistanceForICPMatching; //< maximum distance between keypoints and its neighbors float maxDiameter = 4.; - std::vector nearestIndex(requiredNearest); - std::vector nearestDist(requiredNearest); - unsigned int neighborhoodSize = kdtreePreviousBlobs.knnSearch(pFinal.data(), requiredNearest, nearestIndex.data(), nearestDist.data()); + std::vector nearestIndex; + std::vector nearestDist; + unsigned int neighborhoodSize = kdtreePreviousBlobs.knnSearch(pFinal.data(), requiredNearest, nearestIndex, nearestDist); // It means that there is not enough keypoints in the neighborhood if (neighborhoodSize < requiredNearest) @@ -1660,9 +1660,9 @@ void Slam::GetEgoMotionLineSpecificNeighbor(std::vector& nearestValid, std: nearestValidDist.clear(); // Get nearest neighbors of the query point - std::vector nearestIndex; nearestIndex.reserve(nearestSearch); - std::vector nearestDist; nearestDist.reserve(nearestSearch); - unsigned int neighborhoodSize = kdtreePreviousEdges.knnSearch(pos, nearestSearch, nearestIndex.data(), nearestDist.data()); + std::vector nearestIndex; + std::vector nearestDist; + unsigned int neighborhoodSize = kdtreePreviousEdges.knnSearch(pos, nearestSearch, nearestIndex, nearestDist); // Shortcut to keypoints cloud const PointCloud& previousEdgesPoints = *kdtreePreviousEdges.getInputCloud(); @@ -1705,9 +1705,9 @@ void Slam::GetLocalizationLineSpecificNeighbor(std::vector& nearestValid, s nearestValidDist.clear(); // Get nearest neighbors of the query point - std::vector nearestIndex; nearestIndex.reserve(nearestSearch); - std::vector nearestDist; nearestDist.reserve(nearestSearch); - unsigned int neighborhoodSize = kdtreePreviousEdges.knnSearch(pos, nearestSearch, nearestIndex.data(), nearestDist.data()); + std::vector nearestIndex; + std::vector nearestDist; + unsigned int neighborhoodSize = kdtreePreviousEdges.knnSearch(pos, nearestSearch, nearestIndex, nearestDist); // Shortcut to keypoints cloud const PointCloud& previousEdgesPoints = *kdtreePreviousEdges.getInputCloud(); -- GitLab From 97c8bc068255a6a3cbe93f764388adaa7aad0845 Mon Sep 17 00:00:00 2001 From: Nicolas Cadart Date: Wed, 12 Aug 2020 17:04:08 +0200 Subject: [PATCH 7/7] Update and clean doc --- slam_lib/include/LidarSlam/KDTreePCLAdaptor.h | 112 +++++++++++------- slam_lib/src/Slam.cxx | 18 +-- 2 files changed, 81 insertions(+), 49 deletions(-) diff --git a/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h b/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h index fd7de0ad..7bf34dc4 100644 --- a/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h +++ b/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h @@ -30,71 +30,93 @@ class KDTreePCLAdaptor using PointCloud = pcl::PointCloud; using PointCloudPtr = typename PointCloud::Ptr; - using metric_t = typename nanoflann::metric_L2_Simple::template traits>::distance_t; + using metric_t = typename nanoflann::metric_L2_Simple::traits>::distance_t; using index_t = nanoflann::KDTreeSingleIndexAdaptor, 3, int>; public: KDTreePCLAdaptor() = default; + /** + * \brief Build a Kd-tree from a given pointcloud. + * \param cloud The pointcloud to encode in the kd-tree. + * \param leafMaxSize The maximum size of a leaf of the tree (refer to + * https://github.com/jlblancoc/nanoflann#21-kdtreesingleindexadaptorparamsleaf_max_size) + */ KDTreePCLAdaptor(PointCloudPtr cloud, int leafMaxSize = 16) { this->Reset(cloud, leafMaxSize); } + /** + * \brief Init the Kd-tree from a given pointcloud. + * \param cloud The pointcloud to encode in the kd-tree. + * \param leafMaxSize The maximum size of a leaf of the tree (refer to + * https://github.com/jlblancoc/nanoflann#21-kdtreesingleindexadaptorparamsleaf_max_size) + */ void Reset(PointCloudPtr cloud, int leafMaxSize = 16) { - // copy the input cloud + // Copy the input cloud this->Cloud = cloud; // Build KD-tree - Index = std::make_unique(3, *this, nanoflann::KDTreeSingleIndexAdaptorParams(leafMaxSize)); - Index->buildIndex(); + this->Index = std::make_unique(3, *this, nanoflann::KDTreeSingleIndexAdaptorParams(leafMaxSize)); + this->Index->buildIndex(); } /** - * Find the \a knearest closest neighbors points to the \a query_point[0:dim-1]. - * Their indices are stored inside the result object. - * \sa radiusSearch, findNeighbors - * - * \return Number `N` of valid points in the result set. - * Only the first `N` entries in `out_indices` and `out_distances_sq` will be valid. - * Return may be less than `num_closest` only if the number of elements in - * the tree is less than `num_closest`. - * - * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface. + * \brief Finds the `K` nearest neighbors points in the KD-tree to a given query point. + * \param[in] queryPoint Input point to look closest neighbors to. + * \param[in] knearest Number of nearest neighbors to find. + * \param[out] knnIndices Indices of the NN. + * \param[out] knnSqDistances Squared distances of the NN to the query point. + * \return Number `N` of neighbors found. * - * \note Note that this is a short-cut method for index->findNeighbors(). - * The user can also call index->... methods as desired. + * \note Only the first `N` entries in `knnIndices` and `knnSqDistances` will + * be valid. Return may be less than `knearest` only if the number of + * elements in the tree is less than `knearest`. */ - inline size_t knnSearch(const float query_point[3], int knearest, int* out_indices, float* out_distances_sq/*, const int nChecks_IGNORED = 10*/) const + inline size_t KnnSearch(const float queryPoint[3], int knearest, int* knnIndices, float* knnSqDistances) const { - return this->Index->knnSearch(query_point, knearest, out_indices, out_distances_sq); + return this->Index->knnSearch(queryPoint, knearest, knnIndices, knnSqDistances); } - inline size_t knnSearch(const float query_point[3], int knearest, std::vector& out_indices, std::vector& out_distances_sq/*, const int nChecks_IGNORED = 10*/) const + inline size_t KnnSearch(const float queryPoint[3], int knearest, std::vector& knnIndices, std::vector& knnSqDistances) const { // Init result to have large enough buffers that will be filled by knnSearch - out_indices.resize(knearest); - out_distances_sq.resize(knearest); + knnIndices.resize(knearest); + knnSqDistances.resize(knearest); // Find nearest neighbors - size_t kneighbors = this->knnSearch(query_point, knearest, out_indices.data(), out_distances_sq.data()); + size_t kneighbors = this->KnnSearch(queryPoint, knearest, knnIndices.data(), knnSqDistances.data()); // If less than 'knearest' NN have been found, the last neighbors values are // wrong, therefore we need to ignore them - out_indices.resize(kneighbors); - out_distances_sq.resize(kneighbors); + knnIndices.resize(kneighbors); + knnSqDistances.resize(kneighbors); return kneighbors; } - inline size_t knnSearch(const double query_point[3], int knearest, std::vector& out_indices, std::vector& out_distances_sq/*, const int nChecks_IGNORED = 10*/) const + inline size_t KnnSearch(const double queryPoint[3], int knearest, std::vector& knnIndices, std::vector& knnSqDistances) const { float pt[3]; - std::copy(query_point, query_point + 3, pt); - return this->knnSearch(pt, knearest, out_indices, out_distances_sq); + std::copy(queryPoint, queryPoint + 3, pt); + return this->KnnSearch(pt, knearest, knnIndices, knnSqDistances); } - inline size_t knnSearch(const Point& query_point, int knearest, std::vector& out_indices, std::vector& out_distances_sq/*, const int nChecks_IGNORED = 10*/) const + inline size_t KnnSearch(const Point& queryPoint, int knearest, std::vector& knnIndices, std::vector& knnSqDistances) const { - return this->knnSearch(query_point.data, knearest, out_indices, out_distances_sq); + return this->KnnSearch(queryPoint.data, knearest, knnIndices, knnSqDistances); + } + + /** + * \brief Get the input pointcloud. + * \return The input pointcloud used to build KD-tree. + */ + inline PointCloudPtr GetInputCloud() const + { + return this->Cloud; } + // --------------------------------------------------------------------------- + // Methods required by nanoflann adaptor design + // --------------------------------------------------------------------------- + inline const KDTreePCLAdaptor& derived() const { return *this; @@ -105,26 +127,36 @@ public: return *this; } - // Must return the number of data points - inline int kdtree_get_point_count() const + /** + * \brief Returns the number of points in the input pointcloud. + * \note This method is required by nanoflann design, and should not be used + * by user. + */ + inline size_t kdtree_get_point_count() const { return this->Cloud->size(); } - // Returns the dim'th component of the idx'th point in the class: + /** + * \brief Returns the dim'th component of the idx'th point of the pointcloud. + * \note `dim` should only be in range [0-3]. + * \note This method is required by nanoflann design, and should not be used + * by user. + */ inline float kdtree_get_pt(const int idx, const int dim) const { return this->Cloud->points[idx].data[dim]; } - inline PointCloudPtr getInputCloud() const - { - return this->Cloud; - } - - // Optional bounding-box computation: return false to default to a standard bbox computation loop. - // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again. - // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds) + /** + * Optional bounding-box computation. + * Return false to default to a standard bbox computation loop. + * Return true if the BBOX was already computed by the class and returned in + * "bb" so it can be avoided to redo it again. + * Look at bb.size() to find out the expected dimensionality. + * \note This method is required by nanoflann design, and should not be used + * by user. + */ template inline bool kdtree_get_bbox(BBOX& /*bb*/) const { diff --git a/slam_lib/src/Slam.cxx b/slam_lib/src/Slam.cxx index 5827279d..6ce1d15b 100644 --- a/slam_lib/src/Slam.cxx +++ b/slam_lib/src/Slam.cxx @@ -1317,7 +1317,7 @@ Slam::MatchingResult Slam::ComputeLineDistanceParameters(const KDTreePCLAdaptor< } // Shortcut to keypoints cloud - const PointCloud& previousEdgesPoints = *kdtreePreviousEdges.getInputCloud(); + const PointCloud& previousEdgesPoints = *kdtreePreviousEdges.GetInputCloud(); // ======================================================= // Check if neighborhood is a good line candidate with PCA @@ -1441,7 +1441,7 @@ Slam::MatchingResult Slam::ComputePlaneDistanceParameters(const KDTreePCLAdaptor std::vector nearestIndex; std::vector nearestDist; - unsigned int neighborhoodSize = kdtreePreviousPlanes.knnSearch(pFinal.data(), requiredNearest, nearestIndex, nearestDist); + unsigned int neighborhoodSize = kdtreePreviousPlanes.KnnSearch(pFinal.data(), requiredNearest, nearestIndex, nearestDist); // It means that there is not enough keypoints in the neighborhood if (neighborhoodSize < requiredNearest) @@ -1457,7 +1457,7 @@ Slam::MatchingResult Slam::ComputePlaneDistanceParameters(const KDTreePCLAdaptor } // Shortcut to keypoints cloud - const PointCloud& previousPlanesPoints = *kdtreePreviousPlanes.getInputCloud(); + const PointCloud& previousPlanesPoints = *kdtreePreviousPlanes.GetInputCloud(); // ======================================================== // Check if neighborhood is a good plane candidate with PCA @@ -1554,7 +1554,7 @@ Slam::MatchingResult Slam::ComputeBlobsDistanceParameters(const KDTreePCLAdaptor std::vector nearestIndex; std::vector nearestDist; - unsigned int neighborhoodSize = kdtreePreviousBlobs.knnSearch(pFinal.data(), requiredNearest, nearestIndex, nearestDist); + unsigned int neighborhoodSize = kdtreePreviousBlobs.KnnSearch(pFinal.data(), requiredNearest, nearestIndex, nearestDist); // It means that there is not enough keypoints in the neighborhood if (neighborhoodSize < requiredNearest) @@ -1570,7 +1570,7 @@ Slam::MatchingResult Slam::ComputeBlobsDistanceParameters(const KDTreePCLAdaptor } // Shortcut to keypoints cloud - const PointCloud& previousBlobsPoints = *kdtreePreviousBlobs.getInputCloud(); + const PointCloud& previousBlobsPoints = *kdtreePreviousBlobs.GetInputCloud(); // ====================================== // Check the diameter of the neighborhood @@ -1662,10 +1662,10 @@ void Slam::GetEgoMotionLineSpecificNeighbor(std::vector& nearestValid, std: // Get nearest neighbors of the query point std::vector nearestIndex; std::vector nearestDist; - unsigned int neighborhoodSize = kdtreePreviousEdges.knnSearch(pos, nearestSearch, nearestIndex, nearestDist); + unsigned int neighborhoodSize = kdtreePreviousEdges.KnnSearch(pos, nearestSearch, nearestIndex, nearestDist); // Shortcut to keypoints cloud - const PointCloud& previousEdgesPoints = *kdtreePreviousEdges.getInputCloud(); + const PointCloud& previousEdgesPoints = *kdtreePreviousEdges.GetInputCloud(); // Take the closest point const Point& closest = previousEdgesPoints[nearestIndex[0]]; @@ -1707,10 +1707,10 @@ void Slam::GetLocalizationLineSpecificNeighbor(std::vector& nearestValid, s // Get nearest neighbors of the query point std::vector nearestIndex; std::vector nearestDist; - unsigned int neighborhoodSize = kdtreePreviousEdges.knnSearch(pos, nearestSearch, nearestIndex, nearestDist); + unsigned int neighborhoodSize = kdtreePreviousEdges.KnnSearch(pos, nearestSearch, nearestIndex, nearestDist); // Shortcut to keypoints cloud - const PointCloud& previousEdgesPoints = *kdtreePreviousEdges.getInputCloud(); + const PointCloud& previousEdgesPoints = *kdtreePreviousEdges.GetInputCloud(); // to avoid square root when performing comparison const float squaredMaxDistInlier = maxDistInlier * maxDistInlier; -- GitLab