t/helper: Introduce a small debug frame helper

src/xrt/auxiliary/CMakeLists.txt

@@ -42,6 +42,7 @@ if(BUILD_TRACKING)
 		tracking/t_debug_hsv_viewer.cpp
 		tracking/t_file.cpp
 		tracking/t_fusion.hpp
+		tracking/t_helper_debug_sink.hpp
 		tracking/t_hsv_filter.c
 		tracking/t_kalman.cpp
 		tracking/t_tracker_psmv_fusion.hpp

src/xrt/auxiliary/meson.build

@@ -120,6 +120,7 @@ if build_tracking
 		'tracking/t_debug_hsv_viewer.cpp',
 		'tracking/t_file.cpp',
 		'tracking/t_fusion.hpp',
+		'tracking/t_helper_debug_sink.hpp',
 		'tracking/t_hsv_filter.c',
 		'tracking/t_kalman.cpp',
 		'tracking/t_tracker_psmv.cpp',

src/xrt/auxiliary/tracking/t_helper_debug_sink.hpp

@@ -0,0 +1,127 @@
+// Copyright 2019, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  Small helper struct that for debugging views.
+ * @author Jakob Bornecrantz <jakob@collabora.com>
+ * @ingroup aux_tracking
+ */
+
+#pragma once
+
+#ifndef __cplusplus
+#error "This header is C++-only."
+#endif
+
+#include <opencv2/opencv.hpp>
+#include "util/u_frame.h"
+
+
+struct HelperDebugSink
+{
+public:
+	enum Kind
+	{
+		AllAvailable,
+		AlwaysSingle,
+	};
+
+public:
+	Kind kind = AllAvailable;
+	struct xrt_frame_sink *sink = {};
+	struct xrt_frame *frame = {};
+
+	cv::Mat rgb[2] = {};
+
+
+public:
+	HelperDebugSink(Kind kind)
+	{
+		this->kind = kind;
+	}
+
+	HelperDebugSink() = delete;
+
+	~HelperDebugSink()
+	{
+		xrt_frame_reference(&frame, NULL);
+	}
+
+	void
+	refresh(struct xrt_frame *xf)
+	{
+		if (sink == NULL) {
+			return;
+		}
+
+		// But what about second breakfast?
+		bool second_view = false;
+		int rows, cols, width, height;
+
+		cols = xf->width;
+		rows = xf->height;
+		width = xf->width;
+		height = xf->height;
+		enum xrt_stereo_format stereo_format = xf->stereo_format;
+
+		switch (xf->stereo_format) {
+		case XRT_STEREO_FORMAT_SBS:
+			cols /= 2;
+			if (kind == AllAvailable) {
+				second_view = true;
+			} else {
+				stereo_format = XRT_STEREO_FORMAT_NONE;
+				width /= 2;
+				second_view = false;
+			}
+			break;
+		case XRT_STEREO_FORMAT_NONE:
+			// Noop
+			break;
+		default: return;
+		}
+
+		// Create a new frame and also dereferences the old frame.
+		u_frame_create_one_off(XRT_FORMAT_R8G8B8, width, height,
+		                       &frame);
+
+		// Copy needed info.
+		frame->source_sequence = xf->source_sequence;
+		frame->stereo_format = stereo_format;
+
+		// Doesn't claim ownership of the frame data,
+		// points directly at the frame data.
+		rgb[0] = cv::Mat(   //
+		    rows,           // rows
+		    cols,           // cols
+		    CV_8UC3,        // channels
+		    frame->data,    // data
+		    frame->stride); // stride
+
+		if (second_view) {
+			// Doesn't claim ownership of the frame data,
+			// points directly at the frame data.
+			rgb[1] = cv::Mat(           //
+			    rows,                   // rows
+			    cols,                   // cols
+			    CV_8UC3,                // channels
+			    frame->data + 3 * cols, // data
+			    frame->stride);         // stride
+		}
+	}
+
+	void
+	submit()
+	{
+		if (frame != NULL) {
+			// Make sure that the cv::Mats doesn't use the data.
+			rgb[0] = cv::Mat();
+			rgb[1] = cv::Mat();
+			sink->push_frame(sink, frame);
+		}
+
+		// We unreference the frame here, downstream is either
+		// done with it or have referenced it themselves.
+		xrt_frame_reference(&frame, NULL);
+	}
+};

src/xrt/auxiliary/tracking/t_tracker_psmv.cpp

@@ -14,6 +14,7 @@
 #include "tracking/t_tracking.h"
 #include "tracking/t_calibration_opencv.hpp"
 #include "tracking/t_tracker_psmv_fusion.hpp"
+#include "tracking/t_helper_debug_sink.hpp"
 
 #include "util/u_var.h"
 #include "util/u_misc.h"
@@ -65,13 +66,7 @@ struct TrackerPSMV
 
 	bool tracked = false;
 
-	struct
-	{
-		struct xrt_frame_sink *sink;
-		struct xrt_frame *frame;
-
-		cv::Mat rgb[2];
-	} debug;
+	HelperDebugSink debug = {HelperDebugSink::AllAvailable};
 
 	//! Have we received a new IMU sample.
 	bool has_imu = false;
@@ -95,34 +90,6 @@ struct TrackerPSMV
 	xrt_vec3 tracked_object_position;
 };
 
-static void
-refresh_gui_frame(TrackerPSMV &t, struct xrt_frame *xf)
-{
-	if (t.debug.sink == NULL) {
-		return;
-	}
-
-	// Also dereferences the old frame.
-	u_frame_create_one_off(XRT_FORMAT_R8G8B8, xf->width, xf->height,
-	                       &t.debug.frame);
-	t.debug.frame->source_sequence = xf->source_sequence;
-
-	int rows = xf->height;
-	int cols = xf->width / 2;
-
-	t.debug.rgb[0] = cv::Mat(rows,                   // rows
-	                         cols,                   // cols
-	                         CV_8UC3,                // channels
-	                         t.debug.frame->data,    // data
-	                         t.debug.frame->stride); // stride
-
-	t.debug.rgb[1] = cv::Mat(rows,                           // rows
-	                         cols,                           // cols
-	                         CV_8UC3,                        // channels
-	                         t.debug.frame->data + 3 * cols, // data
-	                         t.debug.frame->stride);         // stride
-}
-
 
 /*!
  * @brief Perform per-view (two in a stereo camera image) processing on an
@@ -255,7 +222,7 @@ process(TrackerPSMV &t, struct xrt_frame *xf)
 	}
 
 	// Create the debug frame if needed.
-	refresh_gui_frame(t, xf);
+	t.debug.refresh(xf);
 
 	t.view[0].keypoints.clear();
 	t.view[1].keypoints.clear();
@@ -315,14 +282,12 @@ process(TrackerPSMV &t, struct xrt_frame *xf)
 		t.filter->clear_position_tracked_flag();
 	}
 
-	if (t.debug.frame != NULL) {
-		t.debug.sink->push_frame(t.debug.sink, t.debug.frame);
-		t.debug.rgb[0] = cv::Mat();
-		t.debug.rgb[1] = cv::Mat();
-	}
+	// We are done with the debug frame.
+	t.debug.submit();
 
+	// We are done with the frame.
 	xrt_frame_reference(&xf, NULL);
-	xrt_frame_reference(&t.debug.frame, NULL);
+
 	if (nearest_world.got_one) {
 #if 0
 		//! @todo something less arbitrary for the lever arm?