feat: merge map change checker features to the main branch

include/pclomp/ndt_omp.h

@@ -54,6 +54,16 @@
 namespace pclomp
 {
 
+struct EigenCmp
+{
+  bool operator()(const Eigen::Vector3i & a, const Eigen::Vector3i & b) const
+  {
+    return (
+      a(0) < b(0) || (a(0) == b(0) && a(1) < b(1)) ||
+      (a(0) == b(0) && a(1) == b(1) && a(2) < b(2)));
+  }
+};
+
 /** \brief A 3D Normal Distribution Transform registration implementation for point cloud data.
  * \note For more information please see
  * <b>Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform —
@@ -211,9 +221,28 @@ class NormalDistributionsTransform : public pcl::Registration<PointSource, Point
     trans = _affine.matrix();
   }
 
+  // add at 20220721 konishi
+  inline const std::vector<double> getScores() const { return scores_; }
+
+  inline const TargetGrid getTargetCells() const { return target_cells_; }
+
+  inline const std::map<size_t, double> getScoreMap() const { return voxel_score_map_; }
+
+  inline const std::map<size_t, size_t> getNoPointMap() const { return nomap_points_num_; }
+
+  std::set<Eigen::Vector3i, EigenCmp> & getEmptyVoxels() { return empty_voxels_; }
+
+  // For debug
+  void cleanScores()
+  {
+    voxel_score_map_.clear();
+    nomap_points_num_.clear();
+  }
+  // End
+
   // negative log likelihood function
   // lower is better
-  double calculateScore(const PointCloudSource & cloud) const;
+  double calculateScore(const PointCloudSource & cloud);
   double calculateTransformationProbability(const PointCloudSource & cloud) const;
   double calculateNearestVoxelTransformationLikelihood(const PointCloudSource & cloud) const;
 
@@ -510,6 +539,13 @@ class NormalDistributionsTransform : public pcl::Registration<PointSource, Point
   boost::optional<Eigen::Matrix4f> regularization_pose_;
   Eigen::Vector3f regularization_pose_translation_;
 
+  // add at 20220721 konishi
+  std::vector<double> scores_;
+  std::map<size_t, double> voxel_score_map_;
+  std::map<size_t, size_t> nomap_points_num_;
+  // For recording voxels that contain no map point
+  std::set<Eigen::Vector3i, EigenCmp> empty_voxels_;
+
   NdtParams params_;
 
 public:

include/pclomp/ndt_omp_impl.hpp

@@ -1,6 +1,3 @@
-#include "ndt_omp.h"
-
-#include <cmath>
 /*
  * Software License Agreement (BSD License)
  *
@@ -44,7 +41,10 @@
 #ifndef PCL_REGISTRATION_NDT_OMP_IMPL_H_
 #define PCL_REGISTRATION_NDT_OMP_IMPL_H_
 
+#include "ndt_omp.h"
+
 #include <algorithm>
+#include <cmath>
 #include <vector>
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -102,7 +102,7 @@ void pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeTran
     // Initialise final transformation to the guessed one
     final_transformation_ = guess;
     // Apply guessed transformation prior to search for neighbours
-    transformPointCloud(output, output, guess);
+    transformPointCloud(*input_, output, guess);
   }
 
   Eigen::Transform<float, 3, Eigen::Affine, Eigen::ColMajor> eig_transformation;
@@ -299,10 +299,7 @@ double pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeDe
     Eigen::Matrix<double, 6, 6> hessian_pt = Eigen::Matrix<double, 6, 6>::Zero();
     int neighborhood_count = 0;
 
-    for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it =
-           neighborhood.begin();
-         neighborhood_it != neighborhood.end(); ++neighborhood_it) {
-      cell = *neighborhood_it;
+    for (auto & cell : neighborhood) {
       x_pt = input_->points[idx];
       x = Eigen::Vector3d(x_pt.x, x_pt.y, x_pt.z);
 
@@ -751,29 +748,23 @@ void pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeHess
         break;
     }
 
-    for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it =
-           neighborhood.begin();
-         neighborhood_it != neighborhood.end(); ++neighborhood_it) {
-      cell = *neighborhood_it;
+    for (auto & cell : neighborhood) {
+      x_pt = input_->points[idx];
+      x = Eigen::Vector3d(x_pt.x, x_pt.y, x_pt.z);
 
-      {
-        x_pt = input_->points[idx];
-        x = Eigen::Vector3d(x_pt.x, x_pt.y, x_pt.z);
+      x_trans = Eigen::Vector3d(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
 
-        x_trans = Eigen::Vector3d(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
+      // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
+      x_trans -= cell->getMean();
+      // Uses precomputed covariance for speed.
+      c_inv = cell->getInverseCov();
 
-        // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
-        x_trans -= cell->getMean();
-        // Uses precomputed covariance for speed.
-        c_inv = cell->getInverseCov();
-
-        // Compute derivative of transform function w.r.t. transform vector, J_E and H_E in
-        // Equations 6.18 and 6.20 [Magnusson 2009]
-        computePointDerivatives(x, point_gradient, point_hessian);
-        // Update hessian, lines 21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13,
-        // respectively [Magnusson 2009]
-        updateHessian(hessian, point_gradient, point_hessian, x_trans, c_inv);
-      }
+      // Compute derivative of transform function w.r.t. transform vector, J_E and H_E in
+      // Equations 6.18 and 6.20 [Magnusson 2009]
+      computePointDerivatives(x, point_gradient, point_hessian);
+      // Update hessian, lines 21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13,
+      // respectively [Magnusson 2009]
+      updateHessian(hessian, point_gradient, point_hessian, x_trans, c_inv);
     }
   }
 }
@@ -1087,11 +1078,13 @@ double pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeSt
   return (a_t);
 }
 
+// change at 20220721 konishi
 template <typename PointSource, typename PointTarget>
 double pclomp::NormalDistributionsTransform<PointSource, PointTarget>::calculateScore(
-  const PointCloudSource & trans_cloud) const
+  const PointCloudSource & trans_cloud)
 {
   double score = 0;
+  std::map<size_t, size_t> voxel_points_num;
 
   for (std::size_t idx = 0; idx < trans_cloud.points.size(); idx++) {
     PointSource x_trans_pt = trans_cloud.points[idx];
@@ -1115,32 +1108,62 @@ double pclomp::NormalDistributionsTransform<PointSource, PointTarget>::calculate
         break;
     }
 
-    for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it =
-           neighborhood.begin();
-         neighborhood_it != neighborhood.end(); ++neighborhood_it) {
-      TargetGridLeafConstPtr cell = *neighborhood_it;
+    // add at 20220218 by konishi
+    size_t voxel_idx;
 
-      Eigen::Vector3d x_trans = Eigen::Vector3d(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
+    if (neighborhood.size() == 0) {
+      // Compute the 3D index of the voxel containing the query point
+      Eigen::Vector3i vid;
 
-      // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
-      x_trans -= cell->getMean();
-      // Uses precomputed covariance for speed.
-      Eigen::Matrix3d c_inv = cell->getInverseCov();
+      vid(0) = static_cast<int>(floor(x_trans_pt.x / params_.resolution));
+      vid(1) = static_cast<int>(floor(x_trans_pt.y / params_.resolution));
+      vid(2) = static_cast<int>(floor(x_trans_pt.z / params_.resolution));
 
-      // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009]
-      double e_x_cov_x = exp(-gauss_d2_ * x_trans.dot(c_inv * x_trans) / 2);
-      // Calculate probability of transformed points existence, Equation 6.9 [Magnusson 2009]
-      double score_inc = -gauss_d1_ * e_x_cov_x - gauss_d3_;
+      empty_voxels_.insert(vid);
+
+      if (nomap_points_num_.count(voxel_idx) == 0) {
+        nomap_points_num_[voxel_idx] = 0;
+      }
+      nomap_points_num_[voxel_idx] += 1;
+
+    } else {
+      for (auto & cell : neighborhood) {
+        PointSource x_pt = input_->points[idx];
+        Eigen::Vector3d x = Eigen::Vector3d(x_pt.x, x_pt.y, x_pt.z);
+
+        Eigen::Vector3d x_trans = Eigen::Vector3d(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
+
+        // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
+        x_trans -= cell->getMean();
+        // Uses precomputed covariance for speed.
+        Eigen::Matrix3d c_inv = cell->getInverseCov();
+
+        // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009]
+        double e_x_cov_x = exp(-gauss_d2_ * x_trans.dot(c_inv * x_trans) / 2);
+        // Calculate probability of transformed points existence, Equation 6.9 [Magnusson 2009]
+        double score_inc = -gauss_d1_ * e_x_cov_x;
 
-      score += score_inc / neighborhood.size();
+        score += score_inc;
+
+        voxel_idx = target_cells_.getLeafIndex(cell->getMean());
+
+        if (voxel_points_num.count(voxel_idx) == 0) {
+          voxel_points_num[voxel_idx] = 0;
+          voxel_score_map_[voxel_idx] = 0;
+        }
+
+        voxel_score_map_[voxel_idx] += score_inc;
+        voxel_points_num[voxel_idx] += 1;
+      }
     }
   }
-
-  double output_score = 0;
-  if (!trans_cloud.points.empty()) {
-    output_score = (score) / static_cast<double>(trans_cloud.size());
+  for (auto & voxel_score_output : voxel_score_map_) {
+    if (voxel_points_num[voxel_score_output.first] != 0) {
+      voxel_score_output.second /= (voxel_points_num[voxel_score_output.first]);
+    }
   }
-  return output_score;
+
+  return (score) / static_cast<double>(trans_cloud.size());
 }
 
 template <typename PointSource, typename PointTarget>
@@ -1172,11 +1195,7 @@ pclomp::NormalDistributionsTransform<PointSource, PointTarget>::calculateTransfo
         break;
     }
 
-    for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it =
-           neighborhood.begin();
-         neighborhood_it != neighborhood.end(); ++neighborhood_it) {
-      TargetGridLeafConstPtr cell = *neighborhood_it;
-
+    for (auto & cell : neighborhood) {
       Eigen::Vector3d x_trans = Eigen::Vector3d(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
 
       // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
@@ -1230,11 +1249,7 @@ double pclomp::NormalDistributionsTransform<PointSource, PointTarget>::
         break;
     }
 
-    for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it =
-           neighborhood.begin();
-         neighborhood_it != neighborhood.end(); ++neighborhood_it) {
-      TargetGridLeafConstPtr cell = *neighborhood_it;
-
+    for (auto & cell : neighborhood) {
       Eigen::Vector3d x_trans = Eigen::Vector3d(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
 
       // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]

include/pclomp/voxel_grid_covariance_omp.h

@@ -101,13 +101,15 @@ class VoxelGridCovariance : public pcl::VoxelGrid<PointT>
   struct Leaf
   {
     /** \brief Constructor.
-     * Sets \ref nr_points, \ref icov_, \ref mean_ and \ref evals_ to 0 and \ref cov_ and \ref
+     * 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),
+    : nr_points_(0),
       mean_(Eigen::Vector3d::Zero()),
-      centroid(),
+      // add 20220721 konishi
+      voxelXYZ_(Eigen::Vector3d::Zero()),
+      centroid_(),
       cov_(Eigen::Matrix3d::Identity()),
       icov_(Eigen::Matrix3d::Zero()),
       evecs_(Eigen::Matrix3d::Identity()),
@@ -129,6 +131,8 @@ class VoxelGridCovariance : public pcl::VoxelGrid<PointT>
      * \return centroid
      */
     Eigen::Vector3d getMean() const { return (mean_); }
+    // add at 20220721 by konishi
+    Eigen::Vector3d getLeafCenter() const { return (voxelXYZ_); }
 
     /** \brief Get the eigen vectors of the voxel covariance.
      * \note Order corresponds with \ref getEvals
@@ -145,18 +149,21 @@ class VoxelGridCovariance : public pcl::VoxelGrid<PointT>
     /** \brief Get the number of points contained by this voxel.
      * \return number of points
      */
-    int getPointCount() const { return (nr_points); }
+    int getPointCount() const { return (nr_points_); }
 
     /** \brief Number of points contained by voxel */
-    int nr_points;
+    int nr_points_;
 
     /** \brief 3D voxel centroid */
     Eigen::Vector3d mean_;
 
+    // add at 20220721 by konishi
+    Eigen::Vector3d voxelXYZ_;
+
     /** \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_;
@@ -329,6 +336,36 @@ class VoxelGridCovariance : public pcl::VoxelGrid<PointT>
       return NULL;
   }
 
+  // add at 20220218 by konishi
+  inline size_t getLeafIndex(const Eigen::Vector3d & p)
+  {
+    // Generate index associated with p
+    int ijk0 = static_cast<int>(floor(p[0] * inverse_leaf_size_[0]) - min_b_[0]);
+    int ijk1 = static_cast<int>(floor(p[1] * inverse_leaf_size_[1]) - min_b_[1]);
+    int ijk2 = static_cast<int>(floor(p[2] * inverse_leaf_size_[2]) - min_b_[2]);
+
+    // Compute the centroid leaf index
+    return (ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]);
+  }
+
+  // add at 20220721 by konishi
+  inline Eigen::Vector3d
+  // getLeafCenterの命名ほうがよい
+  getLeafCenter(const size_t index)
+  {
+    int ijk2 = index / divb_mul_[2];
+    int ijk1 = (index % divb_mul_[2]) / divb_mul_[1];
+    int ijk0 = ((index % divb_mul_[2]) % divb_mul_[1]) / divb_mul_[0];
+
+    Eigen::Vector3d p;
+
+    p[0] = (static_cast<double>(ijk0 + min_b_[0]) + 0.5) / inverse_leaf_size_[0];
+    p[1] = (static_cast<double>(ijk1 + min_b_[1]) + 0.5) / inverse_leaf_size_[1];
+    p[2] = (static_cast<double>(ijk2 + min_b_[2]) + 0.5) / inverse_leaf_size_[2];
+
+    return p;
+  }
+
   /** \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). \param[in] reference_point the point to get the leaf structure at \param[out]