Save and read pose and calibration in stream

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);

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 {