Feature/smooth ndt map update (#26)

* Adding copy/move constructors and assignment operators Signed-off-by: anhnv3991 <[email protected]> * Testing now Signed-off-by: anhnv3991 <[email protected]> * Fixed conflicts Signed-off-by: anhnv3991 <[email protected]> * Remove unused variables and debug functions Signed-off-by: anhnv3991 <[email protected]> * chore: add pre-commit to apply clang-format automatically (#27) * Added pre-commit Signed-off-by: Shintaro Sakoda <[email protected]> * Test commit to confirm if pre-commit working Signed-off-by: Shintaro Sakoda <[email protected]> * style(pre-commit): autofix * Added types to build.yml Signed-off-by: Shintaro Sakoda <[email protected]> * Added types test Signed-off-by: Shintaro Sakoda <[email protected]> * style(pre-commit): autofix * Added workflow_run Signed-off-by: Shintaro Sakoda <[email protected]> * style(pre-commit): autofix * Removed workflow_run Signed-off-by: Shintaro Sakoda <[email protected]> * Removed spaces Signed-off-by: Shintaro Sakoda <[email protected]> * Applied formatter Signed-off-by: Shintaro Sakoda <[email protected]> * style(pre-commit): autofix * Added clang-format --version Signed-off-by: Shintaro Sakoda <[email protected]> * Updated version of mirrors-clang-format Signed-off-by: Shintaro Sakoda <[email protected]> * style(pre-commit): autofix * Removed incorrect version display Signed-off-by: Shintaro Sakoda <[email protected]> --------- Signed-off-by: Shintaro Sakoda <[email protected]> Co-authored-by: SakodaShintaro <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: anhnv3991 <[email protected]> Signed-off-by: Shintaro Sakoda <[email protected]> Co-authored-by: SakodaShintaro <[email protected]> Co-authored-by: SakodaShintaro <[email protected]> Co-authored-by: anhnv3991 <[email protected]>
tier4 · Feb 20, 2024 · fce13f5 · fce13f5
1 parent 0b8426b
commit fce13f5
Show file tree

Hide file tree

Showing 4 changed files with 185 additions and 4 deletions.
diff --git a/include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h b/include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h
@@ -223,6 +223,12 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid<PointT> {
     filter_name_ = "MultiVoxelGridCovariance";
   }
 
+  MultiVoxelGridCovariance(const MultiVoxelGridCovariance &other);
+  MultiVoxelGridCovariance(MultiVoxelGridCovariance &&other);
+
+  MultiVoxelGridCovariance &operator=(const MultiVoxelGridCovariance &other);
+  MultiVoxelGridCovariance &operator=(MultiVoxelGridCovariance &&other);
+
   /** \brief Initializes voxel structure.
    */
   inline void setInputCloudAndFilter(const PointCloudConstPtr &cloud, const std::string &grid_id) {

diff --git a/include/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp b/include/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp
@@ -56,6 +56,62 @@
 #include <pcl/filters/boost.h>
 #include "multigrid_pclomp/multi_voxel_grid_covariance_omp.h"
 
+template<typename PointT>
+pclomp::MultiVoxelGridCovariance<PointT>::MultiVoxelGridCovariance(const MultiVoxelGridCovariance &other)
+    : pcl::VoxelGrid<PointT>(other), leaves_(other.leaves_), grid_leaves_(other.grid_leaves_), leaf_indices_(other.leaf_indices_), kdtree_(other.kdtree_) {
+  min_points_per_voxel_ = other.min_points_per_voxel_;
+  min_covar_eigvalue_mult_ = other.min_covar_eigvalue_mult_;
+
+  // Deep copy
+  voxel_centroids_ptr_.reset(new PointCloud);
+
+  if(other.voxel_centroids_ptr_) {
+    *voxel_centroids_ptr_ = *other.voxel_centroids_ptr_;
+  }
+}
+
+template<typename PointT>
+pclomp::MultiVoxelGridCovariance<PointT>::MultiVoxelGridCovariance(MultiVoxelGridCovariance &&other)
+    : pcl::VoxelGrid<PointT>(std::move(other)), leaves_(std::move(other.leaves_)), grid_leaves_(std::move(other.grid_leaves_)), leaf_indices_(std::move(other.leaf_indices_)), kdtree_(std::move(other.kdtree_)), voxel_centroids_ptr_(std::move(other.voxel_centroids_ptr_)) {
+  min_points_per_voxel_ = other.min_points_per_voxel_;
+  min_covar_eigvalue_mult_ = other.min_covar_eigvalue_mult_;
+}
+
+template<typename PointT>
+pclomp::MultiVoxelGridCovariance<PointT> &pclomp::MultiVoxelGridCovariance<PointT>::operator=(const MultiVoxelGridCovariance &other) {
+  pcl::VoxelGrid<PointT>::operator=(other);
+  leaves_ = other.leaves_;
+  grid_leaves_ = other.grid_leaves_;
+  leaf_indices_ = other.leaf_indices_;
+  kdtree_ = other.kdtree_;
+  voxel_centroids_ptr_ = other.voxel_centroids_ptr_;
+  min_points_per_voxel_ = other.min_points_per_voxel_;
+  min_covar_eigvalue_mult_ = other.min_covar_eigvalue_mult_;
+
+  // Deep copy
+  voxel_centroids_ptr_.reset(new PointCloud);
+
+  if(other.voxel_centroids_ptr_) {
+    *voxel_centroids_ptr_ = *other.voxel_centroids_ptr_;
+  }
+
+  return *this;
+}
+
+template<typename PointT>
+pclomp::MultiVoxelGridCovariance<PointT> &pclomp::MultiVoxelGridCovariance<PointT>::operator=(MultiVoxelGridCovariance &&other) {
+  pcl::VoxelGrid<PointT>::operator=(std::move(other));
+  leaves_ = std::move(other.leaves_);
+  grid_leaves_ = std::move(other.grid_leaves_);
+  leaf_indices_ = std::move(other.leaf_indices_);
+  kdtree_ = std::move(other.kdtree_);
+  voxel_centroids_ptr_ = std::move(other.voxel_centroids_ptr_);
+  min_points_per_voxel_ = other.min_points_per_voxel_;
+  min_covar_eigvalue_mult_ = other.min_covar_eigvalue_mult_;
+
+  return *this;
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////
 template<typename PointT>
 void pclomp::MultiVoxelGridCovariance<PointT>::applyFilter(const PointCloudConstPtr &input, const std::string &grid_id, LeafDict &leaves) const {

diff --git a/include/multigrid_pclomp/multigrid_ndt_omp.h b/include/multigrid_pclomp/multigrid_ndt_omp.h
@@ -72,6 +72,16 @@ struct NdtResult {
   Eigen::Matrix<double, 6, 6> hessian;
   std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> transformation_array;
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  friend std::ostream &operator<<(std::ostream &os, const NdtResult &val) {
+    os << "Pose: " << std::endl << val.pose << std::endl;
+    os << "TP: " << val.transform_probability << std::endl;
+    os << "NVTP: " << val.nearest_voxel_transformation_likelihood << std::endl;
+    os << "Iteration num: " << val.iteration_num << std::endl;
+    os << "Hessian: " << std::endl << val.hessian << std::endl;
+
+    return os;
+  }
 };
 
 struct NdtParams {
@@ -117,6 +127,8 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
   /** \brief Typename of const pointer to searchable voxel grid leaf. */
   typedef typename TargetGrid::LeafConstPtr TargetGridLeafConstPtr;
 
+  typedef pcl::Registration<PointSource, PointTarget> BaseRegType;
+
 public:
 #if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0)
   typedef pcl::shared_ptr<MultiGridNormalDistributionsTransform<PointSource, PointTarget>> Ptr;
@@ -131,6 +143,14 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
    */
   MultiGridNormalDistributionsTransform();
 
+  // Copy & move constructor
+  MultiGridNormalDistributionsTransform(const MultiGridNormalDistributionsTransform &other);
+  MultiGridNormalDistributionsTransform(MultiGridNormalDistributionsTransform &&other);
+
+  // Copy & move assignments
+  MultiGridNormalDistributionsTransform &operator=(const MultiGridNormalDistributionsTransform &other);
+  MultiGridNormalDistributionsTransform &operator=(MultiGridNormalDistributionsTransform &&other);
+
   /** \brief Empty destructor */
   virtual ~MultiGridNormalDistributionsTransform() {}
 
@@ -142,6 +162,14 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
     return num_threads_;
   }
 
+  inline void setInputSource(const PointCloudSourceConstPtr &input) {
+    // This is to avoid segmentation fault when setting null input
+    // No idea why PCL does not check the nullity of input
+    if(input) {
+      pcl::Registration<PointSource, PointTarget>::setInputSource(input);
+    }
+  }
+
   inline void setInputTarget(const PointCloudTargetConstPtr &cloud) {
     const std::string default_target_id = "default";
     addTarget(cloud, default_target_id);
@@ -316,9 +344,7 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
 
 protected:
   using pcl::Registration<PointSource, PointTarget>::reg_name_;
-  using pcl::Registration<PointSource, PointTarget>::getClassName;
   using pcl::Registration<PointSource, PointTarget>::input_;
-  using pcl::Registration<PointSource, PointTarget>::indices_;
   using pcl::Registration<PointSource, PointTarget>::target_;
   using pcl::Registration<PointSource, PointTarget>::nr_iterations_;
   using pcl::Registration<PointSource, PointTarget>::max_iterations_;
@@ -327,8 +353,6 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
   using pcl::Registration<PointSource, PointTarget>::transformation_;
   using pcl::Registration<PointSource, PointTarget>::transformation_epsilon_;
   using pcl::Registration<PointSource, PointTarget>::converged_;
-  using pcl::Registration<PointSource, PointTarget>::corr_dist_threshold_;
-  using pcl::Registration<PointSource, PointTarget>::inlier_threshold_;
 
   using pcl::Registration<PointSource, PointTarget>::update_visualizer_;
 

diff --git a/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp b/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp
@@ -56,6 +56,101 @@
 #ifndef PCL_REGISTRATION_NDT_OMP_MULTI_VOXEL_IMPL_H_
 #define PCL_REGISTRATION_NDT_OMP_MULTI_VOXEL_IMPL_H_
 
+template<typename PointSource, typename PointTarget>
+pclomp::MultiGridNormalDistributionsTransform<PointSource, PointTarget>::MultiGridNormalDistributionsTransform(const MultiGridNormalDistributionsTransform &other) : BaseRegType(other), target_cells_(other.target_cells_) {
+  resolution_ = other.resolution_;
+  step_size_ = other.step_size_;
+  outlier_ratio_ = other.outlier_ratio_;
+  gauss_d1_ = other.gauss_d1_;
+  gauss_d2_ = other.gauss_d2_;
+  gauss_d3_ = other.gauss_d3_;
+  trans_probability_ = other.trans_probability_;
+  // No need to copy j_ang and h_ang, as those matrices are re-computed on every computeDerivatives() call
+
+  num_threads_ = other.num_threads_;
+  hessian_ = other.hessian_;
+  transformation_array_ = other.transformation_array_;
+  nearest_voxel_transformation_likelihood_ = other.nearest_voxel_transformation_likelihood_;
+
+  regularization_scale_factor_ = other.regularization_scale_factor_;
+  regularization_pose_ = other.regularization_pose_;
+  regularization_pose_translation_ = other.regularization_pose_translation_;
+}
+
+template<typename PointSource, typename PointTarget>
+pclomp::MultiGridNormalDistributionsTransform<PointSource, PointTarget>::MultiGridNormalDistributionsTransform(MultiGridNormalDistributionsTransform &&other) : BaseRegType(std::move(other)), target_cells_(std::move(other.target_cells_)) {
+  resolution_ = other.resolution_;
+  step_size_ = other.step_size_;
+  outlier_ratio_ = other.outlier_ratio_;
+  gauss_d1_ = other.gauss_d1_;
+  gauss_d2_ = other.gauss_d2_;
+  gauss_d3_ = other.gauss_d3_;
+  trans_probability_ = other.trans_probability_;
+  // No need to copy j_ang and h_ang, as those matrices are re-computed on every computeDerivatives() call
+
+  num_threads_ = other.num_threads_;
+  hessian_ = other.hessian_;
+  transformation_array_ = other.transformation_array_;
+  nearest_voxel_transformation_likelihood_ = other.nearest_voxel_transformation_likelihood_;
+
+  regularization_scale_factor_ = other.regularization_scale_factor_;
+  regularization_pose_ = other.regularization_pose_;
+  regularization_pose_translation_ = other.regularization_pose_translation_;
+}
+
+template<typename PointSource, typename PointTarget>
+pclomp::MultiGridNormalDistributionsTransform<PointSource, PointTarget> &pclomp::MultiGridNormalDistributionsTransform<PointSource, PointTarget>::operator=(const MultiGridNormalDistributionsTransform &other) {
+  BaseRegType::operator=(other);
+
+  target_cells_ = other.target_cells_;
+
+  resolution_ = other.resolution_;
+  step_size_ = other.step_size_;
+  outlier_ratio_ = other.outlier_ratio_;
+  gauss_d1_ = other.gauss_d1_;
+  gauss_d2_ = other.gauss_d2_;
+  gauss_d3_ = other.gauss_d3_;
+  trans_probability_ = other.trans_probability_;
+  // No need to copy j_ang and h_ang, as those matrices are re-computed on every computeDerivatives() call
+
+  num_threads_ = other.num_threads_;
+  hessian_ = other.hessian_;
+  transformation_array_ = other.transformation_array_;
+  nearest_voxel_transformation_likelihood_ = other.nearest_voxel_transformation_likelihood_;
+
+  regularization_scale_factor_ = other.regularization_scale_factor_;
+  regularization_pose_ = other.regularization_pose_;
+  regularization_pose_translation_ = other.regularization_pose_translation_;
+
+  return *this;
+}
+
+template<typename PointSource, typename PointTarget>
+pclomp::MultiGridNormalDistributionsTransform<PointSource, PointTarget> &pclomp::MultiGridNormalDistributionsTransform<PointSource, PointTarget>::operator=(MultiGridNormalDistributionsTransform &&other) {
+  BaseRegType::operator=(std::move(other));
+
+  target_cells_ = std::move(other.target_cells_);
+  resolution_ = other.resolution_;
+  step_size_ = other.step_size_;
+  outlier_ratio_ = other.outlier_ratio_;
+  gauss_d1_ = other.gauss_d1_;
+  gauss_d2_ = other.gauss_d2_;
+  gauss_d3_ = other.gauss_d3_;
+  trans_probability_ = other.trans_probability_;
+  // No need to copy j_ang and h_ang, as those matrices are re-computed on every computeDerivatives() call
+
+  num_threads_ = other.num_threads_;
+  hessian_ = other.hessian_;
+  transformation_array_ = other.transformation_array_;
+  nearest_voxel_transformation_likelihood_ = other.nearest_voxel_transformation_likelihood_;
+
+  regularization_scale_factor_ = other.regularization_scale_factor_;
+  regularization_pose_ = other.regularization_pose_;
+  regularization_pose_translation_ = other.regularization_pose_translation_;
+
+  return *this;
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template<typename PointSource, typename PointTarget>
 pclomp::MultiGridNormalDistributionsTransform<PointSource, PointTarget>::MultiGridNormalDistributionsTransform()