converter-csv.cpp
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2021 Intel Corporation. All Rights Reserved.
3 
4 
5 #include "converter-csv.hpp"
6 #include <iomanip>
7 #include <algorithm>
8 #include <iostream>
9 
10 using namespace rs2::tools::converter;
11 
13  unsigned long long frame_number,
14  long long frame_ts, long long backend_ts, long long arrival_time,
15  double p1, double p2, double p3,
16  double p4, double p5, double p6, double p7) :
17  _stream_type(stream_type),
18  _stream_idx(stream_index),
19  _frame_number(frame_number),
20  _frame_ts(frame_ts),
21  _backend_ts(backend_ts),
22  _arrival_time(arrival_time),
23  _params({ p1, p2, p3, p4, p5, p6, p7 }){}
24 
26 {
27  std::stringstream ss;
28  ss << rs2_stream_to_string(_stream_type) << "," << _frame_number << ","
29  << std::fixed << std::setprecision(3) << _frame_ts << "," << _backend_ts << "," << _arrival_time;
30 
31  // IMU and Pose frame hold the sample data in addition to the frame's header attributes
32  size_t specific_attributes = 0;
33  if (val_in_range(_stream_type, { RS2_STREAM_GYRO,RS2_STREAM_ACCEL }))
34  specific_attributes = 3;
35  if (val_in_range(_stream_type, { RS2_STREAM_POSE }))
36  specific_attributes = 7;
37 
38  for (auto i = 0; i < specific_attributes; i++)
39  ss << "," << _params[i];
40  ss << std::endl;
41  return ss.str().c_str();
42 }
43 
45  : _filePath(filePath)
46  , _streamType(streamType)
48  , _sub_workers_joined(false)
49  , _m()
50  , _cv()
51 {
52 }
53 
55 {
56  if (frames_map_get_and_set(depthframe.get_profile().stream_type(), depthframe.get_frame_number())) {
57  return;
58  }
59 
61  [this, &depthframe] {
62 
63  std::stringstream filename;
65  << "_" << depthframe.get_profile().stream_name()
66  << "_" << std::setprecision(14) << std::fixed << depthframe.get_timestamp()
67  << ".csv";
68 
69  std::stringstream metadata_file;
70  metadata_file << _filePath
71  << "_" << depthframe.get_profile().stream_name()
72  << "_metadata_" << std::setprecision(14) << std::fixed << depthframe.get_timestamp()
73  << ".txt";
74 
75  std::string filenameS = filename.str();
76  std::string metadataS = metadata_file.str();
77 
79  [filenameS, metadataS, depthframe] {
80  std::ofstream fs(filenameS, std::ios::trunc);
81 
82  if (fs) {
83  for (int y = 0; y < depthframe.get_height(); y++) {
84  auto delim = "";
85 
86  for (int x = 0; x < depthframe.get_width(); x++) {
87  fs << delim << depthframe.get_distance(x, y);
88  delim = ",";
89  }
90  fs << '\n';
91  }
92  fs.flush();
93  }
94  metadata_to_txtfile(depthframe, metadataS);
95  });
97  });
98 }
99 
101 {
102  auto t = time(nullptr);
103  char buffer[20] = {};
104  const tm* time = localtime(&t);
105  if (nullptr != time)
106  strftime(buffer, sizeof(buffer), "%Y%m%d%H%M%S", time);
107 
108  return std::string(buffer);
109 }
110 
112 {
113  if (!_imu_pose_collection.size())
114  {
115  std::cout << "No imu or pose data collected" << std::endl;
116  return;
117  }
118 
119  // Report amount of frames collected
120  std::vector<uint64_t> frames_per_stream;
121  for (const auto& kv : _imu_pose_collection)
122  frames_per_stream.emplace_back(kv.second.size());
123 
124  std::sort(frames_per_stream.begin(), frames_per_stream.end());
125 
126  // Serialize and store data into csv-like format
127  static auto time_Str = get_time_string();
128  std::stringstream filename;
129  filename << _filePath << "_" << time_Str <<"_imu_pose.csv";
130  std::ofstream csv(filename.str());
131  if (!csv.is_open())
132  throw std::runtime_error(stringify() << "Cannot open the requested output file " << _filePath << ", please check permissions");
133 
134  for (const auto& elem : _imu_pose_collection)
135  {
136  csv << "\n\nStream Type,F#,HW Timestamp (ms),Backend Timestamp(ms),Host Timestamp(ms)"
137  << (val_in_range(elem.first.first, { RS2_STREAM_GYRO,RS2_STREAM_ACCEL }) ? ",3DOF_x,3DOF_y,3DOF_z" : "")
138  << (val_in_range(elem.first.first, { RS2_STREAM_POSE }) ? ",t_x,t_y,t_z,r_x,r_y,r_z,r_w" : "")
139  << std::endl;
140 
141  for (auto i = 0; i < elem.second.size(); i++)
142  csv << elem.second[i].to_string();
143  }
144 
145 }
146 
148 {
149  if (frames_map_get_and_set(f.get_profile().stream_type(), f.get_frame_number())) {
150  return;
151  }
152 
153  start_worker(
154  [this, f] {
155 
157  [this, f] {
158  auto stream_uid = std::make_pair(f.get_profile().stream_type(),
159  f.get_profile().stream_index());
160 
161  long long frame_timestamp = 0LL;
162  if (f.supports_frame_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP))
164 
165  long long backend_timestamp = 0LL;
166  if (f.supports_frame_metadata(RS2_FRAME_METADATA_BACKEND_TIMESTAMP))
167  backend_timestamp = f.get_frame_metadata(RS2_FRAME_METADATA_BACKEND_TIMESTAMP);
168 
169  long long time_of_arrival = 0LL;
170  if (f.supports_frame_metadata(RS2_FRAME_METADATA_TIME_OF_ARRIVAL))
172 
173  motion_pose_frame_record record{ f.get_profile().stream_type(),
174  f.get_profile().stream_index(),
175  f.get_frame_number(),
177  backend_timestamp,
179 
180  if (auto motion = f.as<rs2::motion_frame>())
181  {
182  auto axes = motion.get_motion_data();
183  record._params = { axes.x, axes.y, axes.z };
184  }
185 
186  if (auto pf = f.as<rs2::pose_frame>())
187  {
188  auto pose = pf.get_pose_data();
189  record._params = { pose.translation.x, pose.translation.y, pose.translation.z,
190  pose.rotation.x,pose.rotation.y,pose.rotation.z,pose.rotation.w };
191  }
192 
193  _imu_pose_collection[stream_uid].emplace_back(record);
194  });
196  _sub_workers_joined = true;
197  _cv.notify_all();
198  });
199  std::unique_lock<std::mutex> lck(_m);
200  while (!_sub_workers_joined)
201  _cv.wait(lck);
203 }
204 
206 {
208  return;
209 
210  auto depthframe = frame.as<rs2::depth_frame>();
211  if (depthframe)
212  {
213  convert_depth(depthframe);
214  return;
215  }
216 
217  auto motionframe = frame.as<rs2::motion_frame>();
218  auto poseframe = frame.as<rs2::pose_frame>();
219  if (motionframe || poseframe)
220  {
222  return;
223  }
224 }
rs2::tools::converter::val_in_range
bool val_in_range(const T &val, const std::initializer_list< T > &list)
Definition: converter-csv.hpp:27
example1 - object detection.p1
tuple p1
Definition: example1 - object detection.py:75
RS2_FRAME_METADATA_FRAME_TIMESTAMP
@ RS2_FRAME_METADATA_FRAME_TIMESTAMP
Definition: rs_frame.h:32
rs2::tools::converter::converter_csv::_streamType
rs2_stream _streamType
Definition: converter-csv.hpp:58
RS2_STREAM_POSE
@ RS2_STREAM_POSE
Definition: rs_sensor.h:53
rs2::frame
Definition: rs_frame.hpp:345
rs2::tools::converter::converter_csv::converter_csv
converter_csv(const std::string &filePath, rs2_stream streamType=rs2_stream::RS2_STREAM_ANY)
Definition: converter-csv.cpp:44
rs2::depth_frame::get_distance
float get_distance(int x, int y) const
Definition: rs_frame.hpp:837
rs2::tools::converter::converter_csv::_sub_workers_joined
bool _sub_workers_joined
Definition: converter-csv.hpp:61
test-hdr-long.frame_ts
frame_ts
Definition: test-hdr-long.py:350
realsense_legacy_msgs::pose
::realsense_legacy_msgs::pose_< std::allocator< void > > pose
Definition: third-party/realsense-file/rosbag/msgs/realsense_legacy_msgs/pose.h:88
rs2::tools::converter::converter_csv::motion_pose_frame_record
Definition: converter-csv.hpp:39
rs2_stream_to_string
const char * rs2_stream_to_string(rs2_stream stream)
Definition: to-string.cpp:766
rs2::frame::get_frame_number
unsigned long long get_frame_number() const
Definition: rs_frame.hpp:523
string
GLsizei const GLchar *const * string
Definition: glad/glad/glad.h:2861
rs2::tools::converter::converter_csv::_cv
std::condition_variable _cv
Definition: converter-csv.hpp:63
arrival_time
@ arrival_time
Definition: rs-rosbag-inspector.cpp:347
realdds::topics::notification::stream_options::key::stream_name
const std::string stream_name
rs2::stream_profile::stream_name
std::string stream_name() const
Definition: rs_frame.hpp:113
rs2::stream_profile::stream_type
rs2_stream stream_type() const
Definition: rs_frame.hpp:39
rs2::tools::converter::converter_csv::convert_depth
void convert_depth(rs2::depth_frame &depthframe)
Definition: converter-csv.cpp:54
rs2::tools::converter::converter_base::wait_sub_workers
void wait_sub_workers()
Definition: converter.cpp:47
librealsense::legacy_file_format::time_of_arrival
constexpr uint32_t time_of_arrival
Definition: ros_file_format.h:646
rs2::tools::converter::converter_base::add_sub_worker
void add_sub_worker(const F &f)
Definition: converter.hpp:40
converter-csv.hpp
rs2::tools::converter::converter_csv::motion_pose_frame_record::motion_pose_frame_record
motion_pose_frame_record(rs2_stream stream_type, int stream_index, unsigned long long frame_number, long long frame_ts, long long backend_ts, long long arrival_time, double p1=0., double p2=0., double p3=0., double p4=0., double p5=0., double p6=0., double p7=0.)
Definition: converter-csv.cpp:12
rs2::frame::get_timestamp
double get_timestamp() const
Definition: rs_frame.hpp:476
rs2::tools::converter
Definition: converter.hpp:19
RS2_STREAM_GYRO
@ RS2_STREAM_GYRO
Definition: rs_sensor.h:50
buffer
GLenum GLfloat * buffer
Definition: glad/glad/glad.h:2066
test-motion.motion
motion
Definition: sw-dev/test-motion.py:46
RS2_FRAME_METADATA_BACKEND_TIMESTAMP
@ RS2_FRAME_METADATA_BACKEND_TIMESTAMP
Definition: rs_frame.h:41
f
GLdouble f
Definition: glad/glad/glad.h:1517
i
int i
Definition: rs-pcl-color.cpp:54
rs2::tools::converter::converter_csv::_filePath
std::string _filePath
Definition: converter-csv.hpp:59
opencv_pointcloud_viewer.axes
def axes(out, pos, rotation=np.eye(3), size=0.075, thickness=2)
Definition: opencv_pointcloud_viewer.py:202
rs2::tools::converter::converter_base::frames_map_get_and_set
bool frames_map_get_and_set(rs2_stream streamType, frame_number_t frameNumber)
Definition: converter.cpp:31
rs2::tools::converter::converter_csv::motion_pose_frame_record::to_string
std::string to_string() const
Definition: converter-csv.cpp:25
rs2::tools::converter::converter_csv::save_motion_pose_data_to_file
void save_motion_pose_data_to_file()
Definition: converter-csv.cpp:111
rs2::video_frame::get_width
int get_width() const
Definition: rs_frame.hpp:661
librealsense::legacy_file_format::frame_timestamp
constexpr uint32_t frame_timestamp
Definition: ros_file_format.h:641
test-shorten-json.ss
string ss
Definition: test-shorten-json.py:119
RS2_STREAM_ANY
@ RS2_STREAM_ANY
Definition: rs_sensor.h:45
rs2::tools::converter::converter_csv::_m
std::mutex _m
Definition: converter-csv.hpp:62
rs2::pose_frame
Definition: rs_frame.hpp:916
test-projection-from-recording.filename
filename
Definition: test-projection-from-recording.py:15
rs2::tools::converter::metadata_to_txtfile
void metadata_to_txtfile(const rs2::frame &frm, const std::string &filename)
Definition: converter.cpp:9
RS2_STREAM_ACCEL
@ RS2_STREAM_ACCEL
Definition: rs_sensor.h:51
t
GLdouble t
Definition: glad/glad/glad.h:1829
rs2::depth_frame
Definition: rs_frame.hpp:813
realdds::topics::metadata::header::key::frame_number
const std::string frame_number
Definition: sw-dev/test-motion.py:41
rs2::frame::as
T as() const
Definition: rs_frame.hpp:582
rs2::tools::converter::converter_base::start_worker
void start_worker(const F &f)
Definition: converter.hpp:35
sort
void sort(sort_type m_sort_type, const std::string &in, const std::string &out)
Definition: rs-rosbag-inspector.cpp:350
rs2::frame::get_profile
stream_profile get_profile() const
Definition: rs_frame.hpp:559
example1 - object detection.p2
tuple p2
Definition: example1 - object detection.py:76
RS2_FRAME_METADATA_TIME_OF_ARRIVAL
@ RS2_FRAME_METADATA_TIME_OF_ARRIVAL
Definition: rs_frame.h:39
rs2::tools::converter::converter_csv::get_time_string
std::string get_time_string() const
Definition: converter-csv.cpp:100
x
GLdouble x
Definition: glad/glad/glad.h:2279
rs2::tools::converter::converter_csv::convert
void convert(rs2::frame &frame) override
Definition: converter-csv.cpp:205
test-ts-same-fps.fs
fs
Definition: test-ts-same-fps.py:57
rs2::motion_frame
Definition: rs_frame.hpp:888
test-record-and-stream.record
def record(file_name, default_profile)
Definition: test-record-and-stream.py:28
rs2_stream
rs2_stream
Streams are different types of data provided by RealSense devices.
Definition: rs_sensor.h:43
rs2::tools::converter::converter_csv::_imu_pose_collection
std::map< std::pair< rs2_stream, int >, std::vector< motion_pose_frame_record > > _imu_pose_collection
Definition: converter-csv.hpp:60
rs2::video_frame::get_height
int get_height() const
Definition: rs_frame.hpp:673
rs2::tools::converter::stringify
Definition: converter-csv.hpp:19
y
GLint y
Definition: glad/glad/glad.h:1397
rs2::tools::converter::converter_csv::convert_motion_pose
void convert_motion_pose(rs2::frame &f)
Definition: converter-csv.cpp:147


librealsense2
Author(s): LibRealSense ROS Team
autogenerated on Fri Aug 2 2024 08:30:01