Skip to content

Commit

Permalink
refactor(ndt_scan_matcher): isolate align function into ndt_omp (auto…
Browse files Browse the repository at this point in the history 
…warefoundation#2199)

* first commit

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* remove align and NdtResults from ndt_scan_matcher

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* rename variable

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* ci(pre-commit): autofix

* use getResult() function instead of executeScanMatching()

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* for (auto...) -> for (const auto &...)

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* removed unnecessary output_cloud declaration

Signed-off-by: kminoda <koji.minoda@tier4.jp>

Signed-off-by: kminoda <koji.minoda@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
  • Loading branch information
@kminoda @pre-commit-ci @YoshiRi
2 people authored and YoshiRi committed Jan 11, 2023
1 parent 89a9872 commit 108d5d8
Show file tree
Hide file tree
Showing 2 changed files with 25 additions and 47 deletions.
10 changes: 0 additions & 10 deletions localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<geometry_msgs::msg::Pose> transformation_array;
};

struct NDTParams
{
double trans_epsilon;
Expand Down Expand Up @@ -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<NormalDistributionsTransform> & ndt_ptr,
const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov);
Expand Down
62 changes: 25 additions & 37 deletions localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -376,7 +376,7 @@ void NDTScanMatcher::callback_map_points(
// create Thread
// detach
auto output_cloud = std::make_shared<pcl::PointCloud<PointSource>>();
new_ndt_ptr->align(*output_cloud, Eigen::Matrix4f::Identity());
new_ndt_ptr->align(*output_cloud);

// swap
ndt_ptr_mtx_.lock();
Expand Down Expand Up @@ -429,14 +429,25 @@ 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<pcl::PointCloud<PointSource>>();
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();
const double exe_time =
std::chrono::duration_cast<std::chrono::microseconds>(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<geometry_msgs::msg::Pose> 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
Expand All @@ -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);
Expand All @@ -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::PointCloud<PointSource>>();
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);
Expand Down Expand Up @@ -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<pcl::PointCloud<PointSource>>();
ndt_ptr_->align(*output_cloud, initial_pose_matrix);

const std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
transformation_array_matrix = ndt_ptr_->getFinalTransformationArray();
std::vector<geometry_msgs::msg::Pose> 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<NormalDistributionsTransform> & ndt_ptr,
const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov)
Expand All @@ -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::PointCloud<PointSource>>();
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);
}

Expand Down

0 comments on commit 108d5d8

Please sign in to comment.