From d8c0e83d6b2254f2a22a4b91d8de3bb6ec1af134 Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Thu, 31 Oct 2024 01:24:31 +0000 Subject: [PATCH 01/96] chore: update CODEOWNERS (#9203) Signed-off-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: github-actions --- .github/CODEOWNERS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 6397141844469..929cad046c7a1 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -43,7 +43,7 @@ common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.j common/traffic_light_utils/** kotaro.uetake@tier4.jp satoshi.ota@tier4.jp shunsuke.miura@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp control/autoware_autonomous_emergency_braking/** daniel.sanchez@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp -control/autoware_collision_detector/** go.sakayori@tier4.jp kyoichi.sugahara@tier4.jp +control/autoware_collision_detector/** go.sakayori@tier4.jp kyoichi.sugahara@tier4.jp tomohito.ando@tier4.jp control/autoware_control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/autoware_external_cmd_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/autoware_joy_controller/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp From dcdb139d2ec6c7d9f3c5e0f75296f7727befe78c Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Thu, 31 Oct 2024 02:27:07 +0100 Subject: [PATCH 02/96] refactor(time_utils): prefix package and namespace with autoware (#9173) * refactor(time_utils): prefix package and namespace with autoware Signed-off-by: Esteve Fernandez * refactor(time_utils): prefix package and namespace with autoware Signed-off-by: Esteve Fernandez * style(pre-commit): autofix --------- Signed-off-by: Esteve Fernandez Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .github/CODEOWNERS | 2 +- .../CMakeLists.txt | 2 +- .../include/autoware}/time_utils/stopwatch.hpp | 12 ++++++------ .../include/autoware}/time_utils/time_utils.hpp | 12 ++++++------ .../autoware}/time_utils/visibility_control.hpp | 6 +++--- .../{time_utils => autoware_time_utils}/package.xml | 2 +- .../src/time_utils/time_utils.cpp | 6 +++--- 7 files changed, 21 insertions(+), 21 deletions(-) rename common/{time_utils => autoware_time_utils}/CMakeLists.txt (87%) rename common/{time_utils/include => autoware_time_utils/include/autoware}/time_utils/stopwatch.hpp (89%) rename common/{time_utils/include => autoware_time_utils/include/autoware}/time_utils/time_utils.hpp (77%) rename common/{time_utils/include => autoware_time_utils/include/autoware}/time_utils/visibility_control.hpp (89%) rename common/{time_utils => autoware_time_utils}/package.xml (96%) rename common/{time_utils => autoware_time_utils}/src/time_utils/time_utils.cpp (91%) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 929cad046c7a1..1359ca6569056 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -20,6 +20,7 @@ common/autoware_polar_grid/** yukihiro.saito@tier4.jp common/autoware_signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp common/autoware_test_utils/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp zulfaqar.azmi@tier4.jp common/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +common/autoware_time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_universe_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp common/autoware_vehicle_info_utils/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp common/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp @@ -38,7 +39,6 @@ common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp khal common/tier4_system_rviz_plugin/** koji.minoda@tier4.jp common/tier4_traffic_light_rviz_plugin/** satoshi.ota@tier4.jp common/tier4_vehicle_rviz_plugin/** yukihiro.saito@tier4.jp -common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp common/traffic_light_utils/** kotaro.uetake@tier4.jp satoshi.ota@tier4.jp shunsuke.miura@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp diff --git a/common/time_utils/CMakeLists.txt b/common/autoware_time_utils/CMakeLists.txt similarity index 87% rename from common/time_utils/CMakeLists.txt rename to common/autoware_time_utils/CMakeLists.txt index 2fc03843a643f..97c6dce02c4a2 100644 --- a/common/time_utils/CMakeLists.txt +++ b/common/autoware_time_utils/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(time_utils) +project(autoware_time_utils) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/common/time_utils/include/time_utils/stopwatch.hpp b/common/autoware_time_utils/include/autoware/time_utils/stopwatch.hpp similarity index 89% rename from common/time_utils/include/time_utils/stopwatch.hpp rename to common/autoware_time_utils/include/autoware/time_utils/stopwatch.hpp index 223e7ab700ac4..b2e5942434411 100644 --- a/common/time_utils/include/time_utils/stopwatch.hpp +++ b/common/autoware_time_utils/include/autoware/time_utils/stopwatch.hpp @@ -14,10 +14,10 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef TIME_UTILS__STOPWATCH_HPP_ -#define TIME_UTILS__STOPWATCH_HPP_ +#ifndef AUTOWARE__TIME_UTILS__STOPWATCH_HPP_ +#define AUTOWARE__TIME_UTILS__STOPWATCH_HPP_ -#include +#include #include #include @@ -26,7 +26,7 @@ namespace autoware { namespace common { -namespace time_utils +namespace autoware::time_utils { namespace detail @@ -81,8 +81,8 @@ class TIME_UTILS_PUBLIC Stopwatch TimePoint m_start{Clock::now()}; }; -} // namespace time_utils +} // namespace autoware::time_utils } // namespace common } // namespace autoware -#endif // TIME_UTILS__STOPWATCH_HPP_ +#endif // AUTOWARE__TIME_UTILS__STOPWATCH_HPP_ diff --git a/common/time_utils/include/time_utils/time_utils.hpp b/common/autoware_time_utils/include/autoware/time_utils/time_utils.hpp similarity index 77% rename from common/time_utils/include/time_utils/time_utils.hpp rename to common/autoware_time_utils/include/autoware/time_utils/time_utils.hpp index cb369b4874872..450380680e202 100644 --- a/common/time_utils/include/time_utils/time_utils.hpp +++ b/common/autoware_time_utils/include/autoware/time_utils/time_utils.hpp @@ -13,20 +13,20 @@ // limitations under the License. // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef TIME_UTILS__TIME_UTILS_HPP_ -#define TIME_UTILS__TIME_UTILS_HPP_ +#ifndef AUTOWARE__TIME_UTILS__TIME_UTILS_HPP_ +#define AUTOWARE__TIME_UTILS__TIME_UTILS_HPP_ +#include #include #include -#include #include -namespace time_utils +namespace autoware::time_utils { /// Standard interpolation TIME_UTILS_PUBLIC std::chrono::nanoseconds interpolate( std::chrono::nanoseconds a, std::chrono::nanoseconds b, float t) noexcept; -} // namespace time_utils +} // namespace autoware::time_utils -#endif // TIME_UTILS__TIME_UTILS_HPP_ +#endif // AUTOWARE__TIME_UTILS__TIME_UTILS_HPP_ diff --git a/common/time_utils/include/time_utils/visibility_control.hpp b/common/autoware_time_utils/include/autoware/time_utils/visibility_control.hpp similarity index 89% rename from common/time_utils/include/time_utils/visibility_control.hpp rename to common/autoware_time_utils/include/autoware/time_utils/visibility_control.hpp index ca9423d8df9ab..ebf444422ba91 100644 --- a/common/time_utils/include/time_utils/visibility_control.hpp +++ b/common/autoware_time_utils/include/autoware/time_utils/visibility_control.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIME_UTILS__VISIBILITY_CONTROL_HPP_ -#define TIME_UTILS__VISIBILITY_CONTROL_HPP_ +#ifndef AUTOWARE__TIME_UTILS__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE__TIME_UTILS__VISIBILITY_CONTROL_HPP_ #if defined(__WIN32) #if defined(TIME_UTILS_BUILDING_DLL) || defined(TIME_UTILS_EXPORTS) @@ -33,4 +33,4 @@ #error "Unsupported Build Configuration" #endif // defined(_WINDOWS) -#endif // TIME_UTILS__VISIBILITY_CONTROL_HPP_ +#endif // AUTOWARE__TIME_UTILS__VISIBILITY_CONTROL_HPP_ diff --git a/common/time_utils/package.xml b/common/autoware_time_utils/package.xml similarity index 96% rename from common/time_utils/package.xml rename to common/autoware_time_utils/package.xml index c4ad8f098a0e0..15a8699d6d65b 100644 --- a/common/time_utils/package.xml +++ b/common/autoware_time_utils/package.xml @@ -1,7 +1,7 @@ - time_utils + autoware_time_utils 1.0.0 Simple conversion methods to/from std::chrono to simplify algorithm development Christopher Ho diff --git a/common/time_utils/src/time_utils/time_utils.cpp b/common/autoware_time_utils/src/time_utils/time_utils.cpp similarity index 91% rename from common/time_utils/src/time_utils/time_utils.cpp rename to common/autoware_time_utils/src/time_utils/time_utils.cpp index fa1516c4bfef0..9801dbb762ab7 100644 --- a/common/time_utils/src/time_utils/time_utils.cpp +++ b/common/autoware_time_utils/src/time_utils/time_utils.cpp @@ -11,13 +11,13 @@ // 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 "time_utils/time_utils.hpp" +#include "autoware/time_utils/time_utils.hpp" #include #include #include -namespace time_utils +namespace autoware::time_utils { //////////////////////////////////////////////////////////////////////////////// std::chrono::nanoseconds interpolate( @@ -30,4 +30,4 @@ std::chrono::nanoseconds interpolate( return a + del_; } -} // namespace time_utils +} // namespace autoware::time_utils From d3c095eaf0c3e67bb54939515664e8028207a133 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Thu, 31 Oct 2024 10:54:59 +0900 Subject: [PATCH 03/96] feat(rtc_interface): add requested field (#9202) * add requested feature Signed-off-by: Go Sakayori * Update planning/autoware_rtc_interface/test/test_rtc_interface.cpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --------- Signed-off-by: Go Sakayori Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- .../autoware_rtc_interface/CMakeLists.txt | 11 +- .../autoware/rtc_interface/rtc_interface.hpp | 5 +- planning/autoware_rtc_interface/package.xml | 3 + .../src/rtc_interface.cpp | 38 ++-- .../test/test_rtc_interface.cpp | 174 ++++++++++++++++++ 5 files changed, 207 insertions(+), 24 deletions(-) create mode 100644 planning/autoware_rtc_interface/test/test_rtc_interface.cpp diff --git a/planning/autoware_rtc_interface/CMakeLists.txt b/planning/autoware_rtc_interface/CMakeLists.txt index cd36121e6aee8..9cb83577348d5 100644 --- a/planning/autoware_rtc_interface/CMakeLists.txt +++ b/planning/autoware_rtc_interface/CMakeLists.txt @@ -1,6 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(autoware_rtc_interface) +find_package(autoware_cmake REQUIRED) +autoware_package() + ### Compile options if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) @@ -18,8 +21,12 @@ ament_auto_add_library(autoware_rtc_interface SHARED # Test if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_rtc_interface.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) endif() ament_auto_package() diff --git a/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp b/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp index 705395c2b1741..4d7e2aec46e33 100644 --- a/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp +++ b/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp @@ -54,7 +54,7 @@ class RTCInterface void publishCooperateStatus(const rclcpp::Time & stamp); void updateCooperateStatus( const UUID & uuid, const bool safe, const uint8_t state, const double start_distance, - const double finish_distance, const rclcpp::Time & stamp); + const double finish_distance, const rclcpp::Time & stamp, const bool requested = false); void removeCooperateStatus(const UUID & uuid); void removeExpiredCooperateStatus(); void clearCooperateStatus(); @@ -103,6 +103,9 @@ class RTCInterface std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode"; mutable std::mutex mutex_; + +public: + friend class RTCInterfaceTest; }; } // namespace autoware::rtc_interface diff --git a/planning/autoware_rtc_interface/package.xml b/planning/autoware_rtc_interface/package.xml index d08467d8f15a9..8ac9d32ba9b04 100644 --- a/planning/autoware_rtc_interface/package.xml +++ b/planning/autoware_rtc_interface/package.xml @@ -15,12 +15,15 @@ ament_cmake_auto + autoware_universe_utils rclcpp tier4_rtc_msgs unique_identifier_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common + autoware_test_utils ament_cmake diff --git a/planning/autoware_rtc_interface/src/rtc_interface.cpp b/planning/autoware_rtc_interface/src/rtc_interface.cpp index 25dd34332f684..71a197ccb93b4 100644 --- a/planning/autoware_rtc_interface/src/rtc_interface.cpp +++ b/planning/autoware_rtc_interface/src/rtc_interface.cpp @@ -14,8 +14,6 @@ #include "autoware/rtc_interface/rtc_interface.hpp" -#include - namespace { using tier4_rtc_msgs::msg::Module; @@ -33,7 +31,8 @@ std::string command_to_string(const uint8_t type) { if (type == tier4_rtc_msgs::msg::Command::ACTIVATE) { return "ACTIVATE"; - } else if (type == tier4_rtc_msgs::msg::Command::DEACTIVATE) { + } + if (type == tier4_rtc_msgs::msg::Command::DEACTIVATE) { return "DEACTIVATE"; } @@ -44,13 +43,17 @@ std::string state_to_string(const uint8_t type) { if (type == tier4_rtc_msgs::msg::State::WAITING_FOR_EXECUTION) { return "WAITING_FOR_EXECUTION"; - } else if (type == tier4_rtc_msgs::msg::State::RUNNING) { + } + if (type == tier4_rtc_msgs::msg::State::RUNNING) { return "RUNNING"; - } else if (type == tier4_rtc_msgs::msg::State::ABORTING) { + } + if (type == tier4_rtc_msgs::msg::State::ABORTING) { return "ABORTING"; - } else if (type == tier4_rtc_msgs::msg::State::SUCCEEDED) { + } + if (type == tier4_rtc_msgs::msg::State::SUCCEEDED) { return "SUCCEEDED"; - } else if (type == tier4_rtc_msgs::msg::State::FAILED) { + } + if (type == tier4_rtc_msgs::msg::State::FAILED) { return "FAILED"; } @@ -242,7 +245,7 @@ void RTCInterface::onTimer() void RTCInterface::updateCooperateStatus( const UUID & uuid, const bool safe, const uint8_t state, const double start_distance, - const double finish_distance, const rclcpp::Time & stamp) + const double finish_distance, const rclcpp::Time & stamp, const bool requested) { std::lock_guard lock(mutex_); // Find registered status which has same uuid @@ -257,6 +260,7 @@ void RTCInterface::updateCooperateStatus( status.uuid = uuid; status.module = module_; status.safe = safe; + status.requested = requested; status.command_status.type = Command::DEACTIVATE; status.state.type = State::WAITING_FOR_EXECUTION; status.start_distance = start_distance; @@ -370,11 +374,10 @@ bool RTCInterface::isActivated(const UUID & uuid) const if (itr->state.type == State::FAILED || itr->state.type == State::SUCCEEDED) { return false; } - if (itr->auto_mode) { + if (itr->auto_mode && !itr->requested) { return itr->safe; - } else { - return itr->command_status.type == Command::ACTIVATE; } + return itr->command_status.type == Command::ACTIVATE; } RCLCPP_WARN_STREAM( @@ -393,11 +396,8 @@ bool RTCInterface::isForceActivated(const UUID & uuid) const if (itr->state.type != State::WAITING_FOR_EXECUTION && itr->state.type != State::RUNNING) { return false; } - if (itr->command_status.type == Command::ACTIVATE && !itr->safe) { - return true; - } else { - return false; - } + if (itr->command_status.type != Command::ACTIVATE) return false; + return !itr->safe || itr->requested; } RCLCPP_WARN_STREAM( @@ -417,11 +417,7 @@ bool RTCInterface::isForceDeactivated(const UUID & uuid) const if (itr->state.type != State::RUNNING) { return false; } - if (itr->command_status.type == Command::DEACTIVATE && itr->safe && !itr->auto_mode) { - return true; - } else { - return false; - } + return itr->command_status.type == Command::DEACTIVATE && itr->safe && !itr->auto_mode; } RCLCPP_WARN_STREAM( diff --git a/planning/autoware_rtc_interface/test/test_rtc_interface.cpp b/planning/autoware_rtc_interface/test/test_rtc_interface.cpp new file mode 100644 index 0000000000000..84c6dbd343ea2 --- /dev/null +++ b/planning/autoware_rtc_interface/test/test_rtc_interface.cpp @@ -0,0 +1,174 @@ +// Copyright 2024 TIER IV, Inc. All rights reserved. +// +// 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/rtc_interface/rtc_interface.hpp" + +#include +#include +#include + +#include "tier4_rtc_msgs/msg/detail/command__struct.hpp" + +#include + +using tier4_rtc_msgs::msg::State; + +namespace autoware::rtc_interface +{ +class RTCInterfaceTest : public ::testing::Test +{ +public: + void update_cooperate_command_status(const std::vector & commands) + { + rtc_interface_->updateCooperateCommandStatus(commands); + } + + std::vector get_stored_command() { return rtc_interface_->stored_commands_; } + +protected: + rclcpp::Node::SharedPtr node; + std::shared_ptr rtc_interface_; + + void SetUp() override + { + rclcpp::init(0, nullptr); + node = std::make_shared("rtc_interface_node_test"); + rtc_interface_ = + std::make_shared(node.get(), "TestRTC", false); + } + + void TearDown() override { rclcpp::shutdown(); } +}; + +TEST_F(RTCInterfaceTest, uuid_to_string) +{ + auto uuid = autoware::universe_utils::generateUUID(); + rclcpp::Time stamp(1.0, 0); + + // Condition: no registered uuid + EXPECT_FALSE(rtc_interface_->isRegistered(uuid)); + EXPECT_FALSE(rtc_interface_->isActivated(uuid)); + EXPECT_FALSE(rtc_interface_->isForceActivated(uuid)); + EXPECT_FALSE(rtc_interface_->isForceDeactivated(uuid)); + EXPECT_TRUE(rtc_interface_->isRTCEnabled(uuid)); + EXPECT_TRUE(rtc_interface_->isTerminated(uuid)); + + // Condition: register uuid with WAITING_FOR_EXECUTION + rtc_interface_->updateCooperateStatus( + uuid, true, State::WAITING_FOR_EXECUTION, 10.0, 100.0, stamp); + EXPECT_TRUE(rtc_interface_->isRegistered(uuid)); + EXPECT_TRUE(rtc_interface_->isActivated(uuid)); + EXPECT_FALSE(rtc_interface_->isForceActivated(uuid)); + EXPECT_FALSE(rtc_interface_->isForceDeactivated(uuid)); + EXPECT_FALSE(rtc_interface_->isRTCEnabled(uuid)); + EXPECT_FALSE(rtc_interface_->isTerminated(uuid)); + + // Condition: remove registered uuid + rtc_interface_->removeCooperateStatus(uuid); + EXPECT_FALSE(rtc_interface_->isRegistered(uuid)); + + // Condition: register uuid with FAILED (should start with WAITING_FOR_EXECUTION) + rtc_interface_->updateCooperateStatus(uuid, true, State::FAILED, 10.0, 100.0, stamp); + EXPECT_TRUE(rtc_interface_->isRegistered(uuid)); + EXPECT_TRUE(rtc_interface_->isActivated(uuid)); + EXPECT_FALSE(rtc_interface_->isForceActivated(uuid)); + EXPECT_FALSE(rtc_interface_->isForceDeactivated(uuid)); + EXPECT_FALSE(rtc_interface_->isRTCEnabled(uuid)); + EXPECT_FALSE(rtc_interface_->isTerminated(uuid)); + + // Condition: update uuid as force activated + std::vector commands; + CooperateCommand command; + command.uuid = uuid; + command.command.type = tier4_rtc_msgs::msg::Command::ACTIVATE; + commands.push_back(command); + rtc_interface_->updateCooperateStatus(uuid, false, State::RUNNING, 10.0, 100.0, stamp); + update_cooperate_command_status(commands); + EXPECT_TRUE(rtc_interface_->isRegistered(uuid)); + EXPECT_TRUE(rtc_interface_->isActivated(uuid)); + EXPECT_TRUE(rtc_interface_->isForceActivated(uuid)); + EXPECT_FALSE(rtc_interface_->isForceDeactivated(uuid)); + EXPECT_TRUE(rtc_interface_->isRTCEnabled(uuid)); + EXPECT_FALSE(rtc_interface_->isTerminated(uuid)); + + // Condition: terminate uuid by transiting to SUCCEEDED + rtc_interface_->updateCooperateStatus(uuid, false, State::SUCCEEDED, 10.0, 100.0, stamp); + EXPECT_TRUE(rtc_interface_->isRegistered(uuid)); + EXPECT_FALSE(rtc_interface_->isActivated(uuid)); + EXPECT_FALSE(rtc_interface_->isForceActivated(uuid)); + EXPECT_FALSE(rtc_interface_->isForceDeactivated(uuid)); + EXPECT_TRUE(rtc_interface_->isRTCEnabled(uuid)); + EXPECT_TRUE(rtc_interface_->isTerminated(uuid)); + + // Condition: clear registered uuid + rtc_interface_->clearCooperateStatus(); + EXPECT_FALSE(rtc_interface_->isRegistered(uuid)); + EXPECT_TRUE(get_stored_command().empty()); + + // Condition: update uuid as force deactivated + commands.front().command.type = tier4_rtc_msgs::msg::Command::DEACTIVATE; + rtc_interface_->updateCooperateStatus( + uuid, true, State::WAITING_FOR_EXECUTION, 10.0, 100.0, stamp); + rtc_interface_->updateCooperateStatus(uuid, true, State::RUNNING, 10.0, 100.0, stamp); + update_cooperate_command_status(commands); + EXPECT_TRUE(rtc_interface_->isRegistered(uuid)); + EXPECT_FALSE(rtc_interface_->isActivated(uuid)); + EXPECT_FALSE(rtc_interface_->isForceActivated(uuid)); + EXPECT_TRUE(rtc_interface_->isForceDeactivated(uuid)); + EXPECT_TRUE(rtc_interface_->isRTCEnabled(uuid)); + EXPECT_FALSE(rtc_interface_->isTerminated(uuid)); + + // Condition: uuid transiting to ABORTING + rtc_interface_->updateCooperateStatus(uuid, true, State::ABORTING, 10.0, 100.0, stamp); + EXPECT_TRUE(rtc_interface_->isRegistered(uuid)); + EXPECT_FALSE(rtc_interface_->isActivated(uuid)); + EXPECT_FALSE(rtc_interface_->isForceActivated(uuid)); + EXPECT_FALSE(rtc_interface_->isForceDeactivated(uuid)); + EXPECT_TRUE(rtc_interface_->isRTCEnabled(uuid)); + EXPECT_FALSE(rtc_interface_->isTerminated(uuid)); + + // Condition: uuid transiting to FAILED + rtc_interface_->updateCooperateStatus(uuid, true, State::FAILED, 10.0, 100.0, stamp); + EXPECT_TRUE(rtc_interface_->isRegistered(uuid)); + EXPECT_FALSE(rtc_interface_->isActivated(uuid)); + EXPECT_FALSE(rtc_interface_->isForceActivated(uuid)); + EXPECT_FALSE(rtc_interface_->isForceDeactivated(uuid)); + EXPECT_TRUE(rtc_interface_->isRTCEnabled(uuid)); + EXPECT_TRUE(rtc_interface_->isTerminated(uuid)); + + rtc_interface_->clearCooperateStatus(); + + // Condition: register uuid with request to operator and transit to RUNNING (which shouldn't be + // allowed) + rtc_interface_->updateCooperateStatus( + uuid, true, State::WAITING_FOR_EXECUTION, 10.0, 100.0, stamp, true); + rtc_interface_->updateCooperateStatus(uuid, true, State::RUNNING, 10.0, 100.0, stamp); + EXPECT_TRUE(rtc_interface_->isRegistered(uuid)); + EXPECT_FALSE(rtc_interface_->isActivated(uuid)); + EXPECT_FALSE(rtc_interface_->isForceActivated(uuid)); + EXPECT_FALSE(rtc_interface_->isForceDeactivated(uuid)); + EXPECT_FALSE(rtc_interface_->isRTCEnabled(uuid)); + EXPECT_FALSE(rtc_interface_->isTerminated(uuid)); + + // Condition: update uuid cooperate command + commands.front().command.type = tier4_rtc_msgs::msg::Command::ACTIVATE; + update_cooperate_command_status(commands); + EXPECT_TRUE(rtc_interface_->isRegistered(uuid)); + EXPECT_TRUE(rtc_interface_->isActivated(uuid)); + EXPECT_TRUE(rtc_interface_->isForceActivated(uuid)); + EXPECT_FALSE(rtc_interface_->isForceDeactivated(uuid)); + EXPECT_TRUE(rtc_interface_->isRTCEnabled(uuid)); + EXPECT_FALSE(rtc_interface_->isTerminated(uuid)); +} +} // namespace autoware::rtc_interface From a4513b4b793655b745891550ace02740b50aea56 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Thu, 31 Oct 2024 11:03:22 +0900 Subject: [PATCH 04/96] fix(mpc_lateral_controller): correctly resample the MPC trajectory yaws (#9199) Signed-off-by: Maxime CLEMENT --- control/autoware_mpc_lateral_controller/src/mpc_utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp index c257bada0b0b2..820ee848c17fe 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp @@ -143,7 +143,7 @@ std::pair resampleMPCTrajectoryByDistance( output.x = spline_arc_length(input.x); output.y = spline_arc_length(input.y); output.z = spline_arc_length(input.z); - output.yaw = spline_arc_length(input.yaw); + output.yaw = spline_arc_length(input_yaw); output.vx = lerp_arc_length(input.vx); // must be linear output.k = spline_arc_length(input.k); output.smooth_k = spline_arc_length(input.smooth_k); From a339d0cf671f894c74b5f43923cc360d814f4389 Mon Sep 17 00:00:00 2001 From: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Date: Thu, 31 Oct 2024 12:06:32 +0900 Subject: [PATCH 05/96] fix(bpp): prevent accessing nullopt (#9204) fix(bpp): calcDistanceToRedTrafficLight null Signed-off-by: Shumpei Wakabayashi --- .../src/utils/traffic_light_utils.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp index 739f5d21346bd..42f6cd02ad361 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp @@ -98,6 +98,7 @@ std::optional calcDistanceToRedTrafficLight( const auto & ego_pos = planner_data->self_odometry->pose.pose.position; lanelet::ConstLineString3d stop_line = *(element->stopLine()); + if (!stop_line.empty()) return std::nullopt; const auto x = 0.5 * (stop_line.front().x() + stop_line.back().x()); const auto y = 0.5 * (stop_line.front().y() + stop_line.back().y()); const auto z = 0.5 * (stop_line.front().z() + stop_line.back().z()); From a9763cbbf929ce380d544563be9c7fd1197f65cc Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 31 Oct 2024 17:06:56 +0900 Subject: [PATCH 06/96] refactor(autoware_map_based_prediction): split pedestrian and bicycle predictor (#9201) * refactor: grouping functions Signed-off-by: Taekjin LEE * refactor: grouping parameters Signed-off-by: Taekjin LEE * refactor: rename member road_users_history to road_users_history_ Signed-off-by: Taekjin LEE * refactor: separate util functions Signed-off-by: Taekjin LEE * refactor: Add predictor_vru.cpp and utils.cpp to map_based_prediction_node Signed-off-by: Taekjin LEE * refactor: Add explicit template instantiation for removeOldObjectsHistory function Signed-off-by: Taekjin LEE * refactor: Add tf2_geometry_msgs to data_structure Signed-off-by: Taekjin LEE * refactor: Remove unused variables and functions in map_based_prediction_node.cpp Signed-off-by: Taekjin LEE * Update perception/autoware_map_based_prediction/include/map_based_prediction/predictor_vru.hpp * Apply suggestions from code review * style(pre-commit): autofix --------- Signed-off-by: Taekjin LEE Co-authored-by: Mamoru Sobue Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../CMakeLists.txt | 11 +- .../map_based_prediction/data_structure.hpp | 95 ++ .../map_based_prediction_node.hpp | 220 ++-- .../map_based_prediction/path_generator.hpp | 4 +- .../map_based_prediction/predictor_vru.hpp | 140 +++ .../include/map_based_prediction/utils.hpp | 101 ++ .../src/debug.cpp | 1 + .../src/map_based_prediction_node.cpp | 1011 ++--------------- .../src/path_generator.cpp | 6 + .../src/predictor_vru.cpp | 706 ++++++++++++ .../src/utils.cpp | 222 ++++ .../test_path_generator.cpp | 1 + 12 files changed, 1442 insertions(+), 1076 deletions(-) create mode 100644 perception/autoware_map_based_prediction/include/map_based_prediction/data_structure.hpp create mode 100644 perception/autoware_map_based_prediction/include/map_based_prediction/predictor_vru.hpp create mode 100644 perception/autoware_map_based_prediction/include/map_based_prediction/utils.hpp create mode 100644 perception/autoware_map_based_prediction/src/predictor_vru.cpp create mode 100644 perception/autoware_map_based_prediction/src/utils.cpp diff --git a/perception/autoware_map_based_prediction/CMakeLists.txt b/perception/autoware_map_based_prediction/CMakeLists.txt index 9a1d9a1947c7c..71b47472f3d39 100644 --- a/perception/autoware_map_based_prediction/CMakeLists.txt +++ b/perception/autoware_map_based_prediction/CMakeLists.txt @@ -5,8 +5,9 @@ find_package(autoware_cmake REQUIRED) autoware_package() find_package(Eigen3 REQUIRED) - find_package(glog REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) include_directories( SYSTEM @@ -16,10 +17,16 @@ include_directories( ament_auto_add_library(map_based_prediction_node SHARED src/map_based_prediction_node.cpp src/path_generator.cpp + src/predictor_vru.cpp src/debug.cpp + src/utils.cpp ) -target_link_libraries(map_based_prediction_node glog::glog) +target_link_libraries(map_based_prediction_node + glog::glog + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs +) rclcpp_components_register_node(map_based_prediction_node PLUGIN "autoware::map_based_prediction::MapBasedPredictionNode" diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/data_structure.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/data_structure.hpp new file mode 100644 index 0000000000000..ae4e08774f679 --- /dev/null +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/data_structure.hpp @@ -0,0 +1,95 @@ +// Copyright 2024 TIER IV, inc. +// +// 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 MAP_BASED_PREDICTION__DATA_STRUCTURE_HPP_ +#define MAP_BASED_PREDICTION__DATA_STRUCTURE_HPP_ + +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace autoware::map_based_prediction +{ +using PosePath = std::vector; + +struct PredictionTimeHorizon +{ + // NOTE(Mamoru Sobue): motorcycle belongs to "vehicle" and bicycle to "pedestrian" + double vehicle; + double pedestrian; + double unknown; +}; + +struct LateralKinematicsToLanelet +{ + double dist_from_left_boundary; + double dist_from_right_boundary; + double left_lateral_velocity; + double right_lateral_velocity; + double filtered_left_lateral_velocity; + double filtered_right_lateral_velocity; +}; + +enum class Maneuver { + UNINITIALIZED = 0, + LANE_FOLLOW = 1, + LEFT_LANE_CHANGE = 2, + RIGHT_LANE_CHANGE = 3, +}; + +struct LaneletData +{ + lanelet::Lanelet lanelet; + float probability; +}; + +struct PredictedRefPath +{ + float probability; + double speed_limit; + double width; + PosePath path; + Maneuver maneuver; +}; + +struct ObjectData +{ + std_msgs::msg::Header header; + lanelet::ConstLanelets current_lanelets; + lanelet::ConstLanelets future_possible_lanelets; + geometry_msgs::msg::Pose pose; + geometry_msgs::msg::Twist twist; + double time_delay; + // for lane change prediction + std::unordered_map lateral_kinematics_set; + Maneuver one_shot_maneuver{Maneuver::UNINITIALIZED}; + Maneuver output_maneuver{ + Maneuver::UNINITIALIZED}; // output maneuver considering previous one shot maneuvers +}; +struct CrosswalkUserData +{ + std_msgs::msg::Header header; + autoware_perception_msgs::msg::TrackedObject tracked_object; +}; + +} // namespace autoware::map_based_prediction + +#endif // MAP_BASED_PREDICTION__DATA_STRUCTURE_HPP_ diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index cf1ed28a037cb..809692115060c 100644 --- a/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -15,15 +15,16 @@ #ifndef MAP_BASED_PREDICTION__MAP_BASED_PREDICTION_NODE_HPP_ #define MAP_BASED_PREDICTION__MAP_BASED_PREDICTION_NODE_HPP_ -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/ros/update_param.hpp" +#include "map_based_prediction/data_structure.hpp" #include "map_based_prediction/path_generator.hpp" -#include "tf2/LinearMath/Quaternion.h" +#include "map_based_prediction/predictor_vru.hpp" +#include #include #include #include #include +#include #include #include #include @@ -44,6 +45,7 @@ #include #include #include +#include #include #include @@ -75,67 +77,6 @@ struct hash } // namespace std namespace autoware::map_based_prediction { -struct LateralKinematicsToLanelet -{ - double dist_from_left_boundary; - double dist_from_right_boundary; - double left_lateral_velocity; - double right_lateral_velocity; - double filtered_left_lateral_velocity; - double filtered_right_lateral_velocity; -}; - -enum class Maneuver { - UNINITIALIZED = 0, - LANE_FOLLOW = 1, - LEFT_LANE_CHANGE = 2, - RIGHT_LANE_CHANGE = 3, -}; - -struct ObjectData -{ - std_msgs::msg::Header header; - lanelet::ConstLanelets current_lanelets; - lanelet::ConstLanelets future_possible_lanelets; - geometry_msgs::msg::Pose pose; - geometry_msgs::msg::Twist twist; - double time_delay; - // for lane change prediction - std::unordered_map lateral_kinematics_set; - Maneuver one_shot_maneuver{Maneuver::UNINITIALIZED}; - Maneuver output_maneuver{ - Maneuver::UNINITIALIZED}; // output maneuver considering previous one shot maneuvers -}; - -struct CrosswalkUserData -{ - std_msgs::msg::Header header; - autoware_perception_msgs::msg::TrackedObject tracked_object; -}; - -struct LaneletData -{ - lanelet::Lanelet lanelet; - float probability; -}; - -struct PredictedRefPath -{ - float probability; - double speed_limit; - double width; - PosePath path; - Maneuver maneuver; -}; - -struct PredictionTimeHorizon -{ - // NOTE(Mamoru Sobue): motorcycle belongs to "vehicle" and bicycle to "pedestrian" - double vehicle; - double pedestrian; - double unknown; -}; - using LaneletsData = std::vector; using ManeuverProbability = std::unordered_map; using autoware::universe_utils::StopWatch; @@ -174,18 +115,13 @@ class MapBasedPredictionNode : public rclcpp::Node std::unique_ptr processing_time_publisher_; // Object History - std::unordered_map> road_users_history; - std::map, rclcpp::Time> stopped_times_against_green_; - std::unordered_map> crosswalk_users_history_; - std::unordered_map known_matches_; + std::unordered_map> road_users_history_; // Lanelet Map Pointers std::shared_ptr lanelet_map_ptr_; std::shared_ptr routing_graph_ptr_; std::shared_ptr traffic_rules_ptr_; - std::unordered_map traffic_signal_id_map_; - // parameter update OnSetParametersCallbackHandle::SharedPtr set_param_res_; rcl_interfaces::msg::SetParametersResult onParam( @@ -197,112 +133,102 @@ class MapBasedPredictionNode : public rclcpp::Node // Path Generator std::shared_ptr path_generator_; - // Crosswalk Entry Points - lanelet::ConstLanelets crosswalks_; + // Predictor + std::shared_ptr predictor_vru_; - // Fences - lanelet::LaneletMapUPtr fence_layer_{nullptr}; + ////// Parameters - // Parameters + // Object Parameters bool enable_delay_compensation_; - PredictionTimeHorizon prediction_time_horizon_; - double lateral_control_time_horizon_; - double prediction_time_horizon_rate_for_validate_lane_length_; - double prediction_sampling_time_interval_; - double min_velocity_for_map_based_prediction_; - double min_crosswalk_user_velocity_; - double max_crosswalk_user_delta_yaw_threshold_for_lanelet_; - double debug_accumulated_time_; + + //// Vehicle Parameters + // Lanelet Parameters double dist_threshold_for_searching_lanelet_; double delta_yaw_threshold_for_searching_lanelet_; double sigma_lateral_offset_; double sigma_yaw_angle_deg_; + bool consider_only_routable_neighbours_; + + // Pedestrian Parameters + bool remember_lost_crosswalk_users_; + + // Object history parameters double object_buffer_time_length_; double history_time_length_; + double cutoff_freq_of_velocity_lpf_; + + // Prediction Parameters std::string lane_change_detection_method_; double dist_threshold_to_bound_; double time_threshold_to_bound_; - double cutoff_freq_of_velocity_lpf_; double dist_ratio_threshold_to_left_bound_; double dist_ratio_threshold_to_right_bound_; double diff_dist_threshold_to_left_bound_; double diff_dist_threshold_to_right_bound_; int num_continuous_state_transition_; - bool consider_only_routable_neighbours_; - double reference_path_resolution_; + // Path generation parameters + double lateral_control_time_horizon_; + PredictionTimeHorizon prediction_time_horizon_; + double prediction_time_horizon_rate_for_validate_lane_length_; + double prediction_sampling_time_interval_; + double min_velocity_for_map_based_prediction_; + double reference_path_resolution_; bool check_lateral_acceleration_constraints_; double max_lateral_accel_; double min_acceleration_before_curve_; - bool use_vehicle_acceleration_; double speed_limit_multiplier_; double acceleration_exponential_half_life_; - bool use_crosswalk_signal_; - double threshold_velocity_assumed_as_stopping_; - std::vector distance_set_for_no_intention_to_walk_; - std::vector timeout_set_for_no_intention_to_walk_; - - bool match_lost_and_appeared_crosswalk_users_; - bool remember_lost_crosswalk_users_; - - std::unique_ptr published_time_publisher_; - rclcpp::Publisher::SharedPtr - detailed_processing_time_publisher_; - std::shared_ptr time_keeper_; - - // Member Functions + ////// Member Functions + // Node callbacks void mapCallback(const LaneletMapBin::ConstSharedPtr msg); void trafficSignalsCallback(const TrafficLightGroupArray::ConstSharedPtr msg); void objectsCallback(const TrackedObjects::ConstSharedPtr in_objects); - bool doesPathCrossAnyFence(const PredictedPath & predicted_path); + // Map process bool doesPathCrossFence( const PredictedPath & predicted_path, const lanelet::ConstLineString3d & fence_line); lanelet::BasicLineString2d convertToFenceLine(const lanelet::ConstLineString3d & fence); - bool isIntersecting( - const geometry_msgs::msg::Point & point1, const geometry_msgs::msg::Point & point2, - const lanelet::ConstPoint3d & point3, const lanelet::ConstPoint3d & point4); - - PredictedObjectKinematics convertToPredictedKinematics( - const TrackedObjectKinematics & tracked_object); + // Object process PredictedObject convertToPredictedObject(const TrackedObject & tracked_object); + void updateObjectData(TrackedObject & object); + geometry_msgs::msg::Pose compensateTimeDelay( + const geometry_msgs::msg::Pose & delayed_pose, const geometry_msgs::msg::Twist & twist, + const double dt) const; - PredictedObject getPredictedObjectAsCrosswalkUser(const TrackedObject & object); - - void removeStaleTrafficLightInfo(const TrackedObjects::ConstSharedPtr in_objects); - + //// Vehicle process + // Lanelet process LaneletsData getCurrentLanelets(const TrackedObject & object); bool checkCloseLaneletCondition( const std::pair & lanelet, const TrackedObject & object); float calculateLocalLikelihood( const lanelet::Lanelet & current_lanelet, const TrackedObject & object) const; - void updateObjectData(TrackedObject & object); + bool isDuplicated( + const std::pair & target_lanelet, + const LaneletsData & lanelets_data); + bool isDuplicated( + const PredictedPath & predicted_path, const std::vector & predicted_paths); + std::optional getTrafficSignalId(const lanelet::ConstLanelet & way_lanelet); + // Vehicle history process void updateRoadUsersHistory( const std_msgs::msg::Header & header, const TrackedObject & object, const LaneletsData & current_lanelets_data); - void updateCrosswalkUserHistory( - const std_msgs::msg::Header & header, const TrackedObject & object, - const std::string & object_id); - std::string tryMatchNewObjectToDisappeared( - const std::string & object_id, std::unordered_map & current_users); - std::optional searchProperStartingRefPathIndex( - const TrackedObject & object, const PosePath & pose_path) const; - std::vector getPredictedReferencePath( - const TrackedObject & object, const LaneletsData & current_lanelets_data, - const double object_detected_time, const double time_horizon); - std::vector convertPredictedReferencePath( - const TrackedObject & object, - const std::vector & lanelet_ref_paths) const; + + // Vehicle Maneuver Prediction Maneuver predictObjectManeuver( const std::string & object_id, const geometry_msgs::msg::Pose & object_pose, const LaneletData & current_lanelet_data, const double object_detected_time); - geometry_msgs::msg::Pose compensateTimeDelay( - const geometry_msgs::msg::Pose & delayed_pose, const geometry_msgs::msg::Twist & twist, - const double dt) const; + Maneuver predictObjectManeuverByTimeToLaneChange( + const std::string & object_id, const LaneletData & current_lanelet_data, + const double object_detected_time); + Maneuver predictObjectManeuverByLatDiffDistance( + const std::string & object_id, const geometry_msgs::msg::Pose & object_pose, + const LaneletData & current_lanelet_data, const double object_detected_time); + double calcRightLateralOffset( const lanelet::ConstLineString2d & boundary_line, const geometry_msgs::msg::Pose & search_pose); double calcLeftLateralOffset( @@ -311,35 +237,29 @@ class MapBasedPredictionNode : public rclcpp::Node const Maneuver & predicted_maneuver, const bool & left_paths_exists, const bool & right_paths_exists, const bool & center_paths_exists) const; + // Vehicle path process + std::optional searchProperStartingRefPathIndex( + const TrackedObject & object, const PosePath & pose_path) const; + std::vector getPredictedReferencePath( + const TrackedObject & object, const LaneletsData & current_lanelets_data, + const double object_detected_time, const double time_horizon); + std::vector convertPredictedReferencePath( + const TrackedObject & object, + const std::vector & lanelet_ref_paths) const; mutable universe_utils::LRUCache> lru_cache_of_convert_path_type_{1000}; std::pair convertLaneletPathToPosePath( const lanelet::routing::LaneletPath & path) const; - void updateFuturePossibleLanelets( - const std::string & object_id, const lanelet::routing::LaneletPaths & paths); - - bool isDuplicated( - const std::pair & target_lanelet, - const LaneletsData & lanelets_data); - bool isDuplicated( - const PredictedPath & predicted_path, const std::vector & predicted_paths); - std::optional getTrafficSignalId(const lanelet::ConstLanelet & way_lanelet); - std::optional getTrafficSignalElement(const lanelet::Id & id); - bool calcIntentionToCrossWithTrafficSignal( - const TrackedObject & object, const lanelet::ConstLanelet & crosswalk, - const lanelet::Id & signal_id); - + ////// Debugger + std::unique_ptr published_time_publisher_; + rclcpp::Publisher::SharedPtr + detailed_processing_time_publisher_; + std::shared_ptr time_keeper_; visualization_msgs::msg::Marker getDebugMarker( const TrackedObject & object, const Maneuver & maneuver, const size_t obj_num); - Maneuver predictObjectManeuverByTimeToLaneChange( - const std::string & object_id, const LaneletData & current_lanelet_data, - const double object_detected_time); - Maneuver predictObjectManeuverByLatDiffDistance( - const std::string & object_id, const geometry_msgs::msg::Pose & object_pose, - const LaneletData & current_lanelet_data, const double object_detected_time); - + //// Node functions void publish( const PredictedObjects & output, const visualization_msgs::msg::MarkerArray & debug_markers) const; diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp index 416efd2aa5616..8c2244304097f 100644 --- a/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp @@ -15,6 +15,8 @@ #ifndef MAP_BASED_PREDICTION__PATH_GENERATOR_HPP_ #define MAP_BASED_PREDICTION__PATH_GENERATOR_HPP_ +#include "map_based_prediction/data_structure.hpp" + #include #include #include @@ -80,11 +82,11 @@ struct CrosswalkEdgePoints }; using FrenetPath = std::vector; -using PosePath = std::vector; class PathGenerator { public: + explicit PathGenerator(const double sampling_time_interval); PathGenerator(const double sampling_time_interval, const double min_crosswalk_user_velocity); void setTimeKeeper(std::shared_ptr time_keeper_ptr); diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/predictor_vru.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/predictor_vru.hpp new file mode 100644 index 0000000000000..4691178718f60 --- /dev/null +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/predictor_vru.hpp @@ -0,0 +1,140 @@ +// Copyright 2024 TIER IV, inc. +// +// 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 MAP_BASED_PREDICTION__PREDICTOR_VRU_HPP_ +#define MAP_BASED_PREDICTION__PREDICTOR_VRU_HPP_ + +#include "map_based_prediction/data_structure.hpp" +#include "map_based_prediction/path_generator.hpp" + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::map_based_prediction +{ +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjectKinematics; +using autoware_perception_msgs::msg::TrackedObject; +using autoware_perception_msgs::msg::TrackedObjectKinematics; +using autoware_perception_msgs::msg::TrafficLightElement; +using autoware_perception_msgs::msg::TrafficLightGroup; +using autoware_perception_msgs::msg::TrafficLightGroupArray; + +class PredictorVru +{ +public: + explicit PredictorVru(rclcpp::Node & node) : node_(node) {} + ~PredictorVru() = default; + + void setParameters( + bool match_lost_and_appeared_crosswalk_users, double min_crosswalk_user_velocity, + double max_crosswalk_user_delta_yaw_threshold_for_lanelet, bool use_crosswalk_signal, + double threshold_velocity_assumed_as_stopping, + const std::vector & distance_set_for_no_intention_to_walk, + const std::vector & timeout_set_for_no_intention_to_walk, + double prediction_sampling_time_interval, double prediction_time_horizon) + { + match_lost_and_appeared_crosswalk_users_ = match_lost_and_appeared_crosswalk_users; + min_crosswalk_user_velocity_ = min_crosswalk_user_velocity; + max_crosswalk_user_delta_yaw_threshold_for_lanelet_ = + max_crosswalk_user_delta_yaw_threshold_for_lanelet; + use_crosswalk_signal_ = use_crosswalk_signal; + threshold_velocity_assumed_as_stopping_ = threshold_velocity_assumed_as_stopping; + distance_set_for_no_intention_to_walk_ = distance_set_for_no_intention_to_walk; + timeout_set_for_no_intention_to_walk_ = timeout_set_for_no_intention_to_walk; + prediction_time_horizon_ = prediction_time_horizon; + + path_generator_ = std::make_shared( + prediction_sampling_time_interval, min_crosswalk_user_velocity); + } + + void setLaneletMap(std::shared_ptr lanelet_map_ptr); + + void setTimeKeeper(std::shared_ptr time_keeper_ptr) + { + time_keeper_ = std::move(time_keeper_ptr); + } + + void setTrafficSignal(const TrafficLightGroupArray & traffic_signal_groups); + + void initialize(); + + void loadCurrentCrosswalkUsers(const TrackedObjects & objects); + void removeOldKnownMatches(const double current_time, const double buffer_time); + + PredictedObject predict(const std_msgs::msg::Header & header, const TrackedObject & object); + PredictedObjects retrieveUndetectedObjects(); + +private: + rclcpp::Node & node_; + std::shared_ptr time_keeper_; + + // Map data + std::shared_ptr lanelet_map_ptr_; + lanelet::ConstLanelets crosswalks_; + lanelet::LaneletMapUPtr fence_layer_{nullptr}; + std::shared_ptr path_generator_; + + // Data + std::unordered_map current_crosswalk_users_; + std::unordered_set predicted_crosswalk_users_ids_; + std::unordered_map> crosswalk_users_history_; + std::map, rclcpp::Time> stopped_times_against_green_; + std::unordered_map known_matches_; + std::unordered_map traffic_signal_id_map_; + + // Parameters + double prediction_time_horizon_; + bool match_lost_and_appeared_crosswalk_users_; + double min_crosswalk_user_velocity_; + double max_crosswalk_user_delta_yaw_threshold_for_lanelet_; + bool use_crosswalk_signal_; + double threshold_velocity_assumed_as_stopping_; + std::vector distance_set_for_no_intention_to_walk_; + std::vector timeout_set_for_no_intention_to_walk_; + + //// process + std::optional getTrafficSignalId(const lanelet::ConstLanelet & way_lanelet); + PredictedObject getPredictedObjectAsCrosswalkUser(const TrackedObject & object); + void updateCrosswalkUserHistory( + const std_msgs::msg::Header & header, const TrackedObject & object, + const std::string & object_id); + std::string tryMatchNewObjectToDisappeared( + const std::string & object_id, std::unordered_map & current_users); + bool calcIntentionToCrossWithTrafficSignal( + const TrackedObject & object, const lanelet::ConstLanelet & crosswalk, + const lanelet::Id & signal_id); + bool doesPathCrossAnyFence(const PredictedPath & predicted_path); + std::optional getTrafficSignalElement(const lanelet::Id & id); +}; + +} // namespace autoware::map_based_prediction + +#endif // MAP_BASED_PREDICTION__PREDICTOR_VRU_HPP_ diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/utils.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/utils.hpp new file mode 100644 index 0000000000000..06aab91c178eb --- /dev/null +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/utils.hpp @@ -0,0 +1,101 @@ +// Copyright 2024 TIER IV, inc. +// +// 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 MAP_BASED_PREDICTION__UTILS_HPP_ +#define MAP_BASED_PREDICTION__UTILS_HPP_ + +#include "map_based_prediction/data_structure.hpp" + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware::map_based_prediction +{ + +namespace utils +{ + +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjectKinematics; +using autoware_perception_msgs::msg::TrackedObject; +using autoware_perception_msgs::msg::TrackedObjectKinematics; + +/** + * @brief calc absolute normalized yaw difference between lanelet and object + * + * @param object + * @param lanelet + * @return double + */ +double calcAbsYawDiffBetweenLaneletAndObject( + const TrackedObject & object, const lanelet::ConstLanelet & lanelet); + +bool withinRoadLanelet( + const TrackedObject & object, + const std::vector> & surrounding_lanelets_with_dist, + const bool use_yaw_information = false); + +bool withinRoadLanelet( + const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const bool use_yaw_information = false); + +/** + * @brief change label for prediction + * + * @param label + * @return ObjectClassification::_label_type + */ +ObjectClassification::_label_type changeLabelForPrediction( + const ObjectClassification::_label_type & label, const TrackedObject & object, + const lanelet::LaneletMapPtr & lanelet_map_ptr_); + +template +std::unordered_set removeOldObjectsHistory( + const double current_time, const double buffer_time, + std::unordered_map> & target_objects); + +extern template std::unordered_set removeOldObjectsHistory( + const double current_time, const double buffer_time, + std::unordered_map> & target_objects); +extern template std::unordered_set removeOldObjectsHistory( + const double current_time, const double buffer_time, + std::unordered_map> & target_objects); + +PredictedObjectKinematics convertToPredictedKinematics( + const TrackedObjectKinematics & tracked_object); + +PredictedObject convertToPredictedObject(const TrackedObject & tracked_object); + +} // namespace utils + +} // namespace autoware::map_based_prediction + +#endif // MAP_BASED_PREDICTION__UTILS_HPP_ diff --git a/perception/autoware_map_based_prediction/src/debug.cpp b/perception/autoware_map_based_prediction/src/debug.cpp index bd40ee0981c0e..752b53b34322d 100644 --- a/perception/autoware_map_based_prediction/src/debug.cpp +++ b/perception/autoware_map_based_prediction/src/debug.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "autoware/universe_utils/ros/marker_helper.hpp" +#include "map_based_prediction/data_structure.hpp" #include "map_based_prediction/map_based_prediction_node.hpp" namespace autoware::map_based_prediction diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp index d9097f1907f51..59826adb48b7e 100644 --- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp @@ -14,6 +14,8 @@ #include "map_based_prediction/map_based_prediction_node.hpp" +#include "map_based_prediction/utils.hpp" + #include #include #include @@ -27,10 +29,12 @@ #include #include +#include #include #include +#include #include #include #include @@ -38,14 +42,6 @@ #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#include - #include #include #include @@ -207,25 +203,6 @@ void updateLateralKinematicsVector( } } -/** - * @brief calc absolute normalized yaw difference between lanelet and object - * - * @param object - * @param lanelet - * @return double - */ -double calcAbsYawDiffBetweenLaneletAndObject( - const TrackedObject & object, const lanelet::ConstLanelet & lanelet) -{ - const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - const double lane_yaw = - lanelet::utils::getLaneletAngle(lanelet, object.kinematics.pose_with_covariance.pose.position); - const double delta_yaw = object_yaw - lane_yaw; - const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw); - const double abs_norm_delta = std::fabs(normalized_delta_yaw); - return abs_norm_delta; -} - /** * @brief Get the Right LineSharing Lanelets object * @@ -347,335 +324,6 @@ lanelet::ConstLanelets getLanelets(const map_based_prediction::LaneletsData & da return lanelets; } -CrosswalkEdgePoints getCrosswalkEdgePoints(const lanelet::ConstLanelet & crosswalk) -{ - const Eigen::Vector2d r_p_front = crosswalk.rightBound().front().basicPoint2d(); - const Eigen::Vector2d l_p_front = crosswalk.leftBound().front().basicPoint2d(); - const Eigen::Vector2d front_center_point = (r_p_front + l_p_front) / 2.0; - - const Eigen::Vector2d r_p_back = crosswalk.rightBound().back().basicPoint2d(); - const Eigen::Vector2d l_p_back = crosswalk.leftBound().back().basicPoint2d(); - const Eigen::Vector2d back_center_point = (r_p_back + l_p_back) / 2.0; - - return CrosswalkEdgePoints{front_center_point, r_p_front, l_p_front, - back_center_point, r_p_back, l_p_back}; -} - -bool withinRoadLanelet( - const TrackedObject & object, - const std::vector> & surrounding_lanelets_with_dist, - const bool use_yaw_information = false) -{ - for (const auto & [dist, lanelet] : surrounding_lanelets_with_dist) { - if (lanelet.hasAttribute(lanelet::AttributeName::Subtype)) { - lanelet::Attribute attr = lanelet.attribute(lanelet::AttributeName::Subtype); - if ( - attr.value() == lanelet::AttributeValueString::Crosswalk || - attr.value() == lanelet::AttributeValueString::Walkway) { - continue; - } - } - - constexpr float yaw_threshold = 0.6; - bool within_lanelet = std::abs(dist) < 1e-5; - if (use_yaw_information) { - within_lanelet = - within_lanelet && calcAbsYawDiffBetweenLaneletAndObject(object, lanelet) < yaw_threshold; - } - if (within_lanelet) { - return true; - } - } - - return false; -} - -bool withinRoadLanelet( - const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr, - const bool use_yaw_information = false) -{ - const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; - lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y); - // nearest lanelet - constexpr double search_radius = 10.0; // [m] - const auto surrounding_lanelets_with_dist = - lanelet::geometry::findWithin2d(lanelet_map_ptr->laneletLayer, search_point, search_radius); - - return withinRoadLanelet(object, surrounding_lanelets_with_dist, use_yaw_information); -} - -boost::optional isReachableCrosswalkEdgePoints( - const TrackedObject & object, const lanelet::ConstLanelets & surrounding_lanelets, - const lanelet::ConstLanelets & surrounding_crosswalks, const CrosswalkEdgePoints & edge_points, - const double time_horizon, const double min_object_vel) -{ - using Point = boost::geometry::model::d2::point_xy; - - const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; - const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; - const auto yaw = autoware::universe_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; - - lanelet::BasicPoint2d obj_pos_as_lanelet(obj_pos.x, obj_pos.y); - - const auto & p1 = edge_points.front_center_point; - const auto & p2 = edge_points.back_center_point; - - CrosswalkEdgePoints ret{p1, {}, {}, p2, {}, {}}; - auto distance_pedestrian_to_p1 = std::hypot(p1.x() - obj_pos.x, p1.y() - obj_pos.y); - auto distance_pedestrian_to_p2 = std::hypot(p2.x() - obj_pos.x, p2.y() - obj_pos.y); - - if (distance_pedestrian_to_p2 < distance_pedestrian_to_p1) { - ret.swap(); - std::swap(distance_pedestrian_to_p1, distance_pedestrian_to_p2); - } - - constexpr double stop_velocity_th = 0.14; // [m/s] - const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); - const auto is_stop_object = estimated_velocity < stop_velocity_th; - const auto velocity = std::max(min_object_vel, estimated_velocity); - - const auto isAcrossAnyRoad = [&surrounding_lanelets, &surrounding_crosswalks]( - const Point & p_src, const Point & p_dst) { - const auto withinAnyCrosswalk = [&surrounding_crosswalks](const Point & p) { - for (const auto & crosswalk : surrounding_crosswalks) { - if (boost::geometry::within(p, crosswalk.polygon2d().basicPolygon())) { - return true; - } - } - return false; - }; - - const auto isExist = [](const Point & p, const std::vector & points) { - for (const auto & existingPoint : points) { - if (boost::geometry::distance(p, existingPoint) < 1e-1) { - return true; - } - } - return false; - }; - - std::vector points_of_intersect; - const boost::geometry::model::linestring line{p_src, p_dst}; - for (const auto & lanelet : surrounding_lanelets) { - const lanelet::Attribute attr = lanelet.attribute(lanelet::AttributeName::Subtype); - if (attr.value() != lanelet::AttributeValueString::Road) { - continue; - } - - std::vector tmp_intersects; - boost::geometry::intersection(line, lanelet.polygon2d().basicPolygon(), tmp_intersects); - for (const auto & p : tmp_intersects) { - if (isExist(p, points_of_intersect) || withinAnyCrosswalk(p)) { - continue; - } - points_of_intersect.push_back(p); - if (points_of_intersect.size() >= 2) { - return true; - } - } - } - return false; - }; - - const bool first_intersects_road = isAcrossAnyRoad( - {obj_pos.x, obj_pos.y}, {ret.front_center_point.x(), ret.front_center_point.y()}); - const bool second_intersects_road = - isAcrossAnyRoad({obj_pos.x, obj_pos.y}, {ret.back_center_point.x(), ret.back_center_point.y()}); - - if (first_intersects_road && second_intersects_road) { - return {}; - } - - if (first_intersects_road && !second_intersects_road) { - ret.swap(); - } - - const Eigen::Vector2d pedestrian_to_crosswalk( - (ret.front_center_point.x() + ret.back_center_point.x()) / 2.0 - obj_pos.x, - (ret.front_center_point.y() + ret.back_center_point.y()) / 2.0 - obj_pos.y); - const Eigen::Vector2d pedestrian_heading_direction( - obj_vel.x * std::cos(yaw), obj_vel.x * std::sin(yaw)); - const auto reachable = - std::min(distance_pedestrian_to_p1, distance_pedestrian_to_p2) < velocity * time_horizon; - const auto heading_for_crosswalk = - pedestrian_to_crosswalk.dot(pedestrian_heading_direction) > 0.0; - - if ((reachable && heading_for_crosswalk) || (reachable && is_stop_object)) { - return ret; - } - - return {}; -} - -bool hasPotentialToReach( - const TrackedObject & object, const Eigen::Vector2d & center_point, - const Eigen::Vector2d & right_point, const Eigen::Vector2d & left_point, - const double time_horizon, const double min_object_vel, - const double max_crosswalk_user_delta_yaw_threshold_for_lanelet) -{ - const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; - const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; - const auto yaw = autoware::universe_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; - - constexpr double stop_velocity_th = 0.14; // [m/s] - const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); - const auto is_stop_object = estimated_velocity < stop_velocity_th; - const auto velocity = std::max(min_object_vel, estimated_velocity); - - const double pedestrian_to_crosswalk_center_direction = - std::atan2(center_point.y() - obj_pos.y, center_point.x() - obj_pos.x); - - const auto - [pedestrian_to_crosswalk_right_rel_direction, pedestrian_to_crosswalk_left_rel_direction] = - [&]() { - const double pedestrian_to_crosswalk_right_direction = - std::atan2(right_point.y() - obj_pos.y, right_point.x() - obj_pos.x); - const double pedestrian_to_crosswalk_left_direction = - std::atan2(left_point.y() - obj_pos.y, left_point.x() - obj_pos.x); - return std::make_pair( - autoware::universe_utils::normalizeRadian( - pedestrian_to_crosswalk_right_direction - pedestrian_to_crosswalk_center_direction), - autoware::universe_utils::normalizeRadian( - pedestrian_to_crosswalk_left_direction - pedestrian_to_crosswalk_center_direction)); - }(); - - const double pedestrian_heading_rel_direction = [&]() { - const double pedestrian_heading_direction = - std::atan2(obj_vel.x * std::sin(yaw), obj_vel.x * std::cos(yaw)); - return autoware::universe_utils::normalizeRadian( - pedestrian_heading_direction - pedestrian_to_crosswalk_center_direction); - }(); - - const double pedestrian_to_crosswalk_min_rel_direction = std::min( - pedestrian_to_crosswalk_right_rel_direction, pedestrian_to_crosswalk_left_rel_direction); - const double pedestrian_to_crosswalk_max_rel_direction = std::max( - pedestrian_to_crosswalk_right_rel_direction, pedestrian_to_crosswalk_left_rel_direction); - const double pedestrian_vel_angle_against_crosswalk = [&]() { - if (pedestrian_heading_rel_direction < pedestrian_to_crosswalk_min_rel_direction) { - return pedestrian_to_crosswalk_min_rel_direction - pedestrian_heading_rel_direction; - } - if (pedestrian_to_crosswalk_max_rel_direction < pedestrian_heading_rel_direction) { - return pedestrian_to_crosswalk_max_rel_direction - pedestrian_heading_rel_direction; - } - return 0.0; - }(); - const auto heading_for_crosswalk = std::abs(pedestrian_vel_angle_against_crosswalk) < - max_crosswalk_user_delta_yaw_threshold_for_lanelet; - const auto reachable = std::hypot(center_point.x() - obj_pos.x, center_point.y() - obj_pos.y) < - velocity * time_horizon; - - if (reachable && (heading_for_crosswalk || is_stop_object)) { - return true; - } - - return false; -} - -template -std::unordered_set removeOldObjectsHistory( - const double current_time, const double buffer_time, - std::unordered_map> & target_objects) -{ - std::unordered_set invalid_object_id; - for (auto iter = target_objects.begin(); iter != target_objects.end(); ++iter) { - const std::string object_id = iter->first; - std::deque & object_data = iter->second; - - // If object data is empty, we are going to delete the buffer for the obstacle - if (object_data.empty()) { - invalid_object_id.insert(object_id); - continue; - } - - const double latest_object_time = rclcpp::Time(object_data.back().header.stamp).seconds(); - - // Delete Old Objects - if (current_time - latest_object_time > buffer_time) { - invalid_object_id.insert(object_id); - continue; - } - - // Delete old information - while (!object_data.empty()) { - const double post_object_time = rclcpp::Time(object_data.front().header.stamp).seconds(); - if (current_time - post_object_time <= buffer_time) { - break; - } - object_data.pop_front(); - } - - if (object_data.empty()) { - invalid_object_id.insert(object_id); - continue; - } - } - - for (const auto & key : invalid_object_id) { - target_objects.erase(key); - } - - return invalid_object_id; -} - -/** - * @brief change label for prediction - * - * @param label - * @return ObjectClassification::_label_type - */ -ObjectClassification::_label_type changeLabelForPrediction( - const ObjectClassification::_label_type & label, const TrackedObject & object, - const lanelet::LaneletMapPtr & lanelet_map_ptr_) -{ - // for car like vehicle do not change labels - switch (label) { - case ObjectClassification::CAR: - case ObjectClassification::BUS: - case ObjectClassification::TRUCK: - case ObjectClassification::TRAILER: - case ObjectClassification::UNKNOWN: - return label; - - case ObjectClassification::MOTORCYCLE: - case ObjectClassification::BICYCLE: { // if object is within road lanelet and satisfies yaw - // constraints - const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); - // if the object is within lanelet, do the same estimation with vehicle - if (within_road_lanelet) return ObjectClassification::MOTORCYCLE; - - constexpr float high_speed_threshold = - autoware::universe_utils::kmph2mps(25.0); // High speed bicycle 25 km/h - // calc abs speed from x and y velocity - const double abs_speed = std::hypot( - object.kinematics.twist_with_covariance.twist.linear.x, - object.kinematics.twist_with_covariance.twist.linear.y); - const bool high_speed_object = abs_speed > high_speed_threshold; - // high speed object outside road lanelet will move like unknown object - // return ObjectClassification::UNKNOWN; // temporary disabled - if (high_speed_object) return label; // Do nothing for now - return ObjectClassification::BICYCLE; - } - - case ObjectClassification::PEDESTRIAN: { - const float max_velocity_for_human_mps = - autoware::universe_utils::kmph2mps(25.0); // Max human being motion speed is 25km/h - const double abs_speed = std::hypot( - object.kinematics.twist_with_covariance.twist.linear.x, - object.kinematics.twist_with_covariance.twist.linear.y); - const bool high_speed_object = abs_speed > max_velocity_for_human_mps; - // fast, human-like object: like segway - // return ObjectClassification::MOTORCYCLE; - if (high_speed_object) return label; // currently do nothing - // fast human outside road lanelet will move like unknown object - // return ObjectClassification::UNKNOWN; - return label; - } - - default: - return label; - } -} - // NOTE: These two functions are copied from the route_handler package. lanelet::Lanelets getRightOppositeLanelets( const std::shared_ptr & lanelet_map_ptr, @@ -743,7 +391,7 @@ void replaceObjectYawWithLaneletsYaw( } // namespace MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_options) -: Node("map_based_prediction", node_options), debug_accumulated_time_(0.0) +: Node("map_based_prediction", node_options) { if (!google::IsGoogleLoggingInitialized()) { google::InitGoogleLogging("map_based_prediction_node"); @@ -759,9 +407,7 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ prediction_sampling_time_interval_ = declare_parameter("prediction_sampling_delta_time"); min_velocity_for_map_based_prediction_ = declare_parameter("min_velocity_for_map_based_prediction"); - min_crosswalk_user_velocity_ = declare_parameter("min_crosswalk_user_velocity"); - max_crosswalk_user_delta_yaw_threshold_for_lanelet_ = - declare_parameter("max_crosswalk_user_delta_yaw_threshold_for_lanelet"); + dist_threshold_for_searching_lanelet_ = declare_parameter("dist_threshold_for_searching_lanelet"); delta_yaw_threshold_for_searching_lanelet_ = @@ -809,30 +455,49 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ * parameter control the estimated path length = vx * th * (rate) */ prediction_time_horizon_rate_for_validate_lane_length_ = declare_parameter("prediction_time_horizon_rate_for_validate_shoulder_lane_length"); - match_lost_and_appeared_crosswalk_users_ = - declare_parameter("use_crosswalk_user_history.match_lost_and_appeared_users"); - remember_lost_crosswalk_users_ = - declare_parameter("use_crosswalk_user_history.remember_lost_users"); + use_vehicle_acceleration_ = declare_parameter("use_vehicle_acceleration"); speed_limit_multiplier_ = declare_parameter("speed_limit_multiplier"); acceleration_exponential_half_life_ = declare_parameter("acceleration_exponential_half_life"); - use_crosswalk_signal_ = declare_parameter("crosswalk_with_signal.use_crosswalk_signal"); - threshold_velocity_assumed_as_stopping_ = - declare_parameter("crosswalk_with_signal.threshold_velocity_assumed_as_stopping"); - distance_set_for_no_intention_to_walk_ = declare_parameter>( - "crosswalk_with_signal.distance_set_for_no_intention_to_walk"); - timeout_set_for_no_intention_to_walk_ = declare_parameter>( - "crosswalk_with_signal.timeout_set_for_no_intention_to_walk"); + // initialize VRU predictor + predictor_vru_ = std::make_unique(*this); + + // VRU parameters + remember_lost_crosswalk_users_ = + declare_parameter("use_crosswalk_user_history.remember_lost_users"); + { + bool match_lost_and_appeared_crosswalk_users = + declare_parameter("use_crosswalk_user_history.match_lost_and_appeared_users"); + double min_crosswalk_user_velocity = declare_parameter("min_crosswalk_user_velocity"); + double max_crosswalk_user_delta_yaw_threshold_for_lanelet = + declare_parameter("max_crosswalk_user_delta_yaw_threshold_for_lanelet"); + bool use_crosswalk_signal = + declare_parameter("crosswalk_with_signal.use_crosswalk_signal"); + double threshold_velocity_assumed_as_stopping = + declare_parameter("crosswalk_with_signal.threshold_velocity_assumed_as_stopping"); + std::vector distance_set_for_no_intention_to_walk = + declare_parameter>( + "crosswalk_with_signal.distance_set_for_no_intention_to_walk"); + std::vector timeout_set_for_no_intention_to_walk = + declare_parameter>( + "crosswalk_with_signal.timeout_set_for_no_intention_to_walk"); + predictor_vru_->setParameters( + match_lost_and_appeared_crosswalk_users, min_crosswalk_user_velocity, + max_crosswalk_user_delta_yaw_threshold_for_lanelet, use_crosswalk_signal, + threshold_velocity_assumed_as_stopping, distance_set_for_no_intention_to_walk, + timeout_set_for_no_intention_to_walk, prediction_sampling_time_interval_, + prediction_time_horizon_.pedestrian); + } // debug parameter bool use_time_publisher = declare_parameter("publish_processing_time"); bool use_time_keeper = declare_parameter("publish_processing_time_detail"); bool use_debug_marker = declare_parameter("publish_debug_markers"); - path_generator_ = std::make_shared( - prediction_sampling_time_interval_, min_crosswalk_user_velocity_); + // initialize path generator + path_generator_ = std::make_shared(prediction_sampling_time_interval_); path_generator_->setUseVehicleAcceleration(use_vehicle_acceleration_); path_generator_->setAccelerationHalfLife(acceleration_exponential_half_life_); @@ -867,6 +532,7 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ auto time_keeper = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_); time_keeper_ = std::make_shared(time_keeper); path_generator_->setTimeKeeper(time_keeper_); + predictor_vru_->setTimeKeeper(time_keeper_); } if (use_debug_marker) { @@ -901,31 +567,6 @@ rcl_interfaces::msg::SetParametersResult MapBasedPredictionNode::onParam( return result; } -PredictedObjectKinematics MapBasedPredictionNode::convertToPredictedKinematics( - const TrackedObjectKinematics & tracked_object) -{ - PredictedObjectKinematics output; - output.initial_pose_with_covariance = tracked_object.pose_with_covariance; - output.initial_twist_with_covariance = tracked_object.twist_with_covariance; - output.initial_acceleration_with_covariance = tracked_object.acceleration_with_covariance; - return output; -} - -PredictedObject MapBasedPredictionNode::convertToPredictedObject( - const TrackedObject & tracked_object) -{ - PredictedObject predicted_object; - predicted_object.kinematics = convertToPredictedKinematics(tracked_object.kinematics); - predicted_object.classification = tracked_object.classification; - predicted_object.object_id = tracked_object.object_id; - predicted_object.shape.type = tracked_object.shape.type; - predicted_object.shape.footprint = tracked_object.shape.footprint; - predicted_object.shape.dimensions = tracked_object.shape.dimensions; - predicted_object.existence_probability = tracked_object.existence_probability; - - return predicted_object; -} - void MapBasedPredictionNode::mapCallback(const LaneletMapBin::ConstSharedPtr msg) { RCLCPP_DEBUG(get_logger(), "[Map Based Prediction]: Start loading lanelet"); @@ -935,33 +576,14 @@ void MapBasedPredictionNode::mapCallback(const LaneletMapBin::ConstSharedPtr msg lru_cache_of_convert_path_type_.clear(); // clear cache RCLCPP_DEBUG(get_logger(), "[Map Based Prediction]: Map is loaded"); - const auto all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); - const auto crosswalks = lanelet::utils::query::crosswalkLanelets(all_lanelets); - const auto walkways = lanelet::utils::query::walkwayLanelets(all_lanelets); - crosswalks_.insert(crosswalks_.end(), crosswalks.begin(), crosswalks.end()); - crosswalks_.insert(crosswalks_.end(), walkways.begin(), walkways.end()); - - lanelet::LineStrings3d fences; - for (const auto & linestring : lanelet_map_ptr_->lineStringLayer) { - if (const std::string type = linestring.attributeOr(lanelet::AttributeName::Type, "none"); - type == "fence") { - fences.push_back(lanelet::LineString3d( - std::const_pointer_cast(linestring.constData()))); - } - } - fence_layer_ = lanelet::utils::createMap(fences); + predictor_vru_->setLaneletMap(lanelet_map_ptr_); } void MapBasedPredictionNode::trafficSignalsCallback( const TrafficLightGroupArray::ConstSharedPtr msg) { - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - - traffic_signal_id_map_.clear(); - for (const auto & signal : msg->traffic_light_groups) { - traffic_signal_id_map_[signal.traffic_light_group_id] = signal; - } + // load traffic signals to the predictor + predictor_vru_->setTrafficSignal(*msg); } void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects) @@ -984,22 +606,27 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt return; } - // Remove old objects information in object history - const double objects_detected_time = rclcpp::Time(in_objects->header.stamp).seconds(); - removeOldObjectsHistory(objects_detected_time, object_buffer_time_length_, road_users_history); - removeStaleTrafficLightInfo(in_objects); - - auto invalidated_crosswalk_users = removeOldObjectsHistory( - objects_detected_time, object_buffer_time_length_, crosswalk_users_history_); - // delete matches that point to invalid object - for (auto it = known_matches_.begin(); it != known_matches_.end();) { - if (invalidated_crosswalk_users.count(it->second)) { - it = known_matches_.erase(it); - } else { - ++it; - } + // get world to map transform + geometry_msgs::msg::TransformStamped::ConstSharedPtr world2map_transform; + bool is_object_not_in_map_frame = in_objects->header.frame_id != "map"; + if (is_object_not_in_map_frame) { + world2map_transform = transform_listener_.getTransform( + "map", // target + in_objects->header.frame_id, // src + in_objects->header.stamp, rclcpp::Duration::from_seconds(0.1)); + if (!world2map_transform) return; } + // Get objects detected time + const double objects_detected_time = rclcpp::Time(in_objects->header.stamp).seconds(); + + // Remove old objects information in object history + // road users + utils::removeOldObjectsHistory( + objects_detected_time, object_buffer_time_length_, road_users_history_); + // crosswalk users + predictor_vru_->removeOldKnownMatches(objects_detected_time, object_buffer_time_length_); + // result output PredictedObjects output; output.header = in_objects->header; @@ -1009,31 +636,9 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt visualization_msgs::msg::MarkerArray debug_markers; // get current crosswalk users for later prediction - std::unordered_map current_crosswalk_users; - for (const auto & object : in_objects->objects) { - const auto label_for_prediction = - changeLabelForPrediction(object.classification.front().label, object, lanelet_map_ptr_); - if ( - label_for_prediction == ObjectClassification::PEDESTRIAN || - label_for_prediction == ObjectClassification::BICYCLE) { - const std::string object_id = autoware::universe_utils::toHexString(object.object_id); - current_crosswalk_users.emplace(object_id, object); - } - } - std::unordered_set predicted_crosswalk_users_ids; - - // get world to map transform - geometry_msgs::msg::TransformStamped::ConstSharedPtr world2map_transform; - - bool is_object_not_in_map_frame = in_objects->header.frame_id != "map"; - if (is_object_not_in_map_frame) { - world2map_transform = transform_listener_.getTransform( - "map", // target - in_objects->header.frame_id, // src - in_objects->header.stamp, rclcpp::Duration::from_seconds(0.1)); - if (!world2map_transform) return; - } + predictor_vru_->loadCurrentCrosswalkUsers(*in_objects); + // for each object for (const auto & object : in_objects->objects) { TrackedObject transformed_object = object; @@ -1048,20 +653,14 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt // get tracking label and update it for the prediction const auto & label_ = transformed_object.classification.front().label; - const auto label = changeLabelForPrediction(label_, object, lanelet_map_ptr_); + const auto label = utils::changeLabelForPrediction(label_, object, lanelet_map_ptr_); switch (label) { case ObjectClassification::PEDESTRIAN: case ObjectClassification::BICYCLE: { - std::string object_id = autoware::universe_utils::toHexString(object.object_id); - if (match_lost_and_appeared_crosswalk_users_) { - object_id = tryMatchNewObjectToDisappeared(object_id, current_crosswalk_users); - } - predicted_crosswalk_users_ids.insert(object_id); - updateCrosswalkUserHistory(output.header, transformed_object, object_id); - const auto predicted_object_crosswalk = - getPredictedObjectAsCrosswalkUser(transformed_object); - output.objects.push_back(predicted_object_crosswalk); + // Run pedestrian/bicycle prediction + const auto predicted_vru = predictor_vru_->predict(output.header, transformed_object); + output.objects.emplace_back(predicted_vru); break; } case ObjectClassification::CAR: @@ -1085,7 +684,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt predicted_path.confidence = 1.0; if (predicted_path.path.empty()) break; - auto predicted_object_vehicle = convertToPredictedObject(transformed_object); + auto predicted_object_vehicle = utils::convertToPredictedObject(transformed_object); predicted_object_vehicle.kinematics.predicted_paths.push_back(predicted_path); output.objects.push_back(predicted_object_vehicle); break; @@ -1101,7 +700,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt predicted_path.confidence = 1.0; if (predicted_path.path.empty()) break; - auto predicted_slow_object = convertToPredictedObject(transformed_object); + auto predicted_slow_object = utils::convertToPredictedObject(transformed_object); predicted_slow_object.kinematics.predicted_paths.push_back(predicted_path); output.objects.push_back(predicted_slow_object); break; @@ -1121,7 +720,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt predicted_path.confidence = 1.0; if (predicted_path.path.empty()) break; - auto predicted_object_out_of_lane = convertToPredictedObject(transformed_object); + auto predicted_object_out_of_lane = utils::convertToPredictedObject(transformed_object); predicted_object_out_of_lane.kinematics.predicted_paths.push_back(predicted_path); output.objects.push_back(predicted_object_out_of_lane); break; @@ -1206,7 +805,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt const float min_sum_confidence_value = 1e-3; sum_confidence = std::max(sum_confidence, min_sum_confidence_value); - auto predicted_object = convertToPredictedObject(transformed_object); + auto predicted_object = utils::convertToPredictedObject(transformed_object); for (auto & predicted_path : predicted_paths) { predicted_path.confidence = predicted_path.confidence / sum_confidence; @@ -1217,7 +816,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt break; } default: { - auto predicted_unknown_object = convertToPredictedObject(transformed_object); + auto predicted_unknown_object = utils::convertToPredictedObject(transformed_object); PredictedPath predicted_path = path_generator_->generatePathForNonVehicleObject( transformed_object, prediction_time_horizon_.unknown); predicted_path.confidence = 1.0; @@ -1231,15 +830,9 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt // process lost crosswalk users to tackle unstable detection if (remember_lost_crosswalk_users_) { - for (const auto & [id, crosswalk_user] : crosswalk_users_history_) { - // get a predicted path for crosswalk users in history who didn't get path yet using latest - // message - if (predicted_crosswalk_users_ids.count(id) == 0) { - const auto predicted_object = - getPredictedObjectAsCrosswalkUser(crosswalk_user.back().tracked_object); - output.objects.push_back(predicted_object); - } - } + PredictedObjects retrived_objects = predictor_vru_->retrieveUndetectedObjects(); + output.objects.insert( + output.objects.end(), retrived_objects.objects.begin(), retrived_objects.objects.end()); } // Publish Results @@ -1268,293 +861,6 @@ void MapBasedPredictionNode::publish( if (pub_debug_markers_) pub_debug_markers_->publish(debug_markers); } -void MapBasedPredictionNode::updateCrosswalkUserHistory( - const std_msgs::msg::Header & header, const TrackedObject & object, const std::string & object_id) -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - - CrosswalkUserData crosswalk_user_data; - crosswalk_user_data.header = header; - crosswalk_user_data.tracked_object = object; - - if (crosswalk_users_history_.count(object_id) == 0) { - crosswalk_users_history_.emplace(object_id, std::deque{crosswalk_user_data}); - return; - } - - crosswalk_users_history_.at(object_id).push_back(crosswalk_user_data); -} - -std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared( - const std::string & object_id, std::unordered_map & current_users) -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - - const auto known_match_opt = [&]() -> std::optional { - if (!known_matches_.count(object_id)) { - return std::nullopt; - } - - std::string match_id = known_matches_[object_id]; - // object in the history is already matched to something (possibly itself) - if (crosswalk_users_history_.count(match_id)) { - // avoid matching two appeared users to one user in history - current_users[match_id] = crosswalk_users_history_[match_id].back().tracked_object; - return match_id; - } else { - RCLCPP_WARN_STREAM( - get_logger(), "Crosswalk user was " - << object_id << "was matched to " << match_id - << " but history for the crosswalk user was deleted. Rematching"); - } - return std::nullopt; - }(); - // early return if the match is already known - if (known_match_opt.has_value()) { - return known_match_opt.value(); - } - - std::string match_id = object_id; - double best_score = std::numeric_limits::max(); - const auto object_pos = current_users[object_id].kinematics.pose_with_covariance.pose.position; - for (const auto & [user_id, user_history] : crosswalk_users_history_) { - // user present in current_users and will be matched to itself - if (current_users.count(user_id)) { - continue; - } - // TODO(dkoldaev): implement more sophisticated scoring, for now simply dst to last position in - // history - const auto match_candidate_pos = - user_history.back().tracked_object.kinematics.pose_with_covariance.pose.position; - const double score = - std::hypot(match_candidate_pos.x - object_pos.x, match_candidate_pos.y - object_pos.y); - if (score < best_score) { - best_score = score; - match_id = user_id; - } - } - - if (object_id != match_id) { - RCLCPP_INFO_STREAM( - get_logger(), "[Map Based Prediction]: Matched " << object_id << " to " << match_id); - // avoid matching two appeared users to one user in history - current_users[match_id] = crosswalk_users_history_[match_id].back().tracked_object; - } - - known_matches_[object_id] = match_id; - return match_id; -} - -bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path) -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - - lanelet::BasicLineString2d predicted_path_ls; - for (const auto & p : predicted_path.path) - predicted_path_ls.emplace_back(p.position.x, p.position.y); - const auto candidates = - fence_layer_->lineStringLayer.search(lanelet::geometry::boundingBox2d(predicted_path_ls)); - for (const auto & candidate : candidates) { - if (doesPathCrossFence(predicted_path, candidate)) { - return true; - } - } - return false; -} - -bool MapBasedPredictionNode::doesPathCrossFence( - const PredictedPath & predicted_path, const lanelet::ConstLineString3d & fence_line) -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - - // check whether the predicted path cross with fence - for (size_t i = 0; i < predicted_path.path.size() - 1; ++i) { - for (size_t j = 0; j < fence_line.size() - 1; ++j) { - if (isIntersecting( - predicted_path.path[i].position, predicted_path.path[i + 1].position, fence_line[j], - fence_line[j + 1])) { - return true; - } - } - } - return false; -} - -bool MapBasedPredictionNode::isIntersecting( - const geometry_msgs::msg::Point & point1, const geometry_msgs::msg::Point & point2, - const lanelet::ConstPoint3d & point3, const lanelet::ConstPoint3d & point4) -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - - const auto p1 = autoware::universe_utils::createPoint(point1.x, point1.y, 0.0); - const auto p2 = autoware::universe_utils::createPoint(point2.x, point2.y, 0.0); - const auto p3 = autoware::universe_utils::createPoint(point3.x(), point3.y(), 0.0); - const auto p4 = autoware::universe_utils::createPoint(point4.x(), point4.y(), 0.0); - const auto intersection = autoware::universe_utils::intersect(p1, p2, p3, p4); - return intersection.has_value(); -} - -PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( - const TrackedObject & object) -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - - auto predicted_object = convertToPredictedObject(object); - { - PredictedPath predicted_path = - path_generator_->generatePathForNonVehicleObject(object, prediction_time_horizon_.pedestrian); - predicted_path.confidence = 1.0; - - predicted_object.kinematics.predicted_paths.push_back(predicted_path); - } - - boost::optional crossing_crosswalk{boost::none}; - const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; - const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; - const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); - const auto velocity = std::max(min_crosswalk_user_velocity_, estimated_velocity); - const auto surrounding_lanelets_with_dist = lanelet::geometry::findWithin2d( - lanelet_map_ptr_->laneletLayer, lanelet::BasicPoint2d{obj_pos.x, obj_pos.y}, - prediction_time_horizon_.pedestrian * velocity); - lanelet::ConstLanelets surrounding_lanelets; - lanelet::ConstLanelets surrounding_crosswalks; - for (const auto & [dist, lanelet] : surrounding_lanelets_with_dist) { - surrounding_lanelets.push_back(lanelet); - const auto attr = lanelet.attribute(lanelet::AttributeName::Subtype); - if ( - attr.value() == lanelet::AttributeValueString::Crosswalk || - attr.value() == lanelet::AttributeValueString::Walkway) { - const auto & crosswalk = lanelet; - surrounding_crosswalks.push_back(crosswalk); - if (std::abs(dist) < 1e-5) { - crossing_crosswalk = crosswalk; - } - } - } - - // If the object is in the crosswalk, generate path to the crosswalk edge - if (crossing_crosswalk) { - const auto edge_points = getCrosswalkEdgePoints(crossing_crosswalk.get()); - - if (hasPotentialToReach( - object, edge_points.front_center_point, edge_points.front_right_point, - edge_points.front_left_point, std::numeric_limits::max(), - min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) { - PredictedPath predicted_path = - path_generator_->generatePathToTargetPoint(object, edge_points.front_center_point); - predicted_path.confidence = 1.0; - predicted_object.kinematics.predicted_paths.push_back(predicted_path); - } - - if (hasPotentialToReach( - object, edge_points.back_center_point, edge_points.back_right_point, - edge_points.back_left_point, std::numeric_limits::max(), - min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) { - PredictedPath predicted_path = - path_generator_->generatePathToTargetPoint(object, edge_points.back_center_point); - predicted_path.confidence = 1.0; - predicted_object.kinematics.predicted_paths.push_back(predicted_path); - } - - // If the object is not crossing the crosswalk, in the road lanelets, try to find the closest - // crosswalk and generate path to the crosswalk edge - } else if (withinRoadLanelet(object, surrounding_lanelets_with_dist)) { - lanelet::ConstLanelet closest_crosswalk{}; - const auto & obj_pose = object.kinematics.pose_with_covariance.pose; - const auto found_closest_crosswalk = - lanelet::utils::query::getClosestLanelet(crosswalks_, obj_pose, &closest_crosswalk); - - if (found_closest_crosswalk) { - const auto edge_points = getCrosswalkEdgePoints(closest_crosswalk); - - if (hasPotentialToReach( - object, edge_points.front_center_point, edge_points.front_right_point, - edge_points.front_left_point, prediction_time_horizon_.pedestrian * 2.0, - min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) { - PredictedPath predicted_path = - path_generator_->generatePathToTargetPoint(object, edge_points.front_center_point); - predicted_path.confidence = 1.0; - predicted_object.kinematics.predicted_paths.push_back(predicted_path); - } - - if (hasPotentialToReach( - object, edge_points.back_center_point, edge_points.back_right_point, - edge_points.back_left_point, prediction_time_horizon_.pedestrian * 2.0, - min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) { - PredictedPath predicted_path = - path_generator_->generatePathToTargetPoint(object, edge_points.back_center_point); - predicted_path.confidence = 1.0; - predicted_object.kinematics.predicted_paths.push_back(predicted_path); - } - } - } - - // try to find the edge points for other surrounding crosswalks and generate path to the crosswalk - // edge - for (const auto & crosswalk : surrounding_crosswalks) { - if (crossing_crosswalk && crossing_crosswalk.get() == crosswalk) { - continue; - } - const auto crosswalk_signal_id_opt = getTrafficSignalId(crosswalk); - if (crosswalk_signal_id_opt.has_value() && use_crosswalk_signal_) { - if (!calcIntentionToCrossWithTrafficSignal( - object, crosswalk, crosswalk_signal_id_opt.value())) { - continue; - } - } - - const auto edge_points = getCrosswalkEdgePoints(crosswalk); - - const auto reachable_first = hasPotentialToReach( - object, edge_points.front_center_point, edge_points.front_right_point, - edge_points.front_left_point, prediction_time_horizon_.pedestrian, - min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_); - const auto reachable_second = hasPotentialToReach( - object, edge_points.back_center_point, edge_points.back_right_point, - edge_points.back_left_point, prediction_time_horizon_.pedestrian, - min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_); - - if (!reachable_first && !reachable_second) { - continue; - } - - const auto reachable_crosswalk = isReachableCrosswalkEdgePoints( - object, surrounding_lanelets, surrounding_crosswalks, edge_points, - prediction_time_horizon_.pedestrian, min_crosswalk_user_velocity_); - - if (!reachable_crosswalk) { - continue; - } - - PredictedPath predicted_path = path_generator_->generatePathForCrosswalkUser( - object, reachable_crosswalk.get(), prediction_time_horizon_.pedestrian); - predicted_path.confidence = 1.0; - - if (predicted_path.path.empty()) { - continue; - } - // If the predicted path to the crosswalk is crossing the fence, don't use it - if (doesPathCrossAnyFence(predicted_path)) { - continue; - } - - predicted_object.kinematics.predicted_paths.push_back(predicted_path); - } - - const auto n_path = predicted_object.kinematics.predicted_paths.size(); - for (auto & predicted_path : predicted_object.kinematics.predicted_paths) { - predicted_path.confidence = 1.0 / n_path; - } - - return predicted_object; -} - void MapBasedPredictionNode::updateObjectData(TrackedObject & object) { std::unique_ptr st_ptr; @@ -1607,23 +913,6 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object) return; } -void MapBasedPredictionNode::removeStaleTrafficLightInfo( - const TrackedObjects::ConstSharedPtr in_objects) -{ - for (auto it = stopped_times_against_green_.begin(); it != stopped_times_against_green_.end();) { - const bool isDisappeared = std::none_of( - in_objects->objects.begin(), in_objects->objects.end(), - [&it](autoware_perception_msgs::msg::TrackedObject obj) { - return autoware::universe_utils::toHexString(obj.object_id) == it->first.first; - }); - if (isDisappeared) { - it = stopped_times_against_green_.erase(it); - } else { - ++it; - } - } -} - LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & object) { std::unique_ptr st_ptr; @@ -1730,9 +1019,9 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition( // If the object is in the objects history, we check if the target lanelet is // inside the current lanelets id or following lanelets const std::string object_id = autoware::universe_utils::toHexString(object.object_id); - if (road_users_history.count(object_id) != 0) { + if (road_users_history_.count(object_id) != 0) { const std::vector & possible_lanelet = - road_users_history.at(object_id).back().future_possible_lanelets; + road_users_history_.at(object_id).back().future_possible_lanelets; bool not_in_possible_lanelet = std::find(possible_lanelet.begin(), possible_lanelet.end(), lanelet.second) == @@ -1830,13 +1119,13 @@ void MapBasedPredictionNode::updateRoadUsersHistory( single_object_data.lateral_kinematics_set[current_lane] = lateral_kinematics; } - if (road_users_history.count(object_id) == 0) { + if (road_users_history_.count(object_id) == 0) { // New Object(Create a new object in object histories) std::deque object_data = {single_object_data}; - road_users_history.emplace(object_id, object_data); + road_users_history_.emplace(object_id, object_data); } else { // Object that is already in the object buffer - std::deque & object_data = road_users_history.at(object_id); + std::deque & object_data = road_users_history_.at(object_id); // get previous object data and update const auto prev_object_data = object_data.back(); updateLateralKinematicsVector( @@ -2041,9 +1330,9 @@ std::vector MapBasedPredictionNode::getPredictedReferen } // update future possible lanelets - if (road_users_history.count(object_id) != 0) { + if (road_users_history_.count(object_id) != 0) { std::vector & possible_lanelets = - road_users_history.at(object_id).back().future_possible_lanelets; + road_users_history_.at(object_id).back().future_possible_lanelets; for (const auto & ref_path : lanelet_ref_paths) { for (const auto & lanelet : ref_path.first) { if ( @@ -2081,10 +1370,10 @@ Maneuver MapBasedPredictionNode::predictObjectManeuver( throw std::logic_error("Lane change detection method is invalid."); }(); - if (road_users_history.count(object_id) == 0) { + if (road_users_history_.count(object_id) == 0) { return current_maneuver; } - auto & object_info = road_users_history.at(object_id); + auto & object_info = road_users_history_.at(object_id); // update maneuver in object history if (!object_info.empty()) { @@ -2122,11 +1411,11 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByTimeToLaneChange( if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); // Step1. Check if we have the object in the buffer - if (road_users_history.count(object_id) == 0) { + if (road_users_history_.count(object_id) == 0) { return Maneuver::LANE_FOLLOW; } - const std::deque & object_info = road_users_history.at(object_id); + const std::deque & object_info = road_users_history_.at(object_id); // Step2. Check if object history length longer than history_time_length const int latest_id = static_cast(object_info.size()) - 1; @@ -2195,11 +1484,11 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance( if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); // Step1. Check if we have the object in the buffer - if (road_users_history.count(object_id) == 0) { + if (road_users_history_.count(object_id) == 0) { return Maneuver::LANE_FOLLOW; } - const std::deque & object_info = road_users_history.at(object_id); + const std::deque & object_info = road_users_history_.at(object_id); const double current_time = (this->get_clock()->now()).seconds(); // Step2. Get the previous id @@ -2352,29 +1641,6 @@ double MapBasedPredictionNode::calcLeftLateralOffset( return -calcRightLateralOffset(boundary_line, search_pose); } -void MapBasedPredictionNode::updateFuturePossibleLanelets( - const std::string & object_id, const lanelet::routing::LaneletPaths & paths) -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - - if (road_users_history.count(object_id) == 0) { - return; - } - - std::vector & possible_lanelets = - road_users_history.at(object_id).back().future_possible_lanelets; - for (const auto & path : paths) { - for (const auto & lanelet : path) { - bool not_in_buffer = std::find(possible_lanelets.begin(), possible_lanelets.end(), lanelet) == - possible_lanelets.end(); - if (not_in_buffer) { - possible_lanelets.push_back(lanelet); - } - } - } -} - ManeuverProbability MapBasedPredictionNode::calculateManeuverProbability( const Maneuver & predicted_maneuver, const bool & left_paths_exists, const bool & right_paths_exists, const bool & center_paths_exists) const @@ -2722,107 +1988,6 @@ bool MapBasedPredictionNode::isDuplicated( return false; } -std::optional MapBasedPredictionNode::getTrafficSignalId( - const lanelet::ConstLanelet & way_lanelet) -{ - const auto traffic_light_reg_elems = - way_lanelet.regulatoryElementsAs(); - if (traffic_light_reg_elems.empty()) { - return std::nullopt; - } else if (traffic_light_reg_elems.size() > 1) { - RCLCPP_ERROR( - get_logger(), - "[Map Based Prediction]: " - "Multiple regulatory elements as TrafficLight are defined to one lanelet object."); - } - return traffic_light_reg_elems.front()->id(); -} - -std::optional MapBasedPredictionNode::getTrafficSignalElement( - const lanelet::Id & id) -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - - if (traffic_signal_id_map_.count(id) != 0) { - const auto & signal_elements = traffic_signal_id_map_.at(id).elements; - if (signal_elements.size() > 1) { - RCLCPP_ERROR( - get_logger(), "[Map Based Prediction]: Multiple TrafficSignalElement_ are received."); - } else if (!signal_elements.empty()) { - return signal_elements.front(); - } - } - return std::nullopt; -} - -bool MapBasedPredictionNode::calcIntentionToCrossWithTrafficSignal( - const TrackedObject & object, const lanelet::ConstLanelet & crosswalk, - const lanelet::Id & signal_id) -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - - const auto signal_color = [&] { - const auto elem_opt = getTrafficSignalElement(signal_id); - return elem_opt ? elem_opt.value().color : TrafficLightElement::UNKNOWN; - }(); - - const auto key = - std::make_pair(autoware::universe_utils::toHexString(object.object_id), signal_id); - if ( - signal_color == TrafficLightElement::GREEN && - autoware::universe_utils::calcNorm(object.kinematics.twist_with_covariance.twist.linear) < - threshold_velocity_assumed_as_stopping_) { - stopped_times_against_green_.try_emplace(key, this->get_clock()->now()); - - const auto timeout_no_intention_to_walk = [&]() { - auto InterpolateMap = []( - const std::vector & key_set, - const std::vector & value_set, const double query) { - if (query <= key_set.front()) { - return value_set.front(); - } else if (query >= key_set.back()) { - return value_set.back(); - } - for (size_t i = 0; i < key_set.size() - 1; ++i) { - if (key_set.at(i) <= query && query <= key_set.at(i + 1)) { - auto ratio = - (query - key_set.at(i)) / std::max(key_set.at(i + 1) - key_set.at(i), 1.0e-5); - ratio = std::clamp(ratio, 0.0, 1.0); - return value_set.at(i) + ratio * (value_set.at(i + 1) - value_set.at(i)); - } - } - return value_set.back(); - }; - - const auto obj_position = object.kinematics.pose_with_covariance.pose.position; - const double distance_to_crosswalk = boost::geometry::distance( - crosswalk.polygon2d().basicPolygon(), - lanelet::BasicPoint2d(obj_position.x, obj_position.y)); - return InterpolateMap( - distance_set_for_no_intention_to_walk_, timeout_set_for_no_intention_to_walk_, - distance_to_crosswalk); - }(); - - if ( - (this->get_clock()->now() - stopped_times_against_green_.at(key)).seconds() > - timeout_no_intention_to_walk) { - return false; - } - - } else { - stopped_times_against_green_.erase(key); - // If the pedestrian disappears, another function erases the old data. - } - - if (signal_color == TrafficLightElement::RED) { - return false; - } - - return true; -} - } // namespace autoware::map_based_prediction #include diff --git a/perception/autoware_map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp index a2a1b8b3d3fda..57104082d0cf7 100644 --- a/perception/autoware_map_based_prediction/src/path_generator.cpp +++ b/perception/autoware_map_based_prediction/src/path_generator.cpp @@ -25,6 +25,12 @@ namespace autoware::map_based_prediction { using autoware::universe_utils::ScopedTimeTrack; +PathGenerator::PathGenerator(const double sampling_time_interval) +: sampling_time_interval_(sampling_time_interval) +{ + min_crosswalk_user_velocity_ = 0.1; +} + PathGenerator::PathGenerator( const double sampling_time_interval, const double min_crosswalk_user_velocity) : sampling_time_interval_(sampling_time_interval), diff --git a/perception/autoware_map_based_prediction/src/predictor_vru.cpp b/perception/autoware_map_based_prediction/src/predictor_vru.cpp new file mode 100644 index 0000000000000..31630f7645171 --- /dev/null +++ b/perception/autoware_map_based_prediction/src/predictor_vru.cpp @@ -0,0 +1,706 @@ +// Copyright 2024 TIER IV, inc. +// +// 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 "map_based_prediction/predictor_vru.hpp" + +#include "map_based_prediction/utils.hpp" + +#include +#include + +namespace autoware::map_based_prediction +{ +using autoware::universe_utils::ScopedTimeTrack; + +namespace +{ +boost::optional isReachableCrosswalkEdgePoints( + const TrackedObject & object, const lanelet::ConstLanelets & surrounding_lanelets, + const lanelet::ConstLanelets & surrounding_crosswalks, const CrosswalkEdgePoints & edge_points, + const double time_horizon, const double min_object_vel) +{ + using Point = boost::geometry::model::d2::point_xy; + + const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; + const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; + const auto yaw = autoware::universe_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; + + lanelet::BasicPoint2d obj_pos_as_lanelet(obj_pos.x, obj_pos.y); + + const auto & p1 = edge_points.front_center_point; + const auto & p2 = edge_points.back_center_point; + + CrosswalkEdgePoints ret{p1, {}, {}, p2, {}, {}}; + auto distance_pedestrian_to_p1 = std::hypot(p1.x() - obj_pos.x, p1.y() - obj_pos.y); + auto distance_pedestrian_to_p2 = std::hypot(p2.x() - obj_pos.x, p2.y() - obj_pos.y); + + if (distance_pedestrian_to_p2 < distance_pedestrian_to_p1) { + ret.swap(); + std::swap(distance_pedestrian_to_p1, distance_pedestrian_to_p2); + } + + constexpr double stop_velocity_th = 0.14; // [m/s] + const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); + const auto is_stop_object = estimated_velocity < stop_velocity_th; + const auto velocity = std::max(min_object_vel, estimated_velocity); + + const auto isAcrossAnyRoad = [&surrounding_lanelets, &surrounding_crosswalks]( + const Point & p_src, const Point & p_dst) { + const auto withinAnyCrosswalk = [&surrounding_crosswalks](const Point & p) { + for (const auto & crosswalk : surrounding_crosswalks) { + if (boost::geometry::within(p, crosswalk.polygon2d().basicPolygon())) { + return true; + } + } + return false; + }; + + const auto isExist = [](const Point & p, const std::vector & points) { + for (const auto & existingPoint : points) { + if (boost::geometry::distance(p, existingPoint) < 1e-1) { + return true; + } + } + return false; + }; + + std::vector points_of_intersect; + const boost::geometry::model::linestring line{p_src, p_dst}; + for (const auto & lanelet : surrounding_lanelets) { + const lanelet::Attribute attr = lanelet.attribute(lanelet::AttributeName::Subtype); + if (attr.value() != lanelet::AttributeValueString::Road) { + continue; + } + + std::vector tmp_intersects; + boost::geometry::intersection(line, lanelet.polygon2d().basicPolygon(), tmp_intersects); + for (const auto & p : tmp_intersects) { + if (isExist(p, points_of_intersect) || withinAnyCrosswalk(p)) { + continue; + } + points_of_intersect.push_back(p); + if (points_of_intersect.size() >= 2) { + return true; + } + } + } + return false; + }; + + const bool first_intersects_road = isAcrossAnyRoad( + {obj_pos.x, obj_pos.y}, {ret.front_center_point.x(), ret.front_center_point.y()}); + const bool second_intersects_road = + isAcrossAnyRoad({obj_pos.x, obj_pos.y}, {ret.back_center_point.x(), ret.back_center_point.y()}); + + if (first_intersects_road && second_intersects_road) { + return {}; + } + + if (first_intersects_road && !second_intersects_road) { + ret.swap(); + } + + const Eigen::Vector2d pedestrian_to_crosswalk( + (ret.front_center_point.x() + ret.back_center_point.x()) / 2.0 - obj_pos.x, + (ret.front_center_point.y() + ret.back_center_point.y()) / 2.0 - obj_pos.y); + const Eigen::Vector2d pedestrian_heading_direction( + obj_vel.x * std::cos(yaw), obj_vel.x * std::sin(yaw)); + const auto reachable = + std::min(distance_pedestrian_to_p1, distance_pedestrian_to_p2) < velocity * time_horizon; + const auto heading_for_crosswalk = + pedestrian_to_crosswalk.dot(pedestrian_heading_direction) > 0.0; + + if ((reachable && heading_for_crosswalk) || (reachable && is_stop_object)) { + return ret; + } + + return {}; +} + +bool hasPotentialToReach( + const TrackedObject & object, const Eigen::Vector2d & center_point, + const Eigen::Vector2d & right_point, const Eigen::Vector2d & left_point, + const double time_horizon, const double min_object_vel, + const double max_crosswalk_user_delta_yaw_threshold_for_lanelet) +{ + const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; + const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; + const auto yaw = autoware::universe_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; + + constexpr double stop_velocity_th = 0.14; // [m/s] + const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); + const auto is_stop_object = estimated_velocity < stop_velocity_th; + const auto velocity = std::max(min_object_vel, estimated_velocity); + + const double pedestrian_to_crosswalk_center_direction = + std::atan2(center_point.y() - obj_pos.y, center_point.x() - obj_pos.x); + + const auto + [pedestrian_to_crosswalk_right_rel_direction, pedestrian_to_crosswalk_left_rel_direction] = + [&]() { + const double pedestrian_to_crosswalk_right_direction = + std::atan2(right_point.y() - obj_pos.y, right_point.x() - obj_pos.x); + const double pedestrian_to_crosswalk_left_direction = + std::atan2(left_point.y() - obj_pos.y, left_point.x() - obj_pos.x); + return std::make_pair( + autoware::universe_utils::normalizeRadian( + pedestrian_to_crosswalk_right_direction - pedestrian_to_crosswalk_center_direction), + autoware::universe_utils::normalizeRadian( + pedestrian_to_crosswalk_left_direction - pedestrian_to_crosswalk_center_direction)); + }(); + + const double pedestrian_heading_rel_direction = [&]() { + const double pedestrian_heading_direction = + std::atan2(obj_vel.x * std::sin(yaw), obj_vel.x * std::cos(yaw)); + return autoware::universe_utils::normalizeRadian( + pedestrian_heading_direction - pedestrian_to_crosswalk_center_direction); + }(); + + const double pedestrian_to_crosswalk_min_rel_direction = std::min( + pedestrian_to_crosswalk_right_rel_direction, pedestrian_to_crosswalk_left_rel_direction); + const double pedestrian_to_crosswalk_max_rel_direction = std::max( + pedestrian_to_crosswalk_right_rel_direction, pedestrian_to_crosswalk_left_rel_direction); + const double pedestrian_vel_angle_against_crosswalk = [&]() { + if (pedestrian_heading_rel_direction < pedestrian_to_crosswalk_min_rel_direction) { + return pedestrian_to_crosswalk_min_rel_direction - pedestrian_heading_rel_direction; + } + if (pedestrian_to_crosswalk_max_rel_direction < pedestrian_heading_rel_direction) { + return pedestrian_to_crosswalk_max_rel_direction - pedestrian_heading_rel_direction; + } + return 0.0; + }(); + const auto heading_for_crosswalk = std::abs(pedestrian_vel_angle_against_crosswalk) < + max_crosswalk_user_delta_yaw_threshold_for_lanelet; + const auto reachable = std::hypot(center_point.x() - obj_pos.x, center_point.y() - obj_pos.y) < + velocity * time_horizon; + + if (reachable && (heading_for_crosswalk || is_stop_object)) { + return true; + } + + return false; +} + +CrosswalkEdgePoints getCrosswalkEdgePoints(const lanelet::ConstLanelet & crosswalk) +{ + const Eigen::Vector2d r_p_front = crosswalk.rightBound().front().basicPoint2d(); + const Eigen::Vector2d l_p_front = crosswalk.leftBound().front().basicPoint2d(); + const Eigen::Vector2d front_center_point = (r_p_front + l_p_front) / 2.0; + + const Eigen::Vector2d r_p_back = crosswalk.rightBound().back().basicPoint2d(); + const Eigen::Vector2d l_p_back = crosswalk.leftBound().back().basicPoint2d(); + const Eigen::Vector2d back_center_point = (r_p_back + l_p_back) / 2.0; + + return CrosswalkEdgePoints{front_center_point, r_p_front, l_p_front, + back_center_point, r_p_back, l_p_back}; +} + +bool isIntersecting( + const geometry_msgs::msg::Point & point1, const geometry_msgs::msg::Point & point2, + const lanelet::ConstPoint3d & point3, const lanelet::ConstPoint3d & point4) +{ + const auto p1 = autoware::universe_utils::createPoint(point1.x, point1.y, 0.0); + const auto p2 = autoware::universe_utils::createPoint(point2.x, point2.y, 0.0); + const auto p3 = autoware::universe_utils::createPoint(point3.x(), point3.y(), 0.0); + const auto p4 = autoware::universe_utils::createPoint(point4.x(), point4.y(), 0.0); + const auto intersection = autoware::universe_utils::intersect(p1, p2, p3, p4); + return intersection.has_value(); +} + +bool doesPathCrossFence( + const PredictedPath & predicted_path, const lanelet::ConstLineString3d & fence_line) +{ + // check whether the predicted path cross with fence + for (size_t i = 0; i < predicted_path.path.size() - 1; ++i) { + for (size_t j = 0; j < fence_line.size() - 1; ++j) { + if (isIntersecting( + predicted_path.path[i].position, predicted_path.path[i + 1].position, fence_line[j], + fence_line[j + 1])) { + return true; + } + } + } + return false; +} + +} // namespace + +void PredictorVru::initialize() +{ + current_crosswalk_users_.clear(); + predicted_crosswalk_users_ids_.clear(); +} + +void PredictorVru::setTrafficSignal(const TrafficLightGroupArray & traffic_signal) +{ + traffic_signal_id_map_.clear(); + for (const auto & signal : traffic_signal.traffic_light_groups) { + traffic_signal_id_map_[signal.traffic_light_group_id] = signal; + } +} + +void PredictorVru::setLaneletMap(std::shared_ptr lanelet_map_ptr) +{ + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + lanelet_map_ptr_ = std::move(lanelet_map_ptr); + + const auto all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); + const auto crosswalks = lanelet::utils::query::crosswalkLanelets(all_lanelets); + const auto walkways = lanelet::utils::query::walkwayLanelets(all_lanelets); + crosswalks_.insert(crosswalks_.end(), crosswalks.begin(), crosswalks.end()); + crosswalks_.insert(crosswalks_.end(), walkways.begin(), walkways.end()); + + lanelet::LineStrings3d fences; + for (const auto & linestring : lanelet_map_ptr_->lineStringLayer) { + if (const std::string type = linestring.attributeOr(lanelet::AttributeName::Type, "none"); + type == "fence") { + fences.push_back(lanelet::LineString3d( + std::const_pointer_cast(linestring.constData()))); + } + } + fence_layer_ = lanelet::utils::createMap(fences); +} + +bool PredictorVru::doesPathCrossAnyFence(const PredictedPath & predicted_path) +{ + lanelet::BasicLineString2d predicted_path_ls; + for (const auto & p : predicted_path.path) + predicted_path_ls.emplace_back(p.position.x, p.position.y); + const auto candidates = + fence_layer_->lineStringLayer.search(lanelet::geometry::boundingBox2d(predicted_path_ls)); + for (const auto & candidate : candidates) { + if (doesPathCrossFence(predicted_path, candidate)) { + return true; + } + } + return false; +} + +void PredictorVru::loadCurrentCrosswalkUsers(const TrackedObjects & objects) +{ + if (!lanelet_map_ptr_) { + return; + } + + // removeStaleTrafficLightInfo + for (auto it = stopped_times_against_green_.begin(); it != stopped_times_against_green_.end();) { + const bool isDisappeared = std::none_of( + objects.objects.begin(), objects.objects.end(), + [&it](autoware_perception_msgs::msg::TrackedObject obj) { + return autoware::universe_utils::toHexString(obj.object_id) == it->first.first; + }); + if (isDisappeared) { + it = stopped_times_against_green_.erase(it); + } else { + ++it; + } + } + + // + initialize(); + + // load current crosswalk users + for (const auto & object : objects.objects) { + const auto label_for_prediction = utils::changeLabelForPrediction( + object.classification.front().label, object, lanelet_map_ptr_); + if ( + label_for_prediction == ObjectClassification::PEDESTRIAN || + label_for_prediction == ObjectClassification::BICYCLE) { + const std::string object_id = autoware::universe_utils::toHexString(object.object_id); + current_crosswalk_users_.emplace(object_id, object); + } + } +} + +void PredictorVru::removeOldKnownMatches(const double current_time, const double buffer_time) +{ + auto invalidated_crosswalk_users = + utils::removeOldObjectsHistory(current_time, buffer_time, crosswalk_users_history_); + // delete matches that point to invalid object + for (auto it = known_matches_.begin(); it != known_matches_.end();) { + if (invalidated_crosswalk_users.count(it->second)) { + it = known_matches_.erase(it); + } else { + ++it; + } + } +} + +PredictedObject PredictorVru::predict( + const std_msgs::msg::Header & header, const TrackedObject & object) +{ + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + std::string object_id = autoware::universe_utils::toHexString(object.object_id); + if (match_lost_and_appeared_crosswalk_users_) { + object_id = tryMatchNewObjectToDisappeared(object_id, current_crosswalk_users_); + } + predicted_crosswalk_users_ids_.insert(object_id); + updateCrosswalkUserHistory(header, object, object_id); + return getPredictedObjectAsCrosswalkUser(object); +} + +PredictedObjects PredictorVru::retrieveUndetectedObjects() +{ + PredictedObjects output; + for (const auto & [id, crosswalk_user] : crosswalk_users_history_) { + // get a predicted path for crosswalk users in history who didn't get path yet using latest + // message + if (predicted_crosswalk_users_ids_.count(id) == 0) { + const auto predicted_object = + getPredictedObjectAsCrosswalkUser(crosswalk_user.back().tracked_object); + output.objects.push_back(predicted_object); + } + } + return output; +} + +PredictedObject PredictorVru::getPredictedObjectAsCrosswalkUser(const TrackedObject & object) +{ + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + auto predicted_object = utils::convertToPredictedObject(object); + { + PredictedPath predicted_path = + path_generator_->generatePathForNonVehicleObject(object, prediction_time_horizon_); + predicted_path.confidence = 1.0; + + predicted_object.kinematics.predicted_paths.push_back(predicted_path); + } + + boost::optional crossing_crosswalk{boost::none}; + const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; + const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; + const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); + const auto velocity = std::max(min_crosswalk_user_velocity_, estimated_velocity); + const auto surrounding_lanelets_with_dist = lanelet::geometry::findWithin2d( + lanelet_map_ptr_->laneletLayer, lanelet::BasicPoint2d{obj_pos.x, obj_pos.y}, + prediction_time_horizon_ * velocity); + lanelet::ConstLanelets surrounding_lanelets; + lanelet::ConstLanelets surrounding_crosswalks; + for (const auto & [dist, lanelet] : surrounding_lanelets_with_dist) { + surrounding_lanelets.push_back(lanelet); + const auto attr = lanelet.attribute(lanelet::AttributeName::Subtype); + if ( + attr.value() == lanelet::AttributeValueString::Crosswalk || + attr.value() == lanelet::AttributeValueString::Walkway) { + const auto & crosswalk = lanelet; + surrounding_crosswalks.push_back(crosswalk); + if (std::abs(dist) < 1e-5) { + crossing_crosswalk = crosswalk; + } + } + } + + // If the object is in the crosswalk, generate path to the crosswalk edge + if (crossing_crosswalk) { + const auto edge_points = getCrosswalkEdgePoints(crossing_crosswalk.get()); + + if (hasPotentialToReach( + object, edge_points.front_center_point, edge_points.front_right_point, + edge_points.front_left_point, std::numeric_limits::max(), + min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) { + PredictedPath predicted_path = + path_generator_->generatePathToTargetPoint(object, edge_points.front_center_point); + predicted_path.confidence = 1.0; + predicted_object.kinematics.predicted_paths.push_back(predicted_path); + } + + if (hasPotentialToReach( + object, edge_points.back_center_point, edge_points.back_right_point, + edge_points.back_left_point, std::numeric_limits::max(), + min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) { + PredictedPath predicted_path = + path_generator_->generatePathToTargetPoint(object, edge_points.back_center_point); + predicted_path.confidence = 1.0; + predicted_object.kinematics.predicted_paths.push_back(predicted_path); + } + + // If the object is not crossing the crosswalk, in the road lanelets, try to find the closest + // crosswalk and generate path to the crosswalk edge + } else if (utils::withinRoadLanelet(object, surrounding_lanelets_with_dist)) { + lanelet::ConstLanelet closest_crosswalk{}; + const auto & obj_pose = object.kinematics.pose_with_covariance.pose; + const auto found_closest_crosswalk = + lanelet::utils::query::getClosestLanelet(crosswalks_, obj_pose, &closest_crosswalk); + + if (found_closest_crosswalk) { + const auto edge_points = getCrosswalkEdgePoints(closest_crosswalk); + + if (hasPotentialToReach( + object, edge_points.front_center_point, edge_points.front_right_point, + edge_points.front_left_point, prediction_time_horizon_ * 2.0, + min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) { + PredictedPath predicted_path = + path_generator_->generatePathToTargetPoint(object, edge_points.front_center_point); + predicted_path.confidence = 1.0; + predicted_object.kinematics.predicted_paths.push_back(predicted_path); + } + + if (hasPotentialToReach( + object, edge_points.back_center_point, edge_points.back_right_point, + edge_points.back_left_point, prediction_time_horizon_ * 2.0, + min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) { + PredictedPath predicted_path = + path_generator_->generatePathToTargetPoint(object, edge_points.back_center_point); + predicted_path.confidence = 1.0; + predicted_object.kinematics.predicted_paths.push_back(predicted_path); + } + } + } + + // try to find the edge points for other surrounding crosswalks and generate path to the crosswalk + // edge + for (const auto & crosswalk : surrounding_crosswalks) { + if (crossing_crosswalk && crossing_crosswalk.get() == crosswalk) { + continue; + } + const auto crosswalk_signal_id_opt = getTrafficSignalId(crosswalk); + if (crosswalk_signal_id_opt.has_value() && use_crosswalk_signal_) { + if (!calcIntentionToCrossWithTrafficSignal( + object, crosswalk, crosswalk_signal_id_opt.value())) { + continue; + } + } + + const auto edge_points = getCrosswalkEdgePoints(crosswalk); + + const auto reachable_first = hasPotentialToReach( + object, edge_points.front_center_point, edge_points.front_right_point, + edge_points.front_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, + max_crosswalk_user_delta_yaw_threshold_for_lanelet_); + const auto reachable_second = hasPotentialToReach( + object, edge_points.back_center_point, edge_points.back_right_point, + edge_points.back_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, + max_crosswalk_user_delta_yaw_threshold_for_lanelet_); + + if (!reachable_first && !reachable_second) { + continue; + } + + const auto reachable_crosswalk = isReachableCrosswalkEdgePoints( + object, surrounding_lanelets, surrounding_crosswalks, edge_points, prediction_time_horizon_, + min_crosswalk_user_velocity_); + + if (!reachable_crosswalk) { + continue; + } + + PredictedPath predicted_path = path_generator_->generatePathForCrosswalkUser( + object, reachable_crosswalk.get(), prediction_time_horizon_); + predicted_path.confidence = 1.0; + + if (predicted_path.path.empty()) { + continue; + } + // If the predicted path to the crosswalk is crossing the fence, don't use it + if (doesPathCrossAnyFence(predicted_path)) { + continue; + } + + predicted_object.kinematics.predicted_paths.push_back(predicted_path); + } + + const auto n_path = predicted_object.kinematics.predicted_paths.size(); + for (auto & predicted_path : predicted_object.kinematics.predicted_paths) { + predicted_path.confidence = 1.0 / n_path; + } + + return predicted_object; +} + +std::optional PredictorVru::getTrafficSignalId( + const lanelet::ConstLanelet & way_lanelet) +{ + const auto traffic_light_reg_elems = + way_lanelet.regulatoryElementsAs(); + if (traffic_light_reg_elems.empty()) { + return std::nullopt; + } else if (traffic_light_reg_elems.size() > 1) { + RCLCPP_ERROR( + node_.get_logger(), + "[Map Based Prediction]: " + "Multiple regulatory elements as TrafficLight are defined to one lanelet object."); + } + return traffic_light_reg_elems.front()->id(); +} + +void PredictorVru::updateCrosswalkUserHistory( + const std_msgs::msg::Header & header, const TrackedObject & object, const std::string & object_id) +{ + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + CrosswalkUserData crosswalk_user_data; + crosswalk_user_data.header = header; + crosswalk_user_data.tracked_object = object; + + if (crosswalk_users_history_.count(object_id) == 0) { + crosswalk_users_history_.emplace(object_id, std::deque{crosswalk_user_data}); + return; + } + + crosswalk_users_history_.at(object_id).push_back(crosswalk_user_data); +} + +std::string PredictorVru::tryMatchNewObjectToDisappeared( + const std::string & object_id, std::unordered_map & current_users) +{ + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + const auto known_match_opt = [&]() -> std::optional { + if (!known_matches_.count(object_id)) { + return std::nullopt; + } + + std::string match_id = known_matches_[object_id]; + // object in the history is already matched to something (possibly itself) + if (crosswalk_users_history_.count(match_id)) { + // avoid matching two appeared users to one user in history + current_users[match_id] = crosswalk_users_history_[match_id].back().tracked_object; + return match_id; + } else { + RCLCPP_WARN_STREAM( + node_.get_logger(), "Crosswalk user was " + << object_id << "was matched to " << match_id + << " but history for the crosswalk user was deleted. Rematching"); + } + return std::nullopt; + }(); + // early return if the match is already known + if (known_match_opt.has_value()) { + return known_match_opt.value(); + } + + std::string match_id = object_id; + double best_score = std::numeric_limits::max(); + const auto object_pos = current_users[object_id].kinematics.pose_with_covariance.pose.position; + for (const auto & [user_id, user_history] : crosswalk_users_history_) { + // user present in current_users and will be matched to itself + if (current_users.count(user_id)) { + continue; + } + // TODO(dkoldaev): implement more sophisticated scoring, for now simply dst to last position in + // history + const auto match_candidate_pos = + user_history.back().tracked_object.kinematics.pose_with_covariance.pose.position; + const double score = + std::hypot(match_candidate_pos.x - object_pos.x, match_candidate_pos.y - object_pos.y); + if (score < best_score) { + best_score = score; + match_id = user_id; + } + } + + if (object_id != match_id) { + RCLCPP_INFO_STREAM( + node_.get_logger(), "[Map Based Prediction]: Matched " << object_id << " to " << match_id); + // avoid matching two appeared users to one user in history + current_users[match_id] = crosswalk_users_history_[match_id].back().tracked_object; + } + + known_matches_[object_id] = match_id; + return match_id; +} + +bool PredictorVru::calcIntentionToCrossWithTrafficSignal( + const TrackedObject & object, const lanelet::ConstLanelet & crosswalk, + const lanelet::Id & signal_id) +{ + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + const auto signal_color = [&] { + const auto elem_opt = getTrafficSignalElement(signal_id); + return elem_opt ? elem_opt.value().color : TrafficLightElement::UNKNOWN; + }(); + + const auto key = + std::make_pair(autoware::universe_utils::toHexString(object.object_id), signal_id); + if ( + signal_color == TrafficLightElement::GREEN && + autoware::universe_utils::calcNorm(object.kinematics.twist_with_covariance.twist.linear) < + threshold_velocity_assumed_as_stopping_) { + stopped_times_against_green_.try_emplace(key, node_.get_clock()->now()); + + const auto timeout_no_intention_to_walk = [&]() { + auto InterpolateMap = []( + const std::vector & key_set, + const std::vector & value_set, const double query) { + if (query <= key_set.front()) { + return value_set.front(); + } else if (query >= key_set.back()) { + return value_set.back(); + } + for (size_t i = 0; i < key_set.size() - 1; ++i) { + if (key_set.at(i) <= query && query <= key_set.at(i + 1)) { + auto ratio = + (query - key_set.at(i)) / std::max(key_set.at(i + 1) - key_set.at(i), 1.0e-5); + ratio = std::clamp(ratio, 0.0, 1.0); + return value_set.at(i) + ratio * (value_set.at(i + 1) - value_set.at(i)); + } + } + return value_set.back(); + }; + + const auto obj_position = object.kinematics.pose_with_covariance.pose.position; + const double distance_to_crosswalk = boost::geometry::distance( + crosswalk.polygon2d().basicPolygon(), + lanelet::BasicPoint2d(obj_position.x, obj_position.y)); + return InterpolateMap( + distance_set_for_no_intention_to_walk_, timeout_set_for_no_intention_to_walk_, + distance_to_crosswalk); + }(); + + if ( + (node_.get_clock()->now() - stopped_times_against_green_.at(key)).seconds() > + timeout_no_intention_to_walk) { + return false; + } + + } else { + stopped_times_against_green_.erase(key); + // If the pedestrian disappears, another function erases the old data. + } + + if (signal_color == TrafficLightElement::RED) { + return false; + } + + return true; +} + +std::optional PredictorVru::getTrafficSignalElement(const lanelet::Id & id) +{ + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + if (traffic_signal_id_map_.count(id) != 0) { + const auto & signal_elements = traffic_signal_id_map_.at(id).elements; + if (signal_elements.size() > 1) { + RCLCPP_ERROR( + node_.get_logger(), "[Map Based Prediction]: Multiple TrafficSignalElement_ are received."); + } else if (!signal_elements.empty()) { + return signal_elements.front(); + } + } + return std::nullopt; +} + +} // namespace autoware::map_based_prediction diff --git a/perception/autoware_map_based_prediction/src/utils.cpp b/perception/autoware_map_based_prediction/src/utils.cpp new file mode 100644 index 0000000000000..c9ebd9667659a --- /dev/null +++ b/perception/autoware_map_based_prediction/src/utils.cpp @@ -0,0 +1,222 @@ +// Copyright 2024 TIER IV, inc. +// +// 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 "map_based_prediction/utils.hpp" + +namespace autoware::map_based_prediction +{ +namespace utils +{ + +/** + * @brief calc absolute normalized yaw difference between lanelet and object + * + * @param object + * @param lanelet + * @return double + */ +double calcAbsYawDiffBetweenLaneletAndObject( + const TrackedObject & object, const lanelet::ConstLanelet & lanelet) +{ + const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double lane_yaw = + lanelet::utils::getLaneletAngle(lanelet, object.kinematics.pose_with_covariance.pose.position); + const double delta_yaw = object_yaw - lane_yaw; + const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw); + const double abs_norm_delta = std::fabs(normalized_delta_yaw); + return abs_norm_delta; +} + +bool withinRoadLanelet( + const TrackedObject & object, + const std::vector> & surrounding_lanelets_with_dist, + const bool use_yaw_information) +{ + for (const auto & [dist, lanelet] : surrounding_lanelets_with_dist) { + if (lanelet.hasAttribute(lanelet::AttributeName::Subtype)) { + lanelet::Attribute attr = lanelet.attribute(lanelet::AttributeName::Subtype); + if ( + attr.value() == lanelet::AttributeValueString::Crosswalk || + attr.value() == lanelet::AttributeValueString::Walkway) { + continue; + } + } + + constexpr float yaw_threshold = 0.6; + bool within_lanelet = std::abs(dist) < 1e-5; + if (use_yaw_information) { + within_lanelet = + within_lanelet && calcAbsYawDiffBetweenLaneletAndObject(object, lanelet) < yaw_threshold; + } + if (within_lanelet) { + return true; + } + } + + return false; +} + +bool withinRoadLanelet( + const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const bool use_yaw_information) +{ + const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; + lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y); + // nearest lanelet + constexpr double search_radius = 10.0; // [m] + const auto surrounding_lanelets_with_dist = + lanelet::geometry::findWithin2d(lanelet_map_ptr->laneletLayer, search_point, search_radius); + + return withinRoadLanelet(object, surrounding_lanelets_with_dist, use_yaw_information); +} + +/** + * @brief change label for prediction + * + * @param label + * @return ObjectClassification::_label_type + */ +ObjectClassification::_label_type changeLabelForPrediction( + const ObjectClassification::_label_type & label, const TrackedObject & object, + const lanelet::LaneletMapPtr & lanelet_map_ptr_) +{ + // for car like vehicle do not change labels + switch (label) { + case ObjectClassification::CAR: + case ObjectClassification::BUS: + case ObjectClassification::TRUCK: + case ObjectClassification::TRAILER: + case ObjectClassification::UNKNOWN: + return label; + + case ObjectClassification::MOTORCYCLE: + case ObjectClassification::BICYCLE: { // if object is within road lanelet and satisfies yaw + // constraints + const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); + // if the object is within lanelet, do the same estimation with vehicle + if (within_road_lanelet) return ObjectClassification::MOTORCYCLE; + + constexpr float high_speed_threshold = + autoware::universe_utils::kmph2mps(25.0); // High speed bicycle 25 km/h + // calc abs speed from x and y velocity + const double abs_speed = std::hypot( + object.kinematics.twist_with_covariance.twist.linear.x, + object.kinematics.twist_with_covariance.twist.linear.y); + const bool high_speed_object = abs_speed > high_speed_threshold; + // high speed object outside road lanelet will move like unknown object + // return ObjectClassification::UNKNOWN; // temporary disabled + if (high_speed_object) return label; // Do nothing for now + return ObjectClassification::BICYCLE; + } + + case ObjectClassification::PEDESTRIAN: { + const float max_velocity_for_human_mps = + autoware::universe_utils::kmph2mps(25.0); // Max human being motion speed is 25km/h + const double abs_speed = std::hypot( + object.kinematics.twist_with_covariance.twist.linear.x, + object.kinematics.twist_with_covariance.twist.linear.y); + const bool high_speed_object = abs_speed > max_velocity_for_human_mps; + // fast, human-like object: like segway + // return ObjectClassification::MOTORCYCLE; + if (high_speed_object) return label; // currently do nothing + // fast human outside road lanelet will move like unknown object + // return ObjectClassification::UNKNOWN; + return label; + } + + default: + return label; + } +} + +template +std::unordered_set removeOldObjectsHistory( + const double current_time, const double buffer_time, + std::unordered_map> & target_objects) +{ + std::unordered_set invalid_object_id; + for (auto iter = target_objects.begin(); iter != target_objects.end(); ++iter) { + const std::string object_id = iter->first; + std::deque & object_data = iter->second; + + // If object data is empty, we are going to delete the buffer for the obstacle + if (object_data.empty()) { + invalid_object_id.insert(object_id); + continue; + } + + const double latest_object_time = rclcpp::Time(object_data.back().header.stamp).seconds(); + + // Delete Old Objects + if (current_time - latest_object_time > buffer_time) { + invalid_object_id.insert(object_id); + continue; + } + + // Delete old information + while (!object_data.empty()) { + const double post_object_time = rclcpp::Time(object_data.front().header.stamp).seconds(); + if (current_time - post_object_time <= buffer_time) { + break; + } + object_data.pop_front(); + } + + if (object_data.empty()) { + invalid_object_id.insert(object_id); + continue; + } + } + + for (const auto & key : invalid_object_id) { + target_objects.erase(key); + } + + return invalid_object_id; +} + +// Explicit instantiation definitions +template std::unordered_set removeOldObjectsHistory( + const double current_time, const double buffer_time, + std::unordered_map> & target_objects); +template std::unordered_set removeOldObjectsHistory( + const double current_time, const double buffer_time, + std::unordered_map> & target_objects); + +PredictedObjectKinematics convertToPredictedKinematics( + const TrackedObjectKinematics & tracked_object) +{ + PredictedObjectKinematics output; + output.initial_pose_with_covariance = tracked_object.pose_with_covariance; + output.initial_twist_with_covariance = tracked_object.twist_with_covariance; + output.initial_acceleration_with_covariance = tracked_object.acceleration_with_covariance; + return output; +} + +PredictedObject convertToPredictedObject(const TrackedObject & tracked_object) +{ + PredictedObject predicted_object; + predicted_object.kinematics = convertToPredictedKinematics(tracked_object.kinematics); + predicted_object.classification = tracked_object.classification; + predicted_object.object_id = tracked_object.object_id; + predicted_object.shape.type = tracked_object.shape.type; + predicted_object.shape.footprint = tracked_object.shape.footprint; + predicted_object.shape.dimensions = tracked_object.shape.dimensions; + predicted_object.existence_probability = tracked_object.existence_probability; + + return predicted_object; +} + +} // namespace utils +} // namespace autoware::map_based_prediction diff --git a/perception/autoware_map_based_prediction/test/map_based_prediction/test_path_generator.cpp b/perception/autoware_map_based_prediction/test/map_based_prediction/test_path_generator.cpp index 08e5b4977e583..26b7be5c813f6 100644 --- a/perception/autoware_map_based_prediction/test/map_based_prediction/test_path_generator.cpp +++ b/perception/autoware_map_based_prediction/test/map_based_prediction/test_path_generator.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "map_based_prediction/data_structure.hpp" #include "map_based_prediction/path_generator.hpp" #include From 6a062412411f1e3860ca9cd85befffb3057b88b9 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Thu, 31 Oct 2024 18:00:10 +0900 Subject: [PATCH 07/96] refactor(ndt_scan_matcher, ndt_omp): move ndt_omp into ndt_scan_matcher (#8912) * Moved ndt_omp into ndt_scan_matcher Signed-off-by: Shintaro Sakoda * Added Copyright Signed-off-by: Shintaro Sakoda * style(pre-commit): autofix * Fixed include Signed-off-by: Shintaro Sakoda * Fixed cast style Signed-off-by: Shintaro Sakoda * Fixed include Signed-off-by: Shintaro Sakoda * Fixed honorific title Signed-off-by: Shintaro Sakoda * Fixed honorific title Signed-off-by: Shintaro Sakoda * style(pre-commit): autofix * Fixed include hierarchy Signed-off-by: Shintaro Sakoda * style(pre-commit): autofix * Fixed include hierarchy Signed-off-by: Shintaro Sakoda * style(pre-commit): autofix * Fixed hierarchy Signed-off-by: Shintaro Sakoda * Fixed NVTP to NVTL Signed-off-by: Shintaro Sakoda * Added cspell:ignore Signed-off-by: Shintaro Sakoda * Fixed miss spell Signed-off-by: Shintaro Sakoda * style(pre-commit): autofix * Fixed include Signed-off-by: Shintaro Sakoda * Renamed applyFilter Signed-off-by: Shintaro Sakoda * Moved ***_impl.hpp from include/ to src/ Signed-off-by: Shintaro Sakoda * style(pre-commit): autofix * Fixed variable scope Signed-off-by: Shintaro Sakoda * Fixed to pass by reference Signed-off-by: Shintaro Sakoda --------- Signed-off-by: Shintaro Sakoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../autoware_ndt_scan_matcher/CMakeLists.txt | 19 +- .../ndt_scan_matcher/hyper_parameters.hpp | 4 +- .../ndt_scan_matcher/map_update_module.hpp | 6 +- .../autoware/ndt_scan_matcher/ndt_omp/LICENSE | 25 + .../ndt_scan_matcher/ndt_omp/README.md | 12 + .../ndt_omp/estimate_covariance.hpp | 90 ++ .../ndt_omp/multi_voxel_grid_covariance_omp.h | 439 ++++++ .../ndt_omp/multigrid_ndt_omp.h | 573 ++++++++ .../ndt_scan_matcher/ndt_omp/ndt_struct.hpp | 107 ++ .../ndt_scan_matcher_core.hpp | 6 +- .../autoware_ndt_scan_matcher/package.xml | 1 - .../src/ndt_omp/estimate_covariance.cpp | 203 +++ .../multi_voxel_grid_covariance_omp.cpp | 18 + .../multi_voxel_grid_covariance_omp_impl.hpp | 477 +++++++ .../src/ndt_omp/multigrid_ndt_omp.cpp | 18 + .../src/ndt_omp/multigrid_ndt_omp_impl.hpp | 1243 +++++++++++++++++ .../src/ndt_scan_matcher_core.cpp | 2 +- 17 files changed, 3232 insertions(+), 11 deletions(-) create mode 100644 localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/LICENSE create mode 100644 localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/README.md create mode 100644 localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/estimate_covariance.hpp create mode 100644 localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multi_voxel_grid_covariance_omp.h create mode 100644 localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multigrid_ndt_omp.h create mode 100644 localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/ndt_struct.hpp create mode 100644 localization/autoware_ndt_scan_matcher/src/ndt_omp/estimate_covariance.cpp create mode 100644 localization/autoware_ndt_scan_matcher/src/ndt_omp/multi_voxel_grid_covariance_omp.cpp create mode 100644 localization/autoware_ndt_scan_matcher/src/ndt_omp/multi_voxel_grid_covariance_omp_impl.hpp create mode 100644 localization/autoware_ndt_scan_matcher/src/ndt_omp/multigrid_ndt_omp.cpp create mode 100644 localization/autoware_ndt_scan_matcher/src/ndt_omp/multigrid_ndt_omp_impl.hpp diff --git a/localization/autoware_ndt_scan_matcher/CMakeLists.txt b/localization/autoware_ndt_scan_matcher/CMakeLists.txt index 98bdf9be8761e..fdec72e521d43 100644 --- a/localization/autoware_ndt_scan_matcher/CMakeLists.txt +++ b/localization/autoware_ndt_scan_matcher/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.14) project(autoware_ndt_scan_matcher) +# cspell:ignore multigrid + find_package(autoware_cmake REQUIRED) autoware_package() @@ -22,9 +24,24 @@ else() endif() endif() +find_package(OpenMP) + find_package(PCL REQUIRED COMPONENTS common io registration) include_directories(${PCL_INCLUDE_DIRS}) +ament_auto_add_library(multigrid_ndt_omp SHARED + src/ndt_omp/multi_voxel_grid_covariance_omp.cpp + src/ndt_omp/multigrid_ndt_omp.cpp + src/ndt_omp/estimate_covariance.cpp +) +target_link_libraries(multigrid_ndt_omp ${PCL_LIBRARIES}) + +if(OpenMP_CXX_FOUND) + target_link_libraries(multigrid_ndt_omp OpenMP::OpenMP_CXX) +else() + message(WARNING "OpenMP not found") +endif() + ament_auto_add_library(${PROJECT_NAME} SHARED src/map_update_module.cpp src/ndt_scan_matcher_core.cpp @@ -32,7 +49,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) link_directories(${PCL_LIBRARY_DIRS}) -target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES} multigrid_ndt_omp) rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "autoware::ndt_scan_matcher::NDTScanMatcher" diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/hyper_parameters.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/hyper_parameters.hpp index c1ffbe696fda3..84e059f87b9d1 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/hyper_parameters.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/hyper_parameters.hpp @@ -15,9 +15,9 @@ #ifndef AUTOWARE__NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ #define AUTOWARE__NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ -#include +#include "ndt_omp/multigrid_ndt_omp.h" -#include +#include #include #include diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp index 9b140977f4971..d2dce42e3923e 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp @@ -17,8 +17,9 @@ #include "autoware/localization_util/diagnostics_module.hpp" #include "autoware/localization_util/util_func.hpp" -#include "autoware/ndt_scan_matcher/hyper_parameters.hpp" -#include "autoware/ndt_scan_matcher/particle.hpp" +#include "hyper_parameters.hpp" +#include "ndt_omp/multigrid_ndt_omp.h" +#include "particle.hpp" #include #include @@ -30,7 +31,6 @@ #include #include -#include #include #include diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/LICENSE b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/LICENSE new file mode 100644 index 0000000000000..612dceefd4493 --- /dev/null +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/LICENSE @@ -0,0 +1,25 @@ +BSD 2-Clause License + +Copyright (c) 2019, k.koide +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/README.md b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/README.md new file mode 100644 index 0000000000000..b742d988f7774 --- /dev/null +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/README.md @@ -0,0 +1,12 @@ +# ndt_omp + + + +cspell:ignore Kenji, Koide +<--> + +The codes in this directory are derived from , which is a fork of . + +We use this code in accordance with the LICENSE, and we have directly confirmed this with Dr. Kenji Koide. + +We sincerely appreciate Dr. Koide’s support and contributions. diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/estimate_covariance.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/estimate_covariance.hpp new file mode 100644 index 0000000000000..a4e9e64ac39af --- /dev/null +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/estimate_covariance.hpp @@ -0,0 +1,90 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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__NDT_SCAN_MATCHER__NDT_OMP__ESTIMATE_COVARIANCE_HPP_ +#define AUTOWARE__NDT_SCAN_MATCHER__NDT_OMP__ESTIMATE_COVARIANCE_HPP_ + +#include "multigrid_ndt_omp.h" + +#include + +#include +#include +#include + +namespace pclomp +{ + +struct ResultOfMultiNdtCovarianceEstimation +{ + Eigen::Vector2d mean; + Eigen::Matrix2d covariance; + std::vector ndt_initial_poses; + std::vector ndt_results; +}; + +/** \brief Estimate functions */ +Eigen::Matrix2d estimate_xy_covariance_by_laplace_approximation( + const Eigen::Matrix & hessian); +ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt( + const NdtResult & ndt_result, + const std::shared_ptr< + pclomp::MultiGridNormalDistributionsTransform> & ndt_ptr, + const std::vector & poses_to_search); +ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt_score( + const NdtResult & ndt_result, + const std::shared_ptr< + pclomp::MultiGridNormalDistributionsTransform> & ndt_ptr, + const std::vector & poses_to_search, const double temperature); + +/** \brief Find rotation matrix aligning covariance to principal axes + * (1) Compute eigenvalues and eigenvectors + * (2) Compute angle for first eigenvector + * (3) Return rotation matrix + */ +Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes( + const Eigen::Matrix2d & matrix); + +/** \brief Propose poses to search. + * (1) Compute covariance by Laplace approximation + * (2) Find rotation matrix aligning covariance to principal axes + * (3) Propose search points by adding offset_x and offset_y to the center_pose + */ +std::vector propose_poses_to_search( + const NdtResult & ndt_result, const std::vector & offset_x, + const std::vector & offset_y); + +/** \brief Calculate weights by exponential */ +std::vector calc_weight_vec(const std::vector & score_vec, double temperature); + +/** \brief Calculate weighted mean and covariance */ +std::pair calculate_weighted_mean_and_cov( + const std::vector & pose_2d_vec, const std::vector & weight_vec); + +/** \brief Rotate covariance to base_link coordinate */ +Eigen::Matrix2d rotate_covariance_to_base_link( + const Eigen::Matrix2d & covariance, const Eigen::Matrix4f & pose); + +/** \brief Rotate covariance to map coordinate */ +Eigen::Matrix2d rotate_covariance_to_map( + const Eigen::Matrix2d & covariance, const Eigen::Matrix4f & pose); + +/** \brief Adjust diagonal covariance */ +Eigen::Matrix2d adjust_diagonal_covariance( + const Eigen::Matrix2d & covariance, const Eigen::Matrix4f & pose, const double fixed_cov00, + const double fixed_cov11); + +} // namespace pclomp + +#endif // AUTOWARE__NDT_SCAN_MATCHER__NDT_OMP__ESTIMATE_COVARIANCE_HPP_ diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multi_voxel_grid_covariance_omp.h b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multi_voxel_grid_covariance_omp.h new file mode 100644 index 0000000000000..85f37e878bd48 --- /dev/null +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multi_voxel_grid_covariance_omp.h @@ -0,0 +1,439 @@ +// Copyright 2022 TIER IV, Inc. +// +// 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. + +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef AUTOWARE__NDT_SCAN_MATCHER__NDT_OMP__MULTI_VOXEL_GRID_COVARIANCE_OMP_H_ +#define AUTOWARE__NDT_SCAN_MATCHER__NDT_OMP__MULTI_VOXEL_GRID_COVARIANCE_OMP_H_ + +// cspell:ignore Magnusson, Okorn, evecs, evals, covar, eigvalue, futs + +#include +#include + +// pcl_macros.h must come first +// clang-format off +#include +// clang-format on +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace pclomp +{ +/** \brief A searchable voxel structure containing the mean and covariance of the data. + * \note For more information please see + * Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform — + * an Efficient Representation for Registration, Surface Analysis, and Loop Detection. + * PhD thesis, Orebro University. Orebro Studies in Technology 36 + * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific) + */ +template +class MultiVoxelGridCovariance : public pcl::VoxelGrid +{ +protected: + using pcl::VoxelGrid::filter_name_; + using pcl::VoxelGrid::getClassName; + using pcl::VoxelGrid::input_; + using pcl::VoxelGrid::indices_; + using pcl::VoxelGrid::filter_limit_negative_; + using pcl::VoxelGrid::filter_limit_min_; + using pcl::VoxelGrid::filter_limit_max_; + + // using pcl::VoxelGrid::downsample_all_data_; + using pcl::VoxelGrid::leaf_size_; + using pcl::VoxelGrid::min_b_; + using pcl::VoxelGrid::max_b_; + using pcl::VoxelGrid::inverse_leaf_size_; + using pcl::VoxelGrid::div_b_; + using pcl::VoxelGrid::divb_mul_; + + typedef typename pcl::traits::fieldList::type FieldList; + typedef typename pcl::Filter::PointCloud PointCloud; + typedef typename PointCloud::Ptr PointCloudPtr; + typedef typename PointCloud::ConstPtr PointCloudConstPtr; + +public: +#if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0) + typedef pcl::shared_ptr> Ptr; + typedef pcl::shared_ptr> ConstPtr; +#else + typedef boost::shared_ptr> Ptr; + typedef boost::shared_ptr> ConstPtr; +#endif + + /** \brief Simple structure to hold a centroid, covariance and the number of points in a leaf. + * Inverse covariance, eigen vectors and eigen values are precomputed. */ + struct Leaf + { + /** \brief Constructor. + * Sets \ref nr_points_, \ref icov_, \ref mean_ and \ref evals_ to 0 and \ref cov_ and \ref + * evecs_ to the identity matrix + */ + Leaf() + : nr_points_(0), + mean_(Eigen::Vector3d::Zero()), + centroid_(), + cov_(Eigen::Matrix3d::Identity()), + icov_(Eigen::Matrix3d::Zero()), + evecs_(Eigen::Matrix3d::Identity()), + evals_(Eigen::Vector3d::Zero()) + { + } + + Leaf(const Leaf & other) + : mean_(other.mean_), + centroid_(other.centroid_), + cov_(other.cov_), + icov_(other.icov_), + evecs_(other.evecs_), + evals_(other.evals_) + { + nr_points_ = other.nr_points_; + } + + Leaf(Leaf && other) + : mean_(std::move(other.mean_)), + centroid_(std::move(other.centroid_)), + cov_(std::move(other.cov_)), + icov_(std::move(other.icov_)), + evecs_(std::move(other.evecs_)), + evals_(std::move(other.evals_)) + { + nr_points_ = other.nr_points_; + } + + Leaf & operator=(const Leaf & other) + { + mean_ = other.mean_; + centroid_ = other.centroid_; + cov_ = other.cov_; + icov_ = other.icov_; + evecs_ = other.evecs_; + evals_ = other.evals_; + nr_points_ = other.nr_points_; + + return *this; + } + + Leaf & operator=(Leaf && other) + { + mean_ = std::move(other.mean_); + centroid_ = std::move(other.centroid_); + cov_ = std::move(other.cov_); + icov_ = std::move(other.icov_); + evecs_ = std::move(other.evecs_); + evals_ = std::move(other.evals_); + nr_points_ = other.nr_points_; + + return *this; + } + + /** \brief Get the voxel covariance. + * \return covariance matrix + */ + const Eigen::Matrix3d & getCov() const { return (cov_); } + Eigen::Matrix3d & getCov() { return (cov_); } + + /** \brief Get the inverse of the voxel covariance. + * \return inverse covariance matrix + */ + const Eigen::Matrix3d & getInverseCov() const { return (icov_); } + + Eigen::Matrix3d & getInverseCov() { return (icov_); } + + /** \brief Get the voxel centroid. + * \return centroid + */ + const Eigen::Vector3d & getMean() const { return (mean_); } + + Eigen::Vector3d & getMean() { return (mean_); } + + /** \brief Get the eigen vectors of the voxel covariance. + * \note Order corresponds with \ref getEvals + * \return matrix whose columns contain eigen vectors + */ + const Eigen::Matrix3d & getEvecs() const { return (evecs_); } + + Eigen::Matrix3d & getEvecs() { return (evecs_); } + + /** \brief Get the eigen values of the voxel covariance. + * \note Order corresponds with \ref getEvecs + * \return vector of eigen values + */ + const Eigen::Vector3d & getEvals() const { return (evals_); } + + Eigen::Vector3d & getEvals() { return (evals_); } + + /** \brief Get the number of points contained by this voxel. + * \return number of points + */ + int getPointCount() const { return (nr_points_); } + + /** \brief Number of points contained by voxel */ + int nr_points_; + + /** \brief 3D voxel centroid */ + Eigen::Vector3d mean_; + + /** \brief Nd voxel centroid + * \note Differs from \ref mean_ when color data is used + */ + Eigen::VectorXf centroid_; + + /** \brief Voxel covariance matrix */ + Eigen::Matrix3d cov_; + + /** \brief Inverse of voxel covariance matrix */ + Eigen::Matrix3d icov_; + + /** \brief Eigen vectors of voxel covariance matrix */ + Eigen::Matrix3d evecs_; + + /** \brief Eigen values of voxel covariance matrix */ + Eigen::Vector3d evals_; + }; + + /** \brief Pointer to MultiVoxelGridCovariance leaf structure */ + typedef Leaf * LeafPtr; + + /** \brief Const pointer to MultiVoxelGridCovariance leaf structure */ + typedef const Leaf * LeafConstPtr; + + struct BoundingBox + { + Eigen::Vector4i max; + Eigen::Vector4i min; + Eigen::Vector4i div_mul; + }; + + // Each grid contains a vector of leaves and a kdtree built from those leaves + using GridNodeType = std::vector; + using GridNodePtr = std::shared_ptr; + +public: + /** \brief Constructor. + * Sets \ref leaf_size_ to 0 + */ + MultiVoxelGridCovariance() : min_points_per_voxel_(6), min_covar_eigvalue_mult_(0.01) + { + leaf_size_.setZero(); + min_b_.setZero(); + max_b_.setZero(); + filter_name_ = "MultiVoxelGridCovariance"; + + setThreadNum(1); + last_check_tid_ = -1; + } + + MultiVoxelGridCovariance(const MultiVoxelGridCovariance & other); + MultiVoxelGridCovariance(MultiVoxelGridCovariance && other) noexcept; + + MultiVoxelGridCovariance & operator=(const MultiVoxelGridCovariance & other); + MultiVoxelGridCovariance & operator=(MultiVoxelGridCovariance && other) noexcept; + + /** \brief Add a cloud to the voxel grid list and build a ND voxel grid from it. + */ + void setInputCloudAndFilter(const PointCloudConstPtr & cloud, const std::string & grid_id); + + /** \brief Remove a ND voxel grid corresponding to the specified id + */ + void removeCloud(const std::string & grid_id); + + /** \brief Build KdTrees from the NDT voxel for later radius search + */ + void createKdtree(); + + /** \brief Search for all the nearest occupied voxels of the query point in a given radius. + * \note Only voxels containing a sufficient number of points are used. + * \param[in] point the given query point + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_leaves the resultant leaves of the neighboring points + * \param[in] max_nn + * \return number of neighbors found + */ + int radiusSearch( + const PointT & point, double radius, std::vector & k_leaves, + unsigned int max_nn = 0) const; + + /** \brief Search for all the nearest occupied voxels of the query point in a given radius. + * \note Only voxels containing a sufficient number of points are used. + * \param[in] cloud the given query point + * \param[in] index a valid index in cloud representing a valid (i.e., finite) query point + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_leaves the resultant leaves of the neighboring points + * \param[in] max_nn + * \return number of neighbors found + */ + int radiusSearch( + const PointCloud & cloud, int index, double radius, std::vector & k_leaves, + unsigned int max_nn = 0) const; + + // Return a pointer to avoid multiple deep copies + PointCloud getVoxelPCD() const; + + // Return the string indices of currently loaded map pieces + std::vector getCurrentMapIDs() const; + + void setThreadNum(int thread_num) + { + sync(); + + if (thread_num <= 0) { + thread_num = 1; + } + + thread_num_ = thread_num; + thread_futs_.resize(thread_num_); + processing_inputs_.resize(thread_num_); + } + + ~MultiVoxelGridCovariance() + { + // Stop all threads in the case an intermediate interrupt occurs + sync(); + } + +protected: + // Return the index of an idle thread, which is not running any + // job, or has already finished its job and waiting for a join. + inline int get_idle_tid() + { + int tid = (last_check_tid_ == thread_num_ - 1) ? 0 : last_check_tid_ + 1; + std::chrono::microseconds span(50); + + // Loop until an idle thread is found + while (true) { + // Return immediately if a thread that has not been given a job is found + if (!thread_futs_[tid].valid()) { + last_check_tid_ = tid; + return tid; + } + + // If no such thread is found, wait for the current thread to finish its job + if (thread_futs_[tid].wait_for(span) == std::future_status::ready) { + last_check_tid_ = tid; + return tid; + } + + // If the current thread has not finished its job, check the next thread + tid = (tid == thread_num_ - 1) ? 0 : tid + 1; + } + } + + // Wait for all running threads to finish + inline void sync() + { + for (auto & tf : thread_futs_) { + if (tf.valid()) { + tf.wait(); + } + } + } + + // A wrapper of the real apply_filter + inline bool apply_filter_thread(int tid, GridNodeType & node) + { + apply_filter(processing_inputs_[tid], node); + processing_inputs_[tid].reset(); + + return true; + } + + /** \brief Filter cloud and initializes voxel structure. + * \param[out] output cloud containing centroids of voxels containing a sufficient number of + * points + */ + void apply_filter(const PointCloudConstPtr & input, GridNodeType & node); + + void updateLeaf(const PointT & point, const int & centroid_size, Leaf & leaf) const; + + void computeLeafParams( + const Eigen::Vector3d & pt_sum, Eigen::SelfAdjointEigenSolver & eigensolver, + Leaf & leaf) const; + + int64_t getLeafID(const PointT & point, const BoundingBox & bbox) const; + + /** \brief Minimum points contained with in a voxel to allow it to be usable. */ + int min_points_per_voxel_; + + /** \brief Minimum allowable ratio between eigenvalues to prevent singular covariance matrices. */ + double min_covar_eigvalue_mult_; + + // The point cloud containing the centroids of leaves + // Used to build a kdtree for radius search + PointCloudPtr voxel_centroids_ptr_; + + // Thread pooling, for parallel processing + int thread_num_; + std::vector> thread_futs_; + std::vector processing_inputs_; + int last_check_tid_; + + // A map to convert string index to integer index, used for grids + std::map sid_to_iid_; + // Grids of leaves are held in a vector for faster access speed + std::vector grid_list_; + // A kdtree built from the leaves of grids + pcl::KdTreeFLANN kdtree_; + // To access leaf by the search results by kdtree + std::vector leaf_ptrs_; +}; +} // namespace pclomp + +#endif // AUTOWARE__NDT_SCAN_MATCHER__NDT_OMP__MULTI_VOXEL_GRID_COVARIANCE_OMP_H_ diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multigrid_ndt_omp.h b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multigrid_ndt_omp.h new file mode 100644 index 0000000000000..b173d164de97d --- /dev/null +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multigrid_ndt_omp.h @@ -0,0 +1,573 @@ +// Copyright 2022 TIER IV, Inc. +// +// 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. + +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef AUTOWARE__NDT_SCAN_MATCHER__NDT_OMP__MULTIGRID_NDT_OMP_H_ +#define AUTOWARE__NDT_SCAN_MATCHER__NDT_OMP__MULTIGRID_NDT_OMP_H_ + +// cspell:ignore multigrid, Magnusson, Thuente, Todor, Stoyanov, Okorn + +#include "multi_voxel_grid_covariance_omp.h" +#include "ndt_struct.hpp" + +#include +#include + +#include "boost/optional.hpp" + +#include + +#include +#include + +namespace pclomp +{ + +/** \brief A 3D Normal Distribution Transform registration implementation for point cloud data. + * \note For more information please see + * Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform — + * an Efficient Representation for Registration, Surface Analysis, and Loop Detection. + * PhD thesis, Orebro University. Orebro Studies in Technology 36., + * More, J., and Thuente, D. (1994). Line Search Algorithm with Guaranteed Sufficient Decrease + * In ACM Transactions on Mathematical Software. and + * Sun, W. and Yuan, Y, (2006) Optimization Theory and Methods: Nonlinear Programming. 89-100 + * \note Math refactored by Todor Stoyanov. + * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific) + */ +template +class MultiGridNormalDistributionsTransform : public pcl::Registration +{ +protected: + typedef pcl::Registration BaseRegType; + typedef typename BaseRegType::PointCloudSource PointCloudSource; + typedef typename PointCloudSource::Ptr PointCloudSourcePtr; + typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr; + + typedef typename BaseRegType::PointCloudTarget PointCloudTarget; + typedef typename PointCloudTarget::Ptr PointCloudTargetPtr; + typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr; + + /** \brief Typename of searchable voxel grid containing mean and covariance. */ + typedef pclomp::MultiVoxelGridCovariance TargetGrid; + /** \brief Typename of const pointer to searchable voxel grid leaf. */ + typedef typename TargetGrid::LeafConstPtr TargetGridLeafConstPtr; + +public: +#if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0) + typedef pcl::shared_ptr> Ptr; + typedef pcl::shared_ptr> + ConstPtr; +#else + typedef boost::shared_ptr> Ptr; + typedef boost::shared_ptr> + ConstPtr; +#endif + + /** \brief Constructor. + * Sets \ref outlier_ratio_ to 0.35, \ref params_.step_size to 0.05 and \ref params_.resolution + * to 1.0 + */ + MultiGridNormalDistributionsTransform(); + + // Copy & move constructor + MultiGridNormalDistributionsTransform(const MultiGridNormalDistributionsTransform & other); + MultiGridNormalDistributionsTransform(MultiGridNormalDistributionsTransform && other) noexcept; + + // Copy & move assignments + MultiGridNormalDistributionsTransform & operator=( + const MultiGridNormalDistributionsTransform & other); + MultiGridNormalDistributionsTransform & operator=( + MultiGridNormalDistributionsTransform && other) noexcept; + + /** \brief Empty destructor */ + virtual ~MultiGridNormalDistributionsTransform() {} + + void setNumThreads(int n) + { + params_.num_threads = n; + + target_cells_.setThreadNum(params_.num_threads); + } + + inline int getNumThreads() const { return params_.num_threads; } + + inline void setInputSource(const PointCloudSourceConstPtr & input) + { + // This is to avoid segmentation fault when setting null input + // No idea why PCL does not check the nullity of input + if (input) { + BaseRegType::setInputSource(input); + } else { + std::cerr << "Error: Null input source cloud is not allowed" << std::endl; + exit(EXIT_FAILURE); + } + } + + inline void setInputTarget(const PointCloudTargetConstPtr & cloud) + { + const std::string default_target_id = "default"; + addTarget(cloud, default_target_id); + createVoxelKdtree(); + } + + /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the + * input source to). \param[in] cloud the input point cloud target + */ + inline void addTarget(const PointCloudTargetConstPtr & cloud, const std::string & target_id) + { + BaseRegType::setInputTarget(cloud); + target_cells_.setLeafSize(params_.resolution, params_.resolution, params_.resolution); + target_cells_.setInputCloudAndFilter(cloud, target_id); + } + + inline void removeTarget(const std::string & target_id) { target_cells_.removeCloud(target_id); } + + inline void createVoxelKdtree() + { + target_cells_.createKdtree(); + target_cloud_updated_ = false; + } + + /** \brief Set/change the voxel grid resolution. + * \param[in] resolution side length of voxels + */ + inline void setResolution(float resolution) + { + // Prevents unnecessary voxel initiations + if (params_.resolution != resolution) { + params_.resolution = resolution; + if (input_) init(); + } + } + + /** \brief Get voxel grid resolution. + * \return side length of voxels + */ + inline float getResolution() const { return (params_.resolution); } + + /** \brief Get the newton line search maximum step length. + * \return maximum step length + */ + inline double getStepSize() const { return (params_.step_size); } + + /** \brief Set/change the newton line search maximum step length. + * \param[in] step_size maximum step length + */ + inline void setStepSize(double step_size) { params_.step_size = step_size; } + + /** \brief Get the point cloud outlier ratio. + * \return outlier ratio + */ + inline double getOutlierRatio() const { return (outlier_ratio_); } + + /** \brief Set/change the point cloud outlier ratio. + * \param[in] outlier_ratio outlier ratio + */ + inline void setOutlierRatio(double outlier_ratio) { outlier_ratio_ = outlier_ratio; } + + /** \brief Get the registration alignment probability. + * \return transformation probability + */ + inline double getTransformationProbability() const { return (trans_probability_); } + + inline double getNearestVoxelTransformationLikelihood() const + { + return nearest_voxel_transformation_likelihood_; + } + + /** \brief Get the number of iterations required to calculate alignment. + * \return final number of iterations + */ + inline int getFinalNumIteration() const { return (nr_iterations_); } + + /** \brief Return the hessian matrix */ + inline Eigen::Matrix getHessian() const { return hessian_; } + + /** \brief Return the transformation array */ + inline const std::vector> & + getFinalTransformationArray() const + { + return transformation_array_; + } + + /** \brief Convert 6 element transformation vector to affine transformation. + * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw] + * \param[out] trans affine transform corresponding to given transformation vector + */ + static void convertTransform(const Eigen::Matrix & x, Eigen::Affine3f & trans) + { + trans = Eigen::Translation( + static_cast(x(0)), static_cast(x(1)), static_cast(x(2))) * + Eigen::AngleAxis(static_cast(x(3)), Eigen::Vector3f::UnitX()) * + Eigen::AngleAxis(static_cast(x(4)), Eigen::Vector3f::UnitY()) * + Eigen::AngleAxis(static_cast(x(5)), Eigen::Vector3f::UnitZ()); + } + + /** \brief Convert 6 element transformation vector to transformation matrix. + * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw] + * \param[out] trans 4x4 transformation matrix corresponding to given transformation vector + */ + static void convertTransform(const Eigen::Matrix & x, Eigen::Matrix4f & trans) + { + Eigen::Affine3f _affine; + convertTransform(x, _affine); + trans = _affine.matrix(); + } + + // negative log likelihood function + // lower is better + double calculateTransformationProbability(const PointCloudSource & cloud) const; + double calculateNearestVoxelTransformationLikelihood(const PointCloudSource & cloud) const; + pcl::PointCloud calculateNearestVoxelScoreEachPoint( + const PointCloudSource & cloud) const; + + inline void setRegularizationScaleFactor(float regularization_scale_factor) + { + params_.regularization_scale_factor = regularization_scale_factor; + } + + inline void setRegularizationPose(Eigen::Matrix4f regularization_pose) + { + regularization_pose_ = regularization_pose; + } + + inline void unsetRegularizationPose() { regularization_pose_ = boost::none; } + + NdtResult getResult() + { + NdtResult ndt_result; + ndt_result.pose = this->getFinalTransformation(); + ndt_result.transformation_array = getFinalTransformationArray(); + ndt_result.transform_probability = getTransformationProbability(); + ndt_result.transform_probability_array = transform_probability_array_; + ndt_result.nearest_voxel_transformation_likelihood = getNearestVoxelTransformationLikelihood(); + ndt_result.nearest_voxel_transformation_likelihood_array = + nearest_voxel_transformation_likelihood_array_; + ndt_result.iteration_num = getFinalNumIteration(); + ndt_result.hessian = getHessian(); + return ndt_result; + } + + /** \brief Set the transformation epsilon (maximum allowable translation squared + * difference between two consecutive transformations) in order for an optimization to + * be considered as having converged to the final solution. \param[in] epsilon the + * transformation epsilon in order for an optimization to be considered as having + * converged to the final solution. + */ + inline void setTransformationEpsilon(double epsilon) { params_.trans_epsilon = epsilon; } + + /** \brief Get the transformation epsilon (maximum allowable translation squared + * difference between two consecutive transformations) as set by the user. + */ + inline double getTransformationEpsilon() { return (params_.trans_epsilon); } + + inline void setMaximumIterations(int max_iterations) + { + params_.max_iterations = max_iterations; + max_iterations_ = params_.max_iterations; + } + + inline int getMaxIterations() const { return params_.max_iterations; } + + inline int getMaxIterations() { return params_.max_iterations; } + + void setParams(const NdtParams & ndt_params) + { + params_ = ndt_params; + max_iterations_ = params_.max_iterations; + + target_cells_.setThreadNum(params_.num_threads); + } + + NdtParams getParams() const { return params_; } + + pcl::PointCloud getVoxelPCD() const { return target_cells_.getVoxelPCD(); } + + std::vector getCurrentMapIDs() const { return target_cells_.getCurrentMapIDs(); } + +protected: + using BaseRegType::converged_; + using BaseRegType::final_transformation_; + using BaseRegType::input_; + using BaseRegType::max_iterations_; + using BaseRegType::nr_iterations_; + using BaseRegType::previous_transformation_; + using BaseRegType::reg_name_; + using BaseRegType::target_; + using BaseRegType::target_cloud_updated_; + using BaseRegType::transformation_; + using BaseRegType::transformation_epsilon_; + + using BaseRegType::update_visualizer_; + + /** \brief Estimate the transformation and returns the transformed source (input) as output. + * \param[out] output the resultant input transformed point cloud dataset + */ + virtual void computeTransformation(PointCloudSource & output) + { + computeTransformation(output, Eigen::Matrix4f::Identity()); + } + + /** \brief Estimate the transformation and returns the transformed source (input) as output. + * \param[out] output the resultant input transformed point cloud dataset + * \param[in] guess the initial gross estimation of the transformation + */ + virtual void computeTransformation(PointCloudSource & output, const Eigen::Matrix4f & guess); + + /** \brief Initiate covariance voxel structure. */ + void inline init() {} + + /** \brief Compute derivatives of probability function w.r.t. the transformation vector. + * \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009]. + * \param[out] score_gradient the gradient vector of the probability function w.r.t. the + * transformation vector \param[out] hessian the hessian matrix of the probability function w.r.t. + * the transformation vector \param[in] trans_cloud transformed point cloud \param[in] p the + * current transform vector \param[in] compute_hessian flag to calculate hessian, unnecessary for + * step calculation. + */ + double computeDerivatives( + Eigen::Matrix & score_gradient, Eigen::Matrix & hessian, + PointCloudSource & trans_cloud, Eigen::Matrix & p, bool compute_hessian = true); + + /** \brief Compute individual point contributions to derivatives of probability function w.r.t. + * the transformation vector. \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009]. \param[in,out] + * score_gradient the gradient vector of the probability function w.r.t. the transformation vector + * \param[in,out] hessian the hessian matrix of the probability function w.r.t. the transformation + * vector \param[in] x_trans transformed point minus mean of occupied covariance voxel \param[in] + * c_inv covariance of occupied covariance voxel \param[in] compute_hessian flag to calculate + * hessian, unnecessary for step calculation. + */ + double updateDerivatives( + Eigen::Matrix & score_gradient, Eigen::Matrix & hessian, + const Eigen::Matrix & point_gradient, + const Eigen::Matrix & point_hessian, const Eigen::Vector3d & x_trans, + const Eigen::Matrix3d & c_inv, bool compute_hessian = true) const; + + /** \brief Precompute angular components of derivatives. + * \note Equation 6.19 and 6.21 [Magnusson 2009]. + * \param[in] p the current transform vector + * \param[in] compute_hessian flag to calculate hessian, unnecessary for step calculation. + */ + void computeAngleDerivatives(Eigen::Matrix & p, bool compute_hessian = true); + + /** \brief Compute point derivatives. + * \note Equation 6.18-21 [Magnusson 2009]. + * \param[in] x point from the input cloud + * \param[in] compute_hessian flag to calculate hessian, unnecessary for step calculation. + */ + void computePointDerivatives( + Eigen::Vector3d & x, Eigen::Matrix & point_gradient, + Eigen::Matrix & point_hessian, bool compute_hessian = true) const; + + /** \brief Compute hessian of probability function w.r.t. the transformation vector. + * \note Equation 6.13 [Magnusson 2009]. + * \param[out] hessian the hessian matrix of the probability function w.r.t. the transformation + * vector \param[in] trans_cloud transformed point cloud \param[in] p the current transform vector + */ + void computeHessian( + Eigen::Matrix & hessian, PointCloudSource & trans_cloud, + Eigen::Matrix & p); + + /** \brief Compute individual point contributions to hessian of probability function w.r.t. the + * transformation vector. \note Equation 6.13 [Magnusson 2009]. \param[in,out] hessian the hessian + * matrix of the probability function w.r.t. the transformation vector \param[in] x_trans + * transformed point minus mean of occupied covariance voxel \param[in] c_inv covariance of + * occupied covariance voxel + */ + void updateHessian( + Eigen::Matrix & hessian, const Eigen::Matrix & point_gradient, + const Eigen::Matrix & point_hessian, const Eigen::Vector3d & x_trans, + const Eigen::Matrix3d & c_inv) const; + + /** \brief Compute line search step length and update transform and probability derivatives using + * More-Thuente method. \note Search Algorithm [More, Thuente 1994] \param[in] x initial + * transformation vector, \f$ x \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ \vec{p} \f$ in + * Algorithm 2 [Magnusson 2009] \param[in] step_dir descent direction, \f$ p \f$ in Equation 1.3 + * (Moore, Thuente 1994) and \f$ \delta \vec{p} \f$ normalized in Algorithm 2 [Magnusson 2009] + * \param[in] step_init initial step length estimate, \f$ \alpha_0 \f$ in Moore-Thuente (1994) and + * the normal of \f$ \delta \vec{p} \f$ in Algorithm 2 [Magnusson 2009] \param[in] step_max + * maximum step length, \f$ \alpha_max \f$ in Moore-Thuente (1994) \param[in] step_min minimum + * step length, \f$ \alpha_min \f$ in Moore-Thuente (1994) \param[out] score final score function + * value, \f$ f(x + \alpha p) \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ score \f$ in + * Algorithm 2 [Magnusson 2009] \param[in,out] score_gradient gradient of score function w.r.t. + * transformation vector, \f$ f'(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$ \vec{g} \f$ in + * Algorithm 2 [Magnusson 2009] \param[out] hessian hessian of score function w.r.t. + * transformation vector, \f$ f''(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$ H \f$ in + * Algorithm 2 [Magnusson 2009] \param[in,out] trans_cloud transformed point cloud, \f$ X \f$ + * transformed by \f$ T(\vec{p},\vec{x}) \f$ in Algorithm 2 [Magnusson 2009] \return final step + * length + */ + double computeStepLengthMT( + const Eigen::Matrix & x, Eigen::Matrix & step_dir, double step_init, + double step_max, double step_min, double & score, Eigen::Matrix & score_gradient, + Eigen::Matrix & hessian, PointCloudSource & trans_cloud); + + /** \brief Update interval of possible step lengths for More-Thuente method, \f$ I \f$ in + * More-Thuente (1994) \note Updating Algorithm until some value satisfies \f$ \psi(\alpha_k) \leq + * 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$ and Modified Updating Algorithm from then on [More, + * Thuente 1994]. \param[in,out] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$ in + * Moore-Thuente (1994) \param[in,out] f_l value at first endpoint, \f$ f_l \f$ in Moore-Thuente + * (1994), \f$ \psi(\alpha_l) \f$ for Update Algorithm and \f$ \phi(\alpha_l) \f$ for Modified + * Update Algorithm \param[in,out] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente + * (1994), \f$ \psi'(\alpha_l) \f$ for Update Algorithm and \f$ \phi'(\alpha_l) \f$ for Modified + * Update Algorithm \param[in,out] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$ in + * Moore-Thuente (1994) \param[in,out] f_u value at second endpoint, \f$ f_u \f$ in Moore-Thuente + * (1994), \f$ \psi(\alpha_u) \f$ for Update Algorithm and \f$ \phi(\alpha_u) \f$ for Modified + * Update Algorithm \param[in,out] g_u derivative at second endpoint, \f$ g_u \f$ in Moore-Thuente + * (1994), \f$ \psi'(\alpha_u) \f$ for Update Algorithm and \f$ \phi'(\alpha_u) \f$ for Modified + * Update Algorithm \param[in] a_t trial value, \f$ \alpha_t \f$ in Moore-Thuente (1994) + * \param[in] f_t value at trial value, \f$ f_t \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_t) + * \f$ for Update Algorithm and \f$ \phi(\alpha_t) \f$ for Modified Update Algorithm \param[in] + * g_t derivative at trial value, \f$ g_t \f$ in Moore-Thuente (1994), \f$ \psi'(\alpha_t) \f$ for + * Update Algorithm and \f$ \phi'(\alpha_t) \f$ for Modified Update Algorithm \return if interval + * converges + */ + bool updateIntervalMT( + double & a_l, double & f_l, double & g_l, double & a_u, double & f_u, double & g_u, double a_t, + double f_t, double g_t); + + /** \brief Select new trial value for More-Thuente method. + * \note Trial Value Selection [More, Thuente 1994], \f$ \psi(\alpha_k) \f$ is used for \f$ f_k + * \f$ and \f$ g_k \f$ until some value satisfies the test \f$ \psi(\alpha_k) \leq 0 \f$ and \f$ + * \phi'(\alpha_k) \geq 0 \f$ then \f$ \phi(\alpha_k) \f$ is used from then on. \note + * Interpolation Minimizer equations from Optimization Theory and Methods: Nonlinear Programming + * By Wenyu Sun, Ya-xiang Yuan (89-100). \param[in] a_l first endpoint of interval \f$ I \f$, \f$ + * \alpha_l \f$ in Moore-Thuente (1994) \param[in] f_l value at first endpoint, \f$ f_l \f$ in + * Moore-Thuente (1994) \param[in] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente + * (1994) \param[in] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$ in Moore-Thuente + * (1994) \param[in] f_u value at second endpoint, \f$ f_u \f$ in Moore-Thuente (1994) \param[in] + * g_u derivative at second endpoint, \f$ g_u \f$ in Moore-Thuente (1994) \param[in] a_t previous + * trial value, \f$ \alpha_t \f$ in Moore-Thuente (1994) \param[in] f_t value at previous trial + * value, \f$ f_t \f$ in Moore-Thuente (1994) \param[in] g_t derivative at previous trial value, + * \f$ g_t \f$ in Moore-Thuente (1994) \return new trial value + */ + double trialValueSelectionMT( + double a_l, double f_l, double g_l, double a_u, double f_u, double g_u, double a_t, double f_t, + double g_t); + + /** \brief Auxiliary function used to determine endpoints of More-Thuente interval. + * \note \f$ \psi(\alpha) \f$ in Equation 1.6 (Moore, Thuente 1994) + * \param[in] a the step length, \f$ \alpha \f$ in More-Thuente (1994) + * \param[in] f_a function value at step length a, \f$ \phi(\alpha) \f$ in More-Thuente (1994) + * \param[in] f_0 initial function value, \f$ \phi(0) \f$ in Moore-Thuente (1994) + * \param[in] g_0 initial function gradient, \f$ \phi'(0) \f$ in More-Thuente (1994) + * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More, Thuente 1994] + * \return sufficient decrease value + */ + inline double auxiliaryFunction_PsiMT( + double a, double f_a, double f_0, double g_0, double mu = 1.e-4) + { + return (f_a - f_0 - mu * g_0 * a); + } + + /** \brief Auxiliary function derivative used to determine endpoints of More-Thuente interval. + * \note \f$ \psi'(\alpha) \f$, derivative of Equation 1.6 (Moore, Thuente 1994) + * \param[in] g_a function gradient at step length a, \f$ \phi'(\alpha) \f$ in More-Thuente (1994) + * \param[in] g_0 initial function gradient, \f$ \phi'(0) \f$ in More-Thuente (1994) + * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More, Thuente 1994] + * \return sufficient decrease derivative + */ + inline double auxiliaryFunction_dPsiMT(double g_a, double g_0, double mu = 1.e-4) + { + return (g_a - mu * g_0); + } + + /** \brief The voxel grid generated from target cloud containing point means and covariances. */ + TargetGrid target_cells_; + + // double fitness_epsilon_; + + /** \brief The ratio of outliers of points w.r.t. a normal distribution, Equation 6.7 [Magnusson + * 2009]. */ + double outlier_ratio_; + + /** \brief The normalization constants used fit the point distribution to a normal distribution, + * Equation 6.8 [Magnusson 2009]. */ + double gauss_d1_, gauss_d2_, gauss_d3_; + + /** \brief The probability score of the transform applied to the input cloud, Equation 6.9 + * and 6.10 [Magnusson 2009]. */ + double trans_probability_; + + /** \brief Precomputed Angular Gradient + * + * The precomputed angular derivatives for the jacobian of a transformation vector, Equation 6.19 + * [Magnusson 2009]. + */ + Eigen::Matrix j_ang_; + + /** \brief Precomputed Angular Hessian + * + * The precomputed angular derivatives for the hessian of a transformation vector, Equation 6.19 + * [Magnusson 2009]. + */ + Eigen::Matrix h_ang_; + + Eigen::Matrix hessian_; + std::vector> transformation_array_; + std::vector transform_probability_array_; + std::vector nearest_voxel_transformation_likelihood_array_; + double nearest_voxel_transformation_likelihood_{}; + + boost::optional regularization_pose_; + Eigen::Vector3f regularization_pose_translation_; + + NdtParams params_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace pclomp + +#endif // AUTOWARE__NDT_SCAN_MATCHER__NDT_OMP__MULTIGRID_NDT_OMP_H_ diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/ndt_struct.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/ndt_struct.hpp new file mode 100644 index 0000000000000..51c8d47b2a46d --- /dev/null +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/ndt_struct.hpp @@ -0,0 +1,107 @@ +// Copyright 2024 TIER IV, Inc. +// +// 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. +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef AUTOWARE__NDT_SCAN_MATCHER__NDT_OMP__NDT_STRUCT_HPP_ +#define AUTOWARE__NDT_SCAN_MATCHER__NDT_OMP__NDT_STRUCT_HPP_ + +#include +#include + +#include + +namespace pclomp +{ + +enum NeighborSearchMethod { KDTREE, DIRECT26, DIRECT7, DIRECT1 }; + +struct NdtResult +{ + Eigen::Matrix4f pose; + float transform_probability; + float nearest_voxel_transformation_likelihood; + int iteration_num; + std::vector> transformation_array; + Eigen::Matrix hessian; + std::vector transform_probability_array; + std::vector nearest_voxel_transformation_likelihood_array; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + friend std::ostream & operator<<(std::ostream & os, const NdtResult & val) + { + os << "Pose: " << std::endl << val.pose << std::endl; + os << "TP: " << val.transform_probability << std::endl; + os << "NVTL: " << val.nearest_voxel_transformation_likelihood << std::endl; + os << "Iteration num: " << val.iteration_num << std::endl; + os << "Hessian: " << std::endl << val.hessian << std::endl; + return os; + } +}; + +struct NdtParams +{ + double trans_epsilon{}; + double step_size{}; + double resolution{}; + int max_iterations{}; + NeighborSearchMethod search_method{}; + int num_threads{}; + float regularization_scale_factor{}; + + // line search is false by default + // "use_lines_search = true" is not tested well + bool use_line_search = false; +}; + +} // namespace pclomp + +#endif // AUTOWARE__NDT_SCAN_MATCHER__NDT_OMP__NDT_STRUCT_HPP_ diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp index c47af70f4c189..22b6bfb2ff740 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -19,8 +19,9 @@ #include "autoware/localization_util/diagnostics_module.hpp" #include "autoware/localization_util/smart_pose_buffer.hpp" -#include "autoware/ndt_scan_matcher/hyper_parameters.hpp" -#include "autoware/ndt_scan_matcher/map_update_module.hpp" +#include "hyper_parameters.hpp" +#include "map_update_module.hpp" +#include "ndt_omp/multigrid_ndt_omp.h" #include #include @@ -38,7 +39,6 @@ #include #include -#include #include #include #include diff --git a/localization/autoware_ndt_scan_matcher/package.xml b/localization/autoware_ndt_scan_matcher/package.xml index a4e5d71fa5c59..1d7cdadbd93c1 100644 --- a/localization/autoware_ndt_scan_matcher/package.xml +++ b/localization/autoware_ndt_scan_matcher/package.xml @@ -25,7 +25,6 @@ geometry_msgs libpcl-all-dev nav_msgs - ndt_omp pcl_conversions rclcpp rclcpp_components diff --git a/localization/autoware_ndt_scan_matcher/src/ndt_omp/estimate_covariance.cpp b/localization/autoware_ndt_scan_matcher/src/ndt_omp/estimate_covariance.cpp new file mode 100644 index 0000000000000..8145bfa873b86 --- /dev/null +++ b/localization/autoware_ndt_scan_matcher/src/ndt_omp/estimate_covariance.cpp @@ -0,0 +1,203 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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/ndt_scan_matcher/ndt_omp/estimate_covariance.hpp" + +#include +#include + +namespace pclomp +{ + +Eigen::Matrix2d estimate_xy_covariance_by_laplace_approximation( + const Eigen::Matrix & hessian) +{ + const Eigen::Matrix2d hessian_xy = hessian.block<2, 2>(0, 0); + Eigen::Matrix2d covariance_xy = -hessian_xy.inverse(); + return covariance_xy; +} + +ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt( + const NdtResult & ndt_result, + const std::shared_ptr< + pclomp::MultiGridNormalDistributionsTransform> & ndt_ptr, + const std::vector & poses_to_search) +{ + // initialize by the main result + const Eigen::Vector2d ndt_pose_2d(ndt_result.pose(0, 3), ndt_result.pose(1, 3)); + std::vector ndt_pose_2d_vec{ndt_pose_2d}; + + // multiple searches + std::vector ndt_results; + for (const Eigen::Matrix4f & curr_pose : poses_to_search) { + auto sub_output_cloud = std::make_shared>(); + ndt_ptr->align(*sub_output_cloud, curr_pose); + const NdtResult sub_ndt_result = ndt_ptr->getResult(); + ndt_results.push_back(sub_ndt_result); + + const Eigen::Matrix4f sub_ndt_pose = sub_ndt_result.pose; + const Eigen::Vector2d sub_ndt_pose_2d = sub_ndt_pose.topRightCorner<2, 1>().cast(); + ndt_pose_2d_vec.emplace_back(sub_ndt_pose_2d); + } + + // calculate the weights + const int n = static_cast(ndt_results.size()) + 1; + const std::vector weight_vec(n, 1.0 / n); + + // calculate mean and covariance + auto [mean, covariance] = calculate_weighted_mean_and_cov(ndt_pose_2d_vec, weight_vec); + + // unbiased covariance + covariance *= static_cast(n - 1) / n; + + return {mean, covariance, poses_to_search, ndt_results}; +} + +ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt_score( + const NdtResult & ndt_result, + const std::shared_ptr< + pclomp::MultiGridNormalDistributionsTransform> & ndt_ptr, + const std::vector & poses_to_search, const double temperature) +{ + // initialize by the main result + const Eigen::Vector2d ndt_pose_2d(ndt_result.pose(0, 3), ndt_result.pose(1, 3)); + std::vector ndt_pose_2d_vec{ndt_pose_2d}; + std::vector score_vec{ndt_result.nearest_voxel_transformation_likelihood}; + + const pcl::PointCloud::ConstPtr input_cloud = ndt_ptr->getInputCloud(); + pcl::PointCloud trans_cloud; + + // multiple searches + std::vector ndt_results; + for (const Eigen::Matrix4f & curr_pose : poses_to_search) { + const Eigen::Vector2d sub_ndt_pose_2d = curr_pose.topRightCorner<2, 1>().cast(); + ndt_pose_2d_vec.emplace_back(sub_ndt_pose_2d); + + transformPointCloud(*input_cloud, trans_cloud, curr_pose); + const double nvtl = ndt_ptr->calculateNearestVoxelTransformationLikelihood(trans_cloud); + score_vec.emplace_back(nvtl); + + NdtResult sub_ndt_result{}; + sub_ndt_result.pose = curr_pose; + sub_ndt_result.iteration_num = 0; + sub_ndt_result.nearest_voxel_transformation_likelihood = static_cast(nvtl); + ndt_results.push_back(sub_ndt_result); + } + + // calculate the weights + const std::vector weight_vec = calc_weight_vec(score_vec, temperature); + + // calculate mean and covariance + const auto [mean, covariance] = calculate_weighted_mean_and_cov(ndt_pose_2d_vec, weight_vec); + return {mean, covariance, poses_to_search, ndt_results}; +} + +Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes( + const Eigen::Matrix2d & matrix) +{ + const Eigen::SelfAdjointEigenSolver eigensolver(matrix); + if (eigensolver.info() == Eigen::Success) { + const Eigen::Vector2d eigen_vec = eigensolver.eigenvectors().col(0); + const double th = std::atan2(eigen_vec.y(), eigen_vec.x()); + return Eigen::Rotation2Dd(th).toRotationMatrix(); + } + throw std::runtime_error("Eigen solver failed. Return output_pose_covariance value."); +} + +std::vector propose_poses_to_search( + const NdtResult & ndt_result, const std::vector & offset_x, + const std::vector & offset_y) +{ + assert(offset_x.size() == offset_y.size()); + const Eigen::Matrix4f & center_pose = ndt_result.pose; + + // (1) calculate rot by pose (default) + const Eigen::Matrix2d rot = ndt_result.pose.topLeftCorner<2, 2>().cast(); + + // (2) calculate rot by covariance (alternative) + // const Eigen::Matrix & hessian = ndt_result.hessian; + // const Eigen::Matrix2d covariance = estimate_xy_covariance_by_Laplace_approximation(hessian); + // const Eigen::Matrix2d rot = + // find_rotation_matrix_aligning_covariance_to_principal_axes(-covariance); + + std::vector poses_to_search; + for (int i = 0; i < static_cast(offset_x.size()); i++) { + const Eigen::Vector2d pose_offset(offset_x[i], offset_y[i]); + const Eigen::Vector2d rotated_pose_offset_2d = rot * pose_offset; + Eigen::Matrix4f curr_pose = center_pose; + curr_pose(0, 3) += static_cast(rotated_pose_offset_2d.x()); + curr_pose(1, 3) += static_cast(rotated_pose_offset_2d.y()); + poses_to_search.emplace_back(curr_pose); + } + return poses_to_search; +} + +std::vector calc_weight_vec(const std::vector & score_vec, double temperature) +{ + const int n = static_cast(score_vec.size()); + const double max_score = *std::max_element(score_vec.begin(), score_vec.end()); + std::vector exp_score_vec(n); + double exp_score_sum = 0.0; + for (int i = 0; i < n; i++) { + exp_score_vec[i] = std::exp((score_vec[i] - max_score) / temperature); + exp_score_sum += exp_score_vec[i]; + } + for (int i = 0; i < n; i++) { + exp_score_vec[i] /= exp_score_sum; + } + return exp_score_vec; +} + +std::pair calculate_weighted_mean_and_cov( + const std::vector & pose_2d_vec, const std::vector & weight_vec) +{ + const int n = static_cast(pose_2d_vec.size()); + Eigen::Vector2d mean = Eigen::Vector2d::Zero(); + for (int i = 0; i < n; i++) { + mean += weight_vec[i] * pose_2d_vec[i]; + } + Eigen::Matrix2d covariance = Eigen::Matrix2d::Zero(); + for (int i = 0; i < n; i++) { + const Eigen::Vector2d diff = pose_2d_vec[i] - mean; + covariance += weight_vec[i] * diff * diff.transpose(); + } + return {mean, covariance}; +} + +Eigen::Matrix2d rotate_covariance_to_base_link( + const Eigen::Matrix2d & covariance, const Eigen::Matrix4f & pose) +{ + const Eigen::Matrix2d rot = pose.topLeftCorner<2, 2>().cast(); + return rot.transpose() * covariance * rot; +} + +Eigen::Matrix2d rotate_covariance_to_map( + const Eigen::Matrix2d & covariance, const Eigen::Matrix4f & pose) +{ + const Eigen::Matrix2d rot = pose.topLeftCorner<2, 2>().cast(); + return rot * covariance * rot.transpose(); +} + +Eigen::Matrix2d adjust_diagonal_covariance( + const Eigen::Matrix2d & covariance, const Eigen::Matrix4f & pose, const double fixed_cov00, + const double fixed_cov11) +{ + Eigen::Matrix2d cov_base_link = rotate_covariance_to_base_link(covariance, pose); + cov_base_link(0, 0) = std::max(cov_base_link(0, 0), fixed_cov00); + cov_base_link(1, 1) = std::max(cov_base_link(1, 1), fixed_cov11); + Eigen::Matrix2d cov_map = rotate_covariance_to_map(cov_base_link, pose); + return cov_map; +} + +} // namespace pclomp diff --git a/localization/autoware_ndt_scan_matcher/src/ndt_omp/multi_voxel_grid_covariance_omp.cpp b/localization/autoware_ndt_scan_matcher/src/ndt_omp/multi_voxel_grid_covariance_omp.cpp new file mode 100644 index 0000000000000..e9fe39dc523f3 --- /dev/null +++ b/localization/autoware_ndt_scan_matcher/src/ndt_omp/multi_voxel_grid_covariance_omp.cpp @@ -0,0 +1,18 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "multi_voxel_grid_covariance_omp_impl.hpp" + +template class pclomp::MultiVoxelGridCovariance; +template class pclomp::MultiVoxelGridCovariance; diff --git a/localization/autoware_ndt_scan_matcher/src/ndt_omp/multi_voxel_grid_covariance_omp_impl.hpp b/localization/autoware_ndt_scan_matcher/src/ndt_omp/multi_voxel_grid_covariance_omp_impl.hpp new file mode 100644 index 0000000000000..ef64471dd4784 --- /dev/null +++ b/localization/autoware_ndt_scan_matcher/src/ndt_omp/multi_voxel_grid_covariance_omp_impl.hpp @@ -0,0 +1,477 @@ +// Copyright 2022 TIER IV, Inc. +// +// 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. + +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef NDT_OMP__MULTI_VOXEL_GRID_COVARIANCE_OMP_IMPL_HPP_ +#define NDT_OMP__MULTI_VOXEL_GRID_COVARIANCE_OMP_IMPL_HPP_ + +// cspell:ignore evecs, evals, covar, eigvalue, futs + +#include "autoware/ndt_scan_matcher/ndt_omp/multi_voxel_grid_covariance_omp.h" + +#include +#include + +#include +#include +#include +#include +#include + +namespace pclomp +{ + +template +MultiVoxelGridCovariance::MultiVoxelGridCovariance(const MultiVoxelGridCovariance & other) +: pcl::VoxelGrid(other), + sid_to_iid_(other.sid_to_iid_), + grid_list_(other.grid_list_), + kdtree_(other.kdtree_), + leaf_ptrs_(other.leaf_ptrs_) +{ + min_points_per_voxel_ = other.min_points_per_voxel_; + min_covar_eigvalue_mult_ = other.min_covar_eigvalue_mult_; + + // Deep copy + voxel_centroids_ptr_.reset(new PointCloud); + + if (other.voxel_centroids_ptr_) { + *voxel_centroids_ptr_ = *other.voxel_centroids_ptr_; + } + + setThreadNum(other.thread_num_); + last_check_tid_ = -1; +} + +template +MultiVoxelGridCovariance::MultiVoxelGridCovariance( + MultiVoxelGridCovariance && other) noexcept +: pcl::VoxelGrid(std::move(other)), + voxel_centroids_ptr_(std::move(other.voxel_centroids_ptr_)), + sid_to_iid_(std::move(other.sid_to_iid_)), + grid_list_(std::move(other.grid_list_)), + kdtree_(std::move(other.kdtree_)), + leaf_ptrs_(std::move(other.leaf_ptrs_)) +{ + min_points_per_voxel_ = other.min_points_per_voxel_; + min_covar_eigvalue_mult_ = other.min_covar_eigvalue_mult_; + + setThreadNum(other.thread_num_); + last_check_tid_ = -1; +} + +template +MultiVoxelGridCovariance & pclomp::MultiVoxelGridCovariance::operator=( + const MultiVoxelGridCovariance & other) +{ + pcl::VoxelGrid::operator=(other); + voxel_centroids_ptr_ = other.voxel_centroids_ptr_; + sid_to_iid_ = other.sid_to_iid_; + grid_list_ = other.grid_list_; + kdtree_ = other.kdtree_; + leaf_ptrs_ = other.leaf_ptrs_; + min_points_per_voxel_ = other.min_points_per_voxel_; + min_covar_eigvalue_mult_ = other.min_covar_eigvalue_mult_; + + // Deep copy + voxel_centroids_ptr_.reset(new PointCloud); + + if (other.voxel_centroids_ptr_) { + *voxel_centroids_ptr_ = *other.voxel_centroids_ptr_; + } + + setThreadNum(other.thread_num_); + last_check_tid_ = -1; + + return *this; +} + +template +MultiVoxelGridCovariance & pclomp::MultiVoxelGridCovariance::operator=( + MultiVoxelGridCovariance && other) noexcept +{ + voxel_centroids_ptr_ = std::move(other.voxel_centroids_ptr_); + sid_to_iid_ = std::move(other.sid_to_iid_); + grid_list_ = std::move(other.grid_list_); + kdtree_ = std::move(other.kdtree_); + leaf_ptrs_ = std::move(other.leaf_ptrs_); + + min_points_per_voxel_ = other.min_points_per_voxel_; + min_covar_eigvalue_mult_ = other.min_covar_eigvalue_mult_; + + setThreadNum(other.thread_num_); + last_check_tid_ = -1; + + pcl::VoxelGrid::operator=(std::move(other)); + + return *this; +} + +template +void MultiVoxelGridCovariance::setInputCloudAndFilter( + const PointCloudConstPtr & cloud, const std::string & grid_id) +{ + // Creat a new grid and push it to the back of the vector grid_list_ + // The vector grid_list_ will be rebuilt after we remove obsolete grids + GridNodePtr new_grid(new GridNodeType); + + grid_list_.push_back(new_grid); + + sid_to_iid_[grid_id] = grid_list_.size() - 1; + + // If single thread, no parallel processing + if (thread_num_ == 1) { + apply_filter(cloud, *new_grid); + } else { + int idle_tid = get_idle_tid(); + + processing_inputs_[idle_tid] = cloud; + thread_futs_[idle_tid] = std::async( + std::launch::async, &MultiVoxelGridCovariance::apply_filter_thread, this, idle_tid, + std::ref(*new_grid)); + } +} + +template +void MultiVoxelGridCovariance::removeCloud(const std::string & grid_id) +{ + auto iid = sid_to_iid_.find(grid_id); + + if (iid == sid_to_iid_.end()) { + return; + } + + // Set the pointer corresponding to the specified grid to null + grid_list_[iid->second].reset(); + // Remove the specified grid from the conversion map + sid_to_iid_.erase(iid); +} + +template +void MultiVoxelGridCovariance::createKdtree() +{ + // Wait for all threads to finish + sync(); + + // Rebuild the grid_list_ and sid_to_iid_ to delete the data related to the removed clouds + const int new_grid_num = sid_to_iid_.size(); + std::vector new_grid_list(new_grid_num); + int new_pos = 0; + int total_leaf_num = 0; + + for (auto & it : sid_to_iid_) { + int & old_pos = it.second; + auto & grid_ptr = grid_list_[old_pos]; + + new_grid_list[new_pos] = grid_ptr; + old_pos = new_pos; + ++new_pos; + total_leaf_num = grid_ptr->size(); + } + + grid_list_ = std::move(new_grid_list); + + // Rebuild the voxel_centroids_ptr_ + voxel_centroids_ptr_.reset(new PointCloud); + voxel_centroids_ptr_->reserve(total_leaf_num); + leaf_ptrs_.clear(); + leaf_ptrs_.reserve(total_leaf_num); + + for (const auto & grid_ptr : grid_list_) { + for (const auto & leaf : *grid_ptr) { + PointT new_leaf; + + new_leaf.x = leaf.centroid_[0]; + new_leaf.y = leaf.centroid_[1]; + new_leaf.z = leaf.centroid_[2]; + voxel_centroids_ptr_->push_back(new_leaf); + leaf_ptrs_.push_back(&leaf); + } + } + + // Rebuild the kdtree_ of leaves + if (voxel_centroids_ptr_->size() > 0) { + kdtree_.setInputCloud(voxel_centroids_ptr_); + } +} + +template +int MultiVoxelGridCovariance::radiusSearch( + const PointT & point, double radius, std::vector & k_leaves, + unsigned int max_nn) const +{ + k_leaves.clear(); + + // Search from the kdtree to find neighbors of @point + std::vector k_sqr_distances; + std::vector k_indices; + const int k = kdtree_.radiusSearch(point, radius, k_indices, k_sqr_distances, max_nn); + + if (k <= 0) { + return 0; + } + + k_leaves.reserve(k); + + for (auto & nn_idx : k_indices) { + k_leaves.push_back(leaf_ptrs_[nn_idx]); + } + + return k_leaves.size(); +} + +template +int MultiVoxelGridCovariance::radiusSearch( + const PointCloud & cloud, int index, double radius, std::vector & k_leaves, + unsigned int max_nn) const +{ + if (index >= static_cast(cloud.size()) || index < 0) { + return 0; + } + + return (radiusSearch(cloud[index], radius, k_leaves, max_nn)); +} + +template +typename MultiVoxelGridCovariance::PointCloud +MultiVoxelGridCovariance::getVoxelPCD() const +{ + return *voxel_centroids_ptr_; +} + +template +std::vector MultiVoxelGridCovariance::getCurrentMapIDs() const +{ + std::vector output; + + output.reserve(sid_to_iid_.size()); + + for (const auto & element : sid_to_iid_) { + output.push_back(element.first); + } + + return output; +} + +////////////////////////////////////////////////////////////////////////////////////////// +template +void MultiVoxelGridCovariance::apply_filter( + const PointCloudConstPtr & input, GridNodeType & node) +{ + // Has the input dataset been set already? + if (!input) { + PCL_WARN("[pcl::%s::apply_filter] No input dataset given!\n", getClassName().c_str()); + return; + } + + Eigen::Vector4f min_p; + Eigen::Vector4f max_p; + pcl::getMinMax3D(*input, min_p, max_p); + + // Check that the leaf size is not too small, given the size of the data + int64_t dx = static_cast((max_p[0] - min_p[0]) * inverse_leaf_size_[0]) + 1; + int64_t dy = static_cast((max_p[1] - min_p[1]) * inverse_leaf_size_[1]) + 1; + int64_t dz = static_cast((max_p[2] - min_p[2]) * inverse_leaf_size_[2]) + 1; + + if ((dx * dy * dz) > std::numeric_limits::max()) { + PCL_WARN( + "[pcl::%s::apply_filter] Leaf size is too small for the input dataset. Integer indices would " + "overflow.", + getClassName().c_str()); + return; + } + + // Compute the minimum and maximum bounding box values + BoundingBox bbox; + bbox.min[0] = static_cast(floor(min_p[0] * inverse_leaf_size_[0])); + bbox.max[0] = static_cast(floor(max_p[0] * inverse_leaf_size_[0])); + bbox.min[1] = static_cast(floor(min_p[1] * inverse_leaf_size_[1])); + bbox.max[1] = static_cast(floor(max_p[1] * inverse_leaf_size_[1])); + bbox.min[2] = static_cast(floor(min_p[2] * inverse_leaf_size_[2])); + bbox.max[2] = static_cast(floor(max_p[2] * inverse_leaf_size_[2])); + + // Compute the number of divisions needed along all axis + Eigen::Vector4i div_b = bbox.max - bbox.min + Eigen::Vector4i::Ones(); + div_b[3] = 0; + + // Clear the leaves + node.clear(); + + // Set up the division multiplier + bbox.div_mul = Eigen::Vector4i(1, div_b[0], div_b[0] * div_b[1], 0); + + int centroid_size = 4; + std::map map_leaves; + + // First pass: go over all points and insert them into the right leaf + for (auto & p : *input) { + if (!input->is_dense) { + // Check if the point is invalid + if (!std::isfinite(p.x) || !std::isfinite(p.y) || !std::isfinite(p.z)) continue; + } + int64_t lid = getLeafID(p, bbox); + Leaf & leaf = map_leaves[lid]; + updateLeaf(p, centroid_size, leaf); + } + + // Second pass: go over all leaves and compute centroids and covariance matrices + + // Eigen values and vectors calculated to prevent near singular matrices + Eigen::SelfAdjointEigenSolver eigensolver; + Eigen::Vector3d pt_sum; + + node.reserve(map_leaves.size()); + + // Eigen values less than a threshold of max eigen value are inflated to a set fraction of the max + // eigen value. + for (auto & it : map_leaves) { + // Skip leaves that do not have enough points + if (it.second.nr_points_ < min_points_per_voxel_) { + continue; + } + + // Append qualified leaves to the end of the output vector + node.push_back(it.second); + + // Normalize the centroid + Leaf & leaf = node.back(); + + // Normalize the centroid + leaf.centroid_ /= static_cast(leaf.nr_points_); + // Point sum used for single pass covariance calculation + pt_sum = leaf.mean_; + // Normalize mean + leaf.mean_ /= leaf.nr_points_; + // Compute covariance matrices + computeLeafParams(pt_sum, eigensolver, leaf); + } +} + +template +int64_t pclomp::MultiVoxelGridCovariance::getLeafID( + const PointT & point, const BoundingBox & bbox) const +{ + int ijk0 = + static_cast(floor(point.x * inverse_leaf_size_[0]) - static_cast(bbox.min[0])); + int ijk1 = + static_cast(floor(point.y * inverse_leaf_size_[1]) - static_cast(bbox.min[1])); + int ijk2 = + static_cast(floor(point.z * inverse_leaf_size_[2]) - static_cast(bbox.min[2])); + + return ijk0 * bbox.div_mul[0] + ijk1 * bbox.div_mul[1] + ijk2 * bbox.div_mul[2]; +} + +template +void pclomp::MultiVoxelGridCovariance::updateLeaf( + const PointT & point, const int & centroid_size, Leaf & leaf) const +{ + if (leaf.nr_points_ == 0) { + leaf.centroid_.resize(centroid_size); + leaf.centroid_.setZero(); + } + + Eigen::Vector3d pt3d(point.x, point.y, point.z); + // Accumulate point sum for centroid calculation + leaf.mean_ += pt3d; + // Accumulate x*xT for single pass covariance calculation + leaf.cov_ += pt3d * pt3d.transpose(); + + Eigen::Vector4f pt(point.x, point.y, point.z, 0); + leaf.centroid_.template head<4>() += pt; + ++leaf.nr_points_; +} + +template +void pclomp::MultiVoxelGridCovariance::computeLeafParams( + const Eigen::Vector3d & pt_sum, Eigen::SelfAdjointEigenSolver & eigensolver, + Leaf & leaf) const +{ + // Single pass covariance calculation + leaf.cov_ = (leaf.cov_ - 2 * (pt_sum * leaf.mean_.transpose())) / leaf.nr_points_ + + leaf.mean_ * leaf.mean_.transpose(); + leaf.cov_ *= (leaf.nr_points_ - 1.0) / leaf.nr_points_; + + // Normalize Eigen Val such that max no more than 100x min. + eigensolver.compute(leaf.cov_); + Eigen::Matrix3d eigen_val = eigensolver.eigenvalues().asDiagonal(); + leaf.evecs_ = eigensolver.eigenvectors(); + + if (eigen_val(0, 0) < 0 || eigen_val(1, 1) < 0 || eigen_val(2, 2) <= 0) { + leaf.nr_points_ = -1; + return; + } + + // Avoids matrices near singularities (eq 6.11)[Magnusson 2009] + double min_covar_eigvalue = min_covar_eigvalue_mult_ * eigen_val(2, 2); + if (eigen_val(0, 0) < min_covar_eigvalue) { + eigen_val(0, 0) = min_covar_eigvalue; + + if (eigen_val(1, 1) < min_covar_eigvalue) { + eigen_val(1, 1) = min_covar_eigvalue; + } + + leaf.cov_ = leaf.evecs_ * eigen_val * leaf.evecs_.inverse(); + } + leaf.evals_ = eigen_val.diagonal(); + + leaf.icov_ = leaf.cov_.inverse(); + if ( + leaf.icov_.maxCoeff() == std::numeric_limits::infinity() || + leaf.icov_.minCoeff() == -std::numeric_limits::infinity()) { + leaf.nr_points_ = -1; + } +} + +} // namespace pclomp + +#define PCL_INSTANTIATE_VoxelGridCovariance(T) \ + template class PCL_EXPORTS pcl::VoxelGridCovariance; + +#endif // NDT_OMP__MULTI_VOXEL_GRID_COVARIANCE_OMP_IMPL_HPP_ diff --git a/localization/autoware_ndt_scan_matcher/src/ndt_omp/multigrid_ndt_omp.cpp b/localization/autoware_ndt_scan_matcher/src/ndt_omp/multigrid_ndt_omp.cpp new file mode 100644 index 0000000000000..8a096cbf5ac92 --- /dev/null +++ b/localization/autoware_ndt_scan_matcher/src/ndt_omp/multigrid_ndt_omp.cpp @@ -0,0 +1,18 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "multigrid_ndt_omp_impl.hpp" + +template class pclomp::MultiGridNormalDistributionsTransform; +template class pclomp::MultiGridNormalDistributionsTransform; diff --git a/localization/autoware_ndt_scan_matcher/src/ndt_omp/multigrid_ndt_omp_impl.hpp b/localization/autoware_ndt_scan_matcher/src/ndt_omp/multigrid_ndt_omp_impl.hpp new file mode 100644 index 0000000000000..3c5af25186bf4 --- /dev/null +++ b/localization/autoware_ndt_scan_matcher/src/ndt_omp/multigrid_ndt_omp_impl.hpp @@ -0,0 +1,1243 @@ +// Copyright 2022 TIER IV, Inc. +// +// 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. + +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef NDT_OMP__MULTIGRID_NDT_OMP_IMPL_HPP_ +#define NDT_OMP__MULTIGRID_NDT_OMP_IMPL_HPP_ + +// cspell:ignore multigrid, nnvn, colj + +#include "autoware/ndt_scan_matcher/ndt_omp/multigrid_ndt_omp.h" + +#include +#include +#include + +namespace pclomp +{ + +template +MultiGridNormalDistributionsTransform:: + MultiGridNormalDistributionsTransform(const MultiGridNormalDistributionsTransform & other) +: BaseRegType(other), target_cells_(other.target_cells_) +{ + params_ = other.params_; + outlier_ratio_ = other.outlier_ratio_; + gauss_d1_ = other.gauss_d1_; + gauss_d2_ = other.gauss_d2_; + gauss_d3_ = other.gauss_d3_; + trans_probability_ = other.trans_probability_; + // No need to copy j_ang_ and h_ang_, as those matrices are re-computed on every + // computeDerivatives() call + + hessian_ = other.hessian_; + transformation_array_ = other.transformation_array_; + transform_probability_array_ = other.transform_probability_array_; + nearest_voxel_transformation_likelihood_array_ = + other.nearest_voxel_transformation_likelihood_array_; + nearest_voxel_transformation_likelihood_ = other.nearest_voxel_transformation_likelihood_; + + regularization_pose_ = other.regularization_pose_; + regularization_pose_translation_ = other.regularization_pose_translation_; +} + +template +MultiGridNormalDistributionsTransform:: + MultiGridNormalDistributionsTransform(MultiGridNormalDistributionsTransform && other) noexcept +: BaseRegType(std::move(other)), + target_cells_(std::move(other.target_cells_)), + params_(std::move(other.params_)) +{ + outlier_ratio_ = other.outlier_ratio_; + gauss_d1_ = other.gauss_d1_; + gauss_d2_ = other.gauss_d2_; + gauss_d3_ = other.gauss_d3_; + trans_probability_ = other.trans_probability_; + + hessian_ = other.hessian_; + transformation_array_ = other.transformation_array_; + transform_probability_array_ = other.transform_probability_array_; + nearest_voxel_transformation_likelihood_array_ = + other.nearest_voxel_transformation_likelihood_array_; + nearest_voxel_transformation_likelihood_ = other.nearest_voxel_transformation_likelihood_; + + regularization_pose_ = other.regularization_pose_; + regularization_pose_translation_ = other.regularization_pose_translation_; +} + +template +MultiGridNormalDistributionsTransform & +MultiGridNormalDistributionsTransform::operator=( + const MultiGridNormalDistributionsTransform & other) +{ + target_cells_ = other.target_cells_; + params_ = other.params_; + + outlier_ratio_ = other.outlier_ratio_; + gauss_d1_ = other.gauss_d1_; + gauss_d2_ = other.gauss_d2_; + gauss_d3_ = other.gauss_d3_; + trans_probability_ = other.trans_probability_; + + hessian_ = other.hessian_; + transformation_array_ = other.transformation_array_; + transform_probability_array_ = other.transform_probability_array_; + nearest_voxel_transformation_likelihood_array_ = + other.nearest_voxel_transformation_likelihood_array_; + nearest_voxel_transformation_likelihood_ = other.nearest_voxel_transformation_likelihood_; + + regularization_pose_ = other.regularization_pose_; + regularization_pose_translation_ = other.regularization_pose_translation_; + + BaseRegType::operator=(other); + + return *this; +} + +template +MultiGridNormalDistributionsTransform & +MultiGridNormalDistributionsTransform::operator=( + MultiGridNormalDistributionsTransform && other) noexcept +{ + target_cells_ = std::move(other.target_cells_); + params_ = std::move(other.params_); + + outlier_ratio_ = other.outlier_ratio_; + gauss_d1_ = other.gauss_d1_; + gauss_d2_ = other.gauss_d2_; + gauss_d3_ = other.gauss_d3_; + trans_probability_ = other.trans_probability_; + + hessian_ = other.hessian_; + transformation_array_ = other.transformation_array_; + transform_probability_array_ = other.transform_probability_array_; + nearest_voxel_transformation_likelihood_array_ = + other.nearest_voxel_transformation_likelihood_array_; + nearest_voxel_transformation_likelihood_ = other.nearest_voxel_transformation_likelihood_; + + regularization_pose_ = other.regularization_pose_; + regularization_pose_translation_ = other.regularization_pose_translation_; + + BaseRegType::operator=(std::move(other)); + + return *this; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +MultiGridNormalDistributionsTransform< + PointSource, PointTarget>::MultiGridNormalDistributionsTransform() +: target_cells_(), + outlier_ratio_(0.55), + gauss_d1_(), + gauss_d2_(), + gauss_d3_(), + trans_probability_(), + regularization_pose_(boost::none) +{ + reg_name_ = "MultiGridNormalDistributionsTransform"; + + params_.trans_epsilon = 0.1; + params_.step_size = 0.1; + params_.resolution = 1.0f; + params_.max_iterations = 35; + params_.search_method = KDTREE; // Only KDTREE is supported in multigrid_ndt_omp + params_.num_threads = omp_get_max_threads(); + params_.regularization_scale_factor = 0.0f; + params_.use_line_search = false; + + // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009] + double gauss_c1 = 10.0 * (1 - outlier_ratio_); + double gauss_c2 = outlier_ratio_ / pow(params_.resolution, 3); + gauss_d3_ = -log(gauss_c2); + gauss_d1_ = -log(gauss_c1 + gauss_c2) - gauss_d3_; + gauss_d2_ = -2 * log((-log(gauss_c1 * exp(-0.5) + gauss_c2) - gauss_d3_) / gauss_d1_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +void MultiGridNormalDistributionsTransform::computeTransformation( + PointCloudSource & output, const Eigen::Matrix4f & guess) +{ + nr_iterations_ = 0; + converged_ = false; + + // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009] + double gauss_c1 = 10 * (1 - outlier_ratio_); + double gauss_c2 = outlier_ratio_ / pow(params_.resolution, 3); + gauss_d3_ = -log(gauss_c2); + gauss_d1_ = -log(gauss_c1 + gauss_c2) - gauss_d3_; + gauss_d2_ = -2.0 * log((-log(gauss_c1 * exp(-0.5) + gauss_c2) - gauss_d3_) / gauss_d1_); + + if (guess != Eigen::Matrix4f::Identity()) { + // Initialise final transformation to the guessed one + final_transformation_ = guess; + // Apply guessed transformation prior to search for neighbours + transformPointCloud(output, output, guess); + } + + Eigen::Transform eig_transformation; + + eig_transformation.matrix() = final_transformation_; + transformation_array_.clear(); + transformation_array_.push_back(final_transformation_); + + transform_probability_array_.clear(); + nearest_voxel_transformation_likelihood_array_.clear(); + + // Convert initial guess matrix to 6 element transformation vector + Eigen::Matrix p; + Eigen::Matrix delta_p; + Eigen::Matrix score_gradient; + Eigen::Vector3f init_translation = eig_transformation.translation(); + Eigen::Vector3f init_rotation = eig_transformation.rotation().eulerAngles(0, 1, 2); + + p << init_translation(0), init_translation(1), init_translation(2), init_rotation(0), + init_rotation(1), init_rotation(2); + + Eigen::Matrix hessian; + + double score = 0; + + if (regularization_pose_) { + Eigen::Transform regularization_pose_transformation; + regularization_pose_transformation.matrix() = regularization_pose_.get(); + regularization_pose_translation_ = regularization_pose_transformation.translation(); + } + + // Calculate derivatives of initial transform vector, subsequent derivative calculations are done + // in the step length determination. + score = computeDerivatives(score_gradient, hessian, output, p); + + while (!converged_) { + // Store previous transformation + previous_transformation_ = transformation_; + + // Solve for decent direction using newton method, line 23 in Algorithm 2 [Magnusson 2009] + Eigen::JacobiSVD> sv( + hessian, Eigen::ComputeFullU | Eigen::ComputeFullV); + // Negative for maximization as opposed to minimization + delta_p = sv.solve(-score_gradient); + + // Calculate step length with guaranteed sufficient decrease [More, Thuente 1994] + double delta_p_norm = delta_p.norm(); + + if (delta_p_norm == 0 || delta_p_norm != delta_p_norm) { + if (input_->empty()) { + trans_probability_ = 0.0f; + } else { + trans_probability_ = score / static_cast(input_->size()); + } + + converged_ = delta_p_norm == delta_p_norm; + return; + } + + delta_p.normalize(); + delta_p_norm = computeStepLengthMT( + p, delta_p, delta_p_norm, params_.step_size, params_.trans_epsilon / 2.0, score, + score_gradient, hessian, output); + delta_p *= delta_p_norm; + + transformation_ = + (Eigen::Translation( + static_cast(delta_p(0)), static_cast(delta_p(1)), + static_cast(delta_p(2))) * + Eigen::AngleAxis(static_cast(delta_p(3)), Eigen::Vector3f::UnitX()) * + Eigen::AngleAxis(static_cast(delta_p(4)), Eigen::Vector3f::UnitY()) * + Eigen::AngleAxis(static_cast(delta_p(5)), Eigen::Vector3f::UnitZ())) + .matrix(); + + transformation_array_.push_back(final_transformation_); + + p = p + delta_p; + + // Update Visualizer (untested) + if (update_visualizer_ != 0) + update_visualizer_(output, std::vector(), *target_, std::vector()); + + nr_iterations_++; + + if ( + nr_iterations_ >= params_.max_iterations || + (nr_iterations_ && (std::fabs(delta_p_norm) < params_.trans_epsilon))) { + converged_ = true; + } + } + + // Store transformation probability. The relative differences within each scan registration are + // accurate but the normalization constants need to be modified for it to be globally accurate + if (input_->empty()) { + trans_probability_ = 0.0f; + } else { + trans_probability_ = score / static_cast(input_->size()); + } + + hessian_ = hessian; +} + +#ifndef _OPENMP +int omp_get_max_threads() +{ + return 1; +} +int omp_get_thread_num() +{ + return 0; +} +#endif + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +double MultiGridNormalDistributionsTransform::computeDerivatives( + Eigen::Matrix & score_gradient, Eigen::Matrix & hessian, + PointCloudSource & trans_cloud, Eigen::Matrix & p, bool compute_hessian) +{ + score_gradient.setZero(); + hessian.setZero(); + + double score = 0; + int total_neighborhood_count = 0; + double nearest_voxel_score = 0; + size_t found_neighborhood_voxel_num = 0; + + std::vector scores(params_.num_threads); + std::vector nearest_voxel_scores(params_.num_threads); + std::vector found_neighborhood_voxel_nums(params_.num_threads); + std::vector, Eigen::aligned_allocator>> + score_gradients(params_.num_threads); + std::vector, Eigen::aligned_allocator>> + hessians(params_.num_threads); + std::vector neighborhood_counts(params_.num_threads); + + // Pre-allocate thread-wise point derivative matrices to avoid reallocate too many times + std::vector> t_point_gradients(params_.num_threads); + std::vector> t_point_hessians(params_.num_threads); + + for (int i = 0; i < params_.num_threads; ++i) { + scores[i] = 0; + nearest_voxel_scores[i] = 0; + found_neighborhood_voxel_nums[i] = 0; + score_gradients[i].setZero(); + hessians[i].setZero(); + neighborhood_counts[i] = 0; + + // Initialize point derivatives + t_point_gradients[i].setZero(); + t_point_gradients[i].block<3, 3>(0, 0).setIdentity(); + t_point_hessians[i].setZero(); + } + + // Precompute Angular Derivatives (eq. 6.19 and 6.21)[Magnusson 2009] + computeAngleDerivatives(p); + + // Update gradient and hessian for each point, line 17 in Algorithm 2 [Magnusson 2009] +#pragma omp parallel for num_threads(params_.num_threads) schedule(guided, 8) + for (size_t idx = 0; idx < input_->size(); ++idx) { + int tid = omp_get_thread_num(); + // Searching for neighbors of the current transformed point + auto & x_trans_pt = trans_cloud[idx]; + std::vector neighborhood; + + // Neighborhood search method other than kdtree is disabled in multigrid_ndt_omp + target_cells_.radiusSearch(x_trans_pt, params_.resolution, neighborhood); + + if (neighborhood.empty()) { + continue; + } + + // Original Point + auto & x_pt = (*input_)[idx]; + // Original Point and Transformed Point (for math) + Eigen::Vector3d x(x_pt.x, x_pt.y, x_pt.z); + // Current Point Gradient and Hessian + auto & point_gradient = t_point_gradients[tid]; + auto & point_hessian = t_point_hessians[tid]; + + // Compute derivative of transform function w.r.t. transform vector, J_E and H_E in + // Equations 6.18 and 6.20 [Magnusson 2009] + computePointDerivatives(x, point_gradient, point_hessian); + + // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009] + const Eigen::Vector3d x_trans(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z); + + double sum_score_pt = 0; + double nearest_voxel_score_pt = 0; + auto & score_gradient_pt = score_gradients[tid]; + auto & hessian_pt = hessians[tid]; + + for (auto & cell : neighborhood) { + // Update score, gradient and hessian, lines 19-21 in Algorithm 2, according to + // Equations 6.10, 6.12 and 6.13, respectively [Magnusson 2009] + double score_pt = updateDerivatives( + score_gradient_pt, hessian_pt, point_gradient, point_hessian, x_trans - cell->getMean(), + cell->getInverseCov(), compute_hessian); + sum_score_pt += score_pt; + + if (score_pt > nearest_voxel_score_pt) { + nearest_voxel_score_pt = score_pt; + } + } + + ++found_neighborhood_voxel_nums[tid]; + + scores[tid] += sum_score_pt; + nearest_voxel_scores[tid] += nearest_voxel_score_pt; + neighborhood_counts[tid] += neighborhood.size(); + } + + // Ensure that the result is invariant against the summing up order + for (int i = 0; i < params_.num_threads; ++i) { + score += scores[i]; + nearest_voxel_score += nearest_voxel_scores[i]; + found_neighborhood_voxel_num += found_neighborhood_voxel_nums[i]; + score_gradient += score_gradients[i]; + hessian += hessians[i]; + total_neighborhood_count += neighborhood_counts[i]; + } + + if (regularization_pose_) { + float regularization_score = 0.0f; + Eigen::Matrix regularization_gradient = Eigen::Matrix::Zero(); + Eigen::Matrix regularization_hessian = Eigen::Matrix::Zero(); + + const float dx = regularization_pose_translation_(0) - static_cast(p(0, 0)); + const float dy = regularization_pose_translation_(1) - static_cast(p(1, 0)); + const auto sin_yaw = static_cast(sin(p(5, 0))); + const auto cos_yaw = static_cast(cos(p(5, 0))); + const float longitudinal_distance = dy * sin_yaw + dx * cos_yaw; + const auto neighborhood_count_weight = static_cast(total_neighborhood_count); + + regularization_score = -params_.regularization_scale_factor * neighborhood_count_weight * + longitudinal_distance * longitudinal_distance; + + regularization_gradient(0, 0) = params_.regularization_scale_factor * + neighborhood_count_weight * 2.0f * cos_yaw * + longitudinal_distance; + regularization_gradient(1, 0) = params_.regularization_scale_factor * + neighborhood_count_weight * 2.0f * sin_yaw * + longitudinal_distance; + + regularization_hessian(0, 0) = + -params_.regularization_scale_factor * neighborhood_count_weight * 2.0f * cos_yaw * cos_yaw; + regularization_hessian(0, 1) = + -params_.regularization_scale_factor * neighborhood_count_weight * 2.0f * cos_yaw * sin_yaw; + regularization_hessian(1, 1) = + -params_.regularization_scale_factor * neighborhood_count_weight * 2.0f * sin_yaw * sin_yaw; + regularization_hessian(1, 0) = regularization_hessian(0, 1); + + score += regularization_score; + score_gradient += regularization_gradient; + hessian += regularization_hessian; + } + + if (found_neighborhood_voxel_num != 0) { + nearest_voxel_transformation_likelihood_ = + nearest_voxel_score / static_cast(found_neighborhood_voxel_num); + } else { + nearest_voxel_transformation_likelihood_ = 0.0; + } + nearest_voxel_transformation_likelihood_array_.push_back( + nearest_voxel_transformation_likelihood_); + const float transform_probability = + (input_->points.empty() ? 0.0f : score / static_cast(input_->points.size())); + transform_probability_array_.push_back(transform_probability); + return (score); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +void MultiGridNormalDistributionsTransform::computeAngleDerivatives( + Eigen::Matrix & p, bool compute_hessian) +{ + // Simplified math for near 0 angles + double cx = 1.0; + double cy = 1.0; + double cz = 1.0; + double sx = 0.0; + double sy = 0.0; + double sz = 0.0; + if (fabs(p(3)) < 10e-5) { + // p(3) = 0; + cx = 1.0; + sx = 0.0; + } else { + cx = cos(p(3)); + sx = sin(p(3)); + } + if (fabs(p(4)) < 10e-5) { + // p(4) = 0; + cy = 1.0; + sy = 0.0; + } else { + cy = cos(p(4)); + sy = sin(p(4)); + } + + if (fabs(p(5)) < 10e-5) { + // p(5) = 0; + cz = 1.0; + sz = 0.0; + } else { + cz = cos(p(5)); + sz = sin(p(5)); + } + + // Precomputed angular gradient components. Letters correspond to Equation 6.19 [Magnusson 2009] + j_ang_.setZero(); + + j_ang_.row(0) << (-sx * sz + cx * sy * cz), (-sx * cz - cx * sy * sz), (-cx * cy), 0.0f; + j_ang_.row(1) << (cx * sz + sx * sy * cz), (cx * cz - sx * sy * sz), (-sx * cy), 0.0f; + j_ang_.row(2) << (-sy * cz), sy * sz, cy, 0.0f; + j_ang_.row(3) << sx * cy * cz, (-sx * cy * sz), sx * sy, 0.0f; + j_ang_.row(4) << (-cx * cy * cz), cx * cy * sz, (-cx * sy), 0.0f; + j_ang_.row(5) << (-cy * sz), (-cy * cz), 0.0f, 0.0f; + j_ang_.row(6) << (cx * cz - sx * sy * sz), (-cx * sz - sx * sy * cz), 0.0f, 0.0f; + j_ang_.row(7) << (sx * cz + cx * sy * sz), (cx * sy * cz - sx * sz), 0.0f, 0.0f; + + if (compute_hessian) { + // Precomputed angular hessian components. Letters correspond to Equation 6.21 and numbers + // correspond to row index [Magnusson 2009] + h_ang_.setZero(); + + h_ang_.row(0) << (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), sx * cy, 0.0f; // a2 + h_ang_.row(1) << (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), (-cx * cy), 0.0f; // a3 + + h_ang_.row(2) << (cx * cy * cz), (-cx * cy * sz), (cx * sy), 0.0f; // b2 + h_ang_.row(3) << (sx * cy * cz), (-sx * cy * sz), (sx * sy), 0.0f; // b3 + + h_ang_.row(4) << (-sx * cz - cx * sy * sz), (sx * sz - cx * sy * cz), 0.0f, 0.0f; // c2 + h_ang_.row(5) << (cx * cz - sx * sy * sz), (-sx * sy * cz - cx * sz), 0.0f, 0.0f; // c3 + + h_ang_.row(6) << (-cy * cz), (cy * sz), (sy), 0.0f; // d1 + h_ang_.row(7) << (-sx * sy * cz), (sx * sy * sz), (sx * cy), 0.0f; // d2 + h_ang_.row(8) << (cx * sy * cz), (-cx * sy * sz), (-cx * cy), 0.0f; // d3 + + h_ang_.row(9) << (sy * sz), (sy * cz), 0.0f, 0.0f; // e1 + h_ang_.row(10) << (-sx * cy * sz), (-sx * cy * cz), 0.0f, 0.0f; // e2 + h_ang_.row(11) << (cx * cy * sz), (cx * cy * cz), 0.0f, 0.0f; // e3 + + h_ang_.row(12) << (-cy * cz), (cy * sz), 0.0f, 0.0f; // f1 + h_ang_.row(13) << (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), 0.0f, 0.0f; // f2 + h_ang_.row(14) << (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), 0.0f, 0.0f; // f3 + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +void MultiGridNormalDistributionsTransform::computePointDerivatives( + Eigen::Vector3d & x, Eigen::Matrix & point_gradient, + Eigen::Matrix & point_hessian, bool compute_hessian) const +{ + Eigen::Vector4d x4(x[0], x[1], x[2], 0.0f); + + // Calculate first derivative of Transformation Equation 6.17 w.r.t. transform vector p. + // Derivative w.r.t. ith element of transform vector corresponds to column i, Equation 6.18 + // and 6.19 [Magnusson 2009] + auto x_j_ang = j_ang_ * x4; + + point_gradient(1, 3) = x_j_ang[0]; + point_gradient(2, 3) = x_j_ang[1]; + point_gradient(0, 4) = x_j_ang[2]; + point_gradient(1, 4) = x_j_ang[3]; + point_gradient(2, 4) = x_j_ang[4]; + point_gradient(0, 5) = x_j_ang[5]; + point_gradient(1, 5) = x_j_ang[6]; + point_gradient(2, 5) = x_j_ang[7]; + + if (compute_hessian) { + auto x_h_ang = h_ang_ * x4; + + // Vectors from Equation 6.21 [Magnusson 2009] + Eigen::Vector4d a(0.0f, x_h_ang[0], x_h_ang[1], 0.0f); + Eigen::Vector4d b(0.0f, x_h_ang[2], x_h_ang[3], 0.0f); + Eigen::Vector4d c(0.0f, x_h_ang[4], x_h_ang[5], 0.0f); + Eigen::Vector4d d(x_h_ang[6], x_h_ang[7], x_h_ang[8], 0.0f); + Eigen::Vector4d e(x_h_ang[9], x_h_ang[10], x_h_ang[11], 0.0f); + Eigen::Vector4d f(x_h_ang[12], x_h_ang[13], x_h_ang[14], 0.0f); + + // Calculate second derivative of Transformation Equation 6.17 w.r.t. transform vector p. + // Derivative w.r.t. ith and jth elements of transform vector corresponds to the 3x1 block + // matrix starting at (3i,j), Equation 6.20 and 6.21 [Magnusson 2009] + point_hessian.block<4, 1>(12, 3) = a; + point_hessian.block<4, 1>(16, 3) = b; + point_hessian.block<4, 1>(20, 3) = c; + point_hessian.block<4, 1>(12, 4) = b; + point_hessian.block<4, 1>(16, 4) = d; + point_hessian.block<4, 1>(20, 4) = e; + point_hessian.block<4, 1>(12, 5) = c; + point_hessian.block<4, 1>(16, 5) = e; + point_hessian.block<4, 1>(20, 5) = f; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +double MultiGridNormalDistributionsTransform::updateDerivatives( + Eigen::Matrix & score_gradient, Eigen::Matrix & hessian, + const Eigen::Matrix & point_gradient, + const Eigen::Matrix & point_hessian, const Eigen::Vector3d & x_trans, + const Eigen::Matrix3d & c_inv, bool compute_hessian) const +{ + Eigen::Matrix x_trans4(x_trans[0], x_trans[1], x_trans[2], 0.0f); + Eigen::Matrix4d c_inv4 = Eigen::Matrix4d::Zero(); + + c_inv4.topLeftCorner(3, 3) = c_inv; + + // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009] + double e_x_cov_x = exp(-gauss_d2_ * x_trans4.dot(x_trans4 * c_inv4) * 0.5f); + // Calculate probability of transformed points existence, Equation 6.9 [Magnusson 2009] + double score_inc = -gauss_d1_ * e_x_cov_x; + + e_x_cov_x = gauss_d2_ * e_x_cov_x; + + // Error checking for invalid values. + if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x) return (0); + + // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] + e_x_cov_x *= gauss_d1_; + + Eigen::Matrix c_inv4_x_point_gradient4 = c_inv4 * point_gradient; + Eigen::Matrix x_trans4_dot_c_inv4_x_point_gradient4 = + x_trans4 * c_inv4_x_point_gradient4; + + score_gradient.noalias() += (e_x_cov_x * x_trans4_dot_c_inv4_x_point_gradient4).cast(); + + if (compute_hessian) { + Eigen::Matrix x_trans4_x_c_inv4 = x_trans4 * c_inv4; + Eigen::Matrix point_gradient4_colj_dot_c_inv4_x_point_gradient4_col_i = + point_gradient.transpose() * c_inv4_x_point_gradient4; + Eigen::Matrix x_trans4_dot_c_inv4_x_ext_point_hessian_4ij; + + for (int i = 0; i < 6; ++i) { + // Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] + // Update gradient, Equation 6.12 [Magnusson 2009] + x_trans4_dot_c_inv4_x_ext_point_hessian_4ij.noalias() = + x_trans4_x_c_inv4 * point_hessian.block<4, 6>(i * 4, 0); + + for (int j = 0; j < hessian.cols(); j++) { + // Update hessian, Equation 6.13 [Magnusson 2009] + hessian(i, j) += + e_x_cov_x * (-gauss_d2_ * x_trans4_dot_c_inv4_x_point_gradient4(i) * + x_trans4_dot_c_inv4_x_point_gradient4(j) + + x_trans4_dot_c_inv4_x_ext_point_hessian_4ij(j) + + point_gradient4_colj_dot_c_inv4_x_point_gradient4_col_i(j, i)); + } + } + } + + return (score_inc); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +void MultiGridNormalDistributionsTransform::computeHessian( + Eigen::Matrix & hessian, PointCloudSource & trans_cloud, + Eigen::Matrix &) +{ + // Initialize Point Gradient and Hessian + // Pre-allocate thread-wise point gradients and point hessians + std::vector> t_point_gradients(params_.num_threads); + std::vector> t_point_hessians(params_.num_threads); + std::vector> t_hessians(params_.num_threads); + + for (int i = 0; i < params_.num_threads; ++i) { + t_point_gradients[i].setZero(); + t_point_gradients[i].block<3, 3>(0, 0).setIdentity(); + t_point_hessians[i].setZero(); + t_hessians[i].setZero(); + } + + hessian.setZero(); + + // Precompute Angular Derivatives unnecessary because only used after regular derivative + // calculation + + // Update hessian for each point, line 17 in Algorithm 2 [Magnusson 2009] +#pragma omp parallel for num_threads(params_.num_threads) schedule(guided, 8) + for (size_t idx = 0; idx < input_->size(); ++idx) { + int tid = omp_get_thread_num(); + auto & x_trans_pt = trans_cloud[idx]; + + // Find neighbors (Radius search has been experimentally faster than direct neighbor checking. + std::vector neighborhood; + + // Neighborhood search method other than kdtree is disabled in multigrid_ndt_omp + target_cells_.radiusSearch(x_trans_pt, params_.resolution, neighborhood); + + if (neighborhood.empty()) { + continue; + } + + auto & x_pt = (*input_)[idx]; + // For math + Eigen::Vector3d x(x_pt.x, x_pt.y, x_pt.z); + const Eigen::Vector3d x_trans(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z); + + auto & point_gradient = t_point_gradients[tid]; + auto & point_hessian = t_point_hessians[tid]; + auto & tmp_hessian = t_hessians[tid]; + + // Compute derivative of transform function w.r.t. transform vector, J_E and H_E in + // Equations 6.18 and 6.20 [Magnusson 2009] + computePointDerivatives(x, point_gradient, point_hessian); + + for (const auto & cell : neighborhood) { + // Update hessian, lines 21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13, + // respectively [Magnusson 2009] + updateHessian( + tmp_hessian, point_gradient, point_hessian, x_trans - cell->getMean(), + cell->getInverseCov()); + } + } + + // Sum over t_hessians + for (const auto & th : t_hessians) { + hessian += th; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +void MultiGridNormalDistributionsTransform::updateHessian( + Eigen::Matrix & hessian, const Eigen::Matrix & point_gradient, + const Eigen::Matrix & point_hessian, const Eigen::Vector3d & x_trans, + const Eigen::Matrix3d & c_inv) const +{ + Eigen::Vector3d cov_dxd_pi; + // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009] + double e_x_cov_x = gauss_d2_ * exp(-gauss_d2_ * x_trans.dot(c_inv * x_trans) / 2); + + // Error checking for invalid values. + if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x) return; + + // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] + e_x_cov_x *= gauss_d1_; + + for (int i = 0; i < 6; ++i) { + // Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] + cov_dxd_pi = c_inv * point_gradient.block<3, 1>(0, i); + + for (int j = 0; j < hessian.cols(); j++) { + // Update hessian, Equation 6.13 [Magnusson 2009] + Eigen::Vector3d pg_col = point_gradient.block<3, 1>(0, j); + + hessian(i, j) += + e_x_cov_x * + (-gauss_d2_ * x_trans.dot(cov_dxd_pi) * x_trans.dot(c_inv * pg_col) + + x_trans.dot(c_inv * point_hessian.block<3, 1>(3 * i, j)) + pg_col.dot(cov_dxd_pi)); + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +bool MultiGridNormalDistributionsTransform::updateIntervalMT( + double & a_l, double & f_l, double & g_l, double & a_u, double & f_u, double & g_u, double a_t, + double f_t, double g_t) +{ + // Case U1 in Update Algorithm and Case a in Modified Update Algorithm [More, Thuente 1994] + if (f_t > f_l) { + a_u = a_t; + f_u = f_t; + g_u = g_t; + return (false); + } + // Case U2 in Update Algorithm and Case b in Modified Update Algorithm [More, Thuente 1994] + if (g_t * (a_l - a_t) > 0) { + a_l = a_t; + f_l = f_t; + g_l = g_t; + return (false); + } + // Case U3 in Update Algorithm and Case c in Modified Update Algorithm [More, Thuente 1994] + if (g_t * (a_l - a_t) < 0) { + a_u = a_l; + f_u = f_l; + g_u = g_l; + + a_l = a_t; + f_l = f_t; + g_l = g_t; + return (false); + } + // Interval Converged + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +double MultiGridNormalDistributionsTransform::trialValueSelectionMT( + double a_l, double f_l, double g_l, double a_u, double f_u, double g_u, double a_t, double f_t, + double g_t) +{ + // Case 1 in Trial Value Selection [More, Thuente 1994] + if (f_t > f_l) { + // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t + // Equation 2.4.52 [Sun, Yuan 2006] + double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l; + double w = std::sqrt(z * z - g_t * g_l); + // Equation 2.4.56 [Sun, Yuan 2006] + double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w); + + // Calculate the minimizer of the quadratic that interpolates f_l, f_t and g_l + // Equation 2.4.2 [Sun, Yuan 2006] + double a_q = a_l - 0.5 * (a_l - a_t) * g_l / (g_l - (f_l - f_t) / (a_l - a_t)); + + if (std::fabs(a_c - a_l) < std::fabs(a_q - a_l)) { + return (a_c); + } + return (0.5 * (a_q + a_c)); + } + // Case 2 in Trial Value Selection [More, Thuente 1994] + if (g_t * g_l < 0) { + // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t + // Equation 2.4.52 [Sun, Yuan 2006] + double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l; + double w = std::sqrt(z * z - g_t * g_l); + // Equation 2.4.56 [Sun, Yuan 2006] + double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w); + + // Calculate the minimizer of the quadratic that interpolates f_l, g_l and g_t + // Equation 2.4.5 [Sun, Yuan 2006] + double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l; + + if (std::fabs(a_c - a_t) >= std::fabs(a_s - a_t)) { + return (a_c); + } + return (a_s); + } + // Case 3 in Trial Value Selection [More, Thuente 1994] + if (std::fabs(g_t) <= std::fabs(g_l)) { + // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t + // Equation 2.4.52 [Sun, Yuan 2006] + double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l; + double w = std::sqrt(z * z - g_t * g_l); + double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w); + + // Calculate the minimizer of the quadratic that interpolates g_l and g_t + // Equation 2.4.5 [Sun, Yuan 2006] + double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l; + + double a_t_next = 0.0; + + if (std::fabs(a_c - a_t) < std::fabs(a_s - a_t)) + a_t_next = a_c; + else + a_t_next = a_s; + + if (a_t > a_l) { + return (std::min(a_t + 0.66 * (a_u - a_t), a_t_next)); + } + return (std::max(a_t + 0.66 * (a_u - a_t), a_t_next)); + } + // Case 4 in Trial Value Selection [More, Thuente 1994] + // Calculate the minimizer of the cubic that interpolates f_u, f_t, g_u and g_t + // Equation 2.4.52 [Sun, Yuan 2006] + double z = 3 * (f_t - f_u) / (a_t - a_u) - g_t - g_u; + double w = std::sqrt(z * z - g_t * g_u); + // Equation 2.4.56 [Sun, Yuan 2006] + return (a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w)); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +double MultiGridNormalDistributionsTransform::computeStepLengthMT( + const Eigen::Matrix & x, Eigen::Matrix & step_dir, double step_init, + double step_max, double step_min, double & score, Eigen::Matrix & score_gradient, + Eigen::Matrix & hessian, PointCloudSource & trans_cloud) +{ + // Set the value of phi'(0), Equation 1.3 [More, Thuente 1994] + double d_phi_0 = -(score_gradient.dot(step_dir)); + + Eigen::Matrix x_t; + + if (d_phi_0 >= 0) { + // Not a decent direction + if (d_phi_0 == 0) return 0; + + // Reverse step direction and calculate optimal step. + d_phi_0 *= -1; + step_dir *= -1; + } + + double a_t = step_init; + a_t = std::min(a_t, step_max); + a_t = std::max(a_t, step_min); + + x_t = x + step_dir * a_t; + + final_transformation_ = + (Eigen::Translation( + static_cast(x_t(0)), static_cast(x_t(1)), static_cast(x_t(2))) * + Eigen::AngleAxis(static_cast(x_t(3)), Eigen::Vector3f::UnitX()) * + Eigen::AngleAxis(static_cast(x_t(4)), Eigen::Vector3f::UnitY()) * + Eigen::AngleAxis(static_cast(x_t(5)), Eigen::Vector3f::UnitZ())) + .matrix(); + + // New transformed point cloud + transformPointCloud(*input_, trans_cloud, final_transformation_); + + // Updates score, gradient and hessian. Hessian calculation is unnecessary but testing showed + // that most step calculations use the initial step suggestion and recalculation the reusable + // portions of the hessian would entail more computation time. + score = computeDerivatives(score_gradient, hessian, trans_cloud, x_t, true); + + // -------------------------------------------------------------------------------------------------------------------------------- + // FIXME(YamatoAndo): + // Currently, using LineSearch causes the following problems: + // It may converge to a local solution. + // There are cases where a loop process is executed without changing the variable values, + // resulting in unnecessary processing time. As better convergence results are obtained without + // using LineSearch, we have decided to disable this feature. If you have any suggestions on how + // to solve these issues, we would appreciate it if you could contribute. + // -------------------------------------------------------------------------------------------------------------------------------- + int step_iterations = 0; + + if (params_.use_line_search) { + // Set the value of phi(0), Equation 1.3 [More, Thuente 1994] + double phi_0 = -score; + + // The Search Algorithm for T(mu) [More, Thuente 1994] + + int max_step_iterations = 10; + + // Sufficient decrease constant, Equation 1.1 [More, Thuete 1994] + double mu = 1.e-4; + // Curvature condition constant, Equation 1.2 [More, Thuete 1994] + double nu = 0.9; + + // Initial endpoints of Interval I, + double a_l = 0; + double a_u = 0; + + // Auxiliary function psi is used until I is determined ot be a closed interval, Equation 2.1 + // [More, Thuente 1994] + double f_l = auxiliaryFunction_PsiMT(a_l, phi_0, phi_0, d_phi_0, mu); + double g_l = auxiliaryFunction_dPsiMT(d_phi_0, d_phi_0, mu); + + double f_u = auxiliaryFunction_PsiMT(a_u, phi_0, phi_0, d_phi_0, mu); + double g_u = auxiliaryFunction_dPsiMT(d_phi_0, d_phi_0, mu); + + // Check used to allow More-Thuente step length calculation to be skipped by making step_min == + // step_max + bool interval_converged = (step_max - step_min) < 0; + bool open_interval = true; + + // Calculate phi(alpha_t) + double phi_t = -score; + // Calculate phi'(alpha_t) + double d_phi_t = -(score_gradient.dot(step_dir)); + + // Calculate psi(alpha_t) + double psi_t = auxiliaryFunction_PsiMT(a_t, phi_t, phi_0, d_phi_0, mu); + // Calculate psi'(alpha_t) + double d_psi_t = auxiliaryFunction_dPsiMT(d_phi_t, d_phi_0, mu); + + // Iterate until max number of iterations, interval convergence or a value satisfies the + // sufficient decrease, Equation 1.1, and curvature condition, Equation 1.2 [More, Thuente 1994] + while ( + !interval_converged && step_iterations < max_step_iterations && + !(psi_t <= 0 /*Sufficient Decrease*/ && d_phi_t <= -nu * d_phi_0 /*Curvature Condition*/)) { + // Use auxiliary function if interval I is not closed + if (open_interval) { + a_t = trialValueSelectionMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, psi_t, d_psi_t); + } else { + a_t = trialValueSelectionMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, phi_t, d_phi_t); + } + + a_t = std::min(a_t, step_max); + a_t = std::max(a_t, step_min); + + x_t = x + step_dir * a_t; + + final_transformation_ = + (Eigen::Translation( + static_cast(x_t(0)), static_cast(x_t(1)), static_cast(x_t(2))) * + Eigen::AngleAxis(static_cast(x_t(3)), Eigen::Vector3f::UnitX()) * + Eigen::AngleAxis(static_cast(x_t(4)), Eigen::Vector3f::UnitY()) * + Eigen::AngleAxis(static_cast(x_t(5)), Eigen::Vector3f::UnitZ())) + .matrix(); + + // New transformed point cloud + // Done on final cloud to prevent wasted computation + transformPointCloud(*input_, trans_cloud, final_transformation_); + + // Updates score, gradient. Values stored to prevent wasted computation. + score = computeDerivatives(score_gradient, hessian, trans_cloud, x_t, false); + + // Calculate phi(alpha_t+) + phi_t = -score; + // Calculate phi'(alpha_t+) + d_phi_t = -(score_gradient.dot(step_dir)); + + // Calculate psi(alpha_t+) + psi_t = auxiliaryFunction_PsiMT(a_t, phi_t, phi_0, d_phi_0, mu); + // Calculate psi'(alpha_t+) + d_psi_t = auxiliaryFunction_dPsiMT(d_phi_t, d_phi_0, mu); + + // Check if I is now a closed interval + if (open_interval && (psi_t <= 0 && d_psi_t >= 0)) { + open_interval = false; + + // Converts f_l and g_l from psi to phi + f_l = f_l + phi_0 - mu * d_phi_0 * a_l; + g_l = g_l + mu * d_phi_0; + + // Converts f_u and g_u from psi to phi + f_u = f_u + phi_0 - mu * d_phi_0 * a_u; + g_u = g_u + mu * d_phi_0; + } + + if (open_interval) { + // Update interval end points using Updating Algorithm [More, Thuente 1994] + interval_converged = updateIntervalMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, psi_t, d_psi_t); + } else { + // Update interval end points using Modified Updating Algorithm [More, Thuente 1994] + interval_converged = updateIntervalMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, phi_t, d_phi_t); + } + + step_iterations++; + } + } + + // If inner loop was run then hessian needs to be calculated. + // Hessian is unnecessary for step length determination but gradients are required + // so derivative and transform data is stored for the next iteration. + if (step_iterations) computeHessian(hessian, trans_cloud, x_t); + + return (a_t); +} + +template +double +MultiGridNormalDistributionsTransform::calculateTransformationProbability( + const PointCloudSource & trans_cloud) const +{ + double score = 0; + + // Score per thread + std::vector t_scores(params_.num_threads, 0); + +#pragma omp parallel for num_threads(params_.num_threads) schedule(guided, 8) + for (size_t idx = 0; idx < trans_cloud.size(); ++idx) { + int tid = omp_get_thread_num(); + PointSource x_trans_pt = trans_cloud[idx]; + + // Find neighbors (Radius search has been experimentally faster than direct neighbor checking. + std::vector neighborhood; + + // Neighborhood search method other than kdtree is disabled in multigrid_ndt_omp + target_cells_.radiusSearch(x_trans_pt, params_.resolution, neighborhood); + + if (neighborhood.empty()) { + continue; + } + + double tmp_score = 0; + + for (auto & cell : neighborhood) { + Eigen::Vector3d x_trans(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z); + + // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009] + x_trans -= cell->getMean(); + + // Calculate probability of transformed points existence, Equation 6.9 [Magnusson 2009] + // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009] + tmp_score -= gauss_d1_ * exp(-gauss_d2_ * x_trans.dot(cell->getInverseCov() * x_trans) / 2.0); + } + + t_scores[tid] += tmp_score; + } + + // Sum the point-wise scores + for (const auto & ts : t_scores) { + score += ts; + } + + double output_score = 0; + if (!trans_cloud.empty()) { + output_score = (score) / static_cast(trans_cloud.size()); + } + return output_score; +} + +template +double MultiGridNormalDistributionsTransform:: + calculateNearestVoxelTransformationLikelihood(const PointCloudSource & trans_cloud) const +{ + double nearest_voxel_score = 0; + size_t found_neighborhood_voxel_num = 0; + + // Thread-wise results + std::vector t_nvs(params_.num_threads); + std::vector t_found_nnvn(params_.num_threads); + + for (int i = 0; i < params_.num_threads; ++i) { + t_nvs[i] = 0; + t_found_nnvn[i] = 0; + } + +#pragma omp parallel for num_threads(params_.num_threads) schedule(guided, 8) + for (size_t idx = 0; idx < trans_cloud.size(); ++idx) { + int tid = omp_get_thread_num(); + PointSource x_trans_pt = trans_cloud[idx]; + + // Find neighbors (Radius search has been experimentally faster than direct neighbor checking. + std::vector neighborhood; + + // Neighborhood search method other than kdtree is disabled in multigrid_ndt_omp + target_cells_.radiusSearch(x_trans_pt, params_.resolution, neighborhood); + + if (neighborhood.empty()) { + continue; + } + + double nearest_voxel_score_pt = 0; + + for (auto & cell : neighborhood) { + Eigen::Vector3d x_trans(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z); + + // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009] + x_trans -= cell->getMean(); + + // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009] + double e_x_cov_x = exp(-gauss_d2_ * x_trans.dot(cell->getInverseCov() * x_trans) / 2.0); + // Calculate probability of transformed points existence, Equation 6.9 [Magnusson 2009] + double score_inc = -gauss_d1_ * e_x_cov_x; + + if (score_inc > nearest_voxel_score_pt) { + nearest_voxel_score_pt = score_inc; + } + } + + t_nvs[tid] += nearest_voxel_score_pt; + ++t_found_nnvn[tid]; + } + + // Sum up point-wise scores + for (int idx = 0; idx < params_.num_threads; ++idx) { + found_neighborhood_voxel_num += t_found_nnvn[idx]; + nearest_voxel_score += t_nvs[idx]; + } + + double output_score = 0; + + if (found_neighborhood_voxel_num != 0) { + output_score = nearest_voxel_score / static_cast(found_neighborhood_voxel_num); + } + return output_score; +} + +template +pcl::PointCloud MultiGridNormalDistributionsTransform:: + calculateNearestVoxelScoreEachPoint(const PointCloudSource & trans_cloud) const +{ + // Thread-wise results + std::vector> threads_pc(params_.num_threads); + pcl::PointCloud score_points; + +#pragma omp parallel for num_threads(params_.num_threads) schedule(guided, 8) + for (size_t idx = 0; idx < trans_cloud.size(); ++idx) { + int tid = omp_get_thread_num(); + PointSource x_trans_pt = trans_cloud[idx]; + + // Find neighbors (Radius search has been experimentally faster than direct neighbor checking. + std::vector neighborhood; + + // Neighborhood search method other than kdtree is disabled in multigrid_ndt_omp + target_cells_.radiusSearch(x_trans_pt, params_.resolution, neighborhood); + + if (neighborhood.empty()) { + continue; + } + + double nearest_voxel_score_pt = 0; + + for (auto & cell : neighborhood) { + Eigen::Vector3d x_trans(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z); + + // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009] + x_trans -= cell->getMean(); + + // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009] + double e_x_cov_x = exp(-gauss_d2_ * x_trans.dot(cell->getInverseCov() * x_trans) / 2.0); + // Calculate probability of transformed points existence, Equation 6.9 [Magnusson 2009] + double score_inc = -gauss_d1_ * e_x_cov_x; + + if (score_inc > nearest_voxel_score_pt) { + nearest_voxel_score_pt = score_inc; + } + } + + pcl::PointXYZI sensor_point_score; + sensor_point_score.x = trans_cloud.points[idx].x; + sensor_point_score.y = trans_cloud.points[idx].y; + sensor_point_score.z = trans_cloud.points[idx].z; + sensor_point_score.intensity = nearest_voxel_score_pt; + threads_pc[tid].points.push_back(sensor_point_score); + } + + // Sum up point-wise scores + for (int idx = 0; idx < params_.num_threads; ++idx) { + for (size_t p_idx = 0; p_idx < threads_pc[idx].size(); ++p_idx) { + score_points.points.push_back(threads_pc[idx].points[p_idx]); + } + } + + return score_points; +} + +} // namespace pclomp + +#endif // NDT_OMP__MULTIGRID_NDT_OMP_IMPL_HPP_ diff --git a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 8cc397c4531b5..a316c8fa346b9 100644 --- a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -17,11 +17,11 @@ #include "autoware/localization_util/matrix_type.hpp" #include "autoware/localization_util/tree_structured_parzen_estimator.hpp" #include "autoware/localization_util/util_func.hpp" +#include "autoware/ndt_scan_matcher/ndt_omp/estimate_covariance.hpp" #include "autoware/ndt_scan_matcher/particle.hpp" #include #include -#include #include From 458f3d9c1b7d739295a5d81b8ed605f8c79d1b22 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 31 Oct 2024 19:19:12 +0900 Subject: [PATCH 08/96] feat(autoware_test_utils): add traffic light msgs parser (#9177) Signed-off-by: Mamoru Sobue --- .../autoware_test_utils/mock_data_parser.hpp | 23 ++++++++ .../src/mock_data_parser.cpp | 57 +++++++++++++++++++ .../test/test_mock_data_parser.cpp | 31 ++++++++++ 3 files changed, 111 insertions(+) diff --git a/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp b/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp index 714d6d6710b32..8fc71cf95e1ac 100644 --- a/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp +++ b/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp @@ -16,9 +16,11 @@ #define AUTOWARE_TEST_UTILS__MOCK_DATA_PARSER_HPP_ #include +#include #include #include +#include #include #include #include @@ -36,16 +38,22 @@ namespace autoware::test_utils { +using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjectKinematics; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; using autoware_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::TrafficLightElement; +using autoware_perception_msgs::msg::TrafficLightGroup; +using autoware_perception_msgs::msg::TrafficLightGroupArray; + using autoware_planning_msgs::msg::LaneletPrimitive; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::LaneletSegment; using builtin_interfaces::msg::Duration; +using builtin_interfaces::msg::Time; using geometry_msgs::msg::Accel; using geometry_msgs::msg::AccelWithCovariance; using geometry_msgs::msg::AccelWithCovarianceStamped; @@ -79,6 +87,9 @@ Header parse(const YAML::Node & node); template <> Duration parse(const YAML::Node & node); +template <> +Time parse(const YAML::Node & node); + template <> std::vector parse(const YAML::Node & node); @@ -142,6 +153,18 @@ PredictedObject parse(const YAML::Node & node); template <> PredictedObjects parse(const YAML::Node & node); +template <> +TrafficLightGroupArray parse(const YAML::Node & node); + +template <> +TrafficLightGroup parse(const YAML::Node & node); + +template <> +TrafficLightElement parse(const YAML::Node & node); + +template <> +OperationModeState parse(const YAML::Node & node); + /** * @brief Parses a YAML file and converts it into an object of type T. * diff --git a/common/autoware_test_utils/src/mock_data_parser.cpp b/common/autoware_test_utils/src/mock_data_parser.cpp index b876f1312a3e6..d2c6de5c0a46f 100644 --- a/common/autoware_test_utils/src/mock_data_parser.cpp +++ b/common/autoware_test_utils/src/mock_data_parser.cpp @@ -44,6 +44,15 @@ Duration parse(const YAML::Node & node) return msg; } +template <> +Time parse(const YAML::Node & node) +{ + Time msg; + msg.sec = node["sec"].as(); + msg.nanosec = node["nanosec"].as(); + return msg; +} + template <> std::array parse(const YAML::Node & node) { @@ -321,6 +330,54 @@ PredictedObjects parse(const YAML::Node & node) return msg; } +template <> +TrafficLightElement parse(const YAML::Node & node) +{ + TrafficLightElement msg; + msg.color = node["color"].as(); + msg.shape = node["shape"].as(); + msg.status = node["status"].as(); + msg.confidence = node["confidence"].as(); + return msg; +} + +template <> +TrafficLightGroup parse(const YAML::Node & node) +{ + TrafficLightGroup msg; + msg.traffic_light_group_id = node["traffic_light_group_id"].as(); + for (const auto & element_node : node["elements"]) { + msg.elements.push_back(parse(element_node)); + } + return msg; +} + +template <> +TrafficLightGroupArray parse(const YAML::Node & node) +{ + TrafficLightGroupArray msg; + msg.stamp = parse