diff --git a/include/librealsense2/hpp/rs_export.hpp b/include/librealsense2/hpp/rs_export.hpp index cdb31ec466..a0fc8941da 100644 --- a/include/librealsense2/hpp/rs_export.hpp +++ b/include/librealsense2/hpp/rs_export.hpp @@ -8,7 +8,9 @@ #include #include #include +#include #include "rs_processing.hpp" +#include "rs_internal.hpp" namespace rs2 { @@ -155,6 +157,77 @@ namespace rs2 std::string fname; pointcloud _pc; }; + + class save_single_frameset : public filter { + public: + save_single_frameset(std::string filename = "RealSense Frameset ") + : filter([this](frame f, frame_source& s) { save(f, s); }), fname(filename) + {} + + private: + void save(frame data, frame_source& source, bool do_signal=true) + { + software_device dev; + + std::vector> sensors; + if (auto fs = data.as()) { + int uid = 0; + for (int i = 0; i < fs.size(); ++i) { + frame f = fs[i]; + auto profile = f.get_profile(); + auto s = dev.add_sensor(profile.stream_name() + "Sensor (" + std::to_string(uid) + ")"); + stream_profile software_profile; + + if (auto vf = f.as()) { + auto vp = profile.as(); + 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() }; + software_profile = s.add_video_stream(stream); + } else if (f.is()) { + auto mp = profile.as(); + rs2_motion_stream stream{ mp.stream_type(), mp.stream_index(), uid++, mp.fps(), mp.format(), mp.get_motion_intrinsics() }; + software_profile = s.add_motion_stream(stream); + } else if (f.is()) { + rs2_pose_stream stream{ profile.stream_type(), profile.stream_index(), uid++, profile.fps(), profile.format() }; + software_profile = s.add_pose_stream(stream); + } else { + // TODO: How to handle other frame types? (e.g. points) + assert(false); + } + sensors.emplace_back(s, software_profile, i); + } + + // Recorder needs sensors to already exist when its created + recorder rec(fname + std::to_string(data.get_frame_number()) + ".bag", dev); + + for (auto group : sensors) { + auto s = std::get(group); + auto profile = std::get(group); + s.open(profile); + s.start([](frame) {}); + frame f = fs[std::get(group)]; + if (auto vf = f.as()) { + s.on_video_frame({ const_cast(vf.get_data()), [](void*) {}, vf.get_stride_in_bytes(), vf.get_bytes_per_pixel(), + vf.get_timestamp(), vf.get_frame_timestamp_domain(), static_cast(vf.get_frame_number()), profile }); + } else if (f.is()) { + s.on_motion_frame({ const_cast(f.get_data()), [](void*) {}, f.get_timestamp(), + f.get_frame_timestamp_domain(), static_cast(f.get_frame_number()), profile }); + } else if (f.is()) { + s.on_pose_frame({ const_cast(f.get_data()), [](void*) {}, f.get_timestamp(), + f.get_frame_timestamp_domain(), static_cast(f.get_frame_number()), profile }); + } + } + } else { + // single frame + auto set = source.allocate_composite_frame({ data }); + save(set, source, false); + } + + if (do_signal) + source.frame_ready(data); + } + + std::string fname; + }; } #endif \ No newline at end of file