4 #ifndef LIBREALSENSE_RS2_EXPORT_HPP 5 #define LIBREALSENSE_RS2_EXPORT_HPP 21 float length()
const {
return sqrt(x * x + y * y + z * z); }
46 register_simple_option(OPTION_IGNORE_COLOR,
option_range{ 0, 1, 0, 1 });
47 register_simple_option(OPTION_PLY_MESH,
option_range{ 0, 1, 1, 1 });
48 register_simple_option(OPTION_PLY_NORMALS,
option_range{ 0, 1, 0, 1 });
49 register_simple_option(OPTION_PLY_BINARY,
option_range{ 0, 1, 1, 1 });
50 register_simple_option(OPTION_PLY_THRESHOLD,
option_range{ 0, 1, 0.05f, 0 });
67 if (!depth)
throw std::runtime_error(
"Need depth data to save PLY");
69 if (color) _pc.map_to(color);
70 depth = _pc.calculate(depth);
78 const bool use_texcoords = color && !get_option(OPTION_IGNORE_COLOR);
79 bool mesh = get_option(OPTION_PLY_MESH);
80 bool binary = get_option(OPTION_PLY_BINARY);
87 std::vector<rs2::vertex> new_verts;
88 std::vector<vec3d> normals;
89 std::vector<std::array<uint8_t, 3>> new_tex;
90 std::map<size_t, size_t> idx_map;
91 std::map<size_t, std::vector<vec3d>> index_to_normals;
93 new_verts.reserve(p.
size());
94 if (use_texcoords) new_tex.reserve(p.
size());
98 for (
size_t i = 0;
i < p.
size(); ++
i) {
102 idx_map[int(
i)] = int(new_verts.size());
107 new_tex.push_back(rgb);
114 static const auto threshold = get_option(OPTION_PLY_THRESHOLD);
115 std::vector<std::array<size_t, 3>> faces;
118 for (
size_t x = 0;
x < width - 1; ++
x) {
119 for (
size_t y = 0;
y <
height - 1; ++
y) {
120 auto a =
y * width +
x,
b =
y * width + x + 1,
c = (
y + 1)*width + x,
d = (
y + 1)*width + x + 1;
125 if (idx_map.count(
a) == 0 || idx_map.count(b) == 0 || idx_map.count(c) == 0 ||
126 idx_map.count(
d) == 0)
128 faces.push_back({ idx_map[
a], idx_map[
d], idx_map[
b] });
129 faces.push_back({ idx_map[
d], idx_map[
a], idx_map[
c] });
138 auto n1 =
cross(point_d - point_a, point_b - point_a);
139 auto n2 =
cross(point_c - point_a, point_d - point_a);
141 index_to_normals[idx_map[
a]].push_back(n1);
142 index_to_normals[idx_map[
a]].push_back(n2);
144 index_to_normals[idx_map[
b]].push_back(n1);
146 index_to_normals[idx_map[
c]].push_back(n2);
148 index_to_normals[idx_map[
d]].push_back(n1);
149 index_to_normals[idx_map[
d]].push_back(n2);
156 if (mesh && use_normals)
158 for (
size_t i = 0;
i < new_verts.size(); ++
i)
160 auto normals_vec = index_to_normals[
i];
161 vec3d sum = { 0, 0, 0 };
162 for (
auto&
n : normals_vec)
164 if (normals_vec.size() > 0)
167 normals.push_back({ 0, 0, 0 });
174 out <<
"format binary_little_endian 1.0\n";
176 out <<
"format ascii 1.0\n";
177 out <<
"comment pointcloud saved from Realsense Viewer\n";
178 out <<
"element vertex " << new_verts.size() <<
"\n";
179 out <<
"property float" <<
sizeof(float) * 8 <<
" x\n";
180 out <<
"property float" <<
sizeof(float) * 8 <<
" y\n";
181 out <<
"property float" <<
sizeof(float) * 8 <<
" z\n";
182 if (mesh && use_normals)
184 out <<
"property float" <<
sizeof(float) * 8 <<
" nx\n";
185 out <<
"property float" <<
sizeof(float) * 8 <<
" ny\n";
186 out <<
"property float" <<
sizeof(float) * 8 <<
" nz\n";
190 out <<
"property uchar red\n";
191 out <<
"property uchar green\n";
192 out <<
"property uchar blue\n";
196 out <<
"element face " << faces.size() <<
"\n";
197 out <<
"property list uchar int vertex_indices\n";
199 out <<
"end_header\n";
205 for (
size_t i = 0;
i < new_verts.size(); ++
i)
208 out.write(reinterpret_cast<const char*>(&(new_verts[
i].
x)),
sizeof(
float));
209 out.write(reinterpret_cast<const char*>(&(new_verts[
i].
y)),
sizeof(
float));
210 out.write(reinterpret_cast<const char*>(&(new_verts[
i].
z)),
sizeof(
float));
212 if (mesh && use_normals)
214 out.write(reinterpret_cast<const char*>(&(normals[
i].x)),
sizeof(
float));
215 out.write(reinterpret_cast<const char*>(&(normals[
i].y)),
sizeof(
float));
216 out.write(reinterpret_cast<const char*>(&(normals[
i].z)),
sizeof(
float));
221 out.write(reinterpret_cast<const char*>(&(new_tex[
i][0])),
sizeof(
uint8_t));
222 out.write(reinterpret_cast<const char*>(&(new_tex[i][1])),
sizeof(
uint8_t));
223 out.write(reinterpret_cast<const char*>(&(new_tex[i][2])),
sizeof(
uint8_t));
228 auto size = faces.size();
229 for (
size_t i = 0;
i <
size; ++
i) {
230 static const int three = 3;
231 out.write(reinterpret_cast<const char*>(&three),
sizeof(
uint8_t));
232 out.write(reinterpret_cast<const char*>(&(faces[
i][0])),
sizeof(
int));
233 out.write(reinterpret_cast<const char*>(&(faces[i][1])),
sizeof(
int));
234 out.write(reinterpret_cast<const char*>(&(faces[i][2])),
sizeof(
int));
240 for (
size_t i = 0;
i <new_verts.size(); ++
i)
242 out << new_verts[
i].x <<
" ";
243 out << new_verts[
i].y <<
" ";
244 out << new_verts[
i].z <<
" ";
247 if (mesh && use_normals)
249 out << normals[
i].x <<
" ";
250 out << normals[
i].y <<
" ";
251 out << normals[
i].z <<
" ";
257 out << unsigned(new_tex[
i][0]) <<
" ";
258 out << unsigned(new_tex[i][1]) <<
" ";
259 out << unsigned(new_tex[i][2]) <<
" ";
265 auto size = faces.size();
266 for (
size_t i = 0;
i <
size; ++
i) {
269 out << std::get<0>(faces[
i]) <<
" ";
270 out << std::get<1>(faces[
i]) <<
" ";
271 out << std::get<2>(faces[
i]) <<
" ";
281 int x =
std::min(std::max(
int(u*w + .5
f), 0), w - 1);
284 return { texture_data[
idx], texture_data[idx + 1], texture_data[idx + 2] };
302 std::vector<std::tuple<software_sensor, stream_profile, int>> sensors;
303 std::vector<std::tuple<stream_profile, stream_profile>>
extrinsics;
306 for (
int i = 0;
size_t(
i) < fs.size(); ++
i) {
309 std::stringstream sname;
310 sname <<
"Sensor (" <<
i <<
")";
316 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() };
317 software_profile =
s.add_video_stream(
stream);
324 rs2_motion_stream stream{ mp.stream_type(), mp.stream_index(),
i, mp.fps(), mp.format(), mp.get_motion_intrinsics() };
325 software_profile =
s.add_motion_stream(
stream);
328 software_profile =
s.add_pose_stream(
stream);
333 sensors.emplace_back(
s, software_profile, i);
335 bool found_extrin =
false;
336 for (
auto&
root : extrinsics) {
338 std::get<0>(
root).register_extrinsics_to(software_profile,
346 extrinsics.emplace_back(software_profile,
profile);
352 std::stringstream
name;
356 for (
auto group : sensors) {
357 auto s = std::get<0>(
group);
363 s.on_video_frame({
const_cast<void*
>(vf.get_data()), [](
void*) {}, vf.get_stride_in_bytes(), vf.get_bytes_per_pixel(),
364 vf.get_timestamp(), vf.get_frame_timestamp_domain(),
static_cast<int>(vf.get_frame_number()), profile });
378 save(
set, source,
false);
save_to_ply(std::string filename="RealSense Pointcloud ", pointcloud pc=pointcloud())
float3 cross(const float3 &a, const float3 &b)
GLboolean GLboolean GLboolean b
::realsense_legacy_msgs::extrinsics_< std::allocator< void > > extrinsics
GLuint const GLchar * name
rs2_option
Defines general configuration controls. These can generally be mapped to camera UVC controls...
void export_to_ply(Mat points, Mat normals)
save_single_frameset(std::string filename="RealSense Frameset ")
rs2_extrinsics get_extrinsics_to(const stream_profile &to) const
stream_profile get_profile() const
All the parameters required to define a video stream.
int get_bytes_per_pixel() const
GLint GLint GLsizei GLsizei GLsizei depth
const void * get_data() const
GLdouble GLdouble GLdouble w
GLsizei const GLchar *const * string
std::tuple< uint8_t, uint8_t, uint8_t > get_texcolor(video_frame texture, texture_coordinate texcoords)
float3 operator+(const float3 &a, const float3 &b)
software_sensor add_sensor(std::string name)
GLfloat GLfloat GLfloat GLfloat h
const texture_coordinate * get_texture_coordinates() const
void frame_ready(frame result) const
GLboolean GLboolean GLboolean GLboolean a
void func(frame data, frame_source &source)
All the parameters required to define a pose stream.
double get_timestamp() const
void save(frame data, frame_source &source, bool do_signal=true)
float3 operator-(const float3 &a, const float3 &b)
def pointcloud(out, verts, texcoords, color, painter=True)
GLint GLsizei GLsizei height
frame allocate_composite_frame(std::vector< frame > frames) const
const vertex * get_vertices() const
static const char * use_normals
All the parameters required to define a motion stream.
std::array< uint8_t, 3 > get_texcolor(const video_frame &texture, const uint8_t *texture_data, float u, float v)
rs2_timestamp_domain get_frame_timestamp_domain() const
const GLuint GLenum const void * binary
typename::boost::move_detail::remove_reference< T >::type && move(T &&t) BOOST_NOEXCEPT
GLsizei GLsizei GLchar * source
std::array< float, 3 > color
unsigned long long get_frame_number() const
int get_stride_in_bytes() const
std::shared_ptr< sensor > sensor_from_frame(frame f)
void export_to_ply(points p, video_frame color)