diff --git a/applications/player/src/main.cpp b/applications/player/src/main.cpp index 0423600c99c2ace5c6fb20ea4910f78b15e65311..4d3bccafccfc38697974c248db526bceb948aa3f 100644 --- a/applications/player/src/main.cpp +++ b/applications/player/src/main.cpp @@ -3,11 +3,17 @@ #include <ftl/codecs/reader.hpp> #include <ftl/codecs/decoder.hpp> #include <ftl/codecs/packet.hpp> +#include <ftl/rgbd/camera.hpp> #include <fstream> +#include <Eigen/Eigen> + +using ftl::codecs::codec_t; + static ftl::codecs::Decoder *decoder; + static void createDecoder(const ftl::codecs::Packet &pkt) { if (decoder) { if (!decoder->accepts(pkt)) { @@ -41,6 +47,18 @@ int main(int argc, char **argv) { if (spkt.channel & 0x1 > 0) return; if (spkt.streamID == current_stream) { + if (pkt.codec == codec_t::POSE) { + Eigen::Matrix4d p = Eigen::Map<Eigen::Matrix4d>((double*)pkt.data.data()); + LOG(INFO) << "Have pose: " << p; + return; + } + + if (pkt.codec == codec_t::CALIBRATION) { + ftl::rgbd::Camera *camera = (ftl::rgbd::Camera*)pkt.data.data(); + LOG(INFO) << "Have calibration: " << camera->fx; + return; + } + LOG(INFO) << "Reading packet: (" << (int)spkt.streamID << "," << (int)spkt.channel << ") " << (int)pkt.codec << ", " << (int)pkt.definition; cv::Mat frame(cv::Size(ftl::codecs::getWidth(pkt.definition),ftl::codecs::getHeight(pkt.definition)), CV_8UC3); diff --git a/applications/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp index 7a391be6d598270560b9cf00f86ec7e7d94c9792..2f9900634eef11bf83f77aafd12301b431f26bab 100644 --- a/applications/reconstruct/src/main.cpp +++ b/applications/reconstruct/src/main.cpp @@ -64,6 +64,33 @@ static Eigen::Affine3d create_rotation_matrix(float ax, float ay, float az) { return rz * rx * ry; } +static void writeSourceProperties(ftl::codecs::Writer &writer, int id, ftl::rgbd::Source *src) { + ftl::codecs::StreamPacket spkt; + ftl::codecs::Packet pkt; + + spkt.timestamp = 0; + spkt.streamID = id; + spkt.channel = 0; + spkt.channel_count = 1; + pkt.codec = ftl::codecs::codec_t::CALIBRATION; + pkt.definition = ftl::codecs::definition_t::Any; + pkt.block_number = 0; + pkt.block_total = 1; + pkt.flags = 0; + pkt.data = std::move(std::vector<uint8_t>((uint8_t*)&src->parameters(), (uint8_t*)&src->parameters() + sizeof(ftl::rgbd::Camera))); + + writer.write(spkt, pkt); + + pkt.codec = ftl::codecs::codec_t::POSE; + pkt.definition = ftl::codecs::definition_t::Any; + pkt.block_number = 0; + pkt.block_total = 1; + pkt.flags = 0; + pkt.data = std::move(std::vector<uint8_t>((uint8_t*)src->getPose().data(), (uint8_t*)src->getPose().data() + 4*4*sizeof(double))); + + writer.write(spkt, pkt); +} + static void run(ftl::Configurable *root) { Universe *net = ftl::create<Universe>(root, "net"); ftl::ctrl::Slave slave(net, root); @@ -185,6 +212,10 @@ static void run(ftl::Configurable *root) { writer.begin(); // TODO: Write pose+calibration+config packets + auto sources = group.sources(); + for (int i=0; i<sources.size(); ++i) { + writeSourceProperties(writer, i, sources[i]); + } group.addRawCallback(std::function(recorder)); } else {