Skip to content

Commit e07908c

Browse files
kosuke55badai-nguyen
kosuke55
authored and
badai-nguyen
committed
feat(perception_evaluator): counts objects within detection range
Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> detection counter Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> add enable option and refactoring Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> fix update document Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> readme Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> clean up Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
1 parent 02d946a commit e07908c

15 files changed

+1694
-191
lines changed

‎evaluator/perception_online_evaluator/CMakeLists.txt

+2-1
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@ ament_auto_add_library(${PROJECT_NAME}_node SHARED
1212
src/metrics_calculator.cpp
1313
src/${PROJECT_NAME}_node.cpp
1414
src/metrics/deviation_metrics.cpp
15+
src/metrics/detection_count.cpp
1516
src/utils/marker_utils.cpp
1617
src/utils/objects_filtering.cpp
1718
)
@@ -31,7 +32,7 @@ target_link_libraries(${PROJECT_NAME}_node glog::glog)
3132
if(BUILD_TESTING)
3233
ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
3334
test/test_perception_online_evaluator_node.cpp
34-
TIMEOUT 180
35+
TIMEOUT 300
3536
)
3637
target_link_libraries(test_${PROJECT_NAME}
3738
${PROJECT_NAME}_node

‎evaluator/perception_online_evaluator/README.md

+82-8
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,9 @@ The evaluated metrics are as follows:
1515
- lateral_deviation
1616
- yaw_deviation
1717
- yaw_rate
18+
- total_objects_count
19+
- average_objects_count
20+
- interval_objects_count
1821

1922
### Predicted Path Deviation / Predicted Path Deviation Variance
2023

@@ -89,6 +92,67 @@ Calculates the yaw rate of an object based on the change in yaw angle from the p
8992

9093
![yaw_rate](./images/yaw_rate.drawio.svg)
9194

95+
### Object Counts
96+
97+
Counts the number of detections for each object class within the specified detection range. These metrics are measured for the most recent object not past objects.
98+
99+
![detection_counts](./images/detection_counts.drawio.svg)
100+
101+
In the provided illustration, the range \( R \) is determined by a combination of lists of radii (e.g., \( r_1, r_2, \ldots \)) and heights (e.g., \( h_1, h_2, \ldots \)).
102+
For example,
103+
104+
- the number of CAR in range \( R = (r_1, h_1) \) equals 1
105+
- the number of CAR in range \( R = (r_1, h_2) \) equals 2
106+
- the number of CAR in range \( R = (r_2, h_1) \) equals 3
107+
- the number of CAR in range \( R = (r_2, h_2) \) equals 4
108+
109+
#### Total Object Count
110+
111+
Counts the number of unique objects for each class within the specified detection range. The total object count is calculated as follows:
112+
113+
$$
114+
\begin{align}
115+
\text{Total Object Count (Class, Range)} = \left| \bigcup_{t=0}^{T_{\text{now}}} \{ \text{uuid} \mid \text{class}(t, \text{uuid}) = C \wedge \text{position}(t, \text{uuid}) \in R \} \right|
116+
\end{align}
117+
$$
118+
119+
where:
120+
121+
- \( \bigcup \) represents the union across all frames from \( t = 0 \) to \( T\_{\text{now}} \), which ensures that each uuid is counted only once.
122+
- \( \text{class}(t, \text{uuid}) = C \) specifies that the object with uuid at time \( t \) belongs to class \( C \).
123+
- \( \text{position}(t, \text{uuid}) \in R \) indicates that the object with uuid at time \( t \) is within the specified range \( R \).
124+
- \( \left| \{ \ldots \} \right| \) denotes the cardinality of the set, which counts the number of unique uuids that meet the class and range criteria across all considered times.
125+
126+
#### Average Object Count
127+
128+
Counts the average number of objects for each class within the specified detection range. This metric measures how many objects were detected in one frame, without considering uuids. The average object count is calculated as follows:
129+
130+
$$
131+
\begin{align}
132+
\text{Average Object Count (Class, Range)} = \frac{1}{N} \sum_{t=0}^{T_{\text{now}}} \left| \{ \text{object} \mid \text{class}(t, \text{object}) = C \wedge \text{position}(t, \text{object}) \in R \} \right|
133+
\end{align}
134+
$$
135+
136+
where:
137+
138+
- \( N \) represents the total number of frames within the time period time to \( T\_{\text{now}} \) (it is precisely `detection_count_purge_seconds`)
139+
- \(\text{object}\) denotes the number of objects that meet the class and range criteria at time \( t \).
140+
141+
#### Interval Object Count
142+
143+
Counts the average number of objects for each class within the specified detection range over the last `objects_count_window_seconds`. This metric measures how many objects were detected in one frame, without considering uuids. The interval object count is calculated as follows:
144+
145+
$$
146+
\begin{align}
147+
\text{Interval Object Count (Class, Range)} = \frac{1}{W} \sum_{t=T_{\text{now}} - T_W}^{T_{\text{now}}} \left| \{ \text{object} \mid \text{class}(t, \text{object}) = C \wedge \text{position}(t, \text{object}) \in R \} \right|
148+
\end{align}
149+
$$
150+
151+
where:
152+
153+
- \( W \) represents the total number of frames within the last `objects_count_window_seconds`.
154+
- \( T_W \) represents the time window `objects_count_window_seconds`
155+
92156
## Inputs / Outputs
93157

94158
| Name | Type | Description |
@@ -99,14 +163,24 @@ Calculates the yaw rate of an object based on the change in yaw angle from the p
99163

100164
## Parameters
101165

102-
| Name | Type | Description |
103-
| --------------------------------- | ------------ | ------------------------------------------------------------------------------------------------ |
104-
| `selected_metrics` | List | Metrics to be evaluated, such as lateral deviation, yaw deviation, and predicted path deviation. |
105-
| `smoothing_window_size` | Integer | Determines the window size for smoothing path, should be an odd number. |
106-
| `prediction_time_horizons` | list[double] | Time horizons for prediction evaluation in seconds. |
107-
| `stopped_velocity_threshold` | double | threshold velocity to check if vehicle is stopped |
108-
| `target_object.*.check_deviation` | bool | Whether to check deviation for specific object types (car, truck, etc.). |
109-
| `debug_marker.*` | bool | Debugging parameters for marker visualization (history path, predicted path, etc.). |
166+
| Name | Type | Description |
167+
| ------------------------------------------------------ | ------------ | ----------------------------------------------------------------------------------------------------------------------------------------------- |
168+
| `selected_metrics` | List | Metrics to be evaluated, such as lateral deviation, yaw deviation, and predicted path deviation. |
169+
| `smoothing_window_size` | Integer | Determines the window size for smoothing path, should be an odd number. |
170+
| `prediction_time_horizons` | list[double] | Time horizons for prediction evaluation in seconds. |
171+
| `stopped_velocity_threshold` | double | threshold velocity to check if vehicle is stopped |
172+
| `detection_radius_list` | double | Detection radius for objects to be evaluated.(used for objects count only) |
173+
| `detection_height_list` | double | Detection height for objects to be evaluated. (used for objects count only) |
174+
| `detection_count_purge_seconds` | double | Time window for purging object detection counts. |
175+
| `objects_count_window_seconds` | double | Time window for keeping object detection counts. The number of object detections within this time window is stored in `detection_count_vector_` |
176+
| `target_object.*.check_lateral_deviation` | bool | Whether to check lateral deviation for specific object types (car, truck, etc.). |
177+
| `target_object.*.check_yaw_deviation` | bool | Whether to check yaw deviation for specific object types (car, truck, etc.). |
178+
| `target_object.*.check_predicted_path_deviation` | bool | Whether to check predicted path deviation for specific object types (car, truck, etc.). |
179+
| `target_object.*.check_yaw_rate` | bool | Whether to check yaw rate for specific object types (car, truck, etc.). |
180+
| `target_object.*.check_total_objects_count` | bool | Whether to check total object count for specific object types (car, truck, etc.). |
181+
| `target_object.*.check_average_objects_count` | bool | Whether to check average object count for specific object types (car, truck, etc.). |
182+
| `target_object.*.check_interval_average_objects_count` | bool | Whether to check interval average object count for specific object types (car, truck, etc.). |
183+
| `debug_marker.*` | bool | Debugging parameters for marker visualization (history path, predicted path, etc.). |
110184

111185
## Assumptions / Known limits
112186

‎evaluator/perception_online_evaluator/images/detection_counts.drawio.svg

+445
Loading

‎evaluator/perception_online_evaluator/images/yaw_rate.drawio.svg

+153-19
Loading

‎evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/detection_count.hpp

+145
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,145 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef PERCEPTION_ONLINE_EVALUATOR__METRICS__DETECTION_COUNT_HPP_
16+
#define PERCEPTION_ONLINE_EVALUATOR__METRICS__DETECTION_COUNT_HPP_
17+
18+
#include "perception_online_evaluator/parameters.hpp"
19+
#include "perception_online_evaluator/stat.hpp"
20+
#include "tf2_ros/buffer.h"
21+
22+
#include <rclcpp/rclcpp.hpp>
23+
24+
#include "autoware_auto_perception_msgs/msg/object_classification.hpp"
25+
#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
26+
#include <geometry_msgs/msg/pose.hpp>
27+
28+
#include <iomanip>
29+
#include <sstream>
30+
#include <vector>
31+
32+
namespace perception_diagnostics
33+
{
34+
namespace metrics
35+
{
36+
using autoware_auto_perception_msgs::msg::ObjectClassification;
37+
using autoware_auto_perception_msgs::msg::PredictedObjects;
38+
39+
struct DetectionRange
40+
{
41+
double radius;
42+
double height;
43+
44+
DetectionRange(double radius, double height) : radius(radius), height(height) {}
45+
46+
std::string toString() const
47+
{
48+
std::ostringstream oss;
49+
oss << std::fixed << std::setprecision(2);
50+
oss << "r" << radius << "_h" << height;
51+
return oss.str();
52+
}
53+
};
54+
using DetectionRanges = std::vector<DetectionRange>;
55+
56+
/*
57+
* @brief Class to count objects detected within a certain range for each class
58+
* The overall frame is not eternal, and data older than the purge seconds is deleted to prevent
59+
memory bloat.
60+
1. Count the total number of unique objects detected across the overall frame. Data older than the
61+
purge seconds is deleted to prevent memory bloat.
62+
2. Calculate the average number of objects detected in each frame across the overall frame.
63+
3. Calculate the average number of objects detected in each frame within a certain time frame.
64+
*/
65+
class DetectionCounter
66+
{
67+
public:
68+
/*
69+
* @brief Constructor
70+
* @param parameters Parameters for the perception online evaluator
71+
*/
72+
explicit DetectionCounter(const std::shared_ptr<Parameters> & parameters)
73+
: parameters_(parameters)
74+
{
75+
}
76+
/*
77+
* @brief Add objects to the detection counter
78+
* @param objects Predicted objects
79+
* @param tf_buffer tf2 buffer for transforming object poses
80+
*/
81+
void addObjects(const PredictedObjects & objects, const tf2_ros::Buffer & tf_buffer);
82+
83+
/*
84+
* @brief Get the average count of objects detected in the last `seconds` seconds
85+
* @param seconds Time window in seconds
86+
* @return Map of classification to range to average count
87+
*/
88+
std::unordered_map<std::uint8_t, std::unordered_map<std::string, double>> getAverageCount(
89+
double seconds) const;
90+
91+
/*
92+
* @brief Get the overall average count of objects detected
93+
* @return Map of classification to range to average count
94+
*/
95+
std::unordered_map<std::uint8_t, std::unordered_map<std::string, double>> getOverallAverageCount()
96+
const;
97+
98+
/*
99+
* @brief Get the total count of objects detected
100+
* @return Map of classification to range to total count
101+
*/
102+
std::unordered_map<std::uint8_t, std::unordered_map<std::string, size_t>> getTotalCount() const;
103+
104+
private:
105+
/*
106+
* @brief Initialize the detection map
107+
*/
108+
void initializeDetectionMap();
109+
110+
/*
111+
* @brief Update the detection map with a new detection
112+
* @param uuid UUID of the detected object
113+
* @param classification Classification of the detected object
114+
* @param range Range of the detected object
115+
* @param timestamp Timestamp of the detection
116+
*/
117+
void updateDetectionMap(
118+
const std::string uuid, const std::uint8_t classification, const std::string & range,
119+
const rclcpp::Time & timestamp);
120+
121+
/*
122+
* @brief Purge old records from the detection map
123+
* @param current_time Current time
124+
*/
125+
void purgeOldRecords(rclcpp::Time current_time);
126+
127+
/*
128+
* @brief Get the DetectionRanges from parameters
129+
* @return Detection ranges
130+
*/
131+
DetectionRanges getRanges() const;
132+
133+
std::shared_ptr<Parameters> parameters_;
134+
std::set<rclcpp::Time> unique_timestamps_;
135+
136+
// Structures for storing detection counts and UUIDs for uniqueness checks
137+
std::unordered_map<std::uint8_t, std::unordered_map<std::string, std::vector<rclcpp::Time>>>
138+
time_series_counts_;
139+
std::unordered_map<std::uint8_t, std::unordered_map<std::string, std::set<std::string>>>
140+
seen_uuids_;
141+
};
142+
} // namespace metrics
143+
} // namespace perception_diagnostics
144+
145+
#endif // PERCEPTION_ONLINE_EVALUATOR__METRICS__DETECTION_COUNT_HPP_

‎evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp

+7-3
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@ enum class Metric {
3232
yaw_deviation,
3333
predicted_path_deviation,
3434
yaw_rate,
35+
objects_count,
3536
SIZE,
3637
};
3738

@@ -47,20 +48,23 @@ static const std::unordered_map<std::string, Metric> str_to_metric = {
4748
{"lateral_deviation", Metric::lateral_deviation},
4849
{"yaw_deviation", Metric::yaw_deviation},
4950
{"predicted_path_deviation", Metric::predicted_path_deviation},
50-
{"yaw_rate", Metric::yaw_rate}};
51+
{"yaw_rate", Metric::yaw_rate},
52+
{"objects_count", Metric::objects_count}};
5153

5254
static const std::unordered_map<Metric, std::string> metric_to_str = {
5355
{Metric::lateral_deviation, "lateral_deviation"},
5456
{Metric::yaw_deviation, "yaw_deviation"},
5557
{Metric::predicted_path_deviation, "predicted_path_deviation"},
56-
{Metric::yaw_rate, "yaw_rate"}};
58+
{Metric::yaw_rate, "yaw_rate"},
59+
{Metric::objects_count, "objects_count"}};
5760

5861
// Metrics descriptions
5962
static const std::unordered_map<Metric, std::string> metric_descriptions = {
6063
{Metric::lateral_deviation, "Lateral_deviation[m]"},
6164
{Metric::yaw_deviation, "Yaw_deviation[rad]"},
6265
{Metric::predicted_path_deviation, "Predicted_path_deviation[m]"},
63-
{Metric::yaw_rate, "Yaw_rate[rad/s]"}};
66+
{Metric::yaw_rate, "Yaw_rate[rad/s]"},
67+
{Metric::objects_count, "objects_count"}};
6468

6569
namespace details
6670
{

‎evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp

+17-2
Original file line numberDiff line numberDiff line change
@@ -15,21 +15,27 @@
1515
#ifndef PERCEPTION_ONLINE_EVALUATOR__METRICS_CALCULATOR_HPP_
1616
#define PERCEPTION_ONLINE_EVALUATOR__METRICS_CALCULATOR_HPP_
1717

18+
#include "perception_online_evaluator/metrics/detection_count.hpp"
1819
#include "perception_online_evaluator/metrics/deviation_metrics.hpp"
1920
#include "perception_online_evaluator/metrics/metric.hpp"
2021
#include "perception_online_evaluator/parameters.hpp"
2122
#include "perception_online_evaluator/stat.hpp"
2223
#include "perception_online_evaluator/utils/objects_filtering.hpp"
24+
#include "tf2_ros/buffer.h"
2325

2426
#include <rclcpp/time.hpp>
2527

28+
#include "autoware_auto_perception_msgs/msg/object_classification.hpp"
2629
#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
2730
#include "geometry_msgs/msg/pose.hpp"
2831
#include <unique_identifier_msgs/msg/uuid.hpp>
2932

3033
#include <algorithm>
3134
#include <map>
35+
#include <memory>
3236
#include <optional>
37+
#include <string>
38+
#include <unordered_map>
3339
#include <utility>
3440
#include <vector>
3541

@@ -39,6 +45,7 @@ using autoware_auto_perception_msgs::msg::PredictedObject;
3945
using autoware_auto_perception_msgs::msg::PredictedObjects;
4046
using geometry_msgs::msg::Point;
4147
using geometry_msgs::msg::Pose;
48+
using metrics::DetectionCounter;
4249
using unique_identifier_msgs::msg::UUID;
4350

4451
struct ObjectData
@@ -80,7 +87,9 @@ class MetricsCalculator
8087
{
8188
public:
8289
explicit MetricsCalculator(const std::shared_ptr<Parameters> & parameters)
83-
: parameters_(parameters){};
90+
: parameters_(parameters), detection_counter_(parameters)
91+
{
92+
}
8493

8594
/**
8695
* @brief calculate
@@ -92,8 +101,11 @@ class MetricsCalculator
92101
/**
93102
* @brief set the dynamic objects used to calculate obstacle metrics
94103
* @param [in] objects predicted objects
104+
* @param [in] tf_buffer tf buffer
95105
*/
96-
void setPredictedObjects(const PredictedObjects & objects);
106+
void setPredictedObjects(const PredictedObjects & objects, const tf2_ros::Buffer & tf_buffer);
107+
108+
void updateObjectsCountMap(const PredictedObjects & objects, const tf2_ros::Buffer & tf_buffer);
97109

98110
HistoryPathMap getHistoryPathMap() const { return history_path_map_; }
99111
ObjectDataMap getDebugObjectData() const { return debug_target_object_; }
@@ -107,6 +119,8 @@ class MetricsCalculator
107119

108120
rclcpp::Time current_stamp_;
109121

122+
DetectionCounter detection_counter_;
123+
110124
// debug
111125
mutable ObjectDataMap debug_target_object_;
112126

@@ -129,6 +143,7 @@ class MetricsCalculator
129143
PredictedPathDeviationMetrics calcPredictedPathDeviationMetrics(
130144
const PredictedObjects & objects, const double time_horizon) const;
131145
MetricStatMap calcYawRateMetrics(const ClassObjectsMap & class_objects_map) const;
146+
MetricStatMap calcObjectsCountMetrics() const;
132147

133148
bool hasPassedTime(const rclcpp::Time stamp) const;
134149
bool hasPassedTime(const std::string uuid, const rclcpp::Time stamp) const;

0 commit comments

Comments
 (0)