forked from autowarefoundation/autoware.universe
-
Notifications
You must be signed in to change notification settings - Fork 32
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat(lidar_centerpoint): implemented a class remapping according to t…
…he detections shape (autowarefoundation#1876) * Implemented a small package to remap detection classes depending on their shape Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * Update perception/detection_class_adapter/include/detection_class_adapter/detection_class_adapter.hpp Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com> * Removed the hardcoded mapping from centerpoint and pointfusion. Fixed the description of the config file Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * Deleted the new package and moved the logic to centerpoint Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * Update perception/lidar_centerpoint/config/detection_class_adapter.param.yaml Co-authored-by: Yusuke Muramatsu <yukke42@users.noreply.github.com> * Changed: adapter->remapper * Update perception/lidar_centerpoint/lib/detection_class_remapper.cpp Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com> * Delted duplicated file Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * Modified the parameters to match autowarefoundation#1710 Now we do not map cars to buses no mather the size Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * Update perception/lidar_centerpoint/config/detection_class_remapper.param.yaml Co-authored-by: Yusuke Muramatsu <yukke42@users.noreply.github.com> Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com> Co-authored-by: Yusuke Muramatsu <yukke42@users.noreply.github.com> Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>
- Loading branch information
Showing
15 changed files
with
191 additions
and
27 deletions.
There are no files selected for viewing
1 change: 0 additions & 1 deletion
1
perception/image_projection_based_fusion/config/pointpainting.param.yaml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
38 changes: 38 additions & 0 deletions
38
perception/lidar_centerpoint/config/detection_class_remapper.param.yaml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,38 @@ | ||
| /**: | ||
| ros__parameters: | ||
| allow_remapping_by_area_matrix: | ||
| # NOTE(kl): We turn all vehicles into trailers if they go over 3x12 [m^2]. | ||
| # NOTE(kl): We turn cars into trucks if they have an area between 2.2 x 5.5 and 3.0 * 12.0 [m^2] | ||
| # row: original class. column: class to remap to | ||
| #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN | ||
| [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN | ||
| 0, 0, 1, 0, 1, 0, 0, 0, #CAR | ||
| 0, 0, 0, 0, 1, 0, 0, 0, #TRUCK | ||
| 0, 0, 0, 0, 1, 0, 0, 0, #BUS | ||
| 0, 0, 0, 0, 0, 0, 0, 0, #TRAILER | ||
| 0, 0, 0, 0, 0, 0, 0, 0, #MOTORBIKE | ||
| 0, 0, 0, 0, 0, 0, 0, 0, #BICYCLE | ||
| 0, 0, 0, 0, 0, 0, 0, 0] #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 | ||
| 0.000, 0.000, 12.100, 0.000, 36.000, 0.000, 0.000, 0.000, #CAR | ||
| 0.000, 0.000, 0.000, 0.000, 36.000, 0.000, 0.000, 0.000, #TRUCK | ||
| 0.000, 0.000, 0.000, 0.000, 36.000, 0.000, 0.000, 0.000, #BUS | ||
| 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #TRAILER | ||
| 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #MOTORBIKE | ||
| 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #BICYCLE | ||
| 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] #PEDESTRIAN | ||
|
|
||
|
|
||
| max_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 | ||
| 0.000, 0.000, 36.000, 0.000, inf, 0.000, 0.000, 0.000, #CAR | ||
| 0.000, 0.000, 0.000, 0.000, inf, 0.000, 0.000, 0.000, #TRUCK | ||
| 0.000, 0.000, 0.000, 0.000, inf, 0.000, 0.000, 0.000, #BUS | ||
| 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #TRAILER | ||
| 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #MOTORBIKE | ||
| 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #BICYCLE | ||
| 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] #PEDESTRIAN |
47 changes: 47 additions & 0 deletions
47
perception/lidar_centerpoint/include/lidar_centerpoint/detection_class_remapper.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,47 @@ | ||
| // Copyright 2022 TIER IV, Inc. | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
|
|
||
| #ifndef LIDAR_CENTERPOINT__DETECTION_CLASS_REMAPPER_HPP_ | ||
| #define LIDAR_CENTERPOINT__DETECTION_CLASS_REMAPPER_HPP_ | ||
|
|
||
| #include <Eigen/Core> | ||
|
|
||
| #include <autoware_auto_perception_msgs/msg/detected_object_kinematics.hpp> | ||
| #include <autoware_auto_perception_msgs/msg/detected_objects.hpp> | ||
| #include <autoware_auto_perception_msgs/msg/object_classification.hpp> | ||
| #include <autoware_auto_perception_msgs/msg/shape.hpp> | ||
|
|
||
| #include <vector> | ||
|
|
||
| namespace centerpoint | ||
| { | ||
|
|
||
| class DetectionClassRemapper | ||
| { | ||
| public: | ||
| void setParameters( | ||
| const std::vector<int64_t> & allow_remapping_by_area_matrix, | ||
| const std::vector<double> & min_area_matrix, const std::vector<double> & max_area_matrix); | ||
| void mapClasses(autoware_auto_perception_msgs::msg::DetectedObjects & msg); | ||
|
|
||
| protected: | ||
| Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> allow_remapping_by_area_matrix_; | ||
| Eigen::MatrixXd min_area_matrix_; | ||
| Eigen::MatrixXd max_area_matrix_; | ||
| int num_labels_; | ||
| }; | ||
|
|
||
| } // namespace centerpoint | ||
|
|
||
| #endif // LIDAR_CENTERPOINT__DETECTION_CLASS_REMAPPER_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
70 changes: 70 additions & 0 deletions
70
perception/lidar_centerpoint/lib/detection_class_remapper.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,70 @@ | ||
| // Copyright 2022 TIER IV, Inc. | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
|
|
||
| #include <lidar_centerpoint/detection_class_remapper.hpp> | ||
|
|
||
| namespace centerpoint | ||
| { | ||
|
|
||
| void DetectionClassRemapper::setParameters( | ||
| const std::vector<int64_t> & allow_remapping_by_area_matrix, | ||
| const std::vector<double> & min_area_matrix, const std::vector<double> & max_area_matrix) | ||
| { | ||
| assert(allow_remapping_by_area_matrix.size() == min_area_matrix.size()); | ||
| assert(allow_remapping_by_area_matrix.size() == max_area_matrix.size()); | ||
| assert(std::pow(std::sqrt(min_area_matrix.size()), 2) == std::sqrt(min_area_matrix.size())); | ||
|
|
||
| num_labels_ = static_cast<int>(std::sqrt(min_area_matrix.size())); | ||
|
|
||
| Eigen::Map<const Eigen::Matrix<int64_t, Eigen::Dynamic, Eigen::Dynamic>> | ||
| allow_remapping_by_area_matrix_tmp( | ||
| allow_remapping_by_area_matrix.data(), num_labels_, num_labels_); | ||
| allow_remapping_by_area_matrix_ = allow_remapping_by_area_matrix_tmp.transpose() | ||
| .cast<bool>(); // Eigen is column major by default | ||
|
|
||
| Eigen::Map<const Eigen::MatrixXd> min_area_matrix_tmp( | ||
| min_area_matrix.data(), num_labels_, num_labels_); | ||
| min_area_matrix_ = min_area_matrix_tmp.transpose(); // Eigen is column major by default | ||
|
|
||
| Eigen::Map<const Eigen::MatrixXd> max_area_matrix_tmp( | ||
| max_area_matrix.data(), num_labels_, num_labels_); | ||
| max_area_matrix_ = max_area_matrix_tmp.transpose(); // Eigen is column major by default | ||
|
|
||
| min_area_matrix_ = min_area_matrix_.unaryExpr( | ||
| [](double v) { return std::isfinite(v) ? v : std::numeric_limits<double>::max(); }); | ||
| max_area_matrix_ = max_area_matrix_.unaryExpr( | ||
| [](double v) { return std::isfinite(v) ? v : std::numeric_limits<double>::max(); }); | ||
| } | ||
|
|
||
| void DetectionClassRemapper::mapClasses(autoware_auto_perception_msgs::msg::DetectedObjects & msg) | ||
| { | ||
| for (auto & object : msg.objects) { | ||
| const float bev_area = object.shape.dimensions.x * object.shape.dimensions.y; | ||
|
|
||
| for (auto & classification : object.classification) { | ||
| auto & label = classification.label; | ||
|
|
||
| for (int i = 0; i < num_labels_; ++i) { | ||
| if ( | ||
| allow_remapping_by_area_matrix_(label, i) && bev_area >= min_area_matrix_(label, i) && | ||
| bev_area <= max_area_matrix_(label, i)) { | ||
| label = i; | ||
| break; | ||
| } | ||
| } | ||
| } | ||
| } | ||
| } | ||
|
|
||
| } // namespace centerpoint |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters