diff --git a/localization/autoware_localization_util/CHANGELOG.rst b/localization/autoware_localization_util/CHANGELOG.rst deleted file mode 100644 index be40e3ee8560e..0000000000000 --- a/localization/autoware_localization_util/CHANGELOG.rst +++ /dev/null @@ -1,51 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_localization_util -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - localization (`#9567 `_) -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(localization_util)!: prefix package and namespace with autoware (`#8922 `_) - add autoware prefix to localization_util -* Contributors: Masaki Baba, Yutaka Kondo - -0.26.0 (2024-04-03) -------------------- diff --git a/localization/autoware_localization_util/CMakeLists.txt b/localization/autoware_localization_util/CMakeLists.txt deleted file mode 100644 index de62633124f3d..0000000000000 --- a/localization/autoware_localization_util/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_localization_util) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/util_func.cpp - src/smart_pose_buffer.cpp - src/tree_structured_parzen_estimator.cpp - src/covariance_ellipse.cpp -) - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - ament_auto_add_gtest(test_smart_pose_buffer - test/test_smart_pose_buffer.cpp - src/smart_pose_buffer.cpp - ) - - ament_auto_add_gtest(test_tpe - test/test_tpe.cpp - src/tree_structured_parzen_estimator.cpp - ) -endif() - -ament_auto_package( - INSTALL_TO_SHARE -) diff --git a/localization/autoware_localization_util/README.md b/localization/autoware_localization_util/README.md deleted file mode 100644 index f7fddd9eebf05..0000000000000 --- a/localization/autoware_localization_util/README.md +++ /dev/null @@ -1,5 +0,0 @@ -# autoware_localization_util - -`autoware_localization_util` is a localization utility package. - -This package does not have a node, it is just a library. diff --git a/localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp b/localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp deleted file mode 100644 index abd0af46856b0..0000000000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp +++ /dev/null @@ -1,44 +0,0 @@ -// 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 AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ - -#include - -#include -#include - -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 // AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp b/localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp deleted file mode 100644 index bda6ff19f2867..0000000000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2021 TierIV -// -// 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 AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ - -#include - -namespace autoware::localization_util -{ -using Matrix6d = Eigen::Matrix; -using RowMatrixXd = Eigen::Matrix; -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp b/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp deleted file mode 100644 index 8c10506c36753..0000000000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp +++ /dev/null @@ -1,71 +0,0 @@ -// Copyright 2015-2019 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 AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ - -#include "autoware/localization_util/util_func.hpp" - -#include - -#include - -#include - -namespace autoware::localization_util -{ -class SmartPoseBuffer -{ -private: - using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; - -public: - struct InterpolateResult - { - PoseWithCovarianceStamped old_pose; - PoseWithCovarianceStamped new_pose; - PoseWithCovarianceStamped interpolated_pose; - }; - - SmartPoseBuffer() = delete; - SmartPoseBuffer( - const rclcpp::Logger & logger, const double & pose_timeout_sec, - const double & pose_distance_tolerance_meters); - - std::optional interpolate(const rclcpp::Time & target_ros_time); - - void push_back(const PoseWithCovarianceStamped::ConstSharedPtr & pose_msg_ptr); - - void pop_old(const rclcpp::Time & target_ros_time); - - void clear(); - -private: - rclcpp::Logger logger_; - std::deque pose_buffer_; - std::mutex mutex_; // This mutex is for pose_buffer_ - - const double pose_timeout_sec_; - const double pose_distance_tolerance_meters_; - - [[nodiscard]] bool validate_time_stamp_difference( - const rclcpp::Time & target_time, const rclcpp::Time & reference_time, - const double time_tolerance_sec) const; - [[nodiscard]] bool validate_position_difference( - const geometry_msgs::msg::Point & target_point, - const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const; -}; -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp b/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp deleted file mode 100644 index ddf7625c202ec..0000000000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp +++ /dev/null @@ -1,87 +0,0 @@ -// Copyright 2023 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 AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ - -/* -A implementation of tree-structured parzen estimator (TPE) -See below pdf for the TPE algorithm detail. -https://papers.nips.cc/paper_files/paper/2011/file/86e8f7ab32cfd12577bc2619bc635690-Paper.pdf - -Optuna is also used as a reference for implementation. -https://github.com/optuna/optuna -*/ - -#include -#include -#include - -namespace autoware::localization_util -{ -class TreeStructuredParzenEstimator -{ -public: - using Input = std::vector; - using Score = double; - struct Trial - { - Input input; - Score score; - }; - - enum Direction { - MINIMIZE = 0, - MAXIMIZE = 1, - }; - - enum Index { - TRANS_X = 0, - TRANS_Y = 1, - TRANS_Z = 2, - ANGLE_X = 3, - ANGLE_Y = 4, - ANGLE_Z = 5, - INDEX_NUM = 6, - }; - - TreeStructuredParzenEstimator() = delete; - TreeStructuredParzenEstimator( - const Direction direction, const int64_t n_startup_trials, std::vector sample_mean, - std::vector sample_stddev); - void add_trial(const Trial & trial); - [[nodiscard]] Input get_next_input() const; - -private: - static constexpr double max_good_rate = 0.10; - static constexpr int64_t n_ei_candidates = 100; - - static std::mt19937_64 engine; - - [[nodiscard]] double compute_log_likelihood_ratio(const Input & input) const; - [[nodiscard]] static double log_gaussian_pdf( - const Input & input, const Input & mu, const Input & sigma); - - std::vector trials_; - int64_t above_num_; - const Direction direction_; - const int64_t n_startup_trials_; - const int64_t input_dimension_; - const std::vector sample_mean_; - const std::vector sample_stddev_; - Input base_stddev_; -}; -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp b/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp deleted file mode 100644 index bef9968f34b6f..0000000000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp +++ /dev/null @@ -1,88 +0,0 @@ -// Copyright 2015-2019 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 AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ - -#include -#include -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#include -#else -#include - -#include -#endif - -#include -#include -#include -#include -#include -#include - -namespace autoware::localization_util -{ -// ref by http://takacity.blog.fc2.com/blog-entry-69.html -std_msgs::msg::ColorRGBA exchange_color_crc(double x); - -double calc_diff_for_radian(const double lhs_rad, const double rhs_rad); - -// x: roll, y: pitch, z: yaw -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose); -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseStamped & pose); -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); - -geometry_msgs::msg::Quaternion rpy_rad_to_quaternion( - const double r_rad, const double p_rad, const double y_rad); -geometry_msgs::msg::Quaternion rpy_deg_to_quaternion( - const double r_deg, const double p_deg, const double y_deg); - -geometry_msgs::msg::Twist calc_twist( - const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b); - -geometry_msgs::msg::PoseStamped interpolate_pose( - const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b, - const rclcpp::Time & time_stamp); - -geometry_msgs::msg::PoseStamped interpolate_pose( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp); - -Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose); -Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose); -geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_matrix); -Eigen::Vector3d point_to_vector3d(const geometry_msgs::msg::Point & ros_pos); - -template -T transform(const T & input, const geometry_msgs::msg::TransformStamped & transform) -{ - T output; - tf2::doTransform(input, output, transform); - return output; -} - -double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2); - -void output_pose_with_cov_to_log( - const rclcpp::Logger & logger, const std::string & prefix, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov); - -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ diff --git a/localization/autoware_localization_util/package.xml b/localization/autoware_localization_util/package.xml deleted file mode 100644 index 89ad7c24840dd..0000000000000 --- a/localization/autoware_localization_util/package.xml +++ /dev/null @@ -1,35 +0,0 @@ - - - - autoware_localization_util - 0.40.0 - The autoware_localization_util package - Yamato Ando - Masahiro Sakamoto - Shintaro Sakoda - Kento Yabuuchi - NGUYEN Viet Anh - Taiki Yamada - Ryu Yamamoto - Apache License 2.0 - Yamato Ando - - ament_cmake_auto - autoware_cmake - - diagnostic_msgs - geometry_msgs - rclcpp - std_msgs - tf2 - tf2_eigen - tf2_geometry_msgs - visualization_msgs - - ament_cmake_cppcheck - ament_lint_auto - - - ament_cmake - - diff --git a/localization/autoware_localization_util/src/covariance_ellipse.cpp b/localization/autoware_localization_util/src/covariance_ellipse.cpp deleted file mode 100644 index 847f89e0da9b3..0000000000000 --- a/localization/autoware_localization_util/src/covariance_ellipse.cpp +++ /dev/null @@ -1,92 +0,0 @@ -// 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 "autoware/localization_util/covariance_ellipse.hpp" - -#include - -#include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#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 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 diff --git a/localization/autoware_localization_util/src/smart_pose_buffer.cpp b/localization/autoware_localization_util/src/smart_pose_buffer.cpp deleted file mode 100644 index 3b529d68cf6c5..0000000000000 --- a/localization/autoware_localization_util/src/smart_pose_buffer.cpp +++ /dev/null @@ -1,158 +0,0 @@ -// Copyright 2023- 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 "autoware/localization_util/smart_pose_buffer.hpp" - -namespace autoware::localization_util -{ -SmartPoseBuffer::SmartPoseBuffer( - const rclcpp::Logger & logger, const double & pose_timeout_sec, - const double & pose_distance_tolerance_meters) -: logger_(logger), - pose_timeout_sec_(pose_timeout_sec), - pose_distance_tolerance_meters_(pose_distance_tolerance_meters) -{ -} - -std::optional SmartPoseBuffer::interpolate( - const rclcpp::Time & target_ros_time) -{ - InterpolateResult result; - - { - std::lock_guard lock(mutex_); - - if (pose_buffer_.size() < 2) { - RCLCPP_INFO(logger_, "pose_buffer_.size() < 2"); - return std::nullopt; - } - - const rclcpp::Time time_first = pose_buffer_.front()->header.stamp; - const rclcpp::Time time_last = pose_buffer_.back()->header.stamp; - - if (target_ros_time < time_first) { - RCLCPP_INFO(logger_, "Mismatch between pose timestamp and current timestamp"); - return std::nullopt; - } - - // [time_last < target_ros_time] is acceptable here. - // It is possible that the target_ros_time (often sensor timestamp) is newer than the latest - // timestamp of buffered pose (often EKF). - // However, if the timestamp difference is too large, - // it will later be rejected by validate_time_stamp_difference. - - // get the nearest poses - result.old_pose = *pose_buffer_.front(); - for (const PoseWithCovarianceStamped::ConstSharedPtr & pose_cov_msg_ptr : pose_buffer_) { - result.new_pose = *pose_cov_msg_ptr; - const rclcpp::Time pose_time_stamp = result.new_pose.header.stamp; - if (pose_time_stamp > target_ros_time) { - break; - } - result.old_pose = *pose_cov_msg_ptr; - } - } - - // check the time stamp - const bool is_old_pose_valid = validate_time_stamp_difference( - result.old_pose.header.stamp, target_ros_time, pose_timeout_sec_); - const bool is_new_pose_valid = validate_time_stamp_difference( - result.new_pose.header.stamp, target_ros_time, pose_timeout_sec_); - - // check the position jumping (ex. immediately after the initial pose estimation) - const bool is_pose_diff_valid = validate_position_difference( - result.old_pose.pose.pose.position, result.new_pose.pose.pose.position, - pose_distance_tolerance_meters_); - - // all validations must be true - if (!(is_old_pose_valid && is_new_pose_valid && is_pose_diff_valid)) { - return std::nullopt; - } - - // interpolate the pose - const geometry_msgs::msg::PoseStamped interpolated_pose_msg = - interpolate_pose(result.old_pose, result.new_pose, target_ros_time); - result.interpolated_pose.header = interpolated_pose_msg.header; - result.interpolated_pose.pose.pose = interpolated_pose_msg.pose; - // This does not interpolate the covariance. - // interpolated_pose.pose.covariance is always set to old_pose.covariance - result.interpolated_pose.pose.covariance = result.old_pose.pose.covariance; - return result; -} - -void SmartPoseBuffer::push_back(const PoseWithCovarianceStamped::ConstSharedPtr & pose_msg_ptr) -{ - std::lock_guard lock(mutex_); - if (!pose_buffer_.empty()) { - // Check for non-chronological timestamp order - // This situation can arise when replaying a rosbag multiple times - const rclcpp::Time end_time = pose_buffer_.back()->header.stamp; - const rclcpp::Time msg_time = pose_msg_ptr->header.stamp; - if (msg_time < end_time) { - // Clear the buffer if timestamps are reversed to maintain chronological order - pose_buffer_.clear(); - } - } - pose_buffer_.push_back(pose_msg_ptr); -} - -void SmartPoseBuffer::pop_old(const rclcpp::Time & target_ros_time) -{ - std::lock_guard lock(mutex_); - while (!pose_buffer_.empty()) { - if (rclcpp::Time(pose_buffer_.front()->header.stamp) >= target_ros_time) { - break; - } - pose_buffer_.pop_front(); - } -} - -void SmartPoseBuffer::clear() -{ - std::lock_guard lock(mutex_); - pose_buffer_.clear(); -} - -bool SmartPoseBuffer::validate_time_stamp_difference( - const rclcpp::Time & target_time, const rclcpp::Time & reference_time, - const double time_tolerance_sec) const -{ - const double dt = std::abs((target_time - reference_time).seconds()); - const bool success = dt < time_tolerance_sec; - if (!success) { - RCLCPP_WARN( - logger_, - "Validation error. The reference time is %lf[sec], but the target time is %lf[sec]. The " - "difference is %lf[sec] (the tolerance is %lf[sec]).", - reference_time.seconds(), target_time.seconds(), dt, time_tolerance_sec); - } - return success; -} - -bool SmartPoseBuffer::validate_position_difference( - const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Point & reference_point, - const double distance_tolerance_m_) const -{ - const double distance = norm(target_point, reference_point); - const bool success = distance < distance_tolerance_m_; - if (!success) { - RCLCPP_WARN( - logger_, - "Validation error. The distance from reference position to target position is %lf[m] (the " - "tolerance is %lf[m]).", - distance, distance_tolerance_m_); - } - return success; -} -} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp b/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp deleted file mode 100644 index e9f0318d1e06d..0000000000000 --- a/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp +++ /dev/null @@ -1,185 +0,0 @@ -// Copyright 2023 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 "autoware/localization_util/tree_structured_parzen_estimator.hpp" - -#include -#include -#include -#include -#include -#include -#include - -namespace autoware::localization_util -{ -// random number generator -std::mt19937_64 TreeStructuredParzenEstimator::engine(0); - -TreeStructuredParzenEstimator::TreeStructuredParzenEstimator( - const Direction direction, const int64_t n_startup_trials, std::vector sample_mean, - std::vector sample_stddev) -: above_num_(0), - direction_(direction), - n_startup_trials_(n_startup_trials), - input_dimension_(INDEX_NUM), - sample_mean_(std::move(sample_mean)), - sample_stddev_(std::move(sample_stddev)) -{ - if (sample_mean_.size() != ANGLE_Z) { - std::cerr << "sample_mean size is invalid" << std::endl; - throw std::runtime_error("sample_mean size is invalid"); - } - if (sample_stddev_.size() != ANGLE_Z) { - std::cerr << "sample_stddev size is invalid" << std::endl; - throw std::runtime_error("sample_stddev size is invalid"); - } - // base_stddev_ is defined as the stable convergence range of ndt_scan_matcher. - base_stddev_.resize(input_dimension_); - base_stddev_[TRANS_X] = 0.25; // [m] - base_stddev_[TRANS_Y] = 0.25; // [m] - base_stddev_[TRANS_Z] = 0.25; // [m] - base_stddev_[ANGLE_X] = 1.0 / 180.0 * M_PI; // [rad] - base_stddev_[ANGLE_Y] = 1.0 / 180.0 * M_PI; // [rad] - base_stddev_[ANGLE_Z] = 2.5 / 180.0 * M_PI; // [rad] -} - -void TreeStructuredParzenEstimator::add_trial(const Trial & trial) -{ - trials_.push_back(trial); - std::sort(trials_.begin(), trials_.end(), [this](const Trial & lhs, const Trial & rhs) { - return (direction_ == Direction::MAXIMIZE ? lhs.score > rhs.score : lhs.score < rhs.score); - }); - above_num_ = std::min( - {static_cast(10), - static_cast(static_cast(trials_.size()) * max_good_rate)}); -} - -TreeStructuredParzenEstimator::Input TreeStructuredParzenEstimator::get_next_input() const -{ - std::normal_distribution dist_normal_trans_x( - sample_mean_[TRANS_X], sample_stddev_[TRANS_X]); - std::normal_distribution dist_normal_trans_y( - sample_mean_[TRANS_Y], sample_stddev_[TRANS_Y]); - std::normal_distribution dist_normal_trans_z( - sample_mean_[TRANS_Z], sample_stddev_[TRANS_Z]); - std::normal_distribution dist_normal_angle_x( - sample_mean_[ANGLE_X], sample_stddev_[ANGLE_X]); - std::normal_distribution dist_normal_angle_y( - sample_mean_[ANGLE_Y], sample_stddev_[ANGLE_Y]); - std::uniform_real_distribution dist_uniform_angle_z(-M_PI, M_PI); - - if (static_cast(trials_.size()) < n_startup_trials_ || above_num_ == 0) { - // Random sampling based on prior until the number of trials reaches `n_startup_trials_`. - Input input(input_dimension_); - input[TRANS_X] = dist_normal_trans_x(engine); - input[TRANS_Y] = dist_normal_trans_y(engine); - input[TRANS_Z] = dist_normal_trans_z(engine); - input[ANGLE_X] = dist_normal_angle_x(engine); - input[ANGLE_Y] = dist_normal_angle_y(engine); - input[ANGLE_Z] = dist_uniform_angle_z(engine); - return input; - } - - Input best_input; - double best_log_likelihood_ratio = std::numeric_limits::lowest(); - for (int64_t i = 0; i < n_ei_candidates; i++) { - Input input(input_dimension_); - input[TRANS_X] = dist_normal_trans_x(engine); - input[TRANS_Y] = dist_normal_trans_y(engine); - input[TRANS_Z] = dist_normal_trans_z(engine); - input[ANGLE_X] = dist_normal_angle_x(engine); - input[ANGLE_Y] = dist_normal_angle_y(engine); - input[ANGLE_Z] = dist_uniform_angle_z(engine); - const double log_likelihood_ratio = compute_log_likelihood_ratio(input); - if (log_likelihood_ratio > best_log_likelihood_ratio) { - best_log_likelihood_ratio = log_likelihood_ratio; - best_input = input; - } - } - return best_input; -} - -double TreeStructuredParzenEstimator::compute_log_likelihood_ratio(const Input & input) const -{ - const auto n = static_cast(trials_.size()); - - // The above KDE and the below KDE are calculated respectively, and the ratio is the criteria to - // select best sample. - std::vector above_logs; - std::vector below_logs; - - for (int64_t i = 0; i < n; i++) { - const double log_p = log_gaussian_pdf(input, trials_[i].input, base_stddev_); - if (i < above_num_) { - const double w = 1.0 / static_cast(above_num_); - const double log_w = std::log(w); - above_logs.push_back(log_p + log_w); - } else { - const double w = 1.0 / static_cast(n - above_num_); - const double log_w = std::log(w); - below_logs.push_back(log_p + log_w); - } - } - - auto log_sum_exp = [](const std::vector & log_vec) { - const double max = *std::max_element(log_vec.begin(), log_vec.end()); - double sum = std::accumulate( - log_vec.begin(), log_vec.end(), 0.0, - [max](double total, double log_v) { return total + std::exp(log_v - max); }); - return max + std::log(sum); - }; - - const double above = log_sum_exp(above_logs); - const double below = log_sum_exp(below_logs); - - // Multiply by a constant so that the score near the "below sample" becomes lower. - // cspell:disable-line TODO(Shintaro Sakoda): It's theoretically incorrect, consider it again - // later. - const double r = above - below * 5.0; - return r; -} - -double TreeStructuredParzenEstimator::log_gaussian_pdf( - const Input & input, const Input & mu, const Input & sigma) -{ - const double log_2pi = std::log(2.0 * M_PI); - auto log_gaussian_pdf_1d = [&](const double diff, const double sigma) { - return -0.5 * log_2pi - std::log(sigma) - (diff * diff) / (2.0 * sigma * sigma); - }; - - const auto n = static_cast(input.size()); - - double result = 0.0; - for (int64_t i = 0; i < n; i++) { - double diff = input[i] - mu[i]; - if (i == ANGLE_Z) { - // Normalize the loop variable to [-pi, pi) - while (diff >= M_PI) { - diff -= 2 * M_PI; - } - while (diff < -M_PI) { - diff += 2 * M_PI; - } - } - // Experimentally, it is better to consider only trans_xy and yaw, so ignore trans_z, angle_x, - // angle_y. - if (i == TRANS_Z || i == ANGLE_X || i == ANGLE_Y) { - continue; - } - result += log_gaussian_pdf_1d(diff, sigma[i]); - } - return result; -} -} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/src/util_func.cpp b/localization/autoware_localization_util/src/util_func.cpp deleted file mode 100644 index 17187a8d732f0..0000000000000 --- a/localization/autoware_localization_util/src/util_func.cpp +++ /dev/null @@ -1,257 +0,0 @@ -// Copyright 2015-2019 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 "autoware/localization_util/util_func.hpp" - -#include "autoware/localization_util/matrix_type.hpp" - -#include -#include - -namespace autoware::localization_util -{ -// ref by http://takacity.blog.fc2.com/blog-entry-69.html -std_msgs::msg::ColorRGBA exchange_color_crc(double x) -{ - std_msgs::msg::ColorRGBA color; - - x = std::max(x, 0.0); - x = std::min(x, 0.9999); - - if (x <= 0.25) { - color.b = 1.0; - color.g = static_cast(std::sin(x * 2.0 * M_PI)); - color.r = 0; - } else if (x <= 0.5) { - color.b = static_cast(std::sin(x * 2 * M_PI)); - color.g = 1.0; - color.r = 0; - } else if (x <= 0.75) { - color.b = 0; - color.g = 1.0; - color.r = static_cast(-std::sin(x * 2.0 * M_PI)); - } else { - color.b = 0; - color.g = static_cast(-std::sin(x * 2.0 * M_PI)); - color.r = 1.0; - } - color.a = 0.999; - return color; -} - -double calc_diff_for_radian(const double lhs_rad, const double rhs_rad) -{ - double diff_rad = lhs_rad - rhs_rad; - if (diff_rad > M_PI) { - diff_rad = diff_rad - 2 * M_PI; - } else if (diff_rad < -M_PI) { - diff_rad = diff_rad + 2 * M_PI; - } - return diff_rad; -} - -Eigen::Map make_eigen_covariance(const std::array & covariance) -{ - return {covariance.data(), 6, 6}; -} - -// x: roll, y: pitch, z: yaw -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose) -{ - geometry_msgs::msg::Vector3 rpy; - tf2::Quaternion q(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); - tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z); - return rpy; -} - -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseStamped & pose) -{ - return get_rpy(pose.pose); -} - -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) -{ - return get_rpy(pose.pose.pose); -} - -geometry_msgs::msg::Quaternion rpy_rad_to_quaternion( - const double r_rad, const double p_rad, const double y_rad) -{ - tf2::Quaternion q; - q.setRPY(r_rad, p_rad, y_rad); - geometry_msgs::msg::Quaternion quaternion_msg; - quaternion_msg.x = q.x(); - quaternion_msg.y = q.y(); - quaternion_msg.z = q.z(); - quaternion_msg.w = q.w(); - return quaternion_msg; -} - -geometry_msgs::msg::Quaternion rpy_deg_to_quaternion( - const double r_deg, const double p_deg, const double y_deg) -{ - const double r_rad = r_deg * M_PI / 180.0; - const double p_rad = p_deg * M_PI / 180.0; - const double y_rad = y_deg * M_PI / 180.0; - return rpy_rad_to_quaternion(r_rad, p_rad, y_rad); -} - -geometry_msgs::msg::Twist calc_twist( - const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b) -{ - const rclcpp::Duration dt = rclcpp::Time(pose_b.header.stamp) - rclcpp::Time(pose_a.header.stamp); - const double dt_s = dt.seconds(); - - if (dt_s == 0) { - return geometry_msgs::msg::Twist(); - } - - const auto pose_a_rpy = get_rpy(pose_a); - const auto pose_b_rpy = get_rpy(pose_b); - - geometry_msgs::msg::Vector3 diff_xyz; - geometry_msgs::msg::Vector3 diff_rpy; - - diff_xyz.x = pose_b.pose.position.x - pose_a.pose.position.x; - diff_xyz.y = pose_b.pose.position.y - pose_a.pose.position.y; - diff_xyz.z = pose_b.pose.position.z - pose_a.pose.position.z; - diff_rpy.x = calc_diff_for_radian(pose_b_rpy.x, pose_a_rpy.x); - diff_rpy.y = calc_diff_for_radian(pose_b_rpy.y, pose_a_rpy.y); - diff_rpy.z = calc_diff_for_radian(pose_b_rpy.z, pose_a_rpy.z); - - geometry_msgs::msg::Twist twist; - twist.linear.x = diff_xyz.x / dt_s; - twist.linear.y = diff_xyz.y / dt_s; - twist.linear.z = diff_xyz.z / dt_s; - twist.angular.x = diff_rpy.x / dt_s; - twist.angular.y = diff_rpy.y / dt_s; - twist.angular.z = diff_rpy.z / dt_s; - - return twist; -} - -geometry_msgs::msg::PoseStamped interpolate_pose( - const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b, - const rclcpp::Time & time_stamp) -{ - const rclcpp::Time pose_a_time_stamp = pose_a.header.stamp; - const rclcpp::Time pose_b_time_stamp = pose_b.header.stamp; - if ( - (pose_a_time_stamp.seconds() == 0.0) || (pose_b_time_stamp.seconds() == 0.0) || - (time_stamp.seconds() == 0.0)) { - return geometry_msgs::msg::PoseStamped(); - } - - const auto twist = calc_twist(pose_a, pose_b); - const double dt = (time_stamp - pose_a_time_stamp).seconds(); - - const auto pose_a_rpy = get_rpy(pose_a); - - geometry_msgs::msg::Vector3 xyz; - geometry_msgs::msg::Vector3 rpy; - xyz.x = pose_a.pose.position.x + twist.linear.x * dt; - xyz.y = pose_a.pose.position.y + twist.linear.y * dt; - xyz.z = pose_a.pose.position.z + twist.linear.z * dt; - rpy.x = pose_a_rpy.x + twist.angular.x * dt; - rpy.y = pose_a_rpy.y + twist.angular.y * dt; - rpy.z = pose_a_rpy.z + twist.angular.z * dt; - - tf2::Quaternion tf_quaternion; - tf_quaternion.setRPY(rpy.x, rpy.y, rpy.z); - - geometry_msgs::msg::PoseStamped pose; - pose.header = pose_a.header; - pose.header.stamp = time_stamp; - pose.pose.position.x = xyz.x; - pose.pose.position.y = xyz.y; - pose.pose.position.z = xyz.z; - pose.pose.orientation = tf2::toMsg(tf_quaternion); - return pose; -} - -geometry_msgs::msg::PoseStamped interpolate_pose( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp) -{ - geometry_msgs::msg::PoseStamped tmp_pose_a; - tmp_pose_a.header = pose_a.header; - tmp_pose_a.pose = pose_a.pose.pose; - - geometry_msgs::msg::PoseStamped tmp_pose_b; - tmp_pose_b.header = pose_b.header; - tmp_pose_b.pose = pose_b.pose.pose; - - return interpolate_pose(tmp_pose_a, tmp_pose_b, time_stamp); -} - -Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose) -{ - Eigen::Affine3d eigen_pose; - tf2::fromMsg(ros_pose, eigen_pose); - return eigen_pose; -} - -Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose) -{ - Eigen::Affine3d eigen_pose_affine = pose_to_affine3d(ros_pose); - Eigen::Matrix4f eigen_pose_matrix = eigen_pose_affine.matrix().cast(); - return eigen_pose_matrix; -} - -Eigen::Vector3d point_to_vector3d(const geometry_msgs::msg::Point & ros_pos) -{ - Eigen::Vector3d eigen_pos; - eigen_pos.x() = ros_pos.x; - eigen_pos.y() = ros_pos.y; - eigen_pos.z() = ros_pos.z; - return eigen_pos; -} - -geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_matrix) -{ - Eigen::Affine3d eigen_pose_affine; - eigen_pose_affine.matrix() = eigen_pose_matrix.cast(); - geometry_msgs::msg::Pose ros_pose = tf2::toMsg(eigen_pose_affine); - return ros_pose; -} - -double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) -{ - const double dx = p1.x - p2.x; - const double dy = p1.y - p2.y; - const double dz = p1.z - p2.z; - return std::sqrt(dx * dx + dy * dy + dz * dz); -} - -void output_pose_with_cov_to_log( - const rclcpp::Logger & logger, const std::string & prefix, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov) -{ - const Eigen::Map covariance = - make_eigen_covariance(pose_with_cov.pose.covariance); - const geometry_msgs::msg::Pose pose = pose_with_cov.pose.pose; - geometry_msgs::msg::Vector3 rpy = get_rpy(pose); - rpy.x = rpy.x * 180.0 / M_PI; - rpy.y = rpy.y * 180.0 / M_PI; - rpy.z = rpy.z * 180.0 / M_PI; - - RCLCPP_DEBUG_STREAM( - logger, std::fixed << prefix << "," << pose.position.x << "," << pose.position.y << "," - << pose.position.z << "," << pose.orientation.x << "," << pose.orientation.y - << "," << pose.orientation.z << "," << pose.orientation.w << "," << rpy.x - << "," << rpy.y << "," << rpy.z << "," << covariance(0, 0) << "," - << covariance(1, 1) << "," << covariance(2, 2) << "," << covariance(3, 3) - << "," << covariance(4, 4) << "," << covariance(5, 5)); -} -} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp b/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp deleted file mode 100644 index d55555682be84..0000000000000 --- a/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp +++ /dev/null @@ -1,109 +0,0 @@ -// Copyright 2023- 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 "autoware/localization_util/smart_pose_buffer.hpp" -#include "autoware/localization_util/util_func.hpp" - -#include -#include - -#include -#include - -#include -#include -#include - -using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; -using SmartPoseBuffer = autoware::localization_util::SmartPoseBuffer; - -bool compare_pose( - const PoseWithCovarianceStamped & pose_a, const PoseWithCovarianceStamped & pose_b) -{ - return pose_a.header.stamp == pose_b.header.stamp && - pose_a.header.frame_id == pose_b.header.frame_id && - pose_a.pose.covariance == pose_b.pose.covariance && - pose_a.pose.pose.position.x == pose_b.pose.pose.position.x && - pose_a.pose.pose.position.y == pose_b.pose.pose.position.y && - pose_a.pose.pose.position.z == pose_b.pose.pose.position.z && - pose_a.pose.pose.orientation.x == pose_b.pose.pose.orientation.x && - pose_a.pose.pose.orientation.y == pose_b.pose.pose.orientation.y && - pose_a.pose.pose.orientation.z == pose_b.pose.pose.orientation.z && - pose_a.pose.pose.orientation.w == pose_b.pose.pose.orientation.w; -} - -TEST(TestSmartPoseBuffer, interpolate_pose) // NOLINT -{ - rclcpp::Logger logger = rclcpp::get_logger("test_logger"); - const double pose_timeout_sec = 10.0; - const double pose_distance_tolerance_meters = 100.0; - SmartPoseBuffer smart_pose_buffer(logger, pose_timeout_sec, pose_distance_tolerance_meters); - - // first data - PoseWithCovarianceStamped::SharedPtr old_pose_ptr = std::make_shared(); - old_pose_ptr->header.stamp.sec = 10; - old_pose_ptr->header.stamp.nanosec = 0; - old_pose_ptr->pose.pose.position.x = 10.0; - old_pose_ptr->pose.pose.position.y = 20.0; - old_pose_ptr->pose.pose.position.z = 0.0; - old_pose_ptr->pose.pose.orientation = - autoware::localization_util::rpy_deg_to_quaternion(0.0, 0.0, 0.0); - smart_pose_buffer.push_back(old_pose_ptr); - - // second data - PoseWithCovarianceStamped::SharedPtr new_pose_ptr = std::make_shared(); - new_pose_ptr->header.stamp.sec = 20; - new_pose_ptr->header.stamp.nanosec = 0; - new_pose_ptr->pose.pose.position.x = 20.0; - new_pose_ptr->pose.pose.position.y = 40.0; - new_pose_ptr->pose.pose.position.z = 0.0; - new_pose_ptr->pose.pose.orientation = - autoware::localization_util::rpy_deg_to_quaternion(0.0, 0.0, 90.0); - smart_pose_buffer.push_back(new_pose_ptr); - - // interpolate - builtin_interfaces::msg::Time target_ros_time_msg; - target_ros_time_msg.sec = 15; - target_ros_time_msg.nanosec = 0; - const std::optional & interpolate_result = - smart_pose_buffer.interpolate(target_ros_time_msg); - ASSERT_TRUE(interpolate_result.has_value()); - const SmartPoseBuffer::InterpolateResult result = interpolate_result.value(); - - // check old - EXPECT_TRUE(compare_pose(result.old_pose, *old_pose_ptr)); - - // check new - EXPECT_TRUE(compare_pose(result.new_pose, *new_pose_ptr)); - - // check interpolated - EXPECT_EQ(result.interpolated_pose.header.stamp.sec, 15); - EXPECT_EQ(result.interpolated_pose.header.stamp.nanosec, static_cast(0)); - EXPECT_EQ(result.interpolated_pose.pose.pose.position.x, 15.0); - EXPECT_EQ(result.interpolated_pose.pose.pose.position.y, 30.0); - EXPECT_EQ(result.interpolated_pose.pose.pose.position.z, 0.0); - const auto rpy = autoware::localization_util::get_rpy(result.interpolated_pose.pose.pose); - EXPECT_NEAR(rpy.x * 180 / M_PI, 0.0, 1e-6); - EXPECT_NEAR(rpy.y * 180 / M_PI, 0.0, 1e-6); - EXPECT_NEAR(rpy.z * 180 / M_PI, 45.0, 1e-6); -} - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - ::testing::InitGoogleTest(&argc, argv); - int result = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return result; -} diff --git a/localization/autoware_localization_util/test/test_tpe.cpp b/localization/autoware_localization_util/test/test_tpe.cpp deleted file mode 100644 index 2d71a385246b7..0000000000000 --- a/localization/autoware_localization_util/test/test_tpe.cpp +++ /dev/null @@ -1,75 +0,0 @@ -// Copyright 2023 The Autoware Contributors -// -// 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 "autoware/localization_util/tree_structured_parzen_estimator.hpp" - -#include - -#include -#include -#include -#include -#include - -using TreeStructuredParzenEstimator = autoware::localization_util::TreeStructuredParzenEstimator; - -TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphere_function) -{ - auto sphere_function = [](const TreeStructuredParzenEstimator::Input & input) { - double value = 0.0; - const auto n = static_cast(input.size()); - for (int64_t i = 0; i < n; i++) { - const double v = input[i] * 10; - value += v * v; - } - return value; - }; - - constexpr int64_t k_outer_trials_num = 20; - constexpr int64_t k_inner_trials_num = 200; - std::cout << std::fixed; - std::vector mean_scores; - std::vector sample_mean(5, 0.0); - std::vector sample_stddev{1.0, 1.0, 0.1, 0.1, 0.1}; - - for (const int64_t n_startup_trials : {k_inner_trials_num, k_inner_trials_num / 2}) { - const std::string method = ((n_startup_trials == k_inner_trials_num) ? "Random" : "TPE"); - - std::vector scores; - for (int64_t i = 0; i < k_outer_trials_num; i++) { - double best_score = std::numeric_limits::lowest(); - TreeStructuredParzenEstimator estimator( - TreeStructuredParzenEstimator::Direction::MAXIMIZE, n_startup_trials, sample_mean, - sample_stddev); - for (int64_t trial = 0; trial < k_inner_trials_num; trial++) { - const TreeStructuredParzenEstimator::Input input = estimator.get_next_input(); - const double score = -sphere_function(input); - estimator.add_trial({input, score}); - best_score = std::max(best_score, score); - } - scores.push_back(best_score); - } - - const double sum = std::accumulate(scores.begin(), scores.end(), 0.0); - const double mean = sum / static_cast(scores.size()); - mean_scores.push_back(mean); - double sq_sum = std::accumulate( - scores.begin(), scores.end(), 0.0, - [mean](double total, double score) { return total + (score - mean) * (score - mean); }); - const double stddev = std::sqrt(sq_sum / static_cast(scores.size())); - - std::cout << method << ", mean = " << mean << ", stddev = " << stddev << std::endl; - } - ASSERT_LT(mean_scores[0], mean_scores[1]); -}