Merge branch 'tier4/main' into feat/add_covariance_estimation

tier4 · Mar 13, 2024 · caaed39 · caaed39
2 parents 5ff8fde + 19b30f0
commit caaed39
Show file tree

Hide file tree

Showing 4 changed files with 205 additions and 202 deletions.
diff --git a/include/multigrid_pclomp/multigrid_ndt_omp.h b/include/multigrid_pclomp/multigrid_ndt_omp.h
@@ -60,40 +60,11 @@
 #include <pcl/search/impl/search.hpp>
 #include <pcl/registration/registration.h>
 #include "multigrid_pclomp/multi_voxel_grid_covariance_omp.h"
+#include "../pclomp/ndt_struct.hpp"
 
 #include <unsupported/Eigen/NonLinearOptimization>
 
 namespace pclomp {
-enum NeighborSearchMethod { KDTREE, DIRECT26, DIRECT7, DIRECT1 };
-
-struct NdtResult {
-  Eigen::Matrix4f pose;
-  float transform_probability;
-  float nearest_voxel_transformation_likelihood;
-  int iteration_num;
-  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 {
-  double trans_epsilon;
-  double step_size;
-  double resolution;
-  int max_iterations;
-  int num_threads;
-  float regularization_scale_factor;
-};
 
 /** \brief A 3D Normal Distribution Transform registration implementation for point cloud data.
  * \note For more information please see
@@ -169,6 +140,9 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
     // No idea why PCL does not check the nullity of input
     if(input) {
       pcl::Registration<PointSource, PointTarget>::setInputSource(input);
+    } else {
+      std::cerr << "Error: Null input source cloud is not allowed" << std::endl;
+      exit(EXIT_FAILURE);
     }
   }
 
@@ -392,7 +366,7 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
    * \param[in] c_inv covariance of occupied covariance voxel
    * \param[in] compute_hessian flag to calculate hessian, unnecessary for step calculation.
    */
-  double updateDerivatives(Eigen::Matrix<double, 6, 1> &score_gradient, Eigen::Matrix<double, 6, 6> &hessian, const Eigen::Matrix<float, 4, 6> &point_gradient_, const Eigen::Matrix<float, 24, 6> &point_hessian_, const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv, bool compute_hessian = true) const;
+  double updateDerivatives(Eigen::Matrix<double, 6, 1> &score_gradient, Eigen::Matrix<double, 6, 6> &hessian, const Eigen::Matrix<float, 4, 6> &point_gradient, const Eigen::Matrix<float, 24, 6> &point_hessian, const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv, bool compute_hessian = true) const;
 
   /** \brief Precompute angular components of derivatives.
    * \note Equation 6.19 and 6.21 [Magnusson 2009].
@@ -406,9 +380,9 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
    * \param[in] x point from the input cloud
    * \param[in] compute_hessian flag to calculate hessian, unnecessary for step calculation.
    */
-  void computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix<double, 3, 6> &point_gradient_, Eigen::Matrix<double, 18, 6> &point_hessian_, bool compute_hessian = true) const;
+  void computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix<double, 3, 6> &point_gradient, Eigen::Matrix<double, 18, 6> &point_hessian, bool compute_hessian = true) const;
 
-  void computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix<float, 4, 6> &point_gradient_, Eigen::Matrix<float, 24, 6> &point_hessian_, bool compute_hessian = true) const;
+  void computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix<float, 4, 6> &point_gradient, Eigen::Matrix<float, 24, 6> &point_hessian, bool compute_hessian = true) const;
 
   /** \brief Compute hessian of probability function w.r.t. the transformation vector.
    * \note Equation 6.13 [Magnusson 2009].
@@ -424,7 +398,7 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
    * \param[in] x_trans transformed point minus mean of occupied covariance voxel
    * \param[in] c_inv covariance of occupied covariance voxel
    */
-  void updateHessian(Eigen::Matrix<double, 6, 6> &hessian, const Eigen::Matrix<double, 3, 6> &point_gradient_, const Eigen::Matrix<double, 18, 6> &point_hessian_, const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv) const;
+  void updateHessian(Eigen::Matrix<double, 6, 6> &hessian, const Eigen::Matrix<double, 3, 6> &point_gradient, const Eigen::Matrix<double, 18, 6> &point_hessian, const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv) const;
 
   /** \brief Compute line search step length and update transform and probability derivatives using More-Thuente method.
    * \note Search Algorithm [More, Thuente 1994]
@@ -534,13 +508,6 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
   Eigen::Vector3d h_ang_a2_, h_ang_a3_, h_ang_b2_, h_ang_b3_, h_ang_c2_, h_ang_c3_, h_ang_d1_, h_ang_d2_, h_ang_d3_, h_ang_e1_, h_ang_e2_, h_ang_e3_, h_ang_f1_, h_ang_f2_, h_ang_f3_;
 
   Eigen::Matrix<float, 16, 4> h_ang;
-
-  /** \brief The first order derivative of the transformation of a point w.r.t. the transform vector, \f$ J_E \f$ in Equation 6.18 [Magnusson 2009]. */
-  //      Eigen::Matrix<double, 3, 6> point_gradient_;
-
-  /** \brief The second order derivative of the transformation of a point w.r.t. the transform vector, \f$ H_E \f$ in Equation 6.20 [Magnusson 2009]. */
-  //      Eigen::Matrix<double, 18, 6> point_hessian_;
-
   int num_threads_;
 
   Eigen::Matrix<double, 6, 6> hessian_;