data_association.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__DATA_ASSOCIATION_HPP_
#define TRACKING__DATA_ASSOCIATION_HPP_
 
#include <tracking/visibility_control.hpp>
 
#include <autoware_auto_msgs/msg/tracked_dynamic_object_array.hpp>
#include <autoware_auto_msgs/msg/detected_dynamic_object_array.hpp>
#include <hungarian_assigner/hungarian_assigner.hpp>
#include <tracking/tracker_types.hpp>
 
#include <experimental/optional>
#include <limits>
#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 float max_distance, const float max_area_ratio);
 
  inline float get_max_distance_squared() const {return m_max_distance_squared;}
 
  inline float get_max_area_ratio() const {return m_max_area_ratio;}
 
  inline float get_max_area_ratio_inv() const {return m_max_area_ratio_inv;}
 
private:
  float m_max_distance;
  float m_max_distance_squared;
  float m_max_area_ratio;
  float m_max_area_ratio_inv;
};
 
struct TRACKING_PUBLIC AssociatorResult
{
  static constexpr std::size_t UNASSIGNED = std::numeric_limits<std::size_t>::max();
  std::vector<std::size_t> track_assignments;
  std::vector<std::size_t> unassigned_detection_indices;
  std::vector<std::size_t> unassigned_track_indices;
};
 
 
class TRACKING_PUBLIC Associator
{
public:
  explicit Associator(const DataAssociationConfig & association_cfg);
 
  AssociatorResult assign(
    const autoware_auto_msgs::msg::DetectedDynamicObjectArray & detections, const
    autoware_auto_msgs::msg::TrackedDynamicObjectArray & tracks);
 
private:
  void reset();
 
  void compute_weights(
    const autoware_auto_msgs::msg::DetectedDynamicObjectArray & detections,
    const autoware_auto_msgs::msg::TrackedDynamicObjectArray & tracks);
 
  bool consider_associating(
    const autoware_auto_msgs::msg::DetectedDynamicObject & detection, const
    autoware_auto_msgs::msg::TrackedDynamicObject & track) const;
 
  void set_weight(const float weight, const size_t det_idx, const size_t track_idx);
 
  AssociatorResult extract_result() const;
 
  DataAssociationConfig m_association_cfg;
  autoware::fusion::hungarian_assigner::hungarian_assigner_c<MAX_NUM_TRACKS> 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;
};
 
 
}  // namespace tracking
}  // namespace perception
}  // namespace autoware
 
#endif  // TRACKING__DATA_ASSOCIATION_HPP_