From ec8034fbb0bf0f8f73871e40b70a4e0d577e4a9a Mon Sep 17 00:00:00 2001
From: Ryan Pavlik <ryan.pavlik@collabora.com>
Date: Mon, 24 Jun 2019 18:34:53 -0500
Subject: [PATCH] d/mt: Also refactor the filter objects the same way.
---
.../drivers/montrack/filters/common/filter.c | 31 ++---
.../drivers/montrack/filters/common/filter.h | 83 ++++++++++----
.../montrack/filters/filter_opencv_kalman.cpp | 108 +++++++++---------
.../montrack/filters/filter_opencv_kalman.h | 27 +----
src/xrt/drivers/montrack/mt_device.c | 12 +-
src/xrt/drivers/montrack/mt_device.h | 2 +-
6 files changed, 133 insertions(+), 130 deletions(-)
diff --git a/src/xrt/drivers/montrack/filters/common/filter.c b/src/xrt/drivers/montrack/filters/common/filter.c
index 003dc92bc..e9b1c03d2 100644
--- a/src/xrt/drivers/montrack/filters/common/filter.c
+++ b/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;
}
diff --git a/src/xrt/drivers/montrack/filters/common/filter.h b/src/xrt/drivers/montrack/filters/common/filter.h
index 34ae5e6ee..6e7c108d4 100644
--- a/src/xrt/drivers/montrack/filters/common/filter.h
+++ b/src/xrt/drivers/montrack/filters/common/filter.h
@@ -1,15 +1,14 @@
#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
diff --git a/src/xrt/drivers/montrack/filters/filter_opencv_kalman.cpp b/src/xrt/drivers/montrack/filters/filter_opencv_kalman.cpp
index 4e4e3f669..d535096f9 100644
--- a/src/xrt/drivers/montrack/filters/filter_opencv_kalman.cpp
+++ b/src/xrt/drivers/montrack/filters/filter_opencv_kalman.cpp
@@ -1,12 +1,14 @@
#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;
}
diff --git a/src/xrt/drivers/montrack/filters/filter_opencv_kalman.h b/src/xrt/drivers/montrack/filters/filter_opencv_kalman.h
index d4fa11c95..71f4fb933 100644
--- a/src/xrt/drivers/montrack/filters/filter_opencv_kalman.h
+++ b/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"
diff --git a/src/xrt/drivers/montrack/mt_device.c b/src/xrt/drivers/montrack/mt_device.c
index 84fcdfc3c..2a7a1b7ca 100644
--- a/src/xrt/drivers/montrack/mt_device.c
+++ b/src/xrt/drivers/montrack/mt_device.c
@@ -67,20 +67,20 @@ mt_device_get_tracked_pose(struct xrt_device* xdev,
{
mt_device_t* md = mt_device(xdev);
struct xrt_pose pose;
- filter_state_t filtered;
+ struct filter_state filtered;
switch (md->tracker->tracker_type) {
case TRACKER_TYPE_SPHERE_MONO:
out_relation->relation_flags = (enum xrt_space_relation_flags)(
XRT_SPACE_RELATION_POSITION_VALID_BIT |
XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
- md->filter->filter_predict_state(md->filter, &filtered, 0);
+ filter_predict_state(md->filter, &filtered, 0);
out_relation->pose = filtered.pose;
break;
case TRACKER_TYPE_SPHERE_STEREO:
out_relation->relation_flags = (enum xrt_space_relation_flags)(
XRT_SPACE_RELATION_POSITION_VALID_BIT |
XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
- md->filter->filter_predict_state(md->filter, &filtered, 0);
+ filter_predict_state(md->filter, &filtered, 0);
out_relation->pose = filtered.pose;
break;
case TRACKER_TYPE_UVBI:
@@ -599,7 +599,7 @@ mt_create_uvbi_hdk(mt_device_t* md)
return true;
}
#endif // XRT_HAVE_UVC_FRAMESERVER
-#endif
+#endif
bool
mt_create_stereo_ps4(mt_device_t* md)
@@ -698,11 +698,11 @@ mt_create_stereo_ps4(mt_device_t* md)
filter_config.process_noise_cov = 0.1f;
md->filter = filter_create(FILTER_TYPE_OPENCV_KALMAN);
- md->filter->filter_configure(md->filter, &filter_config);
+ filter_configure(md->filter, &filter_config);
// connect our tracker to our filter
md->tracker->tracker_register_measurement_callback(
- md->tracker, md->filter, md->filter->filter_queue);
+ md->tracker, md->filter, md->filter->queue);
md->tracker->tracker_register_event_callback(md->tracker, md,
mt_handle_event);
diff --git a/src/xrt/drivers/montrack/mt_device.h b/src/xrt/drivers/montrack/mt_device.h
index bafe03ea5..96c9be01e 100644
--- a/src/xrt/drivers/montrack/mt_device.h
+++ b/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;
--
GitLab