diff --git a/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h b/slam_lib/include/LidarSlam/KDTreePCLAdaptor.h index 497ca202990503bf15ef6a80e7570d3ca040dd1d..7bf34dc4d71346a7007607dc3fb15b409acaf17e 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"); @@ -19,61 +20,103 @@ #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_Simple::traits>::distance_t; + using index_t = nanoflann::KDTreeSingleIndexAdaptor, 3, int>; public: KDTreePCLAdaptor() = default; - KDTreePCLAdaptor(pcl::PointCloud::Ptr cloud) + /** + * \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); + this->Reset(cloud, leafMaxSize); } - void Reset(pcl::PointCloud::Ptr cloud) + /** + * \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 - int leafMaxSize = 25; - Index = std::make_unique(3, *this, nanoflann::KDTreeSingleIndexAdaptorParams(leafMaxSize)); - Index->buildIndex(); + this->Index = std::make_unique(3, *this, nanoflann::KDTreeSingleIndexAdaptorParams(leafMaxSize)); + this->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. - * \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 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 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 float queryPoint[3], int knearest, int* knnIndices, float* knnSqDistances) const + { + return this->Index->knnSearch(queryPoint, knearest, knnIndices, knnSqDistances); + } + inline size_t KnnSearch(const float queryPoint[3], int knearest, std::vector& knnIndices, std::vector& knnSqDistances) 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()); + // Init result to have large enough buffers that will be filled by knnSearch + knnIndices.resize(knearest); + knnSqDistances.resize(knearest); + // Find nearest neighbors + 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 + knnIndices.resize(kneighbors); + knnSqDistances.resize(kneighbors); + return kneighbors; } - 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 queryPoint[3], int knearest, std::vector& knnIndices, std::vector& knnSqDistances) const + { + float pt[3]; + std::copy(queryPoint, queryPoint + 3, pt); + return this->KnnSearch(pt, knearest, knnIndices, knnSqDistances); + } + inline size_t KnnSearch(const Point& queryPoint, int knearest, std::vector& knnIndices, std::vector& knnSqDistances) const + { + 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 { - nanoflann::KNNResultSet resultSet(knearest); - resultSet.init(out_indices, out_distances_sq); - this->Index->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); + return this->Cloud; } + // --------------------------------------------------------------------------- + // Methods required by nanoflann adaptor design + // --------------------------------------------------------------------------- + inline const KDTreePCLAdaptor& derived() const { return *this; @@ -84,31 +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: - 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; - } - - inline pcl::PointCloud::Ptr getInputCloud() const + /** + * \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; + return this->Cloud->points[idx].data[dim]; } - // 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 { @@ -120,8 +168,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 bb88b497af0990b75b582c33442305c52b7b4777..47c57850c4a377eae2193c827f19b078754985fe 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 5013b59372c99c2cce447ea09e8235ad5a980194..6ce1d15b7685186b6347dfefe5ac99a57d314a58 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 @@ -1280,7 +1280,7 @@ Slam::MatchingResult Slam::ComputeLineDistanceParameters(KDTreePCLAdaptor& kdtre 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; @@ -1317,7 +1317,7 @@ Slam::MatchingResult Slam::ComputeLineDistanceParameters(KDTreePCLAdaptor& kdtre } // Shortcut to keypoints cloud - const PointCloud& previousEdgesPoints = *kdtreePreviousEdges.getInputCloud(); + const PointCloud& previousEdgesPoints = *kdtreePreviousEdges.GetInputCloud(); // ======================================================= // Check if neighborhood is a good line candidate with PCA @@ -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 @@ -1439,12 +1439,12 @@ Slam::MatchingResult Slam::ComputePlaneDistanceParameters(KDTreePCLAdaptor& kdtr 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; + 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 (nearestIndex.back() == -1) + if (neighborhoodSize < requiredNearest) { return MatchingResult::NOT_ENOUGH_NEIGHBORS; } @@ -1457,7 +1457,7 @@ Slam::MatchingResult Slam::ComputePlaneDistanceParameters(KDTreePCLAdaptor& kdtr } // Shortcut to keypoints cloud - const PointCloud& previousPlanesPoints = *kdtreePreviousPlanes.getInputCloud(); + const PointCloud& previousPlanesPoints = *kdtreePreviousPlanes.GetInputCloud(); // ======================================================== // Check if neighborhood is a good plane candidate with PCA @@ -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 @@ -1552,12 +1552,12 @@ Slam::MatchingResult Slam::ComputeBlobsDistanceParameters(KDTreePCLAdaptor& kdtr 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; + 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 (nearestIndex.back() == -1) + if (neighborhoodSize < requiredNearest) { return MatchingResult::NOT_ENOUGH_NEIGHBORS; } @@ -1570,7 +1570,7 @@ Slam::MatchingResult Slam::ComputeBlobsDistanceParameters(KDTreePCLAdaptor& kdtr } // Shortcut to keypoints cloud - const PointCloud& previousBlobsPoints = *kdtreePreviousBlobs.getInputCloud(); + const PointCloud& previousBlobsPoints = *kdtreePreviousBlobs.GetInputCloud(); // ====================================== // Check the diameter of the neighborhood @@ -1652,27 +1652,20 @@ 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(); 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; + std::vector 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]]; @@ -1704,27 +1697,20 @@ 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(); 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; + std::vector 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;