diff --git a/src/archive.cpp b/src/archive.cpp index 7e23aa34f2..57ee5799d1 100644 --- a/src/archive.cpp +++ b/src/archive.cpp @@ -2,6 +2,7 @@ #include "archive.h" #include #include "core/processing.h" +#include "core/video.h" #define MIN_DISTANCE 1e-6 @@ -39,12 +40,19 @@ namespace librealsense return std::make_tuple(texture_data[idx], texture_data[idx + 1], texture_data[idx + 2]); } + void points::export_to_ply(const std::string& fname, const frame_holder& texture) { + auto stream_profile = get_stream().get(); + auto video_stream_profile = dynamic_cast(stream_profile); + if (!video_stream_profile) + throw librealsense::invalid_value_exception("stream must be video stream"); const auto vertices = get_vertices(); const auto texcoords = get_texture_coordinates(); std::vector new_vertices; std::vector> new_tex; + std::map index2reducedIndex; + new_vertices.reserve(get_vertex_count()); new_tex.reserve(get_vertex_count()); assert(get_vertex_count()); @@ -52,6 +60,7 @@ namespace librealsense if (fabs(vertices[i].x) >= MIN_DISTANCE || fabs(vertices[i].y) >= MIN_DISTANCE || fabs(vertices[i].z) >= MIN_DISTANCE) { + index2reducedIndex[i] = new_vertices.size(); new_vertices.push_back(vertices[i]); if (texture) { @@ -60,6 +69,25 @@ namespace librealsense } } + const auto threshold = 0.05f; + auto width = video_stream_profile->get_width(); + std::vector> faces; + for (int x = 0; x < width - 1; ++x) { + for (int y = 0; y < video_stream_profile->get_height() - 1; ++y) { + auto a = y * width + x, b = y * width + x + 1, c = (y + 1)*width + x, d = (y + 1)*width + x + 1; + if (vertices[a].z && vertices[b].z && vertices[c].z && vertices[d].z + && abs(vertices[a].z - vertices[b].z) < threshold && abs(vertices[a].z - vertices[c].z) < threshold + && abs(vertices[b].z - vertices[d].z) < threshold && abs(vertices[c].z - vertices[d].z) < threshold) + { + if (index2reducedIndex.count(a) == 0 || index2reducedIndex.count(b) == 0 || index2reducedIndex.count(c) == 0 || + index2reducedIndex.count(d) == 0) + continue; + faces.emplace_back(index2reducedIndex[a], index2reducedIndex[b], index2reducedIndex[d]); + faces.emplace_back(index2reducedIndex[d], index2reducedIndex[c], index2reducedIndex[a]); + } + } + } + std::ofstream out(fname); out << "ply\n"; out << "format binary_little_endian 1.0\n" /*"format ascii 1.0\n"*/; @@ -74,6 +102,8 @@ namespace librealsense out << "property uchar green\n"; out << "property uchar blue\n"; } + out << "element face " << faces.size() << "\n"; + out << "property list uchar int vertex_indices\n"; out << "end_header\n"; out.close(); @@ -94,6 +124,14 @@ namespace librealsense out.write(reinterpret_cast(&z), sizeof(uint8_t)); } } + auto size = faces.size(); + for (int i = 0; i < size; ++i) { + int three = 3; + out.write(reinterpret_cast(&three), sizeof(uint8_t)); + out.write(reinterpret_cast(&(std::get<0>(faces[i]))), sizeof(int)); + out.write(reinterpret_cast(&(std::get<1>(faces[i]))), sizeof(int)); + out.write(reinterpret_cast(&(std::get<2>(faces[i]))), sizeof(int)); + } } size_t points::get_vertex_count() const @@ -332,7 +370,7 @@ namespace librealsense std::shared_ptr make_archive(rs2_extension type, std::atomic* in_max_frame_queue_size, std::shared_ptr ts, - std::shared_ptr parsers) + std::shared_ptr parsers) { switch (type) {