feat(ndt_scan_matcher): add nearest voxel transfromation probability (t…

…ier4#364) * feat(ndt_scan_matcher): add nearest voxel transfromation probability Signed-off-by: YamatoAndo <[email protected]> * add calculateTransformationProbability funcs Signed-off-by: YamatoAndo <[email protected]> * add calculateTransformationProbability funcs Signed-off-by: YamatoAndo <[email protected]> * add converged_param_nearest_voxel_transformation_probability Signed-off-by: Yamato Ando <[email protected]> * fix error Signed-off-by: YamatoAndo <[email protected]> * refactoring convergence conditions Signed-off-by: YamatoAndo <[email protected]> * fix error Signed-off-by: YamatoAndo <[email protected]> * remove debug code Signed-off-by: YamatoAndo <[email protected]> * remove debug code Signed-off-by: YamatoAndo <[email protected]> * ci(pre-commit): autofix * fix typo Signed-off-by: Yamato Ando <[email protected]> * ci(pre-commit): autofix * rename likelihood Signed-off-by: YamatoAndo <[email protected]> * ci(pre-commit): autofix * avoid a warning unused parameter Signed-off-by: YamatoAndo <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
boyali · Oct 3, 2022 · 55bf535 · 55bf535
1 parent 90ac4e9
commit 55bf535
Show file tree

Hide file tree

Showing 12 changed files with 198 additions and 8 deletions.
diff --git a/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml b/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml
@@ -23,9 +23,19 @@
     # The number of iterations required to calculate alignment
     max_iterations: 30
 
+    # Converged param type
+    # 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD
+    # NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD is only available when NDTImplementType::OMP is selected
+    converged_param_type: 1
+
+    # If converged_param_type is 0
     # Threshold for deciding whether to trust the estimation result
     converged_param_transform_probability: 3.0
 
+    # If converged_param_type is 1
+    # Threshold for deciding whether to trust the estimation result
+    converged_param_nearest_voxel_transformation_likelihood: 2.3
+
     # The number of particles to estimate initial pose
     initial_estimate_particles_num: 100
 

diff --git a/launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml
@@ -3,7 +3,7 @@
 
   <include file="$(find-pkg-share ndt_scan_matcher)/launch/ndt_scan_matcher.launch.xml">
     <arg name="input_map_points_topic" value="/map/pointcloud_map"/>
-    <arg name="input_sensor_points_topic" value="/localization/util/downsample/pointcloud"/>
+    <arg name="input/pointcloud" value="/localization/util/downsample/pointcloud"/>
     <arg name="input_initial_pose_topic" value="/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias"/>
 
 

diff --git a/localization/ndt/include/ndt/base.hpp b/localization/ndt/include/ndt/base.hpp
@@ -44,6 +44,7 @@ class NormalDistributionsTransformBase
   virtual double getStepSize() const = 0;
   virtual double getTransformationEpsilon() = 0;
   virtual double getTransformationProbability() const = 0;
+  virtual double getNearestVoxelTransformationLikelihood() const = 0;
   virtual double getFitnessScore() = 0;
   virtual boost::shared_ptr<const pcl::PointCloud<PointTarget>> getInputTarget() const = 0;
   virtual boost::shared_ptr<const pcl::PointCloud<PointSource>> getInputSource() const = 0;
@@ -54,6 +55,11 @@ class NormalDistributionsTransformBase
   virtual Eigen::Matrix<double, 6, 6> getHessian() const = 0;
 
   virtual boost::shared_ptr<pcl::search::KdTree<PointTarget>> getSearchMethodTarget() const = 0;
+
+  virtual double calculateTransformationProbability(
+    const pcl::PointCloud<PointSource> & trans_cloud) const = 0;
+  virtual double calculateNearestVoxelTransformationLikelihood(
+    const pcl::PointCloud<PointSource> & trans_cloud) const = 0;
 };
 
 #include "ndt/impl/base.hpp"

diff --git a/localization/ndt/include/ndt/impl/omp.hpp b/localization/ndt/include/ndt/impl/omp.hpp
@@ -108,6 +108,13 @@ double NormalDistributionsTransformOMP<PointSource, PointTarget>::getTransformat
   return ndt_ptr_->getTransformationProbability();
 }
 
+template <class PointSource, class PointTarget>
+double NormalDistributionsTransformOMP<
+  PointSource, PointTarget>::getNearestVoxelTransformationLikelihood() const
+{
+  return ndt_ptr_->getNearestVoxelTransformationLikelihood();
+}
+
 template <class PointSource, class PointTarget>
 double NormalDistributionsTransformOMP<PointSource, PointTarget>::getFitnessScore()
 {
@@ -157,6 +164,22 @@ NormalDistributionsTransformOMP<PointSource, PointTarget>::getSearchMethodTarget
   return ndt_ptr_->getSearchMethodTarget();
 }
 
+template <class PointSource, class PointTarget>
+double
+NormalDistributionsTransformOMP<PointSource, PointTarget>::calculateTransformationProbability(
+  const pcl::PointCloud<PointSource> & trans_cloud) const
+{
+  return ndt_ptr_->calculateTransformationProbability(trans_cloud);
+}
+
+template <class PointSource, class PointTarget>
+double NormalDistributionsTransformOMP<PointSource, PointTarget>::
+  calculateNearestVoxelTransformationLikelihood(
+    const pcl::PointCloud<PointSource> & trans_cloud) const
+{
+  return ndt_ptr_->calculateNearestVoxelTransformationLikelihood(trans_cloud);
+}
+
 template <class PointSource, class PointTarget>
 void NormalDistributionsTransformOMP<PointSource, PointTarget>::setNumThreads(int n)
 {

diff --git a/localization/ndt/include/ndt/impl/pcl_generic.hpp b/localization/ndt/include/ndt/impl/pcl_generic.hpp
@@ -110,6 +110,14 @@ double NormalDistributionsTransformPCLGeneric<
   return ndt_ptr_->getTransformationProbability();
 }
 
+template <class PointSource, class PointTarget>
+double NormalDistributionsTransformPCLGeneric<
+  PointSource, PointTarget>::getNearestVoxelTransformationLikelihood() const
+{
+  // return ndt_ptr_->getNearestVoxelTransformationLikelihood();
+  return 0.0;
+}
+
 template <class PointSource, class PointTarget>
 double NormalDistributionsTransformPCLGeneric<PointSource, PointTarget>::getFitnessScore()
 {
@@ -160,4 +168,23 @@ NormalDistributionsTransformPCLGeneric<PointSource, PointTarget>::getSearchMetho
   return ndt_ptr_->getSearchMethodTarget();
 }
 
+template <class PointSource, class PointTarget>
+double NormalDistributionsTransformPCLGeneric<PointSource, PointTarget>::
+  calculateTransformationProbability(const pcl::PointCloud<PointSource> & trans_cloud) const
+{
+  (void)trans_cloud;
+  // return ndt_ptr_->calculateTransformationProbability(trans_cloud);
+  return 0.0;
+}
+
+template <class PointSource, class PointTarget>
+double NormalDistributionsTransformPCLGeneric<PointSource, PointTarget>::
+  calculateNearestVoxelTransformationLikelihood(
+    const pcl::PointCloud<PointSource> & trans_cloud) const
+{
+  (void)trans_cloud;
+  // return ndt_ptr_->calculateNearestVoxelTransformationLikelihood(trans_cloud);
+  return 0.0;
+}
+
 #endif  // NDT__IMPL__PCL_GENERIC_HPP_
diff --git a/localization/ndt/include/ndt/impl/pcl_modified.hpp b/localization/ndt/include/ndt/impl/pcl_modified.hpp
@@ -111,6 +111,14 @@ double NormalDistributionsTransformPCLModified<
   return ndt_ptr_->getTransformationProbability();
 }
 
+template <class PointSource, class PointTarget>
+double NormalDistributionsTransformPCLModified<
+  PointSource, PointTarget>::getNearestVoxelTransformationLikelihood() const
+{
+  // return ndt_ptr_->getTransformationLikelihood();
+  return 0.0;
+}
+
 template <class PointSource, class PointTarget>
 double NormalDistributionsTransformPCLModified<PointSource, PointTarget>::getFitnessScore()
 {
@@ -160,4 +168,23 @@ NormalDistributionsTransformPCLModified<PointSource, PointTarget>::getSearchMeth
   return ndt_ptr_->getSearchMethodTarget();
 }
 
+template <class PointSource, class PointTarget>
+double NormalDistributionsTransformPCLModified<PointSource, PointTarget>::
+  calculateTransformationProbability(const pcl::PointCloud<PointSource> & trans_cloud) const
+{
+  (void)trans_cloud;
+  // return ndt_ptr_->calculateTransformationProbability(trans_cloud);
+  return 0.0;
+}
+
+template <class PointSource, class PointTarget>
+double NormalDistributionsTransformPCLModified<PointSource, PointTarget>::
+  calculateNearestVoxelTransformationLikelihood(
+    const pcl::PointCloud<PointSource> & trans_cloud) const
+{
+  (void)trans_cloud;
+  // return ndt_ptr_->calculateNearestVoxelTransformationLikelihood(trans_cloud);
+  return 0.0;
+}
+
 #endif  // NDT__IMPL__PCL_MODIFIED_HPP_
diff --git a/localization/ndt/include/ndt/omp.hpp b/localization/ndt/include/ndt/omp.hpp
@@ -47,6 +47,7 @@ class NormalDistributionsTransformOMP
   double getStepSize() const override;
   double getTransformationEpsilon() override;
   double getTransformationProbability() const override;
+  double getNearestVoxelTransformationLikelihood() const override;
   double getFitnessScore() override;
   boost::shared_ptr<const pcl::PointCloud<PointTarget>> getInputTarget() const override;
   boost::shared_ptr<const pcl::PointCloud<PointSource>> getInputSource() const override;
@@ -58,6 +59,11 @@ class NormalDistributionsTransformOMP
 
   boost::shared_ptr<pcl::search::KdTree<PointTarget>> getSearchMethodTarget() const override;
 
+  double calculateTransformationProbability(
+    const pcl::PointCloud<PointSource> & trans_cloud) const override;
+  double calculateNearestVoxelTransformationLikelihood(
+    const pcl::PointCloud<PointSource> & trans_cloud) const override;
+
   // only OMP Impl
   void setNumThreads(int n);
   void setNeighborhoodSearchMethod(pclomp::NeighborSearchMethod method);

diff --git a/localization/ndt/include/ndt/pcl_generic.hpp b/localization/ndt/include/ndt/pcl_generic.hpp
@@ -47,6 +47,7 @@ class NormalDistributionsTransformPCLGeneric
   double getStepSize() const override;
   double getTransformationEpsilon() override;
   double getTransformationProbability() const override;
+  double getNearestVoxelTransformationLikelihood() const override;
   double getFitnessScore() override;
   boost::shared_ptr<const pcl::PointCloud<PointTarget>> getInputTarget() const override;
   boost::shared_ptr<const pcl::PointCloud<PointSource>> getInputSource() const override;
@@ -58,6 +59,11 @@ class NormalDistributionsTransformPCLGeneric
 
   boost::shared_ptr<pcl::search::KdTree<PointTarget>> getSearchMethodTarget() const override;
 
+  double calculateTransformationProbability(
+    const pcl::PointCloud<PointSource> & trans_cloud) const override;
+  double calculateNearestVoxelTransformationLikelihood(
+    const pcl::PointCloud<PointSource> & trans_cloud) const override;
+
 private:
   boost::shared_ptr<pcl::NormalDistributionsTransform<PointSource, PointTarget>> ndt_ptr_;
 };

diff --git a/localization/ndt/include/ndt/pcl_modified.hpp b/localization/ndt/include/ndt/pcl_modified.hpp
@@ -48,6 +48,7 @@ class NormalDistributionsTransformPCLModified
   double getStepSize() const override;
   double getTransformationEpsilon() override;
   double getTransformationProbability() const override;
+  double getNearestVoxelTransformationLikelihood() const override;
   double getFitnessScore() override;
   boost::shared_ptr<const pcl::PointCloud<PointTarget>> getInputTarget() const override;
   boost::shared_ptr<const pcl::PointCloud<PointSource>> getInputSource() const override;
@@ -59,6 +60,11 @@ class NormalDistributionsTransformPCLModified
 
   boost::shared_ptr<pcl::search::KdTree<PointTarget>> getSearchMethodTarget() const override;
 
+  double calculateTransformationProbability(
+    const pcl::PointCloud<PointSource> & trans_cloud) const override;
+  double calculateNearestVoxelTransformationLikelihood(
+    const pcl::PointCloud<PointSource> & trans_cloud) const override;
+
 private:
   boost::shared_ptr<pcl::NormalDistributionsTransformModified<PointSource, PointTarget>> ndt_ptr_;
 };

diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml
@@ -24,9 +24,19 @@
     # The number of iterations required to calculate alignment
     max_iterations: 30
 
+    # Converged param type
+    # 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD
+    # NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD is only available when NDTImplementType::OMP is selected
+    converged_param_type: 1
+
+    # If converged_param_type is 0
     # Threshold for deciding whether to trust the estimation result
     converged_param_transform_probability: 3.0
 
+    # If converged_param_type is 1
+    # Threshold for deciding whether to trust the estimation result
+    converged_param_nearest_voxel_transformation_likelihood: 2.3
+
     # The number of particles to estimate initial pose
     initial_estimate_particles_num: 100
 

diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp
@@ -52,6 +52,10 @@
 #include <vector>
 
 enum class NDTImplementType { PCL_GENERIC = 0, PCL_MODIFIED = 1, OMP = 2 };
+enum class ConvergedParamType {
+  TRANSFORM_PROBABILITY = 0,
+  NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1
+};
 
 template <typename PointSource, typename PointTarget>
 std::shared_ptr<NormalDistributionsTransformBase<PointSource, PointTarget>> getNDT(
@@ -134,6 +138,8 @@ class NDTScanMatcher : public rclcpp::Node
     initial_pose_with_covariance_pub_;
   rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr exe_time_pub_;
   rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr transform_probability_pub_;
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr
+    nearest_voxel_transformation_likelihood_pub_;
   rclcpp::Publisher<tier4_debug_msgs::msg::Int32Stamped>::SharedPtr iteration_num_pub_;
   rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr
     initial_to_result_distance_pub_;
@@ -159,7 +165,11 @@ class NDTScanMatcher : public rclcpp::Node
   std::string base_frame_;
   std::string ndt_base_frame_;
   std::string map_frame_;
+
+  ConvergedParamType converged_param_type_;
   double converged_param_transform_probability_;
+  double converged_param_nearest_voxel_transformation_likelihood_;
+
   int initial_estimate_particles_num_;
   double initial_pose_timeout_sec_;
   double initial_pose_distance_tolerance_m_;