Modified some codes

Signed-off-by: Shintaro Sakoda <[email protected]>

include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h

@@ -142,50 +142,20 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid<PointT> {
       return *this;
     }
 
-    /** \brief Get the voxel covariance.
-     * \return covariance matrix
-     */
-    Eigen::Matrix3d getCov() const {
-      return (cov_);
-    }
-
     /** \brief Get the inverse of the voxel covariance.
      * \return inverse covariance matrix
      */
-    Eigen::Matrix3d getInverseCov() const {
+    const Eigen::Matrix3d &getInverseCov() const {
       return (icov_);
     }
 
     /** \brief Get the voxel centroid.
      * \return centroid
      */
-    Eigen::Vector3d getMean() const {
+    const Eigen::Vector3d &getMean() const {
       return (mean_);
     }
 
-    /** \brief Get the eigen vectors of the voxel covariance.
-     * \note Order corresponds with \ref getEvals
-     * \return matrix whose columns contain eigen vectors
-     */
-    Eigen::Matrix3d getEvecs() const {
-      return (evecs_);
-    }
-
-    /** \brief Get the eigen values of the voxel covariance.
-     * \note Order corresponds with \ref getEvecs
-     * \return vector of eigen values
-     */
-    Eigen::Vector3d getEvals() const {
-      return (evals_);
-    }
-
-    /** \brief Get the number of points contained by this voxel.
-     * \return number of points
-     */
-    int getPointCount() const {
-      return (nr_points_);
-    }
-
     /** \brief Number of points contained by voxel */
     int nr_points_;
 
@@ -261,7 +231,7 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid<PointT> {
   MultiVoxelGridCovariance &operator=(const MultiVoxelGridCovariance &other);
   MultiVoxelGridCovariance &operator=(MultiVoxelGridCovariance &&other);
 
-  /** \brief Initializes voxel structure.
+  /** \brief Add a cloud to the voxel grid list and build a ND voxel grid from it.
    */
   void setInputCloudAndFilter(const PointCloudConstPtr &cloud, const std::string &grid_id);
 
@@ -294,8 +264,10 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid<PointT> {
    */
   int radiusSearch(const PointCloud &cloud, int index, double radius, std::vector<LeafConstPtr> &k_leaves, unsigned int max_nn = 0) const;
 
+  // Return a pointer to avoid multiple deep copies
   PointCloud getVoxelPCD() const;
 
+  // Return the string indices of currently loaded map pieces
   std::vector<std::string> getCurrentMapIDs() const;
 
 protected:

include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp

@@ -341,31 +341,25 @@ double MultiGridNormalDistributionsTransform<PointSource, PointTarget>::computeD
     // 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);
 
+    // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
+    const Eigen::Vector3d x_trans(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
+
     double sum_score_pt = 0;
     double nearest_voxel_score_pt = 0;
     auto &score_gradient_pt = score_gradients[tid];
     auto &hessian_pt = hessians[tid];
 
     for(auto &cell : neighborhood) {
-      // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
-      Eigen::Vector3d x_trans(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
-
-      x_trans -= cell->getMean();
-      // Uses precomputed covariance for speed.
-      auto c_inv = cell->getInverseCov();
-
       // Update score, gradient and hessian, lines 19-21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13, respectively [Magnusson 2009]
-      double score_pt = updateDerivatives(score_gradient_pt, hessian_pt, point_gradient, point_hessian, x_trans, c_inv, compute_hessian);
+      double score_pt = updateDerivatives(score_gradient_pt, hessian_pt, point_gradient, point_hessian, x_trans - cell->getMean(), cell->getInverseCov(), compute_hessian);
       sum_score_pt += score_pt;
 
       if(score_pt > nearest_voxel_score_pt) {
         nearest_voxel_score_pt = score_pt;
       }
     }
 
-    if(!neighborhood.empty()) {
-      ++found_neigborhood_voxel_nums[tid];
-    }
+    ++found_neigborhood_voxel_nums[tid];
 
     scores[tid] += sum_score_pt;
     nearest_voxel_scores[tid] += nearest_voxel_score_pt;
@@ -616,6 +610,8 @@ void MultiGridNormalDistributionsTransform<PointSource, PointTarget>::computeHes
     auto &x_pt = (*input_)[idx];
     // For math
     Eigen::Vector3d x(x_pt.x, x_pt.y, x_pt.z);
+
+    // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
     const Eigen::Vector3d x_trans(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
 
     auto &point_gradient = t_point_gradients[tid];
@@ -626,15 +622,8 @@ void MultiGridNormalDistributionsTransform<PointSource, PointTarget>::computeHes
     computePointDerivatives(x, point_gradient, point_hessian);
 
     for(auto &cell : neighborhood) {
-      // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
-      Eigen::Vector3d x_trans(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
-
-      // Uses precomputed covariance for speed.
-      x_trans -= cell->getMean();
-      auto c_inv = cell->getInverseCov();
-
       // Update hessian, lines 21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13, respectively [Magnusson 2009]
-      updateHessian(tmp_hessian, point_gradient, point_hessian, x_trans, c_inv);
+      updateHessian(tmp_hessian, point_gradient, point_hessian, x_trans - cell->getMean(), cell->getInverseCov());
     }
   }
 
@@ -953,12 +942,10 @@ double MultiGridNormalDistributionsTransform<PointSource, PointTarget>::calculat
 
       // 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();
 
       // Calculate probability of transformed points existence, Equation 6.9 [Magnusson 2009]
       // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009]
-      tmp_score -= gauss_d1_ * exp(-gauss_d2_ * x_trans.dot(c_inv * x_trans) / 2.0);
+      tmp_score -= gauss_d1_ * exp(-gauss_d2_ * x_trans.dot(cell->getInverseCov() * x_trans) / 2.0);
     }
 
     t_scores[tid] += tmp_score;
@@ -1012,11 +999,9 @@ double MultiGridNormalDistributionsTransform<PointSource, PointTarget>::calculat
 
       // 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.0);
+      double e_x_cov_x = exp(-gauss_d2_ * x_trans.dot(cell->getInverseCov() * x_trans) / 2.0);
       // Calculate probability of transformed points existence, Equation 6.9 [Magnusson 2009]
       double score_inc = -gauss_d1_ * e_x_cov_x;