diff --git a/perception/radar_object_tracker/CMakeLists.txt b/perception/radar_object_tracker/CMakeLists.txt new file mode 100644 index 0000000000000..424e2b2d2c13e --- /dev/null +++ b/perception/radar_object_tracker/CMakeLists.txt @@ -0,0 +1,54 @@ +cmake_minimum_required(VERSION 3.8) +project(radar_object_tracker) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +### Find Eigen Dependencies +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(nlohmann_json REQUIRED) # for debug + +include_directories( + SYSTEM + ${EIGEN3_INCLUDE_DIR} +) + +# Targets +ament_auto_add_library(mu_successive_shortest_path SHARED + src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp +) + +ament_auto_add_library(radar_object_tracker_node SHARED + src/radar_object_tracker_node/radar_object_tracker_node.cpp + src/tracker/model/tracker_base.cpp + src/tracker/model/linear_motion_tracker.cpp + src/utils/utils.cpp + src/data_association/data_association.cpp +) + +target_link_libraries(radar_object_tracker_node + mu_successive_shortest_path + Eigen3::Eigen + yaml-cpp + nlohmann_json::nlohmann_json # for debug +) + +rclcpp_components_register_node(radar_object_tracker_node + PLUGIN "RadarObjectTrackerNode" + EXECUTABLE radar_object_tracker +) + +# testing +# todo + +# Package +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/perception/radar_object_tracker/README.md b/perception/radar_object_tracker/README.md new file mode 100644 index 0000000000000..9f11d50ce2b3f --- /dev/null +++ b/perception/radar_object_tracker/README.md @@ -0,0 +1,76 @@ +# Radar Object Tracker + +## Purpose + +This package provides a radar object tracking node that processes sequences of detected objects to assign consistent identities to them and estimate their velocities. + +## Inner-workings / Algorithms + +This radar object tracker is a combination of data association and tracking algorithms. + + + + +### Data Association + +The data association algorithm matches detected objects to existing tracks. + +### Tracker Models + +The tracker models used in this package vary based on the class of the detected object. +See more details in the [models.md](models.md). + + + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| --------- | ----------------------------------------------------- | ---------------- | +| `~/input` | `autoware_auto_perception_msgs::msg::DetectedObjects` | Detected objects | + +### Output + +| Name | Type | Description | +| ---------- | ---------------------------------------------------- | --------------- | +| `~/output` | `autoware_auto_perception_msgs::msg::TrackedObjects` | Tracked objects | + +## Parameters + +### Node Parameters + +| Name | Type | Default Value | Description | +| --------------------------- | ------ | ----------------------------- | ----------------------------------------------------------- | +| `publish_rate` | double | 30.0 | The rate at which to publish the output messages | +| `world_frame_id` | string | "world" | The frame ID of the world coordinate system | +| `enable_delay_compensation` | bool | false | Whether to enable delay compensation | +| `tracking_config_directory` | string | "" | The directory containing the tracking configuration files | +| `enable_logging` | bool | false | Whether to enable logging | +| `logging_file_path` | string | "~/.ros/association_log.json" | The path to the file where logs should be written | +| `can_assign_matrix` | array | | An array of integers used in the data association algorithm | +| `max_dist_matrix` | array | | An array of doubles used in the data association algorithm | +| `max_area_matrix` | array | | An array of doubles used in the data association algorithm | +| `min_area_matrix` | array | | An array of doubles used in the data association algorithm | +| `max_rad_matrix` | array | | An array of doubles used in the data association algorithm | +| `min_iou_matrix` | array | | An array of doubles used in the data association algorithm | + +## Assumptions / Known limits + + + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## (Optional) References/External links + + + +## (Optional) Future extensions / Unimplemented parts + + diff --git a/perception/radar_object_tracker/config/data_association_matrix.param.yaml b/perception/radar_object_tracker/config/data_association_matrix.param.yaml new file mode 100644 index 0000000000000..104d790d185db --- /dev/null +++ b/perception/radar_object_tracker/config/data_association_matrix.param.yaml @@ -0,0 +1,66 @@ +/**: + ros__parameters: + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN <-Measurement + [1, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN <-Tracker + 0, 1, 1, 1, 1, 0, 0, 0, #CAR + 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK + 0, 1, 1, 1, 1, 0, 0, 0, #BUS + 0, 1, 1, 1, 1, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN + + max_dist_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [4.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, #UNKNOWN + 4.0, 4.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #CAR + 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRUCK + 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #BUS + 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRAILER + 3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #MOTORBIKE + 3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #BICYCLE + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0] #PEDESTRIAN + max_area_matrix: + # NOTE: The size of truck is 12 m length x 3 m width. + # NOTE: The size of trailer is 20 m length x 3 m width. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [100.00, 100.00, 100.00, 100.00, 100.00, 100.00, 100.00, 100.00, #UNKNOWN + 12.10, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #CAR + 36.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRUCK + 60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #BUS + 60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRAILER + 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #MOTORBIKE + 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #BICYCLE + 2.00, 10000.00, 10000.00, 10000.00, 10000.00, 1.50, 1.50, 1.00] #PEDESTRIAN + min_area_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN + 3.600, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #CAR + 6.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #TRUCK + 10.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #BUS + 10.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #TRAILER + 0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, #MOTORBIKE + 0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, #BICYCLE + 0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100] #PEDESTRIAN + max_rad_matrix: # If value is greater than pi, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN + [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN + + min_iou_matrix: # If value is negative, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [0.0001, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN + 0.1, -0.1, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, #CAR + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRUCK + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #BUS + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRAILER + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #MOTORBIKE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #BICYCLE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.0001] #PEDESTRIAN diff --git a/perception/radar_object_tracker/config/default_tracker.param.yaml b/perception/radar_object_tracker/config/default_tracker.param.yaml new file mode 100644 index 0000000000000..757125202d69b --- /dev/null +++ b/perception/radar_object_tracker/config/default_tracker.param.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + car_tracker: "linear_motion_tracker" + truck_tracker: "linear_motion_tracker" + bus_tracker: "linear_motion_tracker" + trailer_tracker: "linear_motion_tracker" + pedestrian_tracker: "linear_motion_tracker" + bicycle_tracker: "linear_motion_tracker" + motorcycle_tracker: "linear_motion_tracker" diff --git a/perception/radar_object_tracker/config/simulation_tracker.param.yaml b/perception/radar_object_tracker/config/simulation_tracker.param.yaml new file mode 100644 index 0000000000000..325cfacc9a3a3 --- /dev/null +++ b/perception/radar_object_tracker/config/simulation_tracker.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + car_tracker: "pass_through_tracker" + truck_tracker: "pass_through_tracker" + bus_tracker: "pass_through_tracker" + pedestrian_tracker: "pass_through_tracker" + bicycle_tracker: "pass_through_tracker" + motorcycle_tracker: "pass_through_tracker" diff --git a/perception/radar_object_tracker/config/tracking/linear_motion_tracker.yaml b/perception/radar_object_tracker/config/tracking/linear_motion_tracker.yaml new file mode 100644 index 0000000000000..71367f4575193 --- /dev/null +++ b/perception/radar_object_tracker/config/tracking/linear_motion_tracker.yaml @@ -0,0 +1,32 @@ +default: + # This file defines the parameters for the linear motion tracker. + # All this parameter coordinate is assumed to be in the vehicle coordinate system. + # So, the x axis is pointing to the front of the vehicle, y axis is pointing to the left of the vehicle. + ekf_params: + # random walk noise is used to model the acceleration noise + process_noise_std: # [m/s^2] + ax: 0.98 # assume 0.1G acceleration noise + ay: 0.98 + vx: 0.1 # assume 0.1m/s velocity noise + vy: 0.1 + x: 1.0 # assume 1m position noise + y: 1.0 + measurement_noise_std: + x: 0.6 # [m] + y: 0.9 # [m] + vx: 0.4 # [m/s] + vy: 1 # [m/s] + initial_covariance_std: + x: 3.0 # [m] + y: 6.0 # [m] + vx: 1.0 # [m/s] + vy: 5.0 # [m/s] + ax: 0.5 # [m/s^2] + ay: 1.0 # [m/s^2] + # output limitation + limit: + max_speed: 80.0 # [m/s] + # low pass filter is used to smooth the yaw and shape estimation + low_pass_filter: + time_constant: 1.0 # [s] + sampling_time: 0.1 # [s] diff --git a/perception/radar_object_tracker/include/radar_object_tracker/data_association/data_association.hpp b/perception/radar_object_tracker/include/radar_object_tracker/data_association/data_association.hpp new file mode 100644 index 0000000000000..2fc50625a29ea --- /dev/null +++ b/perception/radar_object_tracker/include/radar_object_tracker/data_association/data_association.hpp @@ -0,0 +1,66 @@ +// Copyright 2020 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. +// +// +// Author: v1.0 Yukihiro Saito +// + +#ifndef RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ +#define RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ + +#include +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include "object_recognition_utils/object_recognition_utils.hpp" +#include "radar_object_tracker/data_association/solver/gnn_solver.hpp" +#include "radar_object_tracker/tracker/tracker.hpp" + +#include +#include + +#include + +#include +class DataAssociation +{ +private: + Eigen::MatrixXi can_assign_matrix_; + Eigen::MatrixXd max_dist_matrix_; + Eigen::MatrixXd max_area_matrix_; + Eigen::MatrixXd min_area_matrix_; + Eigen::MatrixXd max_rad_matrix_; + Eigen::MatrixXd min_iou_matrix_; + const double score_threshold_; + std::unique_ptr gnn_solver_ptr_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + DataAssociation( + std::vector can_assign_vector, std::vector max_dist_vector, + std::vector max_area_vector, std::vector min_area_vector, + std::vector max_rad_vector, std::vector min_iou_vector); + void assign( + const Eigen::MatrixXd & src, std::unordered_map & direct_assignment, + std::unordered_map & reverse_assignment); + Eigen::MatrixXd calcScoreMatrix( + const autoware_auto_perception_msgs::msg::DetectedObjects & measurements, + const std::list> & trackers, const bool debug_log, + const std::string & file_name); + virtual ~DataAssociation() {} +}; + +#endif // RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ diff --git a/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/gnn_solver.hpp b/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/gnn_solver.hpp new file mode 100644 index 0000000000000..6d531ac2ee8c2 --- /dev/null +++ b/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/gnn_solver.hpp @@ -0,0 +1,22 @@ +// Copyright 2021 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 RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#define RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ + +#include "radar_object_tracker/data_association/solver/gnn_solver_interface.hpp" +#include "radar_object_tracker/data_association/solver/mu_successive_shortest_path.hpp" +#include "radar_object_tracker/data_association/solver/successive_shortest_path.hpp" + +#endif // RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ diff --git a/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/gnn_solver_interface.hpp b/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/gnn_solver_interface.hpp new file mode 100644 index 0000000000000..9b633f04ca190 --- /dev/null +++ b/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/gnn_solver_interface.hpp @@ -0,0 +1,35 @@ +// Copyright 2021 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 RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#define RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ + +#include +#include + +namespace gnn_solver +{ +class GnnSolverInterface +{ +public: + GnnSolverInterface() = default; + virtual ~GnnSolverInterface() = default; + + virtual void maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) = 0; +}; +} // namespace gnn_solver + +#endif // RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ diff --git a/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/mu_successive_shortest_path.hpp b/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/mu_successive_shortest_path.hpp new file mode 100644 index 0000000000000..d4bdd72248670 --- /dev/null +++ b/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/mu_successive_shortest_path.hpp @@ -0,0 +1,37 @@ +// Copyright 2021 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 RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#define RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ + +#include "radar_object_tracker/data_association/solver/gnn_solver_interface.hpp" + +#include +#include + +namespace gnn_solver +{ +class MuSSP : public GnnSolverInterface +{ +public: + MuSSP() = default; + ~MuSSP() = default; + + void maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) override; +}; +} // namespace gnn_solver + +#endif // RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ diff --git a/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/successive_shortest_path.hpp b/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/successive_shortest_path.hpp new file mode 100644 index 0000000000000..b08e3c0d302a7 --- /dev/null +++ b/perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/successive_shortest_path.hpp @@ -0,0 +1,37 @@ +// Copyright 2021 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 RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#define RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ + +#include "radar_object_tracker/data_association/solver/gnn_solver_interface.hpp" + +#include +#include + +namespace gnn_solver +{ +class SSP : public GnnSolverInterface +{ +public: + SSP() = default; + ~SSP() = default; + + void maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) override; +}; +} // namespace gnn_solver + +#endif // RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ diff --git a/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp b/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp new file mode 100644 index 0000000000000..898a7855aabae --- /dev/null +++ b/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp @@ -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. + +#ifndef RADAR_OBJECT_TRACKER__RADAR_OBJECT_TRACKER_NODE__RADAR_OBJECT_TRACKER_NODE_HPP_ +#define RADAR_OBJECT_TRACKER__RADAR_OBJECT_TRACKER_NODE__RADAR_OBJECT_TRACKER_NODE_HPP_ + +#include "radar_object_tracker/data_association/data_association.hpp" + +#include + +#include +#include +#include + +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include + +#include +#include +#include +#include + +using autoware_auto_perception_msgs::msg::DetectedObject; +using autoware_auto_perception_msgs::msg::DetectedObjects; + +class RadarObjectTrackerNode : public rclcpp::Node +{ +public: + explicit RadarObjectTrackerNode(const rclcpp::NodeOptions & node_options); + +private: + rclcpp::Publisher::SharedPtr + tracked_objects_pub_; + rclcpp::Subscription::SharedPtr + detected_object_sub_; + rclcpp::TimerBase::SharedPtr publish_timer_; // publish timer + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + std::map tracker_map_; + + void onMeasurement( + const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg); + void onTimer(); + + std::string world_frame_id_; // tracking frame + std::string tracker_config_directory_; + std::list> list_tracker_; + std::unique_ptr data_association_; + + // debug parameters + struct logging + { + bool enable = false; + std::string path; + } logging_; + + void checkTrackerLifeCycle( + std::list> & list_tracker, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform); + void sanitizeTracker( + std::list> & list_tracker, const rclcpp::Time & time); + std::shared_ptr createNewTracker( + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) const; + + void publish(const rclcpp::Time & time) const; + inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; +}; + +#endif // RADAR_OBJECT_TRACKER__RADAR_OBJECT_TRACKER_NODE__RADAR_OBJECT_TRACKER_NODE_HPP_ diff --git a/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/linear_motion_tracker.hpp b/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/linear_motion_tracker.hpp new file mode 100644 index 0000000000000..108e79c043ba0 --- /dev/null +++ b/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/linear_motion_tracker.hpp @@ -0,0 +1,106 @@ +// Copyright 2020 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 RADAR_OBJECT_TRACKER__TRACKER__MODEL__LINEAR_MOTION_TRACKER_HPP_ +#define RADAR_OBJECT_TRACKER__TRACKER__MODEL__LINEAR_MOTION_TRACKER_HPP_ + +#include "radar_object_tracker/tracker/model/tracker_base.hpp" + +#include + +#include + +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; +class LinearMotionTracker : public Tracker +{ +private: + autoware_auto_perception_msgs::msg::DetectedObject object_; + rclcpp::Logger logger_; + +private: + KalmanFilter ekf_; + rclcpp::Time last_update_time_; + enum IDX { X = 0, Y = 1, VX = 2, VY = 3, AX = 4, AY = 5 }; + + struct EkfParams + { + // dimension + char dim_x = 6; + // system noise + double q_cov_ax; + double q_cov_ay; + double q_cov_vx; + double q_cov_vy; + double q_cov_x; + double q_cov_y; + // measurement noise + double r_cov_x; + double r_cov_y; + double r_cov_vx; + double r_cov_vy; + // initial state covariance + double p0_cov_x; + double p0_cov_y; + double p0_cov_vx; + double p0_cov_vy; + double p0_cov_ax; + double p0_cov_ay; + } ekf_params_; + + // limitation + double max_vx_; + double max_vy_; + // rough tracking parameters + float z_; + float yaw_; + + // lpf parameter + double filter_tau_; // time constant of 1st order low pass filter + double filter_dt_; // sampling time of 1st order low pass filter + +private: + struct BoundingBox + { + double length; + double width; + double height; + }; + struct Cylinder + { + double width; + double height; + }; + BoundingBox bounding_box_; + Cylinder cylinder_; + +public: + LinearMotionTracker( + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const std::string & tracker_param_file, const std::uint8_t & label); + + void loadDefaultModelParameters(const std::string & path); + bool predict(const rclcpp::Time & time) override; + bool predict(const double dt, KalmanFilter & ekf) const; + bool measure( + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) override; + bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); + bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); + bool getTrackedObject( + const rclcpp::Time & time, + autoware_auto_perception_msgs::msg::TrackedObject & object) const override; + virtual ~LinearMotionTracker() {} +}; + +#endif // RADAR_OBJECT_TRACKER__TRACKER__MODEL__LINEAR_MOTION_TRACKER_HPP_ diff --git a/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/tracker_base.hpp b/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/tracker_base.hpp new file mode 100644 index 0000000000000..4e113ac47f57f --- /dev/null +++ b/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/tracker_base.hpp @@ -0,0 +1,97 @@ +// Copyright 2020 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. +// +// +// Author: v1.0 Yukihiro Saito +// + +#ifndef RADAR_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ +#define RADAR_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ + +#define EIGEN_MPL2_ONLY +#include "object_recognition_utils/object_recognition_utils.hpp" +#include "radar_object_tracker/utils/utils.hpp" + +#include +#include + +#include "autoware_auto_perception_msgs/msg/detected_object.hpp" +#include "autoware_auto_perception_msgs/msg/tracked_object.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "unique_identifier_msgs/msg/uuid.hpp" + +#include + +class Tracker +{ +protected: + unique_identifier_msgs::msg::UUID getUUID() const { return uuid_; } + void setClassification( + const std::vector & classification) + { + classification_ = classification; + } + +private: + unique_identifier_msgs::msg::UUID uuid_; + std::vector classification_; + int no_measurement_count_; + int total_no_measurement_count_; + int total_measurement_count_; + rclcpp::Time last_update_with_measurement_time_; + +public: + Tracker( + const rclcpp::Time & time, + const std::vector & classification); + virtual ~Tracker() {} + bool updateWithMeasurement( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform); + bool updateWithoutMeasurement(); + std::vector getClassification() const + { + return classification_; + } + std::uint8_t getHighestProbLabel() const + { + return object_recognition_utils::getHighestProbLabel(classification_); + } + int getNoMeasurementCount() const { return no_measurement_count_; } + int getTotalNoMeasurementCount() const { return total_no_measurement_count_; } + int getTotalMeasurementCount() const { return total_measurement_count_; } + double getElapsedTimeFromLastUpdate(const rclcpp::Time & current_time) const + { + return (current_time - last_update_with_measurement_time_).seconds(); + } + virtual geometry_msgs::msg::PoseWithCovariance getPoseWithCovariance( + const rclcpp::Time & time) const; + + /* + *　Pure virtual function + */ + +protected: + virtual bool measure( + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) = 0; + +public: + virtual bool getTrackedObject( + const rclcpp::Time & time, + autoware_auto_perception_msgs::msg::TrackedObject & object) const = 0; + virtual bool predict(const rclcpp::Time & time) = 0; +}; + +#endif // RADAR_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ diff --git a/perception/radar_object_tracker/include/radar_object_tracker/tracker/tracker.hpp b/perception/radar_object_tracker/include/radar_object_tracker/tracker/tracker.hpp new file mode 100644 index 0000000000000..7da940912a253 --- /dev/null +++ b/perception/radar_object_tracker/include/radar_object_tracker/tracker/tracker.hpp @@ -0,0 +1,21 @@ +// Copyright 2020 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 RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ +#define RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ + +#include "model/linear_motion_tracker.hpp" +#include "model/tracker_base.hpp" + +#endif // RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ diff --git a/perception/radar_object_tracker/include/radar_object_tracker/utils/utils.hpp b/perception/radar_object_tracker/include/radar_object_tracker/utils/utils.hpp new file mode 100644 index 0000000000000..3fe9172ab5ceb --- /dev/null +++ b/perception/radar_object_tracker/include/radar_object_tracker/utils/utils.hpp @@ -0,0 +1,85 @@ +// Copyright 2020 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. +// +// +// Author: v1.0 Yukihiro Saito +// + +#ifndef RADAR_OBJECT_TRACKER__UTILS__UTILS_HPP_ +#define RADAR_OBJECT_TRACKER__UTILS__UTILS_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace utils +{ +enum MSG_COV_IDX { + X_X = 0, + X_Y = 1, + X_Z = 2, + X_ROLL = 3, + X_PITCH = 4, + X_YAW = 5, + Y_X = 6, + Y_Y = 7, + Y_Z = 8, + Y_ROLL = 9, + Y_PITCH = 10, + Y_YAW = 11, + Z_X = 12, + Z_Y = 13, + Z_Z = 14, + Z_ROLL = 15, + Z_PITCH = 16, + Z_YAW = 17, + ROLL_X = 18, + ROLL_Y = 19, + ROLL_Z = 20, + ROLL_ROLL = 21, + ROLL_PITCH = 22, + ROLL_YAW = 23, + PITCH_X = 24, + PITCH_Y = 25, + PITCH_Z = 26, + PITCH_ROLL = 27, + PITCH_PITCH = 28, + PITCH_YAW = 29, + YAW_X = 30, + YAW_Y = 31, + YAW_Z = 32, + YAW_ROLL = 33, + YAW_PITCH = 34, + YAW_YAW = 35 +}; + +// matrix concatenate +Eigen::MatrixXd stackMatricesVertically(const std::vector & matrices); +Eigen::MatrixXd stackMatricesDiagonally(const std::vector & matrices); + +} // namespace utils + +#endif // RADAR_OBJECT_TRACKER__UTILS__UTILS_HPP_ diff --git a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml b/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml new file mode 100644 index 0000000000000..7ef3e58a64161 --- /dev/null +++ b/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/radar_object_tracker/models.md b/perception/radar_object_tracker/models.md new file mode 100644 index 0000000000000..a4c53db156f0f --- /dev/null +++ b/perception/radar_object_tracker/models.md @@ -0,0 +1,76 @@ +# models + +Tracking models can be chosen from the ros parameter `~tracking_model`: + +Each model has its own parameters, which can be set in the ros parameter server. + +- model name + - parameter name for general + - override parameter name for each tracking object class + +## linear constant acceleration model + +- prediction + +$$ +\begin{bmatrix} +x_{k+1} \\ +y_{k+1} \\ +v_{x_{k+1}} \\ +v_{y_{k+1}} \\ +a_{x_{k+1}} \\ +a_{y_{k+1}} +\end{bmatrix} = +\begin{bmatrix} +1 & 0 & dt & 0 & \frac{1}{2}dt^2 & 0 \\ +0 & 1 & 0 & dt & 0 & \frac{1}{2}dt^2 \\ +0 & 0 & 1 & 0 & dt & 0 \\ +0 & 0 & 0 & 1 & 0 & dt \\ +0 & 0 & 0 & 0 & 1 & 0 \\ +0 & 0 & 0 & 0 & 0 & 1 \\ +\end{bmatrix} +\begin{bmatrix} +x_k \\ +y_k \\ +v_{x_k} \\ +v_{y_k} \\ +a_{x_k} \\ +a_{y_k} +\end{bmatrix} + noise +$$ + +- noise model + + - random walk in acc: 2 parameters(currently disabled) + - random state noise: 6 parameters + $$ + \begin{align} + noise &= \begin{bmatrix} + \frac{dt^{3}}{6} \\ 0\\ \frac{dt^{2}}{2} \\ 0 \\ dt \\0 + \end{bmatrix} \nu_{ax_k} + \begin{bmatrix} + 0 \\ \frac{dt^{3}}{6} \\ 0 \\ \frac{dt^{2}}{2} \\ 0 \\ dt + \end{bmatrix} \nu_{ay_k} & \text{(random walk in acc)} \\ + noise &= \begin{bmatrix} + \nu_{x_k} \\ \nu_{y_k} \\ \nu_{v_{x_k}} \\ \nu_{v_{y_k}} \\ \nu_{a_{x_k}} \\ \nu_{a_{y_k}} + \end{bmatrix} & + \text{(random state noise)} & + \end{align} + $$ + +- observation + - observation: x,y,vx,vy + - observation noise: 4 parameters + +## constant turn rate and velocity model + +Just idea, not implemented yet. + +$$ +\begin{align} +x_{k+1} &= x_k + \frac{v_k}{\omega_k} (sin(\theta_k + \omega_k dt) - sin(\theta_k)) \\ +y_{k+1} &= y_k + \frac{v_k}{\omega_k} (cos(\theta_k) - cos(\theta_k + \omega_k dt)) \\ +v_{k+1} &= v_k \\ +\theta_{k+1} &= \theta_k + \omega_k dt \\ +\omega_{k+1} &= \omega_k +\end{align} +$$ diff --git a/perception/radar_object_tracker/package.xml b/perception/radar_object_tracker/package.xml new file mode 100644 index 0000000000000..46fb0826e30e1 --- /dev/null +++ b/perception/radar_object_tracker/package.xml @@ -0,0 +1,36 @@ + + + + radar_object_tracker + 0.0.0 + Do tracking radar object + Yoshi Ri + Yukihiro Saito + Satoshi Tanaka + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_perception_msgs + eigen + kalman_filter + mussp + object_recognition_utils + rclcpp + rclcpp_components + tf2 + tf2_ros + tier4_autoware_utils + tier4_perception_msgs + unique_identifier_msgs + yaml-cpp + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/radar_object_tracker/src/data_association/data_association.cpp b/perception/radar_object_tracker/src/data_association/data_association.cpp new file mode 100644 index 0000000000000..df1c4b0a9b2ee --- /dev/null +++ b/perception/radar_object_tracker/src/data_association/data_association.cpp @@ -0,0 +1,296 @@ +// Copyright 2020 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 "radar_object_tracker/data_association/data_association.hpp" + +#include "radar_object_tracker/data_association/solver/gnn_solver.hpp" + +#include + +// #include "multi_object_tracker/utils/utils.hpp" + +#include +#include +#include +#include +#include +#include + +namespace +{ +double getMahalanobisDistance( + const geometry_msgs::msg::Point & measurement, const geometry_msgs::msg::Point & tracker, + const Eigen::Matrix2d & covariance) +{ + Eigen::Vector2d measurement_point; + measurement_point << measurement.x, measurement.y; + Eigen::Vector2d tracker_point; + tracker_point << tracker.x, tracker.y; + Eigen::MatrixXd mahalanobis_squared = (measurement_point - tracker_point).transpose() * + covariance.inverse() * (measurement_point - tracker_point); + return std::sqrt(mahalanobis_squared(0)); +} + +Eigen::Matrix2d getXYCovariance(const geometry_msgs::msg::PoseWithCovariance & pose_covariance) +{ + Eigen::Matrix2d covariance; + covariance << pose_covariance.covariance[0], pose_covariance.covariance[1], + pose_covariance.covariance[6], pose_covariance.covariance[7]; + return covariance; +} + +double getFormedYawAngle( + const geometry_msgs::msg::Quaternion & measurement_quat, + const geometry_msgs::msg::Quaternion & tracker_quat, const bool distinguish_front_or_back = true) +{ + const double measurement_yaw = + tier4_autoware_utils::normalizeRadian(tf2::getYaw(measurement_quat)); + const double tracker_yaw = tier4_autoware_utils::normalizeRadian(tf2::getYaw(tracker_quat)); + const double angle_range = distinguish_front_or_back ? M_PI : M_PI_2; + const double angle_step = distinguish_front_or_back ? 2.0 * M_PI : M_PI; + // Fixed measurement_yaw to be in the range of +-90 or 180 degrees of X_t(IDX::YAW) + double measurement_fixed_yaw = measurement_yaw; + while (angle_range <= tracker_yaw - measurement_fixed_yaw) { + measurement_fixed_yaw = measurement_fixed_yaw + angle_step; + } + while (angle_range <= measurement_fixed_yaw - tracker_yaw) { + measurement_fixed_yaw = measurement_fixed_yaw - angle_step; + } + return std::fabs(measurement_fixed_yaw - tracker_yaw); +} +} // namespace + +DataAssociation::DataAssociation( + std::vector can_assign_vector, std::vector max_dist_vector, + std::vector max_area_vector, std::vector min_area_vector, + std::vector max_rad_vector, std::vector min_iou_vector) +: score_threshold_(0.01) +{ + { + const int assign_label_num = static_cast(std::sqrt(can_assign_vector.size())); + Eigen::Map can_assign_matrix_tmp( + can_assign_vector.data(), assign_label_num, assign_label_num); + can_assign_matrix_ = can_assign_matrix_tmp.transpose(); + } + { + const int max_dist_label_num = static_cast(std::sqrt(max_dist_vector.size())); + Eigen::Map max_dist_matrix_tmp( + max_dist_vector.data(), max_dist_label_num, max_dist_label_num); + max_dist_matrix_ = max_dist_matrix_tmp.transpose(); + } + { + const int max_area_label_num = static_cast(std::sqrt(max_area_vector.size())); + Eigen::Map max_area_matrix_tmp( + max_area_vector.data(), max_area_label_num, max_area_label_num); + max_area_matrix_ = max_area_matrix_tmp.transpose(); + } + { + const int min_area_label_num = static_cast(std::sqrt(min_area_vector.size())); + Eigen::Map min_area_matrix_tmp( + min_area_vector.data(), min_area_label_num, min_area_label_num); + min_area_matrix_ = min_area_matrix_tmp.transpose(); + } + { + const int max_rad_label_num = static_cast(std::sqrt(max_rad_vector.size())); + Eigen::Map max_rad_matrix_tmp( + max_rad_vector.data(), max_rad_label_num, max_rad_label_num); + max_rad_matrix_ = max_rad_matrix_tmp.transpose(); + } + { + const int min_iou_label_num = static_cast(std::sqrt(min_iou_vector.size())); + Eigen::Map min_iou_matrix_tmp( + min_iou_vector.data(), min_iou_label_num, min_iou_label_num); + min_iou_matrix_ = min_iou_matrix_tmp.transpose(); + } + + gnn_solver_ptr_ = std::make_unique(); +} + +void DataAssociation::assign( + const Eigen::MatrixXd & src, std::unordered_map & direct_assignment, + std::unordered_map & reverse_assignment) +{ + std::vector> score(src.rows()); + for (int row = 0; row < src.rows(); ++row) { + score.at(row).resize(src.cols()); + for (int col = 0; col < src.cols(); ++col) { + score.at(row).at(col) = src(row, col); + } + } + // Solve + gnn_solver_ptr_->maximizeLinearAssignment(score, &direct_assignment, &reverse_assignment); + + for (auto itr = direct_assignment.begin(); itr != direct_assignment.end();) { + if (src(itr->first, itr->second) < score_threshold_) { + itr = direct_assignment.erase(itr); + continue; + } else { + ++itr; + } + } + for (auto itr = reverse_assignment.begin(); itr != reverse_assignment.end();) { + if (src(itr->second, itr->first) < score_threshold_) { + itr = reverse_assignment.erase(itr); + continue; + } else { + ++itr; + } + } +} + +Eigen::MatrixXd DataAssociation::calcScoreMatrix( + const autoware_auto_perception_msgs::msg::DetectedObjects & measurements, + const std::list> & trackers, const bool debug_log, + const std::string & file_name) +{ + // for debug + nlohmann::json log_data; + std::string log_file_name = "association_log.json"; + if (!file_name.empty()) { + log_file_name = file_name; + } + // current time + log_data["time"] = measurements.header.stamp.sec + measurements.header.stamp.nanosec * 1e-9; + nlohmann::json data_array = nlohmann::json::array(); + + Eigen::MatrixXd score_matrix = + Eigen::MatrixXd::Zero(trackers.size(), measurements.objects.size()); + size_t tracker_idx = 0; + for (auto tracker_itr = trackers.begin(); tracker_itr != trackers.end(); + ++tracker_itr, ++tracker_idx) { + const std::uint8_t tracker_label = (*tracker_itr)->getHighestProbLabel(); + + for (size_t measurement_idx = 0; measurement_idx < measurements.objects.size(); + ++measurement_idx) { + const autoware_auto_perception_msgs::msg::DetectedObject & measurement_object = + measurements.objects.at(measurement_idx); + const std::uint8_t measurement_label = + object_recognition_utils::getHighestProbLabel(measurement_object.classification); + // Create a JSON object to hold the log data for this pair + nlohmann::json pair_log_data; + + autoware_auto_perception_msgs::msg::TrackedObject tracked_object; + (*tracker_itr)->getTrackedObject(measurements.header.stamp, tracked_object); + + std::vector tracker_pose = { + tracked_object.kinematics.pose_with_covariance.pose.position.x, + tracked_object.kinematics.pose_with_covariance.pose.position.y}; + std::vector measurement_pose = { + measurement_object.kinematics.pose_with_covariance.pose.position.x, + measurement_object.kinematics.pose_with_covariance.pose.position.y}; + pair_log_data["tracker_uuid"] = tracked_object.object_id.uuid; + pair_log_data["tracker_idx"] = tracker_idx; + pair_log_data["measurement_idx"] = measurement_idx; + pair_log_data["tracker_label"] = tracker_label; + pair_log_data["measurement_label"] = measurement_label; + pair_log_data["gate_name"] = ""; + pair_log_data["gate_value"] = 0.0; + pair_log_data["gate_threshold"] = 0.0; + pair_log_data["tracker_pose"] = tracker_pose; + pair_log_data["measurement_pose"] = measurement_pose; + + double score = 0.0; + if (can_assign_matrix_(tracker_label, measurement_label)) { + const double max_dist = max_dist_matrix_(tracker_label, measurement_label); + const double dist = tier4_autoware_utils::calcDistance2d( + measurement_object.kinematics.pose_with_covariance.pose.position, + tracked_object.kinematics.pose_with_covariance.pose.position); + + bool passed_gate = true; + // dist gate + if (passed_gate) { + if (max_dist < dist) { + passed_gate = false; + } + pair_log_data["gate_name"] = "dist gate"; + pair_log_data["gate_value"] = dist; + pair_log_data["gate_threshold"] = max_dist; + } + // area gate + if (passed_gate) { + const double max_area = max_area_matrix_(tracker_label, measurement_label); + const double min_area = min_area_matrix_(tracker_label, measurement_label); + const double area = tier4_autoware_utils::getArea(measurement_object.shape); + if (area < min_area || max_area < area) { + passed_gate = false; + } + pair_log_data["gate_name"] = "area gate"; + pair_log_data["gate_value"] = area; + pair_log_data["gate_threshold"] = max_area; + } + // angle gate + if (passed_gate) { + const double max_rad = max_rad_matrix_(tracker_label, measurement_label); + const double angle = getFormedYawAngle( + measurement_object.kinematics.pose_with_covariance.pose.orientation, + tracked_object.kinematics.pose_with_covariance.pose.orientation, false); + if (std::fabs(max_rad) < M_PI && std::fabs(max_rad) < std::fabs(angle)) { + passed_gate = false; + } + pair_log_data["gate_name"] = "angle gate"; + pair_log_data["gate_value"] = angle; + pair_log_data["gate_threshold"] = max_rad; + } + // mahalanobis dist gate + if (passed_gate) { + const double mahalanobis_dist = getMahalanobisDistance( + measurement_object.kinematics.pose_with_covariance.pose.position, + tracked_object.kinematics.pose_with_covariance.pose.position, + getXYCovariance(tracked_object.kinematics.pose_with_covariance)); + if (2.448 /*95%*/ <= mahalanobis_dist) { + passed_gate = false; + } + pair_log_data["gate_name"] = "mahalanobis dist gate"; + pair_log_data["gate_value"] = mahalanobis_dist; + pair_log_data["gate_threshold"] = 2.448; + } + // 2d iou gate + if (passed_gate) { + const double min_iou = min_iou_matrix_(tracker_label, measurement_label); + const double min_union_iou_area = 1e-2; + const double iou = object_recognition_utils::get2dIoU( + measurement_object, tracked_object, min_union_iou_area); + if (iou < min_iou) { + passed_gate = false; + } + pair_log_data["gate_name"] = "2d iou gate"; + pair_log_data["gate_value"] = iou; + pair_log_data["gate_threshold"] = min_iou; + } + + // all gate is passed + if (passed_gate) { + pair_log_data["gate_name"] = "all gate passed"; + score = (max_dist - std::min(dist, max_dist)) / max_dist; + if (score < score_threshold_) { + score = 0.0; + } + } + pair_log_data["passed_gate"] = passed_gate; + pair_log_data["score"] = score; + data_array.push_back(pair_log_data); + } + score_matrix(tracker_idx, measurement_idx) = score; + } + } + // Write the log data to a file + log_data["data"] = data_array; + if (debug_log) { + std::ofstream log_file(log_file_name, std::ios::app); + log_file << log_data << std::endl; + log_file.close(); + } + + return score_matrix; +} diff --git a/perception/radar_object_tracker/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp b/perception/radar_object_tracker/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp new file mode 100644 index 0000000000000..43f974c2621e1 --- /dev/null +++ b/perception/radar_object_tracker/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp @@ -0,0 +1,41 @@ +// Copyright 2021 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 "radar_object_tracker/data_association/solver/mu_successive_shortest_path.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace gnn_solver +{ +void MuSSP::maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) +{ + // Terminate if the graph is empty + if (cost.size() == 0 || cost.at(0).size() == 0) { + return; + } + + // Solve DA by muSSP + solve_muSSP(cost, direct_assignment, reverse_assignment); +} +} // namespace gnn_solver diff --git a/perception/radar_object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp b/perception/radar_object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp new file mode 100644 index 0000000000000..c904a9005b27e --- /dev/null +++ b/perception/radar_object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp @@ -0,0 +1,370 @@ +// Copyright 2020 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 "radar_object_tracker/data_association/solver/successive_shortest_path.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace gnn_solver +{ +struct ResidualEdge +{ + // Destination node + const int dst; + int capacity; + const double cost; + int flow; + // Access to the reverse edge by adjacency_list.at(dst).at(reverse) + const int reverse; + + // ResidualEdge() + // : dst(0), capacity(0), cost(0), flow(0), reverse(0) {} + + ResidualEdge(int dst, int capacity, double cost, int flow, int reverse) + : dst(dst), capacity(capacity), cost(cost), flow(flow), reverse(reverse) + { + } +}; + +void SSP::maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) +{ + // NOTE: Need to set as default arguments + bool sparse_cost = true; + // bool sparse_cost = false; + + // Hyperparameters + // double MAX_COST = 6; + const double MAX_COST = 10; + const double INF_DIST = 10000000; + const double EPS = 1e-5; + + // When there is no agents or no tasks, terminate + if (cost.size() == 0 || cost.at(0).size() == 0) { + return; + } + + // Construct a bipartite graph from the cost matrix + int n_agents = cost.size(); + int n_tasks = cost.at(0).size(); + + int n_dummies; + if (sparse_cost) { + n_dummies = n_agents; + } else { + n_dummies = 0; + } + + int source = 0; + int sink = n_agents + n_tasks + 1; + int n_nodes = n_agents + n_tasks + n_dummies + 2; + + // // Print cost matrix + // std::cout << std::endl; + // for (int agent = 0; agent < n_agents; agent++) + // { + // for (int task = 0; task < n_tasks; task++) + // { + // std::cout << cost.at(agent).at(task) << " "; + // } + // std::cout << std::endl; + // } + + // std::chrono::system_clock::time_point start_time, end_time; + // start_time = std::chrono::system_clock::now(); + + // Adjacency list of residual graph (index: nodes) + // - 0: source node + // - {1, ..., n_agents}: agent nodes + // - {n_agents+1, ..., n_agents+n_tasks}: task nodes + // - n_agents+n_tasks+1: sink node + // - {n_agents+n_tasks+2, ..., n_agents+n_tasks+1+n_agents}: + // dummy node (when sparse_cost is true) + std::vector> adjacency_list(n_nodes); + + // Reserve memory + for (int v = 0; v < n_nodes; ++v) { + if (v == source) { + // Source + adjacency_list.at(v).reserve(n_agents); + } else if (v <= n_agents) { + // Agents + adjacency_list.at(v).reserve(n_tasks + 1 + 1); + } else if (v <= n_agents + n_tasks) { + // Tasks + adjacency_list.at(v).reserve(n_agents + 1); + } else if (v == sink) { + // Sink + adjacency_list.at(v).reserve(n_tasks + n_dummies); + } else { + // Dummies + adjacency_list.at(v).reserve(2); + } + } + + // Add edges form source + for (int agent = 0; agent < n_agents; ++agent) { + // From source to agent + adjacency_list.at(source).emplace_back(agent + 1, 1, 0, 0, adjacency_list.at(agent + 1).size()); + // From agent to source + adjacency_list.at(agent + 1).emplace_back( + source, 0, 0, 0, adjacency_list.at(source).size() - 1); + } + + // Add edges from agents + for (int agent = 0; agent < n_agents; ++agent) { + for (int task = 0; task < n_tasks; ++task) { + if (!sparse_cost || cost.at(agent).at(task) > EPS) { + // From agent to task + adjacency_list.at(agent + 1).emplace_back( + task + n_agents + 1, 1, MAX_COST - cost.at(agent).at(task), 0, + adjacency_list.at(task + n_agents + 1).size()); + + // From task to agent + adjacency_list.at(task + n_agents + 1) + .emplace_back( + agent + 1, 0, cost.at(agent).at(task) - MAX_COST, 0, + adjacency_list.at(agent + 1).size() - 1); + } + } + } + + // Add edges form tasks + for (int task = 0; task < n_tasks; ++task) { + // From task to sink + adjacency_list.at(task + n_agents + 1) + .emplace_back(sink, 1, 0, 0, adjacency_list.at(sink).size()); + + // From sink to task + adjacency_list.at(sink).emplace_back( + task + n_agents + 1, 0, 0, 0, adjacency_list.at(task + n_agents + 1).size() - 1); + } + + // Add edges from dummy + if (sparse_cost) { + for (int agent = 0; agent < n_agents; ++agent) { + // From agent to dummy + adjacency_list.at(agent + 1).emplace_back( + agent + n_agents + n_tasks + 2, 1, MAX_COST, 0, + adjacency_list.at(agent + n_agents + n_tasks + 2).size()); + + // From dummy to agent + adjacency_list.at(agent + n_agents + n_tasks + 2) + .emplace_back(agent + 1, 0, -MAX_COST, 0, adjacency_list.at(agent + 1).size() - 1); + + // From dummy to sink + adjacency_list.at(agent + n_agents + n_tasks + 2) + .emplace_back(sink, 1, 0, 0, adjacency_list.at(sink).size()); + + // From sink to dummy + adjacency_list.at(sink).emplace_back( + agent + n_agents + n_tasks + 2, 0, 0, 0, + adjacency_list.at(agent + n_agents + n_tasks + 2).size() - 1); + } + } + + // Maximum flow value + const int max_flow = std::min(n_agents, n_tasks); + + // Feasible potentials + std::vector potentials(n_nodes, 0); + + // Shortest path lengths + std::vector distances(n_nodes, INF_DIST); + + // Whether previously visited the node or not + std::vector is_visited(n_nodes, false); + + // Parent node () + std::vector> prev_values(n_nodes); + + for (int i = 0; i < max_flow; ++i) { + // Initialize priority queue () + std::priority_queue< + std::pair, std::vector>, + std::greater>> + p_queue; + + // Reset all trajectory states + if (i > 0) { + std::fill(distances.begin(), distances.end(), INF_DIST); + std::fill(is_visited.begin(), is_visited.end(), false); + } + + // Start trajectory from the source node + p_queue.push(std::make_pair(0, source)); + distances.at(source) = 0; + + while (!p_queue.empty()) { + // Get the next element + std::pair cur_elem = p_queue.top(); + // std::cout << "[pop]: (" << cur_elem.first << ", " << cur_elem.second << ")" << std::endl; + p_queue.pop(); + + double cur_node_dist = cur_elem.first; + int cur_node = cur_elem.second; + + // If already visited node, skip and continue + if (is_visited.at(cur_node)) { + continue; + } + assert(cur_node_dist == distances.at(cur_node)); + + // Mark as visited + is_visited.at(cur_node) = true; + // Update potential + potentials.at(cur_node) += cur_node_dist; + + // When reached to the sink node, terminate. + if (cur_node == sink) { + break; + } + + // Loop over the incident nodes(/edges) + for (auto it_incident_edge = adjacency_list.at(cur_node).cbegin(); + it_incident_edge != adjacency_list.at(cur_node).cend(); it_incident_edge++) { + // If the node is not visited and have capacity to increase flow, visit. + if (!is_visited.at(it_incident_edge->dst) && it_incident_edge->capacity > 0) { + // Calculate reduced cost + double reduced_cost = + it_incident_edge->cost + potentials.at(cur_node) - potentials.at(it_incident_edge->dst); + assert(reduced_cost >= 0); + if (distances.at(it_incident_edge->dst) > reduced_cost) { + distances.at(it_incident_edge->dst) = reduced_cost; + prev_values.at(it_incident_edge->dst) = + std::make_pair(cur_node, it_incident_edge - adjacency_list.at(cur_node).cbegin()); + // std::cout << "[push]: (" << reduced_cost << ", " << next_v << ")" << std::endl; + p_queue.push(std::make_pair(reduced_cost, it_incident_edge->dst)); + } + } + } + } + + // Shortest path length to sink is greater than MAX_COST, + // which means no non-dummy routes left, terminate + if (potentials.at(sink) >= MAX_COST) { + break; + } + + // Update potentials of unvisited nodes + for (int v = 0; v < n_nodes; ++v) { + if (!is_visited.at(v)) { + potentials.at(v) += distances.at(sink); + } + } + // //Print potentials + // for (int v = 0; v < n_nodes; ++v) + // { + // std::cout << potentials.at(v) << ", "; + // } + // std::cout << std::endl; + + // Increase/decrease flow and capacity along the shortest path from the source to the sink + int v = sink; + int prev_v; + while (v != source) { + ResidualEdge & e_forward = + adjacency_list.at(prev_values.at(v).first).at(prev_values.at(v).second); + assert(e_forward.dst == v); + ResidualEdge & e_backward = adjacency_list.at(v).at(e_forward.reverse); + prev_v = e_backward.dst; + + if (e_backward.flow == 0) { + // Increase flow + // State A + assert(e_forward.capacity == 1); + assert(e_forward.flow == 0); + assert(e_backward.capacity == 0); + assert(e_backward.flow == 0); + + e_forward.capacity -= 1; + e_forward.flow += 1; + e_backward.capacity += 1; + + // State B + assert(e_forward.capacity == 0); + assert(e_forward.flow == 1); + assert(e_backward.capacity == 1); + assert(e_backward.flow == 0); + } else { + // Decrease flow + // State B + assert(e_forward.capacity == 1); + assert(e_forward.flow == 0); + assert(e_backward.capacity == 0); + assert(e_backward.flow == 1); + + e_forward.capacity -= 1; + e_backward.capacity += 1; + e_backward.flow -= 1; + + // State A + assert(e_forward.capacity == 0); + assert(e_forward.flow == 0); + assert(e_backward.capacity == 1); + assert(e_backward.flow == 0); + } + + v = prev_v; + } + +#ifndef NDEBUG + // Check if the potentials are feasible potentials + for (int v = 0; v < n_nodes; ++v) { + for (auto it_incident_edge = adjacency_list.at(v).cbegin(); + it_incident_edge != adjacency_list.at(v).cend(); ++it_incident_edge) { + if (it_incident_edge->capacity > 0) { + double reduced_cost = + it_incident_edge->cost + potentials.at(v) - potentials.at(it_incident_edge->dst); + assert(reduced_cost >= 0); + } + } + } +#endif + } + + // Output + for (int agent = 0; agent < n_agents; ++agent) { + for (auto it_incident_edge = adjacency_list.at(agent + 1).cbegin(); + it_incident_edge != adjacency_list.at(agent + 1).cend(); ++it_incident_edge) { + int task = it_incident_edge->dst - (n_agents + 1); + + // If the flow value is 1 and task is not dummy, assign the task to the agent. + if (it_incident_edge->flow == 1 && 0 <= task && task < n_tasks) { + (*direct_assignment)[agent] = task; + (*reverse_assignment)[task] = agent; + break; + } + } + } + +#ifndef NDEBUG + // Check if the result is valid assignment + for (int agent = 0; agent < n_agents; ++agent) { + if (direct_assignment->find(agent) != direct_assignment->cend()) { + int task = (*direct_assignment).at(agent); + assert(direct_assignment->at(agent) == task); + assert(reverse_assignment->at(task) == agent); + } + } +#endif +} +} // namespace gnn_solver diff --git a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp new file mode 100644 index 0000000000000..f32084493f34e --- /dev/null +++ b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp @@ -0,0 +1,356 @@ +// 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 "radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp" + +#include "radar_object_tracker/utils/utils.hpp" + +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#define EIGEN_MPL2_ONLY + +namespace +{ +boost::optional getTransformAnonymous( + const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, + const std::string & target_frame_id, const rclcpp::Time & time) +{ + try { + // check if the frames are ready + std::string errstr; // This argument prevents error msg from being displayed in the terminal. + if (!tf_buffer.canTransform( + target_frame_id, source_frame_id, tf2::TimePointZero, tf2::Duration::zero(), &errstr)) { + return boost::none; + } + + geometry_msgs::msg::TransformStamped self_transform_stamped; + self_transform_stamped = tf_buffer.lookupTransform( + /*target*/ target_frame_id, /*src*/ source_frame_id, time, + rclcpp::Duration::from_seconds(0.5)); + return self_transform_stamped.transform; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("radar_object_tracker"), ex.what()); + return boost::none; + } +} + +} // namespace + +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + +RadarObjectTrackerNode::RadarObjectTrackerNode(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("radar_object_tracker", node_options), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_) +{ + // Create publishers and subscribers + detected_object_sub_ = create_subscription( + "input", rclcpp::QoS{1}, + std::bind(&RadarObjectTrackerNode::onMeasurement, this, std::placeholders::_1)); + tracked_objects_pub_ = + create_publisher("output", rclcpp::QoS{1}); + + // Parameters + double publish_rate = declare_parameter("publish_rate", 30.0); + world_frame_id_ = declare_parameter("world_frame_id", "world"); + bool enable_delay_compensation{declare_parameter("enable_delay_compensation", false)}; + tracker_config_directory_ = declare_parameter("tracking_config_directory", ""); + logging_.enable = declare_parameter("enable_logging", false); + logging_.path = + declare_parameter("logging_file_path", "~/.ros/association_log.json"); + + // Load tracking config file + if (tracker_config_directory_.empty()) { + tracker_config_directory_ = + ament_index_cpp::get_package_share_directory("radar_object_tracker") + "/config/tracking/"; + } + + auto cti = std::make_shared( + this->get_node_base_interface(), this->get_node_timers_interface()); + tf_buffer_.setCreateTimerInterface(cti); + + // Create ROS time based timer + if (enable_delay_compensation) { + const auto period_ns = rclcpp::Rate(publish_rate).period(); + publish_timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&RadarObjectTrackerNode::onTimer, this)); + } + + const auto tmp = this->declare_parameter>("can_assign_matrix"); + const std::vector can_assign_matrix(tmp.begin(), tmp.end()); + + // import association matrices + const auto max_dist_matrix = this->declare_parameter>("max_dist_matrix"); + const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); + const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); + const auto max_rad_matrix = this->declare_parameter>("max_rad_matrix"); + const auto min_iou_matrix = this->declare_parameter>("min_iou_matrix"); + data_association_ = std::make_unique( + can_assign_matrix, max_dist_matrix, max_area_matrix, min_area_matrix, max_rad_matrix, + min_iou_matrix); + + // tracker map + tracker_map_.insert( + std::make_pair(Label::CAR, this->declare_parameter("car_tracker"))); + tracker_map_.insert( + std::make_pair(Label::TRUCK, this->declare_parameter("truck_tracker"))); + tracker_map_.insert( + std::make_pair(Label::BUS, this->declare_parameter("bus_tracker"))); + tracker_map_.insert( + std::make_pair(Label::TRAILER, this->declare_parameter("trailer_tracker"))); + tracker_map_.insert( + std::make_pair(Label::PEDESTRIAN, this->declare_parameter("pedestrian_tracker"))); + tracker_map_.insert( + std::make_pair(Label::BICYCLE, this->declare_parameter("bicycle_tracker"))); + tracker_map_.insert( + std::make_pair(Label::MOTORCYCLE, this->declare_parameter("motorcycle_tracker"))); +} + +void RadarObjectTrackerNode::onMeasurement( + const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg) +{ + const auto self_transform = getTransformAnonymous( + tf_buffer_, "base_link", world_frame_id_, input_objects_msg->header.stamp); + if (!self_transform) { + return; + } + + /* transform to world coordinate */ + autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; + if (!object_recognition_utils::transformObjects( + *input_objects_msg, world_frame_id_, tf_buffer_, transformed_objects)) { + return; + } + /* tracker prediction */ + rclcpp::Time measurement_time = input_objects_msg->header.stamp; + for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) { + (*itr)->predict(measurement_time); + } + + /* global nearest neighbor */ + std::unordered_map direct_assignment, reverse_assignment; + Eigen::MatrixXd score_matrix = data_association_->calcScoreMatrix( + transformed_objects, list_tracker_, // row : tracker, col : measurement + logging_.enable, logging_.path); + data_association_->assign(score_matrix, direct_assignment, reverse_assignment); + + /* tracker measurement update */ + int tracker_idx = 0; + for (auto tracker_itr = list_tracker_.begin(); tracker_itr != list_tracker_.end(); + ++tracker_itr, ++tracker_idx) { + if (direct_assignment.find(tracker_idx) != direct_assignment.end()) { // found + (*(tracker_itr)) + ->updateWithMeasurement( + transformed_objects.objects.at(direct_assignment.find(tracker_idx)->second), + measurement_time, *self_transform); + } else { // not found + (*(tracker_itr))->updateWithoutMeasurement(); + } + } + + /* life cycle check */ + checkTrackerLifeCycle(list_tracker_, measurement_time, *self_transform); + /* sanitize trackers */ + sanitizeTracker(list_tracker_, measurement_time); + + /* new tracker */ + for (size_t i = 0; i < transformed_objects.objects.size(); ++i) { + if (reverse_assignment.find(i) != reverse_assignment.end()) { // found + continue; + } + std::shared_ptr tracker = + createNewTracker(transformed_objects.objects.at(i), measurement_time, *self_transform); + if (tracker) { + list_tracker_.push_back(tracker); + } + } + + if (publish_timer_ == nullptr) { + publish(measurement_time); + } +} + +std::shared_ptr RadarObjectTrackerNode::createNewTracker( + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & /*self_transform*/) const +{ + const std::uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); + if (tracker_map_.count(label) != 0) { + const auto tracker = tracker_map_.at(label); + + if (tracker == "linear_motion_tracker") { + std::string config_file = tracker_config_directory_ + "linear_motion_tracker.yaml"; + return std::make_shared(time, object, config_file, label); + } else { + // not implemented yet so put warning + RCLCPP_WARN(get_logger(), "Tracker %s is not implemented yet", tracker.c_str()); + } + } + std::string config_file = tracker_config_directory_ + "linear_motion_tracker.yaml"; + return std::make_shared(time, object, config_file, label); +} + +void RadarObjectTrackerNode::onTimer() +{ + rclcpp::Time current_time = this->now(); + const auto self_transform = + getTransformAnonymous(tf_buffer_, world_frame_id_, "base_link", current_time); + if (!self_transform) { + return; + } + + /* life cycle check */ + checkTrackerLifeCycle(list_tracker_, current_time, *self_transform); + /* sanitize trackers */ + sanitizeTracker(list_tracker_, current_time); + + // Publish + publish(current_time); +} + +void RadarObjectTrackerNode::checkTrackerLifeCycle( + std::list> & list_tracker, const rclcpp::Time & time, + [[maybe_unused]] const geometry_msgs::msg::Transform & self_transform) +{ + /* params */ + constexpr float max_elapsed_time = 1.0; + + /* delete tracker */ + for (auto itr = list_tracker.begin(); itr != list_tracker.end(); ++itr) { + const bool is_old = max_elapsed_time < (*itr)->getElapsedTimeFromLastUpdate(time); + if (is_old) { + auto erase_itr = itr; + --itr; + list_tracker.erase(erase_itr); + } + } +} + +void RadarObjectTrackerNode::sanitizeTracker( + std::list> & list_tracker, const rclcpp::Time & time) +{ + constexpr float min_iou = 0.1; + constexpr float min_iou_for_unknown_object = 0.001; + constexpr double distance_threshold = 5.0; + /* delete collision tracker */ + for (auto itr1 = list_tracker.begin(); itr1 != list_tracker.end(); ++itr1) { + autoware_auto_perception_msgs::msg::TrackedObject object1; + (*itr1)->getTrackedObject(time, object1); + for (auto itr2 = std::next(itr1); itr2 != list_tracker.end(); ++itr2) { + autoware_auto_perception_msgs::msg::TrackedObject object2; + (*itr2)->getTrackedObject(time, object2); + const double distance = std::hypot( + object1.kinematics.pose_with_covariance.pose.position.x - + object2.kinematics.pose_with_covariance.pose.position.x, + object1.kinematics.pose_with_covariance.pose.position.y - + object2.kinematics.pose_with_covariance.pose.position.y); + if (distance_threshold < distance) { + continue; + } + + const double min_union_iou_area = 1e-2; + const auto iou = object_recognition_utils::get2dIoU(object1, object2, min_union_iou_area); + const auto & label1 = (*itr1)->getHighestProbLabel(); + const auto & label2 = (*itr2)->getHighestProbLabel(); + bool should_delete_tracker1 = false; + bool should_delete_tracker2 = false; + + // If at least one of them is UNKNOWN, delete the one with lower IOU. Because the UNKNOWN + // objects are not reliable. + if (label1 == Label::UNKNOWN || label2 == Label::UNKNOWN) { + if (min_iou_for_unknown_object < iou) { + if (label1 == Label::UNKNOWN && label2 == Label::UNKNOWN) { + if ((*itr1)->getTotalMeasurementCount() < (*itr2)->getTotalMeasurementCount()) { + should_delete_tracker1 = true; + } else { + should_delete_tracker2 = true; + } + } else if (label1 == Label::UNKNOWN) { + should_delete_tracker1 = true; + } else if (label2 == Label::UNKNOWN) { + should_delete_tracker2 = true; + } + } + } else { // If neither is UNKNOWN, delete the one with lower IOU. + if (min_iou < iou) { + if ((*itr1)->getTotalMeasurementCount() < (*itr2)->getTotalMeasurementCount()) { + should_delete_tracker1 = true; + } else { + should_delete_tracker2 = true; + } + } + } + + if (should_delete_tracker1) { + itr1 = list_tracker.erase(itr1); + --itr1; + break; + } else if (should_delete_tracker2) { + itr2 = list_tracker.erase(itr2); + --itr2; + } + } + } +} + +inline bool RadarObjectTrackerNode::shouldTrackerPublish( + const std::shared_ptr tracker) const +{ + constexpr int measurement_count_threshold = 3; + if (tracker->getTotalMeasurementCount() < measurement_count_threshold) { + return false; + } + return true; +} + +void RadarObjectTrackerNode::publish(const rclcpp::Time & time) const +{ + const auto subscriber_count = tracked_objects_pub_->get_subscription_count() + + tracked_objects_pub_->get_intra_process_subscription_count(); + if (subscriber_count < 1) { + return; + } + // Create output msg + autoware_auto_perception_msgs::msg::TrackedObjects output_msg; + output_msg.header.frame_id = world_frame_id_; + output_msg.header.stamp = time; + for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) { + if (!shouldTrackerPublish(*itr)) { + continue; + } + autoware_auto_perception_msgs::msg::TrackedObject object; + (*itr)->getTrackedObject(time, object); + output_msg.objects.push_back(object); + } + + // Publish + tracked_objects_pub_->publish(output_msg); +} + +RCLCPP_COMPONENTS_REGISTER_NODE(RadarObjectTrackerNode) diff --git a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp new file mode 100644 index 0000000000000..bfd84159706fb --- /dev/null +++ b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp @@ -0,0 +1,633 @@ +// Copyright 2020 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. +// +// +// Author: v1.0 Yukihiro Saito +// + +#include "radar_object_tracker/tracker/model/linear_motion_tracker.hpp" + +#include "radar_object_tracker/utils/utils.hpp" + +#include + +#include +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#define EIGEN_MPL2_ONLY +#include +#include +#include +#include + +#include + +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + +LinearMotionTracker::LinearMotionTracker( + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const std::string & /*tracker_param_file*/, const std::uint8_t & /*label*/) +: Tracker(time, object.classification), + logger_(rclcpp::get_logger("LinearMotionTracker")), + last_update_time_(time), + z_(object.kinematics.pose_with_covariance.pose.position.z) +{ + object_ = object; + + // load setting from yaml file + const std::string default_setting_file = + ament_index_cpp::get_package_share_directory("radar_object_tracker") + + "/config/tracking/linear_motion_tracker.yaml"; + loadDefaultModelParameters(default_setting_file); + // loadModelParameters(tracker_param_file, label); + + // initialize X matrix and position + Eigen::MatrixXd X(ekf_params_.dim_x, 1); + X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; + X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; + const auto yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + z_ = object.kinematics.pose_with_covariance.pose.position.z; + yaw_ = yaw; + // radar object usually have twist + if (object.kinematics.has_twist) { + const auto v = object.kinematics.twist_with_covariance.twist.linear.x; + X(IDX::VX) = v * std::cos(yaw); + X(IDX::VY) = v * std::sin(yaw); + } else { + X(IDX::VX) = 0.0; + X(IDX::VY) = 0.0; + } + // init ax and ay + X(IDX::AX) = 0.0; + X(IDX::AY) = 0.0; + + // initialize P matrix + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); + + // create rotation matrix to rotate covariance matrix + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + // 2d rotation matrix + Eigen::Matrix2d R; + R << cos_yaw, -sin_yaw, sin_yaw, cos_yaw; + + // covariance matrix in the target vehicle coordinate system + Eigen::Matrix2d P_xy_local, P_vxy_local, P_axy_local; + P_xy_local << ekf_params_.p0_cov_x, 0.0, 0.0, ekf_params_.p0_cov_y; + P_vxy_local << ekf_params_.p0_cov_vx, 0.0, 0.0, ekf_params_.p0_cov_vy; + P_axy_local << ekf_params_.p0_cov_ax, 0.0, 0.0, ekf_params_.p0_cov_ay; + + // Rotated covariance matrix + // covariance is rotated by 2D rotation matrix R + // P′=R P RT + Eigen::Matrix2d P_xy, P_vxy, P_axy; + + // Rotate the covariance matrix according to the vehicle yaw + // because p0_cov_x and y are in the vehicle coordinate system. + if (object.kinematics.has_position_covariance) { + P_xy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X], + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y], + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X], + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + P_xy = R * P_xy_local * R.transpose(); + } else { + // rotate + P_xy = R * P_xy_local * R.transpose(); + } + // covariance often written in object frame + if (object.kinematics.has_twist_covariance) { + const auto vx_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + const auto vy_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + P_vxy_local << vx_cov, 0.0, 0.0, vy_cov; + P_vxy = R * P_vxy_local * R.transpose(); + } else { + P_vxy = R * P_vxy_local * R.transpose(); + } + // acceleration covariance often writen in object frame + const bool has_acceleration_covariance = + false; // currently message does not have acceleration covariance + if (has_acceleration_covariance) { + // const auto ax_cov = + // object.kinematics.acceleration_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; // This + // is future update + // const auto ay_cov = + // object.kinematics.acceleration_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; // This + // is future update + // Eigen::Matrix2d P_axy_local; + // P_axy_local << ax_cov, 0.0, 0.0, ay_cov; + P_axy = R * P_axy_local * R.transpose(); + } else { + P_axy = R * P_axy_local * R.transpose(); + } + + // put value in P matrix + // use block description. This assume x,y,vx,vy,ax,ay in this order + P.block<2, 2>(IDX::X, IDX::X) = P_xy; + P.block<2, 2>(IDX::VX, IDX::VX) = P_vxy; + P.block<2, 2>(IDX::AX, IDX::AX) = P_axy; + + // init shape + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + bounding_box_ = { + object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + cylinder_ = {object.shape.dimensions.x, object.shape.dimensions.z}; + } + + ekf_.init(X, P); +} + +void LinearMotionTracker::loadDefaultModelParameters(const std::string & path) +{ + YAML::Node config = YAML::LoadFile(path); + // initialize ekf params + const float q_stddev_ax = + config["default"]["ekf_params"]["process_noise_std"]["ax"].as(); // [m/(s*s)] + const float q_stddev_ay = + config["default"]["ekf_params"]["process_noise_std"]["ay"].as(); // [m/(s*s)] + const float q_stddev_vx = + config["default"]["ekf_params"]["process_noise_std"]["vx"].as(); // [m/s] + const float q_stddev_vy = + config["default"]["ekf_params"]["process_noise_std"]["vy"].as(); // [m/s] + const float q_stddev_x = + config["default"]["ekf_params"]["process_noise_std"]["x"].as(); // [m] + const float q_stddev_y = + config["default"]["ekf_params"]["process_noise_std"]["y"].as(); // [m] + const float r_stddev_x = + config["default"]["ekf_params"]["measurement_noise_std"]["x"].as(); // [m] + const float r_stddev_y = + config["default"]["ekf_params"]["measurement_noise_std"]["y"].as(); // [m] + const float r_stddev_vx = + config["default"]["ekf_params"]["measurement_noise_std"]["vx"].as(); // [m] + const float r_stddev_vy = + config["default"]["ekf_params"]["measurement_noise_std"]["vy"].as(); // [m] + const float p0_stddev_x = + config["default"]["ekf_params"]["initial_covariance_std"]["x"].as(); // [m/s] + const float p0_stddev_y = + config["default"]["ekf_params"]["initial_covariance_std"]["y"].as(); // [m/s] + const float p0_stddev_vx = + config["default"]["ekf_params"]["initial_covariance_std"]["vx"].as(); // [m/(s)] + const float p0_stddev_vy = + config["default"]["ekf_params"]["initial_covariance_std"]["vy"].as(); // [m/(s)] + const float p0_stddev_ax = + config["default"]["ekf_params"]["initial_covariance_std"]["ax"].as(); // [m/(s*s)] + const float p0_stddev_ay = + config["default"]["ekf_params"]["initial_covariance_std"]["ay"].as(); // [m/(s*s)] + ekf_params_.q_cov_ax = std::pow(q_stddev_ax, 2.0); + ekf_params_.q_cov_ay = std::pow(q_stddev_ay, 2.0); + ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); + ekf_params_.q_cov_vy = std::pow(q_stddev_vy, 2.0); + ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); + ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); + ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); + ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); + ekf_params_.r_cov_vx = std::pow(r_stddev_vx, 2.0); + ekf_params_.r_cov_vy = std::pow(r_stddev_vy, 2.0); + ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); + ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); + ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); + ekf_params_.p0_cov_vy = std::pow(p0_stddev_vy, 2.0); + ekf_params_.p0_cov_ay = std::pow(p0_stddev_ax, 2.0); + ekf_params_.p0_cov_ay = std::pow(p0_stddev_ay, 2.0); + + // lpf filter parameters + filter_tau_ = config["default"]["low_pass_filter"]["time_constant"].as(1.0); // [s] + filter_dt_ = config["default"]["low_pass_filter"]["sampling_time"].as(0.1); // [s] + + // limitation + // (TODO): this may be written in another yaml file based on classify result + const float max_speed_kmph = config["default"]["limit"]["max_speed"].as(); // [km/h] + max_vx_ = tier4_autoware_utils::kmph2mps(max_speed_kmph); // [m/s] + max_vy_ = tier4_autoware_utils::kmph2mps(max_speed_kmph); // [rad/s] + + // shape initialization + // (TODO): this may be written in another yaml file based on classify result + bounding_box_ = {0.5, 0.5, 1.7}; + cylinder_ = {0.3, 1.7}; +} + +bool LinearMotionTracker::predict(const rclcpp::Time & time) +{ + const double dt = (time - last_update_time_).seconds(); + bool ret = predict(dt, ekf_); + if (ret) { + last_update_time_ = time; + } + return ret; +} + +bool LinearMotionTracker::predict(const double dt, KalmanFilter & ekf) const +{ + /* == Linear model == + * + * x_{k+1} = x_k + dt * vx_k + 0.5 * dt^2 * ax_k + * y_{k+1} = y_k + dt * vy_k + 0.5 * dt^2 * ay_k + * vx_{k+1} = vx_k + dt * ax_k + * vy_{k+1} = vy_k + dt * ay_k + * ax_{k+1} = ax_k + * ay_{k+1} = ay_k + */ + + /* == state transition matrix == + * + * A = [1, 0, dt, 0, 0.5 * dt^2, 0, + * 0, 1, 0, dt, 0, 0.5 * dt^2, + * 0, 0, 1, 0, dt, 0, + * 0, 0, 0, 1, 0, dt, + * 0, 0, 0, 0, 1, 0, + * 0, 0, 0, 0, 0, 1] + */ + + // X t + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state + ekf.getX(X_t); + const auto x = X_t(IDX::X); + const auto y = X_t(IDX::Y); + const auto vx = X_t(IDX::VX); + const auto vy = X_t(IDX::VY); + const auto ax = X_t(IDX::AX); + const auto ay = X_t(IDX::AY); + + // X t+1 + Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); + X_next_t(IDX::X) = x + vx * dt + 0.5 * ax * dt * dt; + X_next_t(IDX::Y) = y + vy * dt + 0.5 * ay * dt * dt; + X_next_t(IDX::VX) = vx + ax * dt; + X_next_t(IDX::VY) = vy + ay * dt; + X_next_t(IDX::AX) = ax; + X_next_t(IDX::AY) = ay; + + // A: state transition matrix + Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); + A(IDX::X, IDX::VX) = dt; + A(IDX::Y, IDX::VY) = dt; + A(IDX::X, IDX::AX) = 0.5 * dt * dt; + A(IDX::Y, IDX::AY) = 0.5 * dt * dt; + A(IDX::VX, IDX::AX) = dt; + A(IDX::VY, IDX::AY) = dt; + + // Q: system noise + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); + Eigen::MatrixXd Q_local = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); + + // system noise in local coordinate + // we assume acceleration random walk model + // + // Q_local = [dt^3/6 0 dt^2/2 0 dt 0] ^ T qcov_ax [dt^3/6 0 dt^2/2 0 dt 0] + // + [0 dt^3/6 0 dt^2/2 0 dt] ^ T qcov_ay [0 dt^3/6 0 dt^2/2 0 dt] + // Eigen::MatrixXd qx = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); + // Eigen::MatrixXd qy = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); + // qx << dt * dt * dt / 6, 0, dt * dt / 2, 0, dt, 0; + // qy << 0, dt * dt * dt / 6, 0, dt * dt / 2, 0, dt; + // Q_local = qx * ekf_params_.q_cov_ax * qx.transpose() + qy * ekf_params_.q_cov_ay * + // qy.transpose(); just create diag matrix + Eigen::VectorXd q_diag_vector = Eigen::VectorXd::Zero(ekf_params_.dim_x); + q_diag_vector << ekf_params_.q_cov_x, ekf_params_.q_cov_y, ekf_params_.q_cov_vx, + ekf_params_.q_cov_vy, ekf_params_.q_cov_ax, ekf_params_.q_cov_ay; + Q_local = q_diag_vector.asDiagonal(); + + // Rotate the covariance matrix according to the vehicle yaw + // because q_cov_x and y are in the vehicle coordinate system. + // rotate pose, velocity, acceleration covariance in one matrix + // it is something like + // [R 0 0] [R^T 0 0] + // [0 R 0] * Q_local * [0 R^T 0] + // [0 0 R] [0 0 R^T] + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(2, 2); + R << cos(yaw_), -sin(yaw_), sin(yaw_), cos(yaw_); + Eigen::MatrixXd RotateCovMatrix = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); + RotateCovMatrix.block<2, 2>(IDX::X, IDX::X) = R; + RotateCovMatrix.block<2, 2>(IDX::VX, IDX::VX) = R; + RotateCovMatrix.block<2, 2>(IDX::AX, IDX::AX) = R; + Q = RotateCovMatrix * Q_local * RotateCovMatrix.transpose(); + + Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); + Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); + + // call kalman filter library + if (!ekf.predict(X_next_t, A, Q)) { + RCLCPP_WARN(logger_, "Cannot predict"); + } + + return true; +} + +bool LinearMotionTracker::measureWithPose( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + // Observation pattern will be: + // 1. x, y, vx, vy + // 2. x, y + // 3. vx, vy (Do not consider this right now) + // + // We handle this measurements by stacking observation matrix, measurement vector and measurement + // covariance + // - observation matrix: C + // - measurement vector : Y + // - measurement covariance: R + + // rotation matrix + Eigen::Matrix2d RotationYaw; + const auto yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + RotationYaw << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw); + + // gather matrices as vector + std::vector C_list; + std::vector Y_list; + std::vector R_block_list; + + // 1. add position measurement + const bool enable_position_measurement = true; // assume position is always measured + if (enable_position_measurement) { + Eigen::MatrixXd Cxy = Eigen::MatrixXd::Zero(2, ekf_params_.dim_x); + Cxy(0, IDX::X) = 1; + Cxy(1, IDX::Y) = 1; + C_list.push_back(Cxy); + + Eigen::MatrixXd Yxy = Eigen::MatrixXd::Zero(2, 1); + Yxy << object.kinematics.pose_with_covariance.pose.position.x, + object.kinematics.pose_with_covariance.pose.position.y; + Y_list.push_back(Yxy); + + // covariance need to be rotated since it is in the vehicle coordinate system + Eigen::MatrixXd Rxy_local = Eigen::MatrixXd::Zero(2, 2); + if (!object.kinematics.has_position_covariance) { + Rxy_local << ekf_params_.r_cov_x, 0, 0, ekf_params_.r_cov_y; + } else { + Rxy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X], + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y], + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X], + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + } + Eigen::MatrixXd Rxy = Eigen::MatrixXd::Zero(2, 2); + Rxy = RotationYaw * Rxy_local * RotationYaw.transpose(); + R_block_list.push_back(Rxy); + } + + // 2. add linear velocity measurement + const bool enable_velocity_measurement = object.kinematics.has_twist; + if (enable_velocity_measurement) { + Eigen::MatrixXd Cvxvy = Eigen::MatrixXd::Zero(2, ekf_params_.dim_x); + Cvxvy(0, IDX::VX) = 1; + Cvxvy(1, IDX::VY) = 1; + C_list.push_back(Cvxvy); + + // velocity is in the target vehicle coordinate system + Eigen::MatrixXd Vxy_local = Eigen::MatrixXd::Zero(2, 1); + Eigen::MatrixXd Vxy = Eigen::MatrixXd::Zero(2, 1); + Vxy_local << object.kinematics.twist_with_covariance.twist.linear.x, + object.kinematics.twist_with_covariance.twist.linear.y; + Vxy = RotationYaw * Vxy_local; + Y_list.push_back(Vxy); + + Eigen::Matrix2d Rvxy_local = Eigen::MatrixXd::Zero(2, 2); + if (!object.kinematics.has_twist_covariance) { + Rvxy_local << ekf_params_.r_cov_vx, 0, 0, ekf_params_.r_cov_vy; + } else { + Rvxy_local << object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X], 0, + 0, object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + } + Eigen::MatrixXd Rvxy = Eigen::MatrixXd::Zero(2, 2); + Rvxy = RotationYaw * Rvxy_local * RotationYaw.transpose(); + R_block_list.push_back(Rvxy); + } + + // 3. sumup matrices + const auto row_number = std::accumulate( + C_list.begin(), C_list.end(), 0, + [](const auto & sum, const auto & C) { return sum + C.rows(); }); + if (row_number == 0) { + RCLCPP_WARN(logger_, "No measurement is available"); + return false; + } + // Eigen::MatrixXd C = Eigen::MatrixXd::Zero(row_number, ekf_params_.dim_x); + // Eigen::MatrixXd Y = Eigen::MatrixXd::Zero(row_number, 1); + // Eigen::MatrixXd R = Eigen::MatrixXd::Zero(row_number, row_number); + + // stacking matrices vertically or diagonally + const auto C = utils::stackMatricesVertically(C_list); + const auto Y = utils::stackMatricesVertically(Y_list); + const auto R = utils::stackMatricesDiagonally(R_block_list); + + // 4. EKF update + if (!ekf_.update(Y, C, R)) { + RCLCPP_WARN(logger_, "Cannot update"); + } + + // 5. normalize: limit vx, vy + { + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); + Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); + ekf_.getX(X_t); + ekf_.getP(P_t); + if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { + X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; + } + if (!(-max_vy_ <= X_t(IDX::VY) && X_t(IDX::VY) <= max_vy_)) { + X_t(IDX::VY) = X_t(IDX::VY) < 0 ? -max_vy_ : max_vy_; + } + ekf_.init(X_t, P_t); + } + + // 6. Filter z and yaw + // first order low pass filter + const float gain = filter_tau_ / (filter_tau_ + filter_dt_); + z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + yaw_ = gain * yaw_ + (1.0 - gain) * yaw; + + return true; +} + +bool LinearMotionTracker::measureWithShape( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + // just use first order low pass filter + const float gain = filter_tau_ / (filter_tau_ + filter_dt_); + + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * object.shape.dimensions.x; + bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * object.shape.dimensions.y; + bounding_box_.height = gain * bounding_box_.height + (1.0 - gain) * object.shape.dimensions.z; + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + cylinder_.width = gain * cylinder_.width + (1.0 - gain) * object.shape.dimensions.x; + cylinder_.height = gain * cylinder_.height + (1.0 - gain) * object.shape.dimensions.z; + } else { + return false; + } + + return true; +} + +bool LinearMotionTracker::measure( + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) +{ + const auto & current_classification = getClassification(); + object_ = object; + if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { + setClassification(current_classification); + } + + if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + RCLCPP_WARN( + logger_, "There is a large gap between predicted time and measurement time. (%f)", + (time - last_update_time_).seconds()); + } + + measureWithPose(object); + measureWithShape(object); + + (void)self_transform; // currently do not use self vehicle position + return true; +} + +bool LinearMotionTracker::getTrackedObject( + const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const +{ + object = object_recognition_utils::toTrackedObject(object_); + object.object_id = getUUID(); + object.classification = getClassification(); + + // predict kinematics + KalmanFilter tmp_ekf_for_no_update = ekf_; + const double dt = (time - last_update_time_).seconds(); + if (0.001 /*1msec*/ < dt) { + predict(dt, tmp_ekf_for_no_update); + } + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state + Eigen::MatrixXd P(ekf_params_.dim_x, ekf_params_.dim_x); // predicted state + tmp_ekf_for_no_update.getX(X_t); + tmp_ekf_for_no_update.getP(P); + + auto & pose_with_cov = object.kinematics.pose_with_covariance; + auto & twist_with_cov = object.kinematics.twist_with_covariance; + auto & acceleration_with_cov = object.kinematics.acceleration_with_covariance; + // rotation matrix with yaw_ + Eigen::Matrix2d Ryaw = Eigen::Matrix2d::Zero(); + Ryaw << std::cos(yaw_), -std::sin(yaw_), std::sin(yaw_), std::cos(yaw_); + const Eigen::Matrix2d Ryaw_inv = Ryaw.transpose(); + + // position + pose_with_cov.pose.position.x = X_t(IDX::X); + pose_with_cov.pose.position.y = X_t(IDX::Y); + pose_with_cov.pose.position.z = z_; + // quaternion + { + double roll, pitch, yaw; + tf2::Quaternion original_quaternion; + tf2::fromMsg(object_.kinematics.pose_with_covariance.pose.orientation, original_quaternion); + tf2::Matrix3x3(original_quaternion).getRPY(roll, pitch, yaw); + tf2::Quaternion filtered_quaternion; + filtered_quaternion.setRPY(roll, pitch, yaw_); + pose_with_cov.pose.orientation.x = filtered_quaternion.x(); + pose_with_cov.pose.orientation.y = filtered_quaternion.y(); + pose_with_cov.pose.orientation.z = filtered_quaternion.z(); + pose_with_cov.pose.orientation.w = filtered_quaternion.w(); + object.kinematics.orientation_availability = + autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; + } + // twist + // twist need to converted to local coordinate + const auto vx = X_t(IDX::VX); + const auto vy = X_t(IDX::VY); + // rotate this vector with -yaw + Eigen::Vector2d vxy = Ryaw_inv * Eigen::Vector2d(vx, vy); + twist_with_cov.twist.linear.x = vxy(0); + twist_with_cov.twist.linear.y = vxy(1); + + // acceleration + // acceleration need to converted to local coordinate + const auto ax = X_t(IDX::AX); + const auto ay = X_t(IDX::AY); + // rotate this vector with -yaw + Eigen::Vector2d axy = Ryaw_inv * Eigen::Vector2d(ax, ay); + acceleration_with_cov.accel.linear.x = axy(0); + acceleration_with_cov.accel.linear.y = axy(1); + + // ===== covariance transformation ===== + // since covariance in EKF is in map coordinate and output should be in object coordinate, + // we need to transform covariance + Eigen::Matrix2d Pxy_map, Pvxy_map, Paxy_map; + Pxy_map << P(IDX::X, IDX::X), P(IDX::X, IDX::Y), P(IDX::Y, IDX::X), P(IDX::Y, IDX::Y); + Pvxy_map << P(IDX::VX, IDX::VX), P(IDX::VX, IDX::VY), P(IDX::VY, IDX::VX), P(IDX::VY, IDX::VY); + Paxy_map << P(IDX::AX, IDX::AX), P(IDX::AX, IDX::AY), P(IDX::AY, IDX::AX), P(IDX::AY, IDX::AY); + + // rotate covariance with -yaw + Eigen::Matrix2d Pxy = Ryaw_inv * Pxy_map * Ryaw_inv.transpose(); + Eigen::Matrix2d Pvxy = Ryaw_inv * Pvxy_map * Ryaw_inv.transpose(); + Eigen::Matrix2d Paxy = Ryaw_inv * Paxy_map * Ryaw_inv.transpose(); + + // position covariance + constexpr double no_info_cov = 1e9; // no information + constexpr double z_cov = 1. * 1.; // TODO(yukkysaito) Currently tentative + constexpr double yaw_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + + pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = Pxy(IDX::X, IDX::X); + pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = Pxy(IDX::X, IDX::Y); + pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = Pxy(IDX::Y, IDX::X); + pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = Pxy(IDX::Y, IDX::Y); + pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; + pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov; + pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov; + pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = yaw_cov; + + // twist covariance + constexpr double vz_cov = 0.2 * 0.2; // TODO(Yoshi Ri) Currently tentative + constexpr double wz_cov = 0.2 * 0.2; // TODO(yukkysaito) Currently tentative + + twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = Pvxy(IDX::X, IDX::X); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = Pvxy(IDX::X, IDX::Y); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = Pvxy(IDX::Y, IDX::X); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = Pvxy(IDX::Y, IDX::Y); + twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; + twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov; + twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov; + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = wz_cov; + + // acceleration covariance + acceleration_with_cov.covariance[utils::MSG_COV_IDX::X_X] = Paxy(IDX::X, IDX::X); + acceleration_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = Paxy(IDX::X, IDX::Y); + acceleration_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = Paxy(IDX::Y, IDX::X); + acceleration_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = Paxy(IDX::Y, IDX::Y); + acceleration_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = no_info_cov; + acceleration_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov; + acceleration_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov; + acceleration_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = no_info_cov; + + // set shape + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + object.shape.dimensions.x = bounding_box_.length; + object.shape.dimensions.y = bounding_box_.width; + object.shape.dimensions.z = bounding_box_.height; + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + object.shape.dimensions.x = cylinder_.width; + object.shape.dimensions.y = cylinder_.width; + object.shape.dimensions.z = cylinder_.height; + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); + const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); + object.shape.footprint = + tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + } + + return true; +} diff --git a/perception/radar_object_tracker/src/tracker/model/tracker_base.cpp b/perception/radar_object_tracker/src/tracker/model/tracker_base.cpp new file mode 100644 index 0000000000000..14425b5f33cf2 --- /dev/null +++ b/perception/radar_object_tracker/src/tracker/model/tracker_base.cpp @@ -0,0 +1,63 @@ +// Copyright 2020 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 "radar_object_tracker/tracker/model/tracker_base.hpp" + +#include "radar_object_tracker/utils/utils.hpp" + +#include +#include + +Tracker::Tracker( + const rclcpp::Time & time, + const std::vector & classification) +: classification_(classification), + no_measurement_count_(0), + total_no_measurement_count_(0), + total_measurement_count_(1), + last_update_with_measurement_time_(time) +{ + // Generate random number + std::mt19937 gen(std::random_device{}()); + std::independent_bits_engine bit_eng(gen); + std::generate(uuid_.uuid.begin(), uuid_.uuid.end(), bit_eng); +} + +bool Tracker::updateWithMeasurement( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform) +{ + no_measurement_count_ = 0; + ++total_measurement_count_; + last_update_with_measurement_time_ = measurement_time; + measure(object, measurement_time, self_transform); + return true; +} + +bool Tracker::updateWithoutMeasurement() +{ + ++no_measurement_count_; + ++total_no_measurement_count_; + return true; +} + +geometry_msgs::msg::PoseWithCovariance Tracker::getPoseWithCovariance( + const rclcpp::Time & time) const +{ + autoware_auto_perception_msgs::msg::TrackedObject object; + getTrackedObject(time, object); + return object.kinematics.pose_with_covariance; +} diff --git a/perception/radar_object_tracker/src/utils/utils.cpp b/perception/radar_object_tracker/src/utils/utils.cpp new file mode 100644 index 0000000000000..cb9d3813e0ecc --- /dev/null +++ b/perception/radar_object_tracker/src/utils/utils.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 "radar_object_tracker/utils/utils.hpp" + +namespace utils +{ +// concatenate matrices vertically +Eigen::MatrixXd stackMatricesVertically(const std::vector & matrices) +{ + int totalRows = 0; + int cols = -1; + + // calculate total number of rows and check that all matrices have the same number of columns + for (const auto & matrix : matrices) { + totalRows += matrix.rows(); + if (cols == -1) { + cols = matrix.cols(); + } else if (cols != matrix.cols()) { + throw std::invalid_argument("All matrices must have the same number of columns."); + } + } + + Eigen::MatrixXd result(totalRows, cols); + + int currentRow = 0; + for (const auto & matrix : matrices) { + // copy each matrix into result + result.block(currentRow, 0, matrix.rows(), cols) = matrix; + currentRow += matrix.rows(); + } + return result; +} + +// concatenate matrices diagonally +Eigen::MatrixXd stackMatricesDiagonally(const std::vector & matrices) +{ + int dimension = 0; + + // calc dimension of result matrix + for (const auto & matrix : matrices) { + if (matrix.rows() != matrix.cols()) { + throw std::invalid_argument("All matrices must be square."); + } + dimension += matrix.rows(); + } + + Eigen::MatrixXd result = Eigen::MatrixXd::Zero(dimension, dimension); + + int currentDimension = 0; + for (const auto & matrix : matrices) { + // copy each matrix into result + result.block(currentDimension, currentDimension, matrix.rows(), matrix.cols()) = matrix; + currentDimension += matrix.rows(); + } + + return result; +} + +} // namespace utils