refactor(localization_util): add covariance_ellipse to localization_util

localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp

@@ -15,6 +15,8 @@
 #ifndef LOCALIZATION_ERROR_MONITOR__LOCALIZATION_ERROR_MONITOR_HPP_
 #define LOCALIZATION_ERROR_MONITOR__LOCALIZATION_ERROR_MONITOR_HPP_
 
+#include "localization_util/covariance_ellipse.hpp"
+
 #include <Eigen/Dense>
 #include <autoware/universe_utils/ros/logger_level_configure.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -25,15 +27,6 @@
 
 #include <memory>
 
-struct Ellipse
-{
-  double long_radius;
-  double short_radius;
-  double yaw;
-  Eigen::Matrix2d P;
-  double size_lateral_direction;
-};
-
 class LocalizationErrorMonitor : public rclcpp::Node
 {
 private:
@@ -50,12 +43,9 @@ class LocalizationErrorMonitor : public rclcpp::Node
   double warn_ellipse_size_;
   double error_ellipse_size_lateral_direction_;
   double warn_ellipse_size_lateral_direction_;
-  Ellipse ellipse_;
+  autoware::localization_util::Ellipse ellipse_;
 
   void on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg);
-  visualization_msgs::msg::Marker create_ellipse_marker(
-    const Ellipse & ellipse, nav_msgs::msg::Odometry::ConstSharedPtr odom);
-  static double measure_size_ellipse_along_body_frame(const Eigen::Matrix2d & Pinv, double theta);
 
 public:
   explicit LocalizationErrorMonitor(const rclcpp::NodeOptions & options);

localization/localization_error_monitor/package.xml

@@ -23,6 +23,7 @@
   <depend>autoware_universe_utils</depend>
   <depend>diagnostic_msgs</depend>
   <depend>diagnostic_updater</depend>
+  <depend>localization_util</depend>
   <depend>nav_msgs</depend>
   <depend>rclcpp_components</depend>
   <depend>tf2</depend>

localization/localization_error_monitor/src/localization_error_monitor.cpp

@@ -58,61 +58,12 @@
   logger_configure_ = std::make_unique<autoware::universe_utils::LoggerLevelConfigure>(this);
 }
 
-visualization_msgs::msg::Marker LocalizationErrorMonitor::create_ellipse_marker(
-  const Ellipse & ellipse, nav_msgs::msg::Odometry::ConstSharedPtr odom)
-{
-  tf2::Quaternion quat;
-  quat.setEuler(0, 0, ellipse.yaw);
-
-  const double ellipse_long_radius = std::min(ellipse.long_radius, 30.0);
-  const double ellipse_short_radius = std::min(ellipse.short_radius, 30.0);
-  visualization_msgs::msg::Marker marker;
-  marker.header = odom->header;
-  marker.header.stamp = this->now();
-  marker.ns = "error_ellipse";
-  marker.id = 0;
-  marker.type = visualization_msgs::msg::Marker::SPHERE;
-  marker.action = visualization_msgs::msg::Marker::ADD;
-  marker.pose = odom->pose.pose;
-  marker.pose.orientation = tf2::toMsg(quat);
-  marker.scale.x = ellipse_long_radius * 2;
-  marker.scale.y = ellipse_short_radius * 2;
-  marker.scale.z = 0.01;
-  marker.color.a = 0.1;
-  marker.color.r = 0.0;
-  marker.color.g = 0.0;
-  marker.color.b = 1.0;
-  return marker;
-}
-
 void LocalizationErrorMonitor::on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg)
 {
-  // create xy covariance (2x2 matrix)
-  // input geometry_msgs::PoseWithCovariance contain 6x6 matrix
-  Eigen::Matrix2d xy_covariance;
-  const auto cov = input_msg->pose.covariance;
-  xy_covariance(0, 0) = cov[0 * 6 + 0];
-  xy_covariance(0, 1) = cov[0 * 6 + 1];
-  xy_covariance(1, 0) = cov[1 * 6 + 0];
-  xy_covariance(1, 1) = cov[1 * 6 + 1];
-
-  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(xy_covariance);
-
-  // eigen values and vectors are sorted in ascending order
-  ellipse_.long_radius = scale_ * std::sqrt(eigensolver.eigenvalues()(1));
-  ellipse_.short_radius = scale_ * std::sqrt(eigensolver.eigenvalues()(0));
-
-  // principal component vector
-  const Eigen::Vector2d pc_vector = eigensolver.eigenvectors().col(1);
-  ellipse_.yaw = std::atan2(pc_vector.y(), pc_vector.x());
-
-  // ellipse size along lateral direction (body-frame)
-  ellipse_.P = xy_covariance;
-  const double yaw_vehicle = tf2::getYaw(input_msg->pose.pose.orientation);
-  ellipse_.size_lateral_direction =
-    scale_ * measure_size_ellipse_along_body_frame(ellipse_.P.inverse(), yaw_vehicle);
-
-  const auto ellipse_marker = create_ellipse_marker(ellipse_, input_msg);
+  ellipse_ = autoware::localization_util::calculate_xy_ellipse(input_msg->pose, scale_);
+
+  const auto ellipse_marker = autoware::localization_util::create_ellipse_marker(
+    ellipse_, input_msg->header, input_msg->pose);
   ellipse_marker_pub_->publish(ellipse_marker);
 
   // diagnostics
@@ -134,16 +85,5 @@
   diag_pub_->publish(diag_msg);
 }
 
-double LocalizationErrorMonitor::measure_size_ellipse_along_body_frame(
-  const Eigen::Matrix2d & Pinv, const double theta)
-{
-  Eigen::MatrixXd e(2, 1);
-  e(0, 0) = std::cos(theta);
-  e(1, 0) = std::sin(theta);
-
-  double d = std::sqrt((e.transpose() * Pinv * e)(0, 0) / Pinv.determinant());
-  return d;
-}
-
 #include <rclcpp_components/register_node_macro.hpp>
 RCLCPP_COMPONENTS_REGISTER_NODE(LocalizationErrorMonitor)

localization/localization_util/CMakeLists.txt

@@ -8,6 +8,7 @@ ament_auto_add_library(localization_util SHARED
   src/util_func.cpp
   src/smart_pose_buffer.cpp
   src/tree_structured_parzen_estimator.cpp
+  src/covariance_ellipse.cpp
 )
 
 if(BUILD_TESTING)

localization/localization_util/include/localization_util/covariance_ellipse.hpp

@@ -0,0 +1,44 @@
+// Copyright 2024 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_
+#define LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_
+
+#include <Eigen/Dense>
+
+#include <geometry_msgs/msg/pose_with_covariance.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+namespace autoware::localization_util
+{
+
+struct Ellipse
+{
+  double long_radius;
+  double short_radius;
+  double yaw;
+  Eigen::Matrix2d P;
+  double size_lateral_direction;
+};
+
+Ellipse calculate_xy_ellipse(
+  const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double scale);
+
+visualization_msgs::msg::Marker create_ellipse_marker(
+  const Ellipse & ellipse, const std_msgs::msg::Header & header,
+  const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance);
+
+}  // namespace autoware::localization_util
+
+#endif  // LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_

localization/localization_util/src/covariance_ellipse.cpp

@@ -0,0 +1,90 @@
+// Copyright 2024 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "localization_util/covariance_ellipse.hpp"
+
+#include <tf2/utils.h>
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+namespace autoware::localization_util
+{
+
+Ellipse calculate_xy_ellipse(
+  const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double scale)
+{
+  // input geometry_msgs::PoseWithCovariance contain 6x6 matrix
+  Eigen::Matrix2d xy_covariance;
+  const auto cov = pose_with_covariance.covariance;
+  xy_covariance(0, 0) = cov[0 * 6 + 0];
+  xy_covariance(0, 1) = cov[0 * 6 + 1];
+  xy_covariance(1, 0) = cov[1 * 6 + 0];
+  xy_covariance(1, 1) = cov[1 * 6 + 1];
+
+  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(xy_covariance);
+
+  Ellipse ellipse;
+
+  // eigen values and vectors are sorted in ascending order
+  ellipse.long_radius = scale * std::sqrt(eigensolver.eigenvalues()(1));
+  ellipse.short_radius = scale * std::sqrt(eigensolver.eigenvalues()(0));
+
+  // principal component vector
+  const Eigen::Vector2d pc_vector = eigensolver.eigenvectors().col(1);
+  ellipse.yaw = std::atan2(pc_vector.y(), pc_vector.x());
+
+  // ellipse size along lateral direction (body-frame)
+  ellipse.P = xy_covariance;
+  const double yaw_vehicle = tf2::getYaw(pose_with_covariance.pose.orientation);
+  const Eigen::Matrix2d & p_inv = ellipse.P.inverse();
+  Eigen::MatrixXd e(2, 1);
+  e(0, 0) = std::cos(yaw_vehicle);
+  e(1, 0) = std::sin(yaw_vehicle);
+  const double d = std::sqrt((e.transpose() * p_inv * e)(0, 0) / p_inv.determinant());
+  ellipse.size_lateral_direction = scale * d;
+
+  return ellipse;
+}
+
+visualization_msgs::msg::Marker create_ellipse_marker(
+  const Ellipse & ellipse, const std_msgs::msg::Header & header,
+  const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance)
+{
+  tf2::Quaternion quat;
+  quat.setEuler(0, 0, ellipse.yaw);
+
+  const double ellipse_long_radius = std::min(ellipse.long_radius, 30.0);
+  const double ellipse_short_radius = std::min(ellipse.short_radius, 30.0);
+  visualization_msgs::msg::Marker marker;
+  marker.header = header;
+  marker.ns = "error_ellipse";
+  marker.id = 0;
+  marker.type = visualization_msgs::msg::Marker::SPHERE;
+  marker.action = visualization_msgs::msg::Marker::ADD;
+  marker.pose = pose_with_covariance.pose;
+  marker.pose.orientation = tf2::toMsg(quat);
+  marker.scale.x = ellipse_long_radius * 2;
+  marker.scale.y = ellipse_short_radius * 2;
+  marker.scale.z = 0.01;
+  marker.color.a = 0.1;
+  marker.color.r = 0.0;
+  marker.color.g = 0.0;
+  marker.color.b = 1.0;
+  return marker;
+}
+
+}  // namespace autoware::localization_util