From 55bf5358440161a6620f039024bd6d6398465e1d Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Thu, 31 Mar 2022 15:24:02 +0900 Subject: [PATCH] feat(ndt_scan_matcher): add nearest voxel transfromation probability (#364) * feat(ndt_scan_matcher): add nearest voxel transfromation probability Signed-off-by: YamatoAndo * add calculateTransformationProbability funcs Signed-off-by: YamatoAndo * add calculateTransformationProbability funcs Signed-off-by: YamatoAndo * add converged_param_nearest_voxel_transformation_probability Signed-off-by: Yamato Ando * fix error Signed-off-by: YamatoAndo * refactoring convergence conditions Signed-off-by: YamatoAndo * fix error Signed-off-by: YamatoAndo * remove debug code Signed-off-by: YamatoAndo * remove debug code Signed-off-by: YamatoAndo * ci(pre-commit): autofix * fix typo Signed-off-by: Yamato Ando * ci(pre-commit): autofix * rename likelihood Signed-off-by: YamatoAndo * ci(pre-commit): autofix * avoid a warning unused parameter Signed-off-by: YamatoAndo Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../config/ndt_scan_matcher.param.yaml | 10 +++ .../pose_estimator/pose_estimator.launch.xml | 2 +- localization/ndt/include/ndt/base.hpp | 6 ++ localization/ndt/include/ndt/impl/omp.hpp | 23 ++++++ .../ndt/include/ndt/impl/pcl_generic.hpp | 27 +++++++ .../ndt/include/ndt/impl/pcl_modified.hpp | 27 +++++++ localization/ndt/include/ndt/omp.hpp | 6 ++ localization/ndt/include/ndt/pcl_generic.hpp | 6 ++ localization/ndt/include/ndt/pcl_modified.hpp | 6 ++ .../config/ndt_scan_matcher.param.yaml | 10 +++ .../ndt_scan_matcher_core.hpp | 10 +++ .../src/ndt_scan_matcher_core.cpp | 73 +++++++++++++++++-- 12 files changed, 198 insertions(+), 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 index fd21436b3909b..f5b454a887625 100644 --- 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 index 89934a5c723b5..34df363bf8c3b 100644 --- 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 @@ - + diff --git a/localization/ndt/include/ndt/base.hpp b/localization/ndt/include/ndt/base.hpp index 3f955ee98afc1..f06e5d057f273 100644 --- 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> getInputTarget() const = 0; virtual boost::shared_ptr> getInputSource() const = 0; @@ -54,6 +55,11 @@ class NormalDistributionsTransformBase virtual Eigen::Matrix getHessian() const = 0; virtual boost::shared_ptr> getSearchMethodTarget() const = 0; + + virtual double calculateTransformationProbability( + const pcl::PointCloud & trans_cloud) const = 0; + virtual double calculateNearestVoxelTransformationLikelihood( + const pcl::PointCloud & 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 index e0cd88a9574af..1308ec97d479f 100644 --- a/localization/ndt/include/ndt/impl/omp.hpp +++ b/localization/ndt/include/ndt/impl/omp.hpp @@ -108,6 +108,13 @@ double NormalDistributionsTransformOMP::getTransformat return ndt_ptr_->getTransformationProbability(); } +template +double NormalDistributionsTransformOMP< + PointSource, PointTarget>::getNearestVoxelTransformationLikelihood() const +{ + return ndt_ptr_->getNearestVoxelTransformationLikelihood(); +} + template double NormalDistributionsTransformOMP::getFitnessScore() { @@ -157,6 +164,22 @@ NormalDistributionsTransformOMP::getSearchMethodTarget return ndt_ptr_->getSearchMethodTarget(); } +template +double +NormalDistributionsTransformOMP::calculateTransformationProbability( + const pcl::PointCloud & trans_cloud) const +{ + return ndt_ptr_->calculateTransformationProbability(trans_cloud); +} + +template +double NormalDistributionsTransformOMP:: + calculateNearestVoxelTransformationLikelihood( + const pcl::PointCloud & trans_cloud) const +{ + return ndt_ptr_->calculateNearestVoxelTransformationLikelihood(trans_cloud); +} + template void NormalDistributionsTransformOMP::setNumThreads(int n) { diff --git a/localization/ndt/include/ndt/impl/pcl_generic.hpp b/localization/ndt/include/ndt/impl/pcl_generic.hpp index b817d2de35a5a..d05c946a459a4 100644 --- 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 +double NormalDistributionsTransformPCLGeneric< + PointSource, PointTarget>::getNearestVoxelTransformationLikelihood() const +{ + // return ndt_ptr_->getNearestVoxelTransformationLikelihood(); + return 0.0; +} + template double NormalDistributionsTransformPCLGeneric::getFitnessScore() { @@ -160,4 +168,23 @@ NormalDistributionsTransformPCLGeneric::getSearchMetho return ndt_ptr_->getSearchMethodTarget(); } +template +double NormalDistributionsTransformPCLGeneric:: + calculateTransformationProbability(const pcl::PointCloud & trans_cloud) const +{ + (void)trans_cloud; + // return ndt_ptr_->calculateTransformationProbability(trans_cloud); + return 0.0; +} + +template +double NormalDistributionsTransformPCLGeneric:: + calculateNearestVoxelTransformationLikelihood( + const pcl::PointCloud & 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 index 1b208da132d3e..4715b85ffb4d4 100644 --- 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 +double NormalDistributionsTransformPCLModified< + PointSource, PointTarget>::getNearestVoxelTransformationLikelihood() const +{ + // return ndt_ptr_->getTransformationLikelihood(); + return 0.0; +} + template double NormalDistributionsTransformPCLModified::getFitnessScore() { @@ -160,4 +168,23 @@ NormalDistributionsTransformPCLModified::getSearchMeth return ndt_ptr_->getSearchMethodTarget(); } +template +double NormalDistributionsTransformPCLModified:: + calculateTransformationProbability(const pcl::PointCloud & trans_cloud) const +{ + (void)trans_cloud; + // return ndt_ptr_->calculateTransformationProbability(trans_cloud); + return 0.0; +} + +template +double NormalDistributionsTransformPCLModified:: + calculateNearestVoxelTransformationLikelihood( + const pcl::PointCloud & 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 index 803190d22d4ce..95c5f1bd4b2dc 100644 --- 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> getInputTarget() const override; boost::shared_ptr> getInputSource() const override; @@ -58,6 +59,11 @@ class NormalDistributionsTransformOMP boost::shared_ptr> getSearchMethodTarget() const override; + double calculateTransformationProbability( + const pcl::PointCloud & trans_cloud) const override; + double calculateNearestVoxelTransformationLikelihood( + const pcl::PointCloud & 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 index ae1255b2dece7..8ddb7dcc34006 100644 --- 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> getInputTarget() const override; boost::shared_ptr> getInputSource() const override; @@ -58,6 +59,11 @@ class NormalDistributionsTransformPCLGeneric boost::shared_ptr> getSearchMethodTarget() const override; + double calculateTransformationProbability( + const pcl::PointCloud & trans_cloud) const override; + double calculateNearestVoxelTransformationLikelihood( + const pcl::PointCloud & trans_cloud) const override; + private: boost::shared_ptr> ndt_ptr_; }; diff --git a/localization/ndt/include/ndt/pcl_modified.hpp b/localization/ndt/include/ndt/pcl_modified.hpp index f9fa99bd782e3..34f50cd7553ba 100644 --- 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> getInputTarget() const override; boost::shared_ptr> getInputSource() const override; @@ -59,6 +60,11 @@ class NormalDistributionsTransformPCLModified boost::shared_ptr> getSearchMethodTarget() const override; + double calculateTransformationProbability( + const pcl::PointCloud & trans_cloud) const override; + double calculateNearestVoxelTransformationLikelihood( + const pcl::PointCloud & trans_cloud) const override; + private: boost::shared_ptr> 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 index 7b62785f7f11a..10050a57fe51d 100644 --- 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 index a74c5b47b51e7..e0dd7f4022b15 100644 --- 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 enum class NDTImplementType { PCL_GENERIC = 0, PCL_MODIFIED = 1, OMP = 2 }; +enum class ConvergedParamType { + TRANSFORM_PROBABILITY = 0, + NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1 +}; template std::shared_ptr> getNDT( @@ -134,6 +138,8 @@ class NDTScanMatcher : public rclcpp::Node initial_pose_with_covariance_pub_; rclcpp::Publisher::SharedPtr exe_time_pub_; rclcpp::Publisher::SharedPtr transform_probability_pub_; + rclcpp::Publisher::SharedPtr + nearest_voxel_transformation_likelihood_pub_; rclcpp::Publisher::SharedPtr iteration_num_pub_; rclcpp::Publisher::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_; diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index dca714376599a..ce0c0b804d1ca 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -102,7 +102,9 @@ NDTScanMatcher::NDTScanMatcher() base_frame_("base_link"), ndt_base_frame_("ndt_base_link"), map_frame_("map"), + converged_param_type_(ConvergedParamType::TRANSFORM_PROBABILITY), converged_param_transform_probability_(4.5), + converged_param_nearest_voxel_transformation_likelihood_(2.3), initial_estimate_particles_num_(100), initial_pose_timeout_sec_(1.0), initial_pose_distance_tolerance_m_(10.0), @@ -165,8 +167,23 @@ NDTScanMatcher::NDTScanMatcher() get_logger(), "trans_epsilon: %lf, step_size: %lf, resolution: %lf, max_iterations: %d", trans_epsilon, step_size, resolution, max_iterations); + int converged_param_type_tmp = this->declare_parameter("converged_param_type", 0); + converged_param_type_ = static_cast(converged_param_type_tmp); + if ( + ndt_implement_type_ != NDTImplementType::OMP && + converged_param_type_ == ConvergedParamType::NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) { + RCLCPP_ERROR( + get_logger(), + "ConvergedParamType::NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD is only available when " + "NDTImplementType::OMP is selected."); + return; + } + converged_param_transform_probability_ = this->declare_parameter( "converged_param_transform_probability", converged_param_transform_probability_); + converged_param_nearest_voxel_transformation_likelihood_ = this->declare_parameter( + "converged_param_nearest_voxel_transformation_likelihood", + converged_param_nearest_voxel_transformation_likelihood_); initial_estimate_particles_num_ = this->declare_parameter("initial_estimate_particles_num", initial_estimate_particles_num_); @@ -219,6 +236,9 @@ NDTScanMatcher::NDTScanMatcher() exe_time_pub_ = this->create_publisher("exe_time_ms", 10); transform_probability_pub_ = this->create_publisher("transform_probability", 10); + nearest_voxel_transformation_likelihood_pub_ = + this->create_publisher( + "nearest_voxel_transformation_likelihood", 10); iteration_num_pub_ = this->create_publisher("iteration_num", 10); initial_to_result_distance_pub_ = @@ -233,6 +253,7 @@ NDTScanMatcher::NDTScanMatcher() ndt_monte_carlo_initial_pose_marker_pub_ = this->create_publisher( "monte_carlo_initial_pose_marker", 10); + diagnostics_pub_ = this->create_publisher("/diagnostics", 10); @@ -503,6 +524,8 @@ void NDTScanMatcher::callbackSensorPoints( 1000.0; const float transform_probability = ndt_ptr_->getTransformationProbability(); + const float nearest_voxel_transformation_likelihood = + ndt_ptr_->getNearestVoxelTransformationLikelihood(); const int iteration_num = ndt_ptr_->getFinalNumIteration(); @@ -518,22 +541,54 @@ void NDTScanMatcher::callbackSensorPoints( These bugs are now resolved in original pcl implementation. https://github.com/PointCloudLibrary/pcl/blob/424c1c6a0ca97d94ca63e5daff4b183a4db8aae4/registration/include/pcl/registration/impl/ndt.hpp#L73-L180 *****************************************************************************/ + bool is_ok_iteration_num = iteration_num < ndt_ptr_->getMaximumIterations() + 2; + if (!is_ok_iteration_num) { + RCLCPP_WARN( + get_logger(), + "The number of iterations has reached its upper limit. The number of iterations: %d, Limit: " + "%d", + iteration_num, ndt_ptr_->getMaximumIterations() + 2); + } + bool is_local_optimal_solution_oscillation = false; - if (iteration_num >= ndt_ptr_->getMaximumIterations() + 2) { + if (!is_ok_iteration_num) { is_local_optimal_solution_oscillation = isLocalOptimalSolutionOscillation( result_pose_matrix_array, oscillation_threshold_, inversion_vector_threshold_); } - bool is_converged = true; + bool is_ok_converged_param = false; + if (converged_param_type_ == ConvergedParamType::TRANSFORM_PROBABILITY) { + is_ok_converged_param = transform_probability > converged_param_transform_probability_; + if (!is_ok_converged_param) { + RCLCPP_WARN( + get_logger(), "Transform Probability is below the threshold. Score: %lf, Threshold: %lf", + transform_probability, converged_param_transform_probability_); + } + } else if (converged_param_type_ == ConvergedParamType::NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) { + is_ok_converged_param = nearest_voxel_transformation_likelihood > + converged_param_nearest_voxel_transformation_likelihood_; + if (!is_ok_converged_param) { + RCLCPP_WARN( + get_logger(), + "Nearest Voxel Transform Probability is below the threshold. Score: %lf, Threshold: %lf", + nearest_voxel_transformation_likelihood, + converged_param_nearest_voxel_transformation_likelihood_); + } + } else { + is_ok_converged_param = false; + RCLCPP_ERROR_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1, "Unknown converged param type."); + } + + bool is_converged = false; static size_t skipping_publish_num = 0; - if ( - iteration_num >= ndt_ptr_->getMaximumIterations() + 2 || - transform_probability < converged_param_transform_probability_) { + if (is_ok_iteration_num && is_ok_converged_param) { + is_converged = true; + skipping_publish_num = 0; + } else { is_converged = false; ++skipping_publish_num; RCLCPP_WARN(get_logger(), "Not Converged"); - } else { - skipping_publish_num = 0; } // publish @@ -594,6 +649,8 @@ void NDTScanMatcher::callbackSensorPoints( exe_time_pub_->publish(makeFloat32Stamped(sensor_ros_time, exe_time)); transform_probability_pub_->publish(makeFloat32Stamped(sensor_ros_time, transform_probability)); + nearest_voxel_transformation_likelihood_pub_->publish( + makeFloat32Stamped(sensor_ros_time, nearest_voxel_transformation_likelihood)); iteration_num_pub_->publish(makeInt32Stamped(sensor_ros_time, iteration_num)); @@ -613,6 +670,8 @@ void NDTScanMatcher::callbackSensorPoints( makeFloat32Stamped(sensor_ros_time, initial_to_result_distance_new)); key_value_stdmap_["transform_probability"] = std::to_string(transform_probability); + key_value_stdmap_["nearest_voxel_transformation_likelihood"] = + std::to_string(nearest_voxel_transformation_likelihood); key_value_stdmap_["iteration_num"] = std::to_string(iteration_num); key_value_stdmap_["skipping_publish_num"] = std::to_string(skipping_publish_num); if (is_local_optimal_solution_oscillation) {