Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(ndt_scan_matcher): isolate align function into ndt_omp #2199

Merged
merged 7 commits into from
Nov 7, 2022
Merged

refactor(ndt_scan_matcher): isolate align function into ndt_omp #2199

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
fbcc24c
first commit
kminoda Nov 2, 2022
b968998
remove align and NdtResults from ndt_scan_matcher
kminoda Nov 2, 2022
48c9c59
rename variable
kminoda Nov 2, 2022
73af27b
ci(pre-commit): autofix
pre-commit-ci[bot] Nov 2, 2022
b844ccf
use getResult() function instead of executeScanMatching()
kminoda Nov 4, 2022
717c616
for (auto...) -> for (const auto &...)
kminoda Nov 4, 2022
6580214
removed unnecessary output_cloud declaration
kminoda Nov 7, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 0 additions & 10 deletions localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp
View file
Open in desktop
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
Open in desktop
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