The open source OpenXR runtime
t/slam: Implement new prediction using latest IMU samples for everything
authored by
Mateo de Mayo
and committed by
Jakob Bornecrantz
a860a7f4
17fcaed1
+121
-4
+119
-4
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
+119
-4
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
···
72
72
DEBUG_GET_ONCE_BOOL_OPTION(slam_ui, "SLAM_UI", false)
73
73
DEBUG_GET_ONCE_BOOL_OPTION(slam_submit_from_start, "SLAM_SUBMIT_FROM_START", false)
74
74
DEBUG_GET_ONCE_NUM_OPTION(slam_openvr_groundtruth_device, "SLAM_OPENVR_GROUNDTRUTH_DEVICE", 0)
75
-
DEBUG_GET_ONCE_NUM_OPTION(slam_prediction_type, "SLAM_PREDICTION_TYPE", long(SLAM_PRED_SP_SO_IA_IL))
75
+
DEBUG_GET_ONCE_NUM_OPTION(slam_prediction_type, "SLAM_PREDICTION_TYPE", long(SLAM_PRED_IP_IO_IA_IL))
76
76
DEBUG_GET_ONCE_BOOL_OPTION(slam_write_csvs, "SLAM_WRITE_CSVS", false)
77
77
DEBUG_GET_ONCE_OPTION(slam_csv_path, "SLAM_CSV_PATH", "evaluation/")
78
78
DEBUG_GET_ONCE_BOOL_OPTION(slam_timing_stat, "SLAM_TIMING_STAT", true)
···
374
374
t_slam_prediction_type pred_type;
375
375
u_var_combo pred_combo; //!< UI combo box to select @ref pred_type
376
376
RelationHistory slam_rels{}; //!< A history of relations produced purely from external SLAM tracker data
377
+
int dbg_pred_every = 1; //!< Skip X SLAM poses so that you get tracked mostly by the prediction algo
378
+
int dbg_pred_counter = 0; //!< SLAM pose counter for prediction debugging
379
+
struct os_mutex lock_ff; //!< Lock for gyro_ff and accel_ff.
377
380
struct m_ff_vec3_f32 *gyro_ff; //!< Last gyroscope samples
378
381
struct m_ff_vec3_f32 *accel_ff; //!< Last accelerometer samples
379
382
vector<u_sink_debug> ui_sink; //!< Sink to display frames in UI of each camera
···
817
820
rel.linear_velocity = (npos - lpos) / dt;
818
821
math_quat_finite_difference(&lrot, &nrot, dt, &rel.angular_velocity);
819
822
820
-
t.slam_rels.push(rel, nts);
823
+
// Push to relationship history unless we are debugging prediction
824
+
if (t.dbg_pred_counter % t.dbg_pred_every == 0) {
825
+
t.slam_rels.push(rel, nts);
826
+
}
827
+
t.dbg_pred_counter = (t.dbg_pred_counter + 1) % t.dbg_pred_every;
821
828
822
829
gt_ui_push(t, nts, rel.pose);
823
830
t.slam_traj_writer->push(nts, rel.pose);
831
+
xrt_pose_sample pose_sample = {nts, rel.pose};
832
+
xrt_sink_push_pose(t.euroc_recorder->gt, &pose_sample);
824
833
825
834
// Push even if timing extension is disabled
826
835
auto tss = timing_ui_push(t, np);
···
841
850
return got_one;
842
851
}
843
852
853
+
//! Integrates IMU samples on top of a base pose and predicts from that
854
+
static void
855
+
predict_pose_from_imu(TrackerSlam &t,
856
+
timepoint_ns when_ns,
857
+
xrt_space_relation base_rel, // Pose to integrate IMUs on top of
858
+
timepoint_ns base_rel_ts,
859
+
struct xrt_space_relation *out_relation)
860
+
{
861
+
os_mutex_lock(&t.lock_ff);
862
+
863
+
// Find oldest imu index i that is newer than latest SLAM pose (or -1)
864
+
int i = 0;
865
+
uint64_t imu_ts = UINT64_MAX;
866
+
xrt_vec3 _;
867
+
while (m_ff_vec3_f32_get(t.gyro_ff, i, &_, &imu_ts)) {
868
+
if ((int64_t)imu_ts < base_rel_ts) {
869
+
i--; // Back to the oldest newer-than-SLAM IMU index (or -1)
870
+
break;
871
+
}
872
+
i++;
873
+
}
874
+
875
+
if (i == -1) {
876
+
SLAM_WARN("No IMU samples received after latest SLAM pose (and frame)");
877
+
}
878
+
879
+
xrt_space_relation integ_rel = base_rel;
880
+
timepoint_ns integ_rel_ts = base_rel_ts;
881
+
xrt_quat &o = integ_rel.pose.orientation;
882
+
xrt_vec3 &p = integ_rel.pose.position;
883
+
xrt_vec3 &w = integ_rel.angular_velocity;
884
+
xrt_vec3 &v = integ_rel.linear_velocity;
885
+
bool clamped = false; // If when_ns is older than the latest IMU ts
886
+
887
+
while (i >= 0) { // Decreasing i increases timestamp
888
+
// Get samples
889
+
xrt_vec3 g{};
890
+
xrt_vec3 a{};
891
+
uint64_t g_ts{};
892
+
uint64_t a_ts{};
893
+
bool got = true;
894
+
got &= m_ff_vec3_f32_get(t.gyro_ff, i, &g, &g_ts);
895
+
got &= m_ff_vec3_f32_get(t.accel_ff, i, &a, &a_ts);
896
+
timepoint_ns ts = g_ts;
897
+
898
+
899
+
// Checks
900
+
if (ts > when_ns) {
901
+
clamped = true;
902
+
//! @todo Instead of using same a and g values, do an interpolated sample like this:
903
+
// a = prev_a + ((when_ns - prev_ts) / (ts - prev_ts)) * (a - prev_a);
904
+
// g = prev_g + ((when_ns - prev_ts) / (ts - prev_ts)) * (g - prev_g);
905
+
ts = when_ns; // clamp ts to when_ns
906
+
}
907
+
SLAM_DASSERT(got && g_ts == a_ts, "Failure getting synced gyro and accel samples");
908
+
SLAM_DASSERT(ts >= base_rel_ts, "Accessing imu sample that is older than latest SLAM pose");
909
+
910
+
// Update time
911
+
float dt = (float)time_ns_to_s(ts - integ_rel_ts);
912
+
integ_rel_ts = ts;
913
+
914
+
// Integrate gyroscope
915
+
xrt_quat angvel_delta{};
916
+
xrt_vec3 scaled_half_g = g * dt * 0.5f;
917
+
math_quat_exp(&scaled_half_g, &angvel_delta); // Same as using math_quat_from_angle_vector(g/dt)
918
+
math_quat_rotate(&o, &angvel_delta, &o); // Orientation
919
+
math_quat_rotate_derivative(&o, &g, &w); // Angular velocity
920
+
921
+
// Integrate accelerometer
922
+
xrt_vec3 world_accel{};
923
+
math_quat_rotate_vec3(&o, &a, &world_accel);
924
+
world_accel += t.gravity_correction;
925
+
v += world_accel * dt; // Linear velocity
926
+
p += v * dt + world_accel * (dt * dt * 0.5f); // Position
927
+
928
+
if (clamped) {
929
+
break;
930
+
}
931
+
i--;
932
+
}
933
+
934
+
os_mutex_unlock(&t.lock_ff);
935
+
936
+
// Do the prediction based on the updated relation
937
+
double last_imu_to_now_dt = time_ns_to_s(when_ns - integ_rel_ts);
938
+
xrt_space_relation predicted_relation{};
939
+
m_predict_relation(&integ_rel, last_imu_to_now_dt, &predicted_relation);
940
+
941
+
*out_relation = predicted_relation;
942
+
}
943
+
844
944
//! Return our best guess of the relation at time @p when_ns using all the data the tracker has.
845
945
static void
846
946
predict_pose(TrackerSlam &t, timepoint_ns when_ns, struct xrt_space_relation *out_relation)
847
947
{
848
948
XRT_TRACE_MARKER();
849
949
850
-
bool valid_pred_type = t.pred_type >= SLAM_PRED_NONE && t.pred_type <= SLAM_PRED_SP_SO_IA_IL;
950
+
bool valid_pred_type = t.pred_type >= SLAM_PRED_NONE && t.pred_type < SLAM_PRED_COUNT;
851
951
SLAM_DASSERT(valid_pred_type, "Invalid prediction type (%d)", t.pred_type);
852
952
853
953
// Get last relation computed purely from SLAM data
···
874
974
return;
875
975
}
876
976
977
+
978
+
if (t.pred_type == SLAM_PRED_IP_IO_IA_IL) {
979
+
predict_pose_from_imu(t, when_ns, rel, (int64_t)rel_ts, out_relation);
980
+
return;
981
+
}
982
+
983
+
os_mutex_lock(&t.lock_ff);
984
+
877
985
// Update angular velocity with gyro data
878
986
if (t.pred_type >= SLAM_PRED_SP_SO_IA_SL) {
879
987
xrt_vec3 avg_gyro{};
···
891
999
double slam_to_imu_dt = time_ns_to_s(t.last_imu_ts - rel_ts);
892
1000
rel.linear_velocity += world_accel * slam_to_imu_dt;
893
1001
}
1002
+
1003
+
os_mutex_unlock(&t.lock_ff);
894
1004
895
1005
// Do the prediction based on the updated relation
896
1006
double slam_to_now_dt = time_ns_to_s(when_ns - rel_ts);
···
959
1069
setup_ui(TrackerSlam &t)
960
1070
{
961
1071
t.pred_combo.count = SLAM_PRED_COUNT;
962
-
t.pred_combo.options = "None\0Interpolate SLAM poses\0Also gyro\0Also accel (needs gravity correction)\0\0";
1072
+
t.pred_combo.options = "None\0Interpolate SLAM poses\0Also gyro\0Also accel\0Latest IMU\0";
963
1073
t.pred_combo.value = (int *)&t.pred_type;
964
1074
t.ui_sink = vector<u_sink_debug>(t.cam_count);
965
1075
for (size_t i = 0; i < t.ui_sink.size(); i++) {
966
1076
u_sink_debug_init(&t.ui_sink[i]);
967
1077
}
1078
+
os_mutex_init(&t.lock_ff);
968
1079
m_ff_vec3_f32_alloc(&t.gyro_ff, 1000);
969
1080
m_ff_vec3_f32_alloc(&t.accel_ff, 1000);
970
1081
m_ff_vec3_f32_alloc(&t.filter.pos_ff, 1000);
···
991
1102
992
1103
u_var_add_gui_header(&t, NULL, "Prediction");
993
1104
u_var_add_combo(&t, &t.pred_combo, "Prediction Type");
1105
+
u_var_add_i32(&t, &t.dbg_pred_every, "Debug prediction skips (try 30)");
994
1106
u_var_add_ro_ff_vec3_f32(&t, t.gyro_ff, "Gyroscope");
995
1107
u_var_add_ro_ff_vec3_f32(&t, t.accel_ff, "Accelerometer");
996
1108
u_var_add_f32(&t, &t.gravity_correction.z, "Gravity Correction");
···
1208
1320
1209
1321
struct xrt_vec3 gyro = {(float)w.x, (float)w.y, (float)w.z};
1210
1322
struct xrt_vec3 accel = {(float)a.x, (float)a.y, (float)a.z};
1323
+
os_mutex_lock(&t.lock_ff);
1211
1324
m_ff_vec3_f32_push(t.gyro_ff, &gyro, ts);
1212
1325
m_ff_vec3_f32_push(t.accel_ff, &accel, ts);
1326
+
os_mutex_unlock(&t.lock_ff);
1213
1327
}
1214
1328
1215
1329
//! Push the frame to the external SLAM system
···
1298
1412
}
1299
1413
m_ff_vec3_f32_free(&t.gyro_ff);
1300
1414
m_ff_vec3_f32_free(&t.accel_ff);
1415
+
os_mutex_destroy(&t.lock_ff);
1301
1416
m_ff_vec3_f32_free(&t.filter.pos_ff);
1302
1417
m_ff_vec3_f32_free(&t.filter.rot_ff);
1303
1418
delete t_ptr->slam;
+2
src/xrt/auxiliary/tracking/t_tracking.h
+2
src/xrt/auxiliary/tracking/t_tracking.h
···
599
599
SLAM_PRED_SP_SO_SA_SL, //!< Predicts from last two SLAM poses only
600
600
SLAM_PRED_SP_SO_IA_SL, //!< Predicts from last SLAM pose with angular velocity computed from IMU
601
601
SLAM_PRED_SP_SO_IA_IL, //!< Predicts from last SLAM pose with angular and linear velocity computed from IMU
602
+
SLAM_PRED_IP_IO_IA_IL, //!< Predicts from a pose that is the last SLAM pose with the IMU samples that came after
603
+
//!< it integrated on top; velocities from latest IMU sample.
602
604
SLAM_PRED_COUNT,
603
605
};
604
606