diff --git a/src/xrt/auxiliary/CMakeLists.txt b/src/xrt/auxiliary/CMakeLists.txt index b68aedf0a748fbfdfed5350cacd7bf1489eaff6e..82b6e80bef9ed8a7d10cb940632336dbdb8654a0 100644 --- a/src/xrt/auxiliary/CMakeLists.txt +++ b/src/xrt/auxiliary/CMakeLists.txt @@ -8,6 +8,7 @@ set(MATH_SOURCE_FILES math/m_hash.cpp math/m_optics.c math/m_quatexpmap.cpp + math/m_track.c ) set(OS_SOURCE_FILES diff --git a/src/xrt/auxiliary/math/m_api.h b/src/xrt/auxiliary/math/m_api.h index 36d8ba0173c79243f0a684284e721e28d1e33ace..79be0eb5c45d51f5283e9aa24198f78df9a850e7 100644 --- a/src/xrt/auxiliary/math/m_api.h +++ b/src/xrt/auxiliary/math/m_api.h @@ -307,6 +307,16 @@ math_compute_fovs(double w_total, double vertfov_total, struct xrt_fov *fov); +void +math_euler_to_quat(struct xrt_vec3 euler, struct xrt_quat* q); + +int +math_min(int a, int b); + +int +math_max(int a, int b); + + #ifdef __cplusplus } #endif diff --git a/src/xrt/auxiliary/math/m_track.c b/src/xrt/auxiliary/math/m_track.c index 0a9ecc485edc2bc05934a4f204e9030fdcd145b2..008482edb478ca7045509600e0f398734a984dc4 100644 --- a/src/xrt/auxiliary/math/m_track.c +++ b/src/xrt/auxiliary/math/m_track.c @@ -25,3 +25,19 @@ math_euler_to_quat(struct xrt_vec3 euler, struct xrt_quat* q) q->w=1.0f; } + +int +math_min(int a, int b) { + if (a > b) { + return b; + } + return a; +} + +int +math_max(int a, int b) { + if (a > b) { + return a; + } + return b; +} diff --git a/src/xrt/drivers/montrack/CMakeLists.txt b/src/xrt/drivers/montrack/CMakeLists.txt index 82db94c597786f13dc34367b6b7b0ed79cc60ec3..28310fe5db41a043159a7bf2ed15919c61a567e8 100644 --- a/src/xrt/drivers/montrack/CMakeLists.txt +++ b/src/xrt/drivers/montrack/CMakeLists.txt @@ -6,7 +6,7 @@ set (MONTRACK_SOURCE_FILES mt_events.h mt_framequeue.h mt_framequeue.c - ) +) add_subdirectory(frameservers) diff --git a/src/xrt/drivers/montrack/filters/CMakeLists.txt b/src/xrt/drivers/montrack/filters/CMakeLists.txt index cd5e2e43a85b1656d347a8f092221faac542cbba..f808dd492ec6b656b8bad2d29bbb47672f15d5f4 100644 --- a/src/xrt/drivers/montrack/filters/CMakeLists.txt +++ b/src/xrt/drivers/montrack/filters/CMakeLists.txt @@ -10,8 +10,13 @@ include_directories( set(FILTER_SOURCE_FILES common/filter.h common/filter.c + common/measurementqueue.h + common/measurementqueue.c filter_opencv_kalman.cpp filter_opencv_kalman.h + filter_complementary.c + filter_complementary.h + ) # Use OBJECT to not create a archive, since it just gets in the way. diff --git a/src/xrt/drivers/montrack/filters/common/filter.c b/src/xrt/drivers/montrack/filters/common/filter.c index 003dc92bc3a634908c91e0ce16af00f8f3fe9b94..ea79b6afb68b09e2c654be846ed279e45832914f 100644 --- a/src/xrt/drivers/montrack/filters/common/filter.c +++ b/src/xrt/drivers/montrack/filters/common/filter.c @@ -1,4 +1,5 @@ #include "filter.h" +#include "filter_complementary.h" #include "filter_opencv_kalman.h" #include <string.h> @@ -18,9 +19,21 @@ filter_create(filter_type_t t) 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); + i->internal_instance = filter_opencv_kalman_create(i); + i->measurement_queue = measurement_queue_create(); break; + case FILTER_TYPE_COMPLEMENTARY: + i->tracker_type = t; + i->filter_configure = filter_complementary_configure; + i->filter_get_state = filter_complementary_get_state; + i->filter_predict_state = + filter_complementary_predict_state; + i->filter_set_state = filter_complementary_set_state; + i->filter_queue = filter_complementary_queue; + i->internal_instance = filter_complementary_create(i); + i->measurement_queue = measurement_queue_create(); + + break; case FILTER_TYPE_NONE: default: free(i); diff --git a/src/xrt/drivers/montrack/filters/common/filter.h b/src/xrt/drivers/montrack/filters/common/filter.h index 34ae5e6ee0ebf777fd515938d45163abaf5cd6f5..a953f5df072ebe5ef4e7c740c8cacd363188bae7 100644 --- a/src/xrt/drivers/montrack/filters/common/filter.h +++ b/src/xrt/drivers/montrack/filters/common/filter.h @@ -2,7 +2,8 @@ #define FILTER_H #include <xrt/xrt_defines.h> #include <../auxiliary/util/u_time.h> -#include <../optical_tracking/common/tracker.h> +#include <optical_tracking/common/tracker.h> +#include "measurementqueue.h" typedef void* filter_instance_ptr; typedef void* filter_internal_instance_ptr; @@ -14,18 +15,20 @@ typedef struct filter_state struct xrt_pose pose; bool has_position; bool has_rotation; + struct xrt_vec3 rotation_euler; struct xrt_vec3 velocity; struct xrt_vec3 acceleration; struct xrt_quat angular_velocity; struct xrt_quat angular_accel; - timepoint_ns timestamp; + int64_t timestamp; } filter_state_t; typedef enum filter_type { FILTER_TYPE_NONE, - FILTER_TYPE_OPENCV_KALMAN + FILTER_TYPE_OPENCV_KALMAN, + FILTER_TYPE_COMPLEMENTARY } filter_type_t; typedef struct _filter_instance @@ -43,6 +46,7 @@ typedef struct _filter_instance bool (*filter_configure)(filter_instance_ptr inst, filter_configuration_ptr config); filter_internal_instance_ptr internal_instance; + measurement_queue_t* measurement_queue; } filter_instance_t; filter_instance_t* diff --git a/src/xrt/drivers/montrack/filters/common/measurementqueue.c b/src/xrt/drivers/montrack/filters/common/measurementqueue.c index b2a2508d98bda62051ec886d312d02d142b24a3f..a37062272f741cdfeff25e186a98393eac769088 100644 --- a/src/xrt/drivers/montrack/filters/common/measurementqueue.c +++ b/src/xrt/drivers/montrack/filters/common/measurementqueue.c @@ -95,33 +95,33 @@ uint32_t measurement_queue_get_since_last_frame(measurement_queue_t* mq,uint32_t return count; } -uint32_t measurement_queue_get_since_timestamp(measurement_queue_t* mq,uint32_t source_id,uint64_t timestamp_ns,tracker_measurement_t** cm) { +uint32_t measurement_queue_get_since_timestamp(measurement_queue_t* mq,uint32_t source_id,int64_t timestamp_us,tracker_measurement_t** cm) { pthread_mutex_lock(&mq->queue_lock); //iterate over our measurements, count the number of records we will copy //our starting timestamp is the one on our last frame uint32_t count=0; for (uint32_t i=mq->measurements.head_index;i<mq->measurements.size;i++) { - if (mq->measurements.items[i].source_timestamp > timestamp_ns) { - printf("H %lld\n",mq->measurements.items[i].source_timestamp); + if (mq->measurements.items[i].source_timestamp > timestamp_us) { + //printf("H %lld\n",mq->measurements.items[i].source_timestamp); count++; } } for (uint32_t i=0;i<mq->measurements.tail_index;i++) { - if (mq->measurements.items[i].source_timestamp > timestamp_ns) { - printf("0 %lld\n",mq->measurements.items[i].source_timestamp); + if (mq->measurements.items[i].source_timestamp > timestamp_us) { + //printf("0 %lld\n",mq->measurements.items[i].source_timestamp); count++; } } *cm = U_TYPED_ARRAY_CALLOC(tracker_measurement_t,count); count =0; for (uint32_t i=mq->measurements.head_index;i<mq->measurements.size;i++) { - if (mq->measurements.items[i].source_timestamp > timestamp_ns) { + if (mq->measurements.items[i].source_timestamp > timestamp_us) { memcpy(*cm+count,&mq->measurements.items[i],sizeof(tracker_measurement_t)); count++; } } for (uint32_t i=0;i<mq->measurements.tail_index;i++) { - if (mq->measurements.items[i].source_timestamp > timestamp_ns) { + if (mq->measurements.items[i].source_timestamp > timestamp_us) { memcpy(*cm+count,&mq->measurements.items[i],sizeof(tracker_measurement_t)); count++; } diff --git a/src/xrt/drivers/montrack/filters/common/measurementqueue.h b/src/xrt/drivers/montrack/filters/common/measurementqueue.h index d75c335885bd93cb66aa61191b88500781263db9..639deebcdb5d34b82fbc367f548a97e550905060 100644 --- a/src/xrt/drivers/montrack/filters/common/measurementqueue.h +++ b/src/xrt/drivers/montrack/filters/common/measurementqueue.h @@ -29,7 +29,7 @@ void measurement_queue_destroy(measurement_queue_t* mq); //bool measurement_queue_get_latest_n(measurement_queue_t* mq,uint32_t source_id, uint32_t* count,tracker_measurement_t* cm); //used by consumers uint32_t measurement_queue_get_since_last_frame(measurement_queue_t* mq,uint32_t source_id,tracker_measurement_t** cm); -uint32_t measurement_queue_get_since_timestamp(measurement_queue_t* mq,uint32_t source_id,uint64_t timestamp_ns,tracker_measurement_t** cm); +uint32_t measurement_queue_get_since_timestamp(measurement_queue_t* mq,uint32_t source_id,int64_t timestamp_us,tracker_measurement_t** cm); void measurement_queue_add(measurement_queue_t* mq, tracker_measurement_t* m); //used by producers uint64_t measurement_queue_uniq_source_id(measurement_queue_t* mq); //used by producers diff --git a/src/xrt/drivers/montrack/filters/filter_complementary.c b/src/xrt/drivers/montrack/filters/filter_complementary.c index 40382b8c64953adf3fd26f9cdc6d68dbb72b3866..3b7d64b3cfdf5904f8bf56590020d1815a6c2c3b 100644 --- a/src/xrt/drivers/montrack/filters/filter_complementary.c +++ b/src/xrt/drivers/montrack/filters/filter_complementary.c @@ -2,12 +2,15 @@ #include "filter_complementary.h" #include "util/u_misc.h" + struct filter_complementary_instance_t { bool configured; filter_complementary_configuration_t configuration; filter_state_t last_state; filter_state_t state; + float gyro_yaw_correction; + uint8_t avg_count; float alpha; bool running; }; @@ -44,39 +47,53 @@ filter_complementary_get_state(filter_instance_t* inst, filter_state_t* state) { filter_complementary_instance_t* internal = filter_complementary_instance(inst->internal_instance); - // printf("getting filtered pose\n"); - if (!internal->running) { + if (!internal->running) { return false; } tracker_measurement_t* measurement_array; uint64_t last_timestamp = internal->last_state.timestamp; - if (internal->configuration.max_timestamp > 0) - { - if (last_timestamp >= internal->configuration.max_timestamp){ - //wrap timestamp - last_timestamp = internal->configuration.max_timestamp - last_timestamp; - } - } uint32_t count = measurement_queue_get_since_timestamp(inst->measurement_queue,0,last_timestamp,&measurement_array); float one_minus_bias = 1.0f - internal->configuration.bias; for (uint32_t i=0;i<count;i++) { tracker_measurement_t* m = &measurement_array[i]; - float dt = (m->source_timestamp - internal->last_state.timestamp) * 0.0000001; - float normAccel = sqrt(m->accel.x * m->accel.x + m->accel.y * m->accel.y + m->accel.z+m->accel.z); + float dt = (m->source_timestamp - internal->last_state.timestamp) *0.00000001; + if (dt > 1.0f) { + internal->last_state.timestamp = m->source_timestamp; + return false; //this is our first frame, or something has gone wrong... big dt will blow up calculations. + } + + float magAccel = sqrt(m->accel.x * m->accel.x + m->accel.y * m->accel.y + m->accel.z+m->accel.z); + + //assume that, if acceleration is only gravity, then any change in the gyro is drift and update compensation + int avg_max = 32; + if (internal->avg_count < avg_max) { + internal->avg_count++; + } + + printf("%f %f %f %f %f %f %f\n",m->accel.x,m->accel.y,m->accel.z,m->gyro.x,m->gyro.y,m->gyro.z,dt); + + float accelDiff =1.0f; + if (internal->gyro_yaw_correction > 0.0f) + { + accelDiff = (magAccel - internal->gyro_yaw_correction) / internal->gyro_yaw_correction; + } + + internal->gyro_yaw_correction = magAccel + (magAccel - internal->gyro_yaw_correction) / math_min(internal->avg_count,avg_max); + //printf("accelDiff %f magAccel: %f gyro.z %f yaw corr %f\n",accelDiff,magAccel,m->gyro.z, internal->gyro_yaw_correction); //calculate filtered euler angles - internal->state.rotation_euler.z = internal->last_state.rotation_euler.z + m->gyro.z * dt; - internal->state.rotation_euler.y = internal->configuration.bias * (internal->last_state.rotation_euler.y + m->gyro.y * dt) + one_minus_bias * (m->accel.y * internal->configuration.scale/normAccel); - internal->state.rotation_euler.x = internal->configuration.bias * (internal->last_state.rotation_euler.x + m->gyro.x * dt) + one_minus_bias * (m->accel.x * internal->configuration.scale/normAccel); + internal->state.rotation_euler.z = internal->configuration.bias * (internal->last_state.rotation_euler.z + m->gyro.z * dt) + one_minus_bias * (m->accel.z * internal->configuration.scale/magAccel); + internal->state.rotation_euler.y = internal->configuration.bias * (internal->last_state.rotation_euler.y + m->gyro.y * dt) + one_minus_bias * (m->accel.y * internal->configuration.scale/magAccel); + internal->state.rotation_euler.x = internal->configuration.bias * (internal->last_state.rotation_euler.x + m->gyro.x * dt) + one_minus_bias * (m->accel.x * internal->configuration.scale/magAccel); internal->state.timestamp = m->source_timestamp; internal->last_state = internal->state; - printf("source tstamp: %lld\n",m->source_timestamp); + //printf("source tstamp: %lld\n",m->source_timestamp); } //TODO: come up with a way to avoid alloc/free - use max length buffer? free(measurement_array); //convert to a quat for consumption as pose - printf(" integrated %d measurements after %lld X %f Y %f Z %f\n",count,last_timestamp,internal->state.rotation_euler.x,internal->state.rotation_euler.y,internal->state.rotation_euler.z); + //printf(" integrated %d measurements after %lld X %f Y %f Z %f\n",count,last_timestamp,internal->state.rotation_euler.x,internal->state.rotation_euler.y,internal->state.rotation_euler.z); math_euler_to_quat(internal->state.rotation_euler,&internal->state.pose.orientation); *state = internal->state; return true; @@ -108,6 +125,7 @@ filter_complementary_configure(filter_instance_t* inst, filter_complementary_configuration_t* config = (filter_complementary_configuration_t*)config_generic; internal->configuration = *config; + internal->gyro_yaw_correction=0.0f; internal->configured = true; return true; } @@ -126,7 +144,11 @@ filter_complementary_create(filter_instance_t* inst) // we a are a rotational-only filter i->state.has_rotation = true; i->state.has_position = false; - i->state.timestamp = 0; + i->state.timestamp = 0; + i->last_state.rotation_euler.x=0.0f; + i->last_state.rotation_euler.y=0.0f; + i->last_state.rotation_euler.z=0.0f; + i->last_state = i->state; return i; } diff --git a/src/xrt/drivers/montrack/filters/filter_complementary.h b/src/xrt/drivers/montrack/filters/filter_complementary.h index 23ee85eedf1cc9ef470a01dfd87a748d6dc12627..7cec7e9e84541a52dd8d5096700416bb4d43b6f4 100644 --- a/src/xrt/drivers/montrack/filters/filter_complementary.h +++ b/src/xrt/drivers/montrack/filters/filter_complementary.h @@ -4,6 +4,7 @@ #include <xrt/xrt_defines.h> #include "common/filter.h" #include "common/measurementqueue.h" +#include <stdlib.h> typedef struct filter_complementary_configuration { diff --git a/src/xrt/drivers/montrack/filters/filter_opencv_kalman.cpp b/src/xrt/drivers/montrack/filters/filter_opencv_kalman.cpp index 4e4e3f66952734b4da9e8d06f6788e033b9856c5..22b34f536562b3044af6d01dcde1bd615bb7ced0 100644 --- a/src/xrt/drivers/montrack/filters/filter_opencv_kalman.cpp +++ b/src/xrt/drivers/montrack/filters/filter_opencv_kalman.cpp @@ -40,10 +40,11 @@ filter_opencv_kalman_queue(filter_instance_t* inst, filter_opencv_kalman_instance_t* internal = filter_opencv_kalman_instance(inst->internal_instance); 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; - internal->observation.at<float>(2, 0) = measurement->pose.position.z; - internal->kalman_filter.correct(internal->observation); + measurement_queue_add(inst->measurement_queue,measurement); + //internal->observation.at<float>(0, 0) = measurement->pose.position.x; + //internal->observation.at<float>(1, 0) = measurement->pose.position.y; + //internal->observation.at<float>(2, 0) = measurement->pose.position.z; + //internal->kalman_filter.correct(internal->observation); internal->running = true; return false; } @@ -68,6 +69,11 @@ filter_opencv_kalman_predict_state(filter_instance_t* inst, if (!internal->running) { return false; } + //get all our measurements including the last optical frame, + //and run our filter on them to make a prediction + + tracker_measurement_t* measurement_array; + internal->prediction = internal->kalman_filter.predict(); state->has_position = true; state->pose.position.x = internal->prediction.at<float>(0, 0); diff --git a/src/xrt/drivers/montrack/filters/filter_opencv_kalman.h b/src/xrt/drivers/montrack/filters/filter_opencv_kalman.h index d4fa11c95a150edb251a170489c31b0f76298ac8..082503ea9c88bff75cdb2cf6cf52614664bad9c4 100644 --- a/src/xrt/drivers/montrack/filters/filter_opencv_kalman.h +++ b/src/xrt/drivers/montrack/filters/filter_opencv_kalman.h @@ -3,6 +3,7 @@ #include <xrt/xrt_defines.h> #include "common/filter.h" +#include "common/measurementqueue.h" typedef struct opencv_filter_configuration { @@ -10,11 +11,6 @@ typedef struct opencv_filter_configuration float process_noise_cov; } opencv_filter_configuration_t; -typedef struct opencv_kalman_filter_state -{ - struct xrt_pose pose; -} opencv_kalman_filter_state_t; - #ifdef __cplusplus extern "C" { #endif @@ -26,11 +22,11 @@ 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); - +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 @@ -41,7 +37,7 @@ filter_opencv_kalman_predict_state(filter_instance_t* inst, timepoint_ns time); bool filter_opencv_kalman_configure(filter_instance_t* inst, - filter_configuration_ptr config_generic); + filter_configuration_ptr config); #ifdef __cplusplus } // extern "C" diff --git a/src/xrt/drivers/montrack/mt_device.c b/src/xrt/drivers/montrack/mt_device.c index d1bfbb8c62109c9592e26c9c8d6424a3a0fe5d19..803c8d2943d858cb732271d601af202cab6825b8 100644 --- a/src/xrt/drivers/montrack/mt_device.c +++ b/src/xrt/drivers/montrack/mt_device.c @@ -31,7 +31,7 @@ //#IFDEF have_v4l2 #include "frameservers/v4l2/v4l2_frameserver.h" -#include "mt_framequeue.h" +#include <mt_framequeue.h> static void diff --git a/src/xrt/drivers/montrack/mt_framequeue.h b/src/xrt/drivers/montrack/mt_framequeue.h index 4146c88e2039a6bdd89515e0976193c9450d6830..0d19029d9f9b06497edca5e2f23acdb05557a908 100644 --- a/src/xrt/drivers/montrack/mt_framequeue.h +++ b/src/xrt/drivers/montrack/mt_framequeue.h @@ -1,5 +1,5 @@ -#ifndef VECTOR_H -#define VECTOR_H +#ifndef MT_FRAMEQUEUE_H +#define MT_FRAMEQUEUE_H #include <pthread.h> diff --git a/src/xrt/drivers/montrack/optical_tracking/common/tracked_object.h b/src/xrt/drivers/montrack/optical_tracking/common/tracked_object.h index 3ed7423a7fccc26ce21d3b6a39f83ff57d3c8f17..7c4f31519fc381f251d736c283d90fb717ff63f4 100644 --- a/src/xrt/drivers/montrack/optical_tracking/common/tracked_object.h +++ b/src/xrt/drivers/montrack/optical_tracking/common/tracked_object.h @@ -8,6 +8,10 @@ typedef struct tracked_blob struct xrt_vec2 center; float diameter; struct xrt_vec2 velocity; + uint32_t tracking_tag; // a tracker may assign an opaque tag to denote + // object type + uint32_t tracking_id; // a tracker may assign an opaque id to facilitate + // interframe correlation } tracked_blob_t; typedef struct tracked_object diff --git a/src/xrt/drivers/montrack/optical_tracking/common/tracker.h b/src/xrt/drivers/montrack/optical_tracking/common/tracker.h index 19fdb5143924fd7e9f0a0c7bd08b078921bb1eff..2989268adad752415466ab3e317c07003bfd529e 100644 --- a/src/xrt/drivers/montrack/optical_tracking/common/tracker.h +++ b/src/xrt/drivers/montrack/optical_tracking/common/tracker.h @@ -12,12 +12,25 @@ extern "C" { #endif +typedef enum tracker_measurement_flags {MEASUREMENT_NONE=0, + MEASUREMENT_POSITION=1, + MEASUREMENT_ROTATION=2, + MEASUREMENT_RAW_ACCEL=4, + MEASUREMENT_RAW_GYRO=8, + MEASUREMENT_RAW_MAG=16, + MEASUREMENT_OPTICAL=32, + MEASUREMENT_IMU=64, + } tracker_measurement_flags_t; typedef struct tracker_measurement { struct xrt_pose pose; - bool has_position; - bool has_rotation; - timepoint_ns timestamp; + struct xrt_vec3 accel; + struct xrt_vec3 gyro; + struct xrt_vec3 mag; + tracker_measurement_flags_t flags; + int64_t source_timestamp; + uint8_t source_id; + uint64_t source_sequence; } tracker_measurement_t; typedef enum tracker_type diff --git a/src/xrt/drivers/montrack/optical_tracking/tracker3D_sphere_mono.cpp b/src/xrt/drivers/montrack/optical_tracking/tracker3D_sphere_mono.cpp index e1516a308c4431b3d30e5798276e4804dd005524..8d6794a536c14994391ee260f875b9e29dfd1b52 100644 --- a/src/xrt/drivers/montrack/optical_tracking/tracker3D_sphere_mono.cpp +++ b/src/xrt/drivers/montrack/optical_tracking/tracker3D_sphere_mono.cpp @@ -224,8 +224,8 @@ tracker3D_sphere_mono_track(tracker_instance_t* inst) // printf("%f %f %f\n",x,y,z); - m.has_position = true; - m.timestamp = 0; + m.flags = (tracker_measurement_flags_t)(MEASUREMENT_POSITION | MEASUREMENT_OPTICAL); + m.source_timestamp=0; m.pose.position.x = x; m.pose.position.y = y; m.pose.position.z = z; diff --git a/src/xrt/drivers/montrack/optical_tracking/tracker3D_sphere_stereo.cpp b/src/xrt/drivers/montrack/optical_tracking/tracker3D_sphere_stereo.cpp index 793f2553347b769ad2b5ab7992c430cfa7568321..f7294ab16b420f0ce1a934982649951880aeb167 100644 --- a/src/xrt/drivers/montrack/optical_tracking/tracker3D_sphere_stereo.cpp +++ b/src/xrt/drivers/montrack/optical_tracking/tracker3D_sphere_stereo.cpp @@ -477,8 +477,7 @@ tracker3D_sphere_stereo_track(tracker_instance_t* inst) cv::Point3f world_point = world_points[tracked_index]; // create our measurement for the filter - m.has_position = true; - m.has_rotation = false; + m.flags = (tracker_measurement_flags_t)(MEASUREMENT_POSITION | MEASUREMENT_OPTICAL); m.pose.position.x = world_point.x; m.pose.position.y = world_point.y; m.pose.position.z = world_point.z; diff --git a/src/xrt/drivers/psmv/psmv_driver.c b/src/xrt/drivers/psmv/psmv_driver.c index ab89ccfb1d74260cbd6e33e05ba3c20939fac22a..b3ef63910016b9d80f5f63da95aeb3f974b4843d 100644 --- a/src/xrt/drivers/psmv/psmv_driver.c +++ b/src/xrt/drivers/psmv/psmv_driver.c @@ -24,8 +24,12 @@ #include <unistd.h> #include <optical_tracking/common/tracker.h> +#include <filters/filter_complementary.h> + + #include <mt_framequeue.h> #include <pthread.h> +#include <sys/time.h> /* @@ -198,8 +202,13 @@ struct psmv_device tracker_instance_t* tracker; uint32_t tracked_objects; tracked_object_t* tracked_object_array; - uint64_t last_frame_seq; + + filter_instance_t* filter; + + uint64_t last_frame_seq; pthread_t* tracking_thread; + int64_t start_us; + bool print_spew; bool print_debug; @@ -252,8 +261,8 @@ psmv_read_hid(struct psmv_device *psmv) input.buttons |= data.input.buttons[1] << 16; input.buttons |= data.input.buttons[2] << 8; input.buttons |= data.input.buttons[3] & 0xf0; - input.timestamp |= data.input.timestamp_low; - input.timestamp |= data.input.timestamp_high << 8; + input.timestamp = data.input.timestamp_low; + input.timestamp |= data.input.timestamp_high << 8; input.frame[0].trigger = data.input.trigger_f1; psmv_vec3_from_16_be_to_32(&input.frame[0].accel, @@ -266,7 +275,8 @@ psmv_read_hid(struct psmv_device *psmv) psmv_vec3_from_16_be_to_32(&input.frame[1].gyro, &data.input.gyro_f2); - int32_t diff = input.timestamp - psmv->last.timestamp; + int16_t diff = input.timestamp - psmv->last.timestamp; + bool missed = input.seq_no != ((psmv->last.seqno + 1) & 0x0f); psmv->last.trigger = input.frame[1].trigger; @@ -274,6 +284,26 @@ psmv_read_hid(struct psmv_device *psmv) psmv->last.seqno = input.seq_no; psmv->last.buttons = input.buttons; + if (psmv->filter) { + tracker_measurement_t m = {0}; + m.flags = (tracker_measurement_flags_t)(MEASUREMENT_IMU | MEASUREMENT_RAW_ACCEL | MEASUREMENT_RAW_GYRO); + m.accel.x = input.frame[1].accel.x; + m.accel.y = input.frame[1].accel.y; + m.accel.z = input.frame[1].accel.z; + m.gyro.x = input.frame[1].gyro.x; + m.gyro.y = input.frame[1].gyro.y; + m.gyro.z = input.frame[1].gyro.z; + m.source_id = 0; + m.source_sequence = input.seq_no; + + //m.source_timestamp = input.timestamp; + + + psmv->start_us += (diff*10000); + m.source_timestamp = psmv->start_us; + //printf("queueing timestamp %lld %d %d %d\n",m.source_timestamp,input.timestamp,diff,input.seq_no); + psmv->filter->filter_queue(psmv->filter,&m); + } PSMV_SPEW(psmv, "\n\t" @@ -299,6 +329,8 @@ psmv_read_hid(struct psmv_device *psmv) input.timestamp, diff, input.seq_no); } while (true); + + return 0; } @@ -417,8 +449,9 @@ psmv_device_get_tracked_pose(struct xrt_device *xdev, struct psmv_device *psmv = psmv_device(xdev); psmv_read_hid(psmv); - - //TODO: get the latest pose from the tracker and send it down the pipe + filter_state_t fs; + psmv->filter->filter_get_state(psmv->filter,&fs); + out_relation->pose = fs.pose; } static void @@ -525,6 +558,17 @@ psmv_found(struct xrt_prober *xp, psmv->tracker = tracker_create(TRACKER_TYPE_SPHERE_STEREO); + psmv->filter = filter_create(FILTER_TYPE_COMPLEMENTARY); + + filter_complementary_configuration_t config; + config.bias = 0.1f; + config.scale = 3.14159 / 2.0f; + config.max_timestamp=65535; //ps move timestamps are 16 bit + + psmv->filter->filter_configure(psmv->filter,&config); + + //send our optical measurements to the filter + psmv->tracker->tracker_register_measurement_callback(psmv->tracker,psmv->filter,psmv->filter->filter_queue); // if we want to generate a calibration, we can use this calibration tracker, // instead of the above. this will move to a standalone application. @@ -536,6 +580,9 @@ psmv_found(struct xrt_prober *xp, // initialise some info about how many objects are tracked, and the initial frame seq. psmv->tracker->tracker_get_poses(psmv->tracker,NULL,&psmv->tracked_objects); psmv->last_frame_seq = 0; + struct timeval tp; + gettimeofday(&tp,NULL); + psmv->start_us = tp.tv_sec * 1000000 + tp.tv_usec; pthread_create(&psmv->tracking_thread,NULL,psmv_tracking_run,psmv); // And finally done @@ -549,6 +596,7 @@ void* psmv_tracking_run(void* arg) { frame_t f; //TODO: orderly shutdown while(1) { + if (frame_queue_ref_latest(fq,0,&f)) { if (! psmv->tracker->configured) diff --git a/src/xrt/targets/openxr/make_manifest.cmake b/src/xrt/targets/openxr/make_manifest.cmake new file mode 100644 index 0000000000000000000000000000000000000000..c5a4e6e21b7d871e5e97f184680bebf20958d0b3 --- /dev/null +++ b/src/xrt/targets/openxr/make_manifest.cmake @@ -0,0 +1,31 @@ +# SPDX-License-Identifier: BSL-1.0 + +# Get input from main CMake script +set(MANIFEST_INPUT @MANIFEST_INPUT@) +set(MANIFEST_RELATIVE_DIR @MANIFEST_RELATIVE_DIR@) +set(RUNTIME_RELATIVE_DIR @RUNTIME_RELATIVE_DIR@) +set(RUNTIME_FILENAME @RUNTIME_FILENAME@) +set(OPENXR_INSTALL_ABSOLUTE_RUNTIME_PATH @OPENXR_INSTALL_ABSOLUTE_RUNTIME_PATH@) + +# Remove trailing slash +string(REGEX REPLACE "/$" "" MANIFEST_RELATIVE_DIR "${MANIFEST_RELATIVE_DIR}") + +if(OPENXR_INSTALL_ABSOLUTE_RUNTIME_PATH) + # Absolute path to runtime + set(RUNTIME_PATH ${CMAKE_INSTALL_PREFIX}/${RUNTIME_RELATIVE_DIR}/${RUNTIME_FILENAME}) +else() + # Relative path to runtime: requires it exist on the system shared library search path. + set(RUNTIME_PATH ${RUNTIME_FILENAME}) +endif() + +if("x${CMAKE_INSTALL_COMPONENT}x" STREQUAL "xUnspecifiedx" OR NOT CMAKE_INSTALL_COMPONENT) + + # Create manifest + configure_file(${MANIFEST_INPUT} ${CMAKE_CURRENT_LIST_DIR}/@RUNTIME_TARGET@.json) + + # Install it + file(INSTALL + DESTINATION "${CMAKE_INSTALL_PREFIX}/${MANIFEST_RELATIVE_DIR}" + TYPE FILE + FILES "${CMAKE_CURRENT_LIST_DIR}/@RUNTIME_TARGET@.json") +endif() diff --git a/src/xrt/targets/openxr/openxr_monado.in.json b/src/xrt/targets/openxr/openxr_monado.in.json new file mode 100644 index 0000000000000000000000000000000000000000..c1a469e098e3b6bfb5fdd755248f13bdee0711f0 --- /dev/null +++ b/src/xrt/targets/openxr/openxr_monado.in.json @@ -0,0 +1,6 @@ +{ + "file_format_version": "1.0.0", + "runtime": { + "library_path": "${RUNTIME_PATH}" + } +}