player screenshots and bug fixes

applications/player/src/main.cpp

@@ -11,22 +11,35 @@
 
 #include <Eigen/Eigen>
 
+const static std::string help[] = {
+	"Esc", "close",
+	"0-9", "change source",
+	"D",   "toggle depth",
+	"S",   "save screenshot",
+};
+
+std::string time_now_string() {
+	char timestamp[18];
+	std::time_t t=std::time(NULL);
+	std::strftime(timestamp, sizeof(timestamp), "%F-%H%M%S", std::localtime(&t));
+	return std::string(timestamp);
+}
+
 using ftl::codecs::codec_t;
 using ftl::codecs::Channel;
 
-static ftl::codecs::Decoder *decoder;
+static std::map<Channel, ftl::codecs::Decoder*> decoders;
 
-
-static void createDecoder(const ftl::codecs::Packet &pkt) {
-	if (decoder) {
-		if (!decoder->accepts(pkt)) {
-			ftl::codecs::free(decoder);
+static void createDecoder(const Channel channel, const ftl::codecs::Packet &pkt) {
+	if (decoders[channel]) {
+		if (!decoders[channel]->accepts(pkt)) {
+			ftl::codecs::free(decoders[channel]);
 		} else {
 			return;
 		}
 	}
 
-	decoder = ftl::codecs::allocateDecoder(pkt);
+	decoders[channel] = ftl::codecs::allocateDecoder(pkt);
 }
 
 static void visualizeDepthMap(	const cv::Mat &depth, cv::Mat &out,
@@ -74,10 +87,16 @@ int main(int argc, char **argv) {
 	int current_channel = 0;
 
 	int stream_mask = 0;
+
+	static volatile bool screenshot;
+	static int64_t screenshot_ts;
+	static cv::Mat screenshot_color;
+	static cv::Mat screenshot_depth;
+
 	std::vector<std::bitset<128>> channel_mask;
 
 	ftl::timer::add(ftl::timer::kTimerMain, [&current_stream,&current_channel,&r,&stream_mask,&channel_mask](int64_t ts) {
-		bool res = r.read(ts, [&current_stream,&current_channel,&r,&stream_mask,&channel_mask](const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) {
+		bool res = r.read(ts, [ts, &current_stream,&current_channel,&r,&stream_mask,&channel_mask](const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) {
 			if (!(stream_mask & (1 << spkt.streamID))) {
 				stream_mask |= 1 << spkt.streamID;
 				LOG(INFO) << " - Stream found (" << (int)spkt.streamID << ")";
@@ -95,7 +114,7 @@ int main(int argc, char **argv) {
 				LOG(INFO) << "     - Blocks = " << (int)pkt.block_total;
 			}
 
-			if (spkt.streamID == current_stream) {
+			if (spkt.streamID != current_stream) { return; }
 
 			if (pkt.codec == codec_t::POSE) {
 				Eigen::Matrix4d p = Eigen::Map<Eigen::Matrix4d>((double*)pkt.data.data());
@@ -109,27 +128,62 @@ int main(int argc, char **argv) {
 				return;
 			}
 			
-				if (spkt.channel != static_cast<ftl::codecs::Channel>(current_channel)) return;
-
+			auto channel = static_cast<ftl::codecs::Channel>(current_channel);
+			if (pkt.codec == ftl::codecs::codec_t::MSGPACK) { return; }
 			//LOG(INFO) << "Reading packet: (" << (int)spkt.streamID << "," << (int)spkt.channel << ") " << (int)pkt.codec << ", " << (int)pkt.definition;
 
 			cv::cuda::GpuMat gframe(cv::Size(ftl::codecs::getWidth(pkt.definition),ftl::codecs::getHeight(pkt.definition)), (spkt.channel == Channel::Depth) ? CV_32F : CV_8UC3);
 			cv::Mat frame;
-				createDecoder(pkt);
+			createDecoder(spkt.channel, pkt);
 
 			try {
-					decoder->decode(pkt, gframe);
+				decoders[spkt.channel]->decode(pkt, gframe);
 				gframe.download(frame);
 			} catch (std::exception &e) {
 				LOG(INFO) << "Decoder exception: " << e.what();
 			}
 
+			if (screenshot) {
+				if (!screenshot_depth.empty() && !screenshot_color.empty()) {
+					std::string fname = time_now_string();
+					LOG(INFO) << "Screenshot saved: " << fname;
+					cv::imwrite(fname + "-depth.tiff", screenshot_depth);
+					cv::imwrite(fname + "-color.tiff", screenshot_color);
+					screenshot = false;
+					screenshot_color = cv::Mat();
+					screenshot_depth = cv::Mat();
+					screenshot_ts = 0;
+				}
+				else {
+					if (screenshot_ts != ts) {
+						screenshot_ts = ts;
+						screenshot_color = cv::Mat();
+						screenshot_depth = cv::Mat();
+					}
+					if (spkt.channel == Channel::Colour) { 
+						frame.copyTo(screenshot_color);
+					}
+					else if (spkt.channel == Channel::Depth) { 
+						frame.copyTo(screenshot_depth);
+					}
+				}
+			}
+
+			if (spkt.channel != channel) return;
+
 			if (!frame.empty()) {
 				if (spkt.channel == Channel::Depth) {
 					visualizeDepthMap(frame, frame, 8.0f);
 				}
 				double time = (double)(spkt.timestamp - r.getStartTime()) / 1000.0;
 				cv::putText(frame, std::string("Time: ") + std::to_string(time) + std::string("s"), cv::Point(10,20), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0,0,255));
+
+				// hotkey help text
+				for (int i = 0; i < std::size(help); i += 2) {
+					cv::putText(frame, help[i], cv::Point(10, 40+(i/2)*14), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(64,64,255));
+					cv::putText(frame, help[i+1], cv::Point(50, 40+(i/2)*14), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(64,64,255));
+				}
+
 				cv::imshow("Player", frame);
 			} else {
 				frame.create(cv::Size(600,600), CV_8UC3);
@@ -137,14 +191,18 @@ int main(int argc, char **argv) {
 			}
 			int key = cv::waitKey(1);
 			if (key >= 48 && key <= 57) {
-					current_stream = key - 48;
+				int new_stream = key - 48;
+				if ((0 <= new_stream) && (new_stream < channel_mask.size())) {
+					current_stream = new_stream;
+				}
 			} else if (key == 'd') {
 				current_channel = (current_channel == 0) ? 1 : 0;
 			} else if (key == 'r') {
 				current_channel = (current_channel == 0) ? 2 : 0;
 			} else if (key == 27) {
 				ftl::timer::stop(false);
-				}
+			} else if (key == 115) {
+				screenshot = true;
 			}
 		});
 		if (!res) ftl::timer::stop(false);