greedy_roi_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__GREEDY_ROI_ASSOCIATOR_HPP_
#define TRACKING__GREEDY_ROI_ASSOCIATOR_HPP_
 
#include <autoware_auto_perception_msgs/msg/classified_roi_array.hpp>
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
#include <geometry/common_2d.hpp>
#include <geometry/intersection.hpp>
#include <helper_functions/template_utils.hpp>
#include <lidar_utils/point_cloud_utils.hpp>
#include <tf2/buffer_core.h>
#include <tracking/projection.hpp>
#include <tracking/tracked_object.hpp>
#include <tracking/tracker_types.hpp>
#include <tracking/visibility_control.hpp>
 
#include <string>
#include <unordered_set>
#include <vector>
 
namespace autoware
{
namespace perception
{
namespace tracking
{
struct TRACKING_PUBLIC IOUHeuristic
{
  template<template<typename ...> class Iterable1T,
    template<typename ...> class Iterable2T, typename Point1T, typename Point2T>
  common::types::float32_t operator()(
    const Iterable1T<Point1T> & shape1, const Iterable2T<Point2T> & shape2) const
  {
    return common::geometry::convex_intersection_over_union_2d(shape1, shape2);
  }
};
 
struct GreedyRoiAssociatorConfig
{
  CameraIntrinsics intrinsics;
  float32_t iou_threshold;
};
 
class TRACKING_PUBLIC GreedyRoiAssociator
{
public:
  using float32_t = autoware::common::types::float32_t;
 
  explicit GreedyRoiAssociator(
    const GreedyRoiAssociatorConfig & config,
    const tf2::BufferCore & tf_buffer);
 
  AssociatorResult assign(
    const autoware_auto_perception_msgs::msg::ClassifiedRoiArray & rois,
    const TrackedObjects & tracks) const;
 
  AssociatorResult assign(
    const autoware_auto_perception_msgs::msg::ClassifiedRoiArray & rois,
    const autoware_auto_perception_msgs::msg::DetectedObjects & objects) const;
 
private:
  // Handles extrapolation exception alone. Caller responsible for all else
  geometry_msgs::msg::TransformStamped lookup_transform_handler(
    const std::string & target_frame,
    const std::string & source_frame,
    const tf2::TimePoint & stamp) const;
 
  // Scan the ROIs to find the best matching roi for a given shape by projecting it onto image
  // frame
  std::size_t project_and_match_detection(
    const std::vector<geometry_msgs::msg::Point32> & object_shape_in_camera_frame,
    const std::unordered_set<std::size_t> & available_roi_indices,
    const autoware_auto_perception_msgs::msg::ClassifiedRoiArray & rois) const;
 
  CameraModel m_camera;
  IOUHeuristic m_iou_func{};
  float32_t m_iou_threshold{0.1F};
  const tf2::BufferCore & m_tf_buffer;
 
  static const std::chrono::milliseconds kTfTooOld;
};
 
namespace details
{
class TRACKING_PUBLIC ShapeTransformer
{
public:
  explicit ShapeTransformer(const geometry_msgs::msg::Transform & tf);
  using Point32 = geometry_msgs::msg::Point32;
  std::vector<Point32> operator()(
    const autoware_auto_perception_msgs::msg::Shape & shape,
    const geometry_msgs::msg::Point & centroid,
    const geometry_msgs::msg::Quaternion & orientation) const;
 
private:
  common::lidar_utils::StaticTransformer m_transformer;
};
}  // namespace details
}  // namespace tracking
}  // namespace perception
}  // namespace autoware
 
#endif  // TRACKING__GREEDY_ROI_ASSOCIATOR_HPP_