-
Victor Hexad authoredVictor Hexad authored
Code owners
Assign users and groups as approvers for specific file changes. Learn more.
sensorFusionQuery.h 8.17 KiB
/********************************************************************************
* Copyright (c) 2020 HLRS, University of Stuttgart
* 2018-2020 in-tech GmbH
* 2022 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
* 2022-2025 Hexad GmbH
*
* This program and the accompanying materials are made available under the
* terms of the Eclipse Public License 2.0 which is available at
* http://www.eclipse.org/legal/epl-2.0.
*
* SPDX-License-Identifier: EPL-2.0
********************************************************************************/
//-----------------------------------------------------------------------------
//! @file SensorFusionQuery.h
//! @brief This file provides methods to retrieve structured basic information
//! from sensorData provided by SensorFusionImplementation::Output().
//-----------------------------------------------------------------------------
#pragma once
#include <map>
#include <vector>
#include "osi3/osi_sensordata.pb.h"
namespace SensorFusionHelperFunctions
{
static std::vector<osi3::DetectedMovingObject> RetrieveMovingObjectsBySensorId(const std::vector<int> &sensorIds, const osi3::SensorData &sensorData)
{
std::vector<osi3::DetectedMovingObject> result;
auto detectedMovingObjects = sensorData.moving_object();
for (const auto &object : detectedMovingObjects)
{
if (sensorIds.empty())
{
result.push_back(object);
continue;
}
for (auto sensorId : object.header().sensor_id())
{
if (std::count(sensorIds.cbegin(), sensorIds.cend(), sensorId.value()) > 0)
{
result.push_back(object);
break;
}
}
}
return result;
}
static std::vector<osi3::DetectedStationaryObject> RetrieveStationaryObjectsBySensorId(const std::vector<int> &sensorIds, const osi3::SensorData &sensorData)
{
std::vector<osi3::DetectedStationaryObject> result;
auto detectedStationaryObjects = sensorData.stationary_object();
for (const auto &object : detectedStationaryObjects)
{
if (sensorIds.empty())
{
result.push_back(object);
continue;
}
for (auto sensorId : object.header().sensor_id())
{
if (std::count(sensorIds.cbegin(), sensorIds.cend(), sensorId.value()) > 0)
{
result.push_back(object);
break;
}
}
}
return result;
}
//! Converts a position from the sensor coordinate system to the vehicle coordinate system
//!
//! \param position position to convert
//! \param mountingPosition sensor mounting position
//! \return converted position
static point_t ConvertPositionToVehicleCoordinates(const point_t& position, const osi3::MountingPosition& mountingPosition)
{
point_t convertedPosition;
point_t tempPoint;
bt::rotate_transformer<bg::radian, double, 2, 2> sensorRotation(-mountingPosition.orientation().yaw());
bg::transform(position, tempPoint, sensorRotation);
bt::translate_transformer<double, 2, 2> sensorTranslate(mountingPosition.position().x(), mountingPosition.position().y());
bg::transform(tempPoint, convertedPosition, sensorTranslate);
return convertedPosition;
}
//! Converts a relative vector from the sensor coordinate system to the vehicle coordinate system
//!
//! \param detectedValue vector to convert
//! \param differenceToSensorCoordinate difference between the absolute value between sensor position and reference position (in global coordinates)
//! \param vehicleYaw yaw of the vehicle in global coordinates
//! \param mountingPosition sensor mounting position
//! \return converted vector
static point_t ConvertVectorToVehicleCoordinates(const point_t& detectedValue, const point_t& differenceToSensorCoordinate, double vehicleYaw, const osi3::MountingPosition& mountingPosition)
{
point_t differenceInVehicleCoordinates;
bt::rotate_transformer<bg::radian, double, 2, 2> vehicleRotation(vehicleYaw);
bg::transform(differenceToSensorCoordinate, differenceInVehicleCoordinates, vehicleRotation);
point_t valueInVehicleCoordinates;
bt::rotate_transformer<bg::radian, double, 2, 2> sensorRotation(-mountingPosition.orientation().yaw());
bg::transform(detectedValue, valueInVehicleCoordinates, sensorRotation);
return {valueInVehicleCoordinates.x() + differenceInVehicleCoordinates.x(),
valueInVehicleCoordinates.y() + differenceInVehicleCoordinates.y()};
}
//! Converts the values in a DetectedMovingObject from the sensor coordinate system to the vehicle coordinate system
//!
//! \param object object to convert
//! \param mountingPosition sensor mounting position
//! \return converted object
static osi3::DetectedMovingObject ConvertToVehicleCoordinates(const osi3::DetectedMovingObject& object, const osi3::MountingPosition& mountingPosition, const AgentInterface& agent)
{
osi3::DetectedMovingObject convertedObject = object;
point_t position{object.base().position().x(), object.base().position().y()};
point_t convertedPosition = ConvertPositionToVehicleCoordinates(position, mountingPosition);
convertedObject.mutable_base()->mutable_position()->set_x(convertedPosition.x());
convertedObject.mutable_base()->mutable_position()->set_y(convertedPosition.y());
convertedObject.mutable_base()->mutable_orientation()->set_yaw(object.base().orientation().yaw() + mountingPosition.orientation().yaw());
ObjectPointCustom pointMountingPosition{mountingPosition.position().x(), mountingPosition.position().y()};
const auto differenceVelocity = agent.GetVelocity(pointMountingPosition) - agent.GetVelocity(ObjectPointPredefined::Reference);
point_t velocity{object.base().velocity().x(), object.base().velocity().y()};
point_t convertedVelocity = ConvertVectorToVehicleCoordinates(velocity, {differenceVelocity.x, differenceVelocity.y}, agent.GetYaw(), mountingPosition);
convertedObject.mutable_base()->mutable_velocity()->set_x(convertedVelocity.x());
convertedObject.mutable_base()->mutable_velocity()->set_y(convertedVelocity.y());
const auto differenceAcceleration = agent.GetAcceleration(pointMountingPosition) - agent.GetAcceleration(ObjectPointPredefined::Reference);
point_t acceleration{object.base().acceleration().x(), object.base().acceleration().y()};
point_t convertedAcceleration = ConvertVectorToVehicleCoordinates(acceleration, {differenceAcceleration.x, differenceAcceleration.y}, agent.GetYaw(), mountingPosition);
convertedObject.mutable_base()->mutable_acceleration()->set_x(convertedAcceleration.x());
convertedObject.mutable_base()->mutable_acceleration()->set_y(convertedAcceleration.y());
return convertedObject;
}
//! Converts the values in a DetectedStationaryObject from the sensor coordinate system to the vehicle coordinate system
//!
//! \param object object to convert
//! \param mountingPosition sensor mounting position
//! \return converted object
static osi3::DetectedStationaryObject ConvertToVehicleCoordinates(const osi3::DetectedStationaryObject& object, const osi3::MountingPosition& mountingPosition)
{
osi3::DetectedStationaryObject convertedObject = object;
point_t position{object.base().position().x(), object.base().position().y()};
point_t convertedPosition = ConvertPositionToVehicleCoordinates(position, mountingPosition);
convertedObject.mutable_base()->mutable_position()->set_x(convertedPosition.x());
convertedObject.mutable_base()->mutable_position()->set_y(convertedPosition.y());
convertedObject.mutable_base()->mutable_orientation()->set_yaw(object.base().orientation().yaw() + mountingPosition.orientation().yaw());
return convertedObject;
}
};