Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[filters] Refatoring VoxelGridCovariance to make it multi-thread safe (and more) #4251

Merged
merged 7 commits into from
Jul 12, 2020
Merged

[filters] Refatoring VoxelGridCovariance to make it multi-thread safe (and more) #4251

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
128a875
Add const qualifiers and generalize getNeighborhoodAtPoint
koide3 Jul 7, 2020
1778711
replace tabs with spaces
koide3 Jul 7, 2020
33f2b5d
Use spacing convention of the original implementation
koide3 Jul 7, 2020
9442bab
Use spacing convention of the original implementation
koide3 Jul 7, 2020
ee07117
Refactor VoxelGridCovariance
koide3 Jul 7, 2020
7151167
Refactor VoxelGridCovariance
koide3 Jul 9, 2020
e5af0a0
Add 3x3x3 neighbor getter and make some variables const
koide3 Jul 12, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
61 changes: 50 additions & 11 deletions filters/include/pcl/filters/impl/voxel_grid_covariance.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -371,28 +371,25 @@ pcl::VoxelGridCovariance<PointT>::applyFilter (PointCloud &output)

//////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> int
pcl::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors)
pcl::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint (const Eigen::Matrix<int, 3, Eigen::Dynamic>& relative_coordinates, const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
{
neighbors.clear ();

// Find displacement coordinates
Eigen::MatrixXi relative_coordinates = pcl::getAllNeighborCellIndices ();
Eigen::Vector4i ijk (static_cast<int> (std::floor (reference_point.x / leaf_size_[0])),
static_cast<int> (std::floor (reference_point.y / leaf_size_[1])),
static_cast<int> (std::floor (reference_point.z / leaf_size_[2])), 0);
Eigen::Array4i diff2min = min_b_ - ijk;
Eigen::Array4i diff2max = max_b_ - ijk;
Eigen::Vector4i ijk = (reference_point.getArray4fMap() * inverse_leaf_size_).template cast<int>();
ijk[3] = 0;
const Eigen::Array4i diff2min = min_b_ - ijk;
const Eigen::Array4i diff2max = max_b_ - ijk;
neighbors.reserve (relative_coordinates.cols ());

// Check each neighbor to see if it is occupied and contains sufficient points
// Slower than radius search because needs to check 26 indices
for (Eigen::Index ni = 0; ni < relative_coordinates.cols (); ni++)
{
Eigen::Vector4i displacement = (Eigen::Vector4i () << relative_coordinates.col (ni), 0).finished ();
const Eigen::Vector4i displacement = (Eigen::Vector4i () << relative_coordinates.col (ni), 0).finished ();
// Checking if the specified cell is in the grid
if ((diff2min <= displacement.array ()).all () && (diff2max >= displacement.array ()).all ())
{
typename std::map<std::size_t, Leaf>::iterator leaf_iter = leaves_.find (((ijk + displacement - min_b_).dot (divb_mul_)));
const auto leaf_iter = leaves_.find (((ijk + displacement - min_b_).dot (divb_mul_)));
if (leaf_iter != leaves_.end () && leaf_iter->second.nr_points >= min_points_per_voxel_)
{
LeafConstPtr leaf = &(leaf_iter->second);
Expand All @@ -401,7 +398,49 @@ pcl::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint (const PointT& referenc
}
}

return (static_cast<int> (neighbors.size ()));
return static_cast<int> (neighbors.size());
}

//////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> int
pcl::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
{
Eigen::MatrixXi relative_coordinates = pcl::getAllNeighborCellIndices();
return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors);
}

//////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> int
pcl::VoxelGridCovariance<PointT>::getVoxelAtPoint(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
{
return getNeighborhoodAtPoint(Eigen::Matrix<int, 3, Eigen::Dynamic>::Zero(3,1), reference_point, neighbors);
}

//////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> int
pcl::VoxelGridCovariance<PointT>::getFaceNeighborsAtPoint(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
{
Eigen::Matrix<int, 3, Eigen::Dynamic> relative_coordinates(3, 7);
relative_coordinates.setZero();
relative_coordinates(0, 1) = 1;
relative_coordinates(0, 2) = -1;
relative_coordinates(1, 3) = 1;
relative_coordinates(1, 4) = -1;
relative_coordinates(2, 5) = 1;
relative_coordinates(2, 6) = -1;

return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors);
}

//////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> int
pcl::VoxelGridCovariance<PointT>::getAllNeighborsAtPoint(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
{
Eigen::Matrix<int, 3, Eigen::Dynamic> relative_coordinates(3, 27);
relative_coordinates.col(0).setZero();
relative_coordinates.rightCols(26) = pcl::getAllNeighborCellIndices();

return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors);
}

//////////////////////////////////////////////////////////////////////////////////////////
Expand Down
71 changes: 59 additions & 12 deletions filters/include/pcl/filters/voxel_grid_covariance.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -361,14 +361,51 @@ namespace pcl

}

/** \brief Get the voxels surrounding point p, not including the voxel containing point p.
* \note Only voxels containing a sufficient number of points are used (slower than radius search in practice).
/** \brief Get the voxels surrounding point p designated by #relative_coordinates.
* \note Only voxels containing a sufficient number of points are used.
* \param[in] relative_coordinates 3xN matrix that represents relative coordinates of N neighboring voxels with respect to the center voxel
* \param[in] reference_point the point to get the leaf structure at
* \param[out] neighbors
* \return number of neighbors found
*/
int
getNeighborhoodAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors);
getNeighborhoodAtPoint (const Eigen::Matrix<int, 3, Eigen::Dynamic>& relative_coordinates, const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const;

/** \brief Get the voxels surrounding point p, not including the voxel containing point p.
kunaltyagi marked this conversation as resolved.
Show resolved Hide resolved
* \note Only voxels containing a sufficient number of points are used.
* \param[in] reference_point the point to get the leaf structure at
* \param[out] neighbors
* \return number of neighbors found (up to 26)
*/
int
getNeighborhoodAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const;

/** \brief Get the voxel at p.
* \note Only voxels containing a sufficient number of points are used.
* \param[in] reference_point the point to get the leaf structure at
* \param[out] neighbors
* \return number of neighbors found (up to 1)
*/
int
getVoxelAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const;

/** \brief Get the voxel at p and its facing voxels (up to 7 voxels).
* \note Only voxels containing a sufficient number of points are used.
* \param[in] reference_point the point to get the leaf structure at
* \param[out] neighbors
* \return number of neighbors found (up to 7)
*/
int
getFaceNeighborsAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const;

/** \brief Get all 3x3x3 neighbor voxels of p (up to 27 voxels).
* \note Only voxels containing a sufficient number of points are used.
* \param[in] reference_point the point to get the leaf structure at
* \param[out] neighbors
* \return number of neighbors found (up to 27)
*/
int
getAllNeighborsAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const;

/** \brief Get the leaf structure map
* \return a map contataining all leaves
Expand Down Expand Up @@ -406,7 +443,7 @@ namespace pcl
*/
int
nearestKSearch (const PointT &point, int k,
std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances)
std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances) const
{
k_leaves.clear ();

Expand All @@ -425,9 +462,14 @@ namespace pcl
k_leaves.reserve (k);
for (const int &k_index : k_indices)
{
k_leaves.push_back (&leaves_[voxel_centroids_leaf_indices_[k_index]]);
auto voxel = leaves_.find(voxel_centroids_leaf_indices_[k_index]);
if (voxel == leaves_.end()) {
continue;
kunaltyagi marked this conversation as resolved.
Show resolved Hide resolved
}

k_leaves.push_back(&voxel->second);
}
return k;
return k_leaves.size();
}

/** \brief Search for the k-nearest occupied voxels for the given query point.
Expand All @@ -441,7 +483,7 @@ namespace pcl
*/
inline int
nearestKSearch (const PointCloud &cloud, int index, int k,
std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances)
std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances) const
{
if (index >= static_cast<int> (cloud.points.size ()) || index < 0)
return (0);
Expand All @@ -460,7 +502,7 @@ namespace pcl
*/
int
radiusSearch (const PointT &point, double radius, std::vector<LeafConstPtr> &k_leaves,
std::vector<float> &k_sqr_distances, unsigned int max_nn = 0)
std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const
{
k_leaves.clear ();

Expand All @@ -473,15 +515,20 @@ namespace pcl

// Find neighbors within radius in the occupied voxel centroid cloud
std::vector<int> k_indices;
int k = kdtree_.radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
const int k = kdtree_.radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);

// Find leaves corresponding to neighbors
k_leaves.reserve (k);
for (const int &k_index : k_indices)
{
k_leaves.push_back (&leaves_[voxel_centroids_leaf_indices_[k_index]]);
const auto voxel = leaves_.find(voxel_centroids_leaf_indices_[k_index]);
if(voxel == leaves_.end()) {
continue;
kunaltyagi marked this conversation as resolved.
Show resolved Hide resolved
}

k_leaves.push_back(&voxel->second);
}
return k;
return k_leaves.size();
}

/** \brief Search for all the nearest occupied voxels of the query point in a given radius.
Expand All @@ -497,7 +544,7 @@ namespace pcl
inline int
radiusSearch (const PointCloud &cloud, int index, double radius,
std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances,
unsigned int max_nn = 0)
unsigned int max_nn = 0) const
{
if (index >= static_cast<int> (cloud.points.size ()) || index < 0)
return (0);
Expand Down