d/mt: Also refactor the filter objects the same way.

src/xrt/drivers/montrack/filters/common/filter.c

@@ -4,32 +4,14 @@
 
 #include "util/u_misc.h"
 
-filter_instance_t*
+struct filter_instance*
 filter_create(filter_type_t t)
 {
-	filter_instance_t* i = U_TYPED_CALLOC(filter_instance_t);
-	if (i) {
-		switch (t) {
-		case FILTER_TYPE_OPENCV_KALMAN:
-			i->tracker_type = t;
-			i->filter_configure = filter_opencv_kalman_configure;
-			i->filter_get_state = filter_opencv_kalman_get_state;
-			i->filter_predict_state =
-			    filter_opencv_kalman_predict_state;
-			i->filter_set_state = filter_opencv_kalman_set_state;
-			i->filter_queue = filter_opencv_kalman_queue;
-			i->internal_instance = (filter_internal_instance_ptr)
-			    filter_opencv_kalman_create(i);
-			break;
-		case FILTER_TYPE_NONE:
-		default:
-			free(i);
-			return NULL;
-			break;
-		}
-		return i;
+	switch (t) {
+	case FILTER_TYPE_OPENCV_KALMAN: return filter_opencv_kalman_create();
+	case FILTER_TYPE_NONE:
+	default: return NULL;
 	}
-	return NULL;
 }
 
 bool
@@ -37,7 +19,8 @@ filters_test()
 {
 
 	// create a filter
-	filter_instance_t* filter = filter_create(FILTER_TYPE_OPENCV_KALMAN);
+	struct filter_instance* filter =
+	    filter_create(FILTER_TYPE_OPENCV_KALMAN);
 	if (!filter) {
 		return false;
 	}

src/xrt/drivers/montrack/filters/common/filter.h

 #ifndef FILTER_H
 #define FILTER_H
+
 #include <xrt/xrt_defines.h>
-#include <../auxiliary/util/u_time.h>
-#include <../optical_tracking/common/tracker.h>
+#include "util/u_time.h"
 
-typedef void* filter_instance_ptr;
-typedef void* filter_internal_instance_ptr;
 typedef void* filter_configuration_ptr;
-typedef void* filter_state_ptr;
 
-typedef struct filter_state
+struct filter_instance;
+
+struct filter_state
 {
 	struct xrt_pose pose;
 	bool has_position;
@@ -19,7 +18,7 @@ typedef struct filter_state
 	struct xrt_quat angular_velocity;
 	struct xrt_quat angular_accel;
 	timepoint_ns timestamp;
-} filter_state_t;
+};
 
 
 typedef enum filter_type
@@ -28,28 +27,62 @@ typedef enum filter_type
 	FILTER_TYPE_OPENCV_KALMAN
 } filter_type_t;
 
-typedef struct _filter_instance
+typedef struct tracker_measurement tracker_measurement_t;
+
+struct filter_instance
 {
 	filter_type_t tracker_type;
-	bool (*filter_queue)(filter_instance_ptr inst,
-	                     tracker_measurement_t* measurement);
-	bool (*filter_set_state)(filter_instance_ptr inst,
-	                         filter_state_ptr state);
-	bool (*filter_get_state)(filter_instance_ptr inst,
-	                         filter_state_ptr state);
-	bool (*filter_predict_state)(filter_instance_ptr inst,
-	                             filter_state_t* state,
-	                             timepoint_ns time);
-	bool (*filter_configure)(filter_instance_ptr inst,
-	                         filter_configuration_ptr config);
-	filter_internal_instance_ptr internal_instance;
-} filter_instance_t;
-
-filter_instance_t*
+	bool (*queue)(struct filter_instance* inst,
+	              tracker_measurement_t* measurement);
+	bool (*set_state)(struct filter_instance* inst,
+	                  struct filter_state* state);
+	bool (*get_state)(struct filter_instance* inst,
+	                  struct filter_state* state);
+	bool (*predict_state)(struct filter_instance* inst,
+	                      struct filter_state* state,
+	                      timepoint_ns time);
+	bool (*configure)(struct filter_instance* inst,
+	                  filter_configuration_ptr config);
+	void (*destroy)(struct filter_instance* inst);
+};
+
+struct filter_instance*
 filter_create(filter_type_t t);
-bool
-filter_destroy(filter_instance_t* inst);
+
 bool
 filters_test();
 
+static inline void
+filter_destroy(struct filter_instance* inst)
+{
+	inst->destroy(inst);
+}
+
+static inline bool
+filter_queue(struct filter_instance* inst, tracker_measurement_t* measurement)
+{
+	return inst->queue(inst, measurement);
+}
+static inline bool
+filter_set_state(struct filter_instance* inst, struct filter_state* state)
+{
+	return inst->set_state(inst, state);
+}
+static inline bool
+filter_get_state(struct filter_instance* inst, struct filter_state* state)
+{
+	return inst->get_state(inst, state);
+}
+static inline bool
+filter_predict_state(struct filter_instance* inst,
+                     struct filter_state* state,
+                     timepoint_ns time)
+{
+	return inst->predict_state(inst, state, time);
+}
+static inline bool
+filter_configure(struct filter_instance* inst, filter_configuration_ptr config)
+{
+	return inst->configure(inst, config);
+}
 #endif // FILTER_H

src/xrt/drivers/montrack/filters/filter_opencv_kalman.cpp

 
 #include <opencv2/opencv.hpp>
 
+#include "../optical_tracking/common/tracker.h"
 #include "filter_opencv_kalman.h"
 
 #include "util/u_misc.h"
 
-struct filter_opencv_kalman_instance_t
+struct filter_opencv_kalman
 {
+	struct filter_instance base;
 	bool configured;
 	opencv_filter_configuration_t configuration;
 	cv::KalmanFilter kalman_filter;
@@ -20,25 +22,23 @@ struct filter_opencv_kalman_instance_t
  * Casts the internal instance pointer from the generic opaque type to our
  * opencv_kalman internal type.
  */
-static inline filter_opencv_kalman_instance_t*
-filter_opencv_kalman_instance(filter_internal_instance_ptr ptr)
+static inline struct filter_opencv_kalman*
+filter_opencv_kalman(struct filter_instance* ptr)
 {
-	return (filter_opencv_kalman_instance_t*)ptr;
+	return (struct filter_opencv_kalman*)ptr;
 }
 
-bool
-filter_opencv_kalman__destroy(filter_instance_t* inst)
+static void
+filter_opencv_kalman_destroy(struct filter_instance* inst)
 {
-	// do nothing
-	return false;
+	free(inst);
 }
 
-bool
-filter_opencv_kalman_queue(filter_instance_t* inst,
+static bool
+filter_opencv_kalman_queue(struct filter_instance* inst,
                            tracker_measurement_t* measurement)
 {
-	filter_opencv_kalman_instance_t* internal =
-	    filter_opencv_kalman_instance(inst->internal_instance);
+	struct filter_opencv_kalman* internal = filter_opencv_kalman(inst);
 	printf("queueing measurement in filter\n");
 	internal->observation.at<float>(0, 0) = measurement->pose.position.x;
 	internal->observation.at<float>(1, 0) = measurement->pose.position.y;
@@ -48,22 +48,23 @@ filter_opencv_kalman_queue(filter_instance_t* inst,
 	return false;
 }
 bool
-filter_opencv_kalman_get_state(filter_instance_t* inst, filter_state_t* state)
+filter_opencv_kalman_get_state(struct filter_instance* inst,
+                               struct filter_state* state)
 {
 	return false;
 }
 bool
-filter_opencv_kalman_set_state(filter_instance_t* inst, filter_state_t* state)
+filter_opencv_kalman_set_state(struct filter_instance* inst,
+                               struct filter_state* state)
 {
 	return false;
 }
 bool
-filter_opencv_kalman_predict_state(filter_instance_t* inst,
-                                   filter_state_t* state,
+filter_opencv_kalman_predict_state(struct filter_instance* inst,
+                                   struct filter_state* state,
                                    timepoint_ns time)
 {
-	filter_opencv_kalman_instance_t* internal =
-	    filter_opencv_kalman_instance(inst->internal_instance);
+	struct filter_opencv_kalman* internal = filter_opencv_kalman(inst);
 	// printf("getting filtered pose\n");
 	if (!internal->running) {
 		return false;
@@ -76,11 +77,10 @@ filter_opencv_kalman_predict_state(filter_instance_t* inst,
 	return true;
 }
 bool
-filter_opencv_kalman_configure(filter_instance_t* inst,
+filter_opencv_kalman_configure(struct filter_instance* inst,
                                filter_configuration_ptr config_generic)
 {
-	filter_opencv_kalman_instance_t* internal =
-	    filter_opencv_kalman_instance(inst->internal_instance);
+	struct filter_opencv_kalman* internal = filter_opencv_kalman(inst);
 	opencv_filter_configuration_t* config =
 	    (opencv_filter_configuration_t*)config_generic;
 	internal->configuration = *config;
@@ -96,40 +96,44 @@ filter_opencv_kalman_configure(filter_instance_t* inst,
 
 
 
-filter_opencv_kalman_instance_t*
-filter_opencv_kalman_create(filter_instance_t* inst)
+struct filter_opencv_kalman*
+filter_opencv_kalman_create()
 {
-	filter_opencv_kalman_instance_t* i =
-	    U_TYPED_CALLOC(filter_opencv_kalman_instance_t);
-	if (i) {
-		float dt = 1.0;
-		i->kalman_filter.init(6, 3);
-		i->observation = cv::Mat(3, 1, CV_32F);
-		i->prediction = cv::Mat(6, 1, CV_32F);
-		i->kalman_filter.transitionMatrix =
-		    (cv::Mat_<float>(6, 6) << 1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.0,
-		     1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, dt, 0.0,
-		     0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
-		     0.0, 0.0, 0.0, 0.0, 1.0);
+	struct filter_opencv_kalman* i =
+	    U_TYPED_CALLOC(struct filter_opencv_kalman);
+	if (!i) {
+		return NULL;
+	}
+	i->base.queue = filter_opencv_kalman_queue;
+	i->base.set_state = filter_opencv_kalman_set_state;
+	i->base.get_state = filter_opencv_kalman_get_state;
+	i->base.predict_state = filter_opencv_kalman_predict_state;
+	i->base.configure = filter_opencv_kalman_configure;
+	i->base.destroy = filter_opencv_kalman_destroy;
+	float dt = 1.0;
+	i->kalman_filter.init(6, 3);
+	i->observation = cv::Mat(3, 1, CV_32F);
+	i->prediction = cv::Mat(6, 1, CV_32F);
+	i->kalman_filter.transitionMatrix =
+	    (cv::Mat_<float>(6, 6) << 1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.0, 1.0,
+	     0.0, 0.0, dt, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.0, 1.0,
+	     0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+	     1.0);
 
-		cv::setIdentity(i->kalman_filter.measurementMatrix,
-		                cv::Scalar::all(1.0f));
-		cv::setIdentity(i->kalman_filter.errorCovPost,
-		                cv::Scalar::all(0.0f));
+	cv::setIdentity(i->kalman_filter.measurementMatrix,
+	                cv::Scalar::all(1.0f));
+	cv::setIdentity(i->kalman_filter.errorCovPost, cv::Scalar::all(0.0f));
 
-		// our filter parameters set the process and measurement noise
-		// covariances.
+	// our filter parameters set the process and measurement noise
+	// covariances.
 
-		cv::setIdentity(
-		    i->kalman_filter.processNoiseCov,
-		    cv::Scalar::all(i->configuration.process_noise_cov));
-		cv::setIdentity(
-		    i->kalman_filter.measurementNoiseCov,
-		    cv::Scalar::all(i->configuration.measurement_noise_cov));
+	cv::setIdentity(i->kalman_filter.processNoiseCov,
+	                cv::Scalar::all(i->configuration.process_noise_cov));
+	cv::setIdentity(
+	    i->kalman_filter.measurementNoiseCov,
+	    cv::Scalar::all(i->configuration.measurement_noise_cov));
 
-		i->configured = false;
-		i->running = false;
-		return i;
-	}
-	return NULL;
+	i->configured = false;
+	i->running = false;
+	return i;
 }

src/xrt/drivers/montrack/filters/filter_opencv_kalman.h

@@ -20,28 +20,11 @@ extern "C" {
 #endif
 
 // forward declare this, as it contains C++ stuff
-typedef struct filter_opencv_kalman_instance_t filter_opencv_kalman_instance_t;
-
-
-filter_opencv_kalman_instance_t*
-filter_opencv_kalman_create(filter_instance_t* inst);
-bool
-filter_opencv_kalman__destroy(filter_instance_t* inst);
-
-bool
-filter_opencv_kalman_queue(filter_instance_t* inst,
-                           tracker_measurement_t* measurement);
-bool
-filter_opencv_kalman_get_state(filter_instance_t* inst, filter_state_t* state);
-bool
-filter_opencv_kalman_set_state(filter_instance_t* inst, filter_state_t* state);
-bool
-filter_opencv_kalman_predict_state(filter_instance_t* inst,
-                                   filter_state_t*,
-                                   timepoint_ns time);
-bool
-filter_opencv_kalman_configure(filter_instance_t* inst,
-                               filter_configuration_ptr config_generic);
+struct filter_opencv_kalman;
+
+
+struct filter_opencv_kalman*
+filter_opencv_kalman_create();
 
 #ifdef __cplusplus
 } // extern "C"

src/xrt/drivers/montrack/mt_device.h

@@ -31,7 +31,7 @@ typedef struct mt_device
 	// n-source trackers
 	tracker_mono_configuration_t config_mono;
 	tracker_stereo_configuration_t config_stereo;
-	filter_instance_t* filter;
+	struct filter_instance* filter;
 	bool log_verbose;
 	bool log_debug;
 } mt_device_t;