Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

feat(object_merger): merge known objects #8718

Draft
wants to merge 4 commits into
base: main
Choose a base branch
from
+113 −18
Draft

feat(object_merger): merge known objects #8718

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
28ec1dd
add feat:merge known object
emuemuJP Sep 2, 2024
e8caf27
fix class name diff
emuemuJP Sep 2, 2024
4dc278e
Apply clang-format changes
emuemuJP Sep 2, 2024
d534972
add params for known object merge
emuemuJP Sep 7, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 5 additions & 2 deletions perception/autoware_object_merger/config/object_association_merger.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,8 +1,11 @@
/**:
ros__parameters:
sync_queue_size: 20
precision_threshold_to_judge_overlapped: 0.4
recall_threshold_to_judge_overlapped: 0.5
precision_threshold_to_judge_overlapped_unknown: 0.4
precision_threshold_to_judge_overlapped_known: 0.4
recall_threshold_to_judge_overlapped_unknown: 0.5
recall_threshold_to_judge_overlapped_known: 0.5
remove_overlapped_unknown_objects: true
remove_overlapped_known_objects: true
base_link_frame_id: base_link
priority_mode: 3 # PriorityMode::Confidence
11 changes: 9 additions & 2 deletions perception/autoware_object_merger/config/overlapped_judge.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,8 +1,15 @@
/**:
ros__parameters:
distance_threshold_list:
unknown_obj_distance_threshold_list:
#UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN
[9.0, 9.0, 9.0, 9.0, 9.0, 9.0, 9.0, 9.0] #UNKNOWN

generalized_iou_threshold:
unknown_obj_generalized_iou_threshold:
[-0.1, -0.1, -0.1, -0.6, -0.6, -0.1, -0.1, -0.1]

known_obj_distance_threshold_list:
#UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN
[9.0, 9.0, 9.0, 9.0, 9.0, 9.0, 9.0, 9.0] #KNOWN

known_obj_generalized_iou_threshold:
[-0.1, -0.1, -0.1, -0.6, -0.6, -0.1, -0.1, -0.1]
7 changes: 5 additions & 2 deletions .../autoware_object_merger/include/autoware/object_merger/object_association_merger_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,13 +75,16 @@ class ObjectAssociationMergerNode : public rclcpp::Node

PriorityMode priority_mode_;
bool remove_overlapped_unknown_objects_;
struct
bool remove_overlapped_known_objects_;
using OverlappedJudgeParam = struct
{
double precision_threshold;
double recall_threshold;
std::map<int, double> generalized_iou_threshold;
std::map<int /*class label*/, double /*distance_threshold*/> distance_threshold_map;
} overlapped_judge_param_;
};

OverlappedJudgeParam overlapped_unknown_obj_judge_param_, overlapped_known_obj_judge_param_;

// debug publisher
std::unique_ptr<autoware::universe_utils::DebugPublisher> processing_time_publisher_;
Expand Down
106 changes: 94 additions & 12 deletions perception/autoware_object_merger/src/object_association_merger_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,34 @@
return precision > precision_threshold || recall > recall_threshold ||
generalized_iou > generalized_iou_threshold;
}

bool isKnownObjectOverlapped(
const autoware_perception_msgs::msg::DetectedObject & object1,
const autoware_perception_msgs::msg::DetectedObject & object2, const double precision_threshold,
const double recall_threshold, const std::map<int, double> & distance_threshold_map,
const std::map<int, double> & generalized_iou_threshold_map)
{
int object1_label = object_recognition_utils::getHighestProbLabel(object1.classification);
int object2_label = object_recognition_utils::getHighestProbLabel(object2.classification);

const double generalized_iou_threshold = std::min(
generalized_iou_threshold_map.at(object1_label),
generalized_iou_threshold_map.at(object2_label));
const double distance_threshold =
std::min(distance_threshold_map.at(object1_label), distance_threshold_map.at(object2_label));

const double sq_distance_threshold = std::pow(distance_threshold, 2.0);
const double sq_distance = autoware::universe_utils::calcSquaredDistance2d(
object1.kinematics.pose_with_covariance.pose, object2.kinematics.pose_with_covariance.pose);
if (sq_distance_threshold < sq_distance) return false;

const auto precision = object_recognition_utils::get2dPrecision(object1, object2);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please use a different set of parameters for thresholds.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

  • Added param for known objects.

  • Added unknown keywords to existing parameter names.

const auto recall = object_recognition_utils::get2dRecall(object1, object2);
const auto generalized_iou = object_recognition_utils::get2dGeneralizedIoU(object1, object2);

return precision > precision_threshold || recall > recall_threshold ||
generalized_iou > generalized_iou_threshold;
}

Check notice on line 85 in perception/autoware_object_merger/src/object_association_merger_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ New issue: Excess Number of Function Arguments 

isKnownObjectOverlapped has 6 arguments, threshold = 4. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.
} // namespace

namespace
Expand Down Expand Up @@ -85,20 +113,32 @@
priority_mode_ = static_cast<PriorityMode>(declare_parameter<int>("priority_mode"));
sync_queue_size_ = declare_parameter<int>("sync_queue_size");
remove_overlapped_unknown_objects_ = declare_parameter<bool>("remove_overlapped_unknown_objects");
overlapped_judge_param_.precision_threshold =
declare_parameter<double>("precision_threshold_to_judge_overlapped");
overlapped_judge_param_.recall_threshold =
declare_parameter<double>("recall_threshold_to_judge_overlapped");
overlapped_judge_param_.generalized_iou_threshold =
convertListToClassMap(declare_parameter<std::vector<double>>("generalized_iou_threshold"));

// unknown obj param
overlapped_unknown_obj_judge_param_.precision_threshold =
declare_parameter<double>("precision_threshold_to_judge_overlapped_unknown");
overlapped_unknown_obj_judge_param_.recall_threshold =
declare_parameter<double>("recall_threshold_to_judge_overlapped_unknown", 0.5);
overlapped_unknown_obj_judge_param_.generalized_iou_threshold =
convertListToClassMap(declare_parameter<std::vector<double>>("unknown_obj_generalized_iou_threshold"));

// get distance_threshold_map from distance_threshold_list
/** TODO(Shin-kyoto):
* this implementation assumes index of vector shows class_label.
* if param supports map, refactor this code.
*/
overlapped_judge_param_.distance_threshold_map =
convertListToClassMap(declare_parameter<std::vector<double>>("distance_threshold_list"));
overlapped_unknown_obj_judge_param_.distance_threshold_map =
convertListToClassMap(declare_parameter<std::vector<double>>("unknown_obj_distance_threshold_list"));

// known obj param
overlapped_known_obj_judge_param_.precision_threshold =
declare_parameter<double>("precision_threshold_to_judge_overlapped_known");
overlapped_known_obj_judge_param_.recall_threshold =
declare_parameter<double>("recall_threshold_to_judge_overlapped_known", 0.5);
overlapped_known_obj_judge_param_.generalized_iou_threshold =
convertListToClassMap(declare_parameter<std::vector<double>>("known_obj_generalized_iou_threshold"));
overlapped_known_obj_judge_param_.distance_threshold_map =
convertListToClassMap(declare_parameter<std::vector<double>>("known_obj_distance_threshold_list"));

const auto tmp = this->declare_parameter<std::vector<int64_t>>("can_assign_matrix");
const std::vector<int> can_assign_matrix(tmp.begin(), tmp.end());
Expand Down Expand Up @@ -210,20 +250,62 @@
bool is_overlapped = false;
for (const auto & known_object : known_objects) {
if (isUnknownObjectOverlapped(
unknown_object, known_object, overlapped_judge_param_.precision_threshold,
overlapped_judge_param_.recall_threshold,
overlapped_judge_param_.distance_threshold_map,
overlapped_judge_param_.generalized_iou_threshold)) {
unknown_object, known_object, overlapped_unknown_obj_judge_param_.precision_threshold,
overlapped_unknown_obj_judge_param_.recall_threshold,
overlapped_unknown_obj_judge_param_.distance_threshold_map,
overlapped_unknown_obj_judge_param_.generalized_iou_threshold)) {
is_overlapped = true;
break;
}
}
if (!is_overlapped) {
output_msg.objects.push_back(unknown_object);
}
}
}

// Remove overlapped known object
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

  1. Please integrate the two pieces of code; unknown objects and known objects now have similar coding structures. Please integrate them together.
  2. Regarding efficiency, please optimize the looping, for now, all objects will be compared twice.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Generally similar, but there are some minor differences that may not be as significant as the benefits of putting them together.
First, sorting improves code efficiency.

if (remove_overlapped_known_objects_) {
std::vector<autoware_perception_msgs::msg::DetectedObject> unknown_objects, known_objects;
unknown_objects.reserve(output_msg.objects.size());
known_objects.reserve(output_msg.objects.size());
for (const auto & object : output_msg.objects) {
if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) {
unknown_objects.push_back(object);
} else {
known_objects.push_back(object);
}
}

std::sort(known_objects.begin(), known_objects.end(),
[](const auto& a, const auto& b) {
return a.kinematics.pose_with_covariance.pose.position.x < b.kinematics.pose_with_covariance.pose.position.x;
});

output_msg.objects.clear();
output_msg.objects = unknown_objects;
for (size_t i = 0; i < known_objects.size(); ++i) {
bool should_keep = true;
for (size_t j = i + 1; j < known_objects.size(); ++j) {
if (isKnownObjectOverlapped(known_objects[i], known_objects[j],
overlapped_known_obj_judge_param_.precision_threshold,
overlapped_known_obj_judge_param_.recall_threshold,
overlapped_known_obj_judge_param_.distance_threshold_map,
overlapped_known_obj_judge_param_.generalized_iou_threshold)) {
auto label_i = object_recognition_utils::getHighestProbClassification(known_objects[i].classification);
auto label_j = object_recognition_utils::getHighestProbClassification(known_objects[j].classification);
if (label_i.probability * known_objects[i].existence_probability <= label_j.probability * known_objects[j].existence_probability) {
should_keep = false;
break;
}
}
if (should_keep) {
output_msg.objects.push_back(known_objects[i]);
}
}
}
}

Check warning on line 308 in perception/autoware_object_merger/src/object_association_merger_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

ObjectAssociationMergerNode::objectsCallback increases in cyclomatic complexity from 19 to 27, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check warning on line 308 in perception/autoware_object_merger/src/object_association_merger_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Bumpy Road Ahead 

ObjectAssociationMergerNode::objectsCallback increases from 4 to 6 logical blocks with deeply nested code, threshold is one single block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.

Check notice on line 308 in perception/autoware_object_merger/src/object_association_merger_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Deep, Nested Complexity 

ObjectAssociationMergerNode::objectsCallback increases in nested complexity depth from 4 to 5, threshold = 4. This function contains deeply nested logic such as if statements and/or loops. The deeper the nesting, the lower the code health.
// publish output msg
merged_object_pub_->publish(output_msg);
published_time_publisher_->publish_if_subscribed(merged_object_pub_, output_msg.header.stamp);
Expand Down
Loading