diff --git a/.gitignore b/.gitignore index 4927376..30cd7dc 100644 --- a/.gitignore +++ b/.gitignore @@ -4,4 +4,6 @@ build* .editorconfig out/ .idea/ -.vs/ \ No newline at end of file +.vs/ +*.sh +*.bat \ No newline at end of file diff --git a/.gitmodules b/.gitmodules index e7b761b..70657cc 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,15 +4,6 @@ [submodule "lib/open-simulation-interface"] path = lib/open-simulation-interface url = https://github.com/OpenSimulationInterface/open-simulation-interface.git -[submodule "src/model/strategies/transformation-functions"] - path = src/model/strategies/transformation-functions - url = https://gitlab.com/tuda-fzd/perception-sensor-modeling/transformation-functions.git -[submodule "src/model/strategies/csv-output-gtobjects-strategy"] - path = src/model/strategies/csv-output-gtobjects-strategy - url = https://gitlab.com/tuda-fzd/perception-sensor-modeling/output-strategies/csv-output-gtobjects-strategy.git -[submodule "src/model/strategies/ros-output-gtobjects-strategy"] - path = src/model/strategies/ros-output-gtobjects-strategy - url = https://gitlab.com/tuda-fzd/perception-sensor-modeling/output-strategies/ros-output-gtobjects-strategy.git [submodule "lib/fmi2"] path = lib/fmi2 url = https://github.com/modelica/fmi-standard.git diff --git a/lib/open-simulation-interface b/lib/open-simulation-interface index aeb8541..700133d 160000 --- a/lib/open-simulation-interface +++ b/lib/open-simulation-interface @@ -1 +1 @@ -Subproject commit aeb8541ff391ce0503828016291c9ad5e953b13a +Subproject commit 700133d9fa2df6ba648958468a33af4dacb3f3f0 diff --git a/src/model/strategies/csv-output-gtobjects-strategy b/src/model/strategies/csv-output-gtobjects-strategy deleted file mode 160000 index e00dca6..0000000 --- a/src/model/strategies/csv-output-gtobjects-strategy +++ /dev/null @@ -1 +0,0 @@ -Subproject commit e00dca64155a2ddc43e9b24617dae9047b1b0fe4 diff --git a/src/model/strategies/csv-output-gtobjects-strategy/CMakeLists.txt b/src/model/strategies/csv-output-gtobjects-strategy/CMakeLists.txt new file mode 100644 index 0000000..86c3a5e --- /dev/null +++ b/src/model/strategies/csv-output-gtobjects-strategy/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.12) +project(CsvOutputGTObjects) + +set(CSV_GTOBJECTS_SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src) +set(CSV_GTOBJECTS_TARGET_DIR ${CMAKE_CURRENT_BINARY_DIR}/src) +if (WIN32) + set(CSV_PATH C:/TEMP) +elseif (${CMAKE_SYSTEM_NAME} MATCHES "Linux") + set(CSV_PATH /tmp) +endif() +configure_file(${CMAKE_CURRENT_SOURCE_DIR}/include/csvoutputgtobjects/set_csv_file_path_gtobjects.cpp.in ${CMAKE_CURRENT_BINARY_DIR}/include/csvoutputgtobjects/set_csv_file_path_gtobjects.cpp) + +# TODO MODIFY THE FOLLOWING THREE LINES AS NEEDED +set(STRATEGY_ENTRY_POINT CsvOutputGTObjects) +set(STRATEGY_SOURCES ${CSV_GTOBJECTS_SOURCE_DIR}/CsvOutputGTObjects.cpp ../transformation-functions/TransformationFunctions.cpp) +set(STRATEGY_EXTRA_PUBLIC_LIBS_OR_TARGETS "") +set(STRATEGY_EXTRA_PRIVATE_LIBS_OR_TARGETS "") +set(STRATEGY_EXTRA_INCLUDE_DIRECTORIES ${CMAKE_CURRENT_BINARY_DIR}/include) +#set(STRATEGY_PROFILE_EXTENSION ${PROFILES}) + +# no need to modify commands below this line +set(CMAKE_CXX_STANDARD 17) + +if(NOT COMMAND add_fmu_csv_output_strategy) + message(FATAL_ERROR "This project directory has to be included by OSI FMU platform, it can't be build out of that context!") +endif() + +add_fmu_csv_output_strategy(${PROJECT_NAME} ${STRATEGY_ENTRY_POINT}) +if(STRATEGY_PROFILE_EXTENSION) + add_profile_part(${STRATEGY_PROFILE_EXTENSION}) +endif() + + +add_library(${PROJECT_NAME} STATIC ${STRATEGY_SOURCES}) +target_link_libraries(${PROJECT_NAME} PRIVATE ${STRATEGY_EXTRA_PRIVATE_LIBS_OR_TARGETS} PUBLIC model::strategy open_simulation_interface_obj ${STRATEGY_EXTRA_PUBLIC_LIBS_OR_TARGETS}) +target_include_directories(${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include ${STRATEGY_EXTRA_INCLUDE_DIRECTORIES}) +set_property(TARGET ${PROJECT_NAME} PROPERTY POSITION_INDEPENDENT_CODE ON) + diff --git a/src/model/strategies/csv-output-gtobjects-strategy/README.md b/src/model/strategies/csv-output-gtobjects-strategy/README.md new file mode 100644 index 0000000..a24e3f5 --- /dev/null +++ b/src/model/strategies/csv-output-gtobjects-strategy/README.md @@ -0,0 +1,12 @@ +# Ground Truth Objects to .csv Output Strategy + +This strategy writes ground truth objects in a csv file. +Path for the file is set via CMakeLists. + +## Usage +You need to add the name of the strategy in a new line in the *csv_output_sequence.conf* file in the *src/model/strategies/* folder. + +In order for the strategy to be called during simulation, the FMI parameter *switch_for_csv_output* needs to be set to *1* and be passed to the framework or model fmu. + +NOTE: +This strategy needs transformation-functions from ../transformation-functions. diff --git a/src/model/strategies/csv-output-gtobjects-strategy/include/csvoutputgtobjects/CsvOutputGTObjects.hpp b/src/model/strategies/csv-output-gtobjects-strategy/include/csvoutputgtobjects/CsvOutputGTObjects.hpp new file mode 100644 index 0000000..2ff221e --- /dev/null +++ b/src/model/strategies/csv-output-gtobjects-strategy/include/csvoutputgtobjects/CsvOutputGTObjects.hpp @@ -0,0 +1,61 @@ +// +// Copyright Institute of Automotive Engineering +// of Technical University of Darmstadt 2020. +// Licensed under the EUPL-1.2-or-later +// +// This work covered by the EUPL can be used/merged and distributed +// in other works covered by GPL-2.0, GPL-3.0, LGPL, AGPL, CeCILL, +// OSL, EPL, MPL and other licences listed as compatible in the EUPL +// Appendix. This applies to the other (combined) work, while the +// original project stays covered by the EUPL without re-licensing. +// +// Alternatively, the contents of this file may be used under the +// terms of the Mozilla Public License, v. 2.0. If a copy of the MPL +// was not distributed with this file, you can obtain one at +// http://mozilla.org/MPL/2.0/. +// + +#ifndef CSV_OUTPUT_GTOBJECTS_STRATEGY_HPP +#define CSV_OUTPUT_GTOBJECTS_STRATEGY_HPP + +#include +#include + +using namespace osi3; + +namespace model { + + class CsvOutputGTObjects : public Strategy { + + using Strategy::Strategy; + + void apply(SensorData &) override; + + std::string file_path_gtobjects; + bool first_call = true; + + public: + + private: + + struct GT_object { + size_t id = 0; + float x = 0.0; + float y = 0.0; + float z = 0.0; + float roll = 0.0; + float pitch = 0.0; + float yaw = 0.0; + float width = 0.0; + float length = 0.0; + float height = 0.0; + bool is_moving = false; + }; + + static void write_first_line_to_CSV(const std::string& path); + static void write_data_to_CSV(const std::string& path, double timestamp, size_t object_idx, float x, float y, float z, float roll, float pitch, float yaw, float width, float length, float height, bool is_moving); + }; + +} + +#endif //CSV_OUTPUT_GTOBJECTS_STRATEGY_HPP diff --git a/src/model/strategies/csv-output-gtobjects-strategy/include/csvoutputgtobjects/set_csv_file_path_gtobjects.cpp.in b/src/model/strategies/csv-output-gtobjects-strategy/include/csvoutputgtobjects/set_csv_file_path_gtobjects.cpp.in new file mode 100644 index 0000000..afdae70 --- /dev/null +++ b/src/model/strategies/csv-output-gtobjects-strategy/include/csvoutputgtobjects/set_csv_file_path_gtobjects.cpp.in @@ -0,0 +1,26 @@ +time_t curr_time; +struct tm *detl; +char buf[80]; +time( &curr_time ); +detl = localtime( &curr_time ); +// strftime(buf, 20, "%x - %I:%M%p", detl); +strftime(buf, 20, "%Y-%m-%d_%H-%M-%S", detl); + +std::string start_time = std::string(buf); + +std::string path_string = "@CSV_PATH@/"; +size_t pos; + +path_string = path_string + "@MODEL_NAME@" + "_" + start_time; +std::string filename = "GTObjects.csv"; + +#if defined(_WIN32) + while ((pos = path_string.find("/")) != std::string::npos) { + path_string.replace(pos, 1, "\\"); + } + _mkdir(path_string.c_str()); + file_path_gtobjects = path_string + "\\" + filename; +#else + mkdir(path_string.c_str(), 0777); + file_path_gtobjects = path_string + "/" + filename; +#endif diff --git a/src/model/strategies/csv-output-gtobjects-strategy/src/CsvOutputGTObjects.cpp b/src/model/strategies/csv-output-gtobjects-strategy/src/CsvOutputGTObjects.cpp new file mode 100644 index 0000000..1d2d5f3 --- /dev/null +++ b/src/model/strategies/csv-output-gtobjects-strategy/src/CsvOutputGTObjects.cpp @@ -0,0 +1,157 @@ +// +// Copyright Institute of Automotive Engineering +// of Technical University of Darmstadt 2020. +// Licensed under the EUPL-1.2-or-later +// +// This work covered by the EUPL can be used/merged and distributed +// in other works covered by GPL-2.0, GPL-3.0, LGPL, AGPL, CeCILL, +// OSL, EPL, MPL and other licences listed as compatible in the EUPL +// Appendix. This applies to the other (combined) work, while the +// original project stays covered by the EUPL without re-licensing. +// +// Alternatively, the contents of this file may be used under the +// terms of the Mozilla Public License, v. 2.0. If a copy of the MPL +// was not distributed with this file, you can obtain one at +// http://mozilla.org/MPL/2.0/. +// + +#ifndef _USE_MATH_DEFINES +#define _USE_MATH_DEFINES +#endif + +#include "csvoutputgtobjects/CsvOutputGTObjects.hpp" +#include "../../transformation-functions/TransformationFunctions.hpp" +#include +#include +#include +#include + +#ifdef _WIN32 + #include + #include +#else + #include + #include +#endif + +using namespace model; +using namespace osi3; + +static bool first_call = true; + +void model::CsvOutputGTObjects::apply(SensorData &sensor_data) { + log("Starting .csv output for GT objects"); + + if (sensor_data.sensor_view_size() == 0) { + log("No sensor view received for .csv output"); + return; + } + + TF::EgoData ego_data; + if (!TF::get_ego_info(ego_data, sensor_data.sensor_view(0))) { + log("Ego vehicle has no base, no id, or is not contained in GT moving objects."); + return; + } + + auto time_nanos = sensor_data.sensor_view(0).global_ground_truth().timestamp().nanos(); + auto time_seconds = sensor_data.sensor_view(0).global_ground_truth().timestamp().seconds(); + double timestamp = (double)time_seconds + (double) time_nanos / 1000000000; + + if (!sensor_data.sensor_view(0).has_global_ground_truth()) { + log("No GT for .csv output at timestamp " + std::to_string(timestamp)); + return; + } + + if ((sensor_data.sensor_view(0).global_ground_truth().moving_object_size() == 0) && + (sensor_data.sensor_view(0).global_ground_truth().stationary_object_size() == 0)) { + log("No objects in GT for .csv output at timestamp " + std::to_string(timestamp)); + return; + } + + /// Write header line of .csv on first call + if (first_call) { + #include + write_first_line_to_CSV(file_path_gtobjects); + first_call = false; + } + + auto no_of_moving_objects = sensor_data.sensor_view(0).global_ground_truth().moving_object_size(); + auto no_of_stationary_objects = sensor_data.sensor_view(0).global_ground_truth().stationary_object_size(); + + /// Start a vector for gt_objects with (gt_id, x, y, z, roll, pitch, yaw, width, length, height, is_moving) + std::vector gt_objects; + gt_objects.reserve(no_of_moving_objects + no_of_stationary_objects); + + /// Collect moving objects + for (const auto >_moving_object : sensor_data.sensor_view(0).global_ground_truth().moving_object()) { + if (gt_moving_object.id().value() == sensor_data.sensor_view(0).global_ground_truth().host_vehicle_id().value()) + continue; + + Vector3d relative_position_ego_coordinate_system = TF::transform_position_from_world_to_ego_coordinates(gt_moving_object.base().position(), ego_data); + Orientation3d relative_orientation = TF::calc_relative_orientation_to_local(gt_moving_object.base().orientation(), ego_data.ego_base.orientation()); + GT_object actual_gt_object; + actual_gt_object.id = gt_moving_object.id().value(); + actual_gt_object.x = std::round(float(relative_position_ego_coordinate_system.x()) * 1000) / 1000; + actual_gt_object.y = std::round(float(relative_position_ego_coordinate_system.y()) * 1000) / 1000; + actual_gt_object.z = std::round(float(relative_position_ego_coordinate_system.z()) * 1000) / 1000; + actual_gt_object.roll = (float)std::round(float(relative_orientation.roll()) * 180 / M_PI * 1000) / 1000; + actual_gt_object.pitch = (float)std::round(float(relative_orientation.pitch()) * 180 / M_PI * 1000) / 1000; + actual_gt_object.yaw = (float)std::round(float(relative_orientation.yaw()) * 180 / M_PI * 1000) / 1000; + actual_gt_object.width = float(gt_moving_object.base().dimension().width()); + actual_gt_object.length = float(gt_moving_object.base().dimension().length()); + actual_gt_object.height = float(gt_moving_object.base().dimension().height()); + actual_gt_object.is_moving = true; + gt_objects.emplace_back(actual_gt_object); + } + + /// Collect stationary objects + for (const auto >_stationary_object : sensor_data.sensor_view(0).global_ground_truth().stationary_object()) { + Vector3d relative_position_ego_coordinate_system = TF::transform_position_from_world_to_ego_coordinates( + gt_stationary_object.base().position(), ego_data); + Orientation3d relative_orientation = TF::calc_relative_orientation_to_local( + gt_stationary_object.base().orientation(), + ego_data.ego_base.orientation()); + GT_object actual_gt_object; + actual_gt_object.id = gt_stationary_object.id().value(); + actual_gt_object.x = std::round(float(relative_position_ego_coordinate_system.x()) * 1000) / 1000; + actual_gt_object.y = std::round(float(relative_position_ego_coordinate_system.y()) * 1000) / 1000; + actual_gt_object.z = std::round(float(relative_position_ego_coordinate_system.z()) * 1000) / 1000; + actual_gt_object.roll = (float)std::round(float(relative_orientation.roll()) * 180 / M_PI * 1000) / 1000; + actual_gt_object.pitch = (float)std::round(float(relative_orientation.pitch()) * 180 / M_PI * 1000) / 1000; + actual_gt_object.yaw = (float)std::round(float(relative_orientation.yaw()) * 180 / M_PI * 1000) / 1000; + actual_gt_object.width = float(gt_stationary_object.base().dimension().width()); + actual_gt_object.length = float(gt_stationary_object.base().dimension().length()); + actual_gt_object.height = float(gt_stationary_object.base().dimension().height()); + actual_gt_object.is_moving = false; + gt_objects.emplace_back(actual_gt_object); + } + + for (const auto >_object : gt_objects) { + auto gt_id = gt_object.id; + + auto roll = std::round(gt_object.roll* 180 / M_PI * 1000) / 1000; + auto pitch = std::round(gt_object.pitch * 180 / M_PI * 1000) / 1000; + auto yaw = std::round(gt_object.yaw * 180 / M_PI * 1000) / 1000; + + write_data_to_CSV(file_path_gtobjects, timestamp, gt_id, gt_object.x, gt_object.y, gt_object.z, gt_object.roll, gt_object.pitch, gt_object.yaw, + gt_object.width, gt_object.length, gt_object.height, gt_object.is_moving); + } +} + +void CsvOutputGTObjects::write_first_line_to_CSV(const std::string &path) { + std::fstream my_file; + my_file.open(path, std::ios::app); + my_file + << "timestamp_in_s, gt_object_id, x_in_m, y_in_m, z_in_m, roll_in_deg, pitch_in_deg, yaw_in_deg, width_in_m, length_in_m, height_in_m, is_moving" + << std::endl; + my_file.close(); +} + +void CsvOutputGTObjects::write_data_to_CSV(const std::string& path, double timestamp, size_t object_idx, float x, float y, float z, float roll, float pitch, float yaw, float width, float length, float height, bool is_moving) { + std::fstream my_file; + my_file.open(path, std::ios::app); + my_file << timestamp << ", " << object_idx << ", " << x << ", " << y << ", " << z << ", " << roll << ", " << pitch + << ", " << yaw << ", " << width << ", " << length << ", " << height << ", " + << (is_moving ? "true" : "false") << std::endl; + my_file.close(); +} \ No newline at end of file diff --git a/src/model/strategies/ros-output-gtobjects-strategy b/src/model/strategies/ros-output-gtobjects-strategy deleted file mode 160000 index c479663..0000000 --- a/src/model/strategies/ros-output-gtobjects-strategy +++ /dev/null @@ -1 +0,0 @@ -Subproject commit c479663d65bef435161ce279718249bd5ebee238 diff --git a/src/model/strategies/ros-output-gtobjects-strategy/CMakeLists.txt b/src/model/strategies/ros-output-gtobjects-strategy/CMakeLists.txt new file mode 100644 index 0000000..68123f2 --- /dev/null +++ b/src/model/strategies/ros-output-gtobjects-strategy/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.12) +project(ros_gt_objects) + +# TODO MODIFY THE FOLLOWING LINES AS NEEDED +set(STRATEGY_ENTRY_POINT ros_gt_objects) +set(STRATEGY_SOURCES src/ros_gt_objects.cpp) +set(STRATEGY_EXTRA_PUBLIC_LIBS_OR_TARGETS ${PCL_COMMON_LIBRARIES} ${catkin_LIBRARIES}) +set(STRATEGY_EXTRA_PRIVATE_LIBS_OR_TARGETS "") +set(STRATEGY_EXTRA_INCLUDE_DIRECTORIES ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) +set(STRATEGY_EXTRA_DEPENDENCIES ${catkin_EXPORTED_TARGETS}) +#set(STRATEGY_PROFILE_EXTENSION ${PROFILES}) + +# no need to modify commands below this line +set(CMAKE_CXX_STANDARD 17) + +if(NOT COMMAND add_fmu_ros_output_strategy) + message(FATAL_ERROR "This project directory has to be included by OSI FMU platform, it can't be build out of that context!") +endif() + +add_fmu_ros_output_strategy(${PROJECT_NAME} ${STRATEGY_ENTRY_POINT}) +if(STRATEGY_PROFILE_EXTENSION) + add_profile_part(${STRATEGY_PROFILE_EXTENSION}) +endif() + +add_library(${PROJECT_NAME} STATIC ${STRATEGY_SOURCES}) +target_link_libraries(${PROJECT_NAME} PRIVATE ${STRATEGY_EXTRA_PRIVATE_LIBS_OR_TARGETS} PUBLIC model::strategy open_simulation_interface_obj ${STRATEGY_EXTRA_PUBLIC_LIBS_OR_TARGETS}) +target_include_directories(${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include ${STRATEGY_EXTRA_INCLUDE_DIRECTORIES}) +set_property(TARGET ${PROJECT_NAME} PROPERTY POSITION_INDEPENDENT_CODE ON) +add_dependencies(${PROJECT_NAME} ${STRATEGY_EXTRA_DEPENDENCIES}) diff --git a/src/model/strategies/ros-output-gtobjects-strategy/README.md b/src/model/strategies/ros-output-gtobjects-strategy/README.md new file mode 100644 index 0000000..4a6ae68 --- /dev/null +++ b/src/model/strategies/ros-output-gtobjects-strategy/README.md @@ -0,0 +1,12 @@ +# OSI Ground Truth Objects to ROS Output Strategy + +This strategy outputs OSI global_ground_truth.moving_objects and global_ground_truth.stationary_objects as ROS Marker messages. +Using the ROS tool rviz is a convenient way to visualize OSI sensor view input and sensor data output. + +## Usage +You need to add the name of the strategy in a new line in the *ros_output_sequence.conf* file in the *src/model/strategies/* folder. + +In order for the strategy to be called during simulation, the FMI parameter *switch_for_ros_output* needs to be set to *1* and be passed to the framework or model fmu. + +## ROS Distributions +This strategy needs a full install of either ROS noetic or ROS melodic. The ROS distribution needs to be located in /opt/ros/. diff --git a/src/model/strategies/ros-output-gtobjects-strategy/include/ros_gt_objects/ros_gt_objects.hpp b/src/model/strategies/ros-output-gtobjects-strategy/include/ros_gt_objects/ros_gt_objects.hpp new file mode 100644 index 0000000..bc903a5 --- /dev/null +++ b/src/model/strategies/ros-output-gtobjects-strategy/include/ros_gt_objects/ros_gt_objects.hpp @@ -0,0 +1,64 @@ +// +// Copyright Institute of Automotive Engineering +// of Technical University of Darmstadt 2020. +// Licensed under the EUPL-1.2-or-later +// +// This work covered by the EUPL can be used/merged and distributed +// in other works covered by GPL-2.0, GPL-3.0, LGPL, AGPL, CeCILL, +// OSL, EPL, MPL and other licences listed as compatible in the EUPL +// Appendix. This applies to the other (combined) work, while the +// original project stays covered by the EUPL without re-licensing. +// +// Alternatively, the contents of this file may be used under the +// terms of the Mozilla Public License, v. 2.0. If a copy of the MPL +// was not distributed with this file, you can obtain one at +// http://mozilla.org/MPL/2.0/. +// + +#ifndef ROS_GT_OBJECTS_HPP +#define ROS_GT_OBJECTS_HPP + +#include +#include +#include +#include +#include +#include + +using namespace model; +using namespace osi3; + +namespace ground_truth { + class WorkerMarker final { + public: + WorkerMarker(const std::string &topic, std::string frame_id); + + void inject(SensorData &sensor_data, const Profile &profile, const Log &log, const Alert &alert); + + private: + const std::string frame_id; + ros::NodeHandle node; + ros::Publisher publisher; + tf::TransformBroadcaster transform_broadcaster; + + static visualization_msgs::Marker set_marker(const Vector3d &position, const Orientation3d &orientation, const Dimension3d &dimension, const uint64_t id, std_msgs::ColorRGBA color, std::string frame, ros::Time time); + static std_msgs::ColorRGBA set_color(float r, float g, float b, float a); + }; +} + +namespace model { + class ros_gt_objects : public Strategy { + public: + ros_gt_objects(const Profile &profile, const Log &log, const Alert &alert); + using Strategy::Strategy; + + void apply(SensorData &) override; + + private: + std::unique_ptr worker_gt = nullptr; + }; +} + + + +#endif //ROS_GT_OBJECTS_HPP diff --git a/src/model/strategies/ros-output-gtobjects-strategy/package.xml b/src/model/strategies/ros-output-gtobjects-strategy/package.xml new file mode 100644 index 0000000..73a3754 --- /dev/null +++ b/src/model/strategies/ros-output-gtobjects-strategy/package.xml @@ -0,0 +1,20 @@ + + + ros_gt_objects + 1.0.0 + Forward ground truth object data to ROS + Clemens Linnhoff + EUPL-1.2 + https://git.rwth-aachen.de/fzd/sensormodeling/fmi-and-osi/visualization-and-output/ros-bridge-gt-objects-strategy + Clemens Linnhoff + + catkin + roscpp + roscpp + roscpp + std_msgs + sensor_msgs + tf + visualization_msgs + diff --git a/src/model/strategies/ros-output-gtobjects-strategy/src/ros_gt_objects.cpp b/src/model/strategies/ros-output-gtobjects-strategy/src/ros_gt_objects.cpp new file mode 100644 index 0000000..9e85ea9 --- /dev/null +++ b/src/model/strategies/ros-output-gtobjects-strategy/src/ros_gt_objects.cpp @@ -0,0 +1,208 @@ +// +// Copyright Institute of Automotive Engineering +// of Technical University of Darmstadt 2021. +// Licensed under the EUPL-1.2-or-later +// +// This work covered by the EUPL can be used/merged and distributed +// in other works covered by GPL-2.0, GPL-3.0, LGPL, AGPL, CeCILL, +// OSL, EPL, MPL and other licences listed as compatible in the EUPL +// Appendix. This applies to the other (combined) work, while the +// original project stays covered by the EUPL without re-licensing. +// +// Alternatively, the contents of this file may be used under the +// terms of the Mozilla Public License, v. 2.0. If a copy of the MPL +// was not distributed with this file, you can obtain one at +// http://mozilla.org/MPL/2.0/. +// + +#include "ros_gt_objects/ros_gt_objects.hpp" +#include "visualization_msgs/MarkerArray.h" +#include "std_msgs/String.h" +#include +#include + +using namespace model; +using namespace osi3; + +void ros_gt_objects::apply(SensorData &sensor_data) { + log("Starting ROS output for GT objects"); + + ros::param::set("/use_sim_time", true); + + if (sensor_data.sensor_view_size() == 0) { + log("No sensor view received"); + return; + } + + if (!sensor_data.sensor_view(0).has_global_ground_truth()) { + log("No global ground truth received"); + return; + } + + auto time_nanos = sensor_data.sensor_view(0).global_ground_truth().timestamp().nanos(); + auto time_seconds = sensor_data.sensor_view(0).global_ground_truth().timestamp().seconds(); + double timestamp = (double)time_seconds + (double) time_nanos / 1000000000; + + if ((sensor_data.sensor_view(0).global_ground_truth().moving_object_size() == 0) && + (sensor_data.sensor_view(0).global_ground_truth().stationary_object_size() == 0)) { + log("No objects in GT for ROS output at timestamp " + std::to_string(timestamp)); + return; + } + + worker_gt->inject(sensor_data, profile, log, alert); +} + +ros_gt_objects::ros_gt_objects(const Profile &profile, const Log &log, const Alert &alert) : model::Strategy(profile, + log, + alert) { + auto remapping = std::map(); + remapping.emplace("__master", "http://localhost:11311"); + ros::init(remapping, "sensor_model_fmu"); + worker_gt = std::make_unique("ground_truth", "world"); +} + +/// Objects +ground_truth::WorkerMarker::WorkerMarker(const std::string &topic, std::string frame_id) : publisher( + node.advertise(topic, 1)), frame_id(std::move(frame_id)) {} + +void ground_truth::WorkerMarker::inject(SensorData &sensor_data, const Profile &profile, const Log &log, + const Alert &alert) { +// auto time = ros::Time::now(); + auto sim_seconds = uint32_t(sensor_data.sensor_view(0).global_ground_truth().timestamp().seconds()); + auto sim_nanos = uint32_t(sensor_data.sensor_view(0).global_ground_truth().timestamp().nanos()); + auto sim_time = ros::Time(sim_seconds, sim_nanos); + auto sim_time_behind = ros::Time(sim_seconds, sim_nanos + 500); + + /// Global Ground Truth + visualization_msgs::MarkerArray marker_array; + const MovingObject *ego_vehicle; + + /// Moving objects + std_msgs::ColorRGBA color_gt_moving; + for (const MovingObject >_moving_object: sensor_data.sensor_view(0).global_ground_truth().moving_object()) { + if (gt_moving_object.id().value() == + sensor_data.sensor_view(0).global_ground_truth().host_vehicle_id().value()) { + ego_vehicle = >_moving_object; + color_gt_moving = set_color(1.0, 0.0, 0.0, 0.3); + } else { + color_gt_moving = set_color(1.0, 1.0, 1.0, 0.3); + } + visualization_msgs::Marker marker = set_marker(gt_moving_object.base().position(), + gt_moving_object.base().orientation(), + gt_moving_object.base().dimension(), + gt_moving_object.id().value(), + color_gt_moving, + frame_id, sim_time_behind); + marker_array.markers.push_back(marker); + } + /// Stationary objects + std_msgs::ColorRGBA color_gt_stationary = set_color(1.0, 1.0, 1.0, 0.1); + for (const StationaryObject >_stationary_object: sensor_data.sensor_view( + 0).global_ground_truth().stationary_object()) { + visualization_msgs::Marker marker = set_marker(gt_stationary_object.base().position(), + gt_stationary_object.base().orientation(), + gt_stationary_object.base().dimension(), + gt_stationary_object.id().value(), + color_gt_stationary, + frame_id, sim_time_behind); + marker_array.markers.push_back(marker); + } + + /// Transformations + tf::Transform transform; + tf::Quaternion q; + /// Transform Bounding Box Center to world + if (ego_vehicle->has_base()) { + transform.setOrigin(tf::Vector3(ego_vehicle->base().position().x(), ego_vehicle->base().position().y(), + ego_vehicle->base().position().z())); + q.setRPY(ego_vehicle->base().orientation().roll(), ego_vehicle->base().orientation().pitch(), + ego_vehicle->base().orientation().yaw()); + transform.setRotation(q); + transform_broadcaster.sendTransform(tf::StampedTransform(transform, sim_time, "world", "bb_center")); + } else { + log("Ego vehicle has no base!"); + } + + /// Transform Bounding Box Center to center of rear axle + if (ego_vehicle->has_vehicle_attributes()) { + transform.setOrigin(tf::Vector3(ego_vehicle->vehicle_attributes().bbcenter_to_rear().x(), + ego_vehicle->vehicle_attributes().bbcenter_to_rear().y(), + ego_vehicle->vehicle_attributes().bbcenter_to_rear().z())); + q.setRPY(0, 0, 0); + transform.setRotation(q); + transform_broadcaster.sendTransform(tf::StampedTransform(transform, sim_time, "bb_center", "base_link")); + } else { + log("Ego vehicle has no vehicle_attributes!"); + } + + for (auto &sensor_view: sensor_data.sensor_view()) { + /// Transform center of rear axle to sensor('s) origin + size_t radar_sensor_no = 0; + for (auto &radar_sensor_view: sensor_view.radar_sensor_view()) { + transform.setOrigin(tf::Vector3(radar_sensor_view.view_configuration().mounting_position().position().x(), + radar_sensor_view.view_configuration().mounting_position().position().y(), + radar_sensor_view.view_configuration().mounting_position().position().z())); + q.setRPY(radar_sensor_view.view_configuration().mounting_position().orientation().roll(), + radar_sensor_view.view_configuration().mounting_position().orientation().pitch(), + radar_sensor_view.view_configuration().mounting_position().orientation().yaw()); + transform.setRotation(q); + transform_broadcaster.sendTransform( + tf::StampedTransform(transform, sim_time, "base_link", "sensor_" + std::to_string(radar_sensor_no))); + radar_sensor_no++; + } + size_t lidar_sensor_no = 0; + for (auto &lidar_sensor_view: sensor_view.lidar_sensor_view()) { + transform.setOrigin(tf::Vector3(lidar_sensor_view.view_configuration().mounting_position().position().x(), + lidar_sensor_view.view_configuration().mounting_position().position().y(), + lidar_sensor_view.view_configuration().mounting_position().position().z())); + q.setRPY(lidar_sensor_view.view_configuration().mounting_position().orientation().roll(), + lidar_sensor_view.view_configuration().mounting_position().orientation().pitch(), + lidar_sensor_view.view_configuration().mounting_position().orientation().yaw()); + transform.setRotation(q); + transform_broadcaster.sendTransform( + tf::StampedTransform(transform, sim_time, "base_link", "sensor_" + std::to_string(lidar_sensor_no))); + lidar_sensor_no++; + } + + publisher.publish(marker_array); + } +} + +visualization_msgs::Marker +ground_truth::WorkerMarker::set_marker(const Vector3d &position, const Orientation3d &orientation, + const Dimension3d &dimension, const size_t id, std_msgs::ColorRGBA color, + std::string frame, ros::Time time) { + visualization_msgs::Marker marker; + marker.header.frame_id = std::move(frame); + marker.header.stamp = time; + marker.id = (int)id; + marker.type = visualization_msgs::Marker::CUBE; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.position.x = position.x(); + marker.pose.position.y = position.y(); + marker.pose.position.z = position.z(); + tf::Quaternion orientation_q; + orientation_q.setRPY(orientation.roll(), + orientation.pitch(), + orientation.yaw()); + marker.pose.orientation.x = orientation_q.x(); + marker.pose.orientation.y = orientation_q.y(); + marker.pose.orientation.z = orientation_q.z(); + marker.pose.orientation.w = orientation_q.w(); + marker.scale.x = dimension.length(); + marker.scale.y = dimension.width(); + marker.scale.z = dimension.height(); + marker.color = color; + marker.lifetime = ros::Duration(1); + marker.frame_locked = false; + return marker; +} + +std_msgs::ColorRGBA ground_truth::WorkerMarker::set_color(float r, float g, float b, float a) { + std_msgs::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + color.a = a; + return color; +} \ No newline at end of file diff --git a/src/model/strategies/transformation-functions b/src/model/strategies/transformation-functions deleted file mode 160000 index a651e95..0000000 --- a/src/model/strategies/transformation-functions +++ /dev/null @@ -1 +0,0 @@ -Subproject commit a651e95d6c05e26a1cc73a6b6928e01aab1b3c23 diff --git a/src/model/strategies/transformation-functions/TransformationFunctions.cpp b/src/model/strategies/transformation-functions/TransformationFunctions.cpp new file mode 100644 index 0000000..5eea5e4 --- /dev/null +++ b/src/model/strategies/transformation-functions/TransformationFunctions.cpp @@ -0,0 +1,307 @@ +// +// Copyright Clemens Linnhoff, M. Sc. +// Institute of Automotive Engineering +// of Technical University of Darmstadt, 2021. +// Licensed under the EUPL-1.2-or-later +// +// This work covered by the EUPL can be used/merged and distributed +// in other works covered by GPL-2.0, GPL-3.0, LGPL, AGPL, CeCILL, +// OSL, EPL, MPL and other licences listed as compatible in the EUPL +// Appendix. This applies to the other (combined) work, while the +// original project stays covered by the EUPL without re-licensing. +// +// Alternatively, the contents of this file may be used under the +// terms of the Mozilla Public License, v. 2.0. If a copy of the MPL +// was not distributed with this file, you can obtain one at +// http://mozilla.org/MPL/2.0/. +// + +#ifndef _USE_MATH_DEFINES +#define _USE_MATH_DEFINES +#endif + +#define PREV_INDEX(itr, total_num) ((itr - 1 + total_num) % total_num) +#define NEXT_INDEX(itr, total_num) ((itr + 1 ) % total_num) + +#include "TransformationFunctions.hpp" + +#ifdef _WIN32 +#include +#else +#include +#endif + +using namespace model; +using namespace osi3; + +Orientation3d TF::calc_relative_orientation_to_local(const Orientation3d& object_orientation, const Orientation3d& ego_orientation) { + + Eigen::Matrix3d ego_rotation_matrix = calc_rotation_matrix_from_euler_angles(ego_orientation); + Eigen::Matrix3d object_rotation_matrix = calc_rotation_matrix_from_euler_angles(object_orientation); + Eigen::Matrix3d resulting_rotation_matrix = object_rotation_matrix * ego_rotation_matrix.transpose(); + + Orientation3d relative_orientation = calc_euler_angles_from_rotation_matrix(resulting_rotation_matrix); + + return relative_orientation; +} + +Orientation3d TF::calc_relative_orientation_from_local(const Orientation3d& object_orientation, const Orientation3d& ego_orientation) { + + Eigen::Matrix3d ego_rotation_matrix = calc_rotation_matrix_from_euler_angles(ego_orientation); + Eigen::Matrix3d object_rotation_matrix = calc_rotation_matrix_from_euler_angles(object_orientation); + Eigen::Matrix3d resulting_rotation_matrix = object_rotation_matrix * ego_rotation_matrix; + + Orientation3d relative_orientation = calc_euler_angles_from_rotation_matrix(resulting_rotation_matrix); + + return relative_orientation; +} + +Eigen::Matrix3d TF::calc_rotation_matrix_from_euler_angles(const Orientation3d& orientation) { + Eigen::Matrix3d R; + R(0,0) = cos(orientation.pitch()) * cos(orientation.yaw()); + R(0,1) = cos(orientation.pitch()) * sin(orientation.yaw()); + R(0,2) = -sin(orientation.pitch()); + R(1,0) = sin(orientation.roll()) * sin(orientation.pitch()) * cos(orientation.yaw()) - cos(orientation.roll()) * sin(orientation.yaw()); + R(1,1) = sin(orientation.roll()) * sin(orientation.pitch()) * sin(orientation.yaw()) + cos(orientation.roll()) * cos(orientation.yaw()); + R(1,2) = sin(orientation.roll()) * cos(orientation.pitch()); + R(2,0) = cos(orientation.roll()) * sin(orientation.pitch()) * cos(orientation.yaw()) + sin(orientation.yaw()) * sin(orientation.roll()); + R(2,1) = cos(orientation.roll()) * sin(orientation.pitch()) * sin(orientation.yaw()) - sin(orientation.roll()) * cos(orientation.yaw()); + R(2,2) = cos(orientation.roll()) * cos(orientation.pitch()); + return R; +} + +Orientation3d TF::calc_euler_angles_from_rotation_matrix(Eigen::Matrix3d R) { + Orientation3d orientation; + orientation.set_pitch(-asin(R(0,2))); + if(cos(orientation.pitch()) != 0) { + orientation.set_yaw(atan2(R(0,1)/cos(orientation.pitch()),R(0,0)/cos(orientation.pitch()))); + orientation.set_roll(atan2(R(1,2)/cos(orientation.pitch()),R(2,2)/cos(orientation.pitch()))); + } else { + orientation.set_yaw(0); + orientation.set_roll(atan2(R(1,0),R(1,1))); + } + return orientation; +} + +Vector3d TF::transform_to_local_coordinates(const Vector3d& input_coordinates, const Orientation3d& new_origin_rotation, const Vector3d& new_origin_translation) { + double delta_x = input_coordinates.x() - new_origin_translation.x(); + double delta_y = input_coordinates.y() - new_origin_translation.y(); + double delta_z = input_coordinates.z() - new_origin_translation.z(); + + Eigen::Matrix3d R = calc_rotation_matrix_from_euler_angles(new_origin_rotation); + double x_rel = delta_x * R(0,0) + + delta_y * R(0,1) + + delta_z * R(0,2); + double y_rel = delta_x * R(1,0) + + delta_y * R(1,1) + + delta_z * R(1,2); + double z_rel = delta_x * R(2,0) + + delta_y * R(2,1) + + delta_z * R(2,2); + + Vector3d coordinates_new_origin; + coordinates_new_origin.set_x(x_rel); + coordinates_new_origin.set_y(y_rel); + coordinates_new_origin.set_z(z_rel); + + return coordinates_new_origin; +} + + +Vector3d TF::transform_from_local_coordinates(const Vector3d& input_coordinates, const Orientation3d& new_origin_rotation, const Vector3d& new_origin_translation) { + + Eigen::Matrix3d R = calc_rotation_matrix_from_euler_angles(new_origin_rotation); + double x_rel = input_coordinates.x() * R.transpose()(0,0) + + input_coordinates.y() * R.transpose()(0,1) + + input_coordinates.z() * R.transpose()(0,2); + double y_rel = input_coordinates.x() * R.transpose()(1,0) + + input_coordinates.y() * R.transpose()(1,1) + + input_coordinates.z() * R.transpose()(1,2); + double z_rel = input_coordinates.x() * R.transpose()(2,0) + + input_coordinates.y() * R.transpose()(2,1) + + input_coordinates.z() * R.transpose()(2,2); + + double delta_x = x_rel + new_origin_translation.x(); + double delta_y = y_rel + new_origin_translation.y(); + double delta_z = z_rel + new_origin_translation.z(); + + Vector3d coordinates_new_origin; + coordinates_new_origin.set_x(delta_x); + coordinates_new_origin.set_y(delta_y); + coordinates_new_origin.set_z(delta_z); + + return coordinates_new_origin; +} + +Vector3d TF::transform_position_from_ego_to_world_coordinates(const Vector3d &position_in_ego_coordinates, const TF::EgoData &ego_data) { + Vector3d position_to_ego_bb_center = vector_translation(position_in_ego_coordinates, ego_data.ego_vehicle_attributes.bbcenter_to_rear(), 1); + Vector3d position_in_world_coordinates = transform_from_local_coordinates(position_to_ego_bb_center, ego_data.ego_base.orientation(), ego_data.ego_base.position()); + return position_in_world_coordinates; +} + +Vector3d TF::transform_position_from_world_to_ego_coordinates(const Vector3d &input_in_world_coordinates, const TF::EgoData &ego_data) { + Vector3d position_ego_bb_center = transform_to_local_coordinates(input_in_world_coordinates, ego_data.ego_base.orientation(), ego_data.ego_base.position()); + Vector3d position_ego_coordinates = vector_translation(position_ego_bb_center, ego_data.ego_vehicle_attributes.bbcenter_to_rear(), -1); + return position_ego_coordinates; +} + +Vector3d TF::transform_position_from_world_to_sensor_coordinates(const Vector3d &input_in_world_coordinates, const TF::EgoData &ego_data, const MountingPosition &mounting_pose) { + Vector3d position_ego_bb_center = transform_to_local_coordinates(input_in_world_coordinates, ego_data.ego_base.orientation(), ego_data.ego_base.position()); + Vector3d position_ego_coordinates = vector_translation(position_ego_bb_center, ego_data.ego_vehicle_attributes.bbcenter_to_rear(), -1); + Vector3d position_cartesian_sensor_coordinates = transform_to_local_coordinates(position_ego_coordinates, mounting_pose.orientation(), mounting_pose.position()); + return position_cartesian_sensor_coordinates; +} + +Orientation3d TF::transform_orientation_from_world_to_sensor_coordinates(const Orientation3d &orientation_in_world_coordinates, const TF::EgoData &ego_data, const MountingPosition &mounting_pose) { + Orientation3d relative_orientation_ego_coordinates = calc_relative_orientation_to_local(orientation_in_world_coordinates, ego_data.ego_base.orientation()); + Orientation3d relative_orientation_sensor_coordinates = calc_relative_orientation_to_local(relative_orientation_ego_coordinates, mounting_pose.orientation()); + return relative_orientation_sensor_coordinates; +} + +Orientation3d TF::transform_orientation_from_sensor_to_world_coordinates(const Orientation3d &orientation_in_sensor_coordinates, const TF::EgoData &ego_data, const MountingPosition &mounting_pose) { + Orientation3d relative_orientation_ego_coordinates = calc_relative_orientation_from_local(orientation_in_sensor_coordinates, mounting_pose.orientation()); + Orientation3d relative_orientation_world_coordinates = calc_relative_orientation_from_local(relative_orientation_ego_coordinates, ego_data.ego_base.orientation()); + return relative_orientation_world_coordinates; +} + +Vector3d TF::transform_position_from_object_to_ego_coordinates(const Vector3d &pcl_segment_position_in_object, const TF::EgoData &ego_data, const MovingObject >_object) { + Vector3d position_in_world_coordinates = TF::transform_from_local_coordinates(pcl_segment_position_in_object, gt_object.base().orientation(), gt_object.base().position()); + Vector3d position_in_ego_coordinates = TF::transform_position_from_world_to_ego_coordinates(position_in_world_coordinates, ego_data); + return position_in_ego_coordinates; +} + +Vector3d TF::transform_position_from_ego_to_object_coordinates(const Vector3d &pcl_segment_position_in_ego, const TF::EgoData &ego_data, const MovingObject >_object) { + Vector3d position_in_world_coordinates = TF::transform_position_from_ego_to_world_coordinates(pcl_segment_position_in_ego, ego_data); + Vector3d position_in_object_coordinates = TF::transform_to_local_coordinates(position_in_world_coordinates, gt_object.base().orientation(), gt_object.base().position()); + return position_in_object_coordinates; +} + +Vector3d TF::transform_position_from_sensor_to_object_coordinates(const Vector3d &pcl_segment_position_in_sensor, const TF::EgoData &ego_data, const MountingPosition &mounting_pose, const BaseMoving &object_base) { + Vector3d pcl_segment_position_in_ego = TF::transform_from_local_coordinates(pcl_segment_position_in_sensor, mounting_pose.orientation(), mounting_pose.position()); + Vector3d position_in_world_coordinates = TF::transform_position_from_ego_to_world_coordinates(pcl_segment_position_in_ego, ego_data); + Vector3d position_in_object_coordinates = TF::transform_to_local_coordinates(position_in_world_coordinates, object_base.orientation(), object_base.position()); + return position_in_object_coordinates; +} + +Vector3d TF::transform_position_from_object_to_sensor_coordinates(const Vector3d &pcl_segment_position_in_object, const TF::EgoData &ego_data, const MountingPosition &mounting_pose, const BaseMoving &object_base) { + Vector3d position_in_world_coordinates = TF::transform_from_local_coordinates(pcl_segment_position_in_object, object_base.orientation(), object_base.position()); + Vector3d position_in_ego_coordinates = TF::transform_position_from_world_to_ego_coordinates(position_in_world_coordinates, ego_data); + Vector3d position_in_sensor_coordinates = TF::transform_to_local_coordinates(position_in_ego_coordinates, mounting_pose.orientation(), mounting_pose.position()); + return position_in_sensor_coordinates; +} + +Vector3d TF::transform_position_from_object_to_world_coordinates(const Vector3d &pcl_segment_position_in_object, const BaseMoving &object_base) { + Vector3d position_in_world_coordinates = TF::transform_from_local_coordinates(pcl_segment_position_in_object, object_base.orientation(), object_base.position()); + return position_in_world_coordinates; +} + +Spherical3d TF::transform_cartesian_to_spherical(const Vector3d &input_relative_position) { + double distance; + double azimuth; + double elevation; + distance = sqrt(input_relative_position.x() * input_relative_position.x() + + input_relative_position.y() * input_relative_position.y() + + input_relative_position.z() * input_relative_position.z()); + azimuth = atan2(input_relative_position.y() , input_relative_position.x()); + elevation = asin(input_relative_position.z() / distance); + Spherical3d spherical_coordinate; + spherical_coordinate.set_distance(distance); + spherical_coordinate.set_azimuth(azimuth); + spherical_coordinate.set_elevation(elevation); + return spherical_coordinate; +} + +double TF::projection_to_spherical_surface(std::vector &spherical_coordinates, double distance) { + // the input is in spherical coordinate system + if (spherical_coordinates.size() < 3) return 0; + double processed_prev, processed_next, ready_to_add; + double sum_of_spherical_angle = 0.0; + double elevation_a, azimuth_a, elevation_b, azimuth_b, elevation_c, azimuth_c; + auto points_number = spherical_coordinates.size(); + for (size_t itr = 0; itr != points_number; ++itr) { + azimuth_c = spherical_coordinates.at(NEXT_INDEX(itr, points_number)).azimuth(); + elevation_c = spherical_coordinates.at(NEXT_INDEX(itr, points_number)).elevation(); + azimuth_b = spherical_coordinates.at(itr).azimuth(); + elevation_b = spherical_coordinates.at(itr).elevation(); + azimuth_a = spherical_coordinates.at(PREV_INDEX(itr, points_number)).azimuth(); + elevation_a = spherical_coordinates.at(PREV_INDEX(itr, points_number)).elevation(); + processed_prev = atan2((sin(elevation_a - elevation_b) * cos(azimuth_a)), + (sin(azimuth_a) * cos(azimuth_b) - cos(azimuth_a) * sin(azimuth_b) * cos(elevation_a - elevation_b))); + processed_next = atan2((sin(elevation_c - elevation_b) * cos(azimuth_c)), + (sin(azimuth_c) * cos(azimuth_b) - cos(azimuth_c) * sin(azimuth_b) * cos(elevation_c - elevation_b))); + processed_prev < processed_next ? ready_to_add = (processed_prev - processed_next) + 2 * M_PI : ready_to_add = (processed_prev - processed_next); + sum_of_spherical_angle += ready_to_add; + } + return (sum_of_spherical_angle - double(points_number - 2) * M_PI) * distance * distance; +} + +void TF::transform_spherical_to_cartesian(const Spherical3d &input_spherical_coordinate, Vector3d &vector_xyz) { + double x; + double y; + double z; + x = input_spherical_coordinate.distance() * cos(input_spherical_coordinate.elevation()) * cos(input_spherical_coordinate.azimuth()); + y = input_spherical_coordinate.distance() * cos(input_spherical_coordinate.elevation()) * sin(input_spherical_coordinate.azimuth()); + z = input_spherical_coordinate.distance() * sin(input_spherical_coordinate.elevation()); + vector_xyz.set_x(x); + vector_xyz.set_y(y); + vector_xyz.set_z(z); +} + +void TF::projection_onto_unit_distance_cylinder(const Spherical3d &input_spherical_coordinate, Vector2d &output_position_on_plane) { + output_position_on_plane.set_x(input_spherical_coordinate.azimuth()); + output_position_on_plane.set_y(tan(input_spherical_coordinate.elevation())); +} + +bool TF::get_ego_info(TF::EgoData &ego_data, const SensorView &input_sensor_view) { + if (input_sensor_view.global_ground_truth().has_host_vehicle_id()) { + ego_data.ego_vehicle_id = input_sensor_view.global_ground_truth().host_vehicle_id(); + bool ego_vehicle_found = false; + int mov_obj_no = 0; + while (!ego_vehicle_found && mov_obj_no < input_sensor_view.global_ground_truth().moving_object_size()) { + if (input_sensor_view.global_ground_truth().moving_object(mov_obj_no).id().value() == ego_data.ego_vehicle_id.value()) { + ego_data.ego_vehicle_no = mov_obj_no; + ego_vehicle_found = true; + } + mov_obj_no++; + } + if (!ego_vehicle_found) { + return false; + } + if (input_sensor_view.global_ground_truth().moving_object_size() > ego_data.ego_vehicle_no && + input_sensor_view.global_ground_truth().moving_object(ego_data.ego_vehicle_no).has_base()) { + ego_data.ego_base.CopyFrom(input_sensor_view.global_ground_truth().moving_object(ego_data.ego_vehicle_no).base()); + ego_data.sensor_mounting_parameters.CopyFrom(input_sensor_view.mounting_position()); + ego_data.ego_vehicle_attributes.CopyFrom(input_sensor_view.global_ground_truth().moving_object(ego_data.ego_vehicle_no).vehicle_attributes()); + } else { + return false; + } + } else { + return false; + } + return true; +} + +Vector3d TF::vector_translation(const Vector3d &vector_a, const Vector3d &vector_b, double factor_for_vector_b) { + + Vector3d vector_result; + vector_result.set_x(vector_a.x() + factor_for_vector_b * vector_b.x()); + vector_result.set_y(vector_a.y() + factor_for_vector_b * vector_b.y()); + vector_result.set_z(vector_a.z() + factor_for_vector_b * vector_b.z()); + return vector_result; +} + +double TF::get_vector_abs(const Vector3d& vector) { + return sqrt(pow(vector.x(), 2) + pow(vector.y(), 2) + pow(vector.z(), 2)); +} + +double TF::dot_product(const Vector3d &vector_a, const Vector3d &vector_b) { + return vector_a.x()*vector_b.x() + vector_a.y()*vector_b.y() + vector_a.z()*vector_b.z(); +} + +Vector3d TF::cross_product(const Vector3d &vector_a, const Vector3d &vector_b) { + Vector3d vector_result; + vector_result.set_x(vector_a.y()*vector_b.z() - vector_a.z()*vector_b.y()); + vector_result.set_y(vector_a.z()*vector_b.x() - vector_a.x()*vector_b.z()); + vector_result.set_z(vector_a.x()*vector_b.y() - vector_a.y()*vector_b.x()); + return vector_result; +} diff --git a/src/model/strategies/transformation-functions/TransformationFunctions.hpp b/src/model/strategies/transformation-functions/TransformationFunctions.hpp new file mode 100644 index 0000000..82c44d9 --- /dev/null +++ b/src/model/strategies/transformation-functions/TransformationFunctions.hpp @@ -0,0 +1,78 @@ +// +// Copyright Clemens Linnhoff, M. Sc. +// Institute of Automotive Engineering +// of Technical University of Darmstadt, 2021. +// Licensed under the EUPL-1.2-or-later +// +// This work covered by the EUPL can be used/merged and distributed +// in other works covered by GPL-2.0, GPL-3.0, LGPL, AGPL, CeCILL, +// OSL, EPL, MPL and other licences listed as compatible in the EUPL +// Appendix. This applies to the other (combined) work, while the +// original project stays covered by the EUPL without re-licensing. +// +// Alternatively, the contents of this file may be used under the +// terms of the Mozilla Public License, v. 2.0. If a copy of the MPL +// was not distributed with this file, you can obtain one at +// http://mozilla.org/MPL/2.0/. +// + +#ifndef TRANSFORMATIONFUNCTIONS_H +#define TRANSFORMATIONFUNCTIONS_H + +#include "osi_sensordata.pb.h" +#include + +using namespace osi3; + +namespace model { + + class TF { + public: + struct EgoData { + BaseMoving ego_base; + MountingPosition sensor_mounting_parameters; + Identifier ego_vehicle_id; + MovingObject::VehicleAttributes ego_vehicle_attributes; + int ego_vehicle_no; + }; + + static Orientation3d calc_relative_orientation_to_local(const Orientation3d& object_orientation, const Orientation3d& ego_orientation); + static Orientation3d calc_relative_orientation_from_local(const Orientation3d& object_orientation, const Orientation3d& ego_orientation); + + static Eigen::Matrix3d calc_rotation_matrix_from_euler_angles(const Orientation3d& orientation); + static Orientation3d calc_euler_angles_from_rotation_matrix(Eigen::Matrix3d R); + + static Vector3d transform_to_local_coordinates(const Vector3d& input_coordinates, const Orientation3d& new_origin_rotation, const Vector3d& new_origin_translation); + static Vector3d transform_from_local_coordinates(const Vector3d& input_coordinates, const Orientation3d& new_origin_rotation, const Vector3d& new_origin_translation); + + static Vector3d transform_position_from_world_to_ego_coordinates(const Vector3d &input_in_world_coordinates, const TF::EgoData &ego_data); + static Vector3d transform_position_from_ego_to_world_coordinates(const Vector3d &position_in_ego_coordinates, const TF::EgoData &ego_data); + + static Vector3d transform_position_from_world_to_sensor_coordinates(const Vector3d &input_in_world_coordinates, const TF::EgoData &ego_data, const MountingPosition &mounting_pose); + + static Vector3d transform_position_from_object_to_ego_coordinates(const Vector3d &pcl_segment_position_in_object, const TF::EgoData &ego_data, const MovingObject >_object); + static Vector3d transform_position_from_ego_to_object_coordinates(const Vector3d &pcl_segment_position_in_ego, const EgoData &ego_data, const MovingObject >_object); + static Vector3d transform_position_from_sensor_to_object_coordinates(const Vector3d &pcl_segment_position_in_sensor, const EgoData &ego_data, const MountingPosition &mounting_pose, const BaseMoving &object_base); + static Vector3d transform_position_from_object_to_sensor_coordinates(const Vector3d &pcl_segment_position_in_object, const TF::EgoData &ego_data, const MountingPosition &mounting_pose, const BaseMoving &object_base); + static Vector3d transform_position_from_object_to_world_coordinates(const Vector3d &pcl_segment_position_in_object, const BaseMoving &object_base); + + + static Orientation3d transform_orientation_from_world_to_sensor_coordinates(const Orientation3d &orientation_in_world_coordinates, const TF::EgoData &ego_data, const MountingPosition &mounting_pose); + static Orientation3d transform_orientation_from_sensor_to_world_coordinates(const Orientation3d &orientation_in_sensor_coordinates, const EgoData &ego_data, const MountingPosition &mounting_pose); + + static Spherical3d transform_cartesian_to_spherical(const Vector3d &input_relative_position); + static void transform_spherical_to_cartesian(const Spherical3d &input_spherical_coordinate, Vector3d &vector_xyz); + + static double projection_to_spherical_surface(std::vector &spherical_coordinates, double distance); + static void projection_onto_unit_distance_cylinder(const Spherical3d &input_spherical_coordinate, Vector2d &output_position_on_plane); + + static bool get_ego_info(TF::EgoData &ego_data, const SensorView &input_sensor_view); + + static Vector3d vector_translation(const Vector3d &vector_a, const Vector3d &vector_b, double factor_for_vector_b); + static double get_vector_abs(const Vector3d &vector); + static double dot_product(const Vector3d &vector_a, const Vector3d &vector_b); + static Vector3d cross_product(const Vector3d &vector_a, const Vector3d &vector_b); + }; +} + +#endif //TRANSFORMATIONFUNCTIONS_H