matrixfurry.com / monado
The open source OpenXR runtime
0
fork

Configure Feed

Select the types of activity you want to include in your feed.

t/slam: Adapt camera calibration data for multiple cameras

authored by

Mateo de Mayo and committed by
Jakob Bornecrantz 186ca825 d8e1b7d9

+365 -300
+73 -78
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
··· 81 81 constexpr int UI_TIMING_POSE_COUNT = 192; 82 82 constexpr int UI_FEATURES_POSE_COUNT = 192; 83 83 constexpr int UI_GTDIFF_POSE_COUNT = 192; 84 - constexpr int NUM_CAMS = 2; //!< This should be used as little as possible to allow setups that are not stereo 85 84 86 85 using std::deque; 87 86 using std::ifstream; ··· 1008 1007 } 1009 1008 1010 1009 static void 1011 - add_camera_calibration(const TrackerSlam &t, 1012 - const t_stereo_camera_calibration *stereo_calib, 1013 - const t_slam_calib_extras *extra_calib) 1010 + add_camera_calibration(const TrackerSlam &t, const t_slam_camera_calibration *calib, int cam_index) 1014 1011 { 1015 - for (int i = 0; i < NUM_CAMS; i++) { 1016 - const t_camera_calibration &view = stereo_calib->view[i]; 1017 - const auto &extra = extra_calib->cams[i]; 1018 - const auto params = make_shared<FPARAMS_ACC>(); 1012 + const t_camera_calibration &view = calib->base; 1013 + const auto params = make_shared<FPARAMS_ACC>(); 1019 1014 1020 - params->cam_index = i; 1021 - params->width = view.image_size_pixels.w; 1022 - params->height = view.image_size_pixels.h; 1023 - params->frequency = extra.frequency; 1015 + params->cam_index = cam_index; 1016 + params->width = view.image_size_pixels.w; 1017 + params->height = view.image_size_pixels.h; 1018 + params->frequency = calib->frequency; 1024 1019 1025 - params->fx = view.intrinsics[0][0]; 1026 - params->fy = view.intrinsics[1][1]; 1027 - params->cx = view.intrinsics[0][2]; 1028 - params->cy = view.intrinsics[1][2]; 1020 + params->fx = view.intrinsics[0][0]; 1021 + params->fy = view.intrinsics[1][1]; 1022 + params->cx = view.intrinsics[0][2]; 1023 + params->cy = view.intrinsics[1][2]; 1029 1024 1030 - switch (view.distortion_model) { 1031 - case T_DISTORTION_OPENCV_RADTAN_8: 1032 - params->distortion_model = "rt8"; 1033 - params->distortion.push_back(view.rt8.k1); 1034 - params->distortion.push_back(view.rt8.k2); 1035 - params->distortion.push_back(view.rt8.p1); 1036 - params->distortion.push_back(view.rt8.p2); 1037 - params->distortion.push_back(view.rt8.k3); 1038 - params->distortion.push_back(view.rt8.k4); 1039 - params->distortion.push_back(view.rt8.k5); 1040 - params->distortion.push_back(view.rt8.k6); 1041 - // -1 metric radius tells Basalt to calculate the metric radius on its own. 1042 - params->distortion.push_back(-1.0); 1043 - SLAM_ASSERT_(params->distortion.size() == 9); 1044 - break; 1045 - case T_DISTORTION_WMR: 1046 - params->distortion_model = "rt8"; 1047 - params->distortion.push_back(view.wmr.k1); 1048 - params->distortion.push_back(view.wmr.k2); 1049 - params->distortion.push_back(view.wmr.p1); 1050 - params->distortion.push_back(view.wmr.p2); 1051 - params->distortion.push_back(view.wmr.k3); 1052 - params->distortion.push_back(view.wmr.k4); 1053 - params->distortion.push_back(view.wmr.k5); 1054 - params->distortion.push_back(view.wmr.k6); 1055 - params->distortion.push_back(view.wmr.rpmax); 1056 - SLAM_ASSERT_(params->distortion.size() == 9); 1057 - break; 1058 - case T_DISTORTION_FISHEYE_KB4: 1059 - params->distortion_model = "kb4"; 1060 - params->distortion.push_back(view.kb4.k1); 1061 - params->distortion.push_back(view.kb4.k2); 1062 - params->distortion.push_back(view.kb4.k3); 1063 - params->distortion.push_back(view.kb4.k4); 1064 - SLAM_ASSERT_(params->distortion.size() == 4); 1065 - break; 1066 - default: 1067 - SLAM_ASSERT(false, "SLAM doesn't support distortion type %s", 1068 - t_stringify_camera_distortion_model(view.distortion_model)); 1069 - } 1025 + switch (view.distortion_model) { 1026 + case T_DISTORTION_OPENCV_RADTAN_8: 1027 + params->distortion_model = "rt8"; 1028 + params->distortion.push_back(view.rt8.k1); 1029 + params->distortion.push_back(view.rt8.k2); 1030 + params->distortion.push_back(view.rt8.p1); 1031 + params->distortion.push_back(view.rt8.p2); 1032 + params->distortion.push_back(view.rt8.k3); 1033 + params->distortion.push_back(view.rt8.k4); 1034 + params->distortion.push_back(view.rt8.k5); 1035 + params->distortion.push_back(view.rt8.k6); 1036 + // -1 metric radius tells Basalt to estimate the metric radius on its own. 1037 + params->distortion.push_back(-1.0); 1038 + SLAM_ASSERT_(params->distortion.size() == 9); 1039 + break; 1040 + case T_DISTORTION_WMR: 1041 + params->distortion_model = "rt8"; 1042 + params->distortion.push_back(view.wmr.k1); 1043 + params->distortion.push_back(view.wmr.k2); 1044 + params->distortion.push_back(view.wmr.p1); 1045 + params->distortion.push_back(view.wmr.p2); 1046 + params->distortion.push_back(view.wmr.k3); 1047 + params->distortion.push_back(view.wmr.k4); 1048 + params->distortion.push_back(view.wmr.k5); 1049 + params->distortion.push_back(view.wmr.k6); 1050 + params->distortion.push_back(view.wmr.rpmax); 1051 + SLAM_ASSERT_(params->distortion.size() == 9); 1052 + break; 1053 + case T_DISTORTION_FISHEYE_KB4: 1054 + params->distortion_model = "kb4"; 1055 + params->distortion.push_back(view.kb4.k1); 1056 + params->distortion.push_back(view.kb4.k2); 1057 + params->distortion.push_back(view.kb4.k3); 1058 + params->distortion.push_back(view.kb4.k4); 1059 + SLAM_ASSERT_(params->distortion.size() == 4); 1060 + break; 1061 + default: 1062 + SLAM_ASSERT(false, "SLAM doesn't support distortion type %s", 1063 + t_stringify_camera_distortion_model(view.distortion_model)); 1064 + } 1070 1065 1071 - xrt_matrix_4x4 T; // Row major T_imu_cam 1072 - math_matrix_4x4_transpose(&extra.T_imu_cam, &T); 1073 - params->t_imu_cam = cv::Matx<float, 4, 4>{T.v}; 1066 + xrt_matrix_4x4 T; // Row major T_imu_cam 1067 + math_matrix_4x4_transpose(&calib->T_imu_cam, &T); 1068 + params->t_imu_cam = cv::Matx<float, 4, 4>{T.v}; 1074 1069 1075 - shared_ptr<FRESULT_ACC> result{}; 1076 - t.slam->use_feature(F_ADD_CAMERA_CALIBRATION, params, result); 1077 - } 1070 + shared_ptr<FRESULT_ACC> result{}; 1071 + t.slam->use_feature(F_ADD_CAMERA_CALIBRATION, params, result); 1078 1072 } 1079 1073 1080 1074 static void 1081 - add_imu_calibration(const TrackerSlam &t, const t_imu_calibration *imu_calib, const t_slam_calib_extras *extra_calib) 1075 + add_imu_calibration(const TrackerSlam &t, const t_slam_imu_calibration *imu_calib) 1082 1076 { 1083 1077 const auto params = make_shared<FPARAMS_AIC>(); 1084 1078 params->imu_index = 0; // Multiple IMU setups unsupported 1085 - params->frequency = extra_calib->imu_frequency; 1079 + params->frequency = imu_calib->frequency; 1086 1080 1087 - const t_inertial_calibration &accel = imu_calib->accel; 1081 + const t_inertial_calibration &accel = imu_calib->base.accel; 1088 1082 params->accel.transform = cv::Matx<double, 3, 3>{&accel.transform[0][0]}; 1089 1083 params->accel.offset = cv::Matx<double, 3, 1>{&accel.offset[0]}; 1090 1084 params->accel.bias_std = cv::Matx<double, 3, 1>{&accel.bias_std[0]}; 1091 1085 params->accel.noise_std = cv::Matx<double, 3, 1>{&accel.noise_std[0]}; 1092 1086 1093 - const t_inertial_calibration &gyro = imu_calib->gyro; 1087 + const t_inertial_calibration &gyro = imu_calib->base.gyro; 1094 1088 params->gyro.transform = cv::Matx<double, 3, 3>{&gyro.transform[0][0]}; 1095 1089 params->gyro.offset = cv::Matx<double, 3, 1>{&gyro.offset[0]}; 1096 1090 params->gyro.bias_std = cv::Matx<double, 3, 1>{&gyro.bias_std[0]}; ··· 1101 1095 } 1102 1096 1103 1097 static void 1104 - send_calibration(const TrackerSlam &t, const t_slam_tracker_config &c) 1098 + send_calibration(const TrackerSlam &t, const t_slam_calibration &c) 1105 1099 { 1106 1100 // Try to send camera calibration data to the SLAM system 1107 - if (c.stereo_calib && c.extra_calib && t.slam->supports_feature(F_ADD_CAMERA_CALIBRATION)) { 1108 - SLAM_INFO("Sending Camera calibration from Monado"); 1109 - add_camera_calibration(t, c.stereo_calib, c.extra_calib); 1110 - } else { 1111 - SLAM_INFO("Cameras will use the calibration provided by the SLAM_CONFIG file"); 1101 + for (int i = 0; i < c.cam_count; i++) { 1102 + if (t.slam->supports_feature(F_ADD_CAMERA_CALIBRATION)) { 1103 + SLAM_INFO("Sending Camera %d calibration from Monado", i); 1104 + add_camera_calibration(t, &c.cams[i], i); 1105 + } else { 1106 + SLAM_INFO("Camera %d will use the calibration provided by the SLAM_CONFIG file", i); 1107 + } 1112 1108 } 1113 1109 1114 1110 // Try to send IMU calibration data to the SLAM system 1115 - if (c.imu_calib && c.extra_calib && t.slam->supports_feature(F_ADD_IMU_CALIBRATION)) { 1111 + if (t.slam->supports_feature(F_ADD_IMU_CALIBRATION)) { 1116 1112 SLAM_INFO("Sending IMU calibration from Monado"); 1117 - add_imu_calibration(t, c.imu_calib, c.extra_calib); 1113 + add_imu_calibration(t, &c.imu); 1118 1114 } else { 1119 1115 SLAM_INFO("The IMU will use the calibration provided by the SLAM_CONFIG file"); 1120 1116 } ··· 1318 1314 config->timing_stat = debug_get_bool_option_slam_timing_stat(); 1319 1315 config->features_stat = debug_get_bool_option_slam_features_stat(); 1320 1316 config->cam_count = int(debug_get_num_option_slam_cam_count()); 1321 - config->imu_calib = NULL; 1322 - config->extra_calib = NULL; 1317 + config->slam_calib = NULL; 1323 1318 } 1324 1319 1325 1320 extern "C" int ··· 1352 1347 1353 1348 // Check the user has provided a SLAM_CONFIG file 1354 1349 const char *config_file = config->slam_config; 1355 - bool some_calib = config->stereo_calib || config->imu_calib; 1350 + bool some_calib = config->slam_calib != nullptr; 1356 1351 if (!config_file && !some_calib) { 1357 1352 U_LOG_IFL_W(log_level, "Unable to determine sensor calibration, did you forget to set SLAM_CONFIG?"); 1358 1353 return -1; ··· 1372 1367 1373 1368 if (!config_file) { 1374 1369 SLAM_INFO("Using calibration from driver and default pipeline settings"); 1375 - send_calibration(t, *config); 1370 + send_calibration(t, *config->slam_calib); // Not null because of `some_calib` 1376 1371 } else { 1377 1372 SLAM_INFO("Using sensor calibration provided by the SLAM_CONFIG file"); 1378 1373 }
+31 -13
src/xrt/auxiliary/tracking/t_tracking.h
··· 15 15 #include "util/u_logging.h" 16 16 #include "xrt/xrt_defines.h" 17 17 #include "xrt/xrt_frame.h" 18 + #include "xrt/xrt_tracking.h" 18 19 #include "util/u_misc.h" 19 20 20 21 #include <stdio.h> ··· 602 603 }; 603 604 604 605 /*! 605 - * This struct complements calibration data from @ref 606 - * t_stereo_camera_calibration and @ref t_imu_calibration 606 + * Extension to camera calibration for SLAM tracking 607 + * 608 + * @see xrt_tracked_slam 609 + */ 610 + struct t_slam_camera_calibration 611 + { 612 + struct t_camera_calibration base; 613 + struct xrt_matrix_4x4 T_imu_cam; //!< Transform IMU to camera. Column major. 614 + double frequency; //!< Camera FPS 615 + }; 616 + 617 + /*! 618 + * Extension to IMU calibration for SLAM tracking 619 + * 620 + * @see xrt_tracked_slam 621 + */ 622 + struct t_slam_imu_calibration 623 + { 624 + struct t_imu_calibration base; 625 + double frequency; 626 + }; 627 + 628 + /*! 629 + * Calibration information necessary for SLAM tracking. 607 630 * 608 631 * @see xrt_tracked_slam 609 632 */ 610 - struct t_slam_calib_extras 633 + struct t_slam_calibration 611 634 { 612 - double imu_frequency; //! IMU samples per second 613 - struct 614 - { 615 - double frequency; //!< Camera FPS 616 - struct xrt_matrix_4x4 T_imu_cam; //!< Transform IMU to camera. Column major. 617 - } cams[2]; 635 + struct t_slam_imu_calibration imu; //!< IMU calibration data 636 + struct t_slam_camera_calibration cams[XRT_TRACKING_MAX_SLAM_CAMS]; //!< Calib data of `cam_count` cams 637 + int cam_count; //!< Number of cameras 618 638 }; 619 639 620 640 /*! ··· 635 655 bool timing_stat; //!< Enable timing metric in external system 636 656 bool features_stat; //!< Enable feature metric in external system 637 657 638 - // Instead of a slam_config file you can set custom calibration data 639 - const struct t_stereo_camera_calibration *stereo_calib; //!< Camera calibration data 640 - const struct t_imu_calibration *imu_calib; //!< IMU calibration data 641 - const struct t_slam_calib_extras *extra_calib; //!< Extra calibration data 658 + //!< Instead of a slam_config file you can set custom calibration data 659 + const struct t_slam_calibration *slam_calib; 642 660 }; 643 661 644 662 /*!
+88 -62
src/xrt/auxiliary/vive/vive_config.c
··· 287 287 return true; 288 288 } 289 289 290 + static struct t_camera_calibration 291 + vive_get_camera_calibration(struct vive_config *d, int cam_index) 292 + { 293 + struct t_camera_calibration calib; 294 + 295 + struct index_camera *camera = &d->cameras.view[cam_index]; 296 + calib.image_size_pixels.w = camera->intrinsics.image_size_pixels.w; 297 + calib.image_size_pixels.h = camera->intrinsics.image_size_pixels.h; 298 + 299 + calib.intrinsics[0][0] = camera->intrinsics.focal_x; 300 + calib.intrinsics[0][1] = 0.0f; 301 + calib.intrinsics[0][2] = camera->intrinsics.center_x; 302 + 303 + calib.intrinsics[1][0] = 0.0f; 304 + calib.intrinsics[1][1] = camera->intrinsics.focal_y; 305 + calib.intrinsics[1][2] = camera->intrinsics.center_y; 306 + 307 + calib.intrinsics[2][0] = 0.0f; 308 + calib.intrinsics[2][1] = 0.0f; 309 + calib.intrinsics[2][2] = 1.0f; 310 + 311 + calib.distortion_model = T_DISTORTION_FISHEYE_KB4; 312 + calib.kb4.k1 = camera->intrinsics.distortion[0]; 313 + calib.kb4.k2 = camera->intrinsics.distortion[1]; 314 + calib.kb4.k3 = camera->intrinsics.distortion[2]; 315 + calib.kb4.k4 = camera->intrinsics.distortion[3]; 316 + 317 + return calib; 318 + } 319 + 290 320 bool 291 321 vive_get_stereo_camera_calibration(struct vive_config *d, 292 322 struct t_stereo_camera_calibration **calibration_ptr_to_ref, ··· 297 327 return false; 298 328 } 299 329 300 - struct index_camera *cameras = d->cameras.view; 301 330 struct t_stereo_camera_calibration *calib = NULL; 302 331 303 332 t_stereo_camera_calibration_alloc(&calib, T_DISTORTION_FISHEYE_KB4); 304 333 305 334 for (int i = 0; i < 2; i++) { 306 - calib->view[i].image_size_pixels.w = cameras[i].intrinsics.image_size_pixels.w; 307 - calib->view[i].image_size_pixels.h = cameras[i].intrinsics.image_size_pixels.h; 308 - 309 - // This better be row-major! 310 - calib->view[i].intrinsics[0][0] = cameras[i].intrinsics.focal_x; 311 - calib->view[i].intrinsics[0][1] = 0.0f; 312 - calib->view[i].intrinsics[0][2] = cameras[i].intrinsics.center_x; 313 - 314 - calib->view[i].intrinsics[1][0] = 0.0f; 315 - calib->view[i].intrinsics[1][1] = cameras[i].intrinsics.focal_y; 316 - calib->view[i].intrinsics[1][2] = cameras[i].intrinsics.center_y; 317 - 318 - calib->view[i].intrinsics[2][0] = 0.0f; 319 - calib->view[i].intrinsics[2][1] = 0.0f; 320 - calib->view[i].intrinsics[2][2] = 1.0f; 321 - 322 - calib->view[i].kb4.k1 = cameras[i].intrinsics.distortion[0]; 323 - calib->view[i].kb4.k2 = cameras[i].intrinsics.distortion[1]; 324 - calib->view[i].kb4.k3 = cameras[i].intrinsics.distortion[2]; 325 - calib->view[i].kb4.k4 = cameras[i].intrinsics.distortion[3]; 335 + calib->view[i] = vive_get_camera_calibration(d, i); 326 336 } 327 337 328 338 struct xrt_vec3 pos = d->cameras.opencv.position; ··· 358 368 return true; 359 369 } 360 370 361 - struct t_imu_calibration 362 - vive_get_imu_calibration(struct vive_config *d) 363 - { 364 - 365 - struct xrt_vec3 ab = d->imu.acc_bias; 366 - struct xrt_vec3 as = d->imu.acc_scale; 367 - struct xrt_vec3 gb = d->imu.gyro_bias; 368 - struct xrt_vec3 gs = d->imu.gyro_scale; 369 - 370 - struct t_imu_calibration calib = { 371 - .accel = 372 - { 373 - .transform = {{as.x, 0, 0}, {0, as.y, 0}, {0, 0, as.z}}, 374 - .offset = {-ab.x, -ab.y, -ab.z}, // negative because slam system will add, not subtract 375 - .bias_std = {0.001, 0.001, 0.001}, 376 - .noise_std = {0.016, 0.016, 0.016}, 377 - }, 378 - .gyro = 379 - { 380 - .transform = {{gs.x, 0, 0}, {0, gs.y, 0}, {0, 0, gs.z}}, 381 - .offset = {-gb.x, -gb.y, -gb.z}, // negative because slam system will add, not subtract 382 - .bias_std = {0.0001, 0.0001, 0.0001}, 383 - .noise_std = {0.000282, 0.000282, 0.000282}, 384 - }, 385 - }; 386 - return calib; 387 - } 388 - 389 - //! IMU extrinsics and frequencies 390 - struct t_slam_calib_extras 391 - vive_get_extra_calibration(struct vive_config *d) 371 + //! Camera calibrations for SLAM 372 + void 373 + vive_get_slam_cams_calib(struct vive_config *d, 374 + struct t_slam_camera_calibration *out_calib0, 375 + struct t_slam_camera_calibration *out_calib1) 392 376 { 393 - VIVE_ERROR(d, "Using default factory extrinsics data for vive driver."); 394 - VIVE_ERROR(d, "The rotations of the sensors in the factory data are off."); 395 - VIVE_ERROR(d, "Use a custom calibration instead whenever possible."); 377 + VIVE_WARN(d, "Using default factory extrinsics data for vive driver."); 378 + VIVE_WARN(d, "The rotations of the sensors in the factory data are off."); 379 + VIVE_WARN(d, "Use a custom calibration instead whenever possible."); 396 380 397 381 struct xrt_pose P_tr_imu = d->imu.trackref; 398 382 struct xrt_pose P_tr_cam0 = d->cameras.view[0].trackref; ··· 469 453 math_matrix_4x4_isometry_from_pose(&P_imuxr_cam0slam, &T_imu_cam0); 470 454 math_matrix_4x4_isometry_from_pose(&P_imuxr_cam1slam, &T_imu_cam1); 471 455 472 - // Can we avoid hardcoding these? 456 + // Can we avoid hardcoding camera frequency? 473 457 const int CAMERA_FREQUENCY = 54; 474 - const int IMU_FREQUENCY = 1000; 475 458 476 - struct t_slam_calib_extras calib = { 477 - .imu_frequency = IMU_FREQUENCY, 478 - .cams = 459 + struct t_slam_camera_calibration calib0 = { 460 + .base = vive_get_camera_calibration(d, 0), 461 + .frequency = CAMERA_FREQUENCY, 462 + .T_imu_cam = T_imu_cam0, 463 + }; 464 + 465 + struct t_slam_camera_calibration calib1 = { 466 + .base = vive_get_camera_calibration(d, 1), 467 + .frequency = CAMERA_FREQUENCY, 468 + .T_imu_cam = T_imu_cam1, 469 + }; 470 + 471 + *out_calib0 = calib0; 472 + *out_calib1 = calib1; 473 + } 474 + 475 + struct t_imu_calibration 476 + vive_get_imu_calibration(struct vive_config *d) 477 + { 478 + 479 + struct xrt_vec3 ab = d->imu.acc_bias; 480 + struct xrt_vec3 as = d->imu.acc_scale; 481 + struct xrt_vec3 gb = d->imu.gyro_bias; 482 + struct xrt_vec3 gs = d->imu.gyro_scale; 483 + 484 + struct t_imu_calibration calib = { 485 + .accel = 479 486 { 480 - {.frequency = CAMERA_FREQUENCY, .T_imu_cam = T_imu_cam0}, 481 - {.frequency = CAMERA_FREQUENCY, .T_imu_cam = T_imu_cam1}, 487 + .transform = {{as.x, 0, 0}, {0, as.y, 0}, {0, 0, as.z}}, 488 + .offset = {-ab.x, -ab.y, -ab.z}, // negative because slam system will add, not subtract 489 + .bias_std = {0.001, 0.001, 0.001}, 490 + .noise_std = {0.016, 0.016, 0.016}, 491 + }, 492 + .gyro = 493 + { 494 + .transform = {{gs.x, 0, 0}, {0, gs.y, 0}, {0, 0, gs.z}}, 495 + .offset = {-gb.x, -gb.y, -gb.z}, // negative because slam system will add, not subtract 496 + .bias_std = {0.0001, 0.0001, 0.0001}, 497 + .noise_std = {0.000282, 0.000282, 0.000282}, 482 498 }, 483 499 }; 500 + return calib; 501 + } 502 + 503 + struct t_slam_imu_calibration 504 + vive_get_slam_imu_calibration(struct vive_config *d) 505 + { 506 + struct t_slam_imu_calibration calib; 507 + const int IMU_FREQUENCY = 1000; 508 + calib.base = vive_get_imu_calibration(d); 509 + calib.frequency = IMU_FREQUENCY; 484 510 return calib; 485 511 } 486 512
+6 -2
src/xrt/auxiliary/vive/vive_config.h
··· 224 224 vive_get_stereo_camera_calibration(struct vive_config *d, 225 225 struct t_stereo_camera_calibration **calibration_ptr_to_ref, 226 226 struct xrt_pose *out_head_in_left_camera); 227 + void 228 + vive_get_slam_cams_calib(struct vive_config *d, 229 + struct t_slam_camera_calibration *out_calib0, 230 + struct t_slam_camera_calibration *out_calib1); 227 231 228 232 struct t_imu_calibration 229 233 vive_get_imu_calibration(struct vive_config *d); 230 234 231 - struct t_slam_calib_extras 232 - vive_get_extra_calibration(struct vive_config *d); 235 + struct t_slam_imu_calibration 236 + vive_get_slam_imu_calibration(struct vive_config *d); 233 237 234 238 #ifdef __cplusplus 235 239 } // extern "C"
+36 -28
src/xrt/drivers/rift_s/rift_s_tracker.c
··· 93 93 } 94 94 } 95 95 96 - XRT_MAYBE_UNUSED struct t_imu_calibration 97 - rift_s_create_slam_imu_calib(void) 96 + XRT_MAYBE_UNUSED void 97 + rift_s_fill_slam_imu_calibration(struct rift_s_tracker *t, struct rift_s_hmd_config *hmd_config) 98 98 { 99 99 /* FIXME: Validate these hard coded standard deviations against 100 100 * some actual at-rest IMU measurements */ ··· 107 107 /* we pass already corrected accel and gyro 108 108 * readings to Basalt, so the transforms and 109 109 * offsets are just identity / zero matrices */ 110 - struct t_imu_calibration calib = { 110 + struct t_imu_calibration imu_calib = { 111 111 .accel = 112 112 { 113 113 .transform = ··· 140 140 }, 141 141 }; 142 142 143 - return calib; 143 + struct t_slam_imu_calibration calib = { 144 + .base = imu_calib, 145 + .frequency = hmd_config->imu_config_info.imu_hz, 146 + }; 147 + 148 + t->slam_calib.imu = calib; 144 149 } 145 150 146 - //! IMU extrinsics, frequency 147 - static struct t_slam_calib_extras 148 - rift_s_create_extra_slam_calib(struct rift_s_hmd_config *hmd_config) 151 + //! Extended camera calibration for SLAM 152 + static void 153 + rift_s_fill_slam_cameras_calibration(struct rift_s_tracker *t, struct rift_s_hmd_config *hmd_config) 149 154 { 150 155 /* SLAM frames are every 2nd frame of 60Hz camera feed */ 151 156 const int CAMERA_FREQUENCY = 30; ··· 180 185 P_imu_right_cam.position.y, P_imu_right_cam.position.z, P_imu_right_cam.orientation.x, 181 186 P_imu_right_cam.orientation.y, P_imu_right_cam.orientation.z, P_imu_right_cam.orientation.w); 182 187 183 - double imu_frequency = hmd_config->imu_config_info.imu_hz; 188 + struct t_slam_camera_calibration calib0 = { 189 + .base = rift_s_get_cam_calib(&hmd_config->camera_calibration, RIFT_S_CAMERA_FRONT_LEFT), 190 + .frequency = CAMERA_FREQUENCY, 191 + .T_imu_cam = T_imu_left_cam, 192 + }; 184 193 185 - struct t_slam_calib_extras calib = { 186 - .imu_frequency = imu_frequency, 187 - .cams = 188 - { 189 - { 190 - .frequency = CAMERA_FREQUENCY, 191 - .T_imu_cam = T_imu_left_cam, 192 - }, 193 - { 194 - .frequency = CAMERA_FREQUENCY, 195 - .T_imu_cam = T_imu_right_cam, 196 - }, 197 - }, 194 + struct t_slam_camera_calibration calib1 = { 195 + .base = rift_s_get_cam_calib(&hmd_config->camera_calibration, RIFT_S_CAMERA_FRONT_RIGHT), 196 + .frequency = CAMERA_FREQUENCY, 197 + .T_imu_cam = T_imu_right_cam, 198 198 }; 199 - return calib; 199 + 200 + t->slam_calib.cam_count = 2; 201 + t->slam_calib.cams[0] = calib0; 202 + t->slam_calib.cams[1] = calib1; 203 + } 204 + 205 + static void 206 + rift_s_fill_slam_calibration(struct rift_s_tracker *t, struct rift_s_hmd_config *hmd_config) 207 + { 208 + rift_s_fill_slam_imu_calibration(t, hmd_config); 209 + rift_s_fill_slam_cameras_calibration(t, hmd_config); 200 210 } 201 211 202 212 static struct xrt_slam_sinks * ··· 212 222 213 223 /* No need to refcount these parameters */ 214 224 config.cam_count = 2; 215 - config.imu_calib = &t->slam_imu_calib; 216 - config.extra_calib = &t->slam_extra_calib; 225 + config.slam_calib = &t->slam_calib; 217 226 218 227 int create_status = t_slam_create(xfctx, &config, &t->tracking.slam, &sinks); 219 228 if (create_status != 0) { ··· 375 384 376 385 assert(slam_status != NULL && hand_status != NULL); 377 386 378 - snprintf(t->gui.slam_status, sizeof(t->gui.slam_status), "%s", slam_status); 379 - snprintf(t->gui.hand_status, sizeof(t->gui.hand_status), "%s", hand_status); 387 + (void)snprintf(t->gui.slam_status, sizeof(t->gui.slam_status), "%s", slam_status); 388 + (void)snprintf(t->gui.hand_status, sizeof(t->gui.hand_status), "%s", hand_status); 380 389 381 390 // Initialize 3DoF tracker 382 391 m_imu_3dof_init(&t->fusion.i3dof, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); ··· 385 394 386 395 // Construct the stereo camera calibration for the front cameras 387 396 t->stereo_calib = rift_s_create_stereo_camera_calib_rotated(&hmd_config->camera_calibration); 388 - t->slam_imu_calib = rift_s_create_slam_imu_calib(); 389 - t->slam_extra_calib = rift_s_create_extra_slam_calib(hmd_config); 397 + rift_s_fill_slam_calibration(t, hmd_config); 390 398 391 399 // Initialize the input sinks for the camera to send to 392 400
+1 -2
src/xrt/drivers/rift_s/rift_s_tracker.h
··· 101 101 102 102 /* Stereo calibration for the front 2 cameras */ 103 103 struct t_stereo_camera_calibration *stereo_calib; 104 - struct t_imu_calibration slam_imu_calib; 105 - struct t_slam_calib_extras slam_extra_calib; 104 + struct t_slam_calibration slam_calib; 106 105 107 106 /* Input sinks that the camera delivers SLAM frames to */ 108 107 struct xrt_slam_sinks in_slam_sinks;
+24 -35
src/xrt/drivers/rift_s/rift_s_util.cpp
··· 211 211 }; 212 212 213 213 #define STEPS 21 214 - static bool 215 - convert_camera_calibration(struct rift_s_camera_calibration *rift_s_cam, struct t_camera_calibration *tcc) 214 + struct t_camera_calibration 215 + rift_s_get_cam_calib(struct rift_s_camera_calibration_block *camera_calibration, enum rift_s_camera_id cam_id) 216 216 { 217 + struct t_camera_calibration tcc; 217 218 218 - tcc->image_size_pixels.h = rift_s_cam->roi.extent.h; 219 - tcc->image_size_pixels.w = rift_s_cam->roi.extent.w; 220 - tcc->intrinsics[0][0] = rift_s_cam->projection.fx; 221 - tcc->intrinsics[1][1] = rift_s_cam->projection.fy; 222 - tcc->intrinsics[0][2] = rift_s_cam->projection.cx; 223 - tcc->intrinsics[1][2] = rift_s_cam->projection.cy; 224 - tcc->intrinsics[2][2] = 1.0; 225 - tcc->distortion_model = T_DISTORTION_FISHEYE_KB4; 219 + struct rift_s_camera_calibration *rift_s_cam = &camera_calibration->cameras[cam_id]; 220 + tcc.image_size_pixels.h = rift_s_cam->roi.extent.h; 221 + tcc.image_size_pixels.w = rift_s_cam->roi.extent.w; 222 + tcc.intrinsics[0][0] = rift_s_cam->projection.fx; 223 + tcc.intrinsics[1][1] = rift_s_cam->projection.fy; 224 + tcc.intrinsics[0][2] = rift_s_cam->projection.cx; 225 + tcc.intrinsics[1][2] = rift_s_cam->projection.cy; 226 + tcc.intrinsics[2][2] = 1.0; 227 + tcc.distortion_model = T_DISTORTION_FISHEYE_KB4; 226 228 227 229 TargetPoint xy[STEPS * STEPS]; 228 230 ··· 238 240 * project onto the points of grid spaced across the pixel image plane */ 239 241 for (int y_index = 0; y_index < STEPS; y_index++) { 240 242 for (int x_index = 0; x_index < STEPS; x_index++) { 241 - int x = x_index * (tcc->image_size_pixels.w - 1) / (STEPS - 1); 242 - int y = y_index * (tcc->image_size_pixels.h - 1) / (STEPS - 1); 243 + int x = x_index * (tcc.image_size_pixels.w - 1) / (STEPS - 1); 244 + int y = y_index * (tcc.image_size_pixels.h - 1) / (STEPS - 1); 243 245 TargetPoint *p = &xy[(y_index * STEPS) + x_index]; 244 246 245 247 p->distorted[0] = x; ··· 248 250 Eigen::Matrix<double, 2, 1> result(0, 0); 249 251 250 252 using AutoDiffUndistortFunction = TinySolverAutoDiffFunction<UndistortCostFunctor, 2, 2>; 251 - UndistortCostFunctor undistort_functor(tcc, fisheye62_distort_params, p->distorted); 253 + UndistortCostFunctor undistort_functor(&tcc, fisheye62_distort_params, p->distorted); 252 254 AutoDiffUndistortFunction f(undistort_functor); 253 255 254 256 TinySolver<AutoDiffUndistortFunction> solver; ··· 265 267 266 268 using AutoDiffDistortParamKB4Function = 267 269 TinySolverAutoDiffFunction<DistortParamKB4CostFunctor, 2 * STEPS * STEPS, N_KB4_DISTORT_PARAMS>; 268 - DistortParamKB4CostFunctor distort_param_kb4_functor(tcc, STEPS, xy); 270 + DistortParamKB4CostFunctor distort_param_kb4_functor(&tcc, STEPS, xy); 269 271 AutoDiffDistortParamKB4Function f(distort_param_kb4_functor); 270 272 271 273 TinySolver<AutoDiffDistortParamKB4Function> solver; 272 274 solver.Solve(f, &kb4_distort_params); 273 275 274 - tcc->kb4.k1 = kb4_distort_params[0]; 275 - tcc->kb4.k2 = kb4_distort_params[1]; 276 - tcc->kb4.k3 = kb4_distort_params[2]; 277 - tcc->kb4.k4 = kb4_distort_params[3]; 278 - } 276 + tcc.kb4.k1 = kb4_distort_params[0]; 277 + tcc.kb4.k2 = kb4_distort_params[1]; 278 + tcc.kb4.k3 = kb4_distort_params[2]; 279 + tcc.kb4.k4 = kb4_distort_params[3]; 279 280 280 - return true; 281 + return tcc; 282 + } 281 283 } 282 284 283 285 /*! ··· 299 301 300 302 // Intrinsics 301 303 for (int view = 0; view < 2; view++) { 302 - struct t_camera_calibration *tcc = &calib->view[view]; 303 - struct rift_s_camera_calibration *cam_config; 304 - 305 - if (view == 0) { 306 - cam_config = left; 307 - } else { 308 - cam_config = right; 309 - } 310 - 311 - if (!convert_camera_calibration(cam_config, tcc)) 312 - goto fail; 304 + enum rift_s_camera_id cam_id = view == 0 ? RIFT_S_CAMERA_FRONT_LEFT : RIFT_S_CAMERA_FRONT_RIGHT; 305 + calib->view[view] = rift_s_get_cam_calib(camera_calibration, cam_id); 313 306 } 314 307 315 308 struct xrt_pose device_from_left, device_from_right; ··· 347 340 calib->camera_rotation[2][2] = right_from_left_rot.v[8]; 348 341 349 342 return calib; 350 - 351 - fail: 352 - t_stereo_camera_calibration_reference(&calib, NULL); 353 - return NULL; 354 343 }
+3
src/xrt/drivers/rift_s/rift_s_util.h
··· 20 20 extern "C" { 21 21 #endif 22 22 23 + struct t_camera_calibration 24 + rift_s_get_cam_calib(struct rift_s_camera_calibration_block *camera_calibration, enum rift_s_camera_id cam_id); 25 + 23 26 struct t_stereo_camera_calibration * 24 27 rift_s_create_stereo_camera_calib_rotated(struct rift_s_camera_calibration_block *camera_calibration); 25 28
+89 -70
src/xrt/drivers/wmr/wmr_hmd.c
··· 1289 1289 *out_angle_up = -atanf(tanangle_up); 1290 1290 } 1291 1291 1292 + XRT_MAYBE_UNUSED static struct t_camera_calibration 1293 + wmr_hmd_get_cam_calib(struct wmr_hmd *wh, int cam_index) 1294 + { 1295 + struct t_camera_calibration res; 1296 + struct wmr_camera_config *wcalib = &wh->config.cameras[cam_index]; 1297 + struct wmr_distortion_6KT *intr = &wcalib->distortion6KT; 1298 + 1299 + res.image_size_pixels.h = wcalib->roi.extent.h; 1300 + res.image_size_pixels.w = wcalib->roi.extent.w; 1301 + res.intrinsics[0][0] = intr->params.fx * (double)wcalib->roi.extent.w; 1302 + res.intrinsics[1][1] = intr->params.fy * (double)wcalib->roi.extent.h; 1303 + res.intrinsics[0][2] = intr->params.cx * (double)wcalib->roi.extent.w; 1304 + res.intrinsics[1][2] = intr->params.cy * (double)wcalib->roi.extent.h; 1305 + res.intrinsics[2][2] = 1.0; 1306 + 1307 + res.distortion_model = T_DISTORTION_WMR; 1308 + res.wmr.k1 = intr->params.k[0]; 1309 + res.wmr.k2 = intr->params.k[1]; 1310 + res.wmr.p1 = intr->params.p1; 1311 + res.wmr.p2 = intr->params.p2; 1312 + res.wmr.k3 = intr->params.k[2]; 1313 + res.wmr.k4 = intr->params.k[3]; 1314 + res.wmr.k5 = intr->params.k[4]; 1315 + res.wmr.k6 = intr->params.k[5]; 1316 + res.wmr.codx = intr->params.dist_x; 1317 + res.wmr.cody = intr->params.dist_y; 1318 + res.wmr.rpmax = intr->params.metric_radius; 1319 + 1320 + return res; 1321 + } 1322 + 1292 1323 /*! 1293 1324 * Creates an OpenCV-compatible @ref t_stereo_camera_calibration pointer from 1294 1325 * the WMR config. ··· 1311 1342 1312 1343 1313 1344 // Intrinsics 1314 - for (int view = 0; view < 2; view++) { // Assuming that cameras[0-1] are HT0 and HT1 1315 - struct t_camera_calibration *tcc = &calib->view[view]; 1316 - struct wmr_camera_config *cam = &wh->config.cameras[view]; 1317 - struct wmr_distortion_6KT *intr = &cam->distortion6KT; 1318 - 1319 - tcc->image_size_pixels.h = wh->config.cameras[view].roi.extent.h; 1320 - tcc->image_size_pixels.w = wh->config.cameras[view].roi.extent.w; 1321 - tcc->intrinsics[0][0] = intr->params.fx * (double)cam->roi.extent.w; 1322 - tcc->intrinsics[1][1] = intr->params.fy * (double)cam->roi.extent.h; 1323 - tcc->intrinsics[0][2] = intr->params.cx * (double)cam->roi.extent.w; 1324 - tcc->intrinsics[1][2] = intr->params.cy * (double)cam->roi.extent.h; 1325 - tcc->intrinsics[2][2] = 1.0; 1326 - 1327 - tcc->wmr.k1 = intr->params.k[0]; 1328 - tcc->wmr.k2 = intr->params.k[1]; 1329 - tcc->wmr.p1 = intr->params.p1; 1330 - tcc->wmr.p2 = intr->params.p2; 1331 - tcc->wmr.k3 = intr->params.k[2]; 1332 - tcc->wmr.k4 = intr->params.k[3]; 1333 - tcc->wmr.k5 = intr->params.k[4]; 1334 - tcc->wmr.k6 = intr->params.k[5]; 1335 - 1336 - tcc->wmr.codx = intr->params.dist_x; 1337 - tcc->wmr.cody = intr->params.dist_y; 1338 - 1339 - tcc->wmr.rpmax = intr->params.metric_radius; 1340 - 1341 - tcc->distortion_model = T_DISTORTION_WMR; 1345 + for (int i = 0; i < 2; i++) { // Assuming that cameras[0-1] are HT0 and HT1 1346 + calib->view[i] = wmr_hmd_get_cam_calib(wh, i); 1342 1347 } 1343 1348 1344 1349 // Extrinsics ··· 1361 1366 return calib; 1362 1367 } 1363 1368 1369 + //! Extended camera calibration info for SLAM 1370 + XRT_MAYBE_UNUSED static void 1371 + wmr_hmd_fill_slam_cams_calibration(struct wmr_hmd *wh) 1372 + { 1373 + struct xrt_pose P_imu_ht0 = wh->config.sensors.accel.pose; 1374 + struct xrt_pose P_ht1_ht0 = wh->config.cameras[1].pose; 1375 + struct xrt_pose P_ht0_ht1; 1376 + math_pose_invert(&P_ht1_ht0, &P_ht0_ht1); 1377 + struct xrt_pose P_imu_ht1; 1378 + math_pose_transform(&P_imu_ht0, &P_ht0_ht1, &P_imu_ht1); 1379 + 1380 + struct xrt_matrix_4x4 T_imu_ht0; 1381 + struct xrt_matrix_4x4 T_imu_ht1; 1382 + math_matrix_4x4_isometry_from_pose(&P_imu_ht0, &T_imu_ht0); 1383 + math_matrix_4x4_isometry_from_pose(&P_imu_ht1, &T_imu_ht1); 1384 + 1385 + struct t_slam_camera_calibration calib0 = { 1386 + .base = wmr_hmd_get_cam_calib(wh, 0), 1387 + .T_imu_cam = T_imu_ht0, 1388 + .frequency = CAMERA_FREQUENCY, 1389 + }; 1390 + 1391 + struct t_slam_camera_calibration calib1 = { 1392 + .base = wmr_hmd_get_cam_calib(wh, 1), 1393 + .T_imu_cam = T_imu_ht1, 1394 + .frequency = CAMERA_FREQUENCY, 1395 + }; 1396 + 1397 + wh->tracking.slam_calib.cam_count = 2; 1398 + wh->tracking.slam_calib.cams[0] = calib0; 1399 + wh->tracking.slam_calib.cams[1] = calib1; 1400 + } 1401 + 1364 1402 XRT_MAYBE_UNUSED static struct t_imu_calibration 1365 - wmr_hmd_create_imu_calib(struct wmr_hmd *wh) 1403 + wmr_hmd_get_imu_calib(struct wmr_hmd *wh) 1366 1404 { 1367 1405 float *at = wh->config.sensors.accel.mix_matrix.v; 1368 1406 struct xrt_vec3 ao = wh->config.sensors.accel.bias_offsets; ··· 1393 1431 return calib; 1394 1432 } 1395 1433 1396 - //! IMU extrinsics, frequencies, and rpmax 1397 - XRT_MAYBE_UNUSED static struct t_slam_calib_extras 1398 - wmr_hmd_create_extra_calib(struct wmr_hmd *wh) 1434 + //! Extended IMU calibration data for SLAM 1435 + XRT_MAYBE_UNUSED static void 1436 + wmr_hmd_fill_slam_imu_calibration(struct wmr_hmd *wh) 1399 1437 { 1400 - struct xrt_pose P_imu_ht0 = wh->config.sensors.accel.pose; 1401 - struct xrt_pose P_ht1_ht0 = wh->config.cameras[1].pose; 1402 - struct xrt_pose P_ht0_ht1; 1403 - math_pose_invert(&P_ht1_ht0, &P_ht0_ht1); 1404 - struct xrt_pose P_imu_ht1; 1405 - math_pose_transform(&P_imu_ht0, &P_ht0_ht1, &P_imu_ht1); 1406 - 1407 - struct xrt_matrix_4x4 T_imu_ht0; 1408 - struct xrt_matrix_4x4 T_imu_ht1; 1409 - math_matrix_4x4_isometry_from_pose(&P_imu_ht0, &T_imu_ht0); 1410 - math_matrix_4x4_isometry_from_pose(&P_imu_ht1, &T_imu_ht1); 1411 - 1412 - //! @note This might change during runtime but the calibration data will be already submitted 1438 + //! @note `average_imus` might change during runtime but the calibration data will be already submitted 1413 1439 double imu_frequency = wh->average_imus ? IMU_FREQUENCY / IMU_SAMPLES_PER_PACKET : IMU_FREQUENCY; 1414 1440 1415 - struct t_slam_calib_extras calib = { 1416 - .imu_frequency = imu_frequency, 1417 - .cams = 1418 - { 1419 - { 1420 - .frequency = CAMERA_FREQUENCY, 1421 - .T_imu_cam = T_imu_ht0, 1422 - }, 1423 - { 1424 - .frequency = CAMERA_FREQUENCY, 1425 - .T_imu_cam = T_imu_ht1, 1426 - }, 1427 - }, 1441 + struct t_slam_imu_calibration imu_calib = { 1442 + .base = wmr_hmd_get_imu_calib(wh), 1443 + .frequency = imu_frequency, 1428 1444 }; 1429 - return calib; 1445 + 1446 + wh->tracking.slam_calib.imu = imu_calib; 1447 + } 1448 + 1449 + XRT_MAYBE_UNUSED static void 1450 + wmr_hmd_fill_slam_calibration(struct wmr_hmd *wh) 1451 + { 1452 + wmr_hmd_fill_slam_imu_calibration(wh); 1453 + wmr_hmd_fill_slam_cams_calibration(wh); 1430 1454 } 1431 1455 1432 1456 static void ··· 1450 1474 } 1451 1475 1452 1476 static struct xrt_slam_sinks * 1453 - wmr_hmd_slam_track(struct wmr_hmd *wh, 1454 - struct t_stereo_camera_calibration *stereo_calib, 1455 - struct t_imu_calibration *imu_calib, 1456 - struct t_slam_calib_extras *extra_calib) 1477 + wmr_hmd_slam_track(struct wmr_hmd *wh) 1457 1478 { 1458 1479 DRV_TRACE_MARKER(); 1459 1480 ··· 1463 1484 struct t_slam_tracker_config config = {0}; 1464 1485 t_slam_fill_default_config(&config); 1465 1486 config.cam_count = 2; 1466 - config.imu_calib = imu_calib; 1467 - config.extra_calib = extra_calib; 1487 + config.slam_calib = &wh->tracking.slam_calib; 1468 1488 if (debug_get_option_slam_submit_from_start() == NULL) { 1469 1489 config.submit_from_start = true; 1470 1490 } ··· 1669 1689 1670 1690 assert(slam_status != NULL && hand_status != NULL); 1671 1691 1672 - snprintf(wh->gui.slam_status, sizeof(wh->gui.slam_status), "%s", slam_status); 1673 - snprintf(wh->gui.hand_status, sizeof(wh->gui.hand_status), "%s", hand_status); 1692 + (void)snprintf(wh->gui.slam_status, sizeof(wh->gui.slam_status), "%s", slam_status); 1693 + (void)snprintf(wh->gui.hand_status, sizeof(wh->gui.hand_status), "%s", hand_status); 1674 1694 1675 1695 struct t_stereo_camera_calibration *stereo_calib = wmr_hmd_create_stereo_camera_calib(wh); 1676 - struct t_imu_calibration imu_calib = wmr_hmd_create_imu_calib(wh); 1677 - struct t_slam_calib_extras extra_calib = wmr_hmd_create_extra_calib(wh); 1696 + wmr_hmd_fill_slam_calibration(wh); 1678 1697 1679 1698 // Initialize 3DoF tracker 1680 1699 m_imu_3dof_init(&wh->fusion.i3dof, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); ··· 1682 1701 // Initialize SLAM tracker 1683 1702 struct xrt_slam_sinks *slam_sinks = NULL; 1684 1703 if (wh->tracking.slam_enabled) { 1685 - slam_sinks = wmr_hmd_slam_track(wh, stereo_calib, &imu_calib, &extra_calib); 1704 + slam_sinks = wmr_hmd_slam_track(wh); 1686 1705 if (slam_sinks == NULL) { 1687 1706 WMR_WARN(wh, "Unable to setup the SLAM tracker"); 1688 1707 return false;
+4
src/xrt/drivers/wmr/wmr_hmd.h
··· 14 14 15 15 #pragma once 16 16 17 + #include "tracking/t_tracking.h" 17 18 #include "xrt/xrt_device.h" 18 19 #include "xrt/xrt_frame.h" 19 20 #include "xrt/xrt_prober.h" ··· 156 157 //! trackers. In particular, we have a @ref xrt_tracked_slam field but not 157 158 //! an equivalent for hand tracking. 158 159 struct xrt_tracked_slam *slam; 160 + 161 + //! Calibration data for SLAM 162 + struct t_slam_calibration slam_calib; 159 163 160 164 //! Set at start. Whether the SLAM tracker was initialized. 161 165 bool slam_enabled;
+10 -10
src/xrt/targets/common/target_builder_lighthouse.c
··· 100 100 struct vive_tracking_status vive_tstatus; //!< Visual tracking status for Index under Vive driver 101 101 struct xrt_fs *xfs; //!< Frameserver for Valve Index camera, if we have one. 102 102 struct vive_config *hmd_config; 103 + struct t_slam_calibration slam_calib; //!< Calibration data for SLAM 103 104 }; 104 105 105 106 ··· 150 151 } 151 152 152 153 static struct xrt_slam_sinks * 153 - valve_index_slam_track(struct lighthouse_system *lhs, 154 - struct t_stereo_camera_calibration *stereo_calib, 155 - struct t_imu_calibration *imu_calib, 156 - struct t_slam_calib_extras *extra_calib) 154 + valve_index_slam_track(struct lighthouse_system *lhs) 157 155 { 158 156 struct xrt_slam_sinks *sinks = NULL; 159 157 ··· 163 161 struct t_slam_tracker_config config = {0}; 164 162 t_slam_fill_default_config(&config); 165 163 config.cam_count = 2; 166 - config.stereo_calib = stereo_calib; // Won't hold stereo_calib so no refcount 167 - config.imu_calib = imu_calib; 168 - config.extra_calib = extra_calib; 164 + config.slam_calib = &lhs->slam_calib; 169 165 170 166 int create_status = t_slam_create(&lhs->devices->xfctx, &config, &d->tracking.slam, &sinks); 171 167 if (create_status != 0) { ··· 373 369 bool slam_enabled = lhs->vive_tstatus.slam_enabled; 374 370 bool hand_enabled = lhs->vive_tstatus.hand_enabled; 375 371 372 + // Hand tracking calibration 376 373 struct t_stereo_camera_calibration *stereo_calib = NULL; 377 374 struct xrt_pose head_in_left_cam; 378 375 vive_get_stereo_camera_calibration(lhs->hmd_config, &stereo_calib, &head_in_left_cam); 379 - struct t_imu_calibration imu_calib = vive_get_imu_calibration(lhs->hmd_config); 380 - struct t_slam_calib_extras extra_calib = vive_get_extra_calibration(lhs->hmd_config); 376 + 377 + // SLAM calibration 378 + lhs->slam_calib.cam_count = 2; 379 + vive_get_slam_cams_calib(lhs->hmd_config, &lhs->slam_calib.cams[0], &lhs->slam_calib.cams[1]); 380 + lhs->slam_calib.imu = vive_get_slam_imu_calibration(lhs->hmd_config); 381 381 382 382 // Initialize SLAM tracker 383 383 struct xrt_slam_sinks *slam_sinks = NULL; 384 384 if (slam_enabled) { 385 - slam_sinks = valve_index_slam_track(lhs, stereo_calib, &imu_calib, &extra_calib); 385 + slam_sinks = valve_index_slam_track(lhs); 386 386 if (slam_sinks == NULL) { 387 387 lhs->vive_tstatus.slam_enabled = false; 388 388 slam_enabled = false;