refactor: organized ndt params (#43)

* Organized ndt params Signed-off-by: Shintaro Sakoda <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: Shintaro Sakoda <[email protected]> Co-authored-by: SakodaShintaro <[email protected]>
tier4 · Mar 18, 2024 · 7225f0e · 7225f0e
1 parent 275081c
commit 7225f0e
Show file tree

Hide file tree

Showing 4 changed files with 157 additions and 145 deletions.
diff --git a/include/multigrid_pclomp/multigrid_ndt_omp.h b/include/multigrid_pclomp/multigrid_ndt_omp.h
@@ -107,7 +107,7 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
 #endif
 
   /** \brief Constructor.
-   * Sets \ref outlier_ratio_ to 0.35, \ref step_size_ to 0.05 and \ref resolution_ to 1.0
+   * Sets \ref outlier_ratio_ to 0.35, \ref params_.step_size to 0.05 and \ref params_.resolution to 1.0
    */
   MultiGridNormalDistributionsTransform();
 
@@ -123,11 +123,11 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
   virtual ~MultiGridNormalDistributionsTransform() {}
 
   void setNumThreads(int n) {
-    num_threads_ = n;
+    params_.num_threads = n;
   }
 
   inline int getNumThreads() const {
-    return num_threads_;
+    return params_.num_threads;
   }
 
   inline void setInputSource(const PointCloudSourceConstPtr &input) {
@@ -152,7 +152,7 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
    */
   inline void addTarget(const PointCloudTargetConstPtr &cloud, const std::string &target_id) {
     BaseRegType::setInputTarget(cloud);
-    target_cells_.setLeafSize(resolution_, resolution_, resolution_);
+    target_cells_.setLeafSize(params_.resolution, params_.resolution, params_.resolution);
     target_cells_.setInputCloudAndFilter(cloud, target_id);
   }
 
@@ -169,8 +169,8 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
    */
   inline void setResolution(float resolution) {
     // Prevents unnecessary voxel initiations
-    if(resolution_ != resolution) {
-      resolution_ = resolution;
+    if(params_.resolution != resolution) {
+      params_.resolution = resolution;
       if(input_) init();
     }
   }
@@ -179,21 +179,21 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
    * \return side length of voxels
    */
   inline float getResolution() const {
-    return (resolution_);
+    return (params_.resolution);
   }
 
   /** \brief Get the newton line search maximum step length.
    * \return maximum step length
    */
   inline double getStepSize() const {
-    return (step_size_);
+    return (params_.step_size);
   }
 
   /** \brief Set/change the newton line search maximum step length.
    * \param[in] step_size maximum step length
    */
   inline void setStepSize(double step_size) {
-    step_size_ = step_size;
+    params_.step_size = step_size;
   }
 
   /** \brief Get the point cloud outlier ratio.
@@ -263,7 +263,7 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
   double calculateNearestVoxelTransformationLikelihood(const PointCloudSource &cloud) const;
 
   inline void setRegularizationScaleFactor(float regularization_scale_factor) {
-    regularization_scale_factor_ = regularization_scale_factor;
+    params_.regularization_scale_factor = regularization_scale_factor;
   }
 
   inline void setRegularizationPose(Eigen::Matrix4f regularization_pose) {
@@ -285,24 +285,36 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
     return ndt_result;
   }
 
+  /** \brief Set the transformation epsilon (maximum allowable translation squared
+   * difference between two consecutive transformations) in order for an optimization to
+   * be considered as having converged to the final solution. \param[in] epsilon the
+   * transformation epsilon in order for an optimization to be considered as having
+   * converged to the final solution.
+   */
+  inline void setTransformationEpsilon(double epsilon) {
+    params_.trans_epsilon = epsilon;
+  }
+
+  /** \brief Get the transformation epsilon (maximum allowable translation squared
+   * difference between two consecutive transformations) as set by the user.
+   */
+  inline double getTransformationEpsilon() {
+    return (params_.trans_epsilon);
+  }
+
+  inline void setMaximumIterations(int max_iterations) {
+    params_.max_iterations = max_iterations;
+  }
+  inline int getMaxIterations() const {
+    return params_.max_iterations;
+  }
+
   void setParams(const NdtParams &ndt_params) {
-    this->setTransformationEpsilon(ndt_params.trans_epsilon);
-    this->setStepSize(ndt_params.step_size);
-    this->setResolution(ndt_params.resolution);
-    this->setMaximumIterations(ndt_params.max_iterations);
-    setRegularizationScaleFactor(ndt_params.regularization_scale_factor);
-    setNumThreads(ndt_params.num_threads);
+    params_ = ndt_params;
   }
 
   NdtParams getParams() const {
-    NdtParams ndt_params;
-    ndt_params.trans_epsilon = transformation_epsilon_;
-    ndt_params.step_size = getStepSize();
-    ndt_params.resolution = getResolution();
-    ndt_params.max_iterations = max_iterations_;
-    ndt_params.regularization_scale_factor = regularization_scale_factor_;
-    ndt_params.num_threads = num_threads_;
-    return ndt_params;
+    return params_;
   }
 
   pcl::PointCloud<PointTarget> getVoxelPCD() const {
@@ -471,12 +483,6 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
 
   // double fitness_epsilon_;
 
-  /** \brief The side length of voxels. */
-  float resolution_;
-
-  /** \brief The maximum step length. */
-  double step_size_;
-
   /** \brief The ratio of outliers of points w.r.t. a normal distribution, Equation 6.7 [Magnusson 2009]. */
   double outlier_ratio_;
 
@@ -490,23 +496,25 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
    *
    * The precomputed angular derivatives for the jacobian of a transformation vector, Equation 6.19 [Magnusson 2009].
    */
-  Eigen::Matrix<double, 8, 4> j_ang_;
+  Eigen::Matrix<float, 8, 4> j_ang_;
 
   /** \brief Precomputed Angular Hessian
    *
    * The precomputed angular derivatives for the hessian of a transformation vector, Equation 6.19 [Magnusson 2009].
    */
-  Eigen::Matrix<double, 16, 4> h_ang_;
-  int num_threads_;
+  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;
 
   Eigen::Matrix<double, 6, 6> hessian_;
   std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> transformation_array_;
   double nearest_voxel_transformation_likelihood_;
 
-  float regularization_scale_factor_;
   boost::optional<Eigen::Matrix4f> regularization_pose_;
   Eigen::Vector3f regularization_pose_translation_;
 
+  NdtParams params_;
+
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };