utils.hpp

Go to the documentation of this file.

// Copyright Institute for Automotive Engineering (ika), RWTH Aachen University
// SPDX-License-Identifier: MIT
 
#pragma once
 
#include <cmath>
 
#include <perception_msgs_utils/constants.hpp>
 
 
namespace perception_msgs {
 
namespace object_access {
 
  // --- state size ------------------------------------------------------------
 
  const std::string kExceptionUnknownModel = "Unknown model ID: ";
 
  inline int getContinuousStateSize(const ObjectState& state) {
    return state.continuous_state.size();
  }
 
  template <typename T>
  inline int getContinuousStateSize(const T& obj) {
    return getContinuousStateSize(obj.state);
  }
 
  inline int getContinuousStateSize(const unsigned char& model_id) {
    switch(model_id) {
      case EGO::MODEL_ID:
        return EGO::CONTINUOUS_STATE_SIZE;
      case EGORWS::MODEL_ID:
        return EGORWS::CONTINUOUS_STATE_SIZE;
      case ISCACTR::MODEL_ID:
        return ISCACTR::CONTINUOUS_STATE_SIZE;
      case HEXAMOTION::MODEL_ID:
        return HEXAMOTION::CONTINUOUS_STATE_SIZE;
      case TRAFFICLIGHT::MODEL_ID:
        return TRAFFICLIGHT::CONTINUOUS_STATE_SIZE;
      default:
        throw std::invalid_argument(kExceptionUnknownModel + std::to_string(model_id));
    }
  }
 
  inline int getDiscreteStateSize(const ObjectState& state) {
    return state.discrete_state.size();
  }
 
  template <typename T>
  inline int getDiscreteStateSize(const T& obj) {
    return getDiscreteStateSize(obj.state);
  }
 
  inline int getDiscreteStateSize(const unsigned char& model_id) {
    switch(model_id) {
      case EGO::MODEL_ID:
        return EGO::DISCRETE_STATE_SIZE;
      case EGORWS::MODEL_ID:
        return EGORWS::DISCRETE_STATE_SIZE;
      case ISCACTR::MODEL_ID:
        return ISCACTR::DISCRETE_STATE_SIZE;
      case HEXAMOTION::MODEL_ID:
        return HEXAMOTION::DISCRETE_STATE_SIZE;
      case TRAFFICLIGHT::MODEL_ID:
        return TRAFFICLIGHT::DISCRETE_STATE_SIZE;
      default:
        throw std::invalid_argument(kExceptionUnknownModel + std::to_string(model_id));
    }
  }
 
  inline int getContinuousStateCovarianceSize(const ObjectState& state) {
    if (!state.continuous_state_covariance.empty()) return state.continuous_state_covariance.size();
    const int n = getContinuousStateSize(state.model_id);
    return n * n;
  }
 
  template <typename T>
  inline int getContinuousStateCovarianceSize(const T& obj) {
    return getContinuousStateCovarianceSize(obj.state);
  }
 
  inline int getContinuousStateCovarianceSize(const unsigned char& model_id) {
    return std::pow(getContinuousStateSize(model_id), 2);
  }
 
  inline void setContinuousStateCovarianceToUnknownAt(ObjectState& state, const unsigned int i, const unsigned int j) {
    if (state.continuous_state_covariance.empty()) return;
    const int n = getContinuousStateSize(state);
    state.continuous_state_covariance.at(n * i + j) = CONTINUOUS_STATE_COVARIANCE_UNKNOWN;
  }
 
} // namespace object_access
 
} // namespace perception_msgs