From 6da134a08a022786b999655ec6deb217b5945e6a Mon Sep 17 00:00:00 2001 From: Nicolas Cadart Date: Thu, 29 Oct 2020 13:40:31 +0100 Subject: [PATCH 1/2] [feat][perf] Extract less keypoints from maps for faster KD-trees Resolves #11. Extract less keypoints from maps for faster KD-trees building and NN queries. First, we check in which rolling grid's voxels lie each current frame keypoint. Then we extract map keypoints from these cells and their neighbors. The "voting dilatation" is done by convolving the voxels "to take". This implementation uses unsupported Eigen Tensor module. --- slam_lib/include/LidarSlam/RollingGrid.h | 3 ++ slam_lib/src/RollingGrid.cxx | 52 ++++++++++++++++++++++++ slam_lib/src/Slam.cxx | 8 ++-- 3 files changed, 58 insertions(+), 5 deletions(-) diff --git a/slam_lib/include/LidarSlam/RollingGrid.h b/slam_lib/include/LidarSlam/RollingGrid.h index 50358578..550858b1 100644 --- a/slam_lib/include/LidarSlam/RollingGrid.h +++ b/slam_lib/include/LidarSlam/RollingGrid.h @@ -80,6 +80,9 @@ public: //! Extract all points in map lying in given bounding box PointCloud::Ptr Get(const Eigen::Array3d& minPoint, const Eigen::Array3d& maxPoint) const; + //! Extract all points in map that are close to points from given cloud + PointCloud::Ptr Get(const PointCloud& pcToMatch) const; + //! Get all points PointCloud::Ptr Get() const; diff --git a/slam_lib/src/RollingGrid.cxx b/slam_lib/src/RollingGrid.cxx index e1d792eb..c42af36e 100644 --- a/slam_lib/src/RollingGrid.cxx +++ b/slam_lib/src/RollingGrid.cxx @@ -20,6 +20,8 @@ #include "LidarSlam/RollingGrid.h" #include "LidarSlam/Utilities.h" +#include + // A new PCL Point is added so we need to recompile PCL to be able to use // filters (pcl::VoxelGrid) with this new type #ifndef PCL_NO_PRECOMPILE @@ -135,6 +137,56 @@ RollingGrid::PointCloud::Ptr RollingGrid::Get(const Eigen::Array3d& minPoint, co return intersection; } +//------------------------------------------------------------------------------ +RollingGrid::PointCloud::Ptr RollingGrid::Get(const PointCloud& pcToMatch) const +{ + // Identify voxels in which lie input points + Eigen::Tensor nonEmptyVoxels(this->GridSize, this->GridSize, this->GridSize); + nonEmptyVoxels.setZero(); + Eigen::Array3i voxelGridOrigin = this->PositionToVoxel(this->VoxelGridPosition) - this->GridSize / 2; + for (const Point& point : pcToMatch) + { + // Find the voxel containing this point + Eigen::Array3i cubeIdx = this->PositionToVoxel(point.getArray3fMap()) - voxelGridOrigin; + // Add a vote to the corresponding voxel + if (((0 <= cubeIdx) && (cubeIdx < this->GridSize)).all()) + nonEmptyVoxels(cubeIdx.x(), cubeIdx.y(), cubeIdx.z())++; + } + + // Dilate the votes by convolving with blur kernel + Eigen::TensorFixedSize> kernel; + constexpr float centerWeight = 1.; + constexpr float orthoWeight = 0.4; + constexpr float diagWeight = 0.2; + constexpr float cornerWeight = 0.1; + kernel.setValues({{{cornerWeight, diagWeight, cornerWeight}, + { diagWeight, orthoWeight, diagWeight}, + {cornerWeight, diagWeight, cornerWeight}}, + + {{ diagWeight, orthoWeight, diagWeight}, + { orthoWeight, centerWeight, orthoWeight}, + { diagWeight, orthoWeight, diagWeight}}, + + {{cornerWeight, diagWeight, cornerWeight}, + { diagWeight, orthoWeight, diagWeight}, + {cornerWeight, diagWeight, cornerWeight}}}); + // Perform SAME convolution by adding padding + Eigen::array, 3> padding = {{{1, 1}, {1, 1}, {1, 1}}}; + Eigen::array dims = {0, 1, 2}; + Eigen::Tensor voxelsToUse = nonEmptyVoxels.pad(padding).eval().convolve(kernel, dims); + + // Get all points from voxels to use + constexpr float THRESHOLD = 1.; + PointCloud::Ptr intersection(new PointCloud); + for (int x = 0; x < this->GridSize; x++) + for (int y = 0; y < this->GridSize; y++) + for (int z = 0; z < this->GridSize; z++) + if (voxelsToUse(x, y, z) >= THRESHOLD) + *intersection += *(this->Grid[x][y][z]); + + return intersection; +} + //------------------------------------------------------------------------------ RollingGrid::PointCloud::Ptr RollingGrid::Get() const { diff --git a/slam_lib/src/Slam.cxx b/slam_lib/src/Slam.cxx index 6f7d15fb..06dd5b05 100644 --- a/slam_lib/src/Slam.cxx +++ b/slam_lib/src/Slam.cxx @@ -939,14 +939,12 @@ void Slam::Localization() auto extractMapKeypointsAndBuildKdTree = [this](const PointCloud::Ptr& currKeypoints, RollingGrid& map, PointCloud::Ptr& prevKeypoints, KDTree& kdTree) { - // Estimate current keypoints bounding box + // Estimate current keypoints WORLD positions PointCloud currWordKeypoints; pcl::transformPointCloud(*currKeypoints, currWordKeypoints, this->Tworld.matrix()); - Eigen::Vector4f minPoint, maxPoint; - pcl::getMinMax3D(currWordKeypoints, minPoint, maxPoint); - // Extract all points in maps lying in this bounding box - prevKeypoints = map.Get(minPoint.head<3>().cast().array(), maxPoint.head<3>().cast().array()); + // Extract all points in maps lying near these points + prevKeypoints = map.Get(currWordKeypoints); kdTree.Reset(prevKeypoints); }; -- GitLab From 407c4160dc99528db9e00d6a0cfc8a2e518a2dd4 Mon Sep 17 00:00:00 2001 From: Nicolas Cadart Date: Fri, 30 Oct 2020 17:56:16 +0100 Subject: [PATCH 2/2] [perf] Only convolves the non-empty part of the rolling grid Get the subgrid containing all non-empty voxels, and convolve only this part. This allows saving a lot of time. --- slam_lib/src/RollingGrid.cxx | 38 ++++++++++++++++++++++++++---------- 1 file changed, 28 insertions(+), 10 deletions(-) diff --git a/slam_lib/src/RollingGrid.cxx b/slam_lib/src/RollingGrid.cxx index c42af36e..9455c016 100644 --- a/slam_lib/src/RollingGrid.cxx +++ b/slam_lib/src/RollingGrid.cxx @@ -140,19 +140,37 @@ RollingGrid::PointCloud::Ptr RollingGrid::Get(const Eigen::Array3d& minPoint, co //------------------------------------------------------------------------------ RollingGrid::PointCloud::Ptr RollingGrid::Get(const PointCloud& pcToMatch) const { - // Identify voxels in which lie input points - Eigen::Tensor nonEmptyVoxels(this->GridSize, this->GridSize, this->GridSize); - nonEmptyVoxels.setZero(); + // Identify voxels in which lie input points, and the bounding box of the non-empty voxels + Eigen::Tensor pointsInVoxels(this->GridSize, this->GridSize, this->GridSize); + pointsInVoxels.setZero(); Eigen::Array3i voxelGridOrigin = this->PositionToVoxel(this->VoxelGridPosition) - this->GridSize / 2; + Eigen::Array3i minCell, maxCell; + minCell.setConstant(this->GridSize - 1); + maxCell.setConstant(0); for (const Point& point : pcToMatch) { // Find the voxel containing this point Eigen::Array3i cubeIdx = this->PositionToVoxel(point.getArray3fMap()) - voxelGridOrigin; - // Add a vote to the corresponding voxel + // Notify the voxel if it is within grid if (((0 <= cubeIdx) && (cubeIdx < this->GridSize)).all()) - nonEmptyVoxels(cubeIdx.x(), cubeIdx.y(), cubeIdx.z())++; + { + pointsInVoxels(cubeIdx.x(), cubeIdx.y(), cubeIdx.z())++; + minCell = minCell.min(cubeIdx); + maxCell = maxCell.max(cubeIdx); + } } + // Check if maxPoint is greater than minPoint. + // If not, this means that no point from pcToMatch lies in rolling grid. + if ((minCell > maxCell).any()) + return PointCloud::Ptr(new PointCloud); + + // Extract non empty part of rolling grid using bounding box + // We do that to to save time by avoiding convolving the entire rolling grid + Eigen::array offsets = {minCell.x(), minCell.y(), minCell.z()}; + Eigen::array extents = {maxCell.x() - minCell.x() + 1, maxCell.y() - minCell.y() + 1, maxCell.z() - minCell.z() + 1}; + Eigen::Tensor nonEmptyVoxels = pointsInVoxels.slice(offsets, extents); + // Dilate the votes by convolving with blur kernel Eigen::TensorFixedSize> kernel; constexpr float centerWeight = 1.; @@ -175,14 +193,14 @@ RollingGrid::PointCloud::Ptr RollingGrid::Get(const PointCloud& pcToMatch) const Eigen::array dims = {0, 1, 2}; Eigen::Tensor voxelsToUse = nonEmptyVoxels.pad(padding).eval().convolve(kernel, dims); - // Get all points from voxels to use + // Extract all points from voxels to use constexpr float THRESHOLD = 1.; PointCloud::Ptr intersection(new PointCloud); - for (int x = 0; x < this->GridSize; x++) - for (int y = 0; y < this->GridSize; y++) - for (int z = 0; z < this->GridSize; z++) + for (int x = 0; x < voxelsToUse.dimension(0); x++) + for (int y = 0; y < voxelsToUse.dimension(1); y++) + for (int z = 0; z < voxelsToUse.dimension(2); z++) if (voxelsToUse(x, y, z) >= THRESHOLD) - *intersection += *(this->Grid[x][y][z]); + *intersection += *(this->Grid[x + minCell.x()][y + minCell.y()][z + minCell.z()]); return intersection; } -- GitLab