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

feat(ndt_scan_matcher): estimate NDT covariance in real-time with limited initial positions #5338

Merged
merged 23 commits into from
Nov 7, 2023
Merged

feat(ndt_scan_matcher): estimate NDT covariance in real-time with limited initial positions #5338

Show file tree
Hide file tree
Changes from 7 commits
Commits
Show all changes
23 commits
Select commit Hold shift + click to select a range
711b821
add covariance estiamtion
KOKIAOKI Aug 3, 2023
7025f9e
fix: msg
KOKIAOKI Aug 3, 2023
50180f3
fix: typo
KOKIAOKI Aug 3, 2023
6444d2f
fix: yaml
KOKIAOKI Oct 17, 2023
130a008
fix
KOKIAOKI Oct 17, 2023
f391d75
Merge remote-tracking branch 'upstream/main'
KOKIAOKI Oct 17, 2023
f20bee4
Update localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
KOKIAOKI Oct 18, 2023
692625f
fix: vector initilization, add: explanation in README
KOKIAOKI Oct 18, 2023
dc63ba0
Merge branch 'main' into main
KOKIAOKI Oct 18, 2023
ea68d6b
Merge branch 'main' into main
KOKIAOKI Oct 23, 2023
69f0e2c
fix: initialization and some build errors
KOKIAOKI Oct 24, 2023
af03d2d
Merge remote-tracking branch 'upstream/main'
KOKIAOKI Oct 24, 2023
2cf6018
refactor: variable and function names, changed covariance calculation…
KOKIAOKI Oct 25, 2023
9f6098b
fix: README explanation
KOKIAOKI Oct 25, 2023
dde2a8a
Merge branch 'main' into main
KOKIAOKI Oct 27, 2023
19eb8a5
Merge branch 'main' into main
KOKIAOKI Oct 31, 2023
d129cfb
Merge branch 'main' into main
KOKIAOKI Oct 31, 2023
41442ad
fix: functionalization of eigenvector calculation, README empty line
KOKIAOKI Oct 31, 2023
468d3ce
add warning outputs, rename function, move two functions in util.cpp …
KOKIAOKI Nov 1, 2023
84a5735
Merge branch 'main' into main
KOKIAOKI Nov 1, 2023
3bf2b1f
Merge branch 'main' into main
KOKIAOKI Nov 2, 2023
5eda6c3
fix: format of markdown
KOKIAOKI Nov 2, 2023
3e31b93
fix: add space in readme
KOKIAOKI Nov 2, 2023
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
5 changes: 5 additions & 0 deletions localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,6 +72,11 @@
0.0, 0.0, 0.0, 0.0, 0.0, 0.000625,
]

# 2D Real-time covariance estimation (output_pose_covariance is the minimum value)
use_covariance_estimation: false
offset_array_x: [0.0,0.0,0.5,-0.5,1.0,-1.0]
offset_array_y: [0.5,-0.5,0.0,0.0,0.0,0.0]
KYabuuchi marked this conversation as resolved.
Show resolved Hide resolved

# Regularization switch
regularization_enabled: false

Expand Down
11 changes: 10 additions & 1 deletion 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 @@ -25,6 +25,7 @@

#include <diagnostic_msgs/msg/diagnostic_array.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/pose_array.hpp>
YamatoAndo marked this conversation as resolved.
Show resolved Hide resolved
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
Expand Down Expand Up @@ -105,7 +106,7 @@ class NDTScanMatcher : public rclcpp::Node
const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg);
void publish_pose(
const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg,
const bool is_converged);
const std::array<double, 36> & ndt_covariance, const bool is_converged);
void publish_point_cloud(
const rclcpp::Time & sensor_ros_time, const std::string & frame_id,
const pcl::shared_ptr<pcl::PointCloud<PointSource>> & sensor_points_in_map_ptr);
Expand All @@ -123,6 +124,10 @@ class NDTScanMatcher : public rclcpp::Node
bool validate_converged_param(
const double & transform_probability, const double & nearest_voxel_transformation_likelihood);

void estimate_covariance(
const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix,
std::array<double, 36> & ndt_covariance, const rclcpp::Time & sensor_ros_time);

std::optional<Eigen::Matrix4f> interpolate_regularization_pose(
const rclcpp::Time & sensor_ros_time);
void add_regularization_pose(const rclcpp::Time & sensor_ros_time);
Expand All @@ -141,6 +146,8 @@ class NDTScanMatcher : public rclcpp::Node
ndt_pose_with_covariance_pub_;
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
initial_pose_with_covariance_pub_;
rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr multi_ndt_pose_pub_;
rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr multi_initial_pose_pub_;
rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr exe_time_pub_;
rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr transform_probability_pub_;
rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr
Expand Down Expand Up @@ -187,6 +194,8 @@ class NDTScanMatcher : public rclcpp::Node
double initial_pose_distance_tolerance_m_;
float inversion_vector_threshold_;
float oscillation_threshold_;
bool use_cov_estimation_;
std::vector<Eigen::Vector2d> offset_array_;
std::array<double, 36> output_pose_covariance_;

std::deque<geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr>
Expand Down
4 changes: 4 additions & 0 deletions localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,10 @@
#include <string>
#include <vector>

void create_offset_array(
const std::vector<double> & x, const std::vector<double> & y,
std::vector<Eigen::Vector2d> & offset_array);

// ref by http://takacity.blog.fc2.com/blog-entry-69.html
std_msgs::msg::ColorRGBA exchange_color_crc(double x);

Expand Down
91 changes: 88 additions & 3 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
@@ -1,4 +1,4 @@
// Copyright 2015-2019 Autoware Foundation

Check notice on line 1 in localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Primitive Obsession 

The ratio of primivite types in function arguments is no longer above the threshold
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -141,6 +141,15 @@
initial_pose_distance_tolerance_m_ =
this->declare_parameter<double>("initial_pose_distance_tolerance_m");

use_cov_estimation_ = this->declare_parameter("use_covariance_estimation", use_cov_estimation_);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please do not use the default parameter and instead do as follow

Suggested change
use_cov_estimation_ = this->declare_parameter("use_covariance_estimation", use_cov_estimation_);
use_cov_estimation_ = this->declare_parameter<bool>("use_covariance_estimation");

if (use_cov_estimation_) {
std::vector<double> offset_array_x =
this->declare_parameter<std::vector<double>>("offset_array_x");
std::vector<double> offset_array_y =
this->declare_parameter<std::vector<double>>("offset_array_y");
create_offset_array(offset_array_x, offset_array_y, offset_array_);
}

std::vector<double> output_pose_covariance =
this->declare_parameter<std::vector<double>>("output_pose_covariance");
for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) {
Expand Down Expand Up @@ -191,6 +200,9 @@
initial_pose_with_covariance_pub_ =
this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
"initial_pose_with_covariance", 10);
multi_ndt_pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseArray>("multi_ndt_pose", 10);
multi_initial_pose_pub_ =
this->create_publisher<geometry_msgs::msg::PoseArray>("multi_initial_pose", 10);
exe_time_pub_ = this->create_publisher<tier4_debug_msgs::msg::Float32Stamped>("exe_time_ms", 10);
transform_probability_pub_ =
this->create_publisher<tier4_debug_msgs::msg::Float32Stamped>("transform_probability", 10);
Expand Down Expand Up @@ -478,16 +490,23 @@
RCLCPP_WARN(get_logger(), "Not Converged");
}

// covariance estimation
std::array<double, 36> ndt_covariance;
ndt_covariance = output_pose_covariance_;
KYabuuchi marked this conversation as resolved.
Show resolved Hide resolved
if (is_converged && use_cov_estimation_) {
estimate_covariance(ndt_result, initial_pose_matrix, ndt_covariance, sensor_ros_time);
}
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I believe this part is relatively computationally heavy, so I would recommend to move this right after the following part:

  // perform ndt scan matching
  (*state_ptr_)["state"] = "Aligning";
  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();
  (*state_ptr_)["state"] = "Sleeping

This will also enable users to monitor the total execution time with exe_time as well.


// publish
initial_pose_with_covariance_pub_->publish(interpolator.get_current_pose());
exe_time_pub_->publish(make_float32_stamped(sensor_ros_time, exe_time));
transform_probability_pub_->publish(
make_float32_stamped(sensor_ros_time, ndt_result.transform_probability));
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, result_pose_msg);
publish_pose(sensor_ros_time, result_pose_msg, is_converged);
publish_pose(sensor_ros_time, result_pose_msg, ndt_covariance, is_converged);

Check warning on line 509 in localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

NDTScanMatcher::callback_sensor_points increases in cyclomatic complexity from 16 to 18, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
publish_marker(sensor_ros_time, transformation_msg_array);
publish_initial_to_result(
sensor_ros_time, result_pose_msg, interpolator.get_current_pose(), interpolator.get_old_pose(),
Expand Down Expand Up @@ -570,7 +589,7 @@

void NDTScanMatcher::publish_pose(
const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg,
const bool is_converged)
const std::array<double, 36> & ndt_covariance, const bool is_converged)
{
geometry_msgs::msg::PoseStamped result_pose_stamped_msg;
result_pose_stamped_msg.header.stamp = sensor_ros_time;
Expand All @@ -581,7 +600,7 @@
result_pose_with_cov_msg.header.stamp = sensor_ros_time;
result_pose_with_cov_msg.header.frame_id = map_frame_;
result_pose_with_cov_msg.pose.pose = result_pose_msg;
result_pose_with_cov_msg.pose.covariance = output_pose_covariance_;
result_pose_with_cov_msg.pose.covariance = ndt_covariance;

if (is_converged) {
ndt_pose_pub_->publish(result_pose_stamped_msg);
Expand Down Expand Up @@ -704,6 +723,72 @@
return is_ok_converged_param;
}

void NDTScanMatcher::estimate_covariance(
const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix,
std::array<double, 36> & ndt_covariance, const rclcpp::Time & sensor_ros_time)
{
// Laplace approximation
const Eigen::Matrix2d hessian_inv = -ndt_result.hessian.inverse().block(0, 0, 2, 2);
const Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> lap_eigensolver(hessian_inv);
Eigen::Matrix2d rot;
rot = Eigen::Rotation2Dd(0.0);
KYabuuchi marked this conversation as resolved.
Show resolved Hide resolved
if (lap_eigensolver.info() == Eigen::Success) {
const Eigen::Vector2d lap_eigen_vec = lap_eigensolver.eigenvectors().col(1);
const double th = std::atan2(lap_eigen_vec.y(), lap_eigen_vec.x());
rot = Eigen::Rotation2Dd(th);
}

// first result is added to mean and covariance
Eigen::Vector2d p2d(ndt_result.pose(0, 3), ndt_result.pose(1, 3));
Eigen::Vector2d tmp_centroid = p2d;
Eigen::Matrix2d tmp_cov = p2d * p2d.transpose();

// prepare msg
geometry_msgs::msg::PoseArray multi_ndt_result_msg;
geometry_msgs::msg::PoseArray multi_initial_pose_msg;
multi_ndt_result_msg.header.stamp = sensor_ros_time;
multi_initial_pose_msg.header.stamp = sensor_ros_time;
KYabuuchi marked this conversation as resolved.
Show resolved Hide resolved
multi_ndt_result_msg.poses.push_back(matrix4f_to_pose(ndt_result.pose));
multi_initial_pose_msg.poses.push_back(matrix4f_to_pose(initial_pose_matrix));

// Multiple search
for (const auto & offset_vec : offset_array_) {
Eigen::Vector2d offset_2d;
offset_2d = rot * offset_vec;

Eigen::Matrix4f sub_initial_pose_matrix(Eigen::Matrix4f::Identity());
sub_initial_pose_matrix = ndt_result.pose;
sub_initial_pose_matrix(0, 3) += static_cast<float>(offset_2d.x());
sub_initial_pose_matrix(1, 3) += static_cast<float>(offset_2d.y());

// align
auto sub_output_cloud = std::make_shared<pcl::PointCloud<PointSource>>();
ndt_ptr_->align(*sub_output_cloud, sub_initial_pose_matrix);
const Eigen::Matrix4f sub_ndt_result = ndt_ptr_->getResult().pose;

// Eigen::Vector2d sub_p2d(
// static_cast<double>(sub_ndt_result(0, 3)), static_cast<double>(sub_ndt_result(1, 3)));
Eigen::Vector2d sub_p2d = sub_ndt_result.topRightCorner<2, 1>().cast<double>();
tmp_centroid += sub_p2d;
tmp_cov += sub_p2d * sub_p2d.transpose();
KYabuuchi marked this conversation as resolved.
Show resolved Hide resolved

// push back msg
multi_ndt_result_msg.poses.push_back(matrix4f_to_pose(sub_ndt_result));
multi_initial_pose_msg.poses.push_back(matrix4f_to_pose(sub_initial_pose_matrix));
}
Eigen::Vector2d centroid = tmp_centroid / (offset_array_.size() + 1);
// TODO unbiased covariance: R * ((n-1)/n))
YamatoAndo marked this conversation as resolved.
Show resolved Hide resolved
Eigen::Matrix2d pca_covariance;
KYabuuchi marked this conversation as resolved.
Show resolved Hide resolved
pca_covariance = (tmp_cov / (offset_array_.size() + 1) - (centroid * centroid.transpose()));
ndt_covariance[0] = output_pose_covariance_[0] + pca_covariance(0, 0);
ndt_covariance[1] = pca_covariance(1, 0);
ndt_covariance[6] = pca_covariance(0, 1);
ndt_covariance[7] = output_pose_covariance_[7] + pca_covariance(1, 1);
KYabuuchi marked this conversation as resolved.
Show resolved Hide resolved

multi_ndt_pose_pub_->publish(multi_ndt_result_msg);
multi_initial_pose_pub_->publish(multi_initial_pose_msg);
}

std::optional<Eigen::Matrix4f> NDTScanMatcher::interpolate_regularization_pose(
const rclcpp::Time & sensor_ros_time)
{
Expand Down
13 changes: 13 additions & 0 deletions localization/ndt_scan_matcher/src/util_func.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,19 @@

static std::random_device seed_gen;

void create_offset_array(
const std::vector<double> & x, const std::vector<double> & y,
std::vector<Eigen::Vector2d> & offset_array)
{
int size = x.size();
offset_array.resize(size);
if (x.size() == y.size())
for (int i = 0; i < size; i++) {
offset_array[i].x() = x[i];
offset_array[i].y() = y[i];
}
}

// ref by http://takacity.blog.fc2.com/blog-entry-69.html
std_msgs::msg::ColorRGBA exchange_color_crc(double x)
{
Expand Down
Loading