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 0dd3ae9015e7a..20548a1f21d38 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 @@ -66,15 +66,6 @@ enum class ConvergedParamType { NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1 }; -struct NdtResult -{ - geometry_msgs::msg::Pose pose; - float transform_probability; - float nearest_voxel_transformation_likelihood; - int iteration_num; - std::vector transformation_array; -}; - struct NDTParams { double trans_epsilon; @@ -111,7 +102,6 @@ class NDTScanMatcher : public rclcpp::Node void callback_regularization_pose( geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr); - NdtResult align(const geometry_msgs::msg::Pose & initial_pose_msg); geometry_msgs::msg::PoseWithCovarianceStamped align_using_monte_carlo( const std::shared_ptr & ndt_ptr, const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov); 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 4c4e1beeb772f..48e958abe6a9f 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -376,7 +376,7 @@ void NDTScanMatcher::callback_map_points( // create Thread // detach auto output_cloud = std::make_shared>(); - new_ndt_ptr->align(*output_cloud, Eigen::Matrix4f::Identity()); + new_ndt_ptr->align(*output_cloud); // swap ndt_ptr_mtx_.lock(); @@ -429,7 +429,11 @@ void NDTScanMatcher::callback_sensor_points( // perform ndt scan matching key_value_stdmap_["state"] = "Aligning"; - const NdtResult ndt_result = align(interpolator.get_current_pose().pose.pose); + const Eigen::Matrix4f initial_pose_matrix = + pose_to_matrix4f(interpolator.get_current_pose().pose.pose); + auto output_cloud = std::make_shared>(); + ndt_ptr_->align(*output_cloud, initial_pose_matrix); + const pclomp::NdtResult ndt_result = ndt_ptr_->getResult(); key_value_stdmap_["state"] = "Sleeping"; const auto exe_end_time = std::chrono::system_clock::now(); @@ -437,6 +441,13 @@ void NDTScanMatcher::callback_sensor_points( std::chrono::duration_cast(exe_end_time - exe_start_time).count() / 1000.0; + const geometry_msgs::msg::Pose result_pose_msg = matrix4f_to_pose(ndt_result.pose); + std::vector transformation_msg_array; + for (const auto & pose_matrix : ndt_result.transformation_array) { + geometry_msgs::msg::Pose pose_ros = matrix4f_to_pose(pose_matrix); + transformation_msg_array.push_back(pose_ros); + } + // perform several validations /***************************************************************************** The reason the add 2 to the ndt_ptr_->getMaximumIterations() is that there are bugs in @@ -455,7 +466,7 @@ void NDTScanMatcher::callback_sensor_points( bool is_local_optimal_solution_oscillation = false; if (!is_ok_iteration_num) { is_local_optimal_solution_oscillation = validate_local_optimal_solution_oscillation( - ndt_result.transformation_array, oscillation_threshold_, inversion_vector_threshold_); + transformation_msg_array, oscillation_threshold_, inversion_vector_threshold_); } bool is_ok_converged_param = validate_converged_param( ndt_result.transform_probability, ndt_result.nearest_voxel_transformation_likelihood); @@ -476,16 +487,16 @@ void NDTScanMatcher::callback_sensor_points( nearest_voxel_transformation_likelihood_pub_->publish( make_float32_stamped(sensor_ros_time, ndt_result.nearest_voxel_transformation_likelihood)); iteration_num_pub_->publish(make_int32_stamped(sensor_ros_time, ndt_result.iteration_num)); - publish_tf(sensor_ros_time, ndt_result.pose); - publish_pose(sensor_ros_time, ndt_result.pose, is_converged); - publish_marker(sensor_ros_time, ndt_result.transformation_array); + publish_tf(sensor_ros_time, result_pose_msg); + publish_pose(sensor_ros_time, result_pose_msg, is_converged); + publish_marker(sensor_ros_time, transformation_msg_array); publish_initial_to_result_distances( - sensor_ros_time, ndt_result.pose, interpolator.get_current_pose(), interpolator.get_old_pose(), + sensor_ros_time, result_pose_msg, interpolator.get_current_pose(), interpolator.get_old_pose(), interpolator.get_new_pose()); auto sensor_points_mapTF_ptr = std::make_shared>(); pcl::transformPointCloud( - *sensor_points_baselinkTF_ptr, *sensor_points_mapTF_ptr, pose_to_matrix4f(ndt_result.pose)); + *sensor_points_baselinkTF_ptr, *sensor_points_mapTF_ptr, ndt_result.pose); publish_point_cloud(sensor_ros_time, map_frame_, sensor_points_mapTF_ptr); key_value_stdmap_["transform_probability"] = std::to_string(ndt_result.transform_probability); @@ -516,31 +527,6 @@ void NDTScanMatcher::transform_sensor_measurement( *sensor_points_input_ptr, *sensor_points_output_ptr, base_to_sensor_matrix); } -NdtResult NDTScanMatcher::align(const geometry_msgs::msg::Pose & initial_pose_msg) -{ - const Eigen::Matrix4f initial_pose_matrix = pose_to_matrix4f(initial_pose_msg); - - auto output_cloud = std::make_shared>(); - ndt_ptr_->align(*output_cloud, initial_pose_matrix); - - const std::vector> - transformation_array_matrix = ndt_ptr_->getFinalTransformationArray(); - std::vector transformation_array_msg; - for (auto pose_matrix : transformation_array_matrix) { - geometry_msgs::msg::Pose pose_ros = matrix4f_to_pose(pose_matrix); - transformation_array_msg.push_back(pose_ros); - } - - NdtResult ndt_result; - ndt_result.pose = matrix4f_to_pose(ndt_ptr_->getFinalTransformation()); - ndt_result.transformation_array = transformation_array_msg; - ndt_result.transform_probability = ndt_ptr_->getTransformationProbability(); - ndt_result.nearest_voxel_transformation_likelihood = - ndt_ptr_->getNearestVoxelTransformationLikelihood(); - ndt_result.iteration_num = ndt_ptr_->getFinalNumIteration(); - return ndt_result; -} - geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_using_monte_carlo( const std::shared_ptr & ndt_ptr, const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov) @@ -559,18 +545,20 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_using_monte_ for (unsigned int i = 0; i < initial_poses.size(); i++) { const auto & initial_pose = initial_poses[i]; - const NdtResult ndt_result = align(initial_pose); + const Eigen::Matrix4f initial_pose_matrix = pose_to_matrix4f(initial_pose); + ndt_ptr_->align(*output_cloud, initial_pose_matrix); + const pclomp::NdtResult ndt_result = ndt_ptr_->getResult(); Particle particle( - initial_pose, ndt_result.pose, ndt_result.transform_probability, ndt_result.iteration_num); + initial_pose, matrix4f_to_pose(ndt_result.pose), ndt_result.transform_probability, + ndt_result.iteration_num); particle_array.push_back(particle); const auto marker_array = make_debug_markers( this->now(), map_frame_, tier4_autoware_utils::createMarkerScale(0.3, 0.1, 0.1), particle, i); ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array); auto sensor_points_mapTF_ptr = std::make_shared>(); - pcl::transformPointCloud( - *ndt_ptr->getInputSource(), *sensor_points_mapTF_ptr, pose_to_matrix4f(ndt_result.pose)); + pcl::transformPointCloud(*ndt_ptr->getInputSource(), *sensor_points_mapTF_ptr, ndt_result.pose); publish_point_cloud(initial_pose_with_cov.header.stamp, map_frame_, sensor_points_mapTF_ptr); }