Commit ec8034fb authored by Ryan Pavlik's avatar Ryan Pavlik

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

parent 504ff7d8
Pipeline #44596 passed with stages
in 46 seconds
......@@ -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;
}
......
#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
#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;
}
......@@ -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"
......
......@@ -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;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment