multi_object_tracker.hpp

Go to the documentation of this file.

// Copyright 2021 The Autoware Foundation
//
// 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.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
 
 
#ifndef TRACKING__MULTI_OBJECT_TRACKER_HPP_
#define TRACKING__MULTI_OBJECT_TRACKER_HPP_
 
#include <tracking/detected_object_associator.hpp>
#include <tracking/greedy_roi_associator.hpp>
#include <tracking/track_creator.hpp>
#include <tracking/tracked_object.hpp>
#include <tracking/visibility_control.hpp>
 
#include <autoware_auto_perception_msgs/msg/bounding_box_array.hpp>
#include <autoware_auto_perception_msgs/msg/detected_object.hpp>
#include <autoware_auto_perception_msgs/msg/point_clusters.hpp>
#include <autoware_auto_perception_msgs/msg/tracked_object.hpp>
#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
#include <common/types.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <motion_model/linear_motion_model.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <state_estimation/kalman_filter/kalman_filter.hpp>
#include <state_estimation/noise_model/wiener_noise.hpp>
#include <state_vector/common_states.hpp>
#include <tf2/buffer_core.h>
 
#include <chrono>
#include <cstddef>
#include <limits>
#include <memory>
#include <string>
#include <vector>
 
 
namespace autoware
{
namespace perception
{
namespace tracking
{
 
 
enum class TrackerUpdateStatus
{
  Ok,
  WentBackInTime,
  DetectionFrameMismatch,
  TrackerFrameMismatch,
  FrameNotGravityAligned,
  InvalidShape,
};
 
struct TRACKING_PUBLIC DetectedObjectsUpdateResult
{
  autoware_auto_perception_msgs::msg::TrackedObjects tracks;
  std::vector<std::size_t> unassigned_clusters_indices;
  TrackerUpdateStatus status;
  builtin_interfaces::msg::Time related_rois_stamp;
};
 
struct TRACKING_PUBLIC MultiObjectTrackerOptions
{
  DataAssociationConfig object_association_config;
  GreedyRoiAssociatorConfig vision_association_config;
  std::chrono::nanoseconds pruning_time_threshold = std::chrono::nanoseconds::max();
  std::size_t pruning_ticks_threshold = std::numeric_limits<std::size_t>::max();
  std::string frame = "map";
};
 
template<class TrackCreatorT>
class TRACKING_PUBLIC MultiObjectTracker
{
private:
  using ClustersMsg = autoware_auto_perception_msgs::msg::PointClusters;
  using DetectedObjectsMsg = autoware_auto_perception_msgs::msg::DetectedObjects;
  using DetectedObjectKinematics = autoware_auto_perception_msgs::msg::DetectedObjectKinematics;
  using BoundingBoxArrayMsg = autoware_auto_perception_msgs::msg::BoundingBoxArray;
  using ClassifiedRoiArrayMsg = autoware_auto_perception_msgs::msg::ClassifiedRoiArray;
  using TrackedObjectsMsg = autoware_auto_perception_msgs::msg::TrackedObjects;
 
public:
  explicit MultiObjectTracker(
    MultiObjectTrackerOptions options,
    TrackCreatorT track_creator,
    const tf2::BufferCore & tf2_buffer);
 
  DetectedObjectsUpdateResult update(
    const DetectedObjectsMsg & detections,
    const nav_msgs::msg::Odometry & detection_frame_odometry);
 
  DetectedObjectsUpdateResult update(
    const ClustersMsg & incoming_clusters,
    const nav_msgs::msg::Odometry & detection_frame_odometry);
 
  void update(const ClassifiedRoiArrayMsg & rois);
 
private:
  TrackerUpdateStatus validate(
    const DetectedObjectsMsg & detections,
    const nav_msgs::msg::Odometry & detection_frame_odometry);
 
  TrackedObjects m_tracks;
 
  std::chrono::system_clock::time_point m_last_update;
 
  MultiObjectTrackerOptions m_options;
 
  DetectedObjectAssociator m_object_associator;
 
  GreedyRoiAssociator m_vision_associator;
 
  TrackCreatorT m_track_creator;
};
 
template<>
void TRACKING_PUBLIC MultiObjectTracker<TrackCreator<LidarOnlyPolicy>>::update(
  const ClassifiedRoiArrayMsg &)
{
  throw std::runtime_error("Trying to update a LiDAR-only tracker with a vision message.");
}
 
}  // namespace tracking
}  // namespace perception
}  // namespace autoware
 
#endif  // TRACKING__MULTI_OBJECT_TRACKER_HPP_