Intel® RealSense™ Cross Platform API
Intel Realsense Cross-platform API
rs_export.hpp
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2017 Intel Corporation. All Rights Reserved.
3 
4 #ifndef LIBREALSENSE_RS2_EXPORT_HPP
5 #define LIBREALSENSE_RS2_EXPORT_HPP
6 
7 #include <map>
8 #include <fstream>
9 #include <cmath>
10 #include <sstream>
11 #include <cassert>
12 #include "rs_processing.hpp"
13 #include "rs_internal.hpp"
14 
15 namespace rs2
16 {
17  class save_to_ply : public filter
18  {
19  public:
20  save_to_ply(std::string filename = "RealSense Pointcloud ", pointcloud pc = pointcloud())
21  : filter([this](frame f, frame_source& s) { func(f, s); }),
22  _pc(std::move(pc)), fname(filename)
23  {
25  }
26 
27 
29  private:
30  void func(frame data, frame_source& source)
31  {
32  frame depth, color;
33  if (auto fs = data.as<frameset>()) {
34  for (auto f : fs) {
35  if (f.is<points>()) depth = f;
36  else if (!depth && f.is<depth_frame>()) depth = f;
37  else if (!color && f.is<video_frame>()) color = f;
38  }
39  } else if (data.is<depth_frame>() || data.is<points>()) {
40  depth = data;
41  }
42 
43  if (!depth) throw std::runtime_error("Need depth data to save PLY");
44  if (!depth.is<points>()) {
45  if (color) _pc.map_to(color);
46  depth = _pc.calculate(depth);
47  }
48 
49  export_to_ply(depth, color);
50  source.frame_ready(data); // passthrough filter because processing_block::process doesn't support sinks
51  }
52 
53  void export_to_ply(points p, video_frame color) {
54  const bool use_texcoords = color && !get_option(OPTION_IGNORE_COLOR);
55  const auto verts = p.get_vertices();
56  const auto texcoords = p.get_texture_coordinates();
57  std::vector<rs2::vertex> new_verts;
58  std::vector<std::array<uint8_t, 3>> new_tex;
59  std::map<int, int> idx_map;
60 
61  new_verts.reserve(p.size());
62  if (use_texcoords) new_tex.reserve(p.size());
63 
64  static const auto min_distance = 1e-6;
65 
66  for (size_t i = 0; i < p.size(); ++i) {
67  if (fabs(verts[i].x) >= min_distance || fabs(verts[i].y) >= min_distance ||
68  fabs(verts[i].z) >= min_distance)
69  {
70  idx_map[i] = new_verts.size();
71  new_verts.push_back(verts[i]);
72  if (use_texcoords)
73  {
74  auto rgb = get_texcolor(color, texcoords[i].u, texcoords[i].v);
75  new_tex.push_back(rgb);
76  }
77  }
78  }
79 
80  auto profile = p.get_profile().as<video_stream_profile>();
81  auto width = profile.width(), height = profile.height();
82  static const auto threshold = 0.05f;
83  std::vector<std::array<int, 3>> faces;
84  for (int x = 0; x < width - 1; ++x) {
85  for (int y = 0; y < height - 1; ++y) {
86  auto a = y * width + x, b = y * width + x + 1, c = (y + 1)*width + x, d = (y + 1)*width + x + 1;
87  if (verts[a].z && verts[b].z && verts[c].z && verts[d].z
88  && fabs(verts[a].z - verts[b].z) < threshold && fabs(verts[a].z - verts[c].z) < threshold
89  && fabs(verts[b].z - verts[d].z) < threshold && fabs(verts[c].z - verts[d].z) < threshold)
90  {
91  if (idx_map.count(a) == 0 || idx_map.count(b) == 0 || idx_map.count(c) == 0 ||
92  idx_map.count(d) == 0)
93  continue;
94  faces.push_back({ idx_map[a], idx_map[b], idx_map[d] });
95  faces.push_back({ idx_map[d], idx_map[c], idx_map[a] });
96  }
97  }
98  }
99 
100  std::stringstream name;
101  name << fname << p.get_frame_number() << ".ply";
102  std::ofstream out(name.str());
103  out << "ply\n";
104  out << "format binary_little_endian 1.0\n";
105  out << "comment pointcloud saved from Realsense Viewer\n";
106  out << "element vertex " << new_verts.size() << "\n";
107  out << "property float" << sizeof(float) * 8 << " x\n";
108  out << "property float" << sizeof(float) * 8 << " y\n";
109  out << "property float" << sizeof(float) * 8 << " z\n";
110  if (use_texcoords)
111  {
112  out << "property uchar red\n";
113  out << "property uchar green\n";
114  out << "property uchar blue\n";
115  }
116  out << "element face " << faces.size() << "\n";
117  out << "property list uchar int vertex_indices\n";
118  out << "end_header\n";
119  out.close();
120 
121  out.open(name.str(), std::ios_base::app | std::ios_base::binary);
122  for (int i = 0; i < new_verts.size(); ++i)
123  {
124  // we assume little endian architecture on your device
125  out.write(reinterpret_cast<const char*>(&(new_verts[i].x)), sizeof(float));
126  out.write(reinterpret_cast<const char*>(&(new_verts[i].y)), sizeof(float));
127  out.write(reinterpret_cast<const char*>(&(new_verts[i].z)), sizeof(float));
128 
129  if (use_texcoords)
130  {
131  out.write(reinterpret_cast<const char*>(&(new_tex[i][0])), sizeof(uint8_t));
132  out.write(reinterpret_cast<const char*>(&(new_tex[i][1])), sizeof(uint8_t));
133  out.write(reinterpret_cast<const char*>(&(new_tex[i][2])), sizeof(uint8_t));
134  }
135  }
136  auto size = faces.size();
137  for (int i = 0; i < size; ++i) {
138  static const int three = 3;
139  out.write(reinterpret_cast<const char*>(&three), sizeof(uint8_t));
140  out.write(reinterpret_cast<const char*>(&(faces[i][0])), sizeof(int));
141  out.write(reinterpret_cast<const char*>(&(faces[i][1])), sizeof(int));
142  out.write(reinterpret_cast<const char*>(&(faces[i][2])), sizeof(int));
143  }
144  }
145 
146  // TODO: get_texcolor, options API
147  std::array<uint8_t, 3> get_texcolor(const video_frame& texture, float u, float v)
148  {
149  const int w = texture.get_width(), h = texture.get_height();
150  int x = std::min(std::max(int(u*w + .5f), 0), w - 1);
151  int y = std::min(std::max(int(v*h + .5f), 0), h - 1);
152  int idx = x * texture.get_bytes_per_pixel() + y * texture.get_stride_in_bytes();
153  const auto texture_data = reinterpret_cast<const uint8_t*>(texture.get_data());
154  return { texture_data[idx], texture_data[idx + 1], texture_data[idx + 2] };
155  }
156 
157  std::string fname;
158  pointcloud _pc;
159  };
160 
161  class save_single_frameset : public filter {
162  public:
163  save_single_frameset(std::string filename = "RealSense Frameset ")
164  : filter([this](frame f, frame_source& s) { save(f, s); }), fname(filename)
165  {}
166 
167  private:
168  void save(frame data, frame_source& source, bool do_signal=true)
169  {
170  software_device dev;
171 
172  std::vector<std::tuple<software_sensor, stream_profile, int>> sensors;
173  std::vector<std::tuple<stream_profile, stream_profile>> extrinsics;
174 
175  if (auto fs = data.as<frameset>()) {
176  for (int i = 0; i < fs.size(); ++i) {
177  frame f = fs[i];
178  auto profile = f.get_profile();
179  std::stringstream sname;
180  sname << "Sensor (" << i << ")";
181  auto s = dev.add_sensor(sname.str());
182  stream_profile software_profile;
183 
184  if (auto vf = f.as<video_frame>()) {
185  auto vp = profile.as<video_stream_profile>();
186  rs2_video_stream stream{ vp.stream_type(), vp.stream_index(), i, vp.width(), vp.height(), vp.fps(), vf.get_bytes_per_pixel(), vp.format(), vp.get_intrinsics() };
187  software_profile = s.add_video_stream(stream);
188  if (f.is<rs2::depth_frame>()) {
189  auto ds = sensor_from_frame(f)->as<rs2::depth_sensor>();
190  s.add_read_only_option(RS2_OPTION_DEPTH_UNITS, ds.get_option(RS2_OPTION_DEPTH_UNITS));
191  }
192  } else if (f.is<motion_frame>()) {
193  auto mp = profile.as<motion_stream_profile>();
194  rs2_motion_stream stream{ mp.stream_type(), mp.stream_index(), i, mp.fps(), mp.format(), mp.get_motion_intrinsics() };
195  software_profile = s.add_motion_stream(stream);
196  } else if (f.is<pose_frame>()) {
197  rs2_pose_stream stream{ profile.stream_type(), profile.stream_index(), i, profile.fps(), profile.format() };
198  software_profile = s.add_pose_stream(stream);
199  } else {
200  // TODO: How to handle other frame types? (e.g. points)
201  assert(false);
202  }
203  sensors.emplace_back(s, software_profile, i);
204 
205  bool found_extrin = false;
206  for (auto& root : extrinsics) {
207  try {
208  std::get<0>(root).register_extrinsics_to(software_profile,
209  std::get<1>(root).get_extrinsics_to(profile)
210  );
211  found_extrin = true;
212  break;
213  } catch (...) {}
214  }
215  if (!found_extrin) {
216  extrinsics.emplace_back(software_profile, profile);
217  }
218  }
219 
220 
221 
222  // Recorder needs sensors to already exist when its created
223  std::stringstream name;
224  name << fname << data.get_frame_number() << ".bag";
225  recorder rec(name.str(), dev);
226 
227  for (auto group : sensors) {
228  auto s = std::get<0>(group);
229  auto profile = std::get<1>(group);
230  s.open(profile);
231  s.start([](frame) {});
232  frame f = fs[std::get<2>(group)];
233  if (auto vf = f.as<video_frame>()) {
234  s.on_video_frame({ const_cast<void*>(vf.get_data()), [](void*) {}, vf.get_stride_in_bytes(), vf.get_bytes_per_pixel(),
235  vf.get_timestamp(), vf.get_frame_timestamp_domain(), static_cast<int>(vf.get_frame_number()), profile });
236  } else if (f.is<motion_frame>()) {
237  s.on_motion_frame({ const_cast<void*>(f.get_data()), [](void*) {}, f.get_timestamp(),
238  f.get_frame_timestamp_domain(), static_cast<int>(f.get_frame_number()), profile });
239  } else if (f.is<pose_frame>()) {
240  s.on_pose_frame({ const_cast<void*>(f.get_data()), [](void*) {}, f.get_timestamp(),
241  f.get_frame_timestamp_domain(), static_cast<int>(f.get_frame_number()), profile });
242  }
243  s.stop();
244  s.close();
245  }
246  } else {
247  // single frame
248  auto set = source.allocate_composite_frame({ data });
249  save(set, source, false);
250  }
251 
252  if (do_signal)
253  source.frame_ready(data);
254  }
255 
256  std::string fname;
257  };
258 }
259 
260 #endif
save_to_ply(std::string filename="RealSense Pointcloud ", pointcloud pc=pointcloud())
Definition: rs_export.hpp:20
Definition: rs_option.h:84
Definition: rs_frame.hpp:629
Definition: rs_frame.hpp:336
Definition: rs_export.hpp:17
points calculate(frame depth)
Definition: rs_processing.hpp:412
rs2_option
Defines general configuration controls. These can generally be mapped to camera UVC controls,...
Definition: rs_option.h:22
int fps
Definition: rs_internal.h:64
Definition: rs_sensor.hpp:381
Definition: rs_frame.hpp:712
save_single_frameset(std::string filename="RealSense Frameset ")
Definition: rs_export.hpp:163
void register_simple_option(rs2_option option_id, option_range range)
Definition: rs_processing.hpp:312
All the parameters are required to define video stream.
Definition: rs_internal.h:34
Definition: rs_frame.hpp:908
T as() const
Definition: rs_processing.hpp:379
void map_to(frame mapped)
Definition: rs_processing.hpp:433
Definition: rs_context.hpp:11
bool is() const
Definition: rs_frame.hpp:563
Definition: rs_option.h:52
void frame_ready(frame result) const
Definition: rs_processing.hpp:80
float get_option(rs2_option option) const
Definition: rs_options.hpp:72
Definition: rs_processing.hpp:18
All the parameters are required to define pose stream.
Definition: rs_internal.h:59
Definition: rs_types.hpp:160
T as() const
Definition: rs_frame.hpp:573
int width
Definition: rs_internal.h:39
All the parameters are required to define motion stream.
Definition: rs_internal.h:48
T as() const
Definition: rs_sensor.hpp:308
Definition: rs_processing.hpp:393
Definition: rs_processing.hpp:324
static const auto OPTION_IGNORE_COLOR
Definition: rs_export.hpp:28
int fps
Definition: rs_internal.h:53
Definition: rs_export.hpp:161
std::shared_ptr< sensor > sensor_from_frame(frame f)
Definition: rs_sensor.hpp:332
Definition: rs_frame.hpp:786