detected_object_associator.hpp

Go to the documentation of this file.

// Copyright 2021 Apex.AI, 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 TRACKING__DETECTED_OBJECT_ASSOCIATOR_HPP_
#define TRACKING__DETECTED_OBJECT_ASSOCIATOR_HPP_
 
#include <tracking/visibility_control.hpp>
 
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
#include <common/types.hpp>
#include <hungarian_assigner/hungarian_assigner.hpp>
#include <tracking/objects_with_associations.hpp>
#include <tracking/tracked_object.hpp>
#include <tracking/tracker_types.hpp>
 
#include <experimental/optional>
#include <map>
#include <vector>
 
namespace autoware
{
namespace perception
{
namespace tracking
{
 
using assigner_idx_t = autoware::fusion::hungarian_assigner::index_t;
 
class TRACKING_PUBLIC DataAssociationConfig
{
public:
  DataAssociationConfig(
    const float32_t max_distance, const float32_t max_area_ratio,
    const bool consider_edge_for_big_detections);
 
  inline float32_t get_max_distance() const {return m_max_distance;}
 
  inline float32_t get_max_distance_squared() const {return m_max_distance_squared;}
 
  inline float32_t get_max_area_ratio() const {return m_max_area_ratio;}
 
  inline float32_t get_max_area_ratio_inv() const {return m_max_area_ratio_inv;}
 
  inline bool consider_edge_for_big_detections() const {return m_consider_edge_for_big_detections;}
 
private:
  float32_t m_max_distance;
  float32_t m_max_distance_squared;
  float32_t m_max_area_ratio;
  float32_t m_max_area_ratio_inv;
  bool m_consider_edge_for_big_detections;
};
 
class TRACKING_PUBLIC DetectedObjectAssociator
{
public:
  using Assigner = autoware::fusion::hungarian_assigner::hungarian_assigner_c<MAX_NUM_TRACKS>;
  explicit DetectedObjectAssociator(const DataAssociationConfig & association_cfg);
 
  Associations assign(
    const autoware_auto_perception_msgs::msg::DetectedObjects & detections,
    const TrackedObjects & tracks);
 
  const Associations & track_associations() const noexcept
  {
    return m_track_associations;
  }
 
private:
  void reset();
 
  void compute_weights(
    const autoware_auto_perception_msgs::msg::DetectedObjects & detections,
    const TrackedObjects & tracks);
 
  bool consider_associating(
    const autoware_auto_perception_msgs::msg::DetectedObject & detection,
    const TrackedObject & track) const;
 
  void set_weight(const float32_t weight, const size_t det_idx, const size_t track_idx);
 
  Associations extract_result();
 
  DataAssociationConfig m_association_cfg;
  Assigner m_assigner;
  // Hungarian assigner expects a fat matrix (not tall). Bool tracks if tracks are rows or cols
  bool m_are_tracks_rows;
  size_t m_num_tracks;
  size_t m_num_detections;
  bool m_had_errors = false;
  Associations m_track_associations;
};
 
 
}  // namespace tracking
}  // namespace perception
}  // namespace autoware
 
#endif  // TRACKING__DETECTED_OBJECT_ASSOCIATOR_HPP_