carma_cooperative_perception::MetricSe2 Struct Reference

Calculates distance between a point and detection in SE(2) (special Euclidean) space. More...

Collaboration diagram for carma_cooperative_perception::MetricSe2:

Public Member Functions
template<typename Detection >
auto	operator() (const mot::Point &point, const Detection &detection) const -> double
template<typename... Alternatives>
auto	operator() (mot::Point point, const std::variant< Alternatives... > &detection) const -> double

Detailed Description

Calculates distance between a point and detection in SE(2) (special Euclidean) space.

Definition at line 527 of file multiple_object_tracker_component.cpp.

Member Function Documentation

operator()() [1/2]

template<typename Detection >

auto carma_cooperative_perception::MetricSe2::operator() ( const mot::Point &  point, const Detection &  detection  ) const -> double

inline

Definition at line 530 of file multiple_object_tracker_component.cpp.

  {
    double sum{0};
 
    sum += std::pow(mot::remove_units(point.position_x - detection.state.position_x), 2);
    sum += std::pow(mot::remove_units(point.position_y - detection.state.position_y), 2);
 
    const auto p_yaw_rad{mot::remove_units(point.yaw.get_angle())};
    const auto d_yaw_rad{mot::remove_units(detection.state.yaw.get_angle())};
    const auto abs_diff{std::abs(p_yaw_rad - d_yaw_rad)};
    sum += std::min(abs_diff, 2 * 3.14159265359 - abs_diff);
 
    return sum;
  }

References process_traj_logs::point, and carma_cooperative_perception::remove_units().

Here is the call graph for this function:

operator()() [2/2]

template<typename... Alternatives>

auto carma_cooperative_perception::MetricSe2::operator() ( mot::Point  point, const std::variant< Alternatives... > &  detection  ) const -> double

inline

Definition at line 546 of file multiple_object_tracker_component.cpp.

  {
    const mot::Visitor visitor{
      [this](const mot::Point & p, const mot::CtrvDetection & d) { return this->operator()(p, d); },
      [this](const mot::Point & p, const mot::CtraDetection & d) { return this->operator()(p, d); },
      [](const mot::Point &, const auto &) {
        // Currently on support CTRV and CTRA
        return std::numeric_limits<double>::max();
      }};
 
    return std::visit(visitor, std::variant<mot::Point>{point}, detection);
  }

References process_traj_logs::point.

---

The documentation for this struct was generated from the following file:

• carma_cooperative_perception/src/multiple_object_tracker_component.cpp