4 #ifndef LIBREALSENSE_RS2_EXPORT_HPP 5 #define LIBREALSENSE_RS2_EXPORT_HPP 22 _pc(std::move(pc)), fname(filename)
35 if (f.is<
points>()) depth = f;
39 }
else if (data.
is<depth_frame>() || data.
is<points>()) {
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);
49 export_to_ply(depth, color);
53 void export_to_ply(points p, video_frame 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;
61 new_verts.reserve(p.size());
62 if (use_texcoords) new_tex.reserve(p.size());
64 static const auto min_distance = 1e-6;
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)
70 idx_map[i] = new_verts.size();
71 new_verts.push_back(verts[i]);
74 auto rgb = get_texcolor(color, texcoords[i].u, texcoords[i].v);
75 new_tex.push_back(rgb);
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)
91 if (idx_map.count(a) == 0 || idx_map.count(b) == 0 || idx_map.count(c) == 0 ||
92 idx_map.count(d) == 0)
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] });
100 std::stringstream name;
101 name << fname << p.get_frame_number() <<
".ply";
102 std::ofstream out(name.str());
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";
112 out <<
"property uchar red\n";
113 out <<
"property uchar green\n";
114 out <<
"property uchar blue\n";
116 out <<
"element face " << faces.size() <<
"\n";
117 out <<
"property list uchar int vertex_indices\n";
118 out <<
"end_header\n";
121 out.open(name.str(), std::ios_base::app | std::ios_base::binary);
122 for (
int i = 0; i < new_verts.size(); ++i)
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));
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));
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));
147 std::array<uint8_t, 3> get_texcolor(
const video_frame& texture,
float u,
float v)
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] };
168 void save(frame data, frame_source& source,
bool do_signal=
true)
172 std::vector<std::tuple<software_sensor, stream_profile, int>> sensors;
173 if (
auto fs = data.as<frameset>()) {
175 for (
int i = 0; i < fs.size(); ++i) {
177 auto profile = f.get_profile();
178 std::stringstream sname;
179 sname <<
"Sensor (" << uid <<
")";
180 auto s = dev.add_sensor(sname.str());
181 stream_profile software_profile;
183 if (
auto vf = f.as<video_frame>()) {
184 auto vp = profile.
as<video_stream_profile>();
185 rs2_video_stream stream{ vp.stream_type(), vp.stream_index(), uid++, vp.
width(), vp.height(), vp.fps(), vf.get_bytes_per_pixel(), vp.format(), vp.get_intrinsics() };
186 software_profile = s.add_video_stream(stream);
187 }
else if (f.is<motion_frame>()) {
188 auto mp = profile.as<motion_stream_profile>();
189 rs2_motion_stream stream{ mp.stream_type(), mp.stream_index(), uid++, mp.
fps(), mp.format(), mp.get_motion_intrinsics() };
190 software_profile = s.add_motion_stream(stream);
191 }
else if (f.is<pose_frame>()) {
192 rs2_pose_stream stream{ profile.stream_type(), profile.stream_index(), uid++, profile.
fps(), profile.format() };
193 software_profile = s.add_pose_stream(stream);
198 sensors.emplace_back(s, software_profile, i);
202 std::stringstream name;
203 name << fname << data.get_frame_number() <<
".bag";
204 recorder rec(name.str(), dev);
206 for (
auto group : sensors) {
207 auto s = std::get<0>(group);
208 auto profile = std::get<1>(group);
210 s.start([](frame) {});
211 frame f = fs[std::get<2>(group)];
212 if (
auto vf = f.as<video_frame>()) {
213 s.on_video_frame({
const_cast<void*
>(vf.get_data()), [](
void*) {}, vf.get_stride_in_bytes(), vf.get_bytes_per_pixel(),
214 vf.get_timestamp(), vf.get_frame_timestamp_domain(),
static_cast<int>(vf.get_frame_number()), profile });
215 }
else if (f.is<motion_frame>()) {
216 s.on_motion_frame({
const_cast<void*
>(f.get_data()), [](
void*) {}, f.get_timestamp(),
217 f.get_frame_timestamp_domain(),
static_cast<int>(f.get_frame_number()), profile });
218 }
else if (f.is<pose_frame>()) {
219 s.on_pose_frame({
const_cast<void*
>(f.get_data()), [](
void*) {}, f.get_timestamp(),
220 f.get_frame_timestamp_domain(),
static_cast<int>(f.get_frame_number()), profile });
225 auto set = source.allocate_composite_frame({ data });
226 save(
set, source,
false);
230 source.frame_ready(data);
save_to_ply(std::string filename="RealSense Pointcloud ", pointcloud pc=pointcloud())
Definition: rs_export.hpp:20
Definition: rs_option.h:79
Definition: rs_frame.hpp:578
Definition: rs_frame.hpp:313
Definition: rs_export.hpp:17
points calculate(frame depth)
Definition: rs_processing.hpp:403
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_frame.hpp:661
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:303
All the parameters are required to define video stream.
Definition: rs_internal.h:34
Definition: rs_frame.hpp:857
T as() const
Definition: rs_processing.hpp:370
void map_to(frame mapped)
Definition: rs_processing.hpp:424
Definition: rs_context.hpp:11
bool is() const
Definition: rs_frame.hpp:512
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:153
T as() const
Definition: rs_frame.hpp:522
int width
Definition: rs_internal.h:39
All the parameters are required to define motion stream.
Definition: rs_internal.h:48
Definition: rs_processing.hpp:384
Definition: rs_processing.hpp:315
static const auto OPTION_IGNORE_COLOR
Definition: rs_export.hpp:28
int fps
Definition: rs_internal.h:53
Definition: rs_export.hpp:161
Definition: rs_frame.hpp:735