Feature/smooth ndt map update

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) {

include/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp

@@ -56,6 +56,77 @@
 #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 {

include/multigrid_pclomp/multigrid_ndt_omp.h

@@ -72,6 +72,17 @@ 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 +128,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 +144,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 +163,16 @@ 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);
@@ -183,6 +214,11 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
     return (resolution_);
   }
 
+  inline Eigen::Vector3f getGridLeafSize() const 
+  {
+    return target_cells_.getLeafSize();
+  }
+
   /** \brief Get the newton line search maximum step length.
    * \return maximum step length
    */
@@ -318,7 +354,7 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
   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>::indices_;  // Why need this one?
   using pcl::Registration<PointSource, PointTarget>::target_;
   using pcl::Registration<PointSource, PointTarget>::nr_iterations_;
   using pcl::Registration<PointSource, PointTarget>::max_iterations_;

include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp

@@ -56,6 +56,111 @@
 #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()