Skip to content

Commit

Permalink
refactor(autoware_multi_object_tracker): add configurable tracker par…
Browse files Browse the repository at this point in the history 
…ameters

Signed-off-by: jkoronczok <[email protected]>
  • Loading branch information
@jakor97
jakor97 committed Dec 11, 2024
1 parent da5284c commit 5749585
Show file tree
Hide file tree
Showing 4 changed files with 82 additions and 54 deletions.
15 changes: 15 additions & 0 deletions perception/autoware_multi_object_tracker/config/multi_object_tracker_node.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,21 @@
enable_delay_compensation: false
consider_odometry_uncertainty: false

# tracker parameters
tracker_lifetime: 1.0 # [s]
min_known_object_removal_iou: 0.1 # [ratio]
min_unknown_object_removal_iou: 0.001 # [ratio]
distance_threshold: 5.0 # [m]
confident_count_threshold:
UNKNOWN: 3
CAR: 3
TRUCK: 3
BUS: 3
TRAILER: 3
MOTORBIKE: 3
BICYCLE: 3
PEDESTRIAN: 3

# debug parameters
publish_processing_time: false
publish_tentative_objects: false
Expand Down
42 changes: 32 additions & 10 deletions perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,12 +64,12 @@ boost::optional<geometry_msgs::msg::Transform> getTransformAnonymous(
return boost::none;
}
}

} // namespace

namespace autoware::multi_object_tracker
{
using Label = autoware_perception_msgs::msg::ObjectClassification;
using LabelType = autoware_perception_msgs::msg::ObjectClassification::_label_type;

MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
: rclcpp::Node("multi_object_tracker", node_options),
Expand Down Expand Up @@ -166,23 +166,45 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)

// Initialize processor
{
std::map<std::uint8_t, std::string> tracker_map;
tracker_map.insert(
TrackerProcessorConfig config;
config.tracker_map.insert(
std::make_pair(Label::CAR, this->declare_parameter<std::string>("car_tracker")));
tracker_map.insert(
config.tracker_map.insert(
std::make_pair(Label::TRUCK, this->declare_parameter<std::string>("truck_tracker")));
tracker_map.insert(
config.tracker_map.insert(
std::make_pair(Label::BUS, this->declare_parameter<std::string>("bus_tracker")));
tracker_map.insert(
config.tracker_map.insert(
std::make_pair(Label::TRAILER, this->declare_parameter<std::string>("trailer_tracker")));
tracker_map.insert(std::make_pair(
config.tracker_map.insert(std::make_pair(
Label::PEDESTRIAN, this->declare_parameter<std::string>("pedestrian_tracker")));
tracker_map.insert(
config.tracker_map.insert(
std::make_pair(Label::BICYCLE, this->declare_parameter<std::string>("bicycle_tracker")));
tracker_map.insert(std::make_pair(
config.tracker_map.insert(std::make_pair(
Label::MOTORCYCLE, this->declare_parameter<std::string>("motorcycle_tracker")));
config.channel_size = input_channel_size_;

// Declare parameters
config.tracker_lifetime = declare_parameter<double>("tracker_lifetime", 1.0);
config.min_known_object_removal_iou = declare_parameter<double>("min_known_object_removal_iou", 0.1);
config.min_unknown_object_removal_iou =
declare_parameter<double>("min_unknown_object_removal_iou", 0.05);
config.distance_threshold = declare_parameter<double>("distance_threshold", 5.0);

// Map from class name to label
std::map<std::string, LabelType> class_name_to_label = {
{"UNKNOWN", Label::UNKNOWN}, {"CAR", Label::CAR},
{"TRUCK", Label::TRUCK}, {"BUS", Label::BUS},
{"TRAILER", Label::TRAILER}, {"MOTORBIKE", Label::MOTORCYCLE},
{"BICYCLE", Label::BICYCLE}, {"PEDESTRIAN", Label::PEDESTRIAN}};

// Declare parameters and initialize confident_count_threshold_map
for (const auto & [class_name, class_label] : class_name_to_label) {
int64_t value = declare_parameter<int64_t>("confident_count_threshold." + class_name, 3);
config.confident_count_threshold[class_label] = static_cast<int>(value);
}

processor_ = std::make_unique<TrackerProcessor>(tracker_map, input_channel_size_);
// Initialize processor with parameters
processor_ = std::make_unique<TrackerProcessor>(config);

Check warning on line 207 in perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Large Method 

MultiObjectTracker::MultiObjectTracker increases from 100 to 115 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.
}

// Data association initialization
Expand Down
52 changes: 20 additions & 32 deletions perception/autoware_multi_object_tracker/src/processor/processor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,21 +29,10 @@ namespace autoware::multi_object_tracker
{

using Label = autoware_perception_msgs::msg::ObjectClassification;
using LabelType = autoware_perception_msgs::msg::ObjectClassification::_label_type;

TrackerProcessor::TrackerProcessor(
const std::map<std::uint8_t, std::string> & tracker_map, const size_t & channel_size)
: tracker_map_(tracker_map), channel_size_(channel_size)
TrackerProcessor::TrackerProcessor(const TrackerProcessorConfig & config) : config_(config)
{
// Set tracker lifetime parameters
max_elapsed_time_ = 1.0; // [s]

// Set tracker overlap remover parameters
min_iou_ = 0.1; // [ratio]
min_iou_for_unknown_object_ = 0.001; // [ratio]
distance_threshold_ = 5.0; // [m]

// Set tracker confidence threshold
confident_count_threshold_ = 3; // [count]
}

void TrackerProcessor::predict(const rclcpp::Time & time)
Expand Down Expand Up @@ -94,34 +83,33 @@ std::shared_ptr<Tracker> TrackerProcessor::createNewTracker(
const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const
{
const std::uint8_t label =
autoware::object_recognition_utils::getHighestProbLabel(object.classification);
if (tracker_map_.count(label) != 0) {
const auto tracker = tracker_map_.at(label);
const LabelType label = autoware::object_recognition_utils::getHighestProbLabel(object.classification);
if (config_.tracker_map.count(label) != 0) {
const auto tracker = config_.tracker_map.at(label);
if (tracker == "bicycle_tracker")
return std::make_shared<BicycleTracker>(
time, object, self_transform, channel_size_, channel_index);
time, object, self_transform, config_.channel_size, channel_index);
if (tracker == "big_vehicle_tracker")
return std::make_shared<BigVehicleTracker>(
time, object, self_transform, channel_size_, channel_index);
time, object, self_transform, config_.channel_size, channel_index);
if (tracker == "multi_vehicle_tracker")
return std::make_shared<MultipleVehicleTracker>(
time, object, self_transform, channel_size_, channel_index);
time, object, self_transform, config_.channel_size, channel_index);
if (tracker == "normal_vehicle_tracker")
return std::make_shared<NormalVehicleTracker>(
time, object, self_transform, channel_size_, channel_index);
time, object, self_transform, config_.channel_size, channel_index);
if (tracker == "pass_through_tracker")
return std::make_shared<PassThroughTracker>(
time, object, self_transform, channel_size_, channel_index);
time, object, self_transform, config_.channel_size, channel_index);
if (tracker == "pedestrian_and_bicycle_tracker")
return std::make_shared<PedestrianAndBicycleTracker>(
time, object, self_transform, channel_size_, channel_index);
time, object, self_transform, config_.channel_size, channel_index);
if (tracker == "pedestrian_tracker")
return std::make_shared<PedestrianTracker>(
time, object, self_transform, channel_size_, channel_index);
time, object, self_transform, config_.channel_size, channel_index);
}
return std::make_shared<UnknownTracker>(
time, object, self_transform, channel_size_, channel_index);
time, object, self_transform, config_.channel_size, channel_index);
}

void TrackerProcessor::prune(const rclcpp::Time & time)
Expand All @@ -136,7 +124,7 @@ void TrackerProcessor::removeOldTracker(const rclcpp::Time & time)
{
// Check elapsed time from last update
for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) {
const bool is_old = max_elapsed_time_ < (*itr)->getElapsedTimeFromLastUpdate(time);
const bool is_old = config_.tracker_lifetime < (*itr)->getElapsedTimeFromLastUpdate(time);
// If the tracker is old, delete it
if (is_old) {
auto erase_itr = itr;
Expand Down Expand Up @@ -167,14 +155,13 @@ void TrackerProcessor::removeOverlappedTracker(const rclcpp::Time & time)
object2.kinematics.pose_with_covariance.pose.position.y);

// If the distance is too large, skip
if (distance > distance_threshold_) {
if (distance > config_.distance_threshold) {
continue;
}

// Check the Intersection over Union (IoU) between the two objects
const double min_union_iou_area = 1e-2;
const auto iou =
autoware::object_recognition_utils::get2dIoU(object1, object2, min_union_iou_area);
const auto iou = autoware::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;
Expand All @@ -183,7 +170,7 @@ void TrackerProcessor::removeOverlappedTracker(const rclcpp::Time & time)
// If both trackers are UNKNOWN, delete the younger tracker
// If one side of the tracker is UNKNOWN, delete UNKNOWN objects
if (label1 == Label::UNKNOWN || label2 == Label::UNKNOWN) {
if (iou > min_iou_for_unknown_object_) {
if (iou > config_.min_unknown_object_removal_iou) {
if (label1 == Label::UNKNOWN && label2 == Label::UNKNOWN) {
if ((*itr1)->getTotalMeasurementCount() < (*itr2)->getTotalMeasurementCount()) {
should_delete_tracker1 = true;
Expand All @@ -197,7 +184,7 @@ void TrackerProcessor::removeOverlappedTracker(const rclcpp::Time & time)
}
}
} else { // If neither object is UNKNOWN, delete the younger tracker
if (iou > min_iou_) {
if (iou > config_.min_known_object_removal_iou) {
if ((*itr1)->getTotalMeasurementCount() < (*itr2)->getTotalMeasurementCount()) {
should_delete_tracker1 = true;
} else {
Expand Down Expand Up @@ -225,7 +212,8 @@ bool TrackerProcessor::isConfidentTracker(const std::shared_ptr<Tracker> & track
// Confidence is determined by counting the number of measurements.
// If the number of measurements is equal to or greater than the threshold, the tracker is
// considered confident.
return tracker->getTotalMeasurementCount() >= confident_count_threshold_;
auto label = tracker->getHighestProbLabel();
return tracker->getTotalMeasurementCount() >= config_.confident_count_threshold.at(label);
}

void TrackerProcessor::getTrackedObjects(
Expand Down
27 changes: 15 additions & 12 deletions perception/autoware_multi_object_tracker/src/processor/processor.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,11 +31,23 @@

namespace autoware::multi_object_tracker
{
using LabelType = autoware_perception_msgs::msg::ObjectClassification::_label_type;

struct TrackerProcessorConfig
{
std::map<LabelType, std::string> tracker_map;
size_t channel_size;
float tracker_lifetime; // [s]
float min_known_object_removal_iou; // ratio [0, 1]
float min_unknown_object_removal_iou; // ratio [0, 1]
double distance_threshold; // [m]
std::map<LabelType, int> confident_count_threshold; // [count]
};

class TrackerProcessor
{
public:
explicit TrackerProcessor(
const std::map<std::uint8_t, std::string> & tracker_map, const size_t & channel_size);
explicit TrackerProcessor(const TrackerProcessorConfig & config);

const std::list<std::shared_ptr<Tracker>> & getListTracker() const { return list_tracker_; }
// tracker processes
Expand All @@ -62,17 +74,8 @@ class TrackerProcessor
void getExistenceProbabilities(std::vector<std::vector<float>> & existence_vectors) const;

private:
std::map<std::uint8_t, std::string> tracker_map_;
TrackerProcessorConfig config_;
std::list<std::shared_ptr<Tracker>> list_tracker_;
const size_t channel_size_;

// parameters
float max_elapsed_time_; // [s]
float min_iou_; // [ratio]
float min_iou_for_unknown_object_; // [ratio]
double distance_threshold_; // [m]
int confident_count_threshold_; // [count]

void removeOldTracker(const rclcpp::Time & time);
void removeOverlappedTracker(const rclcpp::Time & time);
std::shared_ptr<Tracker> createNewTracker(
Expand Down

0 comments on commit 5749585

Please sign in to comment.