Skip to content

Commit dc1ed32

Browse files
amadeuszszpre-commit-ci-lite[bot]
authored and
badai-nguyen
committed
feat(perception_utils): add detection class remapper and BEV NMS (autowarefoundation#12786)
* feat(perception_utils): add detection class remapper and BEV NMS Signed-off-by: Amadeusz Szymko <amadeusz.szymko.2@tier4.jp> * fix(perception_utils): keep NMS source objects intact and reject non-finite params Signed-off-by: Amadeusz Szymko <amadeusz.szymko.2@tier4.jp> * style(pre-commit): autofix * refactor(preception_utils): split tests Signed-off-by: Amadeusz Szymko <amadeusz.szymko.2@tier4.jp> * refactor(perception_utils): test utils Signed-off-by: Amadeusz Szymko <amadeusz.szymko.2@tier4.jp> --------- Signed-off-by: Amadeusz Szymko <amadeusz.szymko.2@tier4.jp> Co-authored-by: pre-commit-ci-lite[bot] <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com>
1 parent 27e25a9 commit dc1ed32

9 files changed

Lines changed: 510 additions & 0 deletions

File tree

‎perception/perception_utils/CMakeLists.txt‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,8 @@ find_package(autoware_cmake REQUIRED)
55
autoware_package()
66

77
ament_auto_add_library(${PROJECT_NAME} SHARED
8+
src/detection_class_remapper.cpp
9+
src/iou_bev_nms.cpp
810
src/run_length_encoder.cpp
911
)
1012

@@ -19,6 +21,12 @@ install(
1921
)
2022

2123
if(BUILD_TESTING)
24+
ament_auto_add_gtest(test_iou_bev_nms
25+
test/test_iou_bev_nms.cpp
26+
)
27+
ament_auto_add_gtest(test_detection_class_remapper
28+
test/test_detection_class_remapper.cpp
29+
)
2230
ament_auto_add_gtest(test_utils
2331
test/test_utils.cpp
2432
)

‎perception/perception_utils/include/perception_utils/detection_class_remapper.hpp‎

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
// Copyright 2026 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_UTILS__DETECTION_CLASS_REMAPPER_HPP_
16+
#define PERCEPTION_UTILS__DETECTION_CLASS_REMAPPER_HPP_
17+
18+
#include <autoware_perception_msgs/msg/detected_objects.hpp>
19+
20+
#include <cstddef>
21+
#include <cstdint>
22+
#include <vector>
23+
24+
namespace perception_utils
25+
{
26+
27+
class DetectionClassRemapper
28+
{
29+
public:
30+
void setParameters(
31+
const std::vector<std::int64_t> & allow_remapping_by_area_matrix,
32+
const std::vector<double> & min_area_matrix, const std::vector<double> & max_area_matrix);
33+
34+
void mapClasses(autoware_perception_msgs::msg::DetectedObjects & msg) const;
35+
36+
private:
37+
std::size_t matrixIndex(std::size_t source_label, std::size_t destination_label) const;
38+
39+
std::vector<bool> allow_remapping_by_area_matrix_;
40+
std::vector<double> min_area_matrix_;
41+
std::vector<double> max_area_matrix_;
42+
std::size_t num_labels_{};
43+
};
44+
45+
} // namespace perception_utils
46+
47+
#endif // PERCEPTION_UTILS__DETECTION_CLASS_REMAPPER_HPP_

‎perception/perception_utils/include/perception_utils/iou_bev_nms.hpp‎

Lines changed: 51 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,51 @@
1+
// Copyright 2026 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_UTILS__IOU_BEV_NMS_HPP_
16+
#define PERCEPTION_UTILS__IOU_BEV_NMS_HPP_
17+
18+
#include <autoware_perception_msgs/msg/detected_object.hpp>
19+
20+
#include <vector>
21+
22+
namespace perception_utils
23+
{
24+
25+
struct IouBevNmsParams
26+
{
27+
double search_distance_2d{};
28+
double iou_threshold{};
29+
};
30+
31+
class IouBevNms
32+
{
33+
public:
34+
void setParameters(const IouBevNmsParams & params);
35+
36+
std::vector<autoware_perception_msgs::msg::DetectedObject> apply(
37+
const std::vector<autoware_perception_msgs::msg::DetectedObject> & input_objects,
38+
bool sort = false) const;
39+
40+
private:
41+
bool isTargetPairObject(
42+
const autoware_perception_msgs::msg::DetectedObject & object1,
43+
const autoware_perception_msgs::msg::DetectedObject & object2) const;
44+
45+
IouBevNmsParams params_{};
46+
double search_distance_2d_sq_{};
47+
};
48+
49+
} // namespace perception_utils
50+
51+
#endif // PERCEPTION_UTILS__IOU_BEV_NMS_HPP_

‎perception/perception_utils/package.xml‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,9 @@
1212
<buildtool_depend>ament_cmake_auto</buildtool_depend>
1313
<buildtool_depend>autoware_cmake</buildtool_depend>
1414

15+
<depend>autoware_object_recognition_utils</depend>
16+
<depend>autoware_perception_msgs</depend>
17+
<depend>autoware_utils_geometry</depend>
1518
<depend>libopencv-dev</depend>
1619
<depend>rclcpp</depend>
1720

‎perception/perception_utils/src/detection_class_remapper.cpp‎

Lines changed: 88 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,88 @@
1+
// Copyright 2026 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+
#include "perception_utils/detection_class_remapper.hpp"
16+
17+
#include <algorithm>
18+
#include <cmath>
19+
#include <limits>
20+
#include <stdexcept>
21+
#include <vector>
22+
23+
namespace perception_utils
24+
{
25+
26+
void DetectionClassRemapper::setParameters(
27+
const std::vector<std::int64_t> & allow_remapping_by_area_matrix,
28+
const std::vector<double> & min_area_matrix, const std::vector<double> & max_area_matrix)
29+
{
30+
if (
31+
allow_remapping_by_area_matrix.size() != min_area_matrix.size() ||
32+
allow_remapping_by_area_matrix.size() != max_area_matrix.size()) {
33+
throw std::invalid_argument("Class remapping matrices must have equal sizes.");
34+
}
35+
36+
const auto num_labels = static_cast<std::size_t>(std::sqrt(min_area_matrix.size()));
37+
if (num_labels == 0 || num_labels * num_labels != min_area_matrix.size()) {
38+
throw std::invalid_argument("Class remapping matrices must be non-empty and square.");
39+
}
40+
41+
num_labels_ = num_labels;
42+
allow_remapping_by_area_matrix_.assign(
43+
allow_remapping_by_area_matrix.begin(), allow_remapping_by_area_matrix.end());
44+
min_area_matrix_ = min_area_matrix;
45+
max_area_matrix_ = max_area_matrix;
46+
47+
const auto replace_non_finite = [](double value) {
48+
return std::isfinite(value) ? value : std::numeric_limits<double>::max();
49+
};
50+
std::transform(
51+
min_area_matrix_.begin(), min_area_matrix_.end(), min_area_matrix_.begin(), replace_non_finite);
52+
std::transform(
53+
max_area_matrix_.begin(), max_area_matrix_.end(), max_area_matrix_.begin(), replace_non_finite);
54+
}
55+
56+
std::size_t DetectionClassRemapper::matrixIndex(
57+
const std::size_t source_label, const std::size_t destination_label) const
58+
{
59+
return source_label * num_labels_ + destination_label;
60+
}
61+
62+
void DetectionClassRemapper::mapClasses(autoware_perception_msgs::msg::DetectedObjects & msg) const
63+
{
64+
for (auto & object : msg.objects) {
65+
const float bev_area = object.shape.dimensions.x * object.shape.dimensions.y;
66+
67+
for (auto & classification : object.classification) {
68+
auto & label = classification.label;
69+
const auto source_label = static_cast<std::size_t>(label);
70+
if (source_label >= num_labels_) {
71+
continue;
72+
}
73+
74+
for (std::size_t destination_label = 0; destination_label < num_labels_;
75+
++destination_label) {
76+
const auto index = matrixIndex(source_label, destination_label);
77+
if (
78+
allow_remapping_by_area_matrix_.at(index) && bev_area >= min_area_matrix_.at(index) &&
79+
bev_area <= max_area_matrix_.at(index)) {
80+
label = static_cast<std::uint8_t>(destination_label);
81+
break;
82+
}
83+
}
84+
}
85+
}
86+
}
87+
88+
} // namespace perception_utils

‎perception/perception_utils/src/iou_bev_nms.cpp‎

Lines changed: 106 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,106 @@
1+
// Copyright 2026 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+
#include "perception_utils/iou_bev_nms.hpp"
16+
17+
#include <autoware/object_recognition_utils/geometry.hpp>
18+
#include <autoware/object_recognition_utils/object_recognition_utils.hpp>
19+
#include <autoware_utils_geometry/geometry.hpp>
20+
21+
#include <algorithm>
22+
#include <cmath>
23+
#include <stdexcept>
24+
#include <vector>
25+
26+
namespace perception_utils
27+
{
28+
namespace
29+
{
30+
31+
using DetectedObject = autoware_perception_msgs::msg::DetectedObject;
32+
using Label = autoware_perception_msgs::msg::ObjectClassification;
33+
34+
} // namespace
35+
36+
void IouBevNms::setParameters(const IouBevNmsParams & params)
37+
{
38+
if (!std::isfinite(params.search_distance_2d) || params.search_distance_2d < 0.0) {
39+
throw std::invalid_argument("search_distance_2d must be a finite non-negative value.");
40+
}
41+
if (
42+
!std::isfinite(params.iou_threshold) || params.iou_threshold < 0.0 ||
43+
params.iou_threshold > 1.0) {
44+
throw std::invalid_argument("iou_threshold must be a finite value between 0 and 1.");
45+
}
46+
47+
params_ = params;
48+
search_distance_2d_sq_ = params.search_distance_2d * params.search_distance_2d;
49+
}
50+
51+
bool IouBevNms::isTargetPairObject(
52+
const DetectedObject & object1, const DetectedObject & object2) const
53+
{
54+
const auto label1 =
55+
autoware::object_recognition_utils::getHighestProbLabel(object1.classification);
56+
const auto label2 =
57+
autoware::object_recognition_utils::getHighestProbLabel(object2.classification);
58+
59+
if (label1 != label2 && (label1 == Label::PEDESTRIAN || label2 == Label::PEDESTRIAN)) {
60+
return false;
61+
}
62+
63+
const auto sqr_dist_2d = autoware_utils_geometry::calc_squared_distance2d(
64+
autoware::object_recognition_utils::getPose(object1),
65+
autoware::object_recognition_utils::getPose(object2));
66+
return sqr_dist_2d <= search_distance_2d_sq_;
67+
}
68+
69+
std::vector<DetectedObject> IouBevNms::apply(
70+
const std::vector<DetectedObject> & input_objects, const bool sort) const
71+
{
72+
std::vector<DetectedObject> ordered_objects = input_objects;
73+
if (sort) {
74+
std::stable_sort(
75+
ordered_objects.begin(), ordered_objects.end(), [](const auto & lhs, const auto & rhs) {
76+
return lhs.existence_probability > rhs.existence_probability;
77+
});
78+
}
79+
80+
std::vector<DetectedObject> output_objects;
81+
output_objects.reserve(ordered_objects.size());
82+
for (std::size_t target_i = 0; target_i < ordered_objects.size(); ++target_i) {
83+
double max_iou = 0.0;
84+
for (std::size_t source_i = 0; source_i < target_i; ++source_i) {
85+
const auto & target_object = ordered_objects.at(target_i);
86+
const auto & source_object = ordered_objects.at(source_i);
87+
if (!isTargetPairObject(target_object, source_object)) {
88+
continue;
89+
}
90+
91+
const double iou = autoware::object_recognition_utils::get2dIoU(target_object, source_object);
92+
max_iou = std::max(max_iou, iou);
93+
if (iou > params_.iou_threshold) {
94+
break;
95+
}
96+
}
97+
98+
if (max_iou <= params_.iou_threshold) {
99+
output_objects.emplace_back(ordered_objects.at(target_i));
100+
}
101+
}
102+
103+
return output_objects;
104+
}
105+
106+
} // namespace perception_utils

‎perception/perception_utils/test/detection_test_utils.hpp‎

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
// Copyright 2026 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 DETECTION_TEST_UTILS_HPP_
16+
#define DETECTION_TEST_UTILS_HPP_
17+
18+
#include <autoware_utils_geometry/geometry.hpp>
19+
20+
#include <autoware_perception_msgs/msg/detected_object.hpp>
21+
22+
#include <cstdint>
23+
24+
namespace perception_utils::test
25+
{
26+
27+
inline autoware_perception_msgs::msg::DetectedObject make_object(
28+
const double x, const double y, const float length, const float width, const std::uint8_t label,
29+
const float score)
30+
{
31+
using autoware_perception_msgs::msg::DetectedObject;
32+
using autoware_perception_msgs::msg::ObjectClassification;
33+
using autoware_perception_msgs::msg::Shape;
34+
35+
DetectedObject object;
36+
object.existence_probability = score;
37+
object.classification.push_back(ObjectClassification{}.set__label(label).set__probability(1.0F));
38+
object.kinematics.pose_with_covariance.pose.position =
39+
autoware_utils_geometry::create_point(x, y, 0.0);
40+
object.kinematics.pose_with_covariance.pose.orientation =
41+
autoware_utils_geometry::create_quaternion_from_yaw(0.0);
42+
object.shape.type = Shape::BOUNDING_BOX;
43+
object.shape.dimensions = autoware_utils_geometry::create_translation(length, width, 1.0);
44+
return object;
45+
}
46+
47+
} // namespace perception_utils::test
48+
49+
#endif // DETECTION_TEST_UTILS_HPP_

0 commit comments

Comments
 (0)