diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml index 210e5d049727c..a36520eeee398 100644 --- a/launch/tier4_localization_launch/launch/localization.launch.xml +++ b/launch/tier4_localization_launch/launch/localization.launch.xml @@ -1,6 +1,7 @@ + @@ -27,10 +28,14 @@ - + + + + + diff --git a/launch/tier4_localization_launch/launch/pose_estimator/yabloc.launch.xml b/launch/tier4_localization_launch/launch/pose_estimator/yabloc.launch.xml new file mode 100644 index 0000000000000..ebee784510435 --- /dev/null +++ b/launch/tier4_localization_launch/launch/pose_estimator/yabloc.launch.xml @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index 5d060f884c01e..23806d62a680b 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -21,6 +21,10 @@ pointcloud_preprocessor pose_initializer topic_tools + yabloc_common + yabloc_image_processing + yabloc_particle_filter + yabloc_pose_initializer ament_lint_auto autoware_lint_common diff --git a/localization/pose_initializer/CMakeLists.txt b/localization/pose_initializer/CMakeLists.txt index d048cd1cce26f..81a60a1d6b717 100644 --- a/localization/pose_initializer/CMakeLists.txt +++ b/localization/pose_initializer/CMakeLists.txt @@ -9,6 +9,7 @@ ament_auto_add_executable(pose_initializer_node src/pose_initializer/pose_initializer_core.cpp src/pose_initializer/gnss_module.cpp src/pose_initializer/ndt_module.cpp + src/pose_initializer/yabloc_module.cpp src/pose_initializer/stop_check_module.cpp src/pose_initializer/ekf_localization_trigger_module.cpp src/pose_initializer/ndt_localization_trigger_module.cpp diff --git a/localization/pose_initializer/config/pose_initializer.param.yaml b/localization/pose_initializer/config/pose_initializer.param.yaml index c1486b71aae9c..16c7252fc03f4 100644 --- a/localization/pose_initializer/config/pose_initializer.param.yaml +++ b/localization/pose_initializer/config/pose_initializer.param.yaml @@ -3,4 +3,5 @@ gnss_enabled: true ndt_enabled: true ekf_enabled: true + yabloc_enabled: false stop_check_enabled: true diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml index e7c28eeea88f9..1fa1a467d14b4 100644 --- a/localization/pose_initializer/launch/pose_initializer.launch.xml +++ b/localization/pose_initializer/launch/pose_initializer.launch.xml @@ -6,6 +6,7 @@ + diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp index e4f590a7b1f42..6614cbf56008e 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp @@ -20,6 +20,7 @@ #include "ndt_localization_trigger_module.hpp" #include "ndt_module.hpp" #include "stop_check_module.hpp" +#include "yabloc_module.hpp" #include #include @@ -41,6 +42,9 @@ PoseInitializer::PoseInitializer() : Node("pose_initializer") if (declare_parameter("gnss_enabled")) { gnss_ = std::make_unique(this); } + if (declare_parameter("yabloc_enabled")) { + yabloc_ = std::make_unique(this); + } if (declare_parameter("ndt_enabled")) { ndt_ = std::make_unique(this); ndt_localization_trigger_ = std::make_unique(this); @@ -86,6 +90,10 @@ void PoseInitializer::on_initialize( auto pose = req->pose.empty() ? get_gnss_pose() : req->pose.front(); if (ndt_) { pose = ndt_->align_pose(pose); + } else if (yabloc_) { + // If both the NDT and YabLoc initializer are enabled, prioritize NDT as it offers more + // accuracy pose. + pose = yabloc_->align_pose(pose); } pose.pose.covariance = output_pose_covariance_; pub_reset_->publish(pose); diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp index 5fb6677b6b71f..fa04146cf529e 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp @@ -26,6 +26,7 @@ class StopCheckModule; class NdtModule; class GnssModule; +class YabLocModule; class EkfLocalizationTriggerModule; class NdtLocalizationTriggerModule; @@ -50,6 +51,7 @@ class PoseInitializer : public rclcpp::Node std::array gnss_particle_covariance_; std::unique_ptr gnss_; std::unique_ptr ndt_; + std::unique_ptr yabloc_; std::unique_ptr stop_check_; std::unique_ptr ekf_localization_trigger_; std::unique_ptr ndt_localization_trigger_; diff --git a/localization/pose_initializer/src/pose_initializer/yabloc_module.cpp b/localization/pose_initializer/src/pose_initializer/yabloc_module.cpp new file mode 100644 index 0000000000000..f79f1e58e8d62 --- /dev/null +++ b/localization/pose_initializer/src/pose_initializer/yabloc_module.cpp @@ -0,0 +1,51 @@ +// Copyright 2022 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "yabloc_module.hpp" + +#include +#include + +#include + +using ServiceException = component_interface_utils::ServiceException; +using Initialize = localization_interface::Initialize; +using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + +YabLocModule::YabLocModule(rclcpp::Node * node) : logger_(node->get_logger()) +{ + cli_align_ = node->create_client("yabloc_align"); +} + +PoseWithCovarianceStamped YabLocModule::align_pose(const PoseWithCovarianceStamped & pose) +{ + const auto req = std::make_shared(); + req->pose_with_covariance = pose; + + if (!cli_align_->service_is_ready()) { + throw component_interface_utils::ServiceUnready("YabLoc align server is not ready."); + } + + RCLCPP_INFO(logger_, "Call YabLoc align server."); + const auto res = cli_align_->async_send_request(req).get(); + if (!res->success) { + RCLCPP_INFO(logger_, "YabLoc align server failed."); + throw ServiceException( + Initialize::Service::Response::ERROR_ESTIMATION, "YabLoc align server failed."); + } + RCLCPP_INFO(logger_, "YabLoc align server succeeded."); + + // Overwrite the covariance. + return res->pose_with_covariance; +} diff --git a/localization/pose_initializer/src/pose_initializer/yabloc_module.hpp b/localization/pose_initializer/src/pose_initializer/yabloc_module.hpp new file mode 100644 index 0000000000000..4c6076553b059 --- /dev/null +++ b/localization/pose_initializer/src/pose_initializer/yabloc_module.hpp @@ -0,0 +1,38 @@ +// Copyright 2022 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_INITIALIZER__YABLOC_MODULE_HPP_ +#define POSE_INITIALIZER__YABLOC_MODULE_HPP_ + +#include + +#include +#include + +class YabLocModule +{ +private: + using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + using RequestPoseAlignment = tier4_localization_msgs::srv::PoseWithCovarianceStamped; + +public: + explicit YabLocModule(rclcpp::Node * node); + PoseWithCovarianceStamped align_pose(const PoseWithCovarianceStamped & pose); + +private: + rclcpp::Logger logger_; + rclcpp::Client::SharedPtr cli_align_; +}; + +#endif // POSE_INITIALIZER__YABLOC_MODULE_HPP_ diff --git a/localization/yabloc/README.md b/localization/yabloc/README.md new file mode 100644 index 0000000000000..34d6c70453c6c --- /dev/null +++ b/localization/yabloc/README.md @@ -0,0 +1,31 @@ +# YabLoc + +**YabLoc** is vision-based localization with vector map. [https://youtu.be/Eaf6r_BNFfk](https://youtu.be/Eaf6r_BNFfk) + +[![thumbnail](docs/yabloc_thumbnail.jpg)](https://youtu.be/Eaf6r_BNFfk) + +## Packages + +- [yabloc_common](yabloc_common/README.md) +- [yabloc_image_processing](yabloc_image_processing/README.md) +- [yabloc_particle_filter](yabloc_particle_filter/README.md) +- [yabloc_pose_initializer](yabloc_pose_initializer/README.md) + +## How to launch YabLoc instead of NDT + +When launching autoware, if you set `localization_mode:=yabloc` as an argument, YabLoc will be launched instead of NDT. +By default, `localization_mode` is `ndt`. + +A sample command to run YabLoc is as follows + +```shell +ros2 launch autoware_launch logging_simulator.launch.xml \ + map_path:=$HOME/autoware_map/sample-map-rosbag\ + vehicle_model:=sample_vehicle \ + sensor_model:=sample_sensor_kit \ + localization_mode:=yabloc +``` + +## Architecture + +![node_diagram](docs/yabloc_architecture.drawio.svg) diff --git a/localization/yabloc/docs/yabloc_architecture.drawio.svg b/localization/yabloc/docs/yabloc_architecture.drawio.svg new file mode 100644 index 0000000000000..e26e144b9a51c --- /dev/null +++ b/localization/yabloc/docs/yabloc_architecture.drawio.svg @@ -0,0 +1,909 @@ + + + + + + + +
+
+
+ + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22camera_pose_%26lt%3Bbr%26gt%3Binitializer%22%20style%3D%22rounded%3D0%3BwhiteSpace%3Dwrap%3Bhtml%3D1%3B%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22360%22%20y%3D%22200%22%20width%3D%22120%22%20height%3D%2240%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E + +
+
+
+ + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%... + + + + + + + + +
+
+
+ + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22camera_pose_%26lt%3Bbr%26gt%3Binitializer%22%20style%3D%22rounded%3D0%3BwhiteSpace%3Dwrap%3Bhtml%3D1%3B%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22360%22%20y%3D%22200%22%20width%3D%22120%22%20height%3D%2240%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E + +
+
+
+ + + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3... + + + + + + + + + +
+
+
+ Camera +
+
+
+ + Camera + + + + + + + + + +
+
+
+ /map +
+ /vectormap +
+
+
+ + /map... + + + + + + + +
+
+
+ /localization/yablocmap/ll2_road_marking +
+
+
+ + /localization/yablocmap/ll2_road_marking + + + + + + + +
+
+
+ + /localization/yabloc +
+ /image_processing/undistorted/camera_info + +
+
+
+ + /localization/yabloc... + + + + + + + + +
+
+
+ undistort +
+
+
+ + undistort + + + + + + +
+
+
+ ground_server +
+
+
+ + ground_server + + + + + + +
+
+
+ ll2_decompose +
+
+
+ + ll2_decompose + + + + + + + +
+
+
+ + /localization +
+ /pose_estimator +
+ /pose_with_covariance + +
+
+
+ + /localization... + + + + + + +
+
+
+ particle_predictor +
+
+
+ + particle_predictor + + + + + + + + +
+
+
+ gnss_particle_ +
+ corrector +
+
+
+ + gnss_particle_... + + + + + + + +
+
+
+ + /localization/yabloc/ +
+ pf/weighted_particles + +
+
+
+ + /localization/yabloc/... + + + + + + +
+
+
+ camera_corrector +
+
+
+ + camera_corrector + + + + + + +
+
+
+ segment_filter +
+
+
+ + segment_filter + + + + + + + +
+
+
+ + /localization/yabloc +
+ /image_processing +
+ /segmented_image + +
+
+
+ + /localization/yabloc... + + + + + + +
+
+
+ graph_segment +
+
+
+ + graph_segment + + + + + + + +
+
+
+ + /localization/yabloc +
+ /image_processing/line_segments_cloud + +
+
+
+ + /localization/yabloc... + + + + + + +
+
+
+ line_segment_ +
+ detector +
+
+
+ + line_segment_... + + + + + + +
+
+
+ /image_processing +
+
+
+ + /image_processing + + + + + + +
+
+
+ /map +
+
+
+ + /map + + + + + + +
+
+
+ /pf +
+
+
+ + /pf + + + + + + +
+
+
+ /localization +
+ /twist_estimator +
+ /gyro_odometer +
+
+
+ + /localization... + + + + + + + +
+
+
+ /sensing/gnss +
+ /pose_with_covariance +
+
+
+ + /sensing/gnss... + + + + + + +
+
+
+ /sensing +
+ /gnss +
+ /gnss_poser +
+
+
+ + /sensing... + + + + + + + + +
+
+
+ /map +
+ /lanelet2_map_loader +
+
+
+ + /map... + + + + + + + + +
+
+
+ semantic_segmentation_srv +
+
+
+ + semantic_segmentation_srv + + + + + + +
+
+
+ camera_pose_ +
+ initializer +
+
+
+ + camera_pose_... + + + + + + +
+
+
+ semantic_ +
+ segmentation +
+
+
+ + semantic_... + + + + + + + + +
+
+
+ yabloc_align_srv +
+
+
+ + yabloc_align_srv + + + + + + + +
+
+
+ /initialpose3d +
+
+
+ + /initialpose3d + + + + + + +
+
+
+ /localization +
+ /util +
+ /pose_initializer +
+
+
+ + /localization... + + + + + + +
+
+
+ /initializer +
+
+
+ + /initializer + + + + + + + + + +
+
+
+ + /localization/yabloc +
+ / + + + image_processing +
+ /undistorted/image_raw + +
+
+
+ + /localization/yabloc... + + + + + + + +
+
+
+ + /localization/yabloc +
+ /image_processing +
+ /projected_line_ +
+ segments_cloud + +
+
+
+ + /localization/yabloc... + + + + + + + +
+
+
+ /localization/yabloc/map/height +
+
+
+ + /localization/yabloc/map/height + + + + + + + + +
+
+
+ /localization +
+ /pose_twist_fusion_filter +
+ /ekf_localizer +
+
+
+ + /localization... + + + + + + + + + +
+
+
+ /localization/twist_estimator +
+ /twist_with_covariance +
+
+
+ + /localization/twist_estimator... + + + + + + + +
+
+
+ /localization/yabloc/pf/pose +
+
+
+ + /localization/yabloc/pf/pose + + + + + + + +
+
+
+ + /localization +
+ /yabloc/pf +
+ /predicted_particles + +
+
+
+ + /localization... + + + + + + +
+
+
+ implemented in +
+ yabloc package +
+
+
+ + implemented in... + + + + + + +
+
+
+ implemented in +
+ non-yabloc package +
+
+
+ + implemented in... + + + + + + + Text is not SVG - cannot display + + + diff --git a/localization/yabloc/docs/yabloc_thumbnail.jpg b/localization/yabloc/docs/yabloc_thumbnail.jpg new file mode 100644 index 0000000000000..79b6a278ff3c5 Binary files /dev/null and b/localization/yabloc/docs/yabloc_thumbnail.jpg differ diff --git a/localization/yabloc/yabloc_common/CMakeLists.txt b/localization/yabloc/yabloc_common/CMakeLists.txt new file mode 100644 index 0000000000000..c8474a85ad963 --- /dev/null +++ b/localization/yabloc/yabloc_common/CMakeLists.txt @@ -0,0 +1,87 @@ +cmake_minimum_required(VERSION 3.5) +project(yabloc_common) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +# =================================================== +# Eigen3 +find_package(Eigen3 REQUIRED) + +# OpenCV +find_package(OpenCV REQUIRED) + +# PCL +find_package(PCL REQUIRED) + +# Sophus +find_package(Sophus REQUIRED) + +# https://github.com/strasdat/Sophus/issues/388 +# Sophus's macro cause compile error for the "-Wpedantic" flag, so make sure to import as a system. +# We must always call the following command after "ament_auto_find_build_dependencies()" +# because it rewrite CMAKE_NO_SYSTEM_FROM_IMPORTED to TRUE. +set(CMAKE_NO_SYSTEM_FROM_IMPORTED FALSE) + +# glog +find_package(glog REQUIRED) + +# =================================================== +# GeographicLib +find_package(PkgConfig) +find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h + PATH_SUFFIXES GeographicLib +) +set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR}) +find_library(GeographicLib_LIBRARIES NAMES Geographic) + +# =================================================== +# Libraries +ament_auto_add_library(${PROJECT_NAME} SHARED + src/pub_sub.cpp + src/camera_info_subscriber.cpp + src/cv_decompress.cpp + src/pose_conversions.cpp + src/static_tf_subscriber.cpp + src/extract_line_segments.cpp + src/transform_line_segments.cpp + src/color.cpp) +target_include_directories( + ${PROJECT_NAME} PUBLIC include +) + +target_include_directories( + ${PROJECT_NAME} PRIVATE + SYSTEM + ${PCL_INCLUDE_DIRS} + ${GeographicLib_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} +) +target_link_libraries(${PROJECT_NAME} Geographic ${PCL_LIBRARIES} Sophus::Sophus) + +# =================================================== +# Executables +# ground_server +set(TARGET ground_server_node) +ament_auto_add_executable(${TARGET} + src/ground_server/ground_server_core.cpp + src/ground_server/ground_server_node.cpp + src/ground_server/polygon_operation.cpp) +target_include_directories(${TARGET} PUBLIC include) +target_include_directories(${TARGET} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) +target_link_libraries(${TARGET} ${PCL_LIBRARIES} Sophus::Sophus glog::glog) + +# ll2_decomposer +set(TARGET ll2_decomposer_node) +ament_auto_add_executable(${TARGET} + src/ll2_decomposer/ll2_decomposer_core.cpp + src/ll2_decomposer/ll2_decomposer_node.cpp) +target_include_directories(${TARGET} PUBLIC include) +target_include_directories(${TARGET} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) +target_link_libraries(${TARGET} ${PCL_LIBRARIES}) + +# =================================================== +ament_export_dependencies(PCL Sophus) + +ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/localization/yabloc/yabloc_common/README.md b/localization/yabloc/yabloc_common/README.md new file mode 100644 index 0000000000000..62aaa4dbb51f2 --- /dev/null +++ b/localization/yabloc/yabloc_common/README.md @@ -0,0 +1,70 @@ +# yabloc_common + +This package contains some executable nodes related to map. Also, This provides some yabloc common library. + +- [ground_server](#ground_server) +- [ll2_decomposer](#ll2_decomposer) + +## ground_server + +### Purpose + +It estimates the height and tilt of the ground from lanelet2. + +### Input / Outputs + +#### Input + +| Name | Type | Description | +| ------------------ | -------------------------------------------- | ------------------- | +| `input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map | +| `input/pose` | `geometry_msgs::msg::PoseStamped` | estiamted self pose | + +#### Output + +| Name | Type | Description | +| ----------------------- | ---------------------------------- | ------------------------------------------------------------------------------- | +| `output/ground` | `std_msgs::msg::Float32MultiArray` | estimated ground parameters. it contains x, y, z, normal_x, normal_y, normal_z. | +| `output/ground_markers` | `visualization_msgs::msg::Marker` | visualization of estimated ground plane | +| `output/ground_status` | `std_msgs::msg::String` | status log of ground plane estimation | +| `output/height` | `std_msgs::msg::Float32` | altitude | +| `output/near_cloud` | `sensor_msgs::msg::PointCloud2` | point cloud extracted from lanelet2 and used for ground tilt estimation | + +### Parameters + +| Name | Type | Description | +| ----------------- | ---- | -------------------------------------------------------- | +| `force_zero_tilt` | bool | if true, the tilt is always determined to be horizontal. | +| `K` | int | parameter for nearest k search | +| `R` | int | parameter for radius search | + +## ll2_decomposer + +### Purpose + +This node extracts the elements related to the road surface markings and yabloc from lanelet2. + +### Input / Outputs + +#### Input + +| Name | Type | Description | +| ------------------ | -------------------------------------------- | ----------- | +| `input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map | + +#### Output + +| Name | Type | Description | +| -------------------------- | -------------------------------------- | --------------------------------------------- | +| `output/ll2_bounding_box` | `sensor_msgs::msg::PointCloud2` | bounding boxes extracted from lanelet2 | +| `output/ll2_road_marking` | `sensor_msgs::msg::PointCloud2` | road surface markings extracted from lanelet2 | +| `output/ll2_sign_board` | `sensor_msgs::msg::PointCloud2` | traffic sign boards extracted from lanelet2 | +| `output/sign_board_marker` | `visualization_msgs::msg::MarkerArray` | visualized traffic sign boards | + +### Parameters + +| Name | Type | Description | +| --------------------- | ---------------- | ---------------------------------------------------------------------- | +| `road_marking_labels` | vector\ | This label is used to extract the road surface markings from lanalet2. | +| `sign_board_labels` | vector\ | This label is used to extract the traffic sign boards from lanalet2. | +| `bounding_box_labels` | vector\ | This label is used to extract the bounding boxes from lanalet2. | diff --git a/localization/yabloc/yabloc_common/config/ground_server.param.yaml b/localization/yabloc/yabloc_common/config/ground_server.param.yaml new file mode 100644 index 0000000000000..25873ce799e52 --- /dev/null +++ b/localization/yabloc/yabloc_common/config/ground_server.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + force_zero_tilt: false + K: 50 + R: 10 diff --git a/localization/yabloc/yabloc_common/config/ll2_decomposer.param.yaml b/localization/yabloc/yabloc_common/config/ll2_decomposer.param.yaml new file mode 100644 index 0000000000000..1bc6394a1775a --- /dev/null +++ b/localization/yabloc/yabloc_common/config/ll2_decomposer.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + road_marking_labels: [cross_walk, zebra_marking, line_thin, line_thick, pedestrian_marking, stop_line, road_border] + sign_board_labels: [sign-board] + bounding_box_labels: [none] diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/camera_info_subscriber.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/camera_info_subscriber.hpp new file mode 100644 index 0000000000000..f26a63ebd6317 --- /dev/null +++ b/localization/yabloc/yabloc_common/include/yabloc_common/camera_info_subscriber.hpp @@ -0,0 +1,52 @@ +// 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 YABLOC_COMMON__CAMERA_INFO_SUBSCRIBER_HPP_ +#define YABLOC_COMMON__CAMERA_INFO_SUBSCRIBER_HPP_ + +#include +#include + +#include + +#include +#include +namespace yabloc::common +{ +class CameraInfoSubscriber +{ +public: + using CameraInfo = sensor_msgs::msg::CameraInfo; + + explicit CameraInfoSubscriber(rclcpp::Node * node); + + bool is_camera_info_ready() const; + + bool is_camera_info_nullopt() const; + + Eigen::Vector2i size() const; + + Eigen::Matrix3f intrinsic() const; + + // This member function DOES NOT check isCameraInfoReady() + // If it it not ready, throw bad optional access + std::string get_frame_id() const; + +private: + rclcpp::Subscription::SharedPtr sub_info_; + std::optional opt_info_; +}; +} // namespace yabloc::common + +#endif // YABLOC_COMMON__CAMERA_INFO_SUBSCRIBER_HPP_ diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/color.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/color.hpp new file mode 100644 index 0000000000000..219c50f387cb5 --- /dev/null +++ b/localization/yabloc/yabloc_common/include/yabloc_common/color.hpp @@ -0,0 +1,74 @@ +// 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 YABLOC_COMMON__COLOR_HPP_ +#define YABLOC_COMMON__COLOR_HPP_ + +#include + +#include + +namespace yabloc::common +{ + +struct Color +{ + Color(float r, float g, float b, float a = 1.0f) : r(r), g(g), b(b), a(a) {} + + explicit Color(const std_msgs::msg::ColorRGBA & rgba) : r(rgba.r), g(rgba.g), b(rgba.b), a(rgba.a) + { + } + + explicit Color(const cv::Scalar & rgb, float a = 1.0f) + : r(rgb[2] / 255.f), g(rgb[1] / 255.f), b(rgb[0] / 255.f), a(a) + { + } + + operator uint32_t() const + { + union uchar4_uint32 { + uint8_t rgba[4]; + uint32_t u32; + }; + uchar4_uint32 tmp; + tmp.rgba[0] = static_cast(r * 255); + tmp.rgba[1] = static_cast(g * 255); + tmp.rgba[2] = static_cast(b * 255); + tmp.rgba[3] = static_cast(a * 255); + return tmp.u32; + } + operator const cv::Scalar() const { return cv::Scalar(255 * b, 255 * g, 255 * r); } + operator const std_msgs::msg::ColorRGBA() const + { + std_msgs::msg::ColorRGBA rgba; + rgba.a = a; + rgba.r = r; + rgba.g = g; + rgba.b = b; + return rgba; + } + + float r, g, b, a; +}; + +namespace color_scale +{ +Color hsv_to_rgb(float h, float s, float v); +Color rainbow(float value); +Color blue_red(float value); +} // namespace color_scale + +} // namespace yabloc::common + +#endif // YABLOC_COMMON__COLOR_HPP_ diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/cv_decompress.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/cv_decompress.hpp new file mode 100644 index 0000000000000..57a12a2e674c0 --- /dev/null +++ b/localization/yabloc/yabloc_common/include/yabloc_common/cv_decompress.hpp @@ -0,0 +1,36 @@ +// 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 YABLOC_COMMON__CV_DECOMPRESS_HPP_ +#define YABLOC_COMMON__CV_DECOMPRESS_HPP_ + +#include + +#include +#include + +#include + +namespace yabloc::common +{ +cv::Mat decompress_to_cv_mat(const sensor_msgs::msg::Image & img); + +sensor_msgs::msg::Image::ConstSharedPtr decompress_to_ros_msg( + const sensor_msgs::msg::CompressedImage & compressed_img, const std::string & encoding = "bgr8"); + +cv::Mat decompress_to_cv_mat(const sensor_msgs::msg::CompressedImage & compressed_img); + +} // namespace yabloc::common + +#endif // YABLOC_COMMON__CV_DECOMPRESS_HPP_ diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/extract_line_segments.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/extract_line_segments.hpp new file mode 100644 index 0000000000000..057551c9b3cb7 --- /dev/null +++ b/localization/yabloc/yabloc_common/include/yabloc_common/extract_line_segments.hpp @@ -0,0 +1,31 @@ +// 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 YABLOC_COMMON__EXTRACT_LINE_SEGMENTS_HPP_ +#define YABLOC_COMMON__EXTRACT_LINE_SEGMENTS_HPP_ + +#include + +#include +#include + +namespace yabloc::common +{ +pcl::PointCloud extract_near_line_segments( + const pcl::PointCloud & line_segments, const Sophus::SE3f & transform, + const float max_range = 40); + +} // namespace yabloc::common + +#endif // YABLOC_COMMON__EXTRACT_LINE_SEGMENTS_HPP_ diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/gamma_converter.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/gamma_converter.hpp new file mode 100644 index 0000000000000..98c082e125547 --- /dev/null +++ b/localization/yabloc/yabloc_common/include/yabloc_common/gamma_converter.hpp @@ -0,0 +1,46 @@ +// 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 YABLOC_COMMON__GAMMA_CONVERTER_HPP_ +#define YABLOC_COMMON__GAMMA_CONVERTER_HPP_ +#include + +namespace yabloc::common +{ +class GammaConverter +{ +public: + explicit GammaConverter(float gamma = 1.0f) { reset(gamma); } + + void reset(float gamma) + { + lut_ = cv::Mat(1, 256, CV_8UC1); + for (int i = 0; i < 256; i++) { + lut_.at(0, i) = 256 * std::pow(i / 256.f, gamma); + } + } + + cv::Mat operator()(const cv::Mat & src_image) const + { + cv::Mat dst_image; + cv::LUT(src_image, lut_, dst_image); + return dst_image; + } + +private: + cv::Mat lut_; +}; +} // namespace yabloc::common + +#endif // YABLOC_COMMON__GAMMA_CONVERTER_HPP_ diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ground_plane.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ground_plane.hpp new file mode 100644 index 0000000000000..62a3695a9852f --- /dev/null +++ b/localization/yabloc/yabloc_common/include/yabloc_common/ground_plane.hpp @@ -0,0 +1,91 @@ +// 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 YABLOC_COMMON__GROUND_PLANE_HPP_ +#define YABLOC_COMMON__GROUND_PLANE_HPP_ + +#include +#include +#include + +#include + +namespace yabloc::common +{ +struct GroundPlane +{ + using Float32Array = std_msgs::msg::Float32MultiArray; + Eigen::Vector3f xyz; + Eigen::Vector3f normal; + + GroundPlane() + { + xyz.setZero(); + normal = Eigen::Vector3f::UnitZ(); + } + + explicit GroundPlane(const Float32Array & array) { set(array); } + + void set(const Float32Array & array) + { + if (array.data.size() != 6) exit(EXIT_FAILURE); + for (int i = 0; i < 3; i++) { + xyz(i) = array.data.at(i); + normal(i) = array.data.at(3 + i); + } + } + + float height() const { return xyz.z(); } + + Eigen::Quaternionf align_with_slope(const Eigen::Quaternionf & q) const + { + return Eigen::Quaternionf{align_with_slope(q.toRotationMatrix())}; + } + + Eigen::Matrix3f align_with_slope(const Eigen::Matrix3f & R) const + { + Eigen::Matrix3f R_; + Eigen::Vector3f rz = this->normal; + Eigen::Vector3f azimuth = R * Eigen::Vector3f::UnitX(); + Eigen::Vector3f ry = (rz.cross(azimuth)).normalized(); + Eigen::Vector3f rx = ry.cross(rz); + R_.col(0) = rx; + R_.col(1) = ry; + R_.col(2) = rz; + return R_; + } + + Sophus::SE3f align_with_slope(const Sophus::SE3f & pose) const + { + return {align_with_slope(pose.rotationMatrix()), pose.translation()}; + } + + Eigen::Affine3f align_with_slope(const Eigen::Affine3f & pose) const + { + Eigen::Matrix3f R = pose.rotation(); + Eigen::Vector3f t = pose.translation(); + return Eigen::Translation3f(t) * align_with_slope(R); + } + + Float32Array msg() const + { + Float32Array array; + for (int i = 0; i < 3; i++) array.data.push_back(xyz(i)); + for (int i = 0; i < 3; i++) array.data.push_back(normal(i)); + return array; + } +}; +} // namespace yabloc::common + +#endif // YABLOC_COMMON__GROUND_PLANE_HPP_ diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/filter/moving_averaging.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/filter/moving_averaging.hpp new file mode 100644 index 0000000000000..5b62c8fb79c33 --- /dev/null +++ b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/filter/moving_averaging.hpp @@ -0,0 +1,45 @@ +// 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 YABLOC_COMMON__GROUND_SERVER__FILTER__MOVING_AVERAGING_HPP_ +#define YABLOC_COMMON__GROUND_SERVER__FILTER__MOVING_AVERAGING_HPP_ + +#include + +#include + +namespace yabloc::ground_server +{ +class MovingAveraging +{ +public: + MovingAveraging() : buffer_(50) {} + + Eigen::Vector3f update(const Eigen::Vector3f & normal) + { + buffer_.push_back(normal); + + Eigen::Vector3f mean = Eigen::Vector3f::Zero(); + for (const Eigen::Vector3f & v : buffer_) mean += v; + mean /= buffer_.size(); + + return mean.normalized(); + } + +private: + boost::circular_buffer buffer_; +}; +} // namespace yabloc::ground_server + +#endif // YABLOC_COMMON__GROUND_SERVER__FILTER__MOVING_AVERAGING_HPP_ diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp new file mode 100644 index 0000000000000..154507f8195d9 --- /dev/null +++ b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp @@ -0,0 +1,106 @@ +// 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 YABLOC_COMMON__GROUND_SERVER__GROUND_SERVER_HPP_ +#define YABLOC_COMMON__GROUND_SERVER__GROUND_SERVER_HPP_ + +#include "yabloc_common/ground_server/filter/moving_averaging.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +namespace yabloc::ground_server +{ +class GroundServer : public rclcpp::Node +{ +public: + using GroundPlane = common::GroundPlane; + using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + + using Pose = geometry_msgs::msg::Pose; + using PoseStamped = geometry_msgs::msg::PoseStamped; + using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + + using Float32 = std_msgs::msg::Float32; + // pub_ground_plane publishes x, y, z and normal vector as Float32Array + using Float32Array = std_msgs::msg::Float32MultiArray; + using Marker = visualization_msgs::msg::Marker; + using String = std_msgs::msg::String; + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using Point = geometry_msgs::msg::Point; + GroundServer(); + +private: + const bool force_zero_tilt_; + const float R; + const int K; + + // Subscriber + rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Subscription::SharedPtr sub_pose_stamped_; + rclcpp::Subscription::SharedPtr sub_initial_pose_; + // Publisher + rclcpp::Publisher::SharedPtr pub_ground_height_; + rclcpp::Publisher::SharedPtr pub_ground_plane_; + rclcpp::Publisher::SharedPtr pub_marker_; + rclcpp::Publisher::SharedPtr pub_string_; + rclcpp::Publisher::SharedPtr pub_near_cloud_; + + pcl::PointCloud::Ptr cloud_{nullptr}; + pcl::KdTreeFLANN::Ptr kdtree_{nullptr}; + + // Smoother + MovingAveraging normal_filter_; + LowpassFilter1d height_filter_{0.2}; + + // For debug + std::vector last_indices_; + + // Callback + void on_map(const HADMapBin & msg); + void on_initial_pose(const PoseCovStamped & msg); + void on_pose_stamped(const PoseStamped & msg); + + // Body + GroundPlane estimate_ground(const Point & point); + + // Return inlier indices which are belong to a plane + // Sometimes, this return empty indices due to RANSAC failure + std::vector estimate_inliers_by_ransac(const std::vector & indices_raw); + + // Return the lowest point's height around given point + float estimate_height_simply(const Point & point) const; +}; + +} // namespace yabloc::ground_server + +#endif // YABLOC_COMMON__GROUND_SERVER__GROUND_SERVER_HPP_ diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/polygon_operation.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/polygon_operation.hpp new file mode 100644 index 0000000000000..7a95710e8d496 --- /dev/null +++ b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/polygon_operation.hpp @@ -0,0 +1,30 @@ +// 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 YABLOC_COMMON__GROUND_SERVER__POLYGON_OPERATION_HPP_ +#define YABLOC_COMMON__GROUND_SERVER__POLYGON_OPERATION_HPP_ + +#include +#include +#include + +namespace yabloc::ground_server +{ +pcl::PointCloud sample_from_polygons(const lanelet::PolygonLayer & polygons); + +pcl::PointCloud fill_points_in_polygon( + const pcl::PointCloud & src_cloud); +} // namespace yabloc::ground_server + +#endif // YABLOC_COMMON__GROUND_SERVER__POLYGON_OPERATION_HPP_ diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/util.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/util.hpp new file mode 100644 index 0000000000000..fe3285fb08e8a --- /dev/null +++ b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/util.hpp @@ -0,0 +1,57 @@ +// 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 YABLOC_COMMON__GROUND_SERVER__UTIL_HPP_ +#define YABLOC_COMMON__GROUND_SERVER__UTIL_HPP_ + +#include + +#include +#include +#include + +#include +#include + +namespace yabloc::ground_server +{ +void upsample_line_string( + const lanelet::ConstPoint3d & from, const lanelet::ConstPoint3d & to, + pcl::PointCloud::Ptr cloud) +{ + Eigen::Vector3f f(from.x(), from.y(), from.z()); + Eigen::Vector3f t(to.x(), to.y(), to.z()); + float length = (t - f).norm(); + Eigen::Vector3f d = (t - f).normalized(); + for (float l = 0; l < length; l += 0.5f) { + pcl::PointXYZ xyz; + xyz.getVector3fMap() = (f + l * d); + cloud->push_back(xyz); + } +}; + +std::vector merge_indices(const std::vector & indices1, const std::vector & indices2) +{ + std::unordered_set set; + for (int i : indices1) set.insert(i); + for (int i : indices2) set.insert(i); + + std::vector indices; + indices.assign(set.begin(), set.end()); + return indices; +} + +} // namespace yabloc::ground_server + +#endif // YABLOC_COMMON__GROUND_SERVER__UTIL_HPP_ diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp new file mode 100644 index 0000000000000..f6e6de6b38fdd --- /dev/null +++ b/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp @@ -0,0 +1,81 @@ +// 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 YABLOC_COMMON__LL2_DECOMPOSER__LL2_DECOMPOSER_HPP_ +#define YABLOC_COMMON__LL2_DECOMPOSER__LL2_DECOMPOSER_HPP_ + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +namespace yabloc::ll2_decomposer +{ +class Ll2Decomposer : public rclcpp::Node +{ +public: + using Cloud2 = sensor_msgs::msg::PointCloud2; + using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using Marker = visualization_msgs::msg::Marker; + using MarkerArray = visualization_msgs::msg::MarkerArray; + + Ll2Decomposer(); + +private: + rclcpp::Publisher::SharedPtr pub_road_marking_; + rclcpp::Publisher::SharedPtr pub_sign_board_; + rclcpp::Publisher::SharedPtr pub_bounding_box_; + rclcpp::Publisher::SharedPtr pub_marker_; + + rclcpp::Subscription::SharedPtr sub_map_; + std::set road_marking_labels_; + std::set sign_board_labels_; + std::set bounding_box_labels_; + + void on_map(const HADMapBin & msg); + + pcl::PointNormal to_point_normal( + const lanelet::ConstPoint3d & from, const lanelet::ConstPoint3d & to) const; + + pcl::PointCloud split_line_strings( + const lanelet::ConstLineStrings3d & line_strings); + + pcl::PointCloud load_bounding_boxes(const lanelet::PolygonLayer & polygons) const; + + lanelet::ConstLineStrings3d extract_specified_line_string( + const lanelet::LineStringLayer & line_strings, const std::set & visible_labels); + lanelet::ConstPolygons3d extract_specified_polygon( + const lanelet::PolygonLayer & polygon_layer, const std::set & visible_labels); + + MarkerArray make_sign_marker_msg( + const lanelet::LineStringLayer & line_string_layer, const std::set & labels, + const std::string & ns); + MarkerArray make_polygon_marker_msg( + const lanelet::PolygonLayer & polygon_layer, const std::set & labels, + const std::string & ns); + + void publish_additional_marker(const lanelet::LaneletMapPtr & lanelet_map); +}; +} // namespace yabloc::ll2_decomposer + +#endif // YABLOC_COMMON__LL2_DECOMPOSER__LL2_DECOMPOSER_HPP_ diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/pcl_filter.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/pcl_filter.hpp new file mode 100644 index 0000000000000..0990af97a42c5 --- /dev/null +++ b/localization/yabloc/yabloc_common/include/yabloc_common/pcl_filter.hpp @@ -0,0 +1,37 @@ +// 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 YABLOC_COMMON__PCL_FILTER_HPP_ +#define YABLOC_COMMON__PCL_FILTER_HPP_ + +#include +#include +#include + +namespace yabloc::common +{ +template +pcl::PointCloud extract(const pcl::PointCloud & cloud, pcl::PointIndices::Ptr indices_ptr) +{ + pcl::ExtractIndices extract; + extract.setIndices(indices_ptr); + extract.setInputCloud(cloud); + + pcl::PointCloud extracted_cloud; + extract.filter(extracted_cloud); + return extracted_cloud; +} +} // namespace yabloc::common + +#endif // YABLOC_COMMON__PCL_FILTER_HPP_ diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/pose_conversions.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/pose_conversions.hpp new file mode 100644 index 0000000000000..f664d5ae08254 --- /dev/null +++ b/localization/yabloc/yabloc_common/include/yabloc_common/pose_conversions.hpp @@ -0,0 +1,32 @@ +// 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 YABLOC_COMMON__POSE_CONVERSIONS_HPP_ +#define YABLOC_COMMON__POSE_CONVERSIONS_HPP_ + +#include +#include + +#include + +namespace yabloc::common +{ +Eigen::Affine3f pose_to_affine(const geometry_msgs::msg::Pose & pose); +geometry_msgs::msg::Pose affine_to_pose(const Eigen::Affine3f & affine); + +Sophus::SE3f pose_to_se3(const geometry_msgs::msg::Pose & pose); +geometry_msgs::msg::Pose se3_to_pose(const Sophus::SE3f & se3f); +} // namespace yabloc::common + +#endif // YABLOC_COMMON__POSE_CONVERSIONS_HPP_ diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/pub_sub.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/pub_sub.hpp new file mode 100644 index 0000000000000..9ea43634d95c3 --- /dev/null +++ b/localization/yabloc/yabloc_common/include/yabloc_common/pub_sub.hpp @@ -0,0 +1,39 @@ +// 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 YABLOC_COMMON__PUB_SUB_HPP_ +#define YABLOC_COMMON__PUB_SUB_HPP_ + +#include +#include + +#include +#include + +#include +#include + +namespace yabloc::common +{ +void publish_image( + rclcpp::Publisher & publisher, const cv::Mat & image, + const rclcpp::Time & stamp); + +template +void publish_cloud( + rclcpp::Publisher & publisher, + const pcl::PointCloud & cloud, const rclcpp::Time & stamp); +} // namespace yabloc::common + +#endif // YABLOC_COMMON__PUB_SUB_HPP_ diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/static_tf_subscriber.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/static_tf_subscriber.hpp new file mode 100644 index 0000000000000..88157332471f4 --- /dev/null +++ b/localization/yabloc/yabloc_common/include/yabloc_common/static_tf_subscriber.hpp @@ -0,0 +1,48 @@ +// 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 YABLOC_COMMON__STATIC_TF_SUBSCRIBER_HPP_ +#define YABLOC_COMMON__STATIC_TF_SUBSCRIBER_HPP_ + +#include +#include + +#include +#include + +#include +#include +#include + +namespace yabloc::common +{ +class StaticTfSubscriber +{ +public: + explicit StaticTfSubscriber(rclcpp::Clock::SharedPtr clock); + + std::optional se3f( + const std::string & frame_id, const std::string & parent_frame_id = "base_link"); + + std::optional operator()( + const std::string & frame_id, const std::string & parent_frame_id = "base_link"); + +private: + std::shared_ptr transform_listener_{nullptr}; + std::unique_ptr tf_buffer_; +}; + +} // namespace yabloc::common + +#endif // YABLOC_COMMON__STATIC_TF_SUBSCRIBER_HPP_ diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/synchro_subscriber.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/synchro_subscriber.hpp new file mode 100644 index 0000000000000..7cd03bc75c6de --- /dev/null +++ b/localization/yabloc/yabloc_common/include/yabloc_common/synchro_subscriber.hpp @@ -0,0 +1,67 @@ +// 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 YABLOC_COMMON__SYNCHRO_SUBSCRIBER_HPP_ +#define YABLOC_COMMON__SYNCHRO_SUBSCRIBER_HPP_ + +#include + +#include +#include +#include + +#include +#include + +namespace yabloc::common +{ +template +class SynchroSubscriber +{ +public: + using SharedPtr = std::shared_ptr; + using Sub1 = message_filters::Subscriber; + using Sub2 = message_filters::Subscriber; + using SyncPolicy = message_filters::sync_policies::ApproximateTime; + using UserCallback = std::function; + + SynchroSubscriber(rclcpp::Node * n, const std::string & topic1, const std::string & topic2) + { + using std::placeholders::_1; + using std::placeholders::_2; + using Synchronizer = message_filters::Synchronizer; + + sub1_ = std::make_shared(n, topic1); + sub2_ = std::make_shared(n, topic2); + syncronizer_ = std::make_shared(SyncPolicy(80), *sub1_, *sub2_); + syncronizer_->registerCallback(std::bind(&SynchroSubscriber::raw_callback, this, _1, _2)); + } + + void set_callback(const UserCallback & callback) { user_callback_ = callback; } + +private: + std::shared_ptr sub1_; + std::shared_ptr sub2_; + std::shared_ptr> syncronizer_; + std::optional user_callback_{std::nullopt}; + + void raw_callback(const typename Msg1::ConstPtr & msg1, const typename Msg2::ConstPtr & msg2) + { + if (user_callback_.has_value()) user_callback_.value()(*msg1, *msg2); + } +}; + +} // namespace yabloc::common + +#endif // YABLOC_COMMON__SYNCHRO_SUBSCRIBER_HPP_ diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/transform_line_segments.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/transform_line_segments.hpp new file mode 100644 index 0000000000000..de5cb6a114647 --- /dev/null +++ b/localization/yabloc/yabloc_common/include/yabloc_common/transform_line_segments.hpp @@ -0,0 +1,32 @@ +// 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 YABLOC_COMMON__TRANSFORM_LINE_SEGMENTS_HPP_ +#define YABLOC_COMMON__TRANSFORM_LINE_SEGMENTS_HPP_ + +#include + +#include +#include + +namespace yabloc::common +{ +pcl::PointCloud transform_line_segments( + const pcl::PointCloud & src, const Sophus::SE3f & transform); + +pcl::PointCloud transform_line_segments( + const pcl::PointCloud & src, const Sophus::SE3f & transform); +} // namespace yabloc::common + +#endif // YABLOC_COMMON__TRANSFORM_LINE_SEGMENTS_HPP_ diff --git a/localization/yabloc/yabloc_common/launch/yabloc_common.launch.xml b/localization/yabloc/yabloc_common/launch/yabloc_common.launch.xml new file mode 100644 index 0000000000000..48299e880cc9c --- /dev/null +++ b/localization/yabloc/yabloc_common/launch/yabloc_common.launch.xml @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml new file mode 100644 index 0000000000000..d56b111df45d3 --- /dev/null +++ b/localization/yabloc/yabloc_common/package.xml @@ -0,0 +1,42 @@ + + + yabloc_common + 0.1.0 + YabLoc common library + Kento Yabuuchi + Apache License 2.0 + + ament_cmake_ros + autoware_cmake + rosidl_default_generators + + autoware_auto_mapping_msgs + cv_bridge + geographiclib + geometry_msgs + lanelet2_core + lanelet2_extension + lanelet2_io + libgoogle-glog-dev + pcl_conversions + rcl_interfaces + rclcpp + rclcpp_components + sensor_msgs + signal_processing + sophus + std_msgs + tf2_ros + ublox_msgs + visualization_msgs + + ament_lint_auto + autoware_lint_common + + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/localization/yabloc/yabloc_common/src/camera_info_subscriber.cpp b/localization/yabloc/yabloc_common/src/camera_info_subscriber.cpp new file mode 100644 index 0000000000000..33a2279767107 --- /dev/null +++ b/localization/yabloc/yabloc_common/src/camera_info_subscriber.cpp @@ -0,0 +1,58 @@ +// 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 "yabloc_common/camera_info_subscriber.hpp" + +namespace yabloc::common +{ +CameraInfoSubscriber::CameraInfoSubscriber(rclcpp::Node * node) +{ + rclcpp::QoS qos(10); + auto callback = [this](const CameraInfo & msg) -> void { opt_info_ = msg; }; + sub_info_ = node->create_subscription("~/input/camera_info", qos, callback); +} + +bool CameraInfoSubscriber::is_camera_info_ready() const +{ + return opt_info_.has_value(); +} + +bool CameraInfoSubscriber::is_camera_info_nullopt() const +{ + return !(opt_info_.has_value()); +} + +std::string CameraInfoSubscriber::get_frame_id() const +{ + return opt_info_->header.frame_id; +} + +Eigen::Vector2i CameraInfoSubscriber::size() const +{ + if (!opt_info_.has_value()) { + throw std::runtime_error("camera_info is not ready but it's accessed"); + } + return {opt_info_->width, opt_info_->height}; +} + +Eigen::Matrix3f CameraInfoSubscriber::intrinsic() const +{ + if (!opt_info_.has_value()) { + throw std::runtime_error("camera_info is not ready but it's accessed"); + } + const Eigen::Matrix3d Kd_t = Eigen::Map>(opt_info_->k.data()); + return Kd_t.cast().transpose(); +} + +} // namespace yabloc::common diff --git a/localization/yabloc/yabloc_common/src/color.cpp b/localization/yabloc/yabloc_common/src/color.cpp new file mode 100644 index 0000000000000..485f655bd4151 --- /dev/null +++ b/localization/yabloc/yabloc_common/src/color.cpp @@ -0,0 +1,67 @@ +// 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 "yabloc_common/color.hpp" + +namespace yabloc::common::color_scale +{ +Color rainbow(float value) +{ + // clang-format off + float r = 1.0f, g = 1.0f, b = 1.0f; + value = std::clamp(value, 0.0f, 1.0f); + if (value < 0.25f) { + r = 0; g = 4 * (value); + } else if (value < 0.5f) { + r = 0; b = 1 + 4 * (0.25f - value); + } else if (value < 0.75f) { + r = 4 * (value - 0.5f); b = 0; + } else { + g = 1 + 4 * (0.75f - value); b = 0; + } + // clang-format on + return {r, g, b}; +} + +Color hsv_to_rgb(float h_, float s_, float v_) +{ + const float h = std::clamp(h_, 0.f, 360.f); + const float max = v_; + const float min = max * (1.0f - s_); + + if (h < 60) + return {max, h / 60 * (max - min) + min, min}; + else if (h < 120) + return {(120 - h) / 60 * (max - min) + min, max, min}; + else if (h < 180) + return {min, max, (h - 120) / 60 * (max - min) + min}; + else if (h < 240) + return {min, (240 - h) / 60 * (max - min) + min, max}; + else if (h < 300) + return {(h - 240) / 60 * (max - min) + min, min, max}; + else + return {max, min, (360 - h) / 60 * (max - min) + min}; +} + +Color blue_red(float value) +{ + value = std::clamp(value, 0.0f, 1.0f); + float h = (value < 0.5f) ? 0.f : 240.f; + float s = std::abs(value - 0.5f) / 0.5f; + float v = 1.0f; + return hsv_to_rgb(h, s, v); + // return {h, s, v}; +} + +} // namespace yabloc::common::color_scale diff --git a/localization/yabloc/yabloc_common/src/cv_decompress.cpp b/localization/yabloc/yabloc_common/src/cv_decompress.cpp new file mode 100644 index 0000000000000..dc232488e147c --- /dev/null +++ b/localization/yabloc/yabloc_common/src/cv_decompress.cpp @@ -0,0 +1,77 @@ +// 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 "yabloc_common/cv_decompress.hpp" + +#include +#include + +#include + +#include + +namespace yabloc::common +{ +cv::Mat decompress_image(const sensor_msgs::msg::CompressedImage & compressed_img) +{ + cv::Mat raw_image; + + const std::string & format = compressed_img.format; + const std::string encoding = format.substr(0, format.find(";")); + + constexpr int DECODE_GRAY = 0; + constexpr int DECODE_RGB = 1; + + bool encoding_is_bayer = encoding.find("bayer") != std::string::npos; + if (!encoding_is_bayer) { + return cv::imdecode(cv::Mat(compressed_img.data), DECODE_RGB); + } + raw_image = cv::imdecode(cv::Mat(compressed_img.data), DECODE_GRAY); + + // TODO(KYabuuchi) integrate with implementation in the sensing/perception component + if (encoding == "bayer_rggb8") { + cv::cvtColor(raw_image, raw_image, cv::COLOR_BayerBG2BGR); + } else if (encoding == "bayer_bggr8") { + cv::cvtColor(raw_image, raw_image, cv::COLOR_BayerRG2BGR); + } else if (encoding == "bayer_grbg8") { + cv::cvtColor(raw_image, raw_image, cv::COLOR_BayerGB2BGR); + } else if (encoding == "bayer_gbrg8") { + cv::cvtColor(raw_image, raw_image, cv::COLOR_BayerGR2BGR); + } else { + std::cerr << encoding << " is not supported encoding" << std::endl; + std::cerr << "Please implement additional decoding in " << __FUNCTION__ << std::endl; + exit(EXIT_FAILURE); + } + return raw_image; +} + +cv::Mat decompress_to_cv_mat(const sensor_msgs::msg::CompressedImage & compressed_img) +{ + return decompress_image(compressed_img); +} + +cv::Mat decompress_to_cv_mat(const sensor_msgs::msg::Image & img) +{ + return cv_bridge::toCvCopy(std::make_shared(img), img.encoding)->image; +} + +sensor_msgs::msg::Image::ConstSharedPtr decompress_to_ros_msg( + const sensor_msgs::msg::CompressedImage & compressed_img, const std::string & encoding) +{ + cv_bridge::CvImage cv_image; + cv_image.image = decompress_image(compressed_img); + cv_image.encoding = encoding; + return cv_image.toImageMsg(); +} +} // namespace yabloc::common diff --git a/localization/yabloc/yabloc_common/src/extract_line_segments.cpp b/localization/yabloc/yabloc_common/src/extract_line_segments.cpp new file mode 100644 index 0000000000000..9987c4fbb5e72 --- /dev/null +++ b/localization/yabloc/yabloc_common/src/extract_line_segments.cpp @@ -0,0 +1,52 @@ +// 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 "yabloc_common/extract_line_segments.hpp" + +namespace yabloc::common +{ +pcl::PointCloud extract_near_line_segments( + const pcl::PointCloud & line_segments, const Sophus::SE3f & transform, + const float max_range) +{ + constexpr double sqrt_two = std::sqrt(2); + const Eigen::Vector3f pose_vector = transform.translation(); + + // All line segments contained in a square with max_range on one side must be taken out, + // so pick up those that are closer than the **diagonals** of the square. + auto check_intersection = [max_range, pose_vector](const pcl::PointNormal & pn) -> bool { + const Eigen::Vector3f from = pn.getVector3fMap() - pose_vector; + const Eigen::Vector3f to = pn.getNormalVector3fMap() - pose_vector; + + Eigen::Vector3f tangent = to - from; + if (tangent.squaredNorm() < 1e-3f) { + return from.norm() < sqrt_two * max_range; + } + + float inner = from.dot(tangent); + float mu = std::clamp(inner / tangent.squaredNorm(), -1.0f, 0.0f); + Eigen::Vector3f nearest = from - tangent * mu; + return nearest.norm() < sqrt_two * max_range; + }; + + pcl::PointCloud dst; + for (const pcl::PointNormal & pn : line_segments) { + if (check_intersection(pn)) { + dst.push_back(pn); + } + } + return dst; +} + +} // namespace yabloc::common diff --git a/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp b/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp new file mode 100644 index 0000000000000..4d8e3f807bcd4 --- /dev/null +++ b/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp @@ -0,0 +1,250 @@ +// 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 "yabloc_common/ground_server/ground_server.hpp" +#include "yabloc_common/ground_server/polygon_operation.hpp" +#include "yabloc_common/ground_server/util.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace yabloc::ground_server +{ +GroundServer::GroundServer() +: Node("ground_server"), + force_zero_tilt_(declare_parameter("force_zero_tilt")), + R(declare_parameter("R")), + K(declare_parameter("K")) +{ + using std::placeholders::_1; + using std::placeholders::_2; + const rclcpp::QoS map_qos = rclcpp::QoS(10).transient_local().reliable(); + + auto on_pose = std::bind(&GroundServer::on_pose_stamped, this, _1); + auto on_map = std::bind(&GroundServer::on_map, this, _1); + + sub_map_ = create_subscription("~/input/vector_map", map_qos, on_map); + sub_pose_stamped_ = create_subscription("~/input/pose", 10, on_pose); + + pub_ground_height_ = create_publisher("~/output/height", 10); + pub_ground_plane_ = create_publisher("~/output/ground", 10); + pub_marker_ = create_publisher("~/debug/ground_markers", 10); + pub_string_ = create_publisher("~/debug/ground_status", 10); + pub_near_cloud_ = create_publisher("~/debug/near_cloud", 10); + + height_filter_.reset(0); +} + +void GroundServer::on_initial_pose(const PoseCovStamped & msg) +{ + if (kdtree_ == nullptr) { + RCLCPP_FATAL_STREAM(get_logger(), "ground height is not initialized because map is empty"); + return; + } + + const Point point = msg.pose.pose.position; + height_filter_.reset(estimate_height_simply(point)); +} + +void GroundServer::on_pose_stamped(const PoseStamped & msg) +{ + if (kdtree_ == nullptr) return; + GroundPlane ground_plane = estimate_ground(msg.pose.position); + // Publish value msg + Float32 data; + data.data = ground_plane.height(); + pub_ground_height_->publish(data); + pub_ground_plane_->publish(ground_plane.msg()); + + // Publish string msg + { + std::stringstream ss; + ss << "--- Ground Estimator Status ----" << std::endl; + ss << std::fixed << std::setprecision(2); + ss << "height: " << ground_plane.height() << std::endl; + float cos = ground_plane.normal.dot(Eigen::Vector3f::UnitZ()); + ss << "tilt: " << std::acos(cos) * 180 / 3.14 << " deg" << std::endl; + + String string_msg; + string_msg.data = ss.str(); + pub_string_->publish(string_msg); + } + + // Publish nearest point cloud for debug + { + pcl::PointCloud near_cloud; + for (int index : last_indices_) { + near_cloud.push_back(cloud_->at(index)); + } + if (!near_cloud.empty()) common::publish_cloud(*pub_near_cloud_, near_cloud, msg.header.stamp); + } +} + +void GroundServer::on_map(const HADMapBin & msg) +{ + lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); + lanelet::utils::conversion::fromBinMsg(msg, lanelet_map); + + // These should be loaded from rosparm + const std::set ground_labels = { + "zebra_marking", "virtual", "line_thin", "line_thick", + "pedestrian_marking", "stop_line", "curbstone"}; + + pcl::PointCloud::Ptr upsampled_cloud = + pcl::make_shared>(); + + for (lanelet::LineString3d & line : lanelet_map->lineStringLayer) { + if (!line.hasAttribute(lanelet::AttributeName::Type)) continue; + + lanelet::Attribute attr = line.attribute(lanelet::AttributeName::Type); + if (ground_labels.count(attr.value()) == 0) continue; + + lanelet::ConstPoint3d const * from = nullptr; + for (const lanelet::ConstPoint3d & p : line) { + if (from != nullptr) upsample_line_string(*from, p, upsampled_cloud); + from = &p; + } + } + + // NOTE: Under construction + // This function is used to generate a uniform point cloud from within the polygons surrounding + // the area when there is no lane information, such as at an intersection. + // if(lanelet_map->polygonLayer.size() > 0) + // *upsampled_cloud += sample_from_polygons(lanelet_map->polygonLayer); + + cloud_ = pcl::make_shared>(); + // Voxel + pcl::VoxelGrid filter; + filter.setInputCloud(upsampled_cloud); + filter.setLeafSize(1.0f, 1.0f, 1.0f); + filter.filter(*cloud_); + + kdtree_ = pcl::make_shared>(); + kdtree_->setInputCloud(cloud_); +} + +float GroundServer::estimate_height_simply(const geometry_msgs::msg::Point & point) const +{ + // NOTE: Sometimes it might give not-accurate height + constexpr float sq_radius = 3.0 * 3.0; + const float x = point.x; + const float y = point.y; + + float height = std::numeric_limits::infinity(); + for (const auto & p : cloud_->points) { + const float dx = x - p.x; + const float dy = y - p.y; + const float sd = (dx * dx) + (dy * dy); + if (sd < sq_radius) { + height = std::min(height, static_cast(p.z)); + } + } + return std::isfinite(height) ? height : 0; +} + +std::vector GroundServer::estimate_inliers_by_ransac(const std::vector & indices_raw) +{ + pcl::PointIndicesPtr indices(new pcl::PointIndices); + indices->indices = indices_raw; + + pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); + pcl::PointIndices::Ptr inliers(new pcl::PointIndices); + + pcl::SACSegmentation seg; + seg.setOptimizeCoefficients(true); + seg.setModelType(pcl::SACMODEL_PLANE); + seg.setMethodType(pcl::SAC_RANSAC); + seg.setDistanceThreshold(1.0); + seg.setProbability(0.6); + + seg.setInputCloud(cloud_); + seg.setIndices(indices); + seg.segment(*inliers, *coefficients); + return inliers->indices; +} + +GroundServer::GroundPlane GroundServer::estimate_ground(const Point & point) +{ + // Because height_filter_ is always initialized, getValue does not return nullopt + const float predicted_z = height_filter_.getValue().value(); + const pcl::PointXYZ xyz(point.x, point.y, predicted_z); + + std::vector raw_indices; + std::vector distances; + kdtree_->nearestKSearch(xyz, K, raw_indices, distances); + + std::vector indices = estimate_inliers_by_ransac(raw_indices); + + if (indices.empty()) indices = raw_indices; + last_indices_ = indices; + + // Estimate normal vector using covariance matrix around the target point + Eigen::Matrix3f covariance; + Eigen::Vector4f centroid; + pcl::compute3DCentroid(*cloud_, indices, centroid); + pcl::computeCovarianceMatrix(*cloud_, indices, centroid, covariance); + + // NOTE: I forgot why I don't use coefficients computeed by SACSegmentation + Eigen::Vector4f plane_parameter; + float curvature; + pcl::solvePlaneParameters(covariance, centroid, plane_parameter, curvature); + Eigen::Vector3f normal = plane_parameter.topRows(3).normalized(); + + { + // Reverse if it is upside down + if (normal.z() < 0) normal = -normal; + + // Remove NaN + if (!normal.allFinite()) { + normal = Eigen::Vector3f::UnitZ(); + RCLCPP_WARN_STREAM(get_logger(), "Reject NaN tilt"); + } + // Remove too large tilt (0.707 = cos(45deg)) + if ((normal.dot(Eigen::Vector3f::UnitZ())) < 0.707) { + normal = Eigen::Vector3f::UnitZ(); + RCLCPP_WARN_STREAM(get_logger(), "Reject too large tilt of ground"); + } + } + + const Eigen::Vector3f filt_normal = normal_filter_.update(normal); + + GroundPlane plane; + plane.xyz = Eigen::Vector3f(point.x, point.y, predicted_z); + plane.normal = filt_normal; + + // Compute z value by intersection of estimated plane and orthogonal line + { + Eigen::Vector3f center = centroid.topRows(3); + float inner = center.dot(plane.normal); + float px_nx = point.x * plane.normal.x(); + float py_ny = point.y * plane.normal.y(); + plane.xyz.z() = (inner - px_nx - py_ny) / plane.normal.z(); + } + + height_filter_.filter(plane.xyz.z()); + + if (force_zero_tilt_) plane.normal = Eigen::Vector3f::UnitZ(); + return plane; +} + +} // namespace yabloc::ground_server diff --git a/localization/yabloc/yabloc_common/src/ground_server/ground_server_node.cpp b/localization/yabloc/yabloc_common/src/ground_server/ground_server_node.cpp new file mode 100644 index 0000000000000..ff50eeee0421d --- /dev/null +++ b/localization/yabloc/yabloc_common/src/ground_server/ground_server_node.cpp @@ -0,0 +1,27 @@ +// 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 "yabloc_common/ground_server/ground_server.hpp" + +#include + +int main(int argc, char ** argv) +{ + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); +} diff --git a/localization/yabloc/yabloc_common/src/ground_server/polygon_operation.cpp b/localization/yabloc/yabloc_common/src/ground_server/polygon_operation.cpp new file mode 100644 index 0000000000000..d4b134cf3a203 --- /dev/null +++ b/localization/yabloc/yabloc_common/src/ground_server/polygon_operation.cpp @@ -0,0 +1,93 @@ +// 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 "yabloc_common/ground_server/polygon_operation.hpp" + +#include + +namespace yabloc::ground_server +{ +pcl::PointCloud sample_from_polygons(const lanelet::PolygonLayer & polygons) +{ + // NOTE: Under construction + pcl::PointCloud raw_cloud; + + if (polygons.size() >= 2) { + RCLCPP_FATAL_STREAM( + rclcpp::get_logger("polygon"), "current ground server does not handle multi polygons"); + } + + for (const lanelet::ConstPolygon3d & polygon : polygons) { + for (const lanelet::ConstPoint3d & p : polygon) { + pcl::PointXYZ xyz; + xyz.x = p.x(); + xyz.y = p.y(); + xyz.z = p.z(); + raw_cloud.push_back(xyz); + } + } + return fill_points_in_polygon(raw_cloud); +} + +void push_back_line( + pcl::PointCloud & dst_cloud, const pcl::PointXYZ & from, const pcl::PointXYZ & to) +{ + Eigen::Vector3f f = from.getVector3fMap(); + Eigen::Vector3f t = to.getVector3fMap(); + const float L = (f - t).norm(); + + for (float l = 0.f; l < L; l += 0.25f) { + Eigen::Vector3f xyz = f + (t - f) * l; + dst_cloud.emplace_back(xyz.x(), xyz.y(), xyz.z()); + } +} + +void push_back_contour( + pcl::PointCloud & dst_cloud, const pcl::PointCloud & vertices) +{ + const int N = vertices.size(); + for (int i = 0; i < N - 1; ++i) { + push_back_line(dst_cloud, vertices.at(i), vertices.at(i + 1)); + } + push_back_line(dst_cloud, vertices.at(0), vertices.at(N - 1)); +} + +pcl::PointCloud shrink_vertices( + const pcl::PointCloud & vertices, float rate) +{ + Eigen::Vector3f center = Eigen::Vector3f::Zero(); + for (const pcl::PointXYZ p : vertices) center += p.getVector3fMap(); + center /= vertices.size(); + + pcl::PointCloud dst_cloud; + for (const pcl::PointXYZ p : vertices) { + Eigen::Vector3f xyz = (p.getVector3fMap() - center) * rate + center; + dst_cloud.emplace_back(xyz.x(), xyz.y(), xyz.z()); + } + return dst_cloud; +} + +pcl::PointCloud fill_points_in_polygon( + const pcl::PointCloud & src_cloud) +{ + pcl::PointCloud dst_cloud; + + std::vector shrink_rates = {1.0, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1}; + for (float rate : shrink_rates) { + push_back_contour(dst_cloud, shrink_vertices(src_cloud, rate)); + } + + return dst_cloud; +} +} // namespace yabloc::ground_server diff --git a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp new file mode 100644 index 0000000000000..f2e496a3bcc8b --- /dev/null +++ b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp @@ -0,0 +1,265 @@ +// 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 "yabloc_common/ll2_decomposer/ll2_decomposer.hpp" + +#include +#include +#include + +#include + +#include + +namespace yabloc::ll2_decomposer +{ +Ll2Decomposer::Ll2Decomposer() : Node("ll2_to_image") +{ + using std::placeholders::_1; + const rclcpp::QoS latch_qos = rclcpp::QoS(10).transient_local(); + const rclcpp::QoS map_qos = rclcpp::QoS(10).transient_local().reliable(); + + // Publisher + pub_road_marking_ = create_publisher("~/output/ll2_road_marking", latch_qos); + pub_sign_board_ = create_publisher("~/output/ll2_sign_board", latch_qos); + pub_bounding_box_ = create_publisher("~/output/ll2_bounding_box", latch_qos); + pub_marker_ = create_publisher("~/debug/sign_board_marker", latch_qos); + + // Subscriber + auto cb_map = std::bind(&Ll2Decomposer::on_map, this, _1); + sub_map_ = create_subscription("~/input/vector_map", map_qos, cb_map); + + auto load_lanelet2_labels = + [this](const std::string & param_name, std::set & labels) -> void { + this->template declare_parameter>(param_name); + auto label_array = get_parameter(param_name).as_string_array(); + for (auto l : label_array) labels.insert(l); + }; + + load_lanelet2_labels("road_marking_labels", road_marking_labels_); + load_lanelet2_labels("sign_board_labels", sign_board_labels_); + load_lanelet2_labels("bounding_box_labels", bounding_box_labels_); + + if (road_marking_labels_.empty()) { + RCLCPP_FATAL_STREAM( + get_logger(), "There are no road marking labels. No LL2 elements will publish"); + } +} + +void print_attr(const lanelet::LaneletMapPtr & lanelet_map, const rclcpp::Logger & logger) +{ + std::set types; + for (const lanelet::ConstLineString3d & line : lanelet_map->lineStringLayer) { + if (!line.hasAttribute(lanelet::AttributeName::Type)) continue; + lanelet::Attribute attr = line.attribute(lanelet::AttributeName::Type); + types.insert(attr.value()); + } + for (const lanelet::ConstPolygon3d & polygon : lanelet_map->polygonLayer) { + if (!polygon.hasAttribute(lanelet::AttributeName::Type)) { + continue; + } + lanelet::Attribute attr = polygon.attribute(lanelet::AttributeName::Type); + types.insert(attr.value()); + } + + for (const auto & type : types) { + RCLCPP_INFO_STREAM(logger, "lanelet type: " << type); + } +} + +pcl::PointCloud Ll2Decomposer::load_bounding_boxes( + const lanelet::PolygonLayer & polygons) const +{ + pcl::PointCloud cloud; + int index = 0; + + for (const lanelet::ConstPolygon3d & polygon : polygons) { + if (!polygon.hasAttribute(lanelet::AttributeName::Type)) continue; + lanelet::Attribute attr = polygon.attribute(lanelet::AttributeName::Type); + if (bounding_box_labels_.count(attr.value()) == 0) continue; + + for (const lanelet::ConstPoint3d & p : polygon) { + pcl::PointXYZL xyzl; + xyzl.x = p.x(); + xyzl.y = p.y(); + xyzl.z = p.z(); + xyzl.label = index; + cloud.push_back(xyzl); + } + index++; + } + return cloud; +} + +void Ll2Decomposer::on_map(const HADMapBin & msg) +{ + RCLCPP_INFO_STREAM(get_logger(), "subscribed binary vector map"); + lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); + lanelet::utils::conversion::fromBinMsg(msg, lanelet_map); + print_attr(lanelet_map, get_logger()); + + const rclcpp::Time stamp = msg.header.stamp; + + const auto & ls_layer = lanelet_map->lineStringLayer; + const auto & po_layer = lanelet_map->polygonLayer; + auto tmp1 = extract_specified_line_string(ls_layer, sign_board_labels_); + auto tmp2 = extract_specified_line_string(ls_layer, road_marking_labels_); + pcl::PointCloud ll2_sign_board = split_line_strings(tmp1); + pcl::PointCloud ll2_road_marking = split_line_strings(tmp2); + pcl::PointCloud ll2_bounding_box = load_bounding_boxes(po_layer); + + publish_additional_marker(lanelet_map); + + common::publish_cloud(*pub_road_marking_, ll2_road_marking, stamp); + common::publish_cloud(*pub_sign_board_, ll2_sign_board, stamp); + common::publish_cloud(*pub_bounding_box_, ll2_bounding_box, stamp); + + RCLCPP_INFO_STREAM(get_logger(), "successed map decomposing"); +} + +pcl::PointCloud Ll2Decomposer::split_line_strings( + const lanelet::ConstLineStrings3d & line_strings) +{ + pcl::PointCloud extracted; + for (const lanelet::ConstLineString3d & line : line_strings) { + lanelet::ConstPoint3d const * from = nullptr; + for (const lanelet::ConstPoint3d & to : line) { + if (from != nullptr) { + pcl::PointNormal pn = to_point_normal(*from, to); + extracted.push_back(pn); + } + from = &to; + } + } + return extracted; +} + +lanelet::ConstLineStrings3d Ll2Decomposer::extract_specified_line_string( + const lanelet::LineStringLayer & line_string_layer, const std::set & visible_labels) +{ + lanelet::ConstLineStrings3d line_strings; + for (const lanelet::ConstLineString3d & line : line_string_layer) { + if (!line.hasAttribute(lanelet::AttributeName::Type)) continue; + lanelet::Attribute attr = line.attribute(lanelet::AttributeName::Type); + if (visible_labels.count(attr.value()) == 0) continue; + line_strings.push_back(line); + } + return line_strings; +} + +lanelet::ConstPolygons3d Ll2Decomposer::extract_specified_polygon( + const lanelet::PolygonLayer & polygon_layer, const std::set & visible_labels) +{ + lanelet::ConstPolygons3d polygons; + for (const lanelet::ConstPolygon3d & polygon : polygon_layer) { + if (!polygon.hasAttribute(lanelet::AttributeName::Type)) continue; + lanelet::Attribute attr = polygon.attribute(lanelet::AttributeName::Type); + if (visible_labels.count(attr.value()) == 0) continue; + polygons.push_back(polygon); + } + return polygons; +} + +pcl::PointNormal Ll2Decomposer::to_point_normal( + const lanelet::ConstPoint3d & from, const lanelet::ConstPoint3d & to) const +{ + pcl::PointNormal pn; + pn.x = from.x(); + pn.y = from.y(); + pn.z = from.z(); + pn.normal_x = to.x(); + pn.normal_y = to.y(); + pn.normal_z = to.z(); + return pn; +} + +Ll2Decomposer::MarkerArray Ll2Decomposer::make_sign_marker_msg( + const lanelet::LineStringLayer & line_string_layer, const std::set & labels, + const std::string & ns) +{ + lanelet::ConstLineStrings3d line_strings = + extract_specified_line_string(line_string_layer, labels); + + MarkerArray marker_array; + int id = 0; + for (const lanelet::ConstLineString3d & line_string : line_strings) { + Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = get_clock()->now(); + marker.type = Marker::LINE_STRIP; + marker.color = common::Color(0.6f, 0.6f, 0.6f, 0.999f); + marker.scale.x = 0.1; + marker.ns = ns; + marker.id = id++; + + for (const lanelet::ConstPoint3d & p : line_string) { + geometry_msgs::msg::Point gp; + gp.x = p.x(); + gp.y = p.y(); + gp.z = p.z(); + marker.points.push_back(gp); + } + marker_array.markers.push_back(marker); + } + return marker_array; +} + +Ll2Decomposer::MarkerArray Ll2Decomposer::make_polygon_marker_msg( + const lanelet::PolygonLayer & polygon_layer, const std::set & labels, + const std::string & ns) +{ + lanelet::ConstPolygons3d polygons = extract_specified_polygon(polygon_layer, labels); + + MarkerArray marker_array; + int id = 0; + for (const lanelet::ConstPolygon3d & polygon : polygons) { + Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = get_clock()->now(); + marker.type = Marker::LINE_STRIP; + marker.color = common::Color(0.4f, 0.4f, 0.8f, 0.999f); + marker.scale.x = 0.2; + marker.ns = ns; + marker.id = id++; + + auto gen_point = [](const lanelet::ConstPoint3d & p) -> geometry_msgs::msg::Point { + geometry_msgs::msg::Point gp; + gp.x = p.x(); + gp.y = p.y(); + gp.z = p.z(); + return gp; + }; + + for (const lanelet::ConstPoint3d & p : polygon) marker.points.push_back(gen_point(p)); + marker.points.push_back(gen_point(polygon.front())); + + marker_array.markers.push_back(marker); + } + return marker_array; +} + +void Ll2Decomposer::publish_additional_marker(const lanelet::LaneletMapPtr & lanelet_map) +{ + auto marker1 = + make_sign_marker_msg(lanelet_map->lineStringLayer, sign_board_labels_, "sign_board"); + auto marker2 = make_sign_marker_msg(lanelet_map->lineStringLayer, {"virtual"}, "virtual"); + auto marker3 = + make_polygon_marker_msg(lanelet_map->polygonLayer, {"bounding_box"}, "bounding_box"); + + std::copy(marker2.markers.begin(), marker2.markers.end(), std::back_inserter(marker1.markers)); + std::copy(marker3.markers.begin(), marker3.markers.end(), std::back_inserter(marker1.markers)); + pub_marker_->publish(marker1); +} + +} // namespace yabloc::ll2_decomposer diff --git a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_node.cpp b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_node.cpp new file mode 100644 index 0000000000000..692f6a5da0913 --- /dev/null +++ b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_node.cpp @@ -0,0 +1,23 @@ +// 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 "yabloc_common/ll2_decomposer/ll2_decomposer.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/localization/yabloc/yabloc_common/src/pose_conversions.cpp b/localization/yabloc/yabloc_common/src/pose_conversions.cpp new file mode 100644 index 0000000000000..6ad914eeaf916 --- /dev/null +++ b/localization/yabloc/yabloc_common/src/pose_conversions.cpp @@ -0,0 +1,68 @@ +// 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 "yabloc_common/pose_conversions.hpp" + +#include + +namespace yabloc::common +{ +geometry_msgs::msg::Pose affine_to_pose(const Eigen::Affine3f & affine) +{ + geometry_msgs::msg::Pose pose; + Eigen::Vector3f pos = affine.translation(); + Eigen::Quaternionf ori(affine.rotation()); + pose.position.x = pos.x(); + pose.position.y = pos.y(); + pose.position.z = pos.z(); + pose.orientation.w = ori.w(); + pose.orientation.x = ori.x(); + pose.orientation.y = ori.y(); + pose.orientation.z = ori.z(); + return pose; +} + +Eigen::Affine3f pose_to_affine(const geometry_msgs::msg::Pose & pose) +{ + const auto pos = pose.position; + const auto ori = pose.orientation; + Eigen::Translation3f t(pos.x, pos.y, pos.z); + Eigen::Quaternionf q(ori.w, ori.x, ori.y, ori.z); + return t * q; +} + +Sophus::SE3f pose_to_se3(const geometry_msgs::msg::Pose & pose) +{ + auto ori = pose.orientation; + auto pos = pose.position; + Eigen::Quaternionf q(ori.w, ori.x, ori.y, ori.z); + Eigen::Vector3f t(pos.x, pos.y, pos.z); + return {q, t}; +} + +geometry_msgs::msg::Pose se3_to_pose(const Sophus::SE3f & se3f) +{ + geometry_msgs::msg::Pose pose; + Eigen::Vector3f pos = se3f.translation(); + Eigen::Quaternion ori = se3f.so3().unit_quaternion(); + pose.position.x = pos.x(); + pose.position.y = pos.y(); + pose.position.z = pos.z(); + pose.orientation.w = ori.w(); + pose.orientation.x = ori.x(); + pose.orientation.y = ori.y(); + pose.orientation.z = ori.z(); + return pose; +} +} // namespace yabloc::common diff --git a/localization/yabloc/yabloc_common/src/pub_sub.cpp b/localization/yabloc/yabloc_common/src/pub_sub.cpp new file mode 100644 index 0000000000000..102a9012033fe --- /dev/null +++ b/localization/yabloc/yabloc_common/src/pub_sub.cpp @@ -0,0 +1,79 @@ +// 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 "yabloc_common/pub_sub.hpp" + +#include +#include + +namespace yabloc::common +{ +void publish_image( + rclcpp::Publisher & publisher, const cv::Mat & image, + const rclcpp::Time & stamp) +{ + cv_bridge::CvImage raw_image; + raw_image.header.stamp = stamp; + raw_image.header.frame_id = "map"; + if (image.channels() == 3) + raw_image.encoding = "bgr8"; + else + raw_image.encoding = "mono8"; + + if (image.depth() != CV_8U) { + throw std::runtime_error("publish_image can publish only CV_8U"); + } + + raw_image.image = image; + publisher.publish(*raw_image.toImageMsg()); +} + +template +void publish_cloud( + rclcpp::Publisher & publisher, + const pcl::PointCloud & cloud, const rclcpp::Time & stamp) +{ + // Convert to msg + sensor_msgs::msg::PointCloud2 cloud_msg; + pcl::toROSMsg(cloud, cloud_msg); + cloud_msg.header.stamp = stamp; + cloud_msg.header.frame_id = "map"; + publisher.publish(cloud_msg); +} + +template void publish_cloud( + rclcpp::Publisher &, const pcl::PointCloud &, + const rclcpp::Time &); + +template void publish_cloud( + rclcpp::Publisher &, const pcl::PointCloud &, + const rclcpp::Time &); + +template void publish_cloud( + rclcpp::Publisher &, const pcl::PointCloud &, + const rclcpp::Time &); + +template void publish_cloud( + rclcpp::Publisher &, const pcl::PointCloud &, + const rclcpp::Time &); + +template void publish_cloud( + rclcpp::Publisher &, const pcl::PointCloud &, + const rclcpp::Time &); + +template void publish_cloud( + rclcpp::Publisher &, const pcl::PointCloud &, + const rclcpp::Time &); + +} // namespace yabloc::common diff --git a/localization/yabloc/yabloc_common/src/static_tf_subscriber.cpp b/localization/yabloc/yabloc_common/src/static_tf_subscriber.cpp new file mode 100644 index 0000000000000..baf93ad295e9c --- /dev/null +++ b/localization/yabloc/yabloc_common/src/static_tf_subscriber.cpp @@ -0,0 +1,60 @@ +// 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 "yabloc_common/static_tf_subscriber.hpp" + +namespace yabloc::common +{ +StaticTfSubscriber::StaticTfSubscriber(rclcpp::Clock::SharedPtr clock) +{ + tf_buffer_ = std::make_unique(clock); + transform_listener_ = std::make_shared(*tf_buffer_); +} + +std::optional StaticTfSubscriber::se3f( + const std::string & frame_id, const std::string & parent_frame_id) +{ + std::optional opt_aff = (*this)(frame_id, parent_frame_id); + if (!opt_aff.has_value()) return std::nullopt; + + Sophus::SE3f se3f(opt_aff->rotation(), opt_aff->translation()); + return se3f; +} + +std::optional StaticTfSubscriber::operator()( + const std::string & frame_id, const std::string & parent_frame_id) +{ + std::optional extrinsic_{std::nullopt}; + try { + geometry_msgs::msg::TransformStamped ts = + tf_buffer_->lookupTransform(parent_frame_id, frame_id, tf2::TimePointZero); + Eigen::Vector3f p; + p.x() = ts.transform.translation.x; + p.y() = ts.transform.translation.y; + p.z() = ts.transform.translation.z; + + Eigen::Quaternionf q; + q.w() = ts.transform.rotation.w; + q.x() = ts.transform.rotation.x; + q.y() = ts.transform.rotation.y; + q.z() = ts.transform.rotation.z; + extrinsic_ = Eigen::Affine3f::Identity(); + extrinsic_->translation() = p; + extrinsic_->matrix().topLeftCorner(3, 3) = q.toRotationMatrix(); + } catch (tf2::TransformException & ex) { + } + return extrinsic_; +} + +} // namespace yabloc::common diff --git a/localization/yabloc/yabloc_common/src/transform_line_segments.cpp b/localization/yabloc/yabloc_common/src/transform_line_segments.cpp new file mode 100644 index 0000000000000..7bffbfc91d4aa --- /dev/null +++ b/localization/yabloc/yabloc_common/src/transform_line_segments.cpp @@ -0,0 +1,51 @@ +// 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 "yabloc_common/transform_line_segments.hpp" + +namespace yabloc::common +{ +pcl::PointCloud transform_line_segments( + const pcl::PointCloud & src, const Sophus::SE3f & transform) +{ + pcl::PointCloud dst; + for (const pcl::PointXYZLNormal & line : src) { + Eigen::Vector3f p1 = line.getVector3fMap(); + Eigen::Vector3f p2 = line.getNormalVector3fMap(); + + pcl::PointXYZLNormal transformed; + transformed.getVector3fMap() = transform * p1; + transformed.getNormalVector3fMap() = transform * p2; + transformed.label = line.label; + dst.push_back(transformed); + } + return dst; +} + +pcl::PointCloud transform_line_segments( + const pcl::PointCloud & src, const Sophus::SE3f & transform) +{ + pcl::PointCloud dst; + for (const pcl::PointNormal & line : src) { + Eigen::Vector3f p1 = line.getVector3fMap(); + Eigen::Vector3f p2 = line.getNormalVector3fMap(); + + pcl::PointNormal transformed; + transformed.getVector3fMap() = transform * p1; + transformed.getNormalVector3fMap() = transform * p2; + dst.push_back(transformed); + } + return dst; +} +} // namespace yabloc::common diff --git a/localization/yabloc/yabloc_image_processing/CMakeLists.txt b/localization/yabloc/yabloc_image_processing/CMakeLists.txt new file mode 100644 index 0000000000000..58437c085c4b2 --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/CMakeLists.txt @@ -0,0 +1,69 @@ +cmake_minimum_required(VERSION 3.5) +project(yabloc_image_processing) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +# =================================================== +# Eigen3 +find_package(Eigen3 REQUIRED) + +# OpenCV +find_package(OpenCV REQUIRED) + +# PCL +find_package(PCL REQUIRED COMPONENTS common) + +# =================================================== +# Executable +# line segment detector +set(TARGET line_segment_detector_node) +ament_auto_add_executable(${TARGET} + src/line_segment_detector/line_segment_detector_node.cpp + src/line_segment_detector/line_segment_detector_core.cpp) +target_include_directories(${TARGET} PUBLIC include) +target_include_directories(${TARGET} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIRS}) +target_link_libraries(${TARGET} ${OpenCV_LIBS}) + +# graph based segmentation +set(TARGET graph_segment_node) +ament_auto_add_executable(${TARGET} + src/graph_segment/graph_segment_node.cpp + src/graph_segment/graph_segment_core.cpp + src/graph_segment/similar_area_searcher.cpp) +target_include_directories(${TARGET} PUBLIC include) +target_include_directories(${TARGET} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIRS}) +target_link_libraries(${TARGET} ${OpenCV_LIBS}) + +# segment filter +set(TARGET segment_filter_node) +ament_auto_add_executable(${TARGET} + src/segment_filter/segment_filter_node.cpp + src/segment_filter/segment_filter_core.cpp) +target_include_directories(${TARGET} PUBLIC include ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) +target_link_libraries(${TARGET} ${PCL_LIBRARIES} ${OpenCV_LIBS}) + +# undistort +set(TARGET undistort_node) +ament_auto_add_executable(${TARGET} + src/undistort/undistort_node.cpp) +target_link_libraries(${TARGET} ${OpenCV_LIBS}) + +# line_segments_overlay +set(TARGET line_segments_overlay_node) +ament_auto_add_executable(${TARGET} + src/line_segments_overlay/line_segments_overlay_core.cpp + src/line_segments_overlay/line_segments_overlay_node.cpp) +target_include_directories(${TARGET} PUBLIC include ${EIGEN_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) +target_link_libraries(${TARGET} ${PCL_LIBRARIES}) + +# lanelet2_overlay +set(TARGET lanelet2_overlay_node) +ament_auto_add_executable(${TARGET} + src/lanelet2_overlay/lanelet2_overlay_core.cpp + src/lanelet2_overlay/lanelet2_overlay_node.cpp) +target_include_directories(${TARGET} PUBLIC include ${EIGEN_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) +target_link_libraries(${TARGET} ${PCL_LIBRARIES}) + +# =================================================== +ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/localization/yabloc/yabloc_image_processing/README.md b/localization/yabloc/yabloc_image_processing/README.md new file mode 100644 index 0000000000000..bc31c579d39a1 --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/README.md @@ -0,0 +1,203 @@ +# yabloc_image_processing + +This package contains some executable nodes related to image processing. + +- [line_segment_detector](#line_segment_detector) +- [graph_segmentation](#graph_segmentation) +- [segment_filter](#segment_filter) +- [undistort](#undistort) +- [lanelet2_overlay](#lanelet2_overlay) +- [line_segments_overlay](#line_segments_overlay) + +## line_segment_detector + +### Purpose + +This node extract all line segments from gray scale image. + +### Inputs / Outputs + +#### Input + +| Name | Type | Description | +| ----------------- | ------------------------- | ----------------- | +| `input/image_raw` | `sensor_msgs::msg::Image` | undistorted image | + +#### Output + +| Name | Type | Description | +| --------------------------------- | ------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------ | +| `output/image_with_line_segments` | `sensor_msgs::msg::Image` | image with line segments highlighted | +| `output/line_segments_cloud` | `sensor_msgs::msg::PointCloud2` | detected line segments as point cloud. each point contains x,y,z, normal_x, normal_y, normal_z and z, and normal_z are always empty. | + +## graph_segmentation + +### Purpose + +This node extract road surface region by [graph-based-segmentation](https://docs.opencv.org/4.5.4/dd/d19/classcv_1_1ximgproc_1_1segmentation_1_1GraphSegmentation.html). + +### Inputs / Outputs + +#### Input + +| Name | Type | Description | +| ----------------- | ------------------------- | ----------------- | +| `input/image_raw` | `sensor_msgs::msg::Image` | undistorted image | + +#### Output + +| Name | Type | Description | +| ------------------------ | ------------------------- | ---------------------------------------------------------- | +| `output/mask_image` | `sensor_msgs::msg::Image` | image with masked segments determined as road surface area | +| `output/segmented_image` | `sensor_msgs::msg::Image` | segmented image for visualization | + +### Parameters + +| Name | Type | Description | +| -------------------------------- | ------ | ------------------------------------------------------------------ | +| `target_height_ratio` | double | height on the image to retrieve the candidate road surface | +| `target_candidate_box_width` | int | size of the square area to search for candidate road surfaces | +| `pickup_addtional_graph_segment` | bool | if this is true, additional regions of similar color are retrieved | +| `similarity_score_threshold` | double | threshold for picking up additional areas | +| `sigma` | double | parameters for cv::ximgproc::segmentation | +| `k` | double | parameters for cv::ximgproc::segmentation | +| `min_size` | double | parameters for cv::ximgproc::segmentation | + +## segment_filter + +### Purpose + +This is a node that integrates the results of graph_segment and lsd to extract road surface markings. + +### Inputs / Outputs + +#### Input + +| Name | Type | Description | +| --------------------------- | ------------------------------- | ---------------------------------------------------------- | +| `input/line_segments_cloud` | `sensor_msgs::msg::PointCloud2` | detected line segment | +| `input/mask_image` | `sensor_msgs::msg::Image` | image with masked segments determined as road surface area | +| `input/camera_info` | `sensor_msgs::msg::CameraInfo` | undistorted camera info | + +#### Output + +| Name | Type | Description | +| -------------------------------------- | ------------------------------- | -------------------------------------------------- | +| `output/line_segments_cloud` | `sensor_msgs::msg::PointCloud2` | filtered line segments for visualization | +| `output/projected_image` | `sensor_msgs::msg::Image` | projected filtered line segments for visualization | +| `output/projected_line_segments_cloud` | `sensor_msgs::msg::PointCloud2` | projected filtered line segments | + +### Parameters + +| Name | Type | Description | +| -------------------------------------- | ------ | ------------------------------------------------------------------- | +| `min_segment_length` | double | min lenght threshold (if it is negative, it is unlimited) | +| `max_segment_distance` | double | max distance threshold (if it is negative, it is unlimited) | +| `max_lateral_distance` | double | max lateral distance threshold (if it is negative, it is unlimited) | +| `publish_image_with_segment_for_debug` | bool | toggle whether to publish the filtered line segment for debug | +| `max_range` | double | range of debug projection visualization | +| `image_size` | int | image size of debug projection visualization | + +## undistort + +### Purpose + +This node performs image resizing and undistortion at the same time. + +### Inputs / Outputs + +#### Input + +| Name | Type | Description | +| ---------------------------- | ----------------------------------- | -------------------- | +| `input/camera_info` | `sensor_msgs::msg::CameraInfo` | raw compressed image | +| `input/image_raw/compressed` | `sensor_msgs::msg::CompressedImage` | raw sensor info | + +#### Output + +| Name | Type | Description | +| -------------------- | ----------------------------------- | ----------------------------- | +| `output/camera_info` | `sensor_msgs::msg::CameraInfo` | resized camera info | +| `output/image_raw` | `sensor_msgs::msg::CompressedImage` | undistorted and resized image | + +### Parameters + +| Name | Type | Description | +| ------------------- | ------ | ---------------------------------------------------------------------------------------------- | +| `use_sensor_qos` | bool | where to use sensor qos or not | +| `width` | int | resized image width size | +| `override_frame_id` | string | value for overriding the camera's frame_id. if blank, frame_id of static_tf is not overwritten | + +#### about tf_static overriding + +
click to open
+ +Some nodes requires `/tf_static` from `/base_link` to the frame_id of `/sensing/camera/traffic_light/image_raw/compressed` (e.g. `/traffic_light_left_camera/camera_optical_link`). +You can verify that the tf_static is correct with the following command. + +```shell +ros2 run tf2_ros tf2_echo base_link traffic_light_left_camera/camera_optical_link +``` + +If the wrong `/tf_static` are broadcasted due to using a prototype vehicle, not having accurate calibration data, or some other unavoidable reason, it is useful to give the frame_id in `override_camera_frame_id`. +If you give it a non-empty string, `/image_processing/undistort_node` will rewrite the frame_id in camera_info. +For example, you can give a different tf_static as follows. + +```shell +ros2 launch yabloc_launch sample_launch.xml override_camera_frame_id:=fake_camera_optical_link +ros2 run tf2_ros static_transform_publisher \ + --frame-id base_link \ + --child-frame-id fake_camera_optical_link \ + --roll -1.57 \ + --yaw -1.570 +``` + +
+ +## lanelet2_overlay + +### Purpose + +This node overlays lanelet2 on the camera image based on the estimated self-position. + +### Inputs / Outputs + +#### Input + +| Name | Type | Description | +| ------------------------------------- | ---------------------------------- | --------------------------------------------------- | +| `input/pose` | `geometry_msgs::msg::PoseStamped` | estimated self pose | +| `input/projected_line_segments_cloud` | `sensor_msgs::msg::PointCloud2` | projected line segments including non-road markings | +| `input/camera_info` | `sensor_msgs::msg::CameraInfo` | undistorted camera info | +| `input/image_raw` | `sensor_msgs::msg::Image` | undistorted camera image | +| `input/ground` | `std_msgs::msg::Float32MultiArray` | ground tilt | +| `input/ll2_road_marking` | `sensor_msgs::msg::PointCloud2` | lanelet2 elements regarding road surface markings | +| `input/ll2_sign_board` | `sensor_msgs::msg::PointCloud2` | lanelet2 elements regarding traffic sign boards | + +#### Output + +| Name | Type | Description | +| ------------------------------- | --------------------------------- | ------------------------------------------------------ | +| `output/lanelet2_overlay_image` | `sensor_msgs::msg::Image` | lanelet2 overlayed image | +| `output/projected_marker` | `visualization_msgs::msg::Marker` | 3d projected line segments including non-road markings | + +## line_segments_overlay + +### Purpose + +This node visualize classified line segments on the camera image + +### Inputs / Outputs + +#### Input + +| Name | Type | Description | +| --------------------------- | ------------------------------- | ------------------------ | +| `input/line_segments_cloud` | `sensor_msgs::msg::PointCloud2` | classied line segments | +| `input/image_raw` | `sensor_msgs::msg::Image` | undistorted camera image | + +#### Output + +| Name | Type | Description | +| ----------------------------------------- | ------------------------- | ------------------------------------ | +| `output/image_with_colored_line_segments` | `sensor_msgs::msg::Image` | image with highlighted line segments | diff --git a/localization/yabloc/yabloc_image_processing/config/graph_segment.param.yaml b/localization/yabloc/yabloc_image_processing/config/graph_segment.param.yaml new file mode 100644 index 0000000000000..4c80df94ea289 --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/config/graph_segment.param.yaml @@ -0,0 +1,18 @@ +/**: + ros__parameters: + # graph_node selects a road surface area from around this height + target_height_ratio: 0.85 + + # + target_candidate_box_width: 15 + + # graph_segment_node will pickup additional road-like areas + pickup_additional_graph_segment: true + + # used when the pickup_additional_graph_segment: true + similarity_score_threshold: 0.8 + + # parameters for cv::ximgproc::segmentation + sigma: 0.5 + k: 300.0 + min_size: 100.0 diff --git a/localization/yabloc/yabloc_image_processing/config/segment_filter.param.yaml b/localization/yabloc/yabloc_image_processing/config/segment_filter.param.yaml new file mode 100644 index 0000000000000..e2ba24d267d76 --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/config/segment_filter.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + min_segment_length: 1.5 # if it is negative, it is unlimited + max_segment_distance: 30.0 # if it is negative, it is unlimited + max_lateral_distance: 10.0 # if it is negative, it is unlimited + publish_image_with_segment_for_debug: true + max_range: 20.0 + image_size: 800 # debug diff --git a/localization/yabloc/yabloc_image_processing/config/undistort.param.yaml b/localization/yabloc/yabloc_image_processing/config/undistort.param.yaml new file mode 100644 index 0000000000000..77f9be2c4a2cd --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/config/undistort.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + use_sensor_qos: true + width: 800 + override_frame_id: "" # Value for overriding the camera's frame_id. If blank, frame_id of static_tf is not overwritten diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/graph_segment.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/graph_segment.hpp new file mode 100644 index 0000000000000..a1e9c90eaa362 --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/graph_segment.hpp @@ -0,0 +1,56 @@ +// 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 YABLOC_IMAGE_PROCESSING__GRAPH_SEGMENT__GRAPH_SEGMENT_HPP_ +#define YABLOC_IMAGE_PROCESSING__GRAPH_SEGMENT__GRAPH_SEGMENT_HPP_ + +#include "yabloc_image_processing/graph_segment/similar_area_searcher.hpp" + +#include +#include + +#include +#include + +#include + +namespace yabloc::graph_segment +{ +class GraphSegment : public rclcpp::Node +{ +public: + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using Image = sensor_msgs::msg::Image; + GraphSegment(); + +private: + const float target_height_ratio_; + const int target_candidate_box_width_; + + rclcpp::Subscription::SharedPtr sub_image_; + rclcpp::Publisher::SharedPtr pub_mask_image_; + rclcpp::Publisher::SharedPtr pub_debug_image_; + cv::Ptr segmentation_; + std::unique_ptr similar_area_searcher_{nullptr}; + + void on_image(const Image & msg); + + int search_most_road_like_class(const cv::Mat & segmented) const; + + void draw_and_publish_image( + const cv::Mat & raw_image, const cv::Mat & debug_image, const rclcpp::Time & stamp); +}; +} // namespace yabloc::graph_segment + +#endif // YABLOC_IMAGE_PROCESSING__GRAPH_SEGMENT__GRAPH_SEGMENT_HPP_ diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/histogram.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/histogram.hpp new file mode 100644 index 0000000000000..c3ac2475f7e08 --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/histogram.hpp @@ -0,0 +1,72 @@ +// 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 YABLOC_IMAGE_PROCESSING__GRAPH_SEGMENT__HISTOGRAM_HPP_ +#define YABLOC_IMAGE_PROCESSING__GRAPH_SEGMENT__HISTOGRAM_HPP_ + +#include +#include + +#include + +namespace yabloc::graph_segment +{ +struct Histogram +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + explicit Histogram(int bin = 10) : bin(bin) { data = Eigen::MatrixXf::Zero(3, bin); } + + Eigen::MatrixXf eval() const + { + float sum = data.sum(); + if (sum < 1e-6f) throw std::runtime_error("invalid division"); + return data / sum; + } + + void add(const cv::Vec3b & rgb) + { + for (int ch = 0; ch < 3; ++ch) { + int index = std::clamp(static_cast(rgb[ch] * bin / 255.f), 0, bin - 1); + data(ch, index) += 1.0f; + } + } + const int bin; + Eigen::MatrixXf data; + + static float eval_histogram_intersection(const Eigen::MatrixXf & a, const Eigen::MatrixXf & b) + { + float score = 0; + for (int c = 0; c < a.cols(); c++) { + for (int r = 0; r < a.rows(); r++) { + score += std::min(a(r, c), b(r, c)); + } + } + return score; + }; + + static float eval_bhattacharyya_coeff(const Eigen::MatrixXf & a, const Eigen::MatrixXf & b) + { + float score = 0; + for (int c = 0; c < a.cols(); c++) { + for (int r = 0; r < a.rows(); r++) { + score += std::sqrt(a(r, c) * b(r, c)); + } + } + return score; + }; +}; + +} // namespace yabloc::graph_segment + +#endif // YABLOC_IMAGE_PROCESSING__GRAPH_SEGMENT__HISTOGRAM_HPP_ diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/similar_area_searcher.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/similar_area_searcher.hpp new file mode 100644 index 0000000000000..d6f50b15d04a9 --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/similar_area_searcher.hpp @@ -0,0 +1,52 @@ +// 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 YABLOC_IMAGE_PROCESSING__GRAPH_SEGMENT__SIMILAR_AREA_SEARCHER_HPP_ +#define YABLOC_IMAGE_PROCESSING__GRAPH_SEGMENT__SIMILAR_AREA_SEARCHER_HPP_ + +#include +#include +#include + +#include + +namespace yabloc::graph_segment +{ +// This class takes a region-segmented image and an index referring to one of those +// regions, and returns region with the same color as the specified region. For each region, it +// computes the RGB histogram and determines the similarity of regions based on the distance between +// their histograms. +// +// The class only has one parameter `similarity_score_threshold`, which is the threshold of +// similarity used to determine if two histograms are considered a match. It takes a +// value between 0 and 1. A value closer to 0 is more likely to be being considered a match. +class SimilarAreaSearcher +{ +public: + explicit SimilarAreaSearcher(float similarity_score_threshold) + : similarity_score_threshold_(similarity_score_threshold), + logger_(rclcpp::get_logger("similar_area_searcher")) + { + } + + std::set search( + const cv::Mat & rgb_image, const cv::Mat & segmented, int best_roadlike_class); + +private: + const float similarity_score_threshold_; + rclcpp::Logger logger_; +}; +} // namespace yabloc::graph_segment + +#endif // YABLOC_IMAGE_PROCESSING__GRAPH_SEGMENT__SIMILAR_AREA_SEARCHER_HPP_ diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp new file mode 100644 index 0000000000000..468e765b74175 --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp @@ -0,0 +1,85 @@ +// 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 YABLOC_IMAGE_PROCESSING__LANELET2_OVERLAY__LANELET2_OVERLAY_HPP_ +#define YABLOC_IMAGE_PROCESSING__LANELET2_OVERLAY__LANELET2_OVERLAY_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace yabloc::lanelet2_overlay +{ +class Lanelet2Overlay : public rclcpp::Node +{ +public: + using Pose = geometry_msgs::msg::Pose; + using PoseStamped = geometry_msgs::msg::PoseStamped; + using Marker = visualization_msgs::msg::Marker; + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using LineSegments = pcl::PointCloud; + using CameraInfo = sensor_msgs::msg::CameraInfo; + using Image = sensor_msgs::msg::Image; + using Float32Array = std_msgs::msg::Float32MultiArray; + + Lanelet2Overlay(); + +private: + common::StaticTfSubscriber tf_subscriber_; + common::GroundPlane ground_plane_; + + rclcpp::Publisher::SharedPtr pub_image_; + rclcpp::Subscription::SharedPtr sub_pose_; + rclcpp::Subscription::SharedPtr sub_image_; + rclcpp::Subscription::SharedPtr sub_info_; + rclcpp::Subscription::SharedPtr sub_ll2_; + rclcpp::Subscription::SharedPtr sub_line_segments_cloud_; + rclcpp::Subscription::SharedPtr sub_sign_board_; + rclcpp::Subscription::SharedPtr sub_ground_plane_; + + rclcpp::Publisher::SharedPtr pub_vis_; + + std::optional info_{std::nullopt}; + std::optional camera_extrinsic_{std::nullopt}; + LineSegments ll2_cloud_, sign_board_; + boost::circular_buffer pose_buffer_; + + void on_info(const CameraInfo & msg); + void on_image(const Image & msg); + void on_line_segments(const PointCloud2 & msg); + + void draw_overlay( + const cv::Mat & image, const std::optional & pose, const rclcpp::Time & stamp); + void draw_overlay_line_segments( + cv::Mat & image, const Pose & pose, const LineSegments & line_segments); + + void make_vis_marker(const LineSegments & ls, const Pose & pose, const rclcpp::Time & stamp); +}; +} // namespace yabloc::lanelet2_overlay + +#endif // YABLOC_IMAGE_PROCESSING__LANELET2_OVERLAY__LANELET2_OVERLAY_HPP_ diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp new file mode 100644 index 0000000000000..74ef82dd94f59 --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp @@ -0,0 +1,61 @@ +// 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 YABLOC_IMAGE_PROCESSING__LINE_SEGMENT_DETECTOR__LINE_SEGMENT_DETECTOR_HPP_ +#define YABLOC_IMAGE_PROCESSING__LINE_SEGMENT_DETECTOR__LINE_SEGMENT_DETECTOR_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace yabloc::line_segment_detector +{ +class LineSegmentDetector : public rclcpp::Node +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + using CompressedImage = sensor_msgs::msg::CompressedImage; + using Image = sensor_msgs::msg::Image; + using PointCloud2 = sensor_msgs::msg::PointCloud2; + + LineSegmentDetector(); + +private: + rclcpp::Subscription::SharedPtr sub_image_; + rclcpp::Publisher::SharedPtr pub_image_with_line_segments_; + rclcpp::Publisher::SharedPtr pub_cloud_; + + cv::Ptr line_segment_detector_; + + std::vector remove_too_outer_elements( + const cv::Mat & lines, const cv::Size & size) const; + void on_image(const sensor_msgs::msg::Image & msg); + void execute(const cv::Mat & image, const rclcpp::Time & stamp); +}; +} // namespace yabloc::line_segment_detector + +#endif // YABLOC_IMAGE_PROCESSING__LINE_SEGMENT_DETECTOR__LINE_SEGMENT_DETECTOR_HPP_ diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp new file mode 100644 index 0000000000000..abbc2f75725ed --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp @@ -0,0 +1,51 @@ +// 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 YABLOC_IMAGE_PROCESSING__LINE_SEGMENTS_OVERLAY__LINE_SEGMENTS_OVERLAY_HPP_ +#define YABLOC_IMAGE_PROCESSING__LINE_SEGMENTS_OVERLAY__LINE_SEGMENTS_OVERLAY_HPP_ + +#include + +#include +#include + +#include +#include + +#include + +namespace yabloc::line_segments_overlay +{ +class LineSegmentsOverlay : public rclcpp::Node +{ +public: + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using Image = sensor_msgs::msg::Image; + using LineSegment = pcl::PointXYZLNormal; + using LineSegments = pcl::PointCloud; + LineSegmentsOverlay(); + +private: + void on_image(const Image::ConstSharedPtr & img_msg); + void on_line_segments(const PointCloud2::ConstSharedPtr & line_segments_msg); + rclcpp::Subscription::SharedPtr sub_image_; + rclcpp::Subscription::SharedPtr sub_line_segments_; + rclcpp::Publisher::SharedPtr pub_debug_image_; + + std::map image_buffer_; + size_t max_buffer_size_; +}; +} // namespace yabloc::line_segments_overlay + +#endif // YABLOC_IMAGE_PROCESSING__LINE_SEGMENTS_OVERLAY__LINE_SEGMENTS_OVERLAY_HPP_ diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp new file mode 100644 index 0000000000000..766bb77d4da85 --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp @@ -0,0 +1,79 @@ +// 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 YABLOC_IMAGE_PROCESSING__SEGMENT_FILTER__SEGMENT_FILTER_HPP_ +#define YABLOC_IMAGE_PROCESSING__SEGMENT_FILTER__SEGMENT_FILTER_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include + +namespace yabloc::segment_filter +{ +class SegmentFilter : public rclcpp::Node +{ +public: + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using Image = sensor_msgs::msg::Image; + + SegmentFilter(); + +private: + using ProjectFunc = std::function(const Eigen::Vector3f &)>; + const int image_size_; + const float max_range_; + const float min_segment_length_; + const float max_segment_distance_; + const float max_lateral_distance_; + + common::CameraInfoSubscriber info_; + common::SynchroSubscriber synchro_subscriber_; + common::StaticTfSubscriber tf_subscriber_; + + rclcpp::Publisher::SharedPtr pub_projected_cloud_; + rclcpp::Publisher::SharedPtr pub_debug_cloud_; + rclcpp::Publisher::SharedPtr pub_image_; + + ProjectFunc project_func_ = nullptr; + + // Return true if success to define or already defined + bool define_project_func(); + + pcl::PointCloud project_lines( + const pcl::PointCloud & lines, const std::set & indices, + bool negative = false) const; + + std::set filter_by_mask( + const cv::Mat & mask, const pcl::PointCloud & edges); + + cv::Point2i to_cv_point(const Eigen::Vector3f & v) const; + void execute(const PointCloud2 & msg1, const Image & msg2); + + bool is_near_element(const pcl::PointNormal & pn, pcl::PointNormal & truncated_pn) const; +}; +} // namespace yabloc::segment_filter + +#endif // YABLOC_IMAGE_PROCESSING__SEGMENT_FILTER__SEGMENT_FILTER_HPP_ diff --git a/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml new file mode 100644 index 0000000000000..0d3850bf74590 --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml @@ -0,0 +1,52 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml b/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml new file mode 100644 index 0000000000000..70c3e8f553446 --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/yabloc/yabloc_image_processing/launch/yabloc_image_processing.launch.xml b/localization/yabloc/yabloc_image_processing/launch/yabloc_image_processing.launch.xml new file mode 100644 index 0000000000000..8fab2731fe24e --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/launch/yabloc_image_processing.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/yabloc/yabloc_image_processing/package.xml b/localization/yabloc/yabloc_image_processing/package.xml new file mode 100644 index 0000000000000..92d8210be85a3 --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/package.xml @@ -0,0 +1,32 @@ + + + + yabloc_image_processing + 0.0.0 + YabLoc image processing package + Kento Yabuuchi + Apache License 2.0 + + ament_cmake + autoware_cmake + + cv_bridge + nav_msgs + pcl_conversions + rclcpp + sensor_msgs + sophus + std_msgs + tf2 + tf2_ros + tier4_autoware_utils + visualization_msgs + yabloc_common + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp new file mode 100644 index 0000000000000..75e14b5a4cd4b --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp @@ -0,0 +1,161 @@ +// 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 "yabloc_image_processing/graph_segment/graph_segment.hpp" +#include "yabloc_image_processing/graph_segment/histogram.hpp" + +#include +#include +#include +#include +#include + +namespace yabloc::graph_segment +{ +GraphSegment::GraphSegment() +: Node("graph_segment"), + target_height_ratio_(declare_parameter("target_height_ratio")), + target_candidate_box_width_(declare_parameter("target_candidate_box_width")) +{ + using std::placeholders::_1; + + // Subscriber + sub_image_ = create_subscription( + "~/input/image_raw", 10, std::bind(&GraphSegment::on_image, this, _1)); + + pub_mask_image_ = create_publisher("~/output/mask_image", 10); + pub_debug_image_ = create_publisher("~/debug/segmented_image", 10); + + const double sigma = declare_parameter("sigma"); + const float k = declare_parameter("k"); + const int min_size = declare_parameter("min_size"); + segmentation_ = cv::ximgproc::segmentation::createGraphSegmentation(sigma, k, min_size); + + // additional area pickup module + if (declare_parameter("pickup_additional_areas", true)) { + similar_area_searcher_ = + std::make_unique(declare_parameter("similarity_score_threshold")); + } +} + +cv::Vec3b random_hsv(int index) +{ + // It generates colors that are not too bright or too vivid, but rich in hues. + double base = static_cast(index); + return cv::Vec3b(std::fmod(base * 0.7, 1.0) * 180, 0.7 * 255, 0.5 * 255); +}; + +int GraphSegment::search_most_road_like_class(const cv::Mat & segmented) const +{ + const int W = target_candidate_box_width_; + const float R = target_height_ratio_; + cv::Point2i target_px(segmented.cols * 0.5, segmented.rows * R); + cv::Rect2i rect(target_px + cv::Point2i(-W, -W), target_px + cv::Point2i(W, W)); + + std::unordered_map areas; + std::unordered_set candidates; + for (int h = 0; h < segmented.rows; h++) { + const int * seg_ptr = segmented.ptr(h); + for (int w = 0; w < segmented.cols; w++) { + int key = seg_ptr[w]; + if (areas.count(key) == 0) areas[key] = 0; + areas[key]++; + if (rect.contains(cv::Point2i{w, h})) candidates.insert(key); + } + } + + // Search the largest area and its class + int max_area = 0; + int max_area_class = -1; + for (int c : candidates) { + if (areas.at(c) < max_area) continue; + max_area = areas.at(c); + max_area_class = c; + } + return max_area_class; +} + +void GraphSegment::on_image(const Image & msg) +{ + cv::Mat image = common::decompress_to_cv_mat(msg); + cv::Mat resized; + cv::resize(image, resized, cv::Size(), 0.5, 0.5); + + // Execute graph-based segmentation + tier4_autoware_utils::StopWatch stop_watch; + cv::Mat segmented; + segmentation_->processImage(resized, segmented); + RCLCPP_INFO_STREAM(get_logger(), "segmentation time: " << stop_watch.toc() * 1000 << "[ms]"); + + // + int target_class = search_most_road_like_class(segmented); + // + std::set road_keys = {target_class}; + if (similar_area_searcher_) { + road_keys = similar_area_searcher_->search(resized, segmented, target_class); + } + + // Draw output image and debug image + cv::Mat output_image = cv::Mat::zeros(resized.size(), CV_8UC1); + cv::Mat debug_image = cv::Mat::zeros(resized.size(), CV_8UC3); + for (int h = 0; h < resized.rows; h++) { + // NOTE: Accessing through ptr() is faster than at() + cv::Vec3b * const debug_image_ptr = debug_image.ptr(h); + uchar * const output_image_ptr = output_image.ptr(h); + const int * const segmented_image_ptr = segmented.ptr(h); + + for (int w = 0; w < resized.cols; w++) { + cv::Point2i px(w, h); + const int key = segmented_image_ptr[w]; + if (road_keys.count(key) > 0) { + output_image_ptr[w] = 255; + if (key == target_class) + debug_image_ptr[w] = cv::Vec3b(30, 255, 255); + else + debug_image_ptr[w] = cv::Vec3b(10, 255, 255); + } else { + debug_image_ptr[w] = random_hsv(key); + } + } + } + cv::cvtColor(debug_image, debug_image, cv::COLOR_HSV2BGR); + cv::resize(output_image, output_image, image.size(), 0, 0, cv::INTER_NEAREST); + cv::resize(debug_image, debug_image, image.size(), 0, 0, cv::INTER_NEAREST); + + common::publish_image(*pub_mask_image_, output_image, msg.header.stamp); + + draw_and_publish_image(image, debug_image, msg.header.stamp); + RCLCPP_INFO_STREAM(get_logger(), "total processing time: " << stop_watch.toc() * 1000 << "[ms]"); +} + +void GraphSegment::draw_and_publish_image( + const cv::Mat & raw_image, const cv::Mat & debug_image, const rclcpp::Time & stamp) +{ + cv::Mat show_image; + cv::addWeighted(raw_image, 0.5, debug_image, 0.8, 1.0, show_image); + const cv::Size size = debug_image.size(); + + // Draw target rectangle + { + const int W = target_candidate_box_width_; + const float R = target_height_ratio_; + cv::Point2i target(size.width / 2, size.height * R); + cv::Rect2i rect(target + cv::Point2i(-W, -W), target + cv::Point2i(W, W)); + cv::rectangle(show_image, rect, cv::Scalar::all(0), 2); + } + + common::publish_image(*pub_debug_image_, show_image, stamp); +} + +} // namespace yabloc::graph_segment diff --git a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_node.cpp b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_node.cpp new file mode 100644 index 0000000000000..d11701e115eff --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_node.cpp @@ -0,0 +1,23 @@ +// 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 "yabloc_image_processing/graph_segment/graph_segment.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/localization/yabloc/yabloc_image_processing/src/graph_segment/similar_area_searcher.cpp b/localization/yabloc/yabloc_image_processing/src/graph_segment/similar_area_searcher.cpp new file mode 100644 index 0000000000000..54b5145d05a1a --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/src/graph_segment/similar_area_searcher.cpp @@ -0,0 +1,83 @@ +// 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 "yabloc_image_processing/graph_segment/similar_area_searcher.hpp" + +#include "yabloc_image_processing/graph_segment/histogram.hpp" + +#include + +#include + +namespace yabloc::graph_segment +{ +struct KeyAndArea +{ + KeyAndArea(int key, int count) : key(key), count(count) {} + int key; + int count; +}; + +std::set SimilarAreaSearcher::search( + const cv::Mat & rgb_image, const cv::Mat & segmented, int best_roadlike_class) +{ + std::unordered_map histogram_map; + std::unordered_map count_map; + + for (int h = 0; h < rgb_image.rows; h++) { + const int * seg_ptr = segmented.ptr(h); + const cv::Vec3b * rgb_ptr = rgb_image.ptr(h); + + for (int w = 0; w < rgb_image.cols; w++) { + int key = seg_ptr[w]; + cv::Vec3b rgb = rgb_ptr[w]; + if (count_map.count(key) == 0) { + count_map[key] = 1; + histogram_map[key].add(rgb); + } else { + count_map[key]++; + histogram_map[key].add(rgb); + } + } + } + + auto compare = [](KeyAndArea a, KeyAndArea b) { return a.count < b.count; }; + std::priority_queue, decltype(compare)> key_queue{compare}; + for (auto [key, count] : count_map) { + key_queue.push({key, count}); + } + + const Eigen::MatrixXf ref_histogram = histogram_map.at(best_roadlike_class).eval(); + + std::stringstream debug_ss; + debug_ss << "histogram equality "; + + int index = 0; + std::set acceptable_keys; + while (!key_queue.empty()) { + KeyAndArea key = key_queue.top(); + key_queue.pop(); + + Eigen::MatrixXf query = histogram_map.at(key.key).eval(); + float score = Histogram::eval_histogram_intersection(ref_histogram, query); + debug_ss << " " << score; + + if (score > similarity_score_threshold_) acceptable_keys.insert(key.key); + if (++index > 10) break; + } + RCLCPP_INFO_STREAM(logger_, debug_ss.str()); + + return acceptable_keys; +} +} // namespace yabloc::graph_segment diff --git a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp b/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp new file mode 100644 index 0000000000000..9d27636f16e9d --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp @@ -0,0 +1,213 @@ +// 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 "yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace yabloc::lanelet2_overlay +{ +Lanelet2Overlay::Lanelet2Overlay() +: Node("lanelet2_overlay"), tf_subscriber_(get_clock()), pose_buffer_{40} +{ + using std::placeholders::_1; + + // Subscriber + auto cb_info = std::bind(&Lanelet2Overlay::on_info, this, _1); + auto cb_image = std::bind(&Lanelet2Overlay::on_image, this, _1); + auto cb_line_segments = std::bind(&Lanelet2Overlay::on_line_segments, this, _1); + auto cb_pose = [this](const PoseStamped & msg) -> void { pose_buffer_.push_back(msg); }; + auto cb_ground = [this](const Float32Array & msg) -> void { ground_plane_.set(msg); }; + + sub_ground_plane_ = create_subscription("~/input/ground", 10, cb_ground); + sub_image_ = create_subscription("~/input/image_raw", 10, cb_image); + sub_pose_ = create_subscription("~/input/pose", 10, cb_pose); + sub_line_segments_cloud_ = + create_subscription("~/input/projected_line_segments_cloud", 10, cb_line_segments); + sub_info_ = create_subscription("~/input/camera_info", 10, cb_info); + sub_sign_board_ = create_subscription( + "~/input/ll2_sign_board", 10, + [this](const PointCloud2 & msg) -> void { pcl::fromROSMsg(msg, sign_board_); }); + sub_ll2_ = create_subscription( + "~/input/ll2_road_marking", 10, + [this](const PointCloud2 & msg) -> void { pcl::fromROSMsg(msg, ll2_cloud_); }); + + // Publisher + pub_vis_ = create_publisher("~/debug/projected_marker", 10); + pub_image_ = create_publisher("~/debug/lanelet2_overlay_image", 10); +} + +void Lanelet2Overlay::on_info(const CameraInfo & msg) +{ + info_ = msg; + camera_extrinsic_ = tf_subscriber_(info_->header.frame_id, "base_link"); +} + +void Lanelet2Overlay::on_image(const sensor_msgs::msg::Image & msg) +{ + cv::Mat image = common::decompress_to_cv_mat(msg); + const rclcpp::Time stamp = msg.header.stamp; + + // Search synchronized pose + float min_abs_dt = std::numeric_limits::max(); + std::optional synched_pose{std::nullopt}; + for (auto pose : pose_buffer_) { + auto dt = (rclcpp::Time(pose.header.stamp) - stamp); + auto abs_dt = std::abs(dt.seconds()); + if (abs_dt < min_abs_dt) { + min_abs_dt = abs_dt; + synched_pose = pose.pose; + } + } + if (min_abs_dt > 0.1) synched_pose = std::nullopt; + RCLCPP_INFO_STREAM(get_logger(), "dt: " << min_abs_dt << " image:" << stamp.nanoseconds()); + + draw_overlay(image, synched_pose, stamp); +} + +void Lanelet2Overlay::on_line_segments(const PointCloud2 & msg) +{ + const rclcpp::Time stamp = msg.header.stamp; + + // Search synchronized pose + float min_dt = std::numeric_limits::max(); + geometry_msgs::msg::PoseStamped synched_pose; + for (auto pose : pose_buffer_) { + auto dt = (rclcpp::Time(pose.header.stamp) - stamp); + auto abs_dt = std::abs(dt.seconds()); + if (abs_dt < min_dt) { + min_dt = abs_dt; + synched_pose = pose; + } + } + if (min_dt > 0.1) return; + auto latest_pose_stamp = rclcpp::Time(pose_buffer_.back().header.stamp); + + std::vector a; + + LineSegments line_segments_cloud; + pcl::fromROSMsg(msg, line_segments_cloud); + make_vis_marker(line_segments_cloud, synched_pose.pose, stamp); +} + +void Lanelet2Overlay::draw_overlay( + const cv::Mat & image, const std::optional & pose, const rclcpp::Time & stamp) +{ + if (ll2_cloud_.empty()) return; + + cv::Mat overlayed_image = cv::Mat::zeros(image.size(), CV_8UC3); + + using common::extract_near_line_segments; + if (pose) { + draw_overlay_line_segments( + overlayed_image, *pose, + extract_near_line_segments(ll2_cloud_, common::pose_to_se3(*pose), 60)); + draw_overlay_line_segments( + overlayed_image, *pose, + extract_near_line_segments(sign_board_, common::pose_to_se3(*pose), 60)); + } + + cv::Mat show_image; + cv::addWeighted(image, 0.8, overlayed_image, 0.8, 1, show_image); + common::publish_image(*pub_image_, show_image, stamp); +} + +void Lanelet2Overlay::draw_overlay_line_segments( + cv::Mat & image, const Pose & pose, const LineSegments & near_segments) +{ + if (!camera_extrinsic_.has_value()) return; + if (!info_.has_value()) return; + + Eigen::Matrix3f K = + Eigen::Map >(info_->k.data()).cast().transpose(); + Eigen::Affine3f T = camera_extrinsic_.value(); + + Eigen::Affine3f transform = ground_plane_.align_with_slope(common::pose_to_affine(pose)); + + auto projectLineSegment = + [K, T, transform]( + const Eigen::Vector3f & p1, + const Eigen::Vector3f & p2) -> std::tuple { + Eigen::Vector3f from_camera1 = K * T.inverse() * transform.inverse() * p1; + Eigen::Vector3f from_camera2 = K * T.inverse() * transform.inverse() * p2; + constexpr float EPSILON = 0.1f; + bool p1_is_visible = from_camera1.z() > EPSILON; + bool p2_is_visible = from_camera2.z() > EPSILON; + if ((!p1_is_visible) && (!p2_is_visible)) return {false, cv::Point2i{}, cv::Point2i{}}; + + Eigen::Vector3f uv1, uv2; + if (p1_is_visible) uv1 = from_camera1 / from_camera1.z(); + if (p2_is_visible) uv2 = from_camera2 / from_camera2.z(); + + if ((p1_is_visible) && (p2_is_visible)) + return {true, cv::Point2i(uv1.x(), uv1.y()), cv::Point2i(uv2.x(), uv2.y())}; + + Eigen::Vector3f tangent = from_camera2 - from_camera1; + float mu = (EPSILON - from_camera1.z()) / (tangent.z()); + if (!p1_is_visible) { + from_camera1 = from_camera1 + mu * tangent; + uv1 = from_camera1 / from_camera1.z(); + } + if (!p2_is_visible) { + from_camera2 = from_camera1 + mu * tangent; + uv2 = from_camera2 / from_camera2.z(); + } + return {true, cv::Point2i(uv1.x(), uv1.y()), cv::Point2i(uv2.x(), uv2.y())}; + }; + + for (const pcl::PointNormal & pn : near_segments) { + auto [success, u1, u2] = projectLineSegment(pn.getVector3fMap(), pn.getNormalVector3fMap()); + if (success) cv::line(image, u1, u2, cv::Scalar(0, 255, 255), 2); + } +} + +void Lanelet2Overlay::make_vis_marker( + const LineSegments & ls, const Pose & pose, const rclcpp::Time & stamp) +{ + Marker marker; + marker.type = Marker::LINE_LIST; + marker.header.frame_id = "map"; + marker.header.stamp = stamp; + marker.pose = pose; + marker.scale.x = 0.1; + marker.color.r = 1.0f; + marker.color.g = 1.0f; + marker.color.b = 0.0f; + marker.color.a = 0.7f; + + for (const auto pn : ls) { + geometry_msgs::msg::Point p1, p2; + p1.x = pn.x; + p1.y = pn.y; + p1.z = pn.z; + p2.x = pn.normal_x; + p2.y = pn.normal_y; + p2.z = pn.normal_z; + marker.points.push_back(p1); + marker.points.push_back(p2); + } + pub_vis_->publish(marker); +} + +} // namespace yabloc::lanelet2_overlay diff --git a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_node.cpp b/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_node.cpp new file mode 100644 index 0000000000000..941ac9d84ab16 --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_node.cpp @@ -0,0 +1,23 @@ +// 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 "yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp new file mode 100644 index 0000000000000..6a127c70bb9a0 --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp @@ -0,0 +1,106 @@ +// 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 "yabloc_image_processing/line_segment_detector/line_segment_detector.hpp" + +#include +#include +#include +#include + +#include + +namespace yabloc::line_segment_detector +{ +LineSegmentDetector::LineSegmentDetector() : Node("line_detector") +{ + using std::placeholders::_1; + + // Subscriber + auto cb_image = std::bind(&LineSegmentDetector::on_image, this, _1); + sub_image_ = create_subscription("~/input/image_raw", 10, cb_image); + + // Publisher + pub_image_with_line_segments_ = create_publisher("~/debug/image_with_line_segments", 10); + pub_cloud_ = create_publisher("~/output/line_segments_cloud", 10); + + line_segment_detector_ = + cv::createLineSegmentDetector(cv::LSD_REFINE_STD, 0.8, 0.6, 2.0, 22.5, 0, 0.7, 1024); +} + +void LineSegmentDetector::on_image(const sensor_msgs::msg::Image & msg) +{ + cv::Mat image = common::decompress_to_cv_mat(msg); + execute(image, msg.header.stamp); +} + +void LineSegmentDetector::execute(const cv::Mat & image, const rclcpp::Time & stamp) +{ + cv::Mat gray_image; + cv::cvtColor(image, gray_image, cv::COLOR_BGR2GRAY); + + cv::Mat lines; + { + tier4_autoware_utils::StopWatch stop_watch; + line_segment_detector_->detect(gray_image, lines); + line_segment_detector_->drawSegments(gray_image, lines); + RCLCPP_INFO_STREAM(this->get_logger(), "lsd: " << stop_watch.toc() << "[ms]"); + } + + common::publish_image(*pub_image_with_line_segments_, gray_image, stamp); + + pcl::PointCloud line_cloud; + std::vector filtered_lines = remove_too_outer_elements(lines, image.size()); + + for (const cv::Mat & xy_xy : filtered_lines) { + Eigen::Vector3f xy1, xy2; + xy1 << xy_xy.at(0), xy_xy.at(1), 0; + xy2 << xy_xy.at(2), xy_xy.at(3), 0; + pcl::PointNormal pn; + pn.getVector3fMap() = xy1; + pn.getNormalVector3fMap() = xy2; + line_cloud.push_back(pn); + } + common::publish_cloud(*pub_cloud_, line_cloud, stamp); +} + +std::vector LineSegmentDetector::remove_too_outer_elements( + const cv::Mat & lines, const cv::Size & size) const +{ + std::vector rect_vector; + rect_vector.emplace_back(0, 0, size.width, 3); + rect_vector.emplace_back(0, size.height - 3, size.width, 3); + rect_vector.emplace_back(0, 0, 3, size.height); + rect_vector.emplace_back(size.width - 3, 0, 3, size.height); + + std::vector filtered_lines; + for (int i = 0; i < lines.rows; i++) { + cv::Mat xy_xy = lines.row(i); + cv::Point2f xy1(xy_xy.at(0), xy_xy.at(1)); + cv::Point2f xy2(xy_xy.at(2), xy_xy.at(3)); + + bool enabled = true; + for (const cv::Rect2i & rect : rect_vector) { + if (rect.contains(xy1) && rect.contains(xy2)) { + enabled = false; + break; + } + } + if (enabled) filtered_lines.push_back(xy_xy); + } + + return filtered_lines; +} + +} // namespace yabloc::line_segment_detector diff --git a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_node.cpp b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_node.cpp new file mode 100644 index 0000000000000..42965ae853ea7 --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_node.cpp @@ -0,0 +1,23 @@ +// 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 "yabloc_image_processing/line_segment_detector/line_segment_detector.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp b/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp new file mode 100644 index 0000000000000..0ee4115a39760 --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp @@ -0,0 +1,92 @@ +// 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 "yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp" + +#include +#include +#include + +#include +#include + +namespace yabloc::line_segments_overlay +{ +LineSegmentsOverlay::LineSegmentsOverlay() +: Node("overlay_lanelet2"), + max_buffer_size_(static_cast(declare_parameter("max_buffer_size", 5))) +{ + using std::placeholders::_1; + + auto cb_image = std::bind(&LineSegmentsOverlay::on_image, this, _1); + auto cb_line_segments_ = std::bind(&LineSegmentsOverlay::on_line_segments, this, _1); + + sub_image_ = create_subscription("~/input/image_raw", 10, cb_image); + sub_line_segments_ = + create_subscription("~/input/line_segments", 10, cb_line_segments_); + + pub_debug_image_ = create_publisher("~/debug/image_with_colored_line_segments", 10); +} + +void LineSegmentsOverlay::on_image(const Image::ConstSharedPtr & img_msg) +{ + rclcpp::Time timestamp(img_msg->header.stamp); + image_buffer_[timestamp] = img_msg; + + if (image_buffer_.size() > max_buffer_size_) { + // The image_buffer is an ordered_map whose key is rclcpp::Time. + // rclcpp::Time has a natural ordering system, so image_buffer.begin() always gives the + // oldest message. + image_buffer_.erase(image_buffer_.begin()); + } +} + +void LineSegmentsOverlay::on_line_segments(const PointCloud2::ConstSharedPtr & line_segments_msg) +{ + const rclcpp::Time stamp = rclcpp::Time(line_segments_msg->header.stamp); + auto iter = image_buffer_.find(stamp); + // If the iterator reaches the end of the image_buffer_, it means the image with the given + // timestamp is not found in the buffer. This assumption is based on the fact that images are + // received and stored in the buffer BEFORE their corresponding line segments are processed. If + // this assumption does not hold, the function may throw a runtime error indicating that the image + // with the given timestamp was not found. + if (iter == image_buffer_.end()) { + // However, the above assumption may be violated just after launch. + RCLCPP_ERROR_STREAM(get_logger(), "Image with the given timestamp not found."); + return; + } + + auto image_ptr = iter->second; + cv::Mat image = cv_bridge::toCvShare(image_ptr, "bgr8")->image; + + LineSegments line_segments_cloud; + pcl::fromROSMsg(*line_segments_msg, line_segments_cloud); + + for (size_t index = 0; index < line_segments_cloud.size(); ++index) { + const LineSegment & pn = line_segments_cloud.at(index); + Eigen::Vector3f xy1 = pn.getVector3fMap(); + Eigen::Vector3f xy2 = pn.getNormalVector3fMap(); + + cv::Scalar color(0, 255, 0); // Green + if (pn.label == 0) { // if unreliable + color = cv::Scalar(0, 0, 255); // Red + } + + cv::line(image, cv::Point(xy1(0), xy1(1)), cv::Point(xy2(0), xy2(1)), color, 2); + } + + common::publish_image(*pub_debug_image_, image, stamp); +} + +} // namespace yabloc::line_segments_overlay diff --git a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_node.cpp b/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_node.cpp new file mode 100644 index 0000000000000..cac6a6a0c5a66 --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_node.cpp @@ -0,0 +1,23 @@ +// 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 "yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp b/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp new file mode 100644 index 0000000000000..d3d313284f311 --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp @@ -0,0 +1,283 @@ +// 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 "yabloc_image_processing/segment_filter/segment_filter.hpp" + +#include +#include +#include +#include + +#include + +namespace yabloc::segment_filter +{ +SegmentFilter::SegmentFilter() +: Node("segment_filter"), + image_size_(declare_parameter("image_size")), + max_range_(declare_parameter("max_range")), + min_segment_length_(declare_parameter("min_segment_length")), + max_segment_distance_(declare_parameter("max_segment_distance")), + max_lateral_distance_(declare_parameter("max_lateral_distance")), + info_(this), + synchro_subscriber_(this, "~/input/line_segments_cloud", "~/input/graph_segmented"), + tf_subscriber_(this->get_clock()) +{ + using std::placeholders::_1; + using std::placeholders::_2; + auto cb = std::bind(&SegmentFilter::execute, this, _1, _2); + synchro_subscriber_.set_callback(std::move(cb)); + + pub_projected_cloud_ = + create_publisher("~/output/projected_line_segments_cloud", 10); + pub_debug_cloud_ = create_publisher("~/debug/debug_line_segments", 10); + pub_image_ = create_publisher("~/debug/projected_image", 10); +} + +cv::Point2i SegmentFilter::to_cv_point(const Eigen::Vector3f & v) const +{ + cv::Point pt; + pt.x = -v.y() / max_range_ * image_size_ * 0.5f + image_size_ / 2; + pt.y = -v.x() / max_range_ * image_size_ * 0.5f + image_size_; + return pt; +} + +bool SegmentFilter::define_project_func() +{ + if (project_func_) return true; + + if (info_.is_camera_info_nullopt()) return false; + Eigen::Matrix3f Kinv = info_.intrinsic().inverse(); + + std::optional camera_extrinsic = + tf_subscriber_(info_.get_frame_id(), "base_link"); + if (!camera_extrinsic.has_value()) return false; + + const Eigen::Vector3f t = camera_extrinsic->translation(); + const Eigen::Quaternionf q(camera_extrinsic->rotation()); + + // TODO(KYabuuchi) This will take into account ground tilt and camera vibration someday. + project_func_ = [Kinv, q, t](const Eigen::Vector3f & u) -> std::optional { + Eigen::Vector3f u3(u.x(), u.y(), 1); + Eigen::Vector3f u_bearing = (q * Kinv * u3).normalized(); + if (u_bearing.z() > -0.01) return std::nullopt; + float u_distance = -t.z() / u_bearing.z(); + Eigen::Vector3f v; + v.x() = t.x() + u_bearing.x() * u_distance; + v.y() = t.y() + u_bearing.y() * u_distance; + v.z() = 0; + return v; + }; + return true; +} + +void SegmentFilter::execute(const PointCloud2 & line_segments_msg, const Image & segment_msg) +{ + if (!define_project_func()) { + using namespace std::literals::chrono_literals; + RCLCPP_INFO_STREAM_THROTTLE( + get_logger(), *get_clock(), (1000ms).count(), "project_func cannot be defined"); + return; + } + + const rclcpp::Time stamp = line_segments_msg.header.stamp; + + pcl::PointCloud::Ptr line_segments_cloud{ + new pcl::PointCloud()}; + cv::Mat mask_image = common::decompress_to_cv_mat(segment_msg); + pcl::fromROSMsg(line_segments_msg, *line_segments_cloud); + + const std::set indices = filter_by_mask(mask_image, *line_segments_cloud); + + pcl::PointCloud valid_edges = project_lines(*line_segments_cloud, indices); + pcl::PointCloud invalid_edges = + project_lines(*line_segments_cloud, indices, true); + + // Projected line segments + { + pcl::PointCloud combined_edges; + for (const auto & pn : valid_edges) { + pcl::PointXYZLNormal pln; + pln.getVector3fMap() = pn.getVector3fMap(); + pln.getNormalVector3fMap() = pn.getNormalVector3fMap(); + pln.label = 255; + combined_edges.push_back(pln); + } + for (const auto & pn : invalid_edges) { + pcl::PointXYZLNormal pln; + pln.getVector3fMap() = pn.getVector3fMap(); + pln.getNormalVector3fMap() = pn.getNormalVector3fMap(); + pln.label = 0; + combined_edges.push_back(pln); + } + common::publish_cloud(*pub_projected_cloud_, combined_edges, stamp); + } + + // Image + { + cv::Mat projected_image = cv::Mat::zeros(cv::Size{image_size_, image_size_}, CV_8UC3); + for (auto & pn : valid_edges) { + cv::Point2i p1 = to_cv_point(pn.getVector3fMap()); + cv::Point2i p2 = to_cv_point(pn.getNormalVector3fMap()); + cv::line(projected_image, p1, p2, cv::Scalar(100, 100, 255), 4, cv::LineTypes::LINE_8); + } + for (auto & pn : invalid_edges) { + cv::Point2i p1 = to_cv_point(pn.getVector3fMap()); + cv::Point2i p2 = to_cv_point(pn.getNormalVector3fMap()); + cv::line(projected_image, p1, p2, cv::Scalar(200, 200, 200), 3, cv::LineTypes::LINE_8); + } + common::publish_image(*pub_image_, projected_image, stamp); + } + + // Line segments for debug + { + pcl::PointCloud combined_debug_edges; + for (size_t index = 0; index < line_segments_cloud->size(); ++index) { + const pcl::PointNormal & pn = line_segments_cloud->at(index); + pcl::PointXYZLNormal pln; + pln.getVector3fMap() = pn.getVector3fMap(); + pln.getNormalVector3fMap() = pn.getNormalVector3fMap(); + if (indices.count(index) > 0) + pln.label = 255; + else + pln.label = 0; + combined_debug_edges.push_back(pln); + } + common::publish_cloud(*pub_debug_cloud_, combined_debug_edges, stamp); + } +} + +bool SegmentFilter::is_near_element( + const pcl::PointNormal & pn, pcl::PointNormal & truncated_pn) const +{ + float min_distance = std::min(pn.x, pn.normal_x); + float max_distance = std::max(pn.x, pn.normal_x); + if (min_distance > max_segment_distance_) return false; + if (max_distance < max_segment_distance_) { + truncated_pn = pn; + return true; + } + + truncated_pn = pn; + Eigen::Vector3f t = pn.getVector3fMap() - pn.getNormalVector3fMap(); + float not_zero_tx = t.x() > 0 ? std::max(t.x(), 1e-3f) : std::min(t.x(), -1e-3f); + float lambda = (max_segment_distance_ - pn.x) / not_zero_tx; + Eigen::Vector3f m = pn.getVector3fMap() + lambda * t; + if (pn.x > pn.normal_x) + truncated_pn.getVector3fMap() = m; + else + truncated_pn.getNormalVector3fMap() = m; + return true; +} + +std::set get_unique_pixel_value(cv::Mat & image) +{ + // `image` is a set of ushort. + // The purpose is to find the unduplicated set of values contained in `image`. + // For example, if `image` is {0,1,2,0,1,2,3}, this function returns {0,1,2,3}. + + if (image.depth() != CV_16U) throw std::runtime_error("image's depth must be ushort"); + + auto begin = image.begin(); + auto last = std::unique(begin, image.end()); + std::sort(begin, last); + last = std::unique(begin, last); + return std::set(begin, last); +} + +pcl::PointCloud SegmentFilter::project_lines( + const pcl::PointCloud & points, const std::set & indices, + bool negative) const +{ + pcl::PointCloud projected_points; + for (size_t index = 0; index < points.size(); ++index) { + if (negative) { + if (indices.count(index) > 0) continue; + } else { + if (indices.count(index) == 0) continue; + } + + pcl::PointNormal truncated_pn = points.at(index); + + std::optional opt1 = project_func_(truncated_pn.getVector3fMap()); + std::optional opt2 = project_func_(truncated_pn.getNormalVector3fMap()); + if (!opt1.has_value()) continue; + if (!opt2.has_value()) continue; + + // If linesegment has shoter length than config, it is excluded + if (min_segment_length_ > 0) { + float length = (opt1.value() - opt2.value()).norm(); + if (length < min_segment_length_) continue; + } + if (max_lateral_distance_ > 0) { + float abs_lateral1 = std::abs(opt1.value().y()); + float abs_lateral2 = std::abs(opt2.value().y()); + if (std::min(abs_lateral1, abs_lateral2) > max_lateral_distance_) continue; + } + + pcl::PointNormal xyz; + xyz.x = opt1->x(); + xyz.y = opt1->y(); + xyz.z = opt1->z(); + xyz.normal_x = opt2->x(); + xyz.normal_y = opt2->y(); + xyz.normal_z = opt2->z(); + + // + pcl::PointNormal truncated_xyz = xyz; + if (max_segment_distance_ > 0) + if (!is_near_element(xyz, truncated_xyz)) continue; + + projected_points.push_back(truncated_xyz); + } + return projected_points; +} + +std::set SegmentFilter::filter_by_mask( + const cv::Mat & mask, const pcl::PointCloud & edges) +{ + // Create line image and assign different color to each segment. + cv::Mat line_image = cv::Mat::zeros(mask.size(), CV_16UC1); + for (size_t i = 0; i < edges.size(); i++) { + auto & pn = edges.at(i); + Eigen::Vector3f p1 = pn.getVector3fMap(); + Eigen::Vector3f p2 = pn.getNormalVector3fMap(); + cv::Scalar color = cv::Scalar::all(i + 1); + cv::line( + line_image, cv::Point2i(p1.x(), p1.y()), cv::Point2i(p2.x(), p2.y()), color, 1, + cv::LineTypes::LINE_4); + } + + cv::Mat mask_image; + mask.convertTo(mask_image, CV_16UC1); + cv::threshold(mask_image, mask_image, 1, std::numeric_limits::max(), cv::THRESH_BINARY); + + // TODO(KYabuuchi) Using boost::geometry is more intuitive. + // https://boostjp.github.io/tips/geometry.html#disjoint + + // And operator + cv::Mat masked_line; + cv::bitwise_and(mask_image, line_image, masked_line); + std::set pixel_values = get_unique_pixel_value(masked_line); + + // Extract edges within masks + std::set reliable_indices; + for (size_t i = 0; i < edges.size(); i++) { + if (pixel_values.count(i + 1) != 0) reliable_indices.insert(i); + } + + return reliable_indices; +} + +} // namespace yabloc::segment_filter diff --git a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_node.cpp b/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_node.cpp new file mode 100644 index 0000000000000..ea51babceee60 --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_node.cpp @@ -0,0 +1,23 @@ +// 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 "yabloc_image_processing/segment_filter/segment_filter.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp new file mode 100644 index 0000000000000..7412db28e1550 --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp @@ -0,0 +1,146 @@ +// 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 +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include + +namespace yabloc::undistort +{ +class UndistortNode : public rclcpp::Node +{ +public: + using CompressedImage = sensor_msgs::msg::CompressedImage; + using CameraInfo = sensor_msgs::msg::CameraInfo; + using Image = sensor_msgs::msg::Image; + + UndistortNode() + : Node("undistort"), + OUTPUT_WIDTH(declare_parameter("width")), + OVERRIDE_FRAME_ID(declare_parameter("override_frame_id")) + { + using std::placeholders::_1; + + rclcpp::QoS qos{10}; + if (declare_parameter("use_sensor_qos")) { + qos = rclcpp::QoS(10).durability_volatile().best_effort(); + } + + auto on_image = std::bind(&UndistortNode::on_image, this, _1); + auto on_info = std::bind(&UndistortNode::on_info, this, _1); + sub_image_ = + create_subscription("~/input/image_raw", qos, std::move(on_image)); + sub_info_ = create_subscription("~/input/camera_info", qos, std::move(on_info)); + + pub_info_ = create_publisher("~/output/resized_info", 10); + pub_image_ = create_publisher("~/output/resized_image", 10); + } + +private: + const int OUTPUT_WIDTH; + const std::string OVERRIDE_FRAME_ID; + + rclcpp::Subscription::SharedPtr sub_image_; + rclcpp::Subscription::SharedPtr sub_info_; + rclcpp::Publisher::SharedPtr pub_image_; + rclcpp::Publisher::SharedPtr pub_info_; + std::optional info_{std::nullopt}; + std::optional scaled_info_{std::nullopt}; + + cv::Mat undistort_map_x, undistort_map_y; + + void make_remap_lut() + { + if (!info_.has_value()) return; + cv::Mat K = cv::Mat(cv::Size(3, 3), CV_64FC1, reinterpret_cast(info_->k.data())); + cv::Mat D = cv::Mat(cv::Size(5, 1), CV_64FC1, reinterpret_cast(info_->d.data())); + cv::Size size(info_->width, info_->height); + + cv::Size new_size = size; + if (OUTPUT_WIDTH > 0) + new_size = cv::Size(OUTPUT_WIDTH, 1.0f * OUTPUT_WIDTH / size.width * size.height); + + cv::Mat new_K = cv::getOptimalNewCameraMatrix(K, D, size, 0, new_size); + + cv::initUndistortRectifyMap( + K, D, cv::Mat(), new_K, new_size, CV_32FC1, undistort_map_x, undistort_map_y); + + scaled_info_ = sensor_msgs::msg::CameraInfo{}; + scaled_info_->k.at(0) = new_K.at(0, 0); + scaled_info_->k.at(2) = new_K.at(0, 2); + scaled_info_->k.at(4) = new_K.at(1, 1); + scaled_info_->k.at(5) = new_K.at(1, 2); + scaled_info_->k.at(8) = 1; + scaled_info_->d.resize(5); + scaled_info_->width = new_size.width; + scaled_info_->height = new_size.height; + } + + void on_image(const CompressedImage & msg) + { + if (!info_.has_value()) return; + if (undistort_map_x.empty()) make_remap_lut(); + + tier4_autoware_utils::StopWatch stop_watch; + cv::Mat image = common::decompress_to_cv_mat(msg); + + cv::Mat undistorted_image; + cv::remap(image, undistorted_image, undistort_map_x, undistort_map_y, cv::INTER_LINEAR); + + // Publish CameraInfo + { + scaled_info_->header = info_->header; + if (OVERRIDE_FRAME_ID != "") scaled_info_->header.frame_id = OVERRIDE_FRAME_ID; + pub_info_->publish(scaled_info_.value()); + } + + // Publish Image + { + cv_bridge::CvImage bridge; + bridge.header.stamp = msg.header.stamp; + if (OVERRIDE_FRAME_ID != "") + bridge.header.frame_id = OVERRIDE_FRAME_ID; + else + bridge.header.frame_id = msg.header.frame_id; + bridge.encoding = "bgr8"; + bridge.image = undistorted_image; + pub_image_->publish(*bridge.toImageMsg()); + } + + RCLCPP_INFO_STREAM(get_logger(), "image undistort: " << stop_watch.toc() << "[ms]"); + } + + void on_info(const CameraInfo & msg) { info_ = msg; } +}; +} // namespace yabloc::undistort + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/localization/yabloc/yabloc_particle_filter/CMakeLists.txt b/localization/yabloc/yabloc_particle_filter/CMakeLists.txt new file mode 100644 index 0000000000000..dbba8777bf8df --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/CMakeLists.txt @@ -0,0 +1,114 @@ +cmake_minimum_required(VERSION 3.5) +project(yabloc_particle_filter) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +# =================================================== +# PCL +find_package(PCL REQUIRED) + +# Sophus +find_package(Sophus REQUIRED) + +# glog +find_package(glog REQUIRED) + +# GeographicLib +find_package(PkgConfig) +find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h + PATH_SUFFIXES GeographicLib +) +set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR}) +find_library(GeographicLib_LIBRARIES NAMES Geographic) + +# =================================================== +# Clear ${PYTHON_EXECUTABLE} defined by mrt_cmake_module so that rosidl_generate_interfaces can look for it properly +unset(PYTHON_EXECUTABLE) +message(STATUS "PYTHON_EXECUTABLE: ${PYTHON_EXECUTABLE}") + +# =================================================== +# Message & Service +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Particle.msg" + "msg/ParticleArray.msg" + DEPENDENCIES + std_msgs + geometry_msgs +) + +# =================================================== +# Libraries +# correction library +ament_auto_add_library( + abstract_corrector + SHARED + src/correction/abstract_corrector.cpp + src/common/visualize.cpp + src/common/mean.cpp +) +target_include_directories(abstract_corrector SYSTEM PRIVATE ${PCL_INCLUDE_DIRS}) +target_link_libraries(abstract_corrector Sophus::Sophus ${PCL_LIBRARIES}) +rosidl_target_interfaces(abstract_corrector ${PROJECT_NAME} "rosidl_typesupport_cpp") + +# Prediction library +ament_auto_add_library(predictor + src/prediction/predictor.cpp + src/prediction/resampler.cpp + src/common/visualize.cpp + src/common/mean.cpp +) +target_include_directories(predictor SYSTEM PRIVATE ${PCL_INCLUDE_DIRS}) +target_link_libraries(predictor Sophus::Sophus ${PCL_LIBRARIES}) +rosidl_target_interfaces(predictor ${PROJECT_NAME} "rosidl_typesupport_cpp") + +# Cost map Library +ament_auto_add_library(ll2_cost_map + SHARED + src/ll2_cost_map/hierarchical_cost_map.cpp + src/ll2_cost_map/direct_cost_map.cpp +) +target_include_directories(ll2_cost_map PUBLIC include) +target_include_directories(ll2_cost_map SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) +target_link_libraries(ll2_cost_map ${PCL_LIBRARIES}) + +# =================================================== +# Executables +# Predictor +ament_auto_add_executable(predictor_node + src/prediction/predictor_node.cpp +) +target_link_libraries(predictor_node predictor) + +# Visualizer +ament_auto_add_executable(particle_visualize + src/common/particle_visualize_node.cpp +) + +# GNSS corrector +set(TARGET gnss_particle_corrector_node) +ament_auto_add_executable(${TARGET} + src/gnss_corrector/gnss_corrector_node.cpp + src/gnss_corrector/gnss_corrector_core.cpp) +target_include_directories(${TARGET} PUBLIC include) +target_include_directories(${TARGET} PUBLIC SYSTEM ${GeographicLib_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS}) +target_link_libraries(${TARGET} Sophus::Sophus) + +# Camera corrector +set(TARGET camera_particle_corrector_node) +ament_auto_add_executable(${TARGET} + src/camera_corrector/camera_particle_corrector_node.cpp + src/camera_corrector/filt_line_segments.cpp + src/camera_corrector/logit.cpp + src/camera_corrector/camera_particle_corrector_core.cpp) +target_include_directories(${TARGET} PUBLIC include) +target_include_directories(${TARGET} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) +target_link_libraries(${TARGET} Sophus::Sophus ${PCL_LIBRARIES} glog::glog) + +# =================================================== +# TEST +if(BUILD_TESTING) + add_subdirectory(test) +endif() + +ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/localization/yabloc/yabloc_particle_filter/README.md b/localization/yabloc/yabloc_particle_filter/README.md new file mode 100644 index 0000000000000..d4cea019a8152 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/README.md @@ -0,0 +1,139 @@ +# yabLoc_particle_filter + +This package contains some executable nodes related to particle filter. + +- [particle_predictor](#particle_predictor) +- [gnss_particle_corrector](#gnss_particle_corrector) +- [camera_particle_corrector](#camera_particle_corrector) + +## particle_predictor + +### Purpose + +- This node performs predictive updating and resampling of particles. +- It retroactively reflects the particle weights determined by the corrector node. + +### Inputs / Outputs + +#### Input + +| Name | Type | Description | +| ----------------------------- | ------------------------------------------------ | --------------------------------------------------------- | +| `input/initialpose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | to specity the initial position of particles | +| `input/twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | linear velocity and angular velocity of prediction update | +| `input/height` | `std_msgs::msg::Float32` | ground height | +| `input/weighted_particles` | `yabloc_particle_filter::msg::ParticleArray` | particles weighted by corrector nodes | + +#### Output + +| Name | Type | Description | +| ------------------------------ | ----------------------------------------------- | --------------------------------------------------------- | +| `output/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | particle centroid with covariance | +| `output/pose` | `geometry_msgs::msg::PoseStamped` | particle centroid with covariance | +| `output/predicted_particles` | `yabloc_particle_filter::msg::ParticleArray` | particles weighted by predictor nodes | +| `debug/init_marker` | `visualization_msgs::msg::Marker` | debug visualization of initial position | +| `debug/particles_marker_array` | `visualization_msgs::msg::MarkerArray` | particles visualization. published if `visualize` is true | + +### Parameters + +| Name | Type | Description | +| ----------------------------- | ---------------- | ----------------------------------------------------------------- | +| `visualize` | bool | whether particles are also published in visualization_msgs or not | +| `static_linear_covariance` | double | to override the covariance of `/twist_with_covariance` | +| `static_angular_covariance` | double | to override the covariance of `/twist_with_covariance` | +| `resampling_interval_seconds` | double | the interval of particle resamping | +| `num_of_particles` | int | the number of particles | +| `prediction_rate` | double | frequency of forecast updates, in Hz | +| `cov_xx_yy` | vector\ | the covariance of initial pose | + +## gnss_particle_corrector + +### Purpose + +- This node estimated particles weight using GNSS. +- It supports two types of input: `ublox_msgs::msg::NavPVT` and `geometry_msgs::msg::PoseWithCovarianceStamped`. + +### Inputs / Outputs + +#### Input + +| Name | Type | Description | +| ---------------------------- | ----------------------------------------------- | -------------------------------------------------- | +| `input/height` | `std_msgs::msg::Float32` | ground height | +| `input/predicted_particles` | `yabloc_particle_filter::msg::ParticleArray` | predicted particles | +| `input/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | gnss measurement. used if `use_ublox_msg` is false | +| `input/navpvt` | `ublox_msgs::msg::NavPVT` | gnss measurement. used if `use_ublox_msg` is true | + +#### Output + +| Name | Type | Description | +| ------------------------------ | -------------------------------------------- | --------------------------------------------------------- | +| `output/weighted_particles` | `yabloc_particle_filter::msg::ParticleArray` | weighted particles | +| `debug/gnss_range_marker` | `visualization_msgs::msg::MarkerArray` | gnss weight distribution | +| `debug/particles_marker_array` | `visualization_msgs::msg::MarkerArray` | particles visualization. published if `visualize` is true | + +### Parameters + +| Name | Type | Description | +| -------------------------------- | ------ | ---------------------------------------------------------------------------------------- | +| `acceptable_max_delay` | double | how long to hold the predicted particles | +| `visualize` | double | whether publish particles as marker_array or not | +| `mahalanobis_distance_threshold` | double | if the Mahalanobis distance to the GNSS for particle exceeds this, the correction skips. | +| `for_fixed/max_weight` | bool | parameter for gnss weight distribution | +| `for_fixed/flat_radius` | bool | parameter for gnss weight distribution | +| `for_fixed/max_radius` | bool | parameter for gnss weight distribution | +| `for_fixed/min_weight` | bool | parameter for gnss weight distribution | +| `for_not_fixed/flat_radius` | bool | parameter for gnss weight distribution | +| `for_not_fixed/max_radius` | bool | parameter for gnss weight distribution | +| `for_not_fixed/min_weight` | bool | parameter for gnss weight distribution | +| `for_not_fixed/max_weight` | bool | parameter for gnss weight distribution | + +## camera_particle_corrector + +### Purpose + +- This node estimated particles weight using GNSS. + +### Inputs / Outputs + +#### Input + +| Name | Type | Description | +| ------------------------------------- | -------------------------------------------- | ----------------------------------------------------------- | +| `input/predicted_particles` | `yabloc_particle_filter::msg::ParticleArray` | predicted particles | +| `input/ll2_bounding_box` | `sensor_msgs::msg::PointCloud2` | road surface markings converted to line segments | +| `input/ll2_road_marking` | `sensor_msgs::msg::PointCloud2` | road surface markings converted to line segments | +| `input/projected_line_segments_cloud` | `sensor_msgs::msg::PointCloud2` | projected line segments | +| `input/pose` | `geometry_msgs::msg::PoseStamped` | reference to retrieve the area map around the self location | + +#### Output + +| Name | Type | Description | +| ------------------------------ | -------------------------------------------- | --------------------------------------------------------- | +| `output/weighted_particles` | `yabloc_particle_filter::msg::ParticleArray` | weighted particles | +| `debug/cost_map_image` | `sensor_msgs::msg::Image` | cost map created from lanelet2 | +| `debug/cost_map_range` | `visualization_msgs::msg::MarkerArray` | cost map boundary | +| `debug/match_image` | `sensor_msgs::msg::Image` | projected line segments image | +| `debug/scored_cloud` | `sensor_msgs::msg::PointCloud2` | weighted 3d line segments | +| `debug/scored_post_cloud` | `sensor_msgs::msg::PointCloud2` | weighted 3d line segments which are iffy | +| `debug/state_string` | `std_msgs::msg::String` | string describing the node state | +| `debug/particles_marker_array` | `visualization_msgs::msg::MarkerArray` | particles visualization. published if `visualize` is true | + +### Parameters + +| Name | Type | Description | +| ---------------------- | ------ | ----------------------------------------------------------------------------------------------------------------------------------------- | +| `acceptable_max_delay` | double | how long to hold the predicted particles | +| `visualize` | double | whether publish particles as marker_array or not | +| `image_size` | int | image size of debug/cost_map_image | +| `max_range` | double | width of heararchical cost map | +| `gamma` | double | gamma value of the intensity gradient of the cost map | +| `min_prob` | double | minimum particle weight the corrector node gives | +| `far_weight_gain` | double | `exp(-far_weight_gain_ * squared_distance_from_camera)` is weight gain. if this is large, the nearby road markings will be more important | +| `enabled_at_first` | bool | if it is false, this node is not activated at first. you can activate by service call | + +### Services + +| Name | Type | Description | +| ------------ | ------------------------ | ----------------------------------------- | +| `switch_srv` | `std_srvs::srv::SetBool` | activation and deactivation of correction | diff --git a/localization/yabloc/yabloc_particle_filter/config/camera_particle_corrector.param.yaml b/localization/yabloc/yabloc_particle_filter/config/camera_particle_corrector.param.yaml new file mode 100644 index 0000000000000..17c5a604bf977 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/config/camera_particle_corrector.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + # Declared in AbstractParticleCorrector + acceptable_max_delay: 1.0 + visualize: false + + # Declared in ll2_cost_map + image_size: 800 # cost map image made by lanelet2 + max_range: 40.0 # [m] a cost map scale size + gamma: 5.0 # cost map intensity gradient + + min_prob: 0.1 # minimum weight of particles + far_weight_gain: 0.001 # exp(-far_weight_gain_ * squared_norm) is multiplied each measurement + enabled_at_first: true # developing feature diff --git a/localization/yabloc/yabloc_particle_filter/config/gnss_particle_corrector.param.yaml b/localization/yabloc/yabloc_particle_filter/config/gnss_particle_corrector.param.yaml new file mode 100644 index 0000000000000..32267ce6f15df --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/config/gnss_particle_corrector.param.yaml @@ -0,0 +1,17 @@ +/**: + ros__parameters: + # Declared in AbstractParticleCorrector + acceptable_max_delay: 1.0 + visualize: false + + mahalanobis_distance_threshold: 30.0 # If the distance to GNSS observation exceeds this, the correction is skipped. + # + for_fixed/max_weight: 5.0 + for_fixed/flat_radius: 0.5 + for_fixed/max_radius: 10.0 + for_fixed/min_weight: 0.5 + # + for_not_fixed/flat_radius: 5.0 + for_not_fixed/max_radius: 20.0 + for_not_fixed/min_weight: 0.5 + for_not_fixed/max_weight: 1.0 diff --git a/localization/yabloc/yabloc_particle_filter/config/predictor.param.yaml b/localization/yabloc/yabloc_particle_filter/config/predictor.param.yaml new file mode 100644 index 0000000000000..c7e7b62d1403d --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/config/predictor.param.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + visualize: true + + static_linear_covariance: 0.04 + static_angular_covariance: 0.006 + + resampling_interval_seconds: 1.0 + num_of_particles: 500 + prediction_rate: 50.0 + + cov_xx_yy: [2.0,0.25] diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp new file mode 100644 index 0000000000000..59a163a8cc0a1 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp @@ -0,0 +1,98 @@ +// 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 YABLOC_PARTICLE_FILTER__CAMERA_CORRECTOR__CAMERA_PARTICLE_CORRECTOR_HPP_ +#define YABLOC_PARTICLE_FILTER__CAMERA_CORRECTOR__CAMERA_PARTICLE_CORRECTOR_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +namespace yabloc::modularized_particle_filter +{ +cv::Point2f cv2pt(const Eigen::Vector3f v); +float abs_cos(const Eigen::Vector3f & t, float deg); + +class CameraParticleCorrector : public modularized_particle_filter::AbstractCorrector +{ +public: + using LineSegment = pcl::PointXYZLNormal; + using LineSegments = pcl::PointCloud; + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using PoseStamped = geometry_msgs::msg::PoseStamped; + using Image = sensor_msgs::msg::Image; + using MarkerArray = visualization_msgs::msg::MarkerArray; + using Pose = geometry_msgs::msg::Pose; + using Bool = std_msgs::msg::Bool; + using String = std_msgs::msg::String; + using SetBool = std_srvs::srv::SetBool; + CameraParticleCorrector(); + +private: + const float min_prob_; + const float far_weight_gain_; + HierarchicalCostMap cost_map_; + + rclcpp::Subscription::SharedPtr sub_bounding_box_; + rclcpp::Subscription::SharedPtr sub_line_segments_cloud_; + rclcpp::Subscription::SharedPtr sub_ll2_; + rclcpp::Subscription::SharedPtr sub_pose_; + rclcpp::Service::SharedPtr switch_service_; + rclcpp::TimerBase::SharedPtr timer_; + + rclcpp::Publisher::SharedPtr pub_image_; + rclcpp::Publisher::SharedPtr pub_map_image_; + rclcpp::Publisher::SharedPtr pub_marker_; + rclcpp::Publisher::SharedPtr pub_scored_cloud_; + rclcpp::Publisher::SharedPtr pub_scored_posteriori_cloud_; + rclcpp::Publisher::SharedPtr pub_string_; + + Eigen::Vector3f last_mean_position_; + std::optional latest_pose_{std::nullopt}; + std::function score_converter_; + + bool enable_switch_{true}; + + void on_line_segments(const PointCloud2 & msg); + void on_ll2(const PointCloud2 & msg); + void on_bounding_box(const PointCloud2 & msg); + void on_pose(const PoseStamped & msg); + void on_timer(); + void on_service(SetBool::Request::ConstSharedPtr request, SetBool::Response::SharedPtr response); + + std::pair split_line_segments(const PointCloud2 & msg); + + float compute_logit( + const LineSegments & line_segments_cloud, const Eigen::Vector3f & self_position); + + pcl::PointCloud evaluate_cloud( + const LineSegments & line_segments_cloud, const Eigen::Vector3f & self_position); + + std::pair filt(const LineSegments & lines); +}; +} // namespace yabloc::modularized_particle_filter + +#endif // YABLOC_PARTICLE_FILTER__CAMERA_CORRECTOR__CAMERA_PARTICLE_CORRECTOR_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/logit.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/logit.hpp new file mode 100644 index 0000000000000..09cf3aad36951 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/logit.hpp @@ -0,0 +1,32 @@ +// 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 YABLOC_PARTICLE_FILTER__CAMERA_CORRECTOR__LOGIT_HPP_ +#define YABLOC_PARTICLE_FILTER__CAMERA_CORRECTOR__LOGIT_HPP_ + +namespace yabloc +{ +float logit_to_prob(float logit, float gain = 1.0f); + +/** + * Convert probability to logit + * This function is much faster than logit_to_prob() because it refers to pre-computeed table + * + * @param[in] prob + * @return logit + */ +float prob_to_logit(float prob); +} // namespace yabloc + +#endif // YABLOC_PARTICLE_FILTER__CAMERA_CORRECTOR__LOGIT_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/mean.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/mean.hpp new file mode 100644 index 0000000000000..82c4694c3b92f --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/mean.hpp @@ -0,0 +1,37 @@ +// 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 YABLOC_PARTICLE_FILTER__COMMON__MEAN_HPP_ +#define YABLOC_PARTICLE_FILTER__COMMON__MEAN_HPP_ + +#include +#include + +#include + +namespace yabloc +{ +namespace modularized_particle_filter +{ +geometry_msgs::msg::Pose mean_pose( + const yabloc_particle_filter::msg::ParticleArray & particle_array); + +Eigen::Matrix3f std_of_distribution( + const yabloc_particle_filter::msg::ParticleArray & particle_array); + +float std_of_weight(const yabloc_particle_filter::msg::ParticleArray & particle_array); +} // namespace modularized_particle_filter +} // namespace yabloc + +#endif // YABLOC_PARTICLE_FILTER__COMMON__MEAN_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/prediction_util.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/prediction_util.hpp new file mode 100644 index 0000000000000..6059f16826c98 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/prediction_util.hpp @@ -0,0 +1,68 @@ +// 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 YABLOC_PARTICLE_FILTER__COMMON__PREDICTION_UTIL_HPP_ +#define YABLOC_PARTICLE_FILTER__COMMON__PREDICTION_UTIL_HPP_ + +#include + +#include +#include +#include +#include + +namespace yabloc +{ +namespace modularized_particle_filter::util +{ +std::random_device seed_gen; +std::default_random_engine engine(seed_gen()); + +Eigen::Vector2d nrand_2d(const Eigen::Matrix2d cov) +{ + Eigen::JacobiSVD svd; + svd.compute(cov, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::Vector2d std = svd.singularValues(); + std = std.cwiseMax(0.01); + + std::normal_distribution<> dist(0.0, 1.0); + Eigen::Vector2d xy; + xy.x() = std::sqrt(std.x()) * dist(engine); + xy.y() = std::sqrt(std.y()) * dist(engine); + return svd.matrixU() * xy; +} + +template +T nrand(T std) +{ + std::normal_distribution dist(0.0, std); + return dist(engine); +} + +double normalize_radian(const double rad, const double min_rad = -M_PI) +{ + const auto max_rad = min_rad + 2 * M_PI; + + const auto value = std::fmod(rad, 2 * M_PI); + + if (min_rad <= value && value <= max_rad) { + return value; + } + + return value - std::copysign(2 * M_PI, value); +} + +} // namespace modularized_particle_filter::util +} // namespace yabloc +#endif // YABLOC_PARTICLE_FILTER__COMMON__PREDICTION_UTIL_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/visualize.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/visualize.hpp new file mode 100644 index 0000000000000..f1ecf35e81761 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/visualize.hpp @@ -0,0 +1,45 @@ +// 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 YABLOC_PARTICLE_FILTER__COMMON__VISUALIZE_HPP_ +#define YABLOC_PARTICLE_FILTER__COMMON__VISUALIZE_HPP_ + +#include +#include + +#include +#include + +namespace yabloc +{ +namespace modularized_particle_filter +{ +class ParticleVisualizer +{ +public: + using MarkerArray = visualization_msgs::msg::MarkerArray; + using Particle = yabloc_particle_filter::msg::Particle; + using ParticleArray = yabloc_particle_filter::msg::ParticleArray; + + explicit ParticleVisualizer(rclcpp::Node & node); + void publish(const ParticleArray & msg); + +private: + rclcpp::Publisher::SharedPtr pub_marker_array_; + std_msgs::msg::ColorRGBA compute_color(float value); +}; +} // namespace modularized_particle_filter +} // namespace yabloc + +#endif // YABLOC_PARTICLE_FILTER__COMMON__VISUALIZE_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/abstract_corrector.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/abstract_corrector.hpp new file mode 100644 index 0000000000000..31363742576fd --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/abstract_corrector.hpp @@ -0,0 +1,61 @@ +// 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 YABLOC_PARTICLE_FILTER__CORRECTION__ABSTRACT_CORRECTOR_HPP_ +#define YABLOC_PARTICLE_FILTER__CORRECTION__ABSTRACT_CORRECTOR_HPP_ + +#include "yabloc_particle_filter/common/mean.hpp" +#include "yabloc_particle_filter/common/visualize.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace yabloc +{ +namespace modularized_particle_filter +{ +class AbstractCorrector : public rclcpp::Node +{ +public: + using Particle = yabloc_particle_filter::msg::Particle; + using ParticleArray = yabloc_particle_filter::msg::ParticleArray; + + explicit AbstractCorrector(const std::string & node_name); + +protected: + const float acceptable_max_delay_; // [sec] + const bool visualize_; + const rclcpp::Logger logger_; + + rclcpp::Subscription::SharedPtr particle_sub_; + rclcpp::Publisher::SharedPtr particle_pub_; + std::list particle_array_buffer_; + + std::optional get_synchronized_particle_array(const rclcpp::Time & stamp); + std::shared_ptr visualizer_; + + void set_weighted_particle_array(const ParticleArray & particle_array); + +private: + void on_particle_array(const ParticleArray & particle_array); +}; +} // namespace modularized_particle_filter +} // namespace yabloc + +#endif // YABLOC_PARTICLE_FILTER__CORRECTION__ABSTRACT_CORRECTOR_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/correction_util.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/correction_util.hpp new file mode 100644 index 0000000000000..2c2f10bf7ad2e --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/correction_util.hpp @@ -0,0 +1,35 @@ +// 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 YABLOC_PARTICLE_FILTER__CORRECTION__CORRECTION_UTIL_HPP_ +#define YABLOC_PARTICLE_FILTER__CORRECTION__CORRECTION_UTIL_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "yabloc_particle_filter/msg/particle_array.hpp" + +#include + +#include + +namespace yabloc +{ +namespace modularized_particle_filter +{ +std::optional find_synced_particles( + boost::circular_buffer circular_buffer, + rclcpp::Time time); +} +} // namespace yabloc + +#endif // YABLOC_PARTICLE_FILTER__CORRECTION__CORRECTION_UTIL_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector/gnss_particle_corrector.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector/gnss_particle_corrector.hpp new file mode 100644 index 0000000000000..3b19ab1633656 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector/gnss_particle_corrector.hpp @@ -0,0 +1,74 @@ +// 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 YABLOC_PARTICLE_FILTER__GNSS_CORRECTOR__GNSS_PARTICLE_CORRECTOR_HPP_ +#define YABLOC_PARTICLE_FILTER__GNSS_CORRECTOR__GNSS_PARTICLE_CORRECTOR_HPP_ + +#include "yabloc_particle_filter/gnss_corrector/weight_manager.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +namespace yabloc::modularized_particle_filter +{ +class GnssParticleCorrector : public AbstractCorrector +{ +public: + using PoseStamped = geometry_msgs::msg::PoseStamped; + using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + using NavSatFix = sensor_msgs::msg::NavSatFix; + using Marker = visualization_msgs::msg::Marker; + using MarkerArray = visualization_msgs::msg::MarkerArray; + using Float32 = std_msgs::msg::Float32; + + GnssParticleCorrector(); + +private: + const float mahalanobis_distance_threshold_; + const WeightManager weight_manager_; + + rclcpp::Subscription::SharedPtr height_sub_; + rclcpp::Subscription::SharedPtr pose_sub_; + rclcpp::Publisher::SharedPtr marker_pub_; + + Float32 latest_height_; + Eigen::Vector3f last_mean_position_; + + void on_pose(const PoseCovStamped::ConstSharedPtr pose_msg); + + void process( + const Eigen::Vector3f & gnss_position, const rclcpp::Time & stamp, const bool is_rtk_fixed); + + bool is_gnss_observation_valid( + const Eigen::Matrix3f & sigma, const Eigen::Vector3f & meaned_position, + const Eigen::Vector3f & gnss_position); + + ParticleArray weight_particles( + const ParticleArray & predicted_particles, const Eigen::Vector3f & pose, bool is_rtk_fixed); + + // unstable feature + void add_weight_by_orientation( + ParticleArray & weighted_particles, const Eigen::Vector3f & velocity); + + void publish_marker(const Eigen::Vector3f & position, bool fixed); +}; +} // namespace yabloc::modularized_particle_filter + +#endif // YABLOC_PARTICLE_FILTER__GNSS_CORRECTOR__GNSS_PARTICLE_CORRECTOR_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector/weight_manager.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector/weight_manager.hpp new file mode 100644 index 0000000000000..791f65eb9b835 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector/weight_manager.hpp @@ -0,0 +1,92 @@ +// 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 YABLOC_PARTICLE_FILTER__GNSS_CORRECTOR__WEIGHT_MANAGER_HPP_ +#define YABLOC_PARTICLE_FILTER__GNSS_CORRECTOR__WEIGHT_MANAGER_HPP_ + +#include + +namespace yabloc::modularized_particle_filter +{ +struct WeightManager +{ + struct Parameter + { + float flat_radius_; + float max_radius_; + float max_weight_; + float min_weight_; + + float coeff_; + void compute_coeff() + { + coeff_ = -std::log(min_weight_ / max_weight_) / (max_radius_ * max_radius_); + } + }; + + Parameter for_fixed_; + Parameter for_not_fixed_; + + explicit WeightManager(rclcpp::Node * node) + { + for_fixed_.flat_radius_ = node->declare_parameter("for_fixed/flat_radius"); + for_fixed_.max_radius_ = node->declare_parameter("for_fixed/max_radius"); + for_fixed_.min_weight_ = node->declare_parameter("for_fixed/min_weight"); + for_fixed_.max_weight_ = node->declare_parameter("for_fixed/max_weight"); + for_fixed_.compute_coeff(); + + for_not_fixed_.flat_radius_ = node->declare_parameter("for_not_fixed/flat_radius"); + for_not_fixed_.max_radius_ = node->declare_parameter("for_not_fixed/max_radius"); + for_not_fixed_.min_weight_ = node->declare_parameter("for_not_fixed/min_weight"); + for_not_fixed_.max_weight_ = node->declare_parameter("for_not_fixed/max_weight"); + for_not_fixed_.compute_coeff(); + } + + float normal_pdf(float distance, const Parameter & param) const + { + // NOTE: This is not exact normal distribution because of no scale factor depending on sigma + float d = std::clamp(std::abs(distance) - param.flat_radius_, 0.f, param.max_radius_); + return param.max_weight_ * std::exp(-param.coeff_ * d * d); + } + + float normal_pdf(float distance, bool is_rtk_fixed) const + { + if (is_rtk_fixed) { + return normal_pdf(distance, for_fixed_); + } else { + return normal_pdf(distance, for_not_fixed_); + } + } + + float inverse_normal_pdf(float prob, const Parameter & param) const + { + prob = (param.max_weight_ - param.min_weight_) * prob + param.min_weight_; + + if (prob > param.max_radius_) return param.max_radius_; + if (prob < param.min_weight_) return param.flat_radius_ + param.max_radius_; + return param.flat_radius_ + std::sqrt(-std::log(prob / param.max_weight_) / param.coeff_); + } + + float inverse_normal_pdf(float prob, bool is_rtk_fixed) const + { + if (is_rtk_fixed) { + return inverse_normal_pdf(prob, for_fixed_); + } else { + return inverse_normal_pdf(prob, for_not_fixed_); + } + } +}; +} // namespace yabloc::modularized_particle_filter + +#endif // YABLOC_PARTICLE_FILTER__GNSS_CORRECTOR__WEIGHT_MANAGER_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/direct_cost_map.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/direct_cost_map.hpp new file mode 100644 index 0000000000000..5f56b8f079cd1 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/direct_cost_map.hpp @@ -0,0 +1,28 @@ +// 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 YABLOC_PARTICLE_FILTER__LL2_COST_MAP__DIRECT_COST_MAP_HPP_ +#define YABLOC_PARTICLE_FILTER__LL2_COST_MAP__DIRECT_COST_MAP_HPP_ + +#include + +namespace yabloc +{ +cv::Mat direct_cost_map(const cv::Mat & cost_map, const cv::Mat & intensity); + +cv::Mat visualize_direction_map(const cv::Mat & cost_map); + +} // namespace yabloc + +#endif // YABLOC_PARTICLE_FILTER__LL2_COST_MAP__DIRECT_COST_MAP_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/hierarchical_cost_map.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/hierarchical_cost_map.hpp new file mode 100644 index 0000000000000..32d579c4ab595 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/hierarchical_cost_map.hpp @@ -0,0 +1,148 @@ +// 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 YABLOC_PARTICLE_FILTER__LL2_COST_MAP__HIERARCHICAL_COST_MAP_HPP_ +#define YABLOC_PARTICLE_FILTER__LL2_COST_MAP__HIERARCHICAL_COST_MAP_HPP_ + +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace yabloc +{ +struct Area +{ + Area() {} + explicit Area(const Eigen::Vector2f & v) + { + if (unit_length_ < 0) { + throw_error(); + } + x = static_cast(std::floor(v.x() / unit_length_)); + y = static_cast(std::floor(v.y() / unit_length_)); + } + + Eigen::Vector2f real_scale() const { return {x * unit_length_, y * unit_length_}; } + + std::array real_scale_boundary() const + { + std::array boundary; + boundary.at(0) = real_scale(); + boundary.at(1) = real_scale() + Eigen::Vector2f(unit_length_, unit_length_); + return boundary; + }; + + void throw_error() const + { + std::cerr << "Area::unit_length_ is not initialized" << std::endl; + throw std::runtime_error("invalid Area::unit_length"); + } + int x, y; + static float unit_length_; + static float image_size_; + + friend bool operator==(const Area & one, const Area & other) + { + return one.x == other.x && one.y == other.y; + } + friend bool operator!=(const Area & one, const Area & other) { return !(one == other); } + size_t operator()(const Area & index) const + { + std::size_t seed = 0; + boost::hash_combine(seed, index.x); + boost::hash_combine(seed, index.y); + return seed; + } +}; + +struct CostMapValue +{ + CostMapValue(float intensity, int angle, bool unmapped) + : intensity(intensity), angle(angle), unmapped(unmapped) + { + } + float intensity; // 0~1 + int angle; // 0~180 + bool unmapped; // true/false +}; + +class HierarchicalCostMap +{ +public: + using Marker = visualization_msgs::msg::Marker; + using MarkerArray = visualization_msgs::msg::MarkerArray; + using Pose = geometry_msgs::msg::Pose; + + using BgPoint = boost::geometry::model::d2::point_xy; + using BgPolygon = boost::geometry::model::polygon; + + explicit HierarchicalCostMap(rclcpp::Node * node); + + void set_cloud(const pcl::PointCloud & cloud); + void set_bounding_box(const pcl::PointCloud & cloud); + + /** + * Get pixel value at specified pixel + * + * @param[in] position Real scale position at world frame + * @return The combination of intensity (0-1), angle (0-180), unmapped flag (0, 1) + */ + CostMapValue at(const Eigen::Vector2f & position); + + MarkerArray show_map_range() const; + + cv::Mat get_map_image(const Pose & pose); + + void erase_obsolete(); + + void set_height(float height); + +private: + const float max_range_; + const float image_size_; + const size_t max_map_count_; + rclcpp::Logger logger_; + std::optional height_{std::nullopt}; + + common::GammaConverter gamma_converter{4.0f}; + + std::unordered_map map_accessed_; + + std::list generated_map_history_; + std::optional> cloud_; + std::vector bounding_boxes_; + std::unordered_map cost_maps_; + + cv::Point to_cv_point(const Area & are, const Eigen::Vector2f) const; + void build_map(const Area & area); + + cv::Mat create_available_area_image(const Area & area) const; +}; +} // namespace yabloc + +#endif // YABLOC_PARTICLE_FILTER__LL2_COST_MAP__HIERARCHICAL_COST_MAP_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp new file mode 100644 index 0000000000000..c7794a2be9f51 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp @@ -0,0 +1,110 @@ +// 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 YABLOC_PARTICLE_FILTER__PREDICTION__PREDICTOR_HPP_ +#define YABLOC_PARTICLE_FILTER__PREDICTION__PREDICTOR_HPP_ + +#include "yabloc_particle_filter/common/visualize.hpp" +#include "yabloc_particle_filter/prediction/resampler.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace yabloc::modularized_particle_filter +{ +class Predictor : public rclcpp::Node +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + using ParticleArray = yabloc_particle_filter::msg::ParticleArray; + using PoseStamped = geometry_msgs::msg::PoseStamped; + using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + using TwistCovStamped = geometry_msgs::msg::TwistWithCovarianceStamped; + using TwistStamped = geometry_msgs::msg::TwistStamped; + using Marker = visualization_msgs::msg::Marker; + + Predictor(); + +private: + // The number of particles of particle filter + const int number_of_particles_; + // The minimum resampling interval + const float resampling_interval_seconds_; + // Const value for X linear velocity covariance + const float static_linear_covariance_; + // Const value for Z angular velocity covariance + const float static_angular_covariance_; + // Const value for initial pose covariance + const std::vector cov_xx_yy_; + + // Subscriber + rclcpp::Subscription::SharedPtr initialpose_sub_; + rclcpp::Subscription::SharedPtr twist_cov_sub_; + rclcpp::Subscription::SharedPtr particles_sub_; + rclcpp::Subscription::SharedPtr height_sub_; + + // Publisher + rclcpp::Publisher::SharedPtr predicted_particles_pub_; + rclcpp::Publisher::SharedPtr pose_pub_; + rclcpp::Publisher::SharedPtr pose_cov_pub_; + rclcpp::Publisher::SharedPtr marker_pub_; + std::unique_ptr tf2_broadcaster_; + + // Timer callback + rclcpp::TimerBase::SharedPtr timer_; + + float ground_height_{0}; + + std::optional particle_array_opt_{std::nullopt}; + std::optional latest_twist_opt_{std::nullopt}; + std::optional previous_resampling_time_opt_{std::nullopt}; + + // + std::unique_ptr visualizer_ptr_{nullptr}; + std::unique_ptr resampler_ptr_{nullptr}; + + // Callback + void on_initial_pose(const PoseCovStamped::ConstSharedPtr initialpose); + void on_twist_cov(const TwistCovStamped::ConstSharedPtr twist); + void on_weighted_particles(const ParticleArray::ConstSharedPtr weighted_particles); + void on_timer(); + + // + void initialize_particles(const PoseCovStamped & initialpose); + // + void update_with_dynamic_noise( + ParticleArray & particle_array, const TwistCovStamped & twist, double dt); + // + void publish_mean_pose(const geometry_msgs::msg::Pose & mean_pose, const rclcpp::Time & stamp); + void publish_range_marker(const Eigen::Vector3f & pos, const Eigen::Vector3f & tangent); + PoseCovStamped rectify_initial_pose( + const Eigen::Vector3f & pos, const Eigen::Vector3f & tangent, + const PoseCovStamped & raw_initialpose) const; +}; + +} // namespace yabloc::modularized_particle_filter +#endif // YABLOC_PARTICLE_FILTER__PREDICTION__PREDICTOR_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampler.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampler.hpp new file mode 100644 index 0000000000000..f2a517216020a --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampler.hpp @@ -0,0 +1,65 @@ +// 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 YABLOC_PARTICLE_FILTER__PREDICTION__RESAMPLER_HPP_ +#define YABLOC_PARTICLE_FILTER__PREDICTION__RESAMPLER_HPP_ + +#include "yabloc_particle_filter/msg/particle_array.hpp" +#include "yabloc_particle_filter/prediction/resampling_history.hpp" + +#include + +namespace yabloc::modularized_particle_filter +{ +class resampling_skip_exception : public std::runtime_error +{ +public: + explicit resampling_skip_exception(const char * message) : runtime_error(message) {} +}; + +class RetroactiveResampler +{ +public: + using Particle = yabloc_particle_filter::msg::Particle; + using ParticleArray = yabloc_particle_filter::msg::ParticleArray; + + RetroactiveResampler(int number_of_particles, int max_history_num); + + ParticleArray add_weight_retroactively( + const ParticleArray & predicted_particles, const ParticleArray & weighted_particles); + + ParticleArray resample(const ParticleArray & predicted_particles); + +private: + // Number of updates to keep resampling history. + // Resampling records prior to this will not be kept. + const int max_history_num_; + // Number of particles to be managed. + const int number_of_particles_; + // ROS logger + rclcpp::Logger logger_; + // This is handled like ring buffer. + // It keeps track of which particles each particle has transformed into at each resampling. + ResamplingHistory resampling_history_; + // Indicates how many times the particles were resampled. + int latest_resampling_generation_; + + // Random generator from 0 to 1 + double random_from_01_uniformly() const; + // Check the sanity of the particles obtained from the particle corrector. + bool check_weighted_particles_validity(const ParticleArray & weighted_particles) const; +}; +} // namespace yabloc::modularized_particle_filter + +#endif // YABLOC_PARTICLE_FILTER__PREDICTION__RESAMPLER_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampling_history.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampling_history.hpp new file mode 100644 index 0000000000000..5b24ee96d6df5 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampling_history.hpp @@ -0,0 +1,74 @@ +// 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 YABLOC_PARTICLE_FILTER__PREDICTION__RESAMPLING_HISTORY_HPP_ +#define YABLOC_PARTICLE_FILTER__PREDICTION__RESAMPLING_HISTORY_HPP_ + +#include +#include +#include + +namespace yabloc::modularized_particle_filter +{ +class ResamplingHistory +{ +public: + ResamplingHistory(int max_history_num, int number_of_particles) + : max_history_num_(max_history_num), number_of_particles_(number_of_particles) + { + resampling_history_.resize(max_history_num); + + for (auto & generation : resampling_history_) { + generation.resize(number_of_particles); + std::iota(generation.begin(), generation.end(), 0); + } + } + + bool check_history_validity() const + { + for (auto & generation : resampling_history_) { + bool result = std::any_of(generation.begin(), generation.end(), [this](int x) { + return x < 0 || x >= number_of_particles_; + }); + + if (result) { + return false; + } + } + return true; + } + + std::vector & operator[](int generation_id) + { + return resampling_history_.at(generation_id % max_history_num_); + } + + const std::vector & operator[](int generation_id) const + { + return resampling_history_.at(generation_id % max_history_num_); + } + +private: + // Number of updates to keep resampling history. + // Resampling records prior to this will not be kept. + const int max_history_num_; + // + const int number_of_particles_; + // + std::vector> resampling_history_; +}; + +} // namespace yabloc::modularized_particle_filter + +#endif // YABLOC_PARTICLE_FILTER__PREDICTION__RESAMPLING_HISTORY_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/launch/yabloc_particle_filter.launch.xml b/localization/yabloc/yabloc_particle_filter/launch/yabloc_particle_filter.launch.xml new file mode 100644 index 0000000000000..fcbf64441ffd4 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/launch/yabloc_particle_filter.launch.xml @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/yabloc/yabloc_particle_filter/msg/Particle.msg b/localization/yabloc/yabloc_particle_filter/msg/Particle.msg new file mode 100644 index 0000000000000..3a4ebc3b5f002 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/msg/Particle.msg @@ -0,0 +1,4 @@ +# A representation of particle, composed of weight and pose. + +float32 weight +geometry_msgs/Pose pose diff --git a/localization/yabloc/yabloc_particle_filter/msg/ParticleArray.msg b/localization/yabloc/yabloc_particle_filter/msg/ParticleArray.msg new file mode 100644 index 0000000000000..13326206df300 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/msg/ParticleArray.msg @@ -0,0 +1,6 @@ +# An array of particles with a header for global reference. + +std_msgs/Header header + +int32 id +Particle[] particles diff --git a/localization/yabloc/yabloc_particle_filter/package.xml b/localization/yabloc/yabloc_particle_filter/package.xml new file mode 100644 index 0000000000000..7a70b8ad6a8fb --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/package.xml @@ -0,0 +1,39 @@ + + + + yabloc_particle_filter + 0.1.0 + YabLoc particle filter package + Kento Yabuuchi + Apache License 2.0 + + ament_cmake_ros + autoware_cmake + rosidl_default_generators + + geometry_msgs + libgoogle-glog-dev + rclcpp + sensor_msgs + sophus + std_msgs + std_srvs + tf2 + tf2_geometry_msgs + tf2_ros + tf2_sensor_msgs + tier4_autoware_utils + visualization_msgs + yabloc_common + + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp new file mode 100644 index 0000000000000..ce6c85975c2b0 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp @@ -0,0 +1,323 @@ +// 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 "yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp" +#include "yabloc_particle_filter/camera_corrector/logit.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace yabloc::modularized_particle_filter +{ + +CameraParticleCorrector::CameraParticleCorrector() +: AbstractCorrector("camera_particle_corrector"), + min_prob_(declare_parameter("min_prob")), + far_weight_gain_(declare_parameter("far_weight_gain")), + cost_map_(this) +{ + using std::placeholders::_1; + using std::placeholders::_2; + + enable_switch_ = declare_parameter("enabled_at_first"); + + // Publication + pub_image_ = create_publisher("~/debug/match_image", 10); + pub_map_image_ = create_publisher("~/debug/cost_map_image", 10); + pub_marker_ = create_publisher("~/debug/cost_map_range", 10); + pub_string_ = create_publisher("~/debug/state_string", 10); + pub_scored_cloud_ = create_publisher("~/debug/scored_cloud", 10); + pub_scored_posteriori_cloud_ = create_publisher("~/debug/scored_post_cloud", 10); + + // Subscription + auto on_line_segments = std::bind(&CameraParticleCorrector::on_line_segments, this, _1); + auto on_ll2 = std::bind(&CameraParticleCorrector::on_ll2, this, _1); + auto on_bounding_box = std::bind(&CameraParticleCorrector::on_bounding_box, this, _1); + auto on_pose = std::bind(&CameraParticleCorrector::on_pose, this, _1); + sub_line_segments_cloud_ = + create_subscription("~/input/line_segments_cloud", 10, on_line_segments); + sub_ll2_ = create_subscription("~/input/ll2_road_marking", 10, on_ll2); + sub_bounding_box_ = + create_subscription("~/input/ll2_bounding_box", 10, on_bounding_box); + sub_pose_ = create_subscription("~/input/pose", 10, on_pose); + + auto on_service = std::bind(&CameraParticleCorrector::on_service, this, _1, _2); + switch_service_ = create_service("~/switch_srv", on_service); + + // Timer callback + auto on_timer = std::bind(&CameraParticleCorrector::on_timer, this); + timer_ = + rclcpp::create_timer(this, this->get_clock(), rclcpp::Rate(1).period(), std::move(on_timer)); +} + +void CameraParticleCorrector::on_pose(const PoseStamped & msg) +{ + latest_pose_ = msg; +} + +void CameraParticleCorrector::on_service( + SetBool::Request::ConstSharedPtr request, SetBool::Response::SharedPtr response) +{ + enable_switch_ = request->data; + response->success = true; + if (enable_switch_) + RCLCPP_INFO_STREAM(get_logger(), "camera_corrector is enabled"); + else + RCLCPP_INFO_STREAM(get_logger(), "camera_corrector is disabled"); +} + +void CameraParticleCorrector::on_bounding_box(const PointCloud2 & msg) +{ + // NOTE: Under construction + pcl::PointCloud ll2_bounding_box; + pcl::fromROSMsg(msg, ll2_bounding_box); + cost_map_.set_bounding_box(ll2_bounding_box); + RCLCPP_INFO_STREAM(get_logger(), "Set bounding box into cost map"); +} + +std::pair +CameraParticleCorrector::split_line_segments(const PointCloud2 & msg) +{ + LineSegments all_line_segments_cloud; + pcl::fromROSMsg(msg, all_line_segments_cloud); + LineSegments reliable_cloud, iffy_cloud; + { + for (const auto & p : all_line_segments_cloud) { + if (p.label == 0) + iffy_cloud.push_back(p); + else + reliable_cloud.push_back(p); + } + } + + auto [good_cloud, bad_cloud] = filt(iffy_cloud); + { + cv::Mat debug_image = cv::Mat::zeros(800, 800, CV_8UC3); + auto draw = [&debug_image](LineSegments & cloud, cv::Scalar color) -> void { + for (const auto & line : cloud) { + const Eigen::Vector3f p1 = line.getVector3fMap(); + const Eigen::Vector3f p2 = line.getNormalVector3fMap(); + cv::line(debug_image, cv2pt(p1), cv2pt(p2), color, 2); + } + }; + + draw(reliable_cloud, cv::Scalar(0, 0, 255)); + draw(good_cloud, cv::Scalar(0, 255, 0)); + draw(bad_cloud, cv::Scalar(100, 100, 100)); + common::publish_image(*pub_image_, debug_image, msg.header.stamp); + } + + return {reliable_cloud, good_cloud}; +} + +void CameraParticleCorrector::on_line_segments(const PointCloud2 & line_segments_msg) +{ + tier4_autoware_utils::StopWatch stop_watch; + const rclcpp::Time stamp = line_segments_msg.header.stamp; + std::optional opt_array = this->get_synchronized_particle_array(stamp); + if (!opt_array.has_value()) { + return; + } + + const rclcpp::Duration dt = (stamp - opt_array->header.stamp); + if (std::abs(dt.seconds()) > 0.1) { + const std::string text = "Timestamp gap between image and particles is LARGE "; + RCLCPP_WARN_STREAM(get_logger(), text << dt.seconds()); + } + + auto [line_segments_cloud, iffy_line_segments_cloud] = split_line_segments(line_segments_msg); + ParticleArray weighted_particles = opt_array.value(); + + bool publish_weighted_particles = true; + const Pose meaned_pose = mean_pose(weighted_particles); + { + // Check travel distance and publish weights if it is enough long + Eigen::Vector3f mean_position = common::pose_to_affine(meaned_pose).translation(); + if ((mean_position - last_mean_position_).squaredNorm() > 1) { + last_mean_position_ = mean_position; + } else { + using namespace std::literals::chrono_literals; + publish_weighted_particles = false; + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *get_clock(), 2000, + "Skip particle weighting due to almost the same position"); + } + } + + cost_map_.set_height(meaned_pose.position.z); + + if (publish_weighted_particles) { + for (auto & particle : weighted_particles.particles) { + Sophus::SE3f transform = common::pose_to_se3(particle.pose); + LineSegments transformed_line_segments = + common::transform_line_segments(line_segments_cloud, transform); + LineSegments transformed_iffy_line_segments = + common::transform_line_segments(iffy_line_segments_cloud, transform); + transformed_line_segments += transformed_iffy_line_segments; + + float logit = compute_logit(transformed_line_segments, transform.translation()); + particle.weight = logit_to_prob(logit, 0.01f); + } + + if (enable_switch_) { + this->set_weighted_particle_array(weighted_particles); + } + } + + cost_map_.erase_obsolete(); // NOTE: + pub_marker_->publish(cost_map_.show_map_range()); + + // DEBUG: just visualization + { + Pose meaned_pose = mean_pose(weighted_particles); + Sophus::SE3f transform = common::pose_to_se3(meaned_pose); + + pcl::PointCloud cloud = evaluate_cloud( + common::transform_line_segments(line_segments_cloud, transform), transform.translation()); + pcl::PointCloud iffy_cloud = evaluate_cloud( + common::transform_line_segments(iffy_line_segments_cloud, transform), + transform.translation()); + + pcl::PointCloud rgb_cloud; + pcl::PointCloud rgb_iffy_cloud; + + float max_score = 0; + for (const auto p : cloud) { + max_score = std::max(max_score, std::abs(p.intensity)); + } + for (const auto p : cloud) { + pcl::PointXYZRGB rgb; + rgb.getVector3fMap() = p.getVector3fMap(); + rgb.rgba = common::color_scale::blue_red(p.intensity / max_score); + rgb_cloud.push_back(rgb); + } + for (const auto p : iffy_cloud) { + pcl::PointXYZRGB rgb; + rgb.getVector3fMap() = p.getVector3fMap(); + rgb.rgba = common::color_scale::blue_red(p.intensity / max_score); + rgb_iffy_cloud.push_back(rgb); + } + + common::publish_cloud(*pub_scored_cloud_, rgb_cloud, line_segments_msg.header.stamp); + common::publish_cloud( + *pub_scored_posteriori_cloud_, rgb_iffy_cloud, line_segments_msg.header.stamp); + } + + if (stop_watch.toc() > 0.080) { + RCLCPP_WARN_STREAM(get_logger(), "on_line_segments: " << stop_watch.toc() * 1000.0 << "[ms]"); + } else { + RCLCPP_INFO_STREAM(get_logger(), "on_line_segments: " << stop_watch.toc() * 1000.0 << "[ms]"); + } + + // Publish status as string + { + String msg; + std::stringstream ss; + ss << "-- Camera particle corrector --" << std::endl; + ss << (enable_switch_ ? "ENABLED" : "disabled") << std::endl; + ss << "time: " << stop_watch.toc() << std::endl; + msg.data = ss.str(); + pub_string_->publish(msg); + } +} + +void CameraParticleCorrector::on_timer() +{ + if (latest_pose_.has_value()) + common::publish_image( + *pub_map_image_, cost_map_.get_map_image(latest_pose_->pose), latest_pose_->header.stamp); +} + +void CameraParticleCorrector::on_ll2(const PointCloud2 & ll2_msg) +{ + pcl::PointCloud ll2_cloud; + pcl::fromROSMsg(ll2_msg, ll2_cloud); + cost_map_.set_cloud(ll2_cloud); + RCLCPP_INFO_STREAM(get_logger(), "Set LL2 cloud into Hierarchical cost map"); +} + +float abs_cos(const Eigen::Vector3f & t, float deg) +{ + const float radian = deg * M_PI / 180.0; + Eigen::Vector2f x(t.x(), t.y()); + Eigen::Vector2f y(tier4_autoware_utils::cos(radian), tier4_autoware_utils::sin(radian)); + x.normalize(); + return std::abs(x.dot(y)); +} + +float CameraParticleCorrector::compute_logit( + const LineSegments & line_segments_cloud, const Eigen::Vector3f & self_position) +{ + float logit = 0; + for (const LineSegment & pn : line_segments_cloud) { + const Eigen::Vector3f tangent = (pn.getNormalVector3fMap() - pn.getVector3fMap()).normalized(); + const float length = (pn.getVector3fMap() - pn.getNormalVector3fMap()).norm(); + + for (float distance = 0; distance < length; distance += 0.1f) { + Eigen::Vector3f p = pn.getVector3fMap() + tangent * distance; + + // NOTE: Close points are prioritized + float squared_norm = (p - self_position).topRows(2).squaredNorm(); + float gain = exp(-far_weight_gain_ * squared_norm); // 0 < gain < 1 + + const CostMapValue v3 = cost_map_.at(p.topRows(2)); + + if (v3.unmapped) { + // logit does not change if target pixel is unmapped + continue; + } + if (pn.label == 0) { // posteriori + logit += 0.2f * gain * (abs_cos(tangent, v3.angle) * v3.intensity - 0.5f); + } else { // apriori + logit += gain * (abs_cos(tangent, v3.angle) * v3.intensity - 0.5f); + } + } + } + return logit; +} + +pcl::PointCloud CameraParticleCorrector::evaluate_cloud( + const LineSegments & line_segments_cloud, const Eigen::Vector3f & self_position) +{ + pcl::PointCloud cloud; + for (const LineSegment & pn : line_segments_cloud) { + Eigen::Vector3f tangent = (pn.getNormalVector3fMap() - pn.getVector3fMap()).normalized(); + float length = (pn.getVector3fMap() - pn.getNormalVector3fMap()).norm(); + + for (float distance = 0; distance < length; distance += 0.1f) { + Eigen::Vector3f p = pn.getVector3fMap() + tangent * distance; + + // NOTE: Close points are prioritized + float squared_norm = (p - self_position).topRows(2).squaredNorm(); + float gain = std::exp(-far_weight_gain_ * squared_norm); + + CostMapValue v3 = cost_map_.at(p.topRows(2)); + float logit = 0; + if (!v3.unmapped) logit = gain * (abs_cos(tangent, v3.angle) * v3.intensity - 0.5f); + + pcl::PointXYZI xyzi(logit_to_prob(logit, 10.f)); + xyzi.getVector3fMap() = p; + cloud.push_back(xyzi); + } + } + return cloud; +} +} // namespace yabloc::modularized_particle_filter diff --git a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_node.cpp b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_node.cpp new file mode 100644 index 0000000000000..4def604030c95 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_node.cpp @@ -0,0 +1,29 @@ +// 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 "yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp" + +#include + +int main(int argc, char * argv[]) +{ + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + + namespace mpf = yabloc::modularized_particle_filter; + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/filt_line_segments.cpp b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/filt_line_segments.cpp new file mode 100644 index 0000000000000..3f782d85aec43 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/filt_line_segments.cpp @@ -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. + +#include "yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp" + +#include +#include +#include +#include +#include + +namespace yabloc::modularized_particle_filter +{ +cv::Point2f cv2pt(const Eigen::Vector3f v) +{ + const float METRIC_PER_PIXEL = 0.05; + const float IMAGE_RADIUS = 400; + return {-v.y() / METRIC_PER_PIXEL + IMAGE_RADIUS, -v.x() / METRIC_PER_PIXEL + 2 * IMAGE_RADIUS}; +} + +float normalized_atan2(const Eigen::Vector3f & t, float deg) +{ + float diff = std::atan2(t.y(), t.x()) - deg * M_PI / 180; + diff = std::fmod(diff, M_PI); + + if (diff < 0) diff = -diff; + + if (diff < M_PI_2) { + return 1 - diff / M_PI_2; + } else if (diff < M_PI) { + return diff / M_PI_2 - 1; + } else { + throw std::runtime_error("invalid cos"); + } +} + +std::pair +CameraParticleCorrector::filt(const LineSegments & iffy_lines) +{ + LineSegments good, bad; + if (!latest_pose_.has_value()) { + throw std::runtime_error("latest_pose_ is nullopt"); + } + + const Sophus::SE3f pose = common::pose_to_se3(latest_pose_.value().pose); + + // pcl::PointCloud rgb_cloud; + for (const auto & line : iffy_lines) { + const Eigen::Vector3f p1 = line.getVector3fMap(); + const Eigen::Vector3f p2 = line.getNormalVector3fMap(); + const float length = (p1 - p2).norm(); + const Eigen::Vector3f tangent = (p1 - p2).normalized(); + + float score = 0; + int count = 0; + for (float distance = 0; distance < length; distance += 0.1f) { + Eigen::Vector3f px = pose * (p2 + tangent * distance); + CostMapValue v3 = cost_map_.at(px.topRows(2)); + float cos2 = normalized_atan2(pose.so3() * tangent, v3.angle); + score += (cos2 * v3.intensity); + count++; + + // pcl::PointXYZRGB rgb; + // rgb.getVector3fMap() = px; + // rgb.rgba = common::color_scale::blue_red(cos2 * v3[0] / 255.0f); + // rgb_cloud.push_back(rgb); + } + + if (score / count > 0.5f) { + good.push_back(line); + } else { + bad.push_back(line); + } + } + // common::publish_cloud(*pub_scored_cloud_, rgb_cloud, get_clock()->now()); + + return {good, bad}; +} +} // namespace yabloc::modularized_particle_filter diff --git a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/logit.cpp b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/logit.cpp new file mode 100644 index 0000000000000..d8bb5690b6fc4 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/logit.cpp @@ -0,0 +1,55 @@ +// 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 "yabloc_particle_filter/camera_corrector/logit.hpp" + +#include +#include +#include + +namespace yabloc +{ +namespace +{ +struct ProbToLogitTable +{ + ProbToLogitTable() + { + for (int i = 0; i < 100; ++i) { + float p = i / 100.0f; + table_.at(i) = std::log(p / std::max(1 - p, 1e-6f)); + } + } + float operator()(float prob) const + { + int index = std::clamp(static_cast(prob * 100), 0, 99); + return table_.at(index); + } + + std::array table_; +} prob_to_logit_table; + +} // namespace + +float logit_to_prob(float logit, float gain) +{ + return 1.f / (1 + std::exp(-gain * logit)); +} + +float prob_to_logit(float prob) +{ + return prob_to_logit_table(prob); +} + +} // namespace yabloc diff --git a/localization/yabloc/yabloc_particle_filter/src/common/mean.cpp b/localization/yabloc/yabloc_particle_filter/src/common/mean.cpp new file mode 100644 index 0000000000000..71c2d79ab7696 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/src/common/mean.cpp @@ -0,0 +1,135 @@ +// 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 "yabloc_particle_filter/common/mean.hpp" + +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include + +namespace yabloc::modularized_particle_filter +{ +namespace +{ +double mean_radian(const std::vector & angles, const std::vector & weights) +{ + std::complex c{}; + for (int i{0}; i < static_cast(angles.size()); i++) { + c += weights[i] * std::polar(1.0, angles[i]); + } + std::complex cw{std::accumulate(weights.begin(), weights.end(), 0.0)}; + return std::arg(c / cw); +} +} // namespace + +geometry_msgs::msg::Pose mean_pose( + const yabloc_particle_filter::msg::ParticleArray & particle_array) +{ + using Pose = geometry_msgs::msg::Pose; + using Particle = yabloc_particle_filter::msg::Particle; + + Pose mean_pose; + + double sum_weight{std::accumulate( + particle_array.particles.begin(), particle_array.particles.end(), 0.0, + [](double weight, const Particle & particle) { return weight + particle.weight; })}; + + if (std::isinf(sum_weight)) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("meanPose"), "sum_weight: " << sum_weight); + } + + std::vector rolls{}; + std::vector pitches{}; + std::vector yaws{}; + std::vector normalized_weights{}; + for (const Particle & particle : particle_array.particles) { + double normalized_weight = particle.weight / sum_weight; + + mean_pose.position.x += particle.pose.position.x * normalized_weight; + mean_pose.position.y += particle.pose.position.y * normalized_weight; + mean_pose.position.z += particle.pose.position.z * normalized_weight; + + double yaw{0.0}, pitch{0.0}, roll{0.0}; + tf2::getEulerYPR(particle.pose.orientation, yaw, pitch, roll); + + rolls.push_back(roll); + pitches.push_back(pitch); + yaws.push_back(yaw); + normalized_weights.push_back(normalized_weight); + } + + const double mean_roll{mean_radian(rolls, normalized_weights)}; + const double mean_pitch{mean_radian(pitches, normalized_weights)}; + const double mean_yaw{mean_radian(yaws, normalized_weights)}; + + tf2::Quaternion q; + q.setRPY(mean_roll, mean_pitch, mean_yaw); + mean_pose.orientation = tf2::toMsg(q); + return mean_pose; +} + +Eigen::Matrix3f std_of_distribution(const yabloc_particle_filter::msg::ParticleArray & array) +{ + using Particle = yabloc_particle_filter::msg::Particle; + auto ori = mean_pose(array).orientation; + Eigen::Quaternionf orientation(ori.w, ori.x, ori.y, ori.z); + float invN = 1.f / array.particles.size(); + Eigen::Vector3f mean = Eigen::Vector3f::Zero(); + for (const Particle & p : array.particles) { + Eigen::Affine3f affine = common::pose_to_affine(p.pose); + mean += affine.translation(); + } + mean *= invN; + + Eigen::Matrix3f sigma = Eigen::Matrix3f::Zero(); + for (const Particle & p : array.particles) { + Eigen::Affine3f affine = common::pose_to_affine(p.pose); + Eigen::Vector3f d = affine.translation() - mean; + d = orientation.conjugate() * d; + sigma += (d * d.transpose()) * invN; + } + + return sigma; +} + +float std_of_weight(const yabloc_particle_filter::msg::ParticleArray & particle_array) +{ + using Particle = yabloc_particle_filter::msg::Particle; + + const float invN = 1.f / particle_array.particles.size(); + float mean = 0; + for (const Particle & p : particle_array.particles) { + mean += p.weight; + } + mean *= invN; + + float sigma = 0.0; + for (const Particle & p : particle_array.particles) { + sigma += (p.weight - mean) * (p.weight - mean); + } + sigma *= invN; + + return std::sqrt(sigma); +} +} // namespace yabloc::modularized_particle_filter diff --git a/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp b/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp new file mode 100644 index 0000000000000..efe84cec7fb6c --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp @@ -0,0 +1,115 @@ +// 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 "yabloc_common/color.hpp" + +#include +#include + +#include +#include + +namespace yabloc::modularized_particle_fitler +{ +class ParticleVisualize : public rclcpp::Node +{ +public: + using Particle = yabloc_particle_filter::msg::Particle; + using ParticleArray = yabloc_particle_filter::msg::ParticleArray; + using Marker = visualization_msgs::msg::Marker; + using MarkerArray = visualization_msgs::msg::MarkerArray; + + ParticleVisualize() : Node("particle_visualize") + { + using std::placeholders::_1; + // Subscriber + sub_particles_ = this->create_subscription( + "/particle_array", 10, std::bind(&ParticleVisualize::on_particles, this, _1)); + + // Publisher + pub_marker_array = this->create_publisher("/marker_array", 10); + } + +private: + rclcpp::Subscription::SharedPtr sub_particles_; + rclcpp::Publisher::SharedPtr pub_marker_array; + + std_msgs::msg::ColorRGBA compute_color(float value) const + { + float r = 1.0f, g = 1.0f, b = 1.0f; + // clang-format off + value = std::clamp(value, 0.0f, 1.0f); + if (value < 0.25f) { + r = 0; g = 4 * (value); + } else if (value < 0.5f) { + r = 0; b = 1 + 4 * (0.25f - value); + } else if (value < 0.75f) { + r = 4 * (value - 0.5f); b = 0; + } else { + g = 1 + 4 * (0.75f - value); b = 0; + } + // clang-format on + + std_msgs::msg::ColorRGBA rgba; + rgba.r = r; + rgba.g = g; + rgba.b = b; + rgba.a = 1.0f; + return rgba; + } + + void on_particles(const ParticleArray & msg) + { + visualization_msgs::msg::MarkerArray marker_array; + auto minmax_weight = std::minmax_element( + msg.particles.begin(), msg.particles.end(), + [](const Particle & a, const Particle & b) -> bool { return a.weight < b.weight; }); + + float min = minmax_weight.first->weight; + float max = minmax_weight.second->weight; + max = std::max(max, min + 1e-7f); + auto bound_weight = [min, max](float raw) -> float { return (raw - min) / (max - min); }; + + RCLCPP_INFO_STREAM(get_logger(), "min: " << min << " max: " << max); + int id = 0; + for (const Particle & p : msg.particles) { + Marker marker; + marker.frame_locked = true; + marker.header.frame_id = "map"; + marker.id = id++; + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.scale.x = 0.3; + marker.scale.y = 0.1; + marker.scale.z = 0.1; + + marker.color = common::color_scale::rainbow(bound_weight(p.weight)); + marker.pose.orientation = p.pose.orientation; + marker.pose.position.x = p.pose.position.x; + marker.pose.position.y = p.pose.position.y; + marker.pose.position.z = p.pose.position.z; + marker_array.markers.push_back(marker); + } + pub_marker_array->publish(marker_array); + } +}; +} // namespace yabloc::modularized_particle_fitler + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp b/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp new file mode 100644 index 0000000000000..955ed84fdea2d --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp @@ -0,0 +1,58 @@ +// 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 "yabloc_particle_filter/common/visualize.hpp" + +#include + +namespace yabloc::modularized_particle_filter +{ +ParticleVisualizer::ParticleVisualizer(rclcpp::Node & node) +{ + pub_marker_array_ = node.create_publisher("~/debug/particles_marker_array", 10); +} + +void ParticleVisualizer::publish(const ParticleArray & msg) +{ + visualization_msgs::msg::MarkerArray marker_array; + auto minmax_weight = std::minmax_element( + msg.particles.begin(), msg.particles.end(), + [](const Particle & a, const Particle & b) -> bool { return a.weight < b.weight; }); + + float min = minmax_weight.first->weight; + float max = minmax_weight.second->weight; + max = std::max(max, min + 1e-7f); + auto boundWeight = [min, max](float raw) -> float { return (raw - min) / (max - min); }; + + int id = 0; + for (const Particle & p : msg.particles) { + visualization_msgs::msg::Marker marker; + marker.frame_locked = true; + marker.header.frame_id = "map"; + marker.id = id++; + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.scale.x = 0.3; + marker.scale.y = 0.1; + marker.scale.z = 0.1; + marker.color = common::color_scale::rainbow(boundWeight(p.weight)); + marker.pose.orientation = p.pose.orientation; + marker.pose.position.x = p.pose.position.x; + marker.pose.position.y = p.pose.position.y; + marker.pose.position.z = p.pose.position.z; + marker_array.markers.push_back(marker); + } + + pub_marker_array_->publish(marker_array); +} +} // namespace yabloc::modularized_particle_filter diff --git a/localization/yabloc/yabloc_particle_filter/src/correction/abstract_corrector.cpp b/localization/yabloc/yabloc_particle_filter/src/correction/abstract_corrector.cpp new file mode 100644 index 0000000000000..9d55f384403d8 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/src/correction/abstract_corrector.cpp @@ -0,0 +1,71 @@ +// 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 "yabloc_particle_filter/correction/abstract_corrector.hpp" + +namespace yabloc::modularized_particle_filter +{ +AbstractCorrector::AbstractCorrector(const std::string & node_name) +: Node(node_name), + acceptable_max_delay_(declare_parameter("acceptable_max_delay")), + visualize_(declare_parameter("visualize")), + logger_(rclcpp::get_logger("abstract_corrector")) +{ + using std::placeholders::_1; + particle_pub_ = create_publisher("~/output/weighted_particles", 10); + particle_sub_ = create_subscription( + "~/input/predicted_particles", 10, std::bind(&AbstractCorrector::on_particle_array, this, _1)); + + if (visualize_) visualizer_ = std::make_shared(*this); +} + +void AbstractCorrector::on_particle_array(const ParticleArray & particle_array) +{ + particle_array_buffer_.push_back(particle_array); +} + +std::optional AbstractCorrector::get_synchronized_particle_array( + const rclcpp::Time & stamp) +{ + auto itr = particle_array_buffer_.begin(); + while (itr != particle_array_buffer_.end()) { + rclcpp::Duration dt = rclcpp::Time(itr->header.stamp) - stamp; + if (dt.seconds() < -acceptable_max_delay_) + particle_array_buffer_.erase(itr++); + else + break; + } + + if (particle_array_buffer_.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *get_clock(), 2000, "sychronized particles are requested but buffer is empty"); + } + + if (particle_array_buffer_.empty()) return std::nullopt; + + auto comp = [stamp](ParticleArray & x1, ParticleArray & x2) -> bool { + auto dt1 = rclcpp::Time(x1.header.stamp) - stamp; + auto dt2 = rclcpp::Time(x2.header.stamp) - stamp; + return std::abs(dt1.seconds()) < std::abs(dt2.seconds()); + }; + return *std::min_element(particle_array_buffer_.begin(), particle_array_buffer_.end(), comp); +} + +void AbstractCorrector::set_weighted_particle_array(const ParticleArray & particle_array) +{ + particle_pub_->publish(particle_array); + if (visualize_) visualizer_->publish(particle_array); +} + +} // namespace yabloc::modularized_particle_filter diff --git a/localization/yabloc/yabloc_particle_filter/src/correction/correction_util.cpp b/localization/yabloc/yabloc_particle_filter/src/correction/correction_util.cpp new file mode 100644 index 0000000000000..67ca4f5add947 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/src/correction/correction_util.cpp @@ -0,0 +1,38 @@ +// 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 "yabloc_particle_filter/correction/correction_util.hpp" + +namespace yabloc::modularized_particle_filter +{ +std::optional find_synced_particles( + boost::circular_buffer circular_buffer, + rclcpp::Time time) +{ + for (int i{1}; i < static_cast(circular_buffer.size()); i++) { + if (rclcpp::Time(circular_buffer[i].header.stamp) < time) { + return circular_buffer[i]; + } + } + if (0 < static_cast(circular_buffer.size())) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("modularized_particle_filter.correction_util"), + "the sensor data is too old: " + << rclcpp::Time(circular_buffer[static_cast(circular_buffer.size()) - 1].header.stamp) + .seconds() - + time.seconds()); + } + return std::nullopt; +} +} // namespace yabloc::modularized_particle_filter diff --git a/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp b/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp new file mode 100644 index 0000000000000..f5a79e52b8352 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp @@ -0,0 +1,162 @@ +// 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 "yabloc_particle_filter/gnss_corrector/gnss_particle_corrector.hpp" + +#include +#include + +namespace yabloc::modularized_particle_filter +{ +GnssParticleCorrector::GnssParticleCorrector() +: AbstractCorrector("gnss_particle_corrector"), + mahalanobis_distance_threshold_(declare_parameter("mahalanobis_distance_threshold")), + weight_manager_(this) +{ + using std::placeholders::_1; + + // Subscriber + auto on_pose = std::bind(&GnssParticleCorrector::on_pose, this, _1); + auto on_height = [this](const Float32 & height) { this->latest_height_ = height; }; + pose_sub_ = create_subscription("~/input/pose_with_covariance", 10, on_pose); + height_sub_ = create_subscription("~/input/height", 10, on_height); + + // Publisher + marker_pub_ = create_publisher("~/debug/gnss_range_marker", 10); +} + +bool GnssParticleCorrector::is_gnss_observation_valid( + const Eigen::Matrix3f & sigma, const Eigen::Vector3f & meaned_position, + const Eigen::Vector3f & gnss_position) +{ + Eigen::Matrix3f inv_sigma = sigma.completeOrthogonalDecomposition().pseudoInverse(); + Eigen::Vector3f diff = gnss_position - meaned_position; + diff.z() = 0; + + float mahalanobis_distance = std::sqrt(diff.dot(inv_sigma * diff)); + RCLCPP_INFO_STREAM(get_logger(), "mahalanobis: " << mahalanobis_distance); + + if (mahalanobis_distance > mahalanobis_distance_threshold_) { + RCLCPP_WARN_STREAM( + get_logger(), "Mahalanobis distance is too large: " << mahalanobis_distance << ">" + << mahalanobis_distance_threshold_); + return false; + } + return true; +} + +void GnssParticleCorrector::on_pose(const PoseCovStamped::ConstSharedPtr pose_msg) +{ + const rclcpp::Time stamp = pose_msg->header.stamp; + const auto & position = pose_msg->pose.pose.position; + Eigen::Vector3f gnss_position; + gnss_position << position.x, position.y, position.z; + + constexpr bool is_rtk_fixed = false; + process(gnss_position, stamp, is_rtk_fixed); +} + +void GnssParticleCorrector::process( + const Eigen::Vector3f & gnss_position, const rclcpp::Time & stamp, const bool is_rtk_fixed) +{ + publish_marker(gnss_position, is_rtk_fixed); + + std::optional opt_particles = get_synchronized_particle_array(stamp); + + if (!opt_particles.has_value()) return; + auto dt = (stamp - rclcpp::Time(opt_particles->header.stamp)); + if (std::abs(dt.seconds()) > 0.1) { + RCLCPP_WARN_STREAM( + get_logger(), "Timestamp gap between gnss and particles is too large: " << dt.seconds()); + } + + const Eigen::Matrix3f sigma = modularized_particle_filter::std_of_distribution(*opt_particles); + const geometry_msgs::msg::Pose meaned_pose = mean_pose(opt_particles.value()); + const Eigen::Vector3f meaned_position = common::pose_to_affine(meaned_pose).translation(); + + // Check validity of GNSS measurement by mahalanobis distance + if (!is_gnss_observation_valid(sigma, meaned_position, gnss_position)) { + return; + } + + ParticleArray weighted_particles = + weight_particles(opt_particles.value(), gnss_position, is_rtk_fixed); + + // Compute travel distance from last update position + // If the distance is too short, skip weighting + { + Eigen::Vector3f meaned_position = common::pose_to_affine(meaned_pose).translation(); + if ((meaned_position - last_mean_position_).squaredNorm() > 1) { + this->set_weighted_particle_array(weighted_particles); + last_mean_position_ = meaned_position; + } else { + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *get_clock(), 2000, + "Skip particle weighting due to almost the same position"); + } + } +} + +void GnssParticleCorrector::publish_marker(const Eigen::Vector3f & position, bool is_rtk_fixed) +{ + using namespace std::literals::chrono_literals; + using Point = geometry_msgs::msg::Point; + + auto drawCircle = [](std::vector & points, float radius) -> void { + const int N = 10; + for (int theta = 0; theta < 2 * N + 1; theta++) { + geometry_msgs::msg::Point pt; + pt.x = radius * std::cos(theta * M_PI / N); + pt.y = radius * std::sin(theta * M_PI / N); + points.push_back(pt); + } + }; + + MarkerArray array_msg; + for (int i = 0; i < 5; i++) { + Marker marker; + marker.header.stamp = get_clock()->now(); + marker.header.frame_id = "/map"; + marker.id = i; + marker.type = Marker::LINE_STRIP; + marker.lifetime = rclcpp::Duration(500ms); + marker.pose.position.x = position.x(); + marker.pose.position.y = position.y(); + marker.pose.position.z = latest_height_.data; + + float prob = i / 4.0f; + marker.color = common::color_scale::rainbow(prob); + marker.color.a = 0.5f; + marker.scale.x = 0.1; + drawCircle(marker.points, weight_manager_.inverse_normal_pdf(prob, is_rtk_fixed)); + array_msg.markers.push_back(marker); + } + marker_pub_->publish(array_msg); +} + +GnssParticleCorrector::ParticleArray GnssParticleCorrector::weight_particles( + const ParticleArray & predicted_particles, const Eigen::Vector3f & pose, bool is_rtk_fixed) +{ + ParticleArray weighted_particles{predicted_particles}; + + for (auto & particle : weighted_particles.particles) { + float distance = static_cast( + std::hypot(particle.pose.position.x - pose.x(), particle.pose.position.y - pose.y())); + particle.weight = weight_manager_.normal_pdf(distance, is_rtk_fixed); + } + + return weighted_particles; +} + +} // namespace yabloc::modularized_particle_filter diff --git a/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_node.cpp b/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_node.cpp new file mode 100644 index 0000000000000..6d76fd295ac02 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_node.cpp @@ -0,0 +1,24 @@ +// 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 "yabloc_particle_filter/gnss_corrector/gnss_particle_corrector.hpp" + +int main(int argc, char * argv[]) +{ + namespace mpf = yabloc::modularized_particle_filter; + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/direct_cost_map.cpp b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/direct_cost_map.cpp new file mode 100644 index 0000000000000..7da87e8050ebe --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/direct_cost_map.cpp @@ -0,0 +1,118 @@ +// 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 "yabloc_particle_filter/ll2_cost_map/direct_cost_map.hpp" + +namespace yabloc +{ +cv::Mat direct_cost_map(const cv::Mat & cost_map, const cv::Mat & intensity) +{ + constexpr int MAX_INT = std::numeric_limits::max(); + + std::vector> distances; + distances.resize(cost_map.rows); + for (int i = 0; i < cost_map.rows; i++) { + distances.at(i).resize(cost_map.cols); + std::fill(distances.at(i).begin(), distances.at(i).end(), MAX_INT); + const uchar * intensity_ptr = intensity.ptr(i); + for (int j = 0; j < cost_map.cols; j++) { + if (intensity_ptr[j] == 0) distances.at(i).at(j) = 0; + } + } + + cv::Mat dst = cost_map.clone(); + + // Forward + for (int r = 1; r < cost_map.rows; r++) { + const uchar * upper_ptr = dst.ptr(r - 1); + uchar * current_ptr = dst.ptr(r); + + for (int c = 1; c < cost_map.cols; c++) { + int up = distances.at(r - 1).at(c); + int left = distances.at(r).at(c - 1); + if (up < left) { + if (distances.at(r).at(c) < up + 1) continue; + distances.at(r).at(c) = up + 1; + current_ptr[c] = upper_ptr[c]; + } else { + if (distances.at(r).at(c) < left + 1) continue; + distances.at(r).at(c) = left + 1; + current_ptr[c] = current_ptr[c - 1]; + } + } + } + + // Backward + for (int r = cost_map.rows - 2; r >= 0; r--) { + const uchar * downer_ptr = dst.ptr(r + 1); + uchar * current_ptr = dst.ptr(r); + + for (int c = cost_map.cols - 2; c >= 0; c--) { + int down = distances.at(r + 1).at(c); + int right = distances.at(r).at(c + 1); + if (down < right) { + if (distances.at(r).at(c) < down + 1) continue; + distances.at(r).at(c) = down + 1; + current_ptr[c] = downer_ptr[c]; + } else { + if (distances.at(r).at(c) < right + 1) continue; + distances.at(r).at(c) = right + 1; + current_ptr[c] = current_ptr[c + 1]; + } + } + } + + return dst; +} + +cv::Mat visualize_direction_map(const cv::Mat & cost_map) +{ + cv::Mat s = 255 * cv::Mat::ones(cost_map.size(), CV_8UC1); + cv::Mat v = 255 * cv::Mat::ones(cost_map.size(), CV_8UC1); + cv::Mat hsv, rgb; + cv::merge(std::vector{cost_map, s, v}, hsv); + cv::cvtColor(hsv, rgb, cv::COLOR_HSV2BGR); + + for (int r = 0; r < cost_map.rows; r++) { + const uchar * src_ptr = cost_map.ptr(r); + cv::Vec3b * dst_ptr = rgb.ptr(r); + for (int c = 0; c < cost_map.cols; c++) { + if (src_ptr[c] == 0) dst_ptr[c] = cv::Vec3b(0, 0, 0); + } + } + return rgb; +} + +} // namespace yabloc + +// #include + +// int main() +// { +// // 0~180 +// cv::Mat raw_map = cv::Mat::zeros(cv::Size(640, 480), CV_8UC1); +// cv::line(raw_map, cv::Point(400, 0), cv::Point(400, 800), cv::Scalar::all(10), 3); +// cv::line(raw_map, cv::Point(0, 400), cv::Point(800, 400), cv::Scalar::all(80), 3); +// cv::line(raw_map, cv::Point(0, 0), cv::Point(400, 400), cv::Scalar::all(160), 3); +// cv::line(raw_map, cv::Point(400, 400), cv::Point(800, 800), cv::Scalar::all(30), 3); + +// cv::Mat intensity = raw_map.clone(); + +// cv::Mat directed = image_processing::directCostMap(raw_map, intensity); +// cv::Mat show1 = image_processing::visualizeDirectionMap(raw_map); +// cv::Mat show2 = image_processing::visualizeDirectionMap(directed); +// cv::hconcat(show1, show2, show1); +// cv::imshow("raw + directed", show1); +// cv::waitKey(0); +// } diff --git a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp new file mode 100644 index 0000000000000..2f6c2fe9b0fac --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp @@ -0,0 +1,266 @@ +// 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 "yabloc_particle_filter/ll2_cost_map/hierarchical_cost_map.hpp" + +#include "yabloc_particle_filter/ll2_cost_map/direct_cost_map.hpp" + +#include +#include +#include + +#include + +namespace yabloc +{ +float Area::unit_length_ = -1; + +HierarchicalCostMap::HierarchicalCostMap(rclcpp::Node * node) +: max_range_(node->declare_parameter("max_range")), + image_size_(node->declare_parameter("image_size")), + max_map_count_(10), + logger_(node->get_logger()) +{ + Area::unit_length_ = max_range_; + float gamma = node->declare_parameter("gamma"); + gamma_converter.reset(gamma); +} + +cv::Point2i HierarchicalCostMap::to_cv_point(const Area & area, const Eigen::Vector2f p) const +{ + Eigen::Vector2f relative = p - area.real_scale(); + float px = relative.x() / max_range_ * image_size_; + float py = relative.y() / max_range_ * image_size_; + return {static_cast(px), static_cast(py)}; +} + +CostMapValue HierarchicalCostMap::at(const Eigen::Vector2f & position) +{ + if (!cloud_.has_value()) { + return CostMapValue{0.5f, 0, true}; + } + + Area key(position); + if (cost_maps_.count(key) == 0) { + build_map(key); + } + map_accessed_[key] = true; + + cv::Point2i tmp = to_cv_point(key, position); + cv::Vec3b b3 = cost_maps_.at(key).ptr(tmp.y)[tmp.x]; + return {b3[0] / 255.f, b3[1], b3[2] == 1}; +} + +void HierarchicalCostMap::set_height(float height) +{ + if (height_) { + if (std::abs(*height_ - height) > 2) { + generated_map_history_.clear(); + cost_maps_.clear(); + map_accessed_.clear(); + } + } + + height_ = height; +} + +void HierarchicalCostMap::set_bounding_box(const pcl::PointCloud & cloud) +{ + if (cloud.empty()) return; + BgPolygon poly; + + std::optional last_label = std::nullopt; + for (const pcl::PointXYZL p : cloud) { + if (last_label) { + if ((*last_label) != p.label) { + bounding_boxes_.push_back(poly); + poly.outer().clear(); + } + } + poly.outer().push_back(BgPoint(p.x, p.y)); + last_label = p.label; + } + bounding_boxes_.push_back(poly); +} + +void HierarchicalCostMap::set_cloud(const pcl::PointCloud & cloud) +{ + cloud_ = cloud; +} + +void HierarchicalCostMap::build_map(const Area & area) +{ + if (!cloud_.has_value()) return; + + cv::Mat image = 255 * cv::Mat::ones(cv::Size(image_size_, image_size_), CV_8UC1); + cv::Mat orientation = cv::Mat::zeros(cv::Size(image_size_, image_size_), CV_8UC1); + + auto cvPoint = [this, area](const Eigen::Vector3f & p) -> cv::Point { + return this->to_cv_point(area, p.topRows(2)); + }; + + // TODO(KYabuuchi) We can speed up by skipping too far line_segments + for (const auto pn : cloud_.value()) { + if (height_) { + if (std::abs(pn.z - *height_) > 4) continue; + if (std::abs(pn.normal_z - *height_) > 4) continue; + } + + cv::Point2i from = cvPoint(pn.getVector3fMap()); + cv::Point2i to = cvPoint(pn.getNormalVector3fMap()); + + float radian = std::atan2(from.y - to.y, from.x - to.x); + if (radian < 0) radian += M_PI; + float degree = radian * 180 / M_PI; + + cv::line(image, from, to, cv::Scalar::all(0), 1); + cv::line(orientation, from, to, cv::Scalar::all(degree), 1); + } + + // channel-1 + cv::Mat distance; + cv::distanceTransform(image, distance, cv::DIST_L2, 3); + cv::threshold(distance, distance, 100, 100, cv::THRESH_TRUNC); + distance.convertTo(distance, CV_8UC1, -2.55, 255); + + // channel-2 + cv::Mat whole_orientation = direct_cost_map(orientation, image); + + // channel-3 + cv::Mat available_area = create_available_area_image(area); + + cv::Mat directed_cost_map; + cv::merge( + std::vector{gamma_converter(distance), whole_orientation, available_area}, + directed_cost_map); + + cost_maps_[area] = directed_cost_map; + generated_map_history_.push_back(area); + + RCLCPP_INFO_STREAM( + logger_, "successed to build map " << area(area) << " " << area.real_scale().transpose()); +} + +HierarchicalCostMap::MarkerArray HierarchicalCostMap::show_map_range() const +{ + MarkerArray array_msg; + + auto gpoint = [](float x, float y) -> geometry_msgs::msg::Point { + geometry_msgs::msg::Point gp; + gp.x = x; + gp.y = y; + return gp; + }; + + int id = 0; + for (const Area & area : generated_map_history_) { + Marker marker; + marker.header.frame_id = "map"; + marker.id = id++; + marker.type = Marker::LINE_STRIP; + marker.color = common::Color(0, 0, 1.0f, 1.0f); + marker.scale.x = 0.1; + Eigen::Vector2f xy = area.real_scale(); + marker.points.push_back(gpoint(xy.x(), xy.y())); + marker.points.push_back(gpoint(xy.x() + area.unit_length_, xy.y())); + marker.points.push_back(gpoint(xy.x() + area.unit_length_, xy.y() + area.unit_length_)); + marker.points.push_back(gpoint(xy.x(), xy.y() + area.unit_length_)); + marker.points.push_back(gpoint(xy.x(), xy.y())); + array_msg.markers.push_back(marker); + } + return array_msg; +} + +cv::Mat HierarchicalCostMap::get_map_image(const Pose & pose) +{ + // if (generated_map_history_.empty()) + // return cv::Mat::zeros(cv::Size(image_size_, image_size_), CV_8UC3); + + Eigen::Vector2f center; + center << pose.position.x, pose.position.y; + + float w = pose.orientation.w; + float z = pose.orientation.z; + Eigen::Matrix2f R = Eigen::Rotation2Df(2.f * std::atan2(z, w) - M_PI_2).toRotationMatrix(); + + auto toVector2f = [this, center, R](float h, float w) -> Eigen::Vector2f { + Eigen::Vector2f offset; + offset.x() = (w / this->image_size_ - 0.5f) * this->max_range_ * 1.5f; + offset.y() = -(h / this->image_size_ - 0.5f) * this->max_range_ * 1.5f; + return center + R * offset; + }; + + cv::Mat image = cv::Mat::zeros(cv::Size(image_size_, image_size_), CV_8UC3); + for (int w = 0; w < image_size_; w++) { + for (int h = 0; h < image_size_; h++) { + CostMapValue v3 = this->at(toVector2f(h, w)); + if (v3.unmapped) + image.at(h, w) = cv::Vec3b(v3.angle, 255 * v3.intensity, 50); + else + image.at(h, w) = cv::Vec3b(v3.angle, 255 * v3.intensity, 255 * v3.intensity); + } + } + + cv::Mat rgb_image; + cv::cvtColor(image, rgb_image, cv::COLOR_HSV2BGR); + return rgb_image; +} + +void HierarchicalCostMap::erase_obsolete() +{ + if (cost_maps_.size() < max_map_count_) return; + + for (auto itr = generated_map_history_.begin(); itr != generated_map_history_.end();) { + if (map_accessed_[*itr]) { + ++itr; + continue; + } + cost_maps_.erase(*itr); + itr = generated_map_history_.erase(itr); + } + + map_accessed_.clear(); +} + +cv::Mat HierarchicalCostMap::create_available_area_image(const Area & area) const +{ + cv::Mat available_area = cv::Mat::zeros(cv::Size(image_size_, image_size_), CV_8UC1); + if (bounding_boxes_.empty()) return available_area; + + // Define current area + using BgBox = boost::geometry::model::box; + + BgBox area_polygon; + std::array area_bounding_box = area.real_scale_boundary(); + area_polygon.min_corner() = {area_bounding_box[0].x(), area_bounding_box[0].y()}; + area_polygon.max_corner() = {area_bounding_box[1].x(), area_bounding_box[1].y()}; + + std::vector> contours; + + for (const BgPolygon & box : bounding_boxes_) { + if (boost::geometry::disjoint(area_polygon, box)) { + continue; + } + std::vector contour; + for (BgPoint p : box.outer()) { + contour.push_back(to_cv_point(area, {p.x(), p.y()})); + } + contours.push_back(contour); + } + + cv::drawContours(available_area, contours, -1, cv::Scalar::all(1), -1); + return available_area; +} + +} // namespace yabloc diff --git a/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp new file mode 100644 index 0000000000000..73e0c1c0f8b12 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp @@ -0,0 +1,374 @@ +// 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 "yabloc_particle_filter/prediction/predictor.hpp" + +#include "yabloc_particle_filter/common/mean.hpp" +#include "yabloc_particle_filter/common/prediction_util.hpp" +#include "yabloc_particle_filter/prediction/resampler.hpp" + +#include +#include +#include + +#include + +#include + +#include + +namespace yabloc::modularized_particle_filter +{ + +Predictor::Predictor() +: Node("predictor"), + number_of_particles_(declare_parameter("num_of_particles")), + resampling_interval_seconds_(declare_parameter("resampling_interval_seconds")), + static_linear_covariance_(declare_parameter("static_linear_covariance")), + static_angular_covariance_(declare_parameter("static_angular_covariance")), + cov_xx_yy_{this->template declare_parameter>("cov_xx_yy")} +{ + tf2_broadcaster_ = std::make_unique(*this); + + // Publishers + predicted_particles_pub_ = create_publisher("~/output/predicted_particles", 10); + pose_pub_ = create_publisher("~/output/pose", 10); + pose_cov_pub_ = create_publisher("~/output/pose_with_covariance", 10); + marker_pub_ = create_publisher("~/debug/init_marker", 10); + + // Subscribers + using std::placeholders::_1; + auto on_initial = std::bind(&Predictor::on_initial_pose, this, _1); + auto on_twist_cov = std::bind(&Predictor::on_twist_cov, this, _1); + auto on_particle = std::bind(&Predictor::on_weighted_particles, this, _1); + auto on_height = [this](std_msgs::msg::Float32 m) -> void { this->ground_height_ = m.data; }; + + initialpose_sub_ = create_subscription("~/input/initialpose", 1, on_initial); + particles_sub_ = + create_subscription("~/input/weighted_particles", 10, on_particle); + height_sub_ = create_subscription("~/input/height", 10, on_height); + twist_cov_sub_ = + create_subscription("~/input/twist_with_covariance", 10, on_twist_cov); + + // Timer callback + const double prediction_rate = declare_parameter("prediction_rate"); + auto on_timer = std::bind(&Predictor::on_timer, this); + timer_ = rclcpp::create_timer( + this, this->get_clock(), rclcpp::Rate(prediction_rate).period(), std::move(on_timer)); + + // Optional modules + if (declare_parameter("visualize", false)) { + visualizer_ptr_ = std::make_unique(*this); + } +} + +void Predictor::on_initial_pose(const PoseCovStamped::ConstSharedPtr initialpose) +{ + // Publish initial pose marker + auto position = initialpose->pose.pose.position; + Eigen::Vector3f pos_vec3f; + pos_vec3f << position.x, position.y, position.z; + + auto orientation = initialpose->pose.pose.orientation; + float theta = 2 * std::atan2(orientation.z, orientation.w); + Eigen::Vector3f tangent; + tangent << std::cos(theta), std::sin(theta), 0; + + publish_range_marker(pos_vec3f, tangent); + + // Rectify initial pose + auto initialpose_rectified = rectify_initial_pose(pos_vec3f, tangent, *initialpose); + + // Initialize particles given the initial pose and its covariance + initialize_particles(initialpose_rectified); +} + +void Predictor::initialize_particles(const PoseCovStamped & initialpose) +{ + RCLCPP_INFO_STREAM(this->get_logger(), "initialize_particles"); + yabloc_particle_filter::msg::ParticleArray particle_array{}; + particle_array.header = initialpose.header; + particle_array.id = 0; + particle_array.particles.resize(number_of_particles_); + + Eigen::Matrix2d cov; + cov(0, 0) = initialpose.pose.covariance[6 * 0 + 0]; + cov(0, 1) = initialpose.pose.covariance[6 * 0 + 1]; + cov(1, 0) = initialpose.pose.covariance[6 * 1 + 0]; + cov(1, 1) = initialpose.pose.covariance[6 * 1 + 1]; + + const double yaw = tf2::getYaw(initialpose.pose.pose.orientation); + const double yaw_std = std::sqrt(initialpose.pose.covariance[6 * 5 + 5]); + + for (auto & particle : particle_array.particles) { + geometry_msgs::msg::Pose pose = initialpose.pose.pose; + const Eigen::Vector2d noise = util::nrand_2d(cov); + pose.position.x += noise.x(); + pose.position.y += noise.y(); + + float noised_yaw = util::normalize_radian(yaw + util::nrand(yaw_std)); + pose.orientation.w = std::cos(noised_yaw / 2.0); + pose.orientation.x = 0.0; + pose.orientation.y = 0.0; + pose.orientation.z = std::sin(noised_yaw / 2.0); + + particle.pose = pose; + particle.weight = 1.0; + } + particle_array_opt_ = particle_array; + + // We have to initialize resampler every particles initialization, + // because resampler has particles resampling history and it will be outdate. + resampler_ptr_ = std::make_unique(number_of_particles_, 100); +} + +void Predictor::on_twist_cov(const TwistCovStamped::ConstSharedPtr twist_cov) +{ + const auto twist = twist_cov->twist; + TwistCovStamped twist_covariance; + twist_covariance.header = twist_cov->header; + twist_covariance.twist.twist = twist.twist; + twist_covariance.twist.covariance.at(0) = static_linear_covariance_; + twist_covariance.twist.covariance.at(7) = 1e4; + twist_covariance.twist.covariance.at(14) = 1e4; + twist_covariance.twist.covariance.at(21) = 1e4; + twist_covariance.twist.covariance.at(28) = 1e4; + twist_covariance.twist.covariance.at(35) = static_angular_covariance_; + latest_twist_opt_ = twist_covariance; +} + +void Predictor::update_with_dynamic_noise( + ParticleArray & particle_array, const TwistCovStamped & twist, double dt) +{ + // linear & angular velocity + const float linear_x = twist.twist.twist.linear.x; + const float angular_z = twist.twist.twist.angular.z; + // standard deviation of linear & angular velocity + const float std_linear_x = std::sqrt(twist.twist.covariance[6 * 0 + 0]); + const float std_angular_z = std::sqrt(twist.twist.covariance[6 * 5 + 5]); + // 1[rad/s] = 60[deg/s] + // 1[m/s] = 3.6[km/h] + const float truncated_angular_std = + std_angular_z * std::clamp(std::sqrt(std::abs(linear_x)), 0.1f, 1.0f); + const float truncated_linear_std = std::clamp(std_linear_x * linear_x, 0.1f, 2.0f); + + for (auto & particle : particle_array.particles) { + Sophus::SE3f se3_pose = common::pose_to_se3(particle.pose); + Eigen::Matrix noised_xi; + noised_xi.setZero(); + noised_xi(0) = linear_x + util::nrand(truncated_linear_std); + noised_xi(5) = angular_z + util::nrand(truncated_angular_std); + se3_pose *= Sophus::SE3f::exp(noised_xi * dt); + + geometry_msgs::msg::Pose pose = common::se3_to_pose(se3_pose); + pose.position.z = ground_height_; + particle.pose = pose; + } +} + +void Predictor::on_timer() +{ + // ========================================================================== + // Pre-check section + // Return if particle_array is not initialized yet + if (!particle_array_opt_.has_value()) { + return; + } + // Return if twist is not subscirbed yet + if (!latest_twist_opt_.has_value()) { + return; + } + // + ParticleArray particle_array = particle_array_opt_.value(); + const rclcpp::Time current_time = this->now(); + const rclcpp::Time msg_time = particle_array.header.stamp; + const double dt = (current_time - msg_time).seconds(); + particle_array.header.stamp = current_time; + + // ========================================================================== + // Prediction section + // NOTE: Sometimes particle_array.header.stamp is ancient due to lagged pose_initializer + if (dt < 0.0 || dt > 1.0) { + RCLCPP_WARN_STREAM(get_logger(), "time stamp is wrong? " << dt); + particle_array_opt_->header.stamp = current_time; + return; + } + + update_with_dynamic_noise(particle_array, latest_twist_opt_.value(), dt); + + // ========================================================================== + // Post-process section + // + predicted_particles_pub_->publish(particle_array); + // + publish_mean_pose(mean_pose(particle_array), this->now()); + // If visualizer exists, + if (visualizer_ptr_) { + visualizer_ptr_->publish(particle_array); + } + + particle_array_opt_ = particle_array; +} + +void Predictor::on_weighted_particles(const ParticleArray::ConstSharedPtr weighted_particles_ptr) +{ + // NOTE: **We need not to check particle_array_opt.has_value().** + // Since the weighted_particles is generated from messages published from this node, + // the particle_array must have an entity in this function. + ParticleArray particle_array = particle_array_opt_.value(); + + // ========================================================================== + // From here, weighting section + try { + particle_array = + resampler_ptr_->add_weight_retroactively(particle_array, *weighted_particles_ptr); + } catch (const resampling_skip_exception & e) { + // Do nothing (just skipping the resample()) + RCLCPP_INFO_STREAM(this->get_logger(), "skipped resampling"); + } + + // ========================================================================== + // From here, resampling section + const double current_time = rclcpp::Time(particle_array.header.stamp).seconds(); + try { + // Exit if previous resampling time is not valid. + if (!previous_resampling_time_opt_.has_value()) { + previous_resampling_time_opt_ = current_time; + throw resampling_skip_exception("previous resampling time is not valid"); + } + + if (current_time - previous_resampling_time_opt_.value() <= resampling_interval_seconds_) { + throw resampling_skip_exception("it is not time to resample"); + } + + particle_array = resampler_ptr_->resample(particle_array); + previous_resampling_time_opt_ = current_time; + } catch (const resampling_skip_exception & e) { + void(); + // Do nothing (just skipping the resample()) + } + + // ========================================================================== + particle_array_opt_ = particle_array; +} + +void Predictor::publish_mean_pose( + const geometry_msgs::msg::Pose & mean_pose, const rclcpp::Time & stamp) +{ + // Publish pose + { + PoseStamped pose_stamped; + pose_stamped.header.stamp = stamp; + pose_stamped.header.frame_id = "map"; + pose_stamped.pose = mean_pose; + pose_pub_->publish(pose_stamped); + } + + // Publish pose with covariance + { + // TODO(KYabuuchi) Use particle distribution someday + PoseCovStamped pose_cov_stamped; + pose_cov_stamped.header.stamp = stamp; + pose_cov_stamped.header.frame_id = "map"; + pose_cov_stamped.pose.pose = mean_pose; + pose_cov_stamped.pose.covariance[6 * 0 + 0] = 0.255; + pose_cov_stamped.pose.covariance[6 * 1 + 1] = 0.255; + pose_cov_stamped.pose.covariance[6 * 2 + 2] = 0.255; + pose_cov_stamped.pose.covariance[6 * 3 + 3] = 0.00625; + pose_cov_stamped.pose.covariance[6 * 4 + 4] = 0.00625; + pose_cov_stamped.pose.covariance[6 * 5 + 5] = 0.00625; + + pose_cov_pub_->publish(pose_cov_stamped); + } + + // Publish TF + { + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = particle_array_opt_->header.stamp; + transform.header.frame_id = "map"; + transform.child_frame_id = "particle_filter"; + transform.transform.translation.x = mean_pose.position.x; + transform.transform.translation.y = mean_pose.position.y; + transform.transform.translation.z = mean_pose.position.z; + transform.transform.rotation = mean_pose.orientation; + tf2_broadcaster_->sendTransform(transform); + } +} + +void Predictor::publish_range_marker(const Eigen::Vector3f & pos, const Eigen::Vector3f & tangent) +{ + Marker msg; + msg.type = Marker::LINE_STRIP; + msg.header.stamp = get_clock()->now(); + msg.header.frame_id = "map"; + msg.scale.x = 0.2; + msg.scale.y = 0.2; + + msg.color.r = 0; + msg.color.g = 1; + msg.color.b = 0; + msg.color.a = 1; + + auto cast2gp = [](const Eigen::Vector3f & vec3f) -> geometry_msgs::msg::Point { + geometry_msgs::msg::Point p; + p.x = vec3f.x(); + p.y = vec3f.y(); + p.z = vec3f.z(); + return p; + }; + + Eigen::Vector3f binormal; + binormal << -tangent.y(), tangent.x(), tangent.z(); + + msg.points.push_back(cast2gp(pos + tangent + binormal)); + msg.points.push_back(cast2gp(pos + tangent - binormal)); + msg.points.push_back(cast2gp(pos - tangent - binormal)); + msg.points.push_back(cast2gp(pos - tangent + binormal)); + msg.points.push_back(cast2gp(pos + tangent + binormal)); + + marker_pub_->publish(msg); +} + +Predictor::PoseCovStamped Predictor::rectify_initial_pose( + const Eigen::Vector3f & pos, const Eigen::Vector3f & tangent, + const PoseCovStamped & raw_initialpose) const +{ + PoseCovStamped msg = raw_initialpose; + msg.header.frame_id = "map"; + msg.pose.pose.position.x = pos.x(); + msg.pose.pose.position.y = pos.y(); + msg.pose.pose.position.z = pos.z(); + + float theta = std::atan2(tangent.y(), tangent.x()); + + msg.pose.pose.orientation.w = std::cos(theta / 2); + msg.pose.pose.orientation.x = 0; + msg.pose.pose.orientation.y = 0; + msg.pose.pose.orientation.z = std::sin(theta / 2); + + Eigen::Matrix2f cov; + cov << cov_xx_yy_.at(0), 0, 0, cov_xx_yy_.at(1); + Eigen::Rotation2D r(theta); + cov = r * cov * r.inverse(); + + msg.pose.covariance.at(6 * 0 + 0) = cov(0, 0); + msg.pose.covariance.at(6 * 0 + 1) = cov(0, 1); + msg.pose.covariance.at(6 * 1 + 0) = cov(1, 0); + msg.pose.covariance.at(6 * 1 + 1) = cov(1, 1); + msg.pose.covariance.at(6 * 5 + 5) = 0.0076; // 0.0076 = (5deg)^2 + + return msg; +} + +} // namespace yabloc::modularized_particle_filter diff --git a/localization/yabloc/yabloc_particle_filter/src/prediction/predictor_node.cpp b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor_node.cpp new file mode 100644 index 0000000000000..81c756908b0f8 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor_node.cpp @@ -0,0 +1,28 @@ +// 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 "yabloc_particle_filter/prediction/predictor.hpp" + +#include + +#include + +int main(int argc, char ** argv) +{ + namespace mpf = yabloc::modularized_particle_filter; + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/localization/yabloc/yabloc_particle_filter/src/prediction/resampler.cpp b/localization/yabloc/yabloc_particle_filter/src/prediction/resampler.cpp new file mode 100644 index 0000000000000..3868dc325a533 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/src/prediction/resampler.cpp @@ -0,0 +1,167 @@ +// 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 "yabloc_particle_filter/prediction/resampler.hpp" + +#include + +#include + +#include +#include +#include + +namespace yabloc::modularized_particle_filter +{ +RetroactiveResampler::RetroactiveResampler(int number_of_particles, int max_history_num) +: max_history_num_(max_history_num), + number_of_particles_(number_of_particles), + logger_(rclcpp::get_logger("modularized_particle_filter.retroactive_resampler")), + resampling_history_(max_history_num_, number_of_particles) +{ + latest_resampling_generation_ = 0; +} + +bool RetroactiveResampler::check_weighted_particles_validity( + const ParticleArray & weighted_particles) const +{ + if (static_cast(weighted_particles.particles.size()) != number_of_particles_) { + return false; + } + + // invalid generation + if (weighted_particles.id < 0) { + RCLCPP_ERROR_STREAM(logger_, "invalid generation id"); + return false; + } + + // future data + if (weighted_particles.id > latest_resampling_generation_) { + RCLCPP_ERROR_STREAM(logger_, "future generation id"); + return false; + } + + // not too old data + if (!(weighted_particles.id > latest_resampling_generation_ - max_history_num_)) { + RCLCPP_WARN_STREAM(logger_, "too old generation id"); + return false; + } + return true; +} + +RetroactiveResampler::ParticleArray RetroactiveResampler::add_weight_retroactively( + const ParticleArray & predicted_particles, const ParticleArray & weighted_particles) +{ + if (!check_weighted_particles_validity(weighted_particles)) { + RCLCPP_ERROR_STREAM(logger_, "weighted_particles has invalid data"); + throw resampling_skip_exception("weighted_particles has invalid data"); + } + + // Initialize corresponding index lookup table + // The m-th addres has the m-th particle's parent index + std::vector index_table(weighted_particles.particles.size()); + std::iota(index_table.begin(), index_table.end(), 0); + + // Lookup corresponding indices + for (int generation = latest_resampling_generation_; generation > weighted_particles.id; + generation--) { + for (int m = 0; m < number_of_particles_; m++) { + index_table[m] = resampling_history_[generation][index_table[m]]; + } + } + + ParticleArray reweighted_particles = predicted_particles; + + // Add weights to current particles + float sum_weight = 0; + for (auto && it : reweighted_particles.particles | boost::adaptors::indexed()) { + it.value().weight *= weighted_particles.particles[index_table[it.index()]].weight; + sum_weight += it.value().weight; + } + + // Normalize all weight + for (auto & particle : reweighted_particles.particles) { + particle.weight /= sum_weight; + } + + return reweighted_particles; +} + +RetroactiveResampler::ParticleArray RetroactiveResampler::resample( + const ParticleArray & predicted_particles) +{ + ParticleArray resampled_particles{predicted_particles}; + latest_resampling_generation_++; + resampled_particles.id = latest_resampling_generation_; + + // Summation of current weights + const double sum_weight = std::accumulate( + predicted_particles.particles.begin(), predicted_particles.particles.end(), 0.0, + [](double weight, const Particle & ps) { return weight + ps.weight; }); + // Inverse of the summation of current weight + const double sum_weight_inv = 1.0 / sum_weight; + // Inverse of the number of particle + const double num_of_particles_inv = 1.0 / static_cast(number_of_particles_); + // A residual term for a random selection of particle sampling thresholds. + // This can range from 0 to 1/(num_of_particles) + const double weight_threshold_residual = random_from_01_uniformly() * num_of_particles_inv; + + if (!std::isfinite(sum_weight_inv)) { + RCLCPP_ERROR_STREAM(logger_, "The inverse of the sum of the weights is not a valid value"); + throw std::runtime_error("weighted_particles has invalid data"); + } + + auto n_th_normalized_weight = [&](int index) -> double { + return predicted_particles.particles.at(index).weight * sum_weight_inv; + }; + + // + int predicted_particle_index = 0; + double accumulated_normzlied_weights = n_th_normalized_weight(0); + // Here, 'm' means resampled_particle_index + for (int m = 0; m < number_of_particles_; m++) { + const double m_th_weight_threshold = m * num_of_particles_inv + weight_threshold_residual; + + // Accumulate predicted particles' weight until it exceeds the threshold + // (If the previous weights are sufficiently large, this accumulation can be skipped over + // several resampled particles. It means that a probable particle will make many successors.) + while (accumulated_normzlied_weights < m_th_weight_threshold) { + predicted_particle_index++; + accumulated_normzlied_weights += n_th_normalized_weight(predicted_particle_index); + } + // Copy particle to resampled variable + resampled_particles.particles[m] = predicted_particles.particles[predicted_particle_index]; + // Reset weight uniformly + resampled_particles.particles[m].weight = num_of_particles_inv; + // Make history + resampling_history_[latest_resampling_generation_][m] = predicted_particle_index; + } + + // NOTE: This check wastes the computation time + if (!resampling_history_.check_history_validity()) { + RCLCPP_ERROR_STREAM(logger_, "resampling_hisotry may be broken"); + throw std::runtime_error("resampling_hisotry may be broken"); + } + + return resampled_particles; +} + +double RetroactiveResampler::random_from_01_uniformly() const +{ + static std::default_random_engine engine(0); + std::uniform_real_distribution dist(0.0, 1.0); + return dist(engine); +} + +} // namespace yabloc::modularized_particle_filter diff --git a/localization/yabloc/yabloc_particle_filter/test/CMakeLists.txt b/localization/yabloc/yabloc_particle_filter/test/CMakeLists.txt new file mode 100644 index 0000000000000..05ded2ee3e1cd --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/test/CMakeLists.txt @@ -0,0 +1,6 @@ +ament_add_gtest( + test_resampler + src/test_resampler.cpp +) +target_include_directories(test_resampler PRIVATE ../include) +target_link_libraries(test_resampler predictor) diff --git a/localization/yabloc/yabloc_particle_filter/test/src/test_resampler.cpp b/localization/yabloc/yabloc_particle_filter/test/src/test_resampler.cpp new file mode 100644 index 0000000000000..5341db01e7608 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/test/src/test_resampler.cpp @@ -0,0 +1,208 @@ +// 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 "yabloc_particle_filter/prediction/resampler.hpp" + +#include + +#include + +namespace mpf = yabloc::modularized_particle_filter; +using Particle = yabloc_particle_filter::msg::Particle; +using ParticleArray = yabloc_particle_filter::msg::ParticleArray; + +constexpr int PARTICLE_COUNT = 10; +constexpr int HISTORY_SIZE = 10; + +TEST(ResamplerTestSuite, outOfHistory) +{ + mpf::RetroactiveResampler resampler(PARTICLE_COUNT, HISTORY_SIZE); + ParticleArray predicted; + ParticleArray weighted; + + predicted.header.stamp = rclcpp::Time(0); + predicted.id = 0; + weighted.id = 0; + predicted.particles.resize(PARTICLE_COUNT); + weighted.particles.resize(PARTICLE_COUNT); + for (auto & p : predicted.particles) p.weight = 1; + for (auto & p : weighted.particles) p.weight = 1; + + // Invalid generation + { + weighted.id = -1; + bool catched = false; + try { + resampler.add_weight_retroactively(predicted, weighted); + } catch (...) { + catched = true; + } + EXPECT_TRUE(catched); + } + + // Future generation + { + weighted.id = 1; + bool catched = false; + try { + resampler.add_weight_retroactively(predicted, weighted); + } catch (...) { + catched = true; + } + EXPECT_TRUE(catched); + } + + // Repease resampling to fill all history + for (int t = 0; t < HISTORY_SIZE; ++t) { + auto resampled = resampler.resample(predicted); + EXPECT_EQ(resampled.id, t + 1); + predicted = resampled; + } + + // Too old generation + { + weighted.id = 0; + bool catched = false; + try { + resampler.add_weight_retroactively(predicted, weighted); + } catch (...) { + catched = true; + } + EXPECT_TRUE(catched); + } +} + +TEST(ResamplerTestSuite, simpleResampling) +{ + mpf::RetroactiveResampler resampler(PARTICLE_COUNT, HISTORY_SIZE); + + ParticleArray predicted; + predicted.header.stamp = rclcpp::Time(0); + predicted.particles.resize(PARTICLE_COUNT); + predicted.id = 0; + for (int i = 0; i < PARTICLE_COUNT; ++i) predicted.particles.at(i).weight = 1; + + ParticleArray weighted; + weighted.particles.resize(PARTICLE_COUNT); + + // Update by uniform distribution + { + // Weight + weighted.id = 0; + for (int i = 0; i < PARTICLE_COUNT; ++i) weighted.particles.at(i).weight = 0.5; + ParticleArray array1 = resampler.add_weight_retroactively(predicted, weighted); + + // All weights must be equal + for (const auto & p : array1.particles) EXPECT_NEAR(p.weight, 1.0 / PARTICLE_COUNT, 1e-3); + + // Resample + predicted = array1; + auto resampled = resampler.resample(predicted); + predicted = resampled; + EXPECT_EQ(predicted.id, 1); + } + + // Update half and half + { + // Weight + weighted.id = 0; + for (int i = 0; i < PARTICLE_COUNT; ++i) { + auto & p = predicted.particles.at(i); + auto & q = weighted.particles.at(i); + if (i < PARTICLE_COUNT / 2) { + p.pose.position.x = 1; + q.weight = 2.0; + } else { + p.pose.position.x = -1; + q.weight = 1.0; + } + } + ParticleArray array1 = resampler.add_weight_retroactively(predicted, weighted); + + // All weight must match with following exepection + for (int i = 0; i < PARTICLE_COUNT; ++i) { + const auto & p = array1.particles.at(i); + if (i < PARTICLE_COUNT / 2) { + EXPECT_NEAR(p.weight, 2.0 / 1.5 / PARTICLE_COUNT, 1e-3f); + } else { + EXPECT_NEAR(p.weight, 1.0 / 1.5 / PARTICLE_COUNT, 1e-3f); + } + } + + // Resample + predicted = array1; + auto resampled = resampler.resample(predicted); + predicted = resampled; + EXPECT_EQ(predicted.id, 2); + + int centroid = 0; + for (const auto & p : predicted.particles) centroid += p.pose.position.x; + EXPECT_GT(centroid, 0); + } +} + +TEST(ResamplerTestSuite, resamplingWithRetrogression) +{ + mpf::RetroactiveResampler resampler(PARTICLE_COUNT, HISTORY_SIZE); + + ParticleArray predicted; + predicted.header.stamp = rclcpp::Time(0); + predicted.particles.resize(PARTICLE_COUNT); + predicted.id = 0; + + for (int i = 0; i < PARTICLE_COUNT; ++i) { + auto & p = predicted.particles.at(i); + p.weight = 1.0; + if (i < PARTICLE_COUNT / 2) + p.pose.position.x = 1; + else + p.pose.position.x = -1; + } + + // Fill all history with biased weighted particles + for (int p = 0; p < HISTORY_SIZE; ++p) { + auto resampled = resampler.resample(predicted); + predicted = resampled; + EXPECT_EQ(predicted.id, p + 1); + } + + // Update by ancient measurement + { + double before_centroid = 0; + for (const auto & p : predicted.particles) { + before_centroid += p.pose.position.x * p.weight; + } + + // Weight + ParticleArray weighted; + weighted.particles.resize(PARTICLE_COUNT); + weighted.id = 1; // ancient generation id + for (int i = 0; i < PARTICLE_COUNT; ++i) { + auto & q = weighted.particles.at(i); + if (i < PARTICLE_COUNT / 2) { + q.weight = 2.0; + } else { + q.weight = 1.0; + } + } + + predicted = resampler.add_weight_retroactively(predicted, weighted); + + double after_centroid = 0; + for (const auto & p : predicted.particles) { + after_centroid += p.pose.position.x * p.weight; + } + EXPECT_TRUE(after_centroid > before_centroid); + } +} diff --git a/localization/yabloc/yabloc_pose_initializer/.gitignore b/localization/yabloc/yabloc_pose_initializer/.gitignore new file mode 100644 index 0000000000000..8fce603003c1e --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/.gitignore @@ -0,0 +1 @@ +data/ diff --git a/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt b/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt new file mode 100644 index 0000000000000..011d215c6ac3a --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt @@ -0,0 +1,59 @@ +cmake_minimum_required(VERSION 3.5) +project(yabloc_pose_initializer) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +# =================================================== +# Eigen3 +find_package(Eigen3 REQUIRED) + +# PCL +find_package(PCL REQUIRED COMPONENTS common kdtree) + +# Sophus +find_package(Sophus REQUIRED) + +# =================================================== +# Download DNN model +include(download.cmake) + +# =================================================== +# Clear ${PYTHON_EXECUTABLE} defined by mrt_cmake_module so that rosidl_generate_interfaces can look for it properly +unset(PYTHON_EXECUTABLE) +message(STATUS "PYTHON_EXECUTABLE: ${PYTHON_EXECUTABLE}") + +# =================================================== +# Service +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/SemanticSegmentation.srv" + DEPENDENCIES + std_msgs + sensor_msgs +) + +# =================================================== +# Executable +# Camera +set(TARGET camera_pose_initializer_node) +ament_auto_add_executable(${TARGET} + src/camera/lane_image.cpp + src/camera/marker_module.cpp + src/camera/projector_module.cpp + src/camera/camera_pose_initializer_core.cpp + src/camera/camera_pose_initializer_node.cpp) +target_include_directories(${TARGET} PUBLIC include) +target_include_directories(${TARGET} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) +rosidl_target_interfaces(${TARGET} ${PROJECT_NAME} "rosidl_typesupport_cpp") +target_link_libraries(${TARGET} ${PCL_LIBRARIES} Sophus::Sophus) + +# Semantic segmentation +install(PROGRAMS + src/semantic_segmentation/semantic_segmentation_core.py + src/semantic_segmentation/semantic_segmentation_node.py + src/semantic_segmentation/semantic_segmentation_server.py + DESTINATION lib/${PROJECT_NAME} +) + +# =================================================== +ament_auto_package(INSTALL_TO_SHARE config data launch) diff --git a/localization/yabloc/yabloc_pose_initializer/README.md b/localization/yabloc/yabloc_pose_initializer/README.md new file mode 100644 index 0000000000000..5a018397b96a7 --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/README.md @@ -0,0 +1,77 @@ +# yabloc_pose_initializer + +This package contains some nodes related to initial pose estimation. + +- [camera_pose_initializer](#camera_pose_initializer) +- [semantic_segmentation_server](#semantic_segmentation_server) + +## Note + +This package makes use of external code. The trained files are provided by apollo. The trained files are automatically downloaded when you build. + +Original model URL + + + +> Open Model Zoo is licensed under Apache License Version 2.0. + +Converted model URL + + + +> model conversion scripts are released under the MIT license + +## Special thanks + +- [openvinotoolkit/open_model_zoo](https://github.com/openvinotoolkit/open_model_zoo) +- [PINTO0309](https://github.com/PINTO0309) + +## camera_pose_initializer + +### Purpose + +- This node estimates the initial position using the camera at the request of ADAPI. + +#### Input + +| Name | Type | Description | +| ------------------- | -------------------------------------------- | ------------------------ | +| `input/camera_info` | `sensor_msgs::msg::CameraInfo` | undistorted camera info | +| `input/image_raw` | `sensor_msgs::msg::Image` | undistorted camera image | +| `input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map | + +#### Output + +| Name | Type | Description | +| ------------------- | -------------------------------------- | ----------------------- | +| `output/candidates` | `visualization_msgs::msg::MarkerArray` | initial pose candidates | + +### Parameters + +| Name | Type | Description | +| ------------------ | ---- | ----------------------------------------- | +| `angle_resolution` | int | how many divisions of 1 sigma angle range | + +### Services + +| Name | Type | Description | +| ------------------ | --------------------------------------------------------- | ------------------------------- | +| `yabloc_align_srv` | `tier4_localization_msgs::srv::PoseWithCovarianceStamped` | initial pose estimation request | + +### Clients + +| Name | Type | Description | +| --------------------------- | ---------------------------------------------------- | ----------------------------- | +| `semantic_segmentation_srv` | `yabloc_pose_initializer::srv::SemanticSegmentation` | semantic segmentation request | + +## semantic_segmentation_server + +### Purpose + +- This node performs semantic segmentation. + +### Services + +| Name | Type | Description | +| --------------------------- | ---------------------------------------------------- | ----------------------------- | +| `semantic_segmentation_srv` | `yabloc_pose_initializer::srv::SemanticSegmentation` | semantic segmentation request | diff --git a/localization/yabloc/yabloc_pose_initializer/config/camera_pose_initializer.param.yaml b/localization/yabloc/yabloc_pose_initializer/config/camera_pose_initializer.param.yaml new file mode 100644 index 0000000000000..a0c38682b3e56 --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/config/camera_pose_initializer.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + angle_resolution: 30 diff --git a/localization/yabloc/yabloc_pose_initializer/download.cmake b/localization/yabloc/yabloc_pose_initializer/download.cmake new file mode 100644 index 0000000000000..a17910156b574 --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/download.cmake @@ -0,0 +1,62 @@ +# Copyright 2023 the Autoware Foundation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set(DATA_URL "https://s3.ap-northeast-2.wasabisys.com/pinto-model-zoo/136_road-segmentation-adas-0001/resources.tar.gz") +set(DATA_PATH "${CMAKE_CURRENT_SOURCE_DIR}/data") +set(FILE_HASH 146ed8af689a30b898dc5369870c40fb) +set(FILE_NAME "resources.tar.gz") + +function(download) + message(STATUS "Checking and downloading ${FILE_NAME}") + set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) + set(STATUS_CODE 0) + message(STATUS "start ${FILE_NAME}") + + if(EXISTS ${FILE_PATH}) + message(STATUS "found ${FILE_NAME}") + file(MD5 ${FILE_PATH} EXISTING_FILE_HASH) + + if(${FILE_HASH} STREQUAL ${EXISTING_FILE_HASH}) + message(STATUS "same ${FILE_NAME}") + message(STATUS "File already exists.") + else() + message(STATUS "diff ${FILE_NAME}") + message(STATUS "File hash changes. Downloading now ...") + file(DOWNLOAD ${DATA_URL} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) + list(GET DOWNLOAD_STATUS 0 STATUS_CODE) + list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) + endif() + else() + message(STATUS "not found ${FILE_NAME}") + message(STATUS "File doesn't exists. Downloading now ...") + file(DOWNLOAD ${DATA_URL} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) + list(GET DOWNLOAD_STATUS 0 STATUS_CODE) + list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) + endif() + + if(${STATUS_CODE} EQUAL 0) + message(STATUS "Download completed successfully!") + else() + message(FATAL_ERROR "Error occurred during download: ${ERROR_MESSAGE}") + endif() +endfunction() + +function(extract) + execute_process(COMMAND + ${CMAKE_COMMAND} -E + tar xzf "${DATA_PATH}/${FILE_NAME}" WORKING_DIRECTORY "${DATA_PATH}") +endfunction() + +download() +extract() diff --git a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp new file mode 100644 index 0000000000000..a5db23f262ff2 --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp @@ -0,0 +1,77 @@ +// 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 YABLOC_POSE_INITIALIZER__CAMERA__CAMERA_POSE_INITIALIZER_HPP_ +#define YABLOC_POSE_INITIALIZER__CAMERA__CAMERA_POSE_INITIALIZER_HPP_ + +#include "yabloc_pose_initializer/camera/lane_image.hpp" +#include "yabloc_pose_initializer/camera/marker_module.hpp" +#include "yabloc_pose_initializer/camera/projector_module.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +namespace yabloc +{ +class CameraPoseInitializer : public rclcpp::Node +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using Image = sensor_msgs::msg::Image; + using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using RequestPoseAlignment = tier4_localization_msgs::srv::PoseWithCovarianceStamped; + using SegmentationSrv = yabloc_pose_initializer::srv::SemanticSegmentation; + + CameraPoseInitializer(); + +private: + const int angle_resolution_; + std::unique_ptr lane_image_{nullptr}; + std::unique_ptr marker_module_{nullptr}; + std::unique_ptr projector_module_{nullptr}; + + rclcpp::Subscription::SharedPtr sub_initialpose_; + rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Subscription::SharedPtr sub_image_; + + rclcpp::Service::SharedPtr align_server_; + rclcpp::Client::SharedPtr segmentation_client_; + rclcpp::CallbackGroup::SharedPtr service_callback_group_; + + std::optional latest_image_msg_{std::nullopt}; + lanelet::ConstLanelets const_lanelets_; + + void on_map(const HADMapBin & msg); + void on_service( + const RequestPoseAlignment::Request::SharedPtr, + RequestPoseAlignment::Response::SharedPtr request); + PoseCovStamped create_rectified_initial_pose( + const Eigen::Vector3f & pos, double yaw_angle_rad, const PoseCovStamped & src_msg); + + bool estimate_pose(const Eigen::Vector3f & position, double & yaw_angle_rad, double yaw_std_rad); +}; +} // namespace yabloc + +#endif // YABLOC_POSE_INITIALIZER__CAMERA__CAMERA_POSE_INITIALIZER_HPP_ diff --git a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/lane_image.hpp b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/lane_image.hpp new file mode 100644 index 0000000000000..152f6b6c737fd --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/lane_image.hpp @@ -0,0 +1,46 @@ +// 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 YABLOC_POSE_INITIALIZER__CAMERA__LANE_IMAGE_HPP_ +#define YABLOC_POSE_INITIALIZER__CAMERA__LANE_IMAGE_HPP_ + +#include +#include + +#include + +#include + +#include + +namespace yabloc +{ +class LaneImage +{ +public: + using Pose = geometry_msgs::msg::Pose; + using SharedPtr = std::shared_ptr; + + explicit LaneImage(lanelet::LaneletMapPtr map); + + cv::Mat get_image(const Pose & pose); + + cv::Mat create_vectormap_image(const Eigen::Vector3f & position); + +private: + lanelet::LaneletMapPtr map_; +}; +} // namespace yabloc + +#endif // YABLOC_POSE_INITIALIZER__CAMERA__LANE_IMAGE_HPP_ diff --git a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/marker_module.hpp b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/marker_module.hpp new file mode 100644 index 0000000000000..e6385cac934b3 --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/marker_module.hpp @@ -0,0 +1,43 @@ +// 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 YABLOC_POSE_INITIALIZER__CAMERA__MARKER_MODULE_HPP_ +#define YABLOC_POSE_INITIALIZER__CAMERA__MARKER_MODULE_HPP_ + +#include +#include + +#include + +#include + +namespace yabloc::initializer +{ +class MarkerModule +{ +public: + using Marker = visualization_msgs::msg::Marker; + using MarkerArray = visualization_msgs::msg::MarkerArray; + explicit MarkerModule(rclcpp::Node * node); + + void publish_marker( + const std::vector & scores, const std::vector & angles, + const Eigen::Vector3f & position); + +private: + rclcpp::Publisher::SharedPtr pub_marker_; +}; +} // namespace yabloc::initializer + +#endif // YABLOC_POSE_INITIALIZER__CAMERA__MARKER_MODULE_HPP_ diff --git a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/projector_module.hpp b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/projector_module.hpp new file mode 100644 index 0000000000000..cc084990a0ff3 --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/projector_module.hpp @@ -0,0 +1,46 @@ +// 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 YABLOC_POSE_INITIALIZER__CAMERA__PROJECTOR_MODULE_HPP_ +#define YABLOC_POSE_INITIALIZER__CAMERA__PROJECTOR_MODULE_HPP_ + +#include +#include +#include +#include +#include + +#include + +namespace yabloc::initializer +{ +class ProjectorModule +{ +public: + using ProjectFunc = std::function(const cv::Point2i &)>; + explicit ProjectorModule(rclcpp::Node * node); + + bool define_project_func(); + + cv::Mat project_image(const sensor_msgs::msg::Image & image_msg); + +private: + common::CameraInfoSubscriber info_; + common::StaticTfSubscriber tf_subscriber_; + rclcpp::Logger logger_; + ProjectFunc project_func_ = nullptr; +}; +} // namespace yabloc::initializer + +#endif // YABLOC_POSE_INITIALIZER__CAMERA__PROJECTOR_MODULE_HPP_ diff --git a/localization/yabloc/yabloc_pose_initializer/launch/semantic_segmentation.launch.xml b/localization/yabloc/yabloc_pose_initializer/launch/semantic_segmentation.launch.xml new file mode 100644 index 0000000000000..66c80f7983484 --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/launch/semantic_segmentation.launch.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/localization/yabloc/yabloc_pose_initializer/launch/yabloc_pose_initializer.launch.xml b/localization/yabloc/yabloc_pose_initializer/launch/yabloc_pose_initializer.launch.xml new file mode 100644 index 0000000000000..3402f9ccc7984 --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/launch/yabloc_pose_initializer.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/localization/yabloc/yabloc_pose_initializer/package.xml b/localization/yabloc/yabloc_pose_initializer/package.xml new file mode 100644 index 0000000000000..9adfc2a85f663 --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/package.xml @@ -0,0 +1,35 @@ + + + yabloc_pose_initializer + 0.0.0 + The pose initializer + Kento Yabuuchi + Apache License 2.0 + + ament_cmake_ros + autoware_cmake + rosidl_default_generators + + autoware_auto_mapping_msgs + geometry_msgs + lanelet2_extension + rclcpp + rclpy + sensor_msgs + std_msgs + std_srvs + tf2 + tier4_localization_msgs + visualization_msgs + yabloc_common + + ament_lint_auto + autoware_lint_common + + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp new file mode 100644 index 0000000000000..a1a14d254e9dc --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp @@ -0,0 +1,229 @@ +// 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 "yabloc_pose_initializer/camera/camera_pose_initializer.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace yabloc +{ +CameraPoseInitializer::CameraPoseInitializer() +: Node("camera_pose_initializer"), angle_resolution_(declare_parameter("angle_resolution")) +{ + using std::placeholders::_1; + using std::placeholders::_2; + const rclcpp::QoS map_qos = rclcpp::QoS(1).transient_local().reliable(); + service_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + marker_module_ = std::make_unique(this); + projector_module_ = std::make_unique(this); + + // Subscriber + auto on_map = std::bind(&CameraPoseInitializer::on_map, this, _1); + auto on_image = [this](Image::ConstSharedPtr msg) -> void { latest_image_msg_ = msg; }; + sub_map_ = create_subscription("~/input/vector_map", map_qos, on_map); + sub_image_ = create_subscription("~/input/image_raw", 10, on_image); + + // Client + segmentation_client_ = create_client( + "~/semantic_segmentation_srv", rmw_qos_profile_services_default, service_callback_group_); + + // Server + auto on_service = std::bind(&CameraPoseInitializer::on_service, this, _1, _2); + align_server_ = create_service("~/yabloc_align_srv", on_service); + + using namespace std::literals::chrono_literals; + while (!segmentation_client_->wait_for_service(1s) && rclcpp::ok()) { + RCLCPP_INFO_STREAM(get_logger(), "Waiting for " << segmentation_client_->get_service_name()); + } +} + +cv::Mat bitwise_and_3ch(const cv::Mat src1, const cv::Mat src2) +{ + std::vector src1_array; + std::vector src2_array; + cv::split(src1, src1_array); + cv::split(src2, src2_array); + std::vector dst_array; + for (int i = 0; i < 3; i++) { + cv::Mat dst; + cv::bitwise_and(src1_array.at(i), src2_array.at(i), dst); + dst_array.push_back(dst); + } + cv::Mat merged; + cv::merge(dst_array, merged); + return merged; +} + +int count_nonzero(cv::Mat image_3ch) +{ + std::vector images; + cv::split(image_3ch, images); + int count = 0; + for (int i = 0; i < 3; i++) { + count += cv::countNonZero(images.at(i)); + } + return count; +} + +bool CameraPoseInitializer::estimate_pose( + const Eigen::Vector3f & position, double & yaw_angle_rad, double yaw_std_rad) +{ + if (!projector_module_->define_project_func()) { + return false; + } + if (!lane_image_) { + RCLCPP_WARN_STREAM(get_logger(), "vector map is not ready "); + return false; + } + // TODO(KYabuuchi) check latest_image_msg's time stamp, too + if (!latest_image_msg_.has_value()) { + RCLCPP_WARN_STREAM(get_logger(), "source image is not ready"); + return false; + } + + Image segmented_image; + { + // Call semantic segmentation service + auto request = std::make_shared(); + request->src_image = *latest_image_msg_.value(); + auto result_future = segmentation_client_->async_send_request(request); + using namespace std::literals::chrono_literals; + std::future_status status = result_future.wait_for(1000ms); + if (status == std::future_status::ready) { + segmented_image = result_future.get()->dst_image; + } else { + RCLCPP_ERROR_STREAM(get_logger(), "segmentation service exited unexpectedly"); + return false; + } + } + + geometry_msgs::msg::Pose query_pose; + query_pose.position.x = position.x(); + query_pose.position.y = position.y(); + query_pose.position.z = position.z(); + + lanelet::ConstLanelets current_lanelets; + std::optional lane_angle_rad = std::nullopt; + if (lanelet::utils::query::getCurrentLanelets(const_lanelets_, query_pose, ¤t_lanelets)) { + lane_angle_rad = lanelet::utils::getLaneletAngle(current_lanelets.front(), query_pose.position); + } + + cv::Mat projected_image = projector_module_->project_image(segmented_image); + cv::Mat vectormap_image = lane_image_->create_vectormap_image(position); + + std::vector scores; + std::vector angles_rad; + + for (int i = -angle_resolution_; i < angle_resolution_; i++) { + const double angle_rad = + yaw_angle_rad + yaw_std_rad * static_cast(i) / static_cast(angle_resolution_); + const double angle_deg = angle_rad * 180. / M_PI; + + cv::Mat rot = cv::getRotationMatrix2D(cv::Point2f(400, 400), angle_deg, 1); + cv::Mat rotated_image; + cv::warpAffine(projected_image, rotated_image, rot, vectormap_image.size()); + cv::Mat dst = bitwise_and_3ch(rotated_image, vectormap_image); + + // consider lanelet direction + float gain = 1; + if (lane_angle_rad) { + gain = 2 + std::cos((lane_angle_rad.value() - angle_rad) / 2.0); + } + const float score = gain * count_nonzero(dst); + + // DEBUG: + constexpr bool imshow = false; + if (imshow) { + cv::Mat show_image; + cv::hconcat(std::vector{rotated_image, vectormap_image, dst}, show_image); + cv::imshow("and operator", show_image); + cv::waitKey(50); + } + + scores.push_back(score); + angles_rad.push_back(angle_rad); + } + + { + size_t max_index = + std::distance(scores.begin(), std::max_element(scores.begin(), scores.end())); + yaw_angle_rad = angles_rad.at(max_index); + } + + marker_module_->publish_marker(scores, angles_rad, position); + + return true; +} + +void CameraPoseInitializer::on_map(const HADMapBin & msg) +{ + lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); + lanelet::utils::conversion::fromBinMsg(msg, lanelet_map); + lane_image_ = std::make_unique(lanelet_map); + + const_lanelets_.clear(); + for (auto l : lanelet_map->laneletLayer) { + const_lanelets_.push_back(l); + } +} + +void CameraPoseInitializer::on_service( + const RequestPoseAlignment::Request::SharedPtr request, + RequestPoseAlignment::Response::SharedPtr response) +{ + RCLCPP_INFO_STREAM(get_logger(), "CameraPoseInitializer on_service"); + + response->success = false; + + const auto query_pos_with_cov = request->pose_with_covariance; + const auto query_pos = request->pose_with_covariance.pose.pose.position; + const auto orientation = request->pose_with_covariance.pose.pose.orientation; + const double yaw_std_rad = std::sqrt(query_pos_with_cov.pose.covariance.at(35)); + const Eigen::Vector3f pos_vec3f(query_pos.x, query_pos.y, query_pos.z); + RCLCPP_INFO_STREAM(get_logger(), "Given initial position " << pos_vec3f.transpose()); + + // Estimate orientation + const auto header = request->pose_with_covariance.header; + double yaw_angle_rad = 2 * std::atan2(orientation.z, orientation.w); + if (estimate_pose(pos_vec3f, yaw_angle_rad, yaw_std_rad)) { + response->success = true; + response->pose_with_covariance = + create_rectified_initial_pose(pos_vec3f, yaw_angle_rad, query_pos_with_cov); + } +} + +CameraPoseInitializer::PoseCovStamped CameraPoseInitializer::create_rectified_initial_pose( + const Eigen::Vector3f & pos, double yaw_angle_rad, const PoseCovStamped & src_msg) +{ + PoseCovStamped msg = src_msg; + msg.pose.pose.position.x = pos.x(); + msg.pose.pose.position.y = pos.y(); + msg.pose.pose.position.z = pos.z(); + msg.pose.pose.orientation.w = std::cos(yaw_angle_rad / 2.); + msg.pose.pose.orientation.x = 0; + msg.pose.pose.orientation.y = 0; + msg.pose.pose.orientation.z = std::sin(yaw_angle_rad / 2.); + return msg; +} + +} // namespace yabloc diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_node.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_node.cpp new file mode 100644 index 0000000000000..2bcfe073351f8 --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_node.cpp @@ -0,0 +1,26 @@ +// 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 "yabloc_pose_initializer/camera/camera_pose_initializer.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/lane_image.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/lane_image.cpp new file mode 100644 index 0000000000000..702be59f07674 --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/lane_image.cpp @@ -0,0 +1,111 @@ +// 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 "yabloc_pose_initializer/camera/lane_image.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +namespace yabloc +{ +namespace bg = boost::geometry; + +typedef bg::model::d2::point_xy point_t; +typedef bg::model::box box_t; +typedef bg::model::polygon polygon_t; + +LaneImage::LaneImage(lanelet::LaneletMapPtr map) : map_(map) +{ +} + +cv::Point2i to_cv_point(const Eigen::Vector3f & v) +{ + const float image_size_ = 800; + const float max_range_ = 30; + + cv::Point pt; + pt.x = -v.y() / max_range_ * image_size_ * 0.5f + image_size_ / 2.f; + pt.y = -v.x() / max_range_ * image_size_ * 0.5f + image_size_ / 2.f; + return pt; +} + +cv::Mat LaneImage::create_vectormap_image(const Eigen::Vector3f & position) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = position.x(); + pose.position.y = position.y(); + pose.position.z = position.z(); + return get_image(pose); +} + +void draw_lane(cv::Mat & image, const polygon_t & polygon) +{ + std::vector contour; + for (auto p : polygon.outer()) { + cv::Point2i pt = to_cv_point(Eigen::Vector3f(p.x(), p.y(), 0)); + contour.push_back(pt); + } + + std::vector > contours; + contours.push_back(contour); + cv::drawContours(image, contours, -1, cv::Scalar(255, 0, 0), -1); +} + +void draw_line(cv::Mat & image, const lanelet::LineString2d & line, geometry_msgs::msg::Point xyz) +{ + std::vector contour; + for (auto p : line) { + cv::Point2i pt = to_cv_point(Eigen::Vector3f(p.x() - xyz.x, p.y() - xyz.y, 0)); + contour.push_back(pt); + } + cv::polylines(image, contour, false, cv::Scalar(0, 0, 255), 2); +} + +cv::Mat LaneImage::get_image(const Pose & pose) +{ + const auto xyz = pose.position; + box_t box(point_t(-20, -20), point_t(20, 20)); + + cv::Mat image = cv::Mat::zeros(cv::Size(800, 800), CV_8UC3); + + std::vector joint_lanes; + for (auto lanelet : map_->laneletLayer) { + polygon_t polygon; + for (auto right : lanelet.rightBound2d()) { + polygon.outer().push_back(point_t(right.x() - xyz.x, right.y() - xyz.y)); + } + for (auto left : boost::adaptors::reverse(lanelet.leftBound2d())) { + polygon.outer().push_back(point_t(left.x() - xyz.x, left.y() - xyz.y)); + } + + if (!bg::disjoint(box, polygon)) { + joint_lanes.push_back(lanelet); + draw_lane(image, polygon); + } + } + for (auto lanelet : joint_lanes) { + draw_line(image, lanelet.rightBound2d(), xyz); + draw_line(image, lanelet.leftBound2d(), xyz); + } + + return image; +} + +} // namespace yabloc diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp new file mode 100644 index 0000000000000..95d948e86a7bc --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp @@ -0,0 +1,65 @@ +// 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 "yabloc_pose_initializer/camera/marker_module.hpp" + +#include + +namespace yabloc::initializer +{ + +MarkerModule::MarkerModule(rclcpp::Node * node) +{ + pub_marker_ = node->create_publisher("~/debug/init_candidates", 10); +} + +void MarkerModule::publish_marker( + const std::vector & scores, const std::vector & angles, + const Eigen::Vector3f & position) +{ + const int N = scores.size(); + auto minmax = std::minmax_element(scores.begin(), scores.end()); + auto normalize = [minmax](int score) -> float { + return static_cast(score - *minmax.first) / + static_cast(*minmax.second - *minmax.first); + }; + + MarkerArray array; + for (int i = 0; i < N; i++) { + Marker marker; + marker.header.frame_id = "map"; + marker.type = Marker::ARROW; + marker.id = i; + marker.ns = "arrow"; + marker.color = common::color_scale::rainbow(normalize(scores.at(i))); + marker.color.a = 0.5; + + marker.pose.position.x = position.x(); + marker.pose.position.y = position.y(); + marker.pose.position.z = position.z(); + + const float rad = angles.at(i); + marker.pose.orientation.w = std::cos(rad / 2.f); + marker.pose.orientation.z = std::sin(rad / 2.f); + + marker.scale.x = 2.0; // arrow length + marker.scale.y = 0.2; // arrow width + marker.scale.z = 0.3; // arrow height + + array.markers.push_back(marker); + } + pub_marker_->publish(array); +} + +} // namespace yabloc::initializer diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/projector_module.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/projector_module.cpp new file mode 100644 index 0000000000000..691fe85bbe201 --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/projector_module.cpp @@ -0,0 +1,107 @@ +// 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 "yabloc_pose_initializer/camera/projector_module.hpp" + +#include +#include +#include + +namespace yabloc::initializer +{ +ProjectorModule::ProjectorModule(rclcpp::Node * node) +: info_(node), tf_subscriber_(node->get_clock()), logger_(node->get_logger()) +{ +} + +cv::Point2i to_cv_point(const Eigen::Vector3f & v) +{ + const float image_size_ = 800; + const float max_range_ = 30; + + cv::Point pt; + pt.x = -v.y() / max_range_ * image_size_ * 0.5f + image_size_ / 2.f; + pt.y = -v.x() / max_range_ * image_size_ * 0.5f + image_size_ / 2.f; + return pt; +} + +cv::Mat ProjectorModule::project_image(const sensor_msgs::msg::Image & image_msg) +{ + cv::Mat mask_image = common::decompress_to_cv_mat(image_msg); + + // project semantics on plane + std::vector masks; + cv::split(mask_image, masks); + std::vector colors = { + cv::Scalar(255, 0, 0), cv::Scalar(0, 255, 0), cv::Scalar(0, 0, 255)}; + + cv::Mat projected_image = cv::Mat::zeros(cv::Size(800, 800), CV_8UC3); + for (int i = 0; i < 3; i++) { + std::vector > contours; + cv::findContours(masks[i], contours, cv::RETR_LIST, cv::CHAIN_APPROX_NONE); + + std::vector > projected_contours; + for (auto contour : contours) { + std::vector projected; + for (auto c : contour) { + auto opt = project_func_(c); + if (!opt.has_value()) continue; + + cv::Point2i pt = to_cv_point(opt.value()); + projected.push_back(pt); + } + if (projected.size() > 2) { + projected_contours.push_back(projected); + } + } + cv::drawContours(projected_image, projected_contours, -1, colors[i], -1); + } + return projected_image; +} + +bool ProjectorModule::define_project_func() +{ + if (project_func_) return true; + + if (info_.is_camera_info_nullopt()) { + RCLCPP_WARN_STREAM(logger_, "camera info is not ready"); + return false; + } + Eigen::Matrix3f Kinv = info_.intrinsic().inverse(); + + std::optional camera_extrinsic = + tf_subscriber_(info_.get_frame_id(), "base_link"); + if (!camera_extrinsic.has_value()) { + RCLCPP_WARN_STREAM(logger_, "camera tf_static is not ready"); + return false; + } + + const Eigen::Vector3f t = camera_extrinsic->translation(); + const Eigen::Quaternionf q(camera_extrinsic->rotation()); + + // TODO(KYabuuchi) This will take into account ground tilt and camera vibration someday. + project_func_ = [Kinv, q, t](const cv::Point & u) -> std::optional { + Eigen::Vector3f u3(u.x, u.y, 1); + Eigen::Vector3f u_bearing = (q * Kinv * u3).normalized(); + if (u_bearing.z() > -0.01) return std::nullopt; + float u_distance = -t.z() / u_bearing.z(); + Eigen::Vector3f v; + v.x() = t.x() + u_bearing.x() * u_distance; + v.y() = t.y() + u_bearing.y() * u_distance; + v.z() = 0; + return v; + }; + return true; +} +} // namespace yabloc::initializer diff --git a/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_core.py b/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_core.py new file mode 100755 index 0000000000000..70d116fe0b295 --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_core.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python3 + +# 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. + +import copy + +import cv2 +import numpy as np + + +class SemanticSegmentationCore: + def __init__(self, model_path): + self.net_ = cv2.dnn.readNet(model_path) + self.net_.setPreferableBackend(cv2.dnn.DNN_BACKEND_OPENCV) + self.net_.setPreferableTarget(cv2.dnn.DNN_TARGET_CPU) + + def makeBlob(self, image: np.ndarray) -> np.ndarray: + scale = 1.0 + size = (896, 512) + mean = (0, 0, 0) + swap = True + crop = False + depth = cv2.CV_32F + return cv2.dnn.blobFromImage(image, scale, size, mean, swap, crop, depth) + + def inference(self, image: np.ndarray, score_threshold=0.5) -> np.ndarray: + blob = self.makeBlob(image) + self.net_.setInput(blob) + output_layers = self.net_.getUnconnectedOutLayersNames() + mask = self.net_.forward(output_layers)[0] + mask = np.squeeze(mask).transpose(1, 2, 0) + + mask = cv2.resize( + mask, dsize=(image.shape[1], image.shape[0]), interpolation=cv2.INTER_LINEAR + ) + + return self.__normalize(mask, score_threshold) + + def __normalize(self, mask, score_threshold=0.5) -> np.ndarray: + masks = cv2.split(mask)[1:] + bin_masks = [] + for mask in masks: + bin_mask = np.where(mask > score_threshold, 0, 1).astype("uint8") + bin_masks.append(255 - 255 * bin_mask) + return cv2.merge(bin_masks) + + def drawOverlay(self, image, segmentation) -> np.ndarray: + overlay_image = copy.deepcopy(image) + return cv2.addWeighted(overlay_image, 0.5, segmentation, 0.5, 1.0) + + +def main(): + print("This script is not intended to be run alone.") + + +if __name__ == "__main__": + main() diff --git a/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_node.py b/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_node.py new file mode 100755 index 0000000000000..9b486f16c01e0 --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_node.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python3 + +# 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. + +import sys +import time + +import cv2 +from cv_bridge import CvBridge +import rclpy +from rclpy.node import Node +import semantic_segmentation_core as core +from sensor_msgs.msg import Image + + +class SemanticSegmentationNode(Node): + def __init__(self): + super().__init__("semantic_segmentation_node") + + model_path = self.declare_parameter("model_path", "").value + self.imshow_ = self.declare_parameter("imshow", False).value + + self.get_logger().info("model path: " + model_path) + + self.sub_image_ = self.create_subscription( + Image, "~/input/image_raw", self.imageCallback, 10 + ) + + self.pub_overlay_image_ = self.create_publisher(Image, "~/output/overlay_image", 10) + self.pub_image_ = self.create_publisher(Image, "~/output/semantic_image", 10) + + self.dnn_ = core.SemanticSegmentationCore(model_path) + self.bridge_ = CvBridge() + + def imageCallback(self, msg: Image): + stamp = msg.header.stamp + self.get_logger().info("Subscribed image: " + str(stamp)) + + src_image = self.bridge_.imgmsg_to_cv2(msg) + start_time = time.time() + mask = self.dnn_.inference(src_image) + elapsed_time = time.time() - start_time + + show_image = self.dnn_.drawOverlay(src_image, mask) + cv2.putText( + show_image, + "Inference: " + "{:.1f}".format(elapsed_time * 1000) + "ms", + (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, + 0.7, + (0, 255, 255), + 2, + cv2.LINE_AA, + ) + + # visualize + if self.imshow_: + cv2.imshow("segmentation", show_image) + cv2.waitKey(1) + + # publish dst image + self.__publish_image(mask, self.pub_image_) + self.__publish_image(show_image, self.pub_overlay_image_) + + def __publish_image(self, image, publisher): + out_msg = self.bridge_.cv2_to_imgmsg(image) + out_msg.encoding = "bgr8" + publisher.publish(out_msg) + + +def main(): + rclpy.init(args=sys.argv) + + segmentation_node = SemanticSegmentationNode() + rclpy.spin(segmentation_node) + segmentation_node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_server.py b/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_server.py new file mode 100755 index 0000000000000..e7d212a9758aa --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_server.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python3 + +# 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. + +import sys + +from cv_bridge import CvBridge +import rclpy +from rclpy.node import Node +import semantic_segmentation_core as core +from sensor_msgs.msg import Image +from yabloc_pose_initializer.srv import SemanticSegmentation + + +class SemanticSegmentationServer(Node): + def __init__(self): + super().__init__("segmentation_server_node") + + model_path = self.declare_parameter("model_path", "").value + + self.get_logger().info("model path: " + model_path) + self.dnn_ = core.SemanticSegmentationCore(model_path) + self.bridge_ = CvBridge() + + self.srv = self.create_service( + SemanticSegmentation, "semantic_segmentation_srv", self.on_service + ) + + def on_service(self, request, response): + response.dst_image = self.__inference(request.src_image) + return response + + def __inference(self, msg: Image): + stamp = msg.header.stamp + self.get_logger().info("Subscribed image: " + str(stamp)) + + src_image = self.bridge_.imgmsg_to_cv2(msg) + mask = self.dnn_.inference(src_image) + + dst_msg = self.bridge_.cv2_to_imgmsg(mask) + dst_msg.encoding = "bgr8" + return dst_msg + + +def main(): + rclpy.init(args=sys.argv) + + server_node = SemanticSegmentationServer() + rclpy.spin(server_node) + server_node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/localization/yabloc/yabloc_pose_initializer/srv/SemanticSegmentation.srv b/localization/yabloc/yabloc_pose_initializer/srv/SemanticSegmentation.srv new file mode 100644 index 0000000000000..b0fef18afdcb0 --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/srv/SemanticSegmentation.srv @@ -0,0 +1,3 @@ +sensor_msgs/Image src_image +--- +sensor_msgs/Image dst_image