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

Refactored MultiVoxelGridCovariance and MultiGridNormalDistributionsTransform #30

Merged
merged 1 commit into from
Mar 7, 2024
Merged

Refactored MultiVoxelGridCovariance and MultiGridNormalDistributionsTransform #30

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
128 changes: 52 additions & 76 deletions include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -95,20 +95,52 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid<PointT> {

public:
#if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0)
typedef pcl::shared_ptr<pcl::VoxelGrid<PointT> > Ptr;
typedef pcl::shared_ptr<const pcl::VoxelGrid<PointT> > ConstPtr;
typedef pcl::shared_ptr<pcl::VoxelGrid<PointT>> Ptr;
typedef pcl::shared_ptr<const pcl::VoxelGrid<PointT>> ConstPtr;
#else
typedef boost::shared_ptr<pcl::VoxelGrid<PointT> > Ptr;
typedef boost::shared_ptr<const pcl::VoxelGrid<PointT> > ConstPtr;
typedef boost::shared_ptr<pcl::VoxelGrid<PointT>> Ptr;
typedef boost::shared_ptr<const pcl::VoxelGrid<PointT>> ConstPtr;
#endif

/** \brief Simple structure to hold a centroid, covariance and the number of points in a leaf.
* Inverse covariance, eigen vectors and eigen values are precomputed. */
struct Leaf {
/** \brief Constructor.
* Sets \ref nr_points, \ref icov_, \ref mean_ and \ref evals_ to 0 and \ref cov_ and \ref evecs_ to the identity matrix
* Sets \ref nr_points_, \ref icov_, \ref mean_ and \ref evals_ to 0 and \ref cov_ and \ref evecs_ to the identity matrix
*/
Leaf() : nr_points(0), mean_(Eigen::Vector3d::Zero()), centroid(), cov_(Eigen::Matrix3d::Identity()), icov_(Eigen::Matrix3d::Zero()), evecs_(Eigen::Matrix3d::Identity()), evals_(Eigen::Vector3d::Zero()) {}
Leaf() : nr_points_(0), mean_(Eigen::Vector3d::Zero()), centroid_(), cov_(Eigen::Matrix3d::Identity()), icov_(Eigen::Matrix3d::Zero()), evecs_(Eigen::Matrix3d::Identity()), evals_(Eigen::Vector3d::Zero()) {}

Leaf(const Leaf &other) : mean_(other.mean_), centroid_(other.centroid_), cov_(other.cov_), icov_(other.icov_), evecs_(other.evecs_), evals_(other.evals_) {
nr_points_ = other.nr_points_;
}

Leaf(Leaf &&other) : mean_(std::move(other.mean_)), centroid_(std::move(other.centroid_)), cov_(std::move(other.cov_)), icov_(std::move(other.icov_)), evecs_(std::move(other.evecs_)), evals_(std::move(other.evals_)) {
nr_points_ = other.nr_points_;
}

Leaf &operator=(const Leaf &other) {
mean_ = other.mean_;
centroid_ = other.centroid_;
cov_ = other.cov_;
icov_ = other.icov_;
evecs_ = other.evecs_;
evals_ = other.evals_;
nr_points_ = other.nr_points_;

return *this;
}

Leaf &operator=(Leaf &&other) {
mean_ = std::move(other.mean_);
centroid_ = std::move(other.centroid_);
cov_ = std::move(other.cov_);
icov_ = std::move(other.icov_);
evecs_ = std::move(other.evecs_);
evals_ = std::move(other.evals_);
nr_points_ = other.nr_points_;

return *this;
}

/** \brief Get the voxel covariance.
* \return covariance matrix
Expand Down Expand Up @@ -151,19 +183,19 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid<PointT> {
* \return number of points
*/
int getPointCount() const {
return (nr_points);
return (nr_points_);
}

/** \brief Number of points contained by voxel */
int nr_points;
int nr_points_;

/** \brief 3D voxel centroid */
Eigen::Vector3d mean_;

/** \brief Nd voxel centroid
* \note Differs from \ref mean_ when color data is used
*/
Eigen::VectorXf centroid;
Eigen::VectorXf centroid_;

/** \brief Voxel covariance matrix */
Eigen::Matrix3d cov_;
Expand Down Expand Up @@ -231,42 +263,15 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid<PointT> {

/** \brief Initializes voxel structure.
*/
inline void setInputCloudAndFilter(const PointCloudConstPtr &cloud, const std::string &grid_id) {
LeafDict leaves;
applyFilter(cloud, grid_id, leaves);

grid_leaves_[grid_id] = leaves;
}

inline void removeCloud(const std::string &grid_id) {
grid_leaves_.erase(grid_id);
}
void setInputCloudAndFilter(const PointCloudConstPtr &cloud, const std::string &grid_id);

inline void createKdtree() {
leaves_.clear();
for(const auto &kv : grid_leaves_) {
leaves_.insert(kv.second.begin(), kv.second.end());
}

leaf_indices_.clear();
voxel_centroids_ptr_.reset(new PointCloud);
voxel_centroids_ptr_->height = 1;
voxel_centroids_ptr_->is_dense = true;
voxel_centroids_ptr_->points.clear();
voxel_centroids_ptr_->points.reserve(leaves_.size());
for(const auto &element : leaves_) {
leaf_indices_.push_back(element.first);
voxel_centroids_ptr_->push_back(PointT());
voxel_centroids_ptr_->points.back().x = element.second.centroid[0];
voxel_centroids_ptr_->points.back().y = element.second.centroid[1];
voxel_centroids_ptr_->points.back().z = element.second.centroid[2];
}
voxel_centroids_ptr_->width = static_cast<uint32_t>(voxel_centroids_ptr_->points.size());
/** \brief Remove a ND voxel grid corresponding to the specified id
*/
void removeCloud(const std::string &grid_id);

if(voxel_centroids_ptr_->size() > 0) {
kdtree_.setInputCloud(voxel_centroids_ptr_);
}
}
/** \brief Build Kdtrees from the NDT voxel for later radius search
*/
void createKdtree();

/** \brief Search for all the nearest occupied voxels of the query point in a given radius.
* \note Only voxels containing a sufficient number of points are used.
Expand All @@ -277,25 +282,7 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid<PointT> {
* \param[in] max_nn
* \return number of neighbors found
*/
int radiusSearch(const PointT &point, double radius, std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const {
k_leaves.clear();

// 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);

// Find leaves corresponding to neighbors
k_leaves.reserve(k);
for(std::vector<int>::iterator iter = k_indices.begin(); iter != k_indices.end(); iter++) {
auto leaf = leaves_.find(leaf_indices_[*iter]);
if(leaf == leaves_.end()) {
std::cerr << "error : could not find the leaf corresponding to the voxel" << std::endl;
std::cin.ignore(1);
}
k_leaves.push_back(&(leaf->second));
}
return k;
}
int radiusSearch(const PointT &point, double radius, std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const;

/** \brief Search for all the nearest occupied voxels of the query point in a given radius.
* \note Only voxels containing a sufficient number of points are used.
Expand All @@ -307,22 +294,11 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid<PointT> {
* \param[in] max_nn
* \return number of neighbors found
*/
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) const {
if(index >= static_cast<int>(cloud.points.size()) || index < 0) return (0);
return (radiusSearch(cloud.points[index], radius, k_leaves, k_sqr_distances, max_nn));
}
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) const;

PointCloud getVoxelPCD() const {
return *voxel_centroids_ptr_;
}
PointCloud getVoxelPCD() const;

std::vector<std::string> getCurrentMapIDs() const {
std::vector<std::string> output{};
for(const auto &element : grid_leaves_) {
output.push_back(element.first);
}
return output;
}
std::vector<std::string> getCurrentMapIDs() const;

protected:
/** \brief Filter cloud and initializes voxel structure.
Expand Down
Loading
Loading