include/tracking/tracked_object.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__TRACKED_OBJECT_HPP_
#define TRACKING__TRACKED_OBJECT_HPP_
 
#include <autoware_auto_perception_msgs/msg/detected_object.hpp>
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
#include <autoware_auto_perception_msgs/msg/shape.hpp>
#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
#include <common/types.hpp>
#include <motion_model/linear_motion_model.hpp>
#include <state_estimation/kalman_filter/kalman_filter.hpp>
#include <state_estimation/noise_model/wiener_noise.hpp>
#include <state_vector/common_states.hpp>
#include <tracking/classification_tracker.hpp>
#include <tracking/visibility_control.hpp>
 
#include <chrono>
#include <cstddef>
#include <vector>
 
namespace autoware
{
namespace perception
{
namespace tracking
{
 
class TRACKING_PUBLIC TrackedObject
{
public:
  // Aliases for convenience
  using CA = autoware::common::state_vector::ConstAccelerationXY64;
  using MotionModel = autoware::common::motion_model::LinearMotionModel<CA>;
  using NoiseModel = autoware::common::state_estimation::WienerNoise<CA>;
  using EKF = autoware::common::state_estimation::KalmanFilter<MotionModel, NoiseModel>;
  using TrackedObjectMsg = autoware_auto_perception_msgs::msg::TrackedObject;
  using DetectedObjectMsg = autoware_auto_perception_msgs::msg::DetectedObject;
  using ObjectClassifications =
    autoware_auto_perception_msgs::msg::TrackedObject::_classification_type;
  using ShapeMsg = autoware_auto_perception_msgs::msg::Shape;
 
  explicit TrackedObject(
    const DetectedObjectMsg & detection,
    common::types::float64_t default_variance,
    common::types::float64_t noise_variance);
 
  explicit TrackedObject(
    const DetectedObjectMsg & detection,
    const DetectedObjectMsg::_classification_type & override_classification,
    common::types::float64_t default_variance,
    common::types::float64_t noise_variance);
  // TODO(nikolai.morin): Change signature to use absolute time after #1002
  void predict(std::chrono::nanoseconds dt);
 
  void update(const DetectedObjectMsg & detection);
 
  void update(
    const ObjectClassifications & obj_type, const common::types::float32_t covariance);
 
  void no_update();
 
  bool should_be_removed(
    const std::chrono::nanoseconds time_threshold,
    const std::size_t ticks_threshold) const;
 
  const TrackedObjectMsg & msg();
 
  inline Eigen::Matrix2d position_covariance() const
  {
    return m_ekf.covariance().topLeftCorner<2, 2>();
  }
 
  inline Eigen::Vector2d centroid() const
  {
    return Eigen::Vector2d {
      m_ekf.state().at<autoware::common::state_vector::variable::X>(),
      m_ekf.state().at<autoware::common::state_vector::variable::Y>()};
  }
 
  inline double z() const noexcept {return m_msg.kinematics.centroid_position.z;}
 
  inline const ShapeMsg & shape() const
  {
    return m_msg.shape[0];
  }
 
  inline const geometry_msgs::msg::Quaternion orientation() const
  {
    return m_msg.kinematics.orientation;
  }
 
private:
  TrackedObjectMsg m_msg;
  EKF m_ekf;
  std::chrono::nanoseconds m_time_since_last_seen = std::chrono::nanoseconds::zero();
  std::size_t m_ticks_since_last_seen = 0;
  std::size_t m_ticks_alive = 1;
  common::types::float64_t m_default_variance = -1.0;
  ClassificationTracker m_classifier;
};
 
 
struct TRACKING_PUBLIC TrackedObjects
{
  std::vector<TrackedObject> objects;
  std_msgs::msg::Header::_frame_id_type frame_id;
};
 
}  // namespace tracking
}  // namespace perception
}  // namespace autoware
 
#endif  // TRACKING__TRACKED_OBJECT_HPP_