|
| 1 | +// Copyright 2024 Autoware Foundation |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +#include "localization_util/covariance_ellipse.hpp" |
| 16 | + |
| 17 | +#include <tf2/utils.h> |
| 18 | +#ifdef ROS_DISTRO_GALACTIC |
| 19 | +#include <tf2_geometry_msgs/tf2_geometry_msgs.h> |
| 20 | +#else |
| 21 | +#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp> |
| 22 | +#endif |
| 23 | + |
| 24 | +namespace autoware::localization_util |
| 25 | +{ |
| 26 | + |
| 27 | +Ellipse calculate_xy_ellipse( |
| 28 | + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double scale) |
| 29 | +{ |
| 30 | + // input geometry_msgs::PoseWithCovariance contain 6x6 matrix |
| 31 | + Eigen::Matrix2d xy_covariance; |
| 32 | + const auto cov = pose_with_covariance.covariance; |
| 33 | + xy_covariance(0, 0) = cov[0 * 6 + 0]; |
| 34 | + xy_covariance(0, 1) = cov[0 * 6 + 1]; |
| 35 | + xy_covariance(1, 0) = cov[1 * 6 + 0]; |
| 36 | + xy_covariance(1, 1) = cov[1 * 6 + 1]; |
| 37 | + |
| 38 | + Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(xy_covariance); |
| 39 | + |
| 40 | + Ellipse ellipse; |
| 41 | + |
| 42 | + // eigen values and vectors are sorted in ascending order |
| 43 | + ellipse.long_radius = scale * std::sqrt(eigensolver.eigenvalues()(1)); |
| 44 | + ellipse.short_radius = scale * std::sqrt(eigensolver.eigenvalues()(0)); |
| 45 | + |
| 46 | + // principal component vector |
| 47 | + const Eigen::Vector2d pc_vector = eigensolver.eigenvectors().col(1); |
| 48 | + ellipse.yaw = std::atan2(pc_vector.y(), pc_vector.x()); |
| 49 | + |
| 50 | + // ellipse size along lateral direction (body-frame) |
| 51 | + ellipse.P = xy_covariance; |
| 52 | + const double yaw_vehicle = tf2::getYaw(pose_with_covariance.pose.orientation); |
| 53 | + const Eigen::Matrix2d & p_inv = ellipse.P.inverse(); |
| 54 | + Eigen::MatrixXd e(2, 1); |
| 55 | + e(0, 0) = std::cos(yaw_vehicle); |
| 56 | + e(1, 0) = std::sin(yaw_vehicle); |
| 57 | + const double d = std::sqrt((e.transpose() * p_inv * e)(0, 0) / p_inv.determinant()); |
| 58 | + ellipse.size_lateral_direction = scale * d; |
| 59 | + |
| 60 | + return ellipse; |
| 61 | +} |
| 62 | + |
| 63 | +visualization_msgs::msg::Marker create_ellipse_marker( |
| 64 | + const Ellipse & ellipse, const std_msgs::msg::Header & header, |
| 65 | + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) |
| 66 | +{ |
| 67 | + tf2::Quaternion quat; |
| 68 | + quat.setEuler(0, 0, ellipse.yaw); |
| 69 | + |
| 70 | + const double ellipse_long_radius = std::min(ellipse.long_radius, 30.0); |
| 71 | + const double ellipse_short_radius = std::min(ellipse.short_radius, 30.0); |
| 72 | + visualization_msgs::msg::Marker marker; |
| 73 | + marker.header = header; |
| 74 | + marker.ns = "error_ellipse"; |
| 75 | + marker.id = 0; |
| 76 | + marker.type = visualization_msgs::msg::Marker::SPHERE; |
| 77 | + marker.action = visualization_msgs::msg::Marker::ADD; |
| 78 | + marker.pose = pose_with_covariance.pose; |
| 79 | + marker.pose.orientation = tf2::toMsg(quat); |
| 80 | + marker.scale.x = ellipse_long_radius * 2; |
| 81 | + marker.scale.y = ellipse_short_radius * 2; |
| 82 | + marker.scale.z = 0.01; |
| 83 | + marker.color.a = 0.1; |
| 84 | + marker.color.r = 0.0; |
| 85 | + marker.color.g = 0.0; |
| 86 | + marker.color.b = 1.0; |
| 87 | + return marker; |
| 88 | +} |
| 89 | + |
| 90 | +} // namespace autoware::localization_util |
0 commit comments