t/slam: Handle multiple camera sinks · matrixfurry.com/monado@d8e1b7d

+73 -58

src/xrt/auxiliary/tracking/t_tracker_slam.cpp

··· 37 37 #include <iomanip> 38 38 #include <map> 39 39 #include <string> 40 + #include <vector> 40 41 41 42 #define SLAM_TRACE(...) U_LOG_IFL_T(t.log_level, __VA_ARGS__) 42 43 #define SLAM_DEBUG(...) U_LOG_IFL_D(t.log_level, __VA_ARGS__) ··· 73 74 DEBUG_GET_ONCE_OPTION(slam_csv_path, "SLAM_CSV_PATH", "evaluation/") 74 75 DEBUG_GET_ONCE_BOOL_OPTION(slam_timing_stat, "SLAM_TIMING_STAT", true) 75 76 DEBUG_GET_ONCE_BOOL_OPTION(slam_features_stat, "SLAM_FEATURES_STAT", true) 77 + DEBUG_GET_ONCE_NUM_OPTION(slam_cam_count, "SLAM_CAM_COUNT", 2) 76 78 77 79 //! Namespace for the interface to the external SLAM tracking system 78 80 namespace xrt::auxiliary::tracking::slam { ··· 346 348 struct xrt_frame_node node = {}; //!< Will be called on destruction 347 349 slam_tracker *slam; //!< Pointer to the external SLAM system implementation 348 350 349 - struct xrt_slam_sinks sinks = {}; //!< Pointers to the sinks below 350 - struct xrt_frame_sink left_sink = {}; //!< Sends left camera frames to the SLAM system 351 - struct xrt_frame_sink right_sink = {}; //!< Sends right camera frames to the SLAM system 352 - struct xrt_imu_sink imu_sink = {}; //!< Sends imu samples to the SLAM system 353 - struct xrt_pose_sink gt_sink = {}; //!< Register groundtruth trajectory for stats 354 - bool submit; //!< Whether to submit data pushed to sinks to the SLAM tracker 351 + struct xrt_slam_sinks sinks = {}; //!< Pointers to the sinks below 352 + struct xrt_frame_sink cam_sinks[XRT_TRACKING_MAX_SLAM_CAMS]; //!< Sends camera frames to the SLAM system 353 + struct xrt_imu_sink imu_sink = {}; //!< Sends imu samples to the SLAM system 354 + struct xrt_pose_sink gt_sink = {}; //!< Register groundtruth trajectory for stats 355 + bool submit; //!< Whether to submit data pushed to sinks to the SLAM tracker 356 + int cam_count; //!< Number of cameras used for tracking 355 357 356 358 enum u_logging_level log_level; //!< Logging level for the SLAM tracker, set by SLAM_LOG var 357 359 struct os_thread_helper oth; //!< Thread where the external SLAM system runs ··· 360 362 struct xrt_slam_sinks *euroc_recorder; //!< EuRoC dataset recording sinks 361 363 362 364 // Used mainly for checking that the timestamps come in order 363 - timepoint_ns last_imu_ts = INT64_MIN; //!< Last received IMU sample timestamp 364 - timepoint_ns last_left_ts = INT64_MIN; //!< Last received left image timestamp 365 - timepoint_ns last_right_ts = INT64_MIN; //!< Last received right image timestamp 365 + timepoint_ns last_imu_ts; //!< Last received IMU sample timestamp 366 + vector<timepoint_ns> last_cam_ts; //!< Last received image timestamp per cam 366 367 367 368 // Prediction 368 369 369 370 //! Type of prediction to use 370 371 t_slam_prediction_type pred_type; 371 - u_var_combo pred_combo; //!< UI combo box to select @ref pred_type 372 - RelationHistory slam_rels{}; //!< A history of relations produced purely from external SLAM tracker data 373 - struct m_ff_vec3_f32 *gyro_ff; //!< Last gyroscope samples 374 - struct m_ff_vec3_f32 *accel_ff; //!< Last accelerometer samples 375 - struct u_sink_debug ui_left_sink; //!< Sink to display left frames in UI 376 - struct u_sink_debug ui_right_sink; //!< Sink to display right frames in UI 372 + u_var_combo pred_combo; //!< UI combo box to select @ref pred_type 373 + RelationHistory slam_rels{}; //!< A history of relations produced purely from external SLAM tracker data 374 + struct m_ff_vec3_f32 *gyro_ff; //!< Last gyroscope samples 375 + struct m_ff_vec3_f32 *accel_ff; //!< Last accelerometer samples 376 + vector<u_sink_debug> ui_sink; //!< Sink to display frames in UI of each camera 377 377 378 378 //! Used to correct accelerometer measurements when integrating into the prediction. 379 379 //! @todo Should be automatically computed instead of required to be filled manually through the UI. ··· 621 621 return {time_ns_to_s(now - ts), double(count)}; 622 622 }; 623 623 624 - t.features.fcs_ui.curve_count = NUM_CAMS; 624 + t.features.fcs_ui.curve_count = t.cam_count; 625 625 t.features.fcs_ui.xlabel = "Last seconds"; 626 626 t.features.fcs_ui.ylabel = "Number of features"; 627 627 628 - t.features.fcs.resize(NUM_CAMS); 629 - for (int i = 0; i < NUM_CAMS; i++) { 628 + t.features.fcs.resize(t.cam_count); 629 + for (int i = 0; i < t.cam_count; i++) { 630 630 auto &fc = t.features.fcs[i]; 631 631 fc.cam_name = "Cam" + to_string(i); 632 632 ··· 956 956 t.pred_combo.count = SLAM_PRED_COUNT; 957 957 t.pred_combo.options = "None\0Interpolate SLAM poses\0Also gyro\0Also accel (needs gravity correction)\0\0"; 958 958 t.pred_combo.value = (int *)&t.pred_type; 959 - u_sink_debug_init(&t.ui_left_sink); 960 - u_sink_debug_init(&t.ui_right_sink); 959 + t.ui_sink = vector<u_sink_debug>(t.cam_count); 960 + for (size_t i = 0; i < t.ui_sink.size(); i++) { 961 + u_sink_debug_init(&t.ui_sink[i]); 962 + } 961 963 m_ff_vec3_f32_alloc(&t.gyro_ff, 1000); 962 964 m_ff_vec3_f32_alloc(&t.accel_ff, 1000); 963 965 m_ff_vec3_f32_alloc(&t.filter.pos_ff, 1000); ··· 987 989 u_var_add_ro_ff_vec3_f32(&t, t.gyro_ff, "Gyroscope"); 988 990 u_var_add_ro_ff_vec3_f32(&t, t.accel_ff, "Accelerometer"); 989 991 u_var_add_f32(&t, &t.gravity_correction.z, "Gravity Correction"); 990 - u_var_add_sink_debug(&t, &t.ui_left_sink, "Left Camera"); 991 - u_var_add_sink_debug(&t, &t.ui_right_sink, "Right Camera"); 992 + for (size_t i = 0; i < t.ui_sink.size(); i++) { 993 + char label[] = "Camera NNNN"; 994 + (void)snprintf(label, sizeof(label), "Camera %zu", i); 995 + u_var_add_sink_debug(&t, &t.ui_sink[i], label); 996 + } 992 997 993 998 u_var_add_gui_header(&t, NULL, "Stats"); 994 999 u_var_add_ro_ftext(&t, "\n%s", "Record to CSV files"); ··· 1115 1120 } 1116 1121 } 1117 1122 1118 - 1119 1123 } // namespace xrt::auxiliary::tracking::slam 1120 1124 1121 1125 using namespace xrt::auxiliary::tracking::slam; ··· 1171 1175 1172 1176 //! Receive and send IMU samples to the external SLAM system 1173 1177 extern "C" void 1174 - t_slam_imu_sink_push(struct xrt_imu_sink *sink, struct xrt_imu_sample *s) 1178 + t_slam_receive_imu(struct xrt_imu_sink *sink, struct xrt_imu_sample *s) 1175 1179 { 1176 1180 auto &t = *container_of(sink, TrackerSlam, imu_sink); 1177 1181 ··· 1201 1205 1202 1206 //! Push the frame to the external SLAM system 1203 1207 static void 1204 - push_frame(TrackerSlam &t, struct xrt_frame *frame, bool is_left) 1208 + receive_frame(TrackerSlam &t, struct xrt_frame *frame, int cam_index) 1205 1209 { 1206 - SLAM_DASSERT(t.last_left_ts != INT64_MIN || is_left, "First frame was a right frame"); 1210 + SLAM_DASSERT(t.last_cam_ts[0] != INT64_MIN || cam_index == 0, "First frame was not a cam0 frame"); 1207 1211 1208 1212 // Construct and send the image sample 1209 1213 cv::Mat img = t.cv_wrapper->wrap(frame); 1210 1214 SLAM_DASSERT_(frame->timestamp < INT64_MAX); 1211 - img_sample sample{(int64_t)frame->timestamp, img, is_left}; 1215 + img_sample sample{(int64_t)frame->timestamp, img, cam_index}; 1212 1216 if (t.submit) { 1213 1217 t.slam->push_frame(sample); 1214 1218 } 1215 - SLAM_TRACE("%s frame t=%lu", is_left ? " left" : "right", frame->timestamp); 1219 + SLAM_TRACE("cam%d frame t=%lu", cam_index, frame->timestamp); 1216 1220 1217 1221 // Check monotonically increasing timestamps 1218 - timepoint_ns &last_ts = is_left ? t.last_left_ts : t.last_right_ts; 1222 + timepoint_ns &last_ts = t.last_cam_ts[cam_index]; 1219 1223 SLAM_DASSERT(sample.timestamp > last_ts, "Frame (%ld) is older than last (%ld)", sample.timestamp, last_ts); 1220 1224 last_ts = sample.timestamp; 1221 1225 } 1222 1226 1223 - extern "C" void 1224 - t_slam_frame_sink_push_left(struct xrt_frame_sink *sink, struct xrt_frame *frame) 1225 - { 1226 - auto &t = *container_of(sink, TrackerSlam, left_sink); 1227 - int cam_id = 0; 1228 - push_frame(t, frame, cam_id); 1229 - u_sink_debug_push_frame(&t.ui_left_sink, frame); 1230 - xrt_sink_push_frame(t.euroc_recorder->cams[0], frame); 1231 - } 1227 + #define DEFINE_RECEIVE_CAM(cam_id) \ 1228 + extern "C" void t_slam_receive_cam##cam_id(struct xrt_frame_sink *sink, struct xrt_frame *frame) \ 1229 + { \ 1230 + auto &t = *container_of(sink, TrackerSlam, cam_sinks[cam_id]); \ 1231 + receive_frame(t, frame, cam_id); \ 1232 + u_sink_debug_push_frame(&t.ui_sink[cam_id], frame); \ 1233 + xrt_sink_push_frame(t.euroc_recorder->cams[cam_id], frame); \ 1234 + } 1235 + 1236 + DEFINE_RECEIVE_CAM(0) 1237 + DEFINE_RECEIVE_CAM(1) 1238 + DEFINE_RECEIVE_CAM(2) 1239 + DEFINE_RECEIVE_CAM(3) 1240 + DEFINE_RECEIVE_CAM(4) 1232 1241 1233 - extern "C" void 1234 - t_slam_frame_sink_push_right(struct xrt_frame_sink *sink, struct xrt_frame *frame) 1235 - { 1236 - auto &t = *container_of(sink, TrackerSlam, right_sink); 1237 - int cam_id = 1; 1238 - push_frame(t, frame, cam_id); 1239 - u_sink_debug_push_frame(&t.ui_right_sink, frame); 1240 - xrt_sink_push_frame(t.euroc_recorder->cams[1], frame); 1241 - } 1242 + //! Define a function for each XRT_TRACKING_MAX_SLAM_CAMS and reference it in this array 1243 + void (*t_slam_receive_cam[XRT_TRACKING_MAX_SLAM_CAMS])(xrt_frame_sink *, xrt_frame *) = { 1244 + t_slam_receive_cam0, // 1245 + t_slam_receive_cam1, // 1246 + t_slam_receive_cam2, // 1247 + t_slam_receive_cam3, // 1248 + t_slam_receive_cam4, // 1249 + }; 1242 1250 1243 1251 extern "C" void 1244 1252 t_slam_node_break_apart(struct xrt_frame_node *node) ··· 1264 1272 delete t.pred_traj_writer; 1265 1273 delete t.filt_traj_writer; 1266 1274 u_var_remove_root(t_ptr); 1267 - u_sink_debug_destroy(&t.ui_left_sink); 1268 - u_sink_debug_destroy(&t.ui_right_sink); 1275 + for (size_t i = 0; i < t.ui_sink.size(); i++) { 1276 + u_sink_debug_destroy(&t.ui_sink[i]); 1277 + } 1269 1278 m_ff_vec3_f32_free(&t.gyro_ff); 1270 1279 m_ff_vec3_f32_free(&t.accel_ff); 1271 1280 m_ff_vec3_f32_free(&t.filter.pos_ff); ··· 1308 1317 config->csv_path = debug_get_option_slam_csv_path(); 1309 1318 config->timing_stat = debug_get_bool_option_slam_timing_stat(); 1310 1319 config->features_stat = debug_get_bool_option_slam_features_stat(); 1311 - config->stereo_calib = NULL; 1320 + config->cam_count = int(debug_get_num_option_slam_cam_count()); 1312 1321 config->imu_calib = NULL; 1313 1322 config->extra_calib = NULL; 1314 1323 } ··· 1357 1366 1358 1367 slam_config system_config = {}; 1359 1368 system_config.config_file = config_file ? make_shared<string>(config_file) : nullptr; 1360 - system_config.cam_count = NUM_CAMS; 1369 + system_config.cam_count = config->cam_count; 1361 1370 system_config.show_ui = config->slam_ui; 1362 1371 t.slam = new slam_tracker{system_config}; 1363 1372 ··· 1370 1379 1371 1380 t.slam->initialize(); 1372 1381 1373 - t.left_sink.push_frame = t_slam_frame_sink_push_left; 1374 - t.right_sink.push_frame = t_slam_frame_sink_push_right; 1375 - t.imu_sink.push_imu = t_slam_imu_sink_push; 1376 - t.gt_sink.push_pose = t_slam_gt_sink_push; 1382 + SLAM_ASSERT(t_slam_receive_cam[ARRAY_SIZE(t_slam_receive_cam) - 1] != nullptr, "See `cam_sink_push` docs"); 1383 + t.sinks.cam_count = config->cam_count; 1384 + for (int i = 0; i < XRT_TRACKING_MAX_SLAM_CAMS; i++) { 1385 + t.cam_sinks[i].push_frame = t_slam_receive_cam[i]; 1386 + t.sinks.cams[i] = &t.cam_sinks[i]; 1387 + } 1377 1388 1378 - t.sinks.cam_count = NUM_CAMS; 1379 - t.sinks.cams[0] = &t.left_sink; 1380 - t.sinks.cams[1] = &t.right_sink; 1389 + t.imu_sink.push_imu = t_slam_receive_imu; 1381 1390 t.sinks.imu = &t.imu_sink; 1391 + 1392 + t.gt_sink.push_pose = t_slam_gt_sink_push; 1382 1393 t.sinks.gt = &t.gt_sink; 1383 1394 1384 1395 t.submit = config->submit_from_start; 1396 + t.cam_count = config->cam_count; 1385 1397 1386 1398 t.node.break_apart = t_slam_node_break_apart; 1387 1399 t.node.destroy = t_slam_node_destroy; ··· 1392 1404 xrt_frame_context_add(xfctx, &t.node); 1393 1405 1394 1406 t.euroc_recorder = euroc_recorder_create(xfctx, NULL, false); 1407 + 1408 + t.last_imu_ts = INT64_MIN; 1409 + t.last_cam_ts = vector<timepoint_ns>(t.cam_count, INT64_MIN); 1395 1410 1396 1411 t.pred_type = config->prediction; 1397 1412

+1

src/xrt/auxiliary/tracking/t_tracking.h

··· 626 626 { 627 627 enum u_logging_level log_level; //!< SLAM tracking logging level 628 628 const char *slam_config; //!< Config file path, format is specific to the SLAM implementation in use 629 + int cam_count; //!< Number of cameras in use 629 630 bool slam_ui; //!< Whether to open the external UI of the external SLAM system 630 631 bool submit_from_start; //!< Whether to submit data to the SLAM tracker without user action 631 632 enum t_slam_prediction_type prediction; //!< Which level of prediction to use

+1 -1

src/xrt/drivers/rift_s/rift_s_tracker.c

··· 211 211 t_slam_fill_default_config(&config); 212 212 213 213 /* No need to refcount these parameters */ 214 - config.stereo_calib = t->stereo_calib; 214 + config.cam_count = 2; 215 215 config.imu_calib = &t->slam_imu_calib; 216 216 config.extra_calib = &t->slam_extra_calib; 217 217

+1 -1

src/xrt/drivers/wmr/wmr_hmd.c

··· 1462 1462 #ifdef XRT_FEATURE_SLAM 1463 1463 struct t_slam_tracker_config config = {0}; 1464 1464 t_slam_fill_default_config(&config); 1465 - config.stereo_calib = stereo_calib; // No need to do refcount here 1465 + config.cam_count = 2; 1466 1466 config.imu_calib = imu_calib; 1467 1467 config.extra_calib = extra_calib; 1468 1468 if (debug_get_option_slam_submit_from_start() == NULL) {

+4 -1

src/xrt/state_trackers/prober/p_tracking.c

··· 255 255 assert(fact->xfs->source_id == 0xECD0FEED && "xfs is not Euroc, unsynced open_video_device?"); 256 256 257 257 #ifdef XRT_FEATURE_SLAM 258 - int ret = t_slam_create(&fact->xfctx, NULL, &fact->xts, &sinks); 258 + struct t_slam_tracker_config st_config; 259 + t_slam_fill_default_config(&st_config); 260 + 261 + int ret = t_slam_create(&fact->xfctx, &st_config, &fact->xts, &sinks); 259 262 if (ret != 0) { 260 263 U_LOG_W("Unable to initialize SLAM tracking, the Euroc driver will not be tracked"); 261 264 }

+1

src/xrt/targets/common/target_builder_lighthouse.c

··· 162 162 163 163 struct t_slam_tracker_config config = {0}; 164 164 t_slam_fill_default_config(&config); 165 + config.cam_count = 2; 165 166 config.stereo_calib = stereo_calib; // Won't hold stereo_calib so no refcount 166 167 config.imu_calib = imu_calib; 167 168 config.extra_calib = extra_calib;

Configure Feed

Configure Feed