1+ //
2+ // Copyright Institute of Automotive Engineering
3+ // of Technical University of Darmstadt 2020.
4+ // Licensed under the EUPL-1.2-or-later
5+ //
6+ // This work covered by the EUPL can be used/merged and distributed
7+ // in other works covered by GPL-2.0, GPL-3.0, LGPL, AGPL, CeCILL,
8+ // OSL, EPL, MPL and other licences listed as compatible in the EUPL
9+ // Appendix. This applies to the other (combined) work, while the
10+ // original project stays covered by the EUPL without re-licensing.
11+ //
12+ // Alternatively, the contents of this file may be used under the
13+ // terms of the Mozilla Public License, v. 2.0. If a copy of the MPL
14+ // was not distributed with this file, you can obtain one at
15+ // http://mozilla.org/MPL/2.0/.
16+ //
17+
18+ #ifndef _USE_MATH_DEFINES
19+ #define _USE_MATH_DEFINES
20+ #endif
21+
22+ #include " csvoutputgtobjects/CsvOutputGTObjects.hpp"
23+ #include " ../../transformation-functions/TransformationFunctions.hpp"
24+ #include < fstream>
25+ #include < iostream>
26+ #include < vector>
27+ #include < ctime>
28+
29+ #ifdef _WIN32
30+ #include < math.h>
31+ #include < direct.h>
32+ #else
33+ #include < cmath>
34+ #include < sys/stat.h>
35+ #endif
36+
37+ using namespace model ;
38+ using namespace osi3 ;
39+
40+ static bool first_call = true ;
41+
42+ void model::CsvOutputGTObjects::apply (SensorData &sensor_data) {
43+ log (" Starting .csv output for GT objects"
44+
45+ if (sensor_data.sensor_view_size () == 0 ) {
46+ log (" No sensor view received for .csv output"
47+ return ;
48+ }
49+
50+ TF::EgoData ego_data;
51+ if (!TF::get_ego_info (ego_data, sensor_data.sensor_view (0 ))) {
52+ log (" Ego vehicle has no base, no id, or is not contained in GT moving objects."
53+ return ;
54+ }
55+
56+ auto time_nanos = sensor_data.sensor_view (0 ).global_ground_truth ().timestamp ().nanos ();
57+ auto time_seconds = sensor_data.sensor_view (0 ).global_ground_truth ().timestamp ().seconds ();
58+ double timestamp = (double )time_seconds + (double ) time_nanos / 1000000000 ;
59+
60+ if (!sensor_data.sensor_view (0 ).has_global_ground_truth ()) {
61+ log (" No GT for .csv output at timestamp " std::to_string (timestamp));
62+ return ;
63+ }
64+
65+ if ((sensor_data.sensor_view (0 ).global_ground_truth ().moving_object_size () == 0 ) &&
66+ (sensor_data.sensor_view (0 ).global_ground_truth ().stationary_object_size () == 0 )) {
67+ log (" No objects in GT for .csv output at timestamp " std::to_string (timestamp));
68+ return ;
69+ }
70+
71+ // / Write header line of .csv on first call
72+ if (first_call) {
73+ #include < csvoutputgtobjects/set_csv_file_path_gtobjects.cpp>
74+ write_first_line_to_CSV (file_path_gtobjects);
75+ first_call = false ;
76+ }
77+
78+ auto no_of_moving_objects = sensor_data.sensor_view (0 ).global_ground_truth ().moving_object_size ();
79+ auto no_of_stationary_objects = sensor_data.sensor_view (0 ).global_ground_truth ().stationary_object_size ();
80+
81+ // / Start a vector for gt_objects with (gt_id, x, y, z, roll, pitch, yaw, width, length, height, is_moving)
82+ std::vector<GT_object> gt_objects;
83+ gt_objects.reserve (no_of_moving_objects + no_of_stationary_objects);
84+
85+ // / Collect moving objects
86+ for (const auto >_moving_object : sensor_data.sensor_view (0 ).global_ground_truth ().moving_object ()) {
87+ if (gt_moving_object.id ().value () == sensor_data.sensor_view (0 ).global_ground_truth ().host_vehicle_id ().value ())
88+ continue ;
89+
90+ Vector3d relative_position_ego_coordinate_system = TF::transform_position_from_world_to_ego_coordinates (gt_moving_object.base ().position (), ego_data);
91+ Orientation3d relative_orientation = TF::calc_relative_orientation_to_local (gt_moving_object.base ().orientation (), ego_data.ego_base .orientation ());
92+ GT_object actual_gt_object;
93+ actual_gt_object.id = gt_moving_object.id ().value ();
94+ actual_gt_object.x = std::round (float (relative_position_ego_coordinate_system.x ()) * 1000 ) / 1000 ;
95+ actual_gt_object.y = std::round (float (relative_position_ego_coordinate_system.y ()) * 1000 ) / 1000 ;
96+ actual_gt_object.z = std::round (float (relative_position_ego_coordinate_system.z ()) * 1000 ) / 1000 ;
97+ actual_gt_object.roll = (float )std::round (float (relative_orientation.roll ()) * 180 / M_PI * 1000 ) / 1000 ;
98+ actual_gt_object.pitch = (float )std::round (float (relative_orientation.pitch ()) * 180 / M_PI * 1000 ) / 1000 ;
99+ actual_gt_object.yaw = (float )std::round (float (relative_orientation.yaw ()) * 180 / M_PI * 1000 ) / 1000 ;
100+ actual_gt_object.width = float (gt_moving_object.base ().dimension ().width ());
101+ actual_gt_object.length = float (gt_moving_object.base ().dimension ().length ());
102+ actual_gt_object.height = float (gt_moving_object.base ().dimension ().height ());
103+ actual_gt_object.is_moving = true ;
104+ gt_objects.emplace_back (actual_gt_object);
105+ }
106+
107+ // / Collect stationary objects
108+ for (const auto >_stationary_object : sensor_data.sensor_view (0 ).global_ground_truth ().stationary_object ()) {
109+ Vector3d relative_position_ego_coordinate_system = TF::transform_position_from_world_to_ego_coordinates (
110+ gt_stationary_object.base ().position (), ego_data);
111+ Orientation3d relative_orientation = TF::calc_relative_orientation_to_local (
112+ gt_stationary_object.base ().orientation (),
113+ ego_data.ego_base .orientation ());
114+ GT_object actual_gt_object;
115+ actual_gt_object.id = gt_stationary_object.id ().value ();
116+ actual_gt_object.x = std::round (float (relative_position_ego_coordinate_system.x ()) * 1000 ) / 1000 ;
117+ actual_gt_object.y = std::round (float (relative_position_ego_coordinate_system.y ()) * 1000 ) / 1000 ;
118+ actual_gt_object.z = std::round (float (relative_position_ego_coordinate_system.z ()) * 1000 ) / 1000 ;
119+ actual_gt_object.roll = (float )std::round (float (relative_orientation.roll ()) * 180 / M_PI * 1000 ) / 1000 ;
120+ actual_gt_object.pitch = (float )std::round (float (relative_orientation.pitch ()) * 180 / M_PI * 1000 ) / 1000 ;
121+ actual_gt_object.yaw = (float )std::round (float (relative_orientation.yaw ()) * 180 / M_PI * 1000 ) / 1000 ;
122+ actual_gt_object.width = float (gt_stationary_object.base ().dimension ().width ());
123+ actual_gt_object.length = float (gt_stationary_object.base ().dimension ().length ());
124+ actual_gt_object.height = float (gt_stationary_object.base ().dimension ().height ());
125+ actual_gt_object.is_moving = false ;
126+ gt_objects.emplace_back (actual_gt_object);
127+ }
128+
129+ for (const auto >_object : gt_objects) {
130+ auto gt_id = gt_object.id ;
131+
132+ auto roll = std::round (gt_object.roll * 180 / M_PI * 1000 ) / 1000 ;
133+ auto pitch = std::round (gt_object.pitch * 180 / M_PI * 1000 ) / 1000 ;
134+ auto yaw = std::round (gt_object.yaw * 180 / M_PI * 1000 ) / 1000 ;
135+
136+ 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 ,
137+ gt_object.width , gt_object.length , gt_object.height , gt_object.is_moving );
138+ }
139+ }
140+
141+ void CsvOutputGTObjects::write_first_line_to_CSV (const std::string &path) {
142+ std::fstream my_file;
143+ my_file.open (path, std::ios::app);
144+ my_file
145+ << " 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"
146+ << std::endl;
147+ my_file.close ();
148+ }
149+
150+ 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) {
151+ std::fstream my_file;
152+ my_file.open (path, std::ios::app);
153+ my_file << timestamp << " , " " , " " , " " , " " , " " , "
154+ << " , " " , " " , " " , " " , "
155+ << (is_moving ? " true" " false"
156+ my_file.close ();
157+ }
0 commit comments