From 3252160b0aaa546dfb6b65fd159cebad4035f9ff Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 12 Feb 2026 18:32:39 +0900 Subject: [PATCH 1/3] Removed `setInputSource` Signed-off-by: Shintaro Sakoda --- .../ndt_omp/estimate_covariance.hpp | 6 +- .../ndt_omp/multigrid_ndt_omp.h | 115 ++++++++---------- .../ndt_scan_matcher_core.hpp | 2 + .../src/map_update_module.cpp | 8 -- .../src/ndt_omp/estimate_covariance.cpp | 11 +- .../src/ndt_omp/multigrid_ndt_omp_impl.hpp | 99 +++++++++------ .../src/ndt_scan_matcher_core.cpp | 22 ++-- 7 files changed, 139 insertions(+), 124 deletions(-) diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/estimate_covariance.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/estimate_covariance.hpp index cd3f7bf6a2..43980026b9 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/estimate_covariance.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/estimate_covariance.hpp @@ -41,12 +41,14 @@ ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt( const NdtResult & ndt_result, const std::shared_ptr< pclomp::MultiGridNormalDistributionsTransform> & ndt_ptr, - const std::vector & poses_to_search); + const std::vector & poses_to_search, + const pcl::shared_ptr> & source); ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt_score( const NdtResult & ndt_result, const std::shared_ptr< pclomp::MultiGridNormalDistributionsTransform> & ndt_ptr, - const std::vector & poses_to_search, const double temperature); + const std::vector & poses_to_search, + const pcl::shared_ptr> & source, const double temperature); /** \brief Propose poses to search. * (1) Compute covariance by Laplace approximation diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multigrid_ndt_omp.h b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multigrid_ndt_omp.h index b7c9877739..ce412ac748 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multigrid_ndt_omp.h +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multigrid_ndt_omp.h @@ -60,13 +60,12 @@ #include "multi_voxel_grid_covariance_omp.h" #include "ndt_struct.hpp" -#include +#include +#include #include #include "boost/optional.hpp" -#include - #include #include #include @@ -86,15 +85,14 @@ namespace pclomp * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific) */ template -class MultiGridNormalDistributionsTransform : public pcl::Registration +class MultiGridNormalDistributionsTransform { protected: - typedef pcl::Registration BaseRegType; - typedef typename BaseRegType::PointCloudSource PointCloudSource; + typedef pcl::PointCloud PointCloudSource; typedef typename PointCloudSource::Ptr PointCloudSourcePtr; typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr; - typedef typename BaseRegType::PointCloudTarget PointCloudTarget; + typedef pcl::PointCloud PointCloudTarget; typedef typename PointCloudTarget::Ptr PointCloudTargetPtr; typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr; @@ -131,19 +129,7 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration getHessian() const { return hessian_; } @@ -219,6 +209,15 @@ class MultiGridNormalDistributionsTransform : public pcl::RegistrationgetFinalTransformation(); + ndt_result.pose = getFinalTransformation(); ndt_result.transformation_array = getFinalTransformationArray(); ndt_result.transform_probability = getTransformationProbability(); ndt_result.transform_probability_array = transform_probability_array_; @@ -263,48 +262,27 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration getCurrentMapIDs() const { return target_cells_.getCurrentMapIDs(); } protected: - using BaseRegType::converged_; - using BaseRegType::final_transformation_; - using BaseRegType::input_; - using BaseRegType::max_iterations_; - using BaseRegType::nr_iterations_; - using BaseRegType::previous_transformation_; - using BaseRegType::reg_name_; - using BaseRegType::target_; - using BaseRegType::target_cloud_updated_; - using BaseRegType::transformation_; - using BaseRegType::transformation_epsilon_; - - using BaseRegType::update_visualizer_; - - /** \brief Estimate the transformation and returns the transformed source (input) as output. - * \param[out] output the resultant input transformed point cloud dataset - */ - virtual void computeTransformation(PointCloudSource & output) - { - computeTransformation(output, Eigen::Matrix4f::Identity()); - } - /** \brief Estimate the transformation and returns the transformed source (input) as output. * \param[out] output the resultant input transformed point cloud dataset * \param[in] guess the initial gross estimation of the transformation + * \param[in] source the original input source point cloud */ - virtual void computeTransformation(PointCloudSource & output, const Eigen::Matrix4f & guess); - - /** \brief Initiate covariance voxel structure. */ - void inline init() {} + void computeTransformation( + PointCloudSource & output, const Eigen::Matrix4f & guess, + const PointCloudSourceConstPtr & source); /** \brief Compute derivatives of probability function w.r.t. the transformation vector. * \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009]. * \param[out] score_gradient the gradient vector of the probability function w.r.t. the * transformation vector \param[out] hessian the hessian matrix of the probability function w.r.t. * the transformation vector \param[in] trans_cloud transformed point cloud \param[in] p the - * current transform vector \param[in] compute_hessian flag to calculate hessian, unnecessary for - * step calculation. + * current transform vector \param[in] source the original input source point cloud + * \param[in] compute_hessian flag to calculate hessian, unnecessary for step calculation. */ double computeDerivatives( Eigen::Matrix & score_gradient, Eigen::Matrix & hessian, - PointCloudSource & trans_cloud, Eigen::Matrix & p, bool compute_hessian = true); + PointCloudSource & trans_cloud, Eigen::Matrix & p, + const PointCloudSourceConstPtr & source, bool compute_hessian = true); /** \brief Compute individual point contributions to derivatives of probability function w.r.t. * the transformation vector. \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009]. \param[in,out] @@ -340,10 +318,11 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration & hessian, PointCloudSource & trans_cloud, - Eigen::Matrix & p); + Eigen::Matrix & p, const PointCloudSourceConstPtr & source); /** \brief Compute individual point contributions to hessian of probability function w.r.t. the * transformation vector. \note Equation 6.13 [Magnusson 2009]. \param[in,out] hessian the hessian @@ -371,13 +350,15 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration & x, Eigen::Matrix & step_dir, double step_init, - double step_max, double step_min, double & score, Eigen::Matrix & score_gradient, - Eigen::Matrix & hessian, PointCloudSource & trans_cloud); + const Eigen::Matrix & x, Eigen::Matrix & step_dir, + double step_init, double step_max, double step_min, double & score, + Eigen::Matrix & score_gradient, Eigen::Matrix & hessian, + PointCloudSource & trans_cloud, const PointCloudSourceConstPtr & source); /** \brief Update interval of possible step lengths for More-Thuente method, \f$ I \f$ in * More-Thuente (1994) \note Updating Algorithm until some value satisfies \f$ \psi(\alpha_k) \leq @@ -452,8 +433,6 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration ndt_ptr_; + pcl::shared_ptr> sensor_points_in_baselink_frame_; + Eigen::Matrix4f base_to_sensor_matrix_; std::mutex ndt_ptr_mtx_; diff --git a/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp b/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp index 084e7a8d87..b4bd6d2464 100644 --- a/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp @@ -153,14 +153,10 @@ void MapUpdateModule::update_map( ndt_ptr_mutex_->lock(); auto param = ndt_ptr_->getParams(); - auto input_source = ndt_ptr_->getInputSource(); ndt_ptr_.reset(new NdtType); ndt_ptr_->setParams(param); - if (input_source != nullptr) { - ndt_ptr_->setInputSource(input_source); - } const bool updated = update_ndt(position, *ndt_ptr_, diagnostics_ptr); @@ -207,11 +203,7 @@ void MapUpdateModule::update_map( ndt_ptr_mutex_->lock(); auto dummy_ptr = ndt_ptr_; - auto input_source = ndt_ptr_->getInputSource(); ndt_ptr_ = secondary_ndt_ptr_; - if (input_source != nullptr) { - ndt_ptr_->setInputSource(input_source); - } ndt_ptr_mutex_->unlock(); dummy_ptr.reset(); diff --git a/localization/autoware_ndt_scan_matcher/src/ndt_omp/estimate_covariance.cpp b/localization/autoware_ndt_scan_matcher/src/ndt_omp/estimate_covariance.cpp index a1ac175709..fc2acbb063 100644 --- a/localization/autoware_ndt_scan_matcher/src/ndt_omp/estimate_covariance.cpp +++ b/localization/autoware_ndt_scan_matcher/src/ndt_omp/estimate_covariance.cpp @@ -36,7 +36,8 @@ ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt( const NdtResult & ndt_result, const std::shared_ptr< pclomp::MultiGridNormalDistributionsTransform> & ndt_ptr, - const std::vector & poses_to_search) + const std::vector & poses_to_search, + const pcl::shared_ptr> & source) { // initialize by the main result const Eigen::Vector2d ndt_pose_2d(ndt_result.pose(0, 3), ndt_result.pose(1, 3)); @@ -46,7 +47,7 @@ ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt( std::vector ndt_results; for (const Eigen::Matrix4f & curr_pose : poses_to_search) { auto sub_output_cloud = std::make_shared>(); - ndt_ptr->align(*sub_output_cloud, curr_pose); + ndt_ptr->align(*sub_output_cloud, curr_pose, source); const NdtResult sub_ndt_result = ndt_ptr->getResult(); ndt_results.push_back(sub_ndt_result); @@ -72,14 +73,14 @@ ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt_score( const NdtResult & ndt_result, const std::shared_ptr< pclomp::MultiGridNormalDistributionsTransform> & ndt_ptr, - const std::vector & poses_to_search, const double temperature) + const std::vector & poses_to_search, + const pcl::shared_ptr> & source, const double temperature) { // initialize by the main result const Eigen::Vector2d ndt_pose_2d(ndt_result.pose(0, 3), ndt_result.pose(1, 3)); std::vector ndt_pose_2d_vec{ndt_pose_2d}; std::vector score_vec{ndt_result.nearest_voxel_transformation_likelihood}; - const pcl::PointCloud::ConstPtr input_cloud = ndt_ptr->getInputCloud(); pcl::PointCloud trans_cloud; // multiple searches @@ -88,7 +89,7 @@ ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt_score( const Eigen::Vector2d sub_ndt_pose_2d = curr_pose.topRightCorner<2, 1>().cast(); ndt_pose_2d_vec.emplace_back(sub_ndt_pose_2d); - transformPointCloud(*input_cloud, trans_cloud, curr_pose); + pcl::transformPointCloud(*source, trans_cloud, curr_pose); const double nvtl = ndt_ptr->calculateNearestVoxelTransformationLikelihood(trans_cloud); score_vec.emplace_back(nvtl); diff --git a/localization/autoware_ndt_scan_matcher/src/ndt_omp/multigrid_ndt_omp_impl.hpp b/localization/autoware_ndt_scan_matcher/src/ndt_omp/multigrid_ndt_omp_impl.hpp index 08792af4bd..1a5f833772 100644 --- a/localization/autoware_ndt_scan_matcher/src/ndt_omp/multigrid_ndt_omp_impl.hpp +++ b/localization/autoware_ndt_scan_matcher/src/ndt_omp/multigrid_ndt_omp_impl.hpp @@ -69,7 +69,7 @@ namespace pclomp template MultiGridNormalDistributionsTransform:: MultiGridNormalDistributionsTransform(const MultiGridNormalDistributionsTransform & other) -: BaseRegType(other), target_cells_(other.target_cells_) +: target_cells_(other.target_cells_) { params_ = other.params_; outlier_ratio_ = other.outlier_ratio_; @@ -77,8 +77,6 @@ MultiGridNormalDistributionsTransform:: gauss_d2_ = other.gauss_d2_; gauss_d3_ = other.gauss_d3_; trans_probability_ = other.trans_probability_; - // No need to copy j_ang_ and h_ang_, as those matrices are re-computed on every - // computeDerivatives() call hessian_ = other.hessian_; transformation_array_ = other.transformation_array_; @@ -89,14 +87,19 @@ MultiGridNormalDistributionsTransform:: regularization_pose_ = other.regularization_pose_; regularization_pose_translation_ = other.regularization_pose_translation_; + + final_transformation_ = other.final_transformation_; + transformation_ = other.transformation_; + previous_transformation_ = other.previous_transformation_; + nr_iterations_ = other.nr_iterations_; + max_iterations_ = other.max_iterations_; + converged_ = other.converged_; } template MultiGridNormalDistributionsTransform:: MultiGridNormalDistributionsTransform(MultiGridNormalDistributionsTransform && other) noexcept -: BaseRegType(std::move(other)), - target_cells_(std::move(other.target_cells_)), - params_(std::move(other.params_)) +: target_cells_(std::move(other.target_cells_)), params_(std::move(other.params_)) { outlier_ratio_ = other.outlier_ratio_; gauss_d1_ = other.gauss_d1_; @@ -113,6 +116,13 @@ MultiGridNormalDistributionsTransform:: regularization_pose_ = other.regularization_pose_; regularization_pose_translation_ = other.regularization_pose_translation_; + + final_transformation_ = other.final_transformation_; + transformation_ = other.transformation_; + previous_transformation_ = other.previous_transformation_; + nr_iterations_ = other.nr_iterations_; + max_iterations_ = other.max_iterations_; + converged_ = other.converged_; } template @@ -139,7 +149,12 @@ MultiGridNormalDistributionsTransform::operator=( regularization_pose_ = other.regularization_pose_; regularization_pose_translation_ = other.regularization_pose_translation_; - BaseRegType::operator=(other); + final_transformation_ = other.final_transformation_; + transformation_ = other.transformation_; + previous_transformation_ = other.previous_transformation_; + nr_iterations_ = other.nr_iterations_; + max_iterations_ = other.max_iterations_; + converged_ = other.converged_; return *this; } @@ -168,7 +183,12 @@ MultiGridNormalDistributionsTransform::operator=( regularization_pose_ = other.regularization_pose_; regularization_pose_translation_ = other.regularization_pose_translation_; - BaseRegType::operator=(std::move(other)); + final_transformation_ = other.final_transformation_; + transformation_ = other.transformation_; + previous_transformation_ = other.previous_transformation_; + nr_iterations_ = other.nr_iterations_; + max_iterations_ = other.max_iterations_; + converged_ = other.converged_; return *this; } @@ -185,8 +205,6 @@ MultiGridNormalDistributionsTransform< trans_probability_(), regularization_pose_(boost::none) { - reg_name_ = "MultiGridNormalDistributionsTransform"; - params_.trans_epsilon = 0.1; params_.step_size = 0.1; params_.resolution = 1.0f; @@ -204,10 +222,23 @@ MultiGridNormalDistributionsTransform< gauss_d2_ = -2 * log((-log(gauss_c1 * exp(-0.5) + gauss_c2) - gauss_d3_) / gauss_d1_); } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +void MultiGridNormalDistributionsTransform::align( + PointCloudSource & output, const Eigen::Matrix4f & guess, + const PointCloudSourceConstPtr & source) +{ + output = *source; + final_transformation_ = transformation_ = previous_transformation_ = Eigen::Matrix4f::Identity(); + converged_ = false; + computeTransformation(output, guess, source); +} + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void MultiGridNormalDistributionsTransform::computeTransformation( - PointCloudSource & output, const Eigen::Matrix4f & guess) + PointCloudSource & output, const Eigen::Matrix4f & guess, + const PointCloudSourceConstPtr & source) { nr_iterations_ = 0; converged_ = false; @@ -223,7 +254,7 @@ void MultiGridNormalDistributionsTransform::computeTra // Initialise final transformation to the guessed one final_transformation_ = guess; // Apply guessed transformation prior to search for neighbours - transformPointCloud(output, output, guess); + pcl::transformPointCloud(output, output, guess); } Eigen::Transform eig_transformation; @@ -257,7 +288,7 @@ void MultiGridNormalDistributionsTransform::computeTra // Calculate derivatives of initial transform vector, subsequent derivative calculations are done // in the step length determination. - score = computeDerivatives(score_gradient, hessian, output, p); + score = computeDerivatives(score_gradient, hessian, output, p, source); while (!converged_) { // Store previous transformation @@ -273,10 +304,10 @@ void MultiGridNormalDistributionsTransform::computeTra double delta_p_norm = delta_p.norm(); if (delta_p_norm == 0 || delta_p_norm != delta_p_norm) { - if (input_->empty()) { + if (source->empty()) { trans_probability_ = 0.0f; } else { - trans_probability_ = score / static_cast(input_->size()); + trans_probability_ = score / static_cast(source->size()); } converged_ = delta_p_norm == delta_p_norm; @@ -286,7 +317,7 @@ void MultiGridNormalDistributionsTransform::computeTra delta_p.normalize(); delta_p_norm = computeStepLengthMT( p, delta_p, delta_p_norm, params_.step_size, params_.trans_epsilon / 2.0, score, - score_gradient, hessian, output); + score_gradient, hessian, output, source); delta_p *= delta_p_norm; transformation_ = @@ -302,10 +333,6 @@ void MultiGridNormalDistributionsTransform::computeTra p = p + delta_p; - // Update Visualizer (untested) - if (update_visualizer_ != 0) - update_visualizer_(output, std::vector(), *target_, std::vector()); - nr_iterations_++; if ( @@ -317,10 +344,10 @@ void MultiGridNormalDistributionsTransform::computeTra // Store transformation probability. The relative differences within each scan registration are // accurate but the normalization constants need to be modified for it to be globally accurate - if (input_->empty()) { + if (source->empty()) { trans_probability_ = 0.0f; } else { - trans_probability_ = score / static_cast(input_->size()); + trans_probability_ = score / static_cast(source->size()); } hessian_ = hessian; @@ -341,7 +368,8 @@ int omp_get_thread_num() template double MultiGridNormalDistributionsTransform::computeDerivatives( Eigen::Matrix & score_gradient, Eigen::Matrix & hessian, - PointCloudSource & trans_cloud, Eigen::Matrix & p, bool compute_hessian) + PointCloudSource & trans_cloud, Eigen::Matrix & p, + const PointCloudSourceConstPtr & source, bool compute_hessian) { score_gradient.setZero(); hessian.setZero(); @@ -383,7 +411,7 @@ double MultiGridNormalDistributionsTransform::computeD // Update gradient and hessian for each point, line 17 in Algorithm 2 [Magnusson 2009] #pragma omp parallel for num_threads(params_.num_threads) schedule(guided, 8) - for (size_t idx = 0; idx < input_->size(); ++idx) { + for (size_t idx = 0; idx < source->size(); ++idx) { int tid = omp_get_thread_num(); // Searching for neighbors of the current transformed point auto & x_trans_pt = trans_cloud[idx]; @@ -397,7 +425,7 @@ double MultiGridNormalDistributionsTransform::computeD } // Original Point - auto & x_pt = (*input_)[idx]; + auto & x_pt = (*source)[idx]; // Original Point and Transformed Point (for math) Eigen::Vector3d x(x_pt.x, x_pt.y, x_pt.z); // Current Point Gradient and Hessian @@ -490,7 +518,7 @@ double MultiGridNormalDistributionsTransform::computeD nearest_voxel_transformation_likelihood_array_.push_back( nearest_voxel_transformation_likelihood_); const float transform_probability = - (input_->points.empty() ? 0.0f : score / static_cast(input_->points.size())); + (source->points.empty() ? 0.0f : score / static_cast(source->points.size())); transform_probability_array_.push_back(transform_probability); return (score); } @@ -683,7 +711,7 @@ double MultiGridNormalDistributionsTransform::updateDe template void MultiGridNormalDistributionsTransform::computeHessian( Eigen::Matrix & hessian, PointCloudSource & trans_cloud, - Eigen::Matrix &) + Eigen::Matrix &, const PointCloudSourceConstPtr & source) { // Initialize Point Gradient and Hessian // Pre-allocate thread-wise point gradients and point hessians @@ -705,7 +733,7 @@ void MultiGridNormalDistributionsTransform::computeHes // Update hessian for each point, line 17 in Algorithm 2 [Magnusson 2009] #pragma omp parallel for num_threads(params_.num_threads) schedule(guided, 8) - for (size_t idx = 0; idx < input_->size(); ++idx) { + for (size_t idx = 0; idx < source->size(); ++idx) { int tid = omp_get_thread_num(); auto & x_trans_pt = trans_cloud[idx]; @@ -719,7 +747,7 @@ void MultiGridNormalDistributionsTransform::computeHes continue; } - auto & x_pt = (*input_)[idx]; + auto & x_pt = (*source)[idx]; // For math Eigen::Vector3d x(x_pt.x, x_pt.y, x_pt.z); const Eigen::Vector3d x_trans(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z); @@ -895,7 +923,8 @@ template double MultiGridNormalDistributionsTransform::computeStepLengthMT( const Eigen::Matrix & x, Eigen::Matrix & step_dir, double step_init, double step_max, double step_min, double & score, Eigen::Matrix & score_gradient, - Eigen::Matrix & hessian, PointCloudSource & trans_cloud) + Eigen::Matrix & hessian, PointCloudSource & trans_cloud, + const PointCloudSourceConstPtr & source) { // Set the value of phi'(0), Equation 1.3 [More, Thuente 1994] double d_phi_0 = -(score_gradient.dot(step_dir)); @@ -926,12 +955,12 @@ double MultiGridNormalDistributionsTransform::computeS .matrix(); // New transformed point cloud - transformPointCloud(*input_, trans_cloud, final_transformation_); + pcl::transformPointCloud(*source, trans_cloud, final_transformation_); // Updates score, gradient and hessian. Hessian calculation is unnecessary but testing showed // that most step calculations use the initial step suggestion and recalculation the reusable // portions of the hessian would entail more computation time. - score = computeDerivatives(score_gradient, hessian, trans_cloud, x_t, true); + score = computeDerivatives(score_gradient, hessian, trans_cloud, x_t, source, true); // -------------------------------------------------------------------------------------------------------------------------------- // FIXME(YamatoAndo): @@ -1011,10 +1040,10 @@ double MultiGridNormalDistributionsTransform::computeS // New transformed point cloud // Done on final cloud to prevent wasted computation - transformPointCloud(*input_, trans_cloud, final_transformation_); + pcl::transformPointCloud(*source, trans_cloud, final_transformation_); // Updates score, gradient. Values stored to prevent wasted computation. - score = computeDerivatives(score_gradient, hessian, trans_cloud, x_t, false); + score = computeDerivatives(score_gradient, hessian, trans_cloud, x_t, source, false); // Calculate phi(alpha_t+) phi_t = -score; @@ -1054,7 +1083,7 @@ double MultiGridNormalDistributionsTransform::computeS // If inner loop was run then hessian needs to be calculated. // Hessian is unnecessary for step length determination but gradients are required // so derivative and transform data is stored for the next iteration. - if (step_iterations) computeHessian(hessian, trans_cloud, x_t); + if (step_iterations) computeHessian(hessian, trans_cloud, x_t, source); return (a_t); } diff --git a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index c0f7673976..de241ab56b 100644 --- a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -411,8 +411,8 @@ bool NDTScanMatcher::callback_sensor_points_main( // lock mutex std::lock_guard lock(ndt_ptr_mtx_); - // set sensor points to ndt class - ndt_ptr_->setInputSource(sensor_points_in_baselink_frame); + // store sensor points for ndt alignment + sensor_points_in_baselink_frame_ = sensor_points_in_baselink_frame; // check is_activated diagnostics_scan_points_->add_key_value("is_activated", static_cast(is_activated_)); @@ -465,7 +465,7 @@ bool NDTScanMatcher::callback_sensor_points_main( } // check is_set_map_points - const bool is_set_map_points = (ndt_ptr_->getInputTarget() != nullptr); + const bool is_set_map_points = ndt_ptr_->hasTarget(); diagnostics_scan_points_->add_key_value("is_set_map_points", is_set_map_points); if (!is_set_map_points) { std::stringstream message; @@ -479,7 +479,7 @@ bool NDTScanMatcher::callback_sensor_points_main( const Eigen::Matrix4f initial_pose_matrix = pose_to_matrix4f(interpolation_result.interpolated_pose.pose.pose); auto output_cloud = std::make_shared>(); - ndt_ptr_->align(*output_cloud, initial_pose_matrix); + ndt_ptr_->align(*output_cloud, initial_pose_matrix, sensor_points_in_baselink_frame_); const pclomp::NdtResult ndt_result = ndt_ptr_->getResult(); const geometry_msgs::msg::Pose result_pose_msg = matrix4f_to_pose(ndt_result.pose); @@ -881,7 +881,8 @@ Eigen::Matrix2d NDTScanMatcher::estimate_covariance( ndt_result, param_.covariance.covariance_estimation.initial_pose_offset_model_x, param_.covariance.covariance_estimation.initial_pose_offset_model_y); const pclomp::ResultOfMultiNdtCovarianceEstimation result_of_multi_ndt_covariance_estimation = - estimate_xy_covariance_by_multi_ndt(ndt_result, ndt_ptr_, poses_to_search); + estimate_xy_covariance_by_multi_ndt( + ndt_result, ndt_ptr_, poses_to_search, sensor_points_in_baselink_frame_); for (size_t i = 0; i < result_of_multi_ndt_covariance_estimation.ndt_initial_poses.size(); i++) { multi_ndt_result_msg.poses.push_back( @@ -900,7 +901,8 @@ Eigen::Matrix2d NDTScanMatcher::estimate_covariance( param_.covariance.covariance_estimation.initial_pose_offset_model_y); const pclomp::ResultOfMultiNdtCovarianceEstimation result_of_multi_ndt_score_covariance_estimation = estimate_xy_covariance_by_multi_ndt_score( - ndt_result, ndt_ptr_, poses_to_search, param_.covariance.covariance_estimation.temperature); + ndt_result, ndt_ptr_, poses_to_search, sensor_points_in_baselink_frame_, + param_.covariance.covariance_estimation.temperature); for (const auto & sub_initial_pose_matrix : poses_to_search) { multi_initial_pose_msg.poses.push_back(matrix4f_to_pose(sub_initial_pose_matrix)); } @@ -1036,7 +1038,7 @@ void NDTScanMatcher::service_ndt_align_main( std::lock_guard lock(ndt_ptr_mtx_); // check is_set_map_points - bool is_set_map_points = (ndt_ptr_->getInputTarget() != nullptr); + bool is_set_map_points = ndt_ptr_->hasTarget(); diagnostics_ndt_align_->add_key_value("is_set_map_points", is_set_map_points); if (!is_set_map_points) { std::stringstream message; @@ -1049,7 +1051,7 @@ void NDTScanMatcher::service_ndt_align_main( } // check is_set_sensor_points - bool is_set_sensor_points = (ndt_ptr_->getInputSource() != nullptr); + bool is_set_sensor_points = (sensor_points_in_baselink_frame_ != nullptr); diagnostics_ndt_align_->add_key_value("is_set_sensor_points", is_set_sensor_points); if (!is_set_sensor_points) { std::stringstream message; @@ -1130,7 +1132,7 @@ std::tuple NDTScanMatcher initial_pose.orientation = tf2::toMsg(tf_quaternion); const Eigen::Matrix4f initial_pose_matrix = pose_to_matrix4f(initial_pose); - ndt_ptr_->align(*output_cloud, initial_pose_matrix); + ndt_ptr_->align(*output_cloud, initial_pose_matrix, sensor_points_in_baselink_frame_); const pclomp::NdtResult ndt_result = ndt_ptr_->getResult(); Particle particle( @@ -1158,7 +1160,7 @@ std::tuple NDTScanMatcher auto sensor_points_in_map_ptr = std::make_shared>(); autoware_utils_pcl::transform_pointcloud( - *ndt_ptr_->getInputSource(), *sensor_points_in_map_ptr, ndt_result.pose); + *sensor_points_in_baselink_frame_, *sensor_points_in_map_ptr, ndt_result.pose); publish_point_cloud( initial_pose_with_cov.header.stamp, param_.frame.map_frame, sensor_points_in_map_ptr); } From 2ece640a713f90739212c6b7bfdfbc2accde43d4 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 12 Feb 2026 19:06:06 +0900 Subject: [PATCH 2/3] Applied `pre-commit run -a` Signed-off-by: Shintaro Sakoda --- .../ndt_scan_matcher/ndt_omp/multigrid_ndt_omp.h | 13 +++++++------ .../src/ndt_omp/multigrid_ndt_omp_impl.hpp | 6 ++---- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multigrid_ndt_omp.h b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multigrid_ndt_omp.h index ce412ac748..55d00ddf9d 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multigrid_ndt_omp.h +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multigrid_ndt_omp.h @@ -60,12 +60,13 @@ #include "multi_voxel_grid_covariance_omp.h" #include "ndt_struct.hpp" -#include -#include #include #include "boost/optional.hpp" +#include +#include + #include #include #include @@ -355,10 +356,10 @@ class MultiGridNormalDistributionsTransform * \return final step length */ double computeStepLengthMT( - const Eigen::Matrix & x, Eigen::Matrix & step_dir, - double step_init, double step_max, double step_min, double & score, - Eigen::Matrix & score_gradient, Eigen::Matrix & hessian, - PointCloudSource & trans_cloud, const PointCloudSourceConstPtr & source); + const Eigen::Matrix & x, Eigen::Matrix & step_dir, double step_init, + double step_max, double step_min, double & score, Eigen::Matrix & score_gradient, + Eigen::Matrix & hessian, PointCloudSource & trans_cloud, + const PointCloudSourceConstPtr & source); /** \brief Update interval of possible step lengths for More-Thuente method, \f$ I \f$ in * More-Thuente (1994) \note Updating Algorithm until some value satisfies \f$ \psi(\alpha_k) \leq diff --git a/localization/autoware_ndt_scan_matcher/src/ndt_omp/multigrid_ndt_omp_impl.hpp b/localization/autoware_ndt_scan_matcher/src/ndt_omp/multigrid_ndt_omp_impl.hpp index 1a5f833772..f5a9900f89 100644 --- a/localization/autoware_ndt_scan_matcher/src/ndt_omp/multigrid_ndt_omp_impl.hpp +++ b/localization/autoware_ndt_scan_matcher/src/ndt_omp/multigrid_ndt_omp_impl.hpp @@ -225,8 +225,7 @@ MultiGridNormalDistributionsTransform< ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void MultiGridNormalDistributionsTransform::align( - PointCloudSource & output, const Eigen::Matrix4f & guess, - const PointCloudSourceConstPtr & source) + PointCloudSource & output, const Eigen::Matrix4f & guess, const PointCloudSourceConstPtr & source) { output = *source; final_transformation_ = transformation_ = previous_transformation_ = Eigen::Matrix4f::Identity(); @@ -237,8 +236,7 @@ void MultiGridNormalDistributionsTransform::align( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void MultiGridNormalDistributionsTransform::computeTransformation( - PointCloudSource & output, const Eigen::Matrix4f & guess, - const PointCloudSourceConstPtr & source) + PointCloudSource & output, const Eigen::Matrix4f & guess, const PointCloudSourceConstPtr & source) { nr_iterations_ = 0; converged_ = false; From 353791535e50c09b2f02ea59857809c2b3a7cf33 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 12 Feb 2026 19:08:54 +0900 Subject: [PATCH 3/3] Corrected according to cppcheck's indications Signed-off-by: Shintaro Sakoda --- .../autoware/ndt_scan_matcher/ndt_omp/multigrid_ndt_omp.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multigrid_ndt_omp.h b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multigrid_ndt_omp.h index 55d00ddf9d..5466f15ce3 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multigrid_ndt_omp.h +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multigrid_ndt_omp.h @@ -233,7 +233,7 @@ class MultiGridNormalDistributionsTransform inline void unsetRegularizationPose() { regularization_pose_ = boost::none; } - NdtResult getResult() + NdtResult getResult() const { NdtResult ndt_result; ndt_result.pose = getFinalTransformation(); @@ -413,7 +413,7 @@ class MultiGridNormalDistributionsTransform * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More, Thuente 1994] * \return sufficient decrease value */ - inline double auxiliaryFunction_PsiMT( + static inline double auxiliaryFunction_PsiMT( double a, double f_a, double f_0, double g_0, double mu = 1.e-4) { return (f_a - f_0 - mu * g_0 * a); @@ -426,7 +426,7 @@ class MultiGridNormalDistributionsTransform * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More, Thuente 1994] * \return sufficient decrease derivative */ - inline double auxiliaryFunction_dPsiMT(double g_a, double g_0, double mu = 1.e-4) + static inline double auxiliaryFunction_dPsiMT(double g_a, double g_0, double mu = 1.e-4) { return (g_a - mu * g_0); }