+1 -1

src/external/CMakeLists.txt

reviewed

··· 80 80 xrt-external-openxr INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}/openxr_includes 81 81 ) 82 82 83 83 - # External SLAM tracking 83 83 + # External SLAM interface 84 84 if(XRT_FEATURE_SLAM) 85 85 add_library(xrt-external-slam STATIC slam_tracker/slam_tracker.hpp) 86 86 set_target_properties(xrt-external-slam PROPERTIES LINKER_LANGUAGE CXX)

+310 -234

src/xrt/auxiliary/tracking/t_tracker_slam.cpp

reviewed

··· 28 28 #include "tracking/t_euroc_recorder.h" 29 29 #include "tracking/t_openvr_tracker.h" 30 30 #include "tracking/t_tracking.h" 31 31 + #include "tracking/t_vit_loader.h" 31 32 32 32 - #include <slam_tracker.hpp> 33 33 + #include "vit/vit_interface.h" 34 34 + 33 35 #include <opencv2/core/mat.hpp> 34 36 #include <opencv2/core/version.hpp> 35 37 ··· 69 71 70 72 //! @see t_slam_tracker_config 71 73 DEBUG_GET_ONCE_LOG_OPTION(slam_log, "SLAM_LOG", U_LOGGING_INFO) 74 74 + DEBUG_GET_ONCE_OPTION(vit_system_library_path, "VIT_SYSTEM_LIBRARY_PATH", NULL) 72 75 DEBUG_GET_ONCE_OPTION(slam_config, "SLAM_CONFIG", nullptr) 73 76 DEBUG_GET_ONCE_BOOL_OPTION(slam_ui, "SLAM_UI", false) 74 77 DEBUG_GET_ONCE_BOOL_OPTION(slam_submit_from_start, "SLAM_SUBMIT_FROM_START", false) ··· 344 347 struct TrackerSlam 345 348 { 346 349 struct xrt_tracked_slam base = {}; 347 347 - struct xrt_frame_node node = {}; //!< Will be called on destruction 348 348 - slam_tracker *slam; //!< Pointer to the external SLAM system implementation 350 350 + struct xrt_frame_node node = {}; //!< Will be called on destruction 351 351 + struct t_vit_bundle vit; //!< VIT system function pointers 352 352 + enum vit_tracker_pose_capability caps; //!< VIT tracker bitfield capabilities 353 353 + struct vit_tracker *tracker; //!< Pointer to the tracker created by the loaded VIT system; 349 354 350 355 struct xrt_slam_sinks sinks = {}; //!< Pointers to the sinks below 351 356 struct xrt_frame_sink cam_sinks[XRT_TRACKING_MAX_SLAM_CAMS]; //!< Sends camera frames to the SLAM system 352 357 struct xrt_imu_sink imu_sink = {}; //!< Sends imu samples to the SLAM system 353 358 struct xrt_pose_sink gt_sink = {}; //!< Register groundtruth trajectory for stats 354 354 - bool submit; //!< Whether to submit data pushed to sinks to the SLAM tracker 355 355 - int cam_count; //!< Number of cameras used for tracking 359 359 + bool submit; //!< Whether to submit data pushed to sinks to the SLAM tracker 360 360 + uint32_t cam_count; //!< Number of cameras used for tracking 356 361 357 362 struct u_var_button reset_state_btn; //!< Reset tracker state button 358 363 359 364 enum u_logging_level log_level; //!< Logging level for the SLAM tracker, set by SLAM_LOG var 360 360 - struct os_thread_helper oth; //!< Thread where the external SLAM system runs 361 365 MatFrame *cv_wrapper; //!< Wraps a xrt_frame in a cv::Mat to send to the SLAM system 362 366 363 367 struct xrt_slam_sinks *euroc_recorder; //!< EuRoC dataset recording sinks ··· 426 430 //! Tracker timing info for performance evaluation 427 431 struct 428 432 { 429 429 - bool ext_available = false; //!< Whether the SLAM system supports the timing extension 430 430 - bool ext_enabled = false; //!< Whether the timing extension is enabled 433 433 + bool enabled = false; //!< Whether the timing extension is enabled 431 434 float dur_ms[UI_TIMING_POSE_COUNT]; //!< Timing durations in ms 432 435 int idx = 0; //!< Index of latest entry in @ref dur_ms 433 436 u_var_combo start_ts; //!< UI combo box to select initial timing measurement ··· 465 468 vector<FeatureCounter> fcs; //!< Store feature count info for each camera 466 469 u_var_curves fcs_ui; //!< Display of `fcs` in UI 467 470 468 468 - bool ext_available = false; //!< Whether the SLAM system supports the features extension 469 469 - bool ext_enabled = false; //!< Whether the features extension is enabled 471 471 + bool enabled = false; //!< Whether the features extension is enabled 470 472 struct u_var_button enable_btn; //!< Toggle extension 471 473 } features; 472 474 ··· 492 494 static void 493 495 timing_ui_setup(TrackerSlam &t) 494 496 { 497 497 + t.timing.enabled = false; 498 498 + 495 499 u_var_add_ro_ftext(&t, "\n%s", "Tracker timing"); 496 500 497 501 // Setup toggle button ··· 499 503 u_var_button_cb cb = [](void *t_ptr) { 500 504 TrackerSlam *t = (TrackerSlam *)t_ptr; 501 505 u_var_button &btn = t->timing.enable_btn; 502 502 - bool &e = t->timing.ext_enabled; 503 503 - e = !e; 506 506 + bool e = !t->timing.enabled; 504 507 snprintf(btn.label, sizeof(btn.label), "%s", msg[e]); 505 505 - const auto params = make_shared<FPARAMS_EPET>(e); 506 506 - shared_ptr<void> _; 507 507 - t->slam->use_feature(F_ENABLE_POSE_EXT_TIMING, params, _); 508 508 + vit_result_t vres = 509 509 + t->vit.tracker_set_pose_capabilities(t->tracker, VIT_TRACKER_POSE_CAPABILITY_TIMING, e); 510 510 + if (vres != VIT_SUCCESS) { 511 511 + U_LOG_IFL_E(t->log_level, "Failed to set tracker timing capability"); 512 512 + return; 513 513 + } 514 514 + t->timing.enabled = e; 508 515 }; 509 516 t.timing.enable_btn.cb = cb; 510 510 - t.timing.enable_btn.disabled = !t.timing.ext_available; 517 517 + t.timing.enable_btn.disabled = (t.caps & VIT_TRACKER_POSE_CAPABILITY_TIMING) == 0; 511 518 t.timing.enable_btn.ptr = &t; 512 512 - u_var_add_button(&t, &t.timing.enable_btn, msg[t.timing.ext_enabled]); 519 519 + u_var_add_button(&t, &t.timing.enable_btn, msg[t.timing.enabled]); 520 520 + 521 521 + // We provide two timing columns by default, even if there is no extension support 522 522 + t.timing.columns = {"sampled", "received_by_monado"}; 513 523 514 514 - // Setup graph 524 524 + // Only fill the timing columns if the tracker supports pose timing 525 525 + if ((t.caps & VIT_TRACKER_POSE_CAPABILITY_TIMING) != 0) { 526 526 + vit_tracker_timing_titles titles = {}; 527 527 + vit_result_t vres = t.vit.tracker_get_timing_titles(t.tracker, &titles); 528 528 + if (vres != VIT_SUCCESS) { 529 529 + SLAM_ERROR("Failed to get timing titles from tracker"); 530 530 + return; 531 531 + } 532 532 + 533 533 + // Copies the titles locally. 534 534 + std::vector<std::string> cols(titles.titles, titles.titles + titles.count); 535 535 + 536 536 + t.timing.columns.insert(t.timing.columns.begin() + 1, cols.begin(), cols.end()); 537 537 + } 515 538 516 539 // Construct null-separated array of options for the combo box 517 540 using namespace std::string_literals; ··· 546 569 547 570 //! Updates timing UI with info from a computed pose and returns that info 548 571 static vector<timepoint_ns> 549 549 - timing_ui_push(TrackerSlam &t, const pose &p) 572 572 + timing_ui_push(TrackerSlam &t, const vit_pose_t *pose, int64_t ts) 550 573 { 551 574 timepoint_ns now = os_monotonic_get_ns(); 552 552 - vector<timepoint_ns> tss = {p.timestamp, now}; 575 575 + vector<timepoint_ns> tss = {ts, now}; 553 576 554 577 // Add extra timestamps if the SLAM tracker provides them 555 555 - shared_ptr<pose_extension> ext = p.find_pose_extension(pose_ext_type::TIMING); 556 556 - if (ext) { 557 557 - pose_ext_timing pet = *std::static_pointer_cast<pose_ext_timing>(ext); 558 558 - tss.insert(tss.begin() + 1, pet.timing.begin(), pet.timing.end()); 559 559 - } 578 578 + if (t.timing.enabled) { 579 579 + vit_pose_timing timing; 580 580 + vit_result_t vres = t.vit.pose_get_timing(pose, &timing); 581 581 + if (vres != VIT_SUCCESS) { 582 582 + // Even if the timing is enabled, some of the poses already in the queue won't have it enabled. 583 583 + if (vres != VIT_ERROR_NOT_ENABLED) { 584 584 + SLAM_ERROR("Failed to get pose timing"); 585 585 + } 560 586 561 561 - // The two timestamps to compare in the graph 562 562 - timepoint_ns start = tss.at(t.timing.start_ts_idx); 563 563 - timepoint_ns end = tss.at(t.timing.end_ts_idx); 587 587 + return {}; 588 588 + } 564 589 565 565 - // Push to the UI graph 566 566 - float tss_ms = (end - start) / U_TIME_1MS_IN_NS; 567 567 - t.timing.idx = (t.timing.idx + 1) % UI_TIMING_POSE_COUNT; 568 568 - t.timing.dur_ms[t.timing.idx] = tss_ms; 569 569 - constexpr float a = 1.0f / UI_TIMING_POSE_COUNT; // Exponential moving average 570 570 - t.timing.ui.reference_timing = (1 - a) * t.timing.ui.reference_timing + a * tss_ms; 590 590 + std::vector<int64_t> data(timing.timestamps, timing.timestamps + timing.count); 591 591 + tss.insert(tss.begin() + 1, data.begin(), data.end()); 592 592 + 593 593 + // The two timestamps to compare in the graph 594 594 + timepoint_ns start = tss.at(t.timing.start_ts_idx); 595 595 + timepoint_ns end = tss.at(t.timing.end_ts_idx); 596 596 + 597 597 + // Push to the UI graph 598 598 + float tss_ms = (end - start) / U_TIME_1MS_IN_NS; 599 599 + t.timing.idx = (t.timing.idx + 1) % UI_TIMING_POSE_COUNT; 600 600 + t.timing.dur_ms[t.timing.idx] = tss_ms; 601 601 + constexpr float a = 1.0f / UI_TIMING_POSE_COUNT; // Exponential moving average 602 602 + t.timing.ui.reference_timing = (1 - a) * t.timing.ui.reference_timing + a * tss_ms; 603 603 + } 571 604 572 605 return tss; 573 606 } ··· 582 615 static void 583 616 features_ui_setup(TrackerSlam &t) 584 617 { 585 585 - // We can't do anything useful if the system doesn't implement the feature 586 586 - if (!t.features.ext_available) { 587 587 - return; 588 588 - } 618 618 + t.features.enabled = false; 589 619 590 620 u_var_add_ro_ftext(&t, "\n%s", "Tracker features"); 591 621 ··· 594 624 u_var_button_cb cb = [](void *t_ptr) { 595 625 TrackerSlam *t = (TrackerSlam *)t_ptr; 596 626 u_var_button &btn = t->features.enable_btn; 597 597 - bool &e = t->features.ext_enabled; 598 598 - e = !e; 627 627 + bool e = !t->features.enabled; 599 628 snprintf(btn.label, sizeof(btn.label), "%s", msg[e]); 600 600 - const auto params = make_shared<FPARAMS_EPEF>(e); 601 601 - shared_ptr<void> _; 602 602 - t->slam->use_feature(F_ENABLE_POSE_EXT_FEATURES, params, _); 629 629 + vit_result_t vres = 630 630 + t->vit.tracker_set_pose_capabilities(t->tracker, VIT_TRACKER_POSE_CAPABILITY_FEATURES, e); 631 631 + if (vres != VIT_SUCCESS) { 632 632 + U_LOG_IFL_E(t->log_level, "Failed to set tracker features capability"); 633 633 + return; 634 634 + } 635 635 + t->features.enabled = e; 603 636 }; 604 637 t.features.enable_btn.cb = cb; 605 605 - t.features.enable_btn.disabled = !t.features.ext_available; 638 638 + t.features.enable_btn.disabled = (t.caps & VIT_TRACKER_POSE_CAPABILITY_FEATURES) == 0; 606 639 t.features.enable_btn.ptr = &t; 607 607 - u_var_add_button(&t, &t.features.enable_btn, msg[t.features.ext_enabled]); 640 640 + u_var_add_button(&t, &t.features.enable_btn, msg[t.features.enabled]); 608 641 609 642 // Setup graph 610 610 - 611 643 u_var_curve_getter getter = [](void *fs_ptr, int i) -> u_var_curve_point { 612 644 auto *fs = (TrackerSlam::Features::FeatureCounter *)fs_ptr; 613 645 timepoint_ns now = os_monotonic_get_ns(); ··· 631 663 t.features.fcs_ui.ylabel = "Number of features"; 632 664 633 665 t.features.fcs.resize(t.cam_count); 634 634 - for (int i = 0; i < t.cam_count; i++) { 666 666 + for (uint32_t i = 0; i < t.cam_count; ++i) { 635 667 auto &fc = t.features.fcs[i]; 636 668 fc.cam_name = "Cam" + to_string(i); 637 669 ··· 646 678 } 647 679 648 680 static vector<int> 649 649 - features_ui_push(TrackerSlam &t, const pose &ppp) 681 681 + features_ui_push(TrackerSlam &t, const vit_pose_t *pose, int64_t ts) 650 682 { 651 651 - if (!t.features.ext_available) { 652 652 - return {}; 653 653 - } 654 654 - 655 655 - shared_ptr<pose_extension> ext = ppp.find_pose_extension(pose_ext_type::FEATURES); 656 656 - if (!ext) { 683 683 + if (!t.features.enabled) { 657 684 return {}; 658 685 } 659 686 660 660 - pose_ext_features pef = *std::static_pointer_cast<pose_ext_features>(ext); 661 661 - 662 687 // Push to the UI graph 663 688 vector<int> fcs{}; 664 664 - for (size_t i = 0; i < pef.features_per_cam.size(); i++) { 665 665 - int count = pef.features_per_cam.at(i).size(); 666 666 - t.features.fcs.at(i).addFeatureCount(ppp.timestamp, count); 667 667 - fcs.push_back(count); 689 689 + for (uint32_t i = 0; i < t.cam_count; ++i) { 690 690 + vit_pose_features features = {}; 691 691 + vit_result_t vres = t.vit.pose_get_features(pose, i, &features); 692 692 + if (vres != VIT_SUCCESS) { 693 693 + // Even if the features are enabled, some of the poses already in the queue won't have it 694 694 + // enabled. 695 695 + if (vres != VIT_ERROR_NOT_ENABLED) { 696 696 + SLAM_ERROR("Failed to get pose features for camera %u", i); 697 697 + } 698 698 + 699 699 + return {}; 700 700 + } 701 701 + 702 702 + t.features.fcs.at(i).addFeatureCount(ts, features.count); 703 703 + fcs.push_back(features.count); 668 704 } 669 705 670 706 return fcs; ··· 789 825 static bool 790 826 flush_poses(TrackerSlam &t) 791 827 { 792 792 - pose tracked_pose{}; 793 793 - bool got_one = t.slam->try_dequeue_pose(tracked_pose); 794 828 795 795 - bool dequeued = got_one; 796 796 - while (dequeued) { 829 829 + vit_pose_t *pose; 830 830 + vit_result_t vres = t.vit.tracker_pop_pose(t.tracker, &pose); 831 831 + if (vres != VIT_SUCCESS) { 832 832 + SLAM_ERROR("Failed to get pose from VIT tracker"); 833 833 + } 834 834 + 835 835 + if (pose == NULL) { 836 836 + SLAM_TRACE("No poses to flush"); 837 837 + return false; 838 838 + } 839 839 + 840 840 + do { 797 841 // New pose 798 798 - pose np = tracked_pose; 799 799 - int64_t nts = np.timestamp; 800 800 - xrt_vec3 npos{np.px, np.py, np.pz}; 801 801 - xrt_quat nrot{np.rx, np.ry, np.rz, np.rw}; 842 842 + vit_pose_data_t data; 843 843 + vres = t.vit.pose_get_data(pose, &data); 844 844 + if (vres != VIT_SUCCESS) { 845 845 + SLAM_ERROR("Failed to get pose data from VIT tracker"); 846 846 + return false; 847 847 + } 848 848 + 849 849 + int64_t nts = data.timestamp; 850 850 + 851 851 + xrt_vec3 npos{data.px, data.py, data.pz}; 852 852 + xrt_quat nrot{data.ox, data.oy, data.oz, data.ow}; 802 853 803 854 // Last relation 804 855 xrt_space_relation lr = XRT_SPACE_RELATION_ZERO; ··· 810 861 double dt = time_ns_to_s(nts - lts); 811 862 812 863 SLAM_TRACE("Dequeued SLAM pose ts=%ld p=[%f,%f,%f] r=[%f,%f,%f,%f]", // 813 813 - nts, np.px, np.py, np.pz, np.rx, np.ry, np.rz, np.rw); 864 864 + nts, data.px, data.py, data.pz, data.ox, data.oy, data.oz, data.ow); 814 865 866 866 + // TODO linear velocity from the VIT system 815 867 // Compute new relation based on new pose and velocities since last pose 816 868 xrt_space_relation rel{}; 817 869 rel.relation_flags = XRT_SPACE_RELATION_BITMASK_ALL; ··· 830 882 xrt_pose_sample pose_sample = {nts, rel.pose}; 831 883 xrt_sink_push_pose(t.euroc_recorder->gt, &pose_sample); 832 884 833 833 - // Push even if timing extension is disabled 834 834 - auto tss = timing_ui_push(t, np); 885 885 + auto tss = timing_ui_push(t, pose, nts); 835 886 t.slam_times_writer->push(tss); 836 887 837 837 - if (t.features.ext_enabled) { 838 838 - vector feat_count = features_ui_push(t, np); 888 888 + if (t.features.enabled) { 889 889 + vector feat_count = features_ui_push(t, pose, nts); 839 890 t.slam_features_writer->push({nts, feat_count}); 840 891 } 841 892 842 842 - dequeued = t.slam->try_dequeue_pose(tracked_pose); 843 843 - } 844 844 - 845 845 - if (!got_one) { 846 846 - SLAM_TRACE("No poses to flush"); 847 847 - } 893 893 + t.vit.pose_destroy(pose); 894 894 + } while (t.vit.tracker_pop_pose(t.tracker, &pose) == VIT_SUCCESS && pose); 848 895 849 849 - return got_one; 896 896 + return true; 850 897 } 851 898 852 899 //! Integrates IMU samples on top of a base pose and predicts from that ··· 1083 1130 u_var_add_root(&t, "SLAM Tracker", true); 1084 1131 u_var_add_log_level(&t, &t.log_level, "Log Level"); 1085 1132 u_var_add_bool(&t, &t.submit, "Submit data to SLAM"); 1133 1133 + 1086 1134 u_var_button_cb reset_state_cb = [](void *t_ptr) { 1087 1087 - TrackerSlam *t = (TrackerSlam *)t_ptr; 1088 1088 - shared_ptr<void> _; 1089 1089 - t->slam->use_feature(F_RESET_TRACKER_STATE, _, _); 1135 1135 + TrackerSlam &t = *(TrackerSlam *)t_ptr; 1136 1136 + 1137 1137 + vit_result_t vres = t.vit.tracker_reset(t.tracker); 1138 1138 + if (vres != VIT_SUCCESS) { 1139 1139 + SLAM_WARN("Failed to reset VIT tracker"); 1140 1140 + } 1090 1141 }; 1091 1142 t.reset_state_btn.cb = reset_state_cb; 1092 1143 t.reset_state_btn.ptr = &t; ··· 1133 1184 } 1134 1185 1135 1186 static void 1136 1136 - add_camera_calibration(const TrackerSlam &t, const t_slam_camera_calibration *calib, int cam_index) 1187 1187 + add_camera_calibration(const TrackerSlam &t, const t_slam_camera_calibration *calib, uint32_t cam_index) 1137 1188 { 1138 1189 const t_camera_calibration &view = calib->base; 1139 1139 - const auto params = make_shared<FPARAMS_ACC>(); 1140 1190 1141 1141 - params->cam_index = cam_index; 1142 1142 - params->width = view.image_size_pixels.w; 1143 1143 - params->height = view.image_size_pixels.h; 1144 1144 - params->frequency = calib->frequency; 1191 1191 + vit_camera_calibration params = {}; 1192 1192 + params.camera_index = cam_index; 1193 1193 + params.width = view.image_size_pixels.w; 1194 1194 + params.height = view.image_size_pixels.h; 1195 1195 + params.frequency = calib->frequency; 1145 1196 1146 1146 - params->fx = view.intrinsics[0][0]; 1147 1147 - params->fy = view.intrinsics[1][1]; 1148 1148 - params->cx = view.intrinsics[0][2]; 1149 1149 - params->cy = view.intrinsics[1][2]; 1197 1197 + params.fx = view.intrinsics[0][0]; 1198 1198 + params.fy = view.intrinsics[1][1]; 1199 1199 + params.cx = view.intrinsics[0][2]; 1200 1200 + params.cy = view.intrinsics[1][2]; 1150 1201 1151 1202 switch (view.distortion_model) { 1152 1152 - case T_DISTORTION_OPENCV_RADTAN_8: 1153 1153 - params->distortion_model = "rt8"; 1154 1154 - params->distortion.push_back(view.rt8.k1); 1155 1155 - params->distortion.push_back(view.rt8.k2); 1156 1156 - params->distortion.push_back(view.rt8.p1); 1157 1157 - params->distortion.push_back(view.rt8.p2); 1158 1158 - params->distortion.push_back(view.rt8.k3); 1159 1159 - params->distortion.push_back(view.rt8.k4); 1160 1160 - params->distortion.push_back(view.rt8.k5); 1161 1161 - params->distortion.push_back(view.rt8.k6); 1203 1203 + case T_DISTORTION_OPENCV_RADTAN_8: { 1204 1204 + params.model = VIT_CAMERA_DISTORTION_RT8; 1205 1205 + const size_t size = sizeof(struct t_camera_calibration_rt8_params) + sizeof(double); 1206 1206 + params.distortion_count = size / sizeof(double); 1207 1207 + SLAM_ASSERT_(params.distortion_count == 9); 1208 1208 + 1209 1209 + memcpy(params.distortion, &view.rt8, size); 1210 1210 + 1162 1211 // -1 metric radius tells Basalt to estimate the metric radius on its own. 1163 1163 - params->distortion.push_back(-1.0); 1164 1164 - SLAM_ASSERT_(params->distortion.size() == 9); 1212 1212 + params.distortion[8] = -1.f; 1165 1213 break; 1166 1166 - case T_DISTORTION_WMR: 1167 1167 - params->distortion_model = "rt8"; 1168 1168 - params->distortion.push_back(view.wmr.k1); 1169 1169 - params->distortion.push_back(view.wmr.k2); 1170 1170 - params->distortion.push_back(view.wmr.p1); 1171 1171 - params->distortion.push_back(view.wmr.p2); 1172 1172 - params->distortion.push_back(view.wmr.k3); 1173 1173 - params->distortion.push_back(view.wmr.k4); 1174 1174 - params->distortion.push_back(view.wmr.k5); 1175 1175 - params->distortion.push_back(view.wmr.k6); 1176 1176 - params->distortion.push_back(view.wmr.rpmax); 1177 1177 - SLAM_ASSERT_(params->distortion.size() == 9); 1214 1214 + } 1215 1215 + case T_DISTORTION_WMR: { 1216 1216 + params.model = VIT_CAMERA_DISTORTION_RT8; 1217 1217 + const size_t size = sizeof(struct t_camera_calibration_rt8_params) + sizeof(double); 1218 1218 + params.distortion_count = size / sizeof(double); 1219 1219 + SLAM_ASSERT_(params.distortion_count == 9); 1220 1220 + 1221 1221 + memcpy(params.distortion, &view.wmr, size); 1222 1222 + 1223 1223 + params.distortion[8] = view.wmr.rpmax; 1224 1224 + 1178 1225 break; 1179 1179 - case T_DISTORTION_FISHEYE_KB4: 1180 1180 - params->distortion_model = "kb4"; 1181 1181 - params->distortion.push_back(view.kb4.k1); 1182 1182 - params->distortion.push_back(view.kb4.k2); 1183 1183 - params->distortion.push_back(view.kb4.k3); 1184 1184 - params->distortion.push_back(view.kb4.k4); 1185 1185 - SLAM_ASSERT_(params->distortion.size() == 4); 1226 1226 + } 1227 1227 + case T_DISTORTION_FISHEYE_KB4: { 1228 1228 + params.model = VIT_CAMERA_DISTORTION_KB4; 1229 1229 + const size_t size = sizeof(struct t_camera_calibration_kb4_params); 1230 1230 + params.distortion_count = size / sizeof(double); 1231 1231 + SLAM_ASSERT_(params.distortion_count == 4); 1232 1232 + 1233 1233 + memcpy(params.distortion, &view.kb4, size); 1186 1234 break; 1235 1235 + } 1187 1236 default: 1188 1237 SLAM_ASSERT(false, "SLAM doesn't support distortion type %s", 1189 1238 t_stringify_camera_distortion_model(view.distortion_model)); 1239 1239 + break; 1190 1240 } 1191 1241 1192 1242 xrt_matrix_4x4 T; // Row major T_imu_cam 1193 1243 math_matrix_4x4_transpose(&calib->T_imu_cam, &T); 1194 1194 - params->t_imu_cam = cv::Matx<float, 4, 4>{T.v}; 1195 1244 1196 1196 - shared_ptr<FRESULT_ACC> result{}; 1197 1197 - t.slam->use_feature(F_ADD_CAMERA_CALIBRATION, params, result); 1245 1245 + // Converts the xrt_matrix_4x4 from float to double 1246 1246 + for (size_t i = 0; i < ARRAY_SIZE(params.transform); ++i) 1247 1247 + params.transform[i] = T.v[i]; 1248 1248 + 1249 1249 + vit_result_t vres = t.vit.tracker_add_camera_calibration(t.tracker, &params); 1250 1250 + if (vres != VIT_SUCCESS) { 1251 1251 + SLAM_ERROR("Failed to add camera calibration for camera %u", cam_index); 1252 1252 + } 1198 1253 } 1199 1254 1200 1255 static void 1201 1256 add_imu_calibration(const TrackerSlam &t, const t_slam_imu_calibration *imu_calib) 1202 1257 { 1203 1203 - const auto params = make_shared<FPARAMS_AIC>(); 1204 1204 - params->imu_index = 0; // Multiple IMU setups unsupported 1205 1205 - params->frequency = imu_calib->frequency; 1258 1258 + vit_imu_calibration_t params = {}; 1259 1259 + params.imu_index = 0; 1260 1260 + params.frequency = imu_calib->frequency; 1261 1261 + 1262 1262 + // TODO improve memcpy size calculation 1206 1263 1207 1264 const t_inertial_calibration &accel = imu_calib->base.accel; 1208 1208 - params->accel.transform = cv::Matx<double, 3, 3>{&accel.transform[0][0]}; 1209 1209 - params->accel.offset = cv::Matx<double, 3, 1>{&accel.offset[0]}; 1210 1210 - params->accel.bias_std = cv::Matx<double, 3, 1>{&accel.bias_std[0]}; 1211 1211 - params->accel.noise_std = cv::Matx<double, 3, 1>{&accel.noise_std[0]}; 1265 1265 + memcpy(params.accel.transform, accel.transform, sizeof(double) * 9); 1266 1266 + memcpy(params.accel.offset, accel.offset, sizeof(double) * 3); 1267 1267 + memcpy(params.accel.bias_std, accel.bias_std, sizeof(double) * 3); 1268 1268 + memcpy(params.accel.noise_std, accel.noise_std, sizeof(double) * 3); 1212 1269 1213 1270 const t_inertial_calibration &gyro = imu_calib->base.gyro; 1214 1214 - params->gyro.transform = cv::Matx<double, 3, 3>{&gyro.transform[0][0]}; 1215 1215 - params->gyro.offset = cv::Matx<double, 3, 1>{&gyro.offset[0]}; 1216 1216 - params->gyro.bias_std = cv::Matx<double, 3, 1>{&gyro.bias_std[0]}; 1217 1217 - params->gyro.noise_std = cv::Matx<double, 3, 1>{&gyro.noise_std[0]}; 1271 1271 + memcpy(params.gyro.transform, gyro.transform, sizeof(double) * 9); 1272 1272 + memcpy(params.gyro.offset, gyro.offset, sizeof(double) * 3); 1273 1273 + memcpy(params.gyro.bias_std, gyro.bias_std, sizeof(double) * 3); 1274 1274 + memcpy(params.gyro.noise_std, gyro.noise_std, sizeof(double) * 3); 1218 1275 1219 1219 - shared_ptr<FRESULT_AIC> result{}; 1220 1220 - t.slam->use_feature(F_ADD_IMU_CALIBRATION, params, result); 1276 1276 + vit_result_t vres = t.vit.tracker_add_imu_calibration(t.tracker, &params); 1277 1277 + if (vres != VIT_SUCCESS) { 1278 1278 + SLAM_ERROR("Failed to add imu calibration"); 1279 1279 + } 1221 1280 } 1222 1281 1223 1282 static void 1224 1283 send_calibration(const TrackerSlam &t, const t_slam_calibration &c) 1225 1284 { 1285 1285 + vit_tracker_capability_t caps; 1286 1286 + vit_result_t vres = t.vit.tracker_get_capabilities(t.tracker, &caps); 1287 1287 + if (vres != VIT_SUCCESS) { 1288 1288 + SLAM_ERROR("Failed to get VIT tracker capabilities"); 1289 1289 + return; 1290 1290 + } 1291 1291 + 1226 1292 // Try to send camera calibration data to the SLAM system 1227 1227 - for (int i = 0; i < c.cam_count; i++) { 1228 1228 - if (t.slam->supports_feature(F_ADD_CAMERA_CALIBRATION)) { 1293 1293 + if ((caps & VIT_TRACKER_CAPABILITY_CAMERA_CALIBRATION) != 0) { 1294 1294 + for (int i = 0; i < c.cam_count; i++) { 1229 1295 SLAM_INFO("Sending Camera %d calibration from Monado", i); 1230 1296 add_camera_calibration(t, &c.cams[i], i); 1231 1231 - } else { 1232 1232 - SLAM_INFO("Camera %d will use the calibration provided by the SLAM_CONFIG file", i); 1233 1297 } 1298 1298 + } else { 1299 1299 + SLAM_WARN("Tracker doesn't support camera calibration"); 1234 1300 } 1235 1301 1236 1302 // Try to send IMU calibration data to the SLAM system 1237 1237 - if (t.slam->supports_feature(F_ADD_IMU_CALIBRATION)) { 1303 1303 + if ((caps & VIT_TRACKER_CAPABILITY_IMU_CALIBRATION) != 0) { 1238 1304 SLAM_INFO("Sending IMU calibration from Monado"); 1239 1305 add_imu_calibration(t, &c.imu); 1240 1306 } else { 1241 1241 - SLAM_INFO("The IMU will use the calibration provided by the SLAM_CONFIG file"); 1307 1307 + SLAM_WARN("Tracker doesn't support IMU calibration"); 1242 1308 } 1243 1309 } 1244 1310 ··· 1323 1389 1324 1390 //! @todo There are many conversions like these between xrt and 1325 1391 //! slam_tracker.hpp types. Implement a casting mechanism to avoid copies. 1326 1326 - imu_sample sample{ts, a.x, a.y, a.z, w.x, w.y, w.z}; 1392 1392 + vit_imu_sample_t sample = {}; 1393 1393 + sample.timestamp = ts; 1394 1394 + sample.ax = a.x; 1395 1395 + sample.ay = a.y; 1396 1396 + sample.az = a.z; 1397 1397 + sample.wx = w.x; 1398 1398 + sample.wy = w.y; 1399 1399 + sample.wz = w.z; 1400 1400 + 1327 1401 if (t.submit) { 1328 1328 - t.slam->push_imu_sample(sample); 1402 1402 + t.vit.tracker_push_imu_sample(t.tracker, &sample); 1329 1403 } 1330 1404 1331 1405 xrt_sink_push_imu(t.euroc_recorder->imu, s); ··· 1340 1414 1341 1415 //! Push the frame to the external SLAM system 1342 1416 static void 1343 1343 - receive_frame(TrackerSlam &t, struct xrt_frame *frame, int cam_index) 1417 1417 + receive_frame(TrackerSlam &t, struct xrt_frame *frame, uint32_t cam_index) 1344 1418 { 1345 1419 XRT_TRACE_MARKER(); 1346 1420 1421 1421 + SLAM_DASSERT_(frame->timestamp < INT64_MAX); 1422 1422 + 1423 1423 + // Return early if we don't submit 1424 1424 + if (!t.submit) { 1425 1425 + return; 1426 1426 + } 1427 1427 + 1347 1428 if (cam_index == t.cam_count - 1) { 1348 1429 flush_poses(t); // Useful to flush SLAM poses when no openxr app is open 1349 1430 } 1431 1431 + 1350 1432 SLAM_DASSERT(t.last_cam_ts[0] != INT64_MIN || cam_index == 0, "First frame was not a cam0 frame"); 1351 1433 1352 1434 // Check monotonically increasing timestamps ··· 1360 1442 1361 1443 // Construct and send the image sample 1362 1444 cv::Mat img = t.cv_wrapper->wrap(frame); 1363 1363 - SLAM_DASSERT_(frame->timestamp < INT64_MAX); 1364 1364 - img_sample sample{ts, img, cam_index}; 1365 1365 - if (t.submit) { 1445 1445 + 1446 1446 + vit_img_sample sample = {}; 1447 1447 + sample.cam_index = cam_index; 1448 1448 + sample.timestamp = ts; 1449 1449 + sample.data = img.ptr(); 1450 1450 + sample.width = img.cols; 1451 1451 + sample.height = img.rows; 1452 1452 + sample.stride = img.step; 1453 1453 + sample.size = img.cols * img.rows; 1454 1454 + 1455 1455 + // TODO check format before 1456 1456 + switch (frame->format) { 1457 1457 + case XRT_FORMAT_L8: sample.format = VIT_IMAGE_FORMAT_L8; break; 1458 1458 + case XRT_FORMAT_R8G8B8: sample.format = VIT_IMAGE_FORMAT_R8G8B8; break; 1459 1459 + default: SLAM_ERROR("Unknown image format"); return; 1460 1460 + } 1461 1461 + 1462 1462 + // TODO masks 1463 1463 + 1464 1464 + { 1366 1465 XRT_TRACE_IDENT(slam_push); 1367 1367 - t.slam->push_frame(sample); 1466 1466 + t.vit.tracker_push_img_sample(t.tracker, &sample); 1368 1467 } 1369 1468 } 1370 1469 ··· 1392 1491 t_slam_receive_cam4, // 1393 1492 }; 1394 1493 1494 1494 + 1395 1495 extern "C" void 1396 1496 t_slam_node_break_apart(struct xrt_frame_node *node) 1397 1497 { ··· 1399 1499 if (t.ovr_tracker != NULL) { 1400 1500 t_openvr_tracker_stop(t.ovr_tracker); 1401 1501 } 1402 1402 - t.slam->finalize(); 1403 1403 - t.slam->stop(); 1404 1404 - os_thread_helper_stop_and_wait(&t.oth); 1502 1502 + 1503 1503 + vit_result_t vres = t.vit.tracker_stop(t.tracker); 1504 1504 + if (vres != VIT_SUCCESS) { 1505 1505 + SLAM_ERROR("Failed to stop VIT tracker"); 1506 1506 + return; 1507 1507 + } 1508 1508 + 1405 1509 SLAM_DEBUG("SLAM tracker dismantled"); 1406 1510 } 1407 1511 ··· 1414 1518 if (t.ovr_tracker != NULL) { 1415 1519 t_openvr_tracker_destroy(t.ovr_tracker); 1416 1520 } 1417 1417 - os_thread_helper_destroy(&t_ptr->oth); 1418 1521 delete t.gt.trajectory; 1419 1522 delete t.slam_times_writer; 1420 1523 delete t.slam_features_writer; ··· 1430 1533 os_mutex_destroy(&t.lock_ff); 1431 1534 m_ff_vec3_f32_free(&t.filter.pos_ff); 1432 1535 m_ff_vec3_f32_free(&t.filter.rot_ff); 1433 1433 - delete t_ptr->slam; 1536 1536 + 1434 1537 delete t_ptr->cv_wrapper; 1435 1435 - delete t_ptr; 1436 1436 - } 1437 1538 1438 1438 - //! Runs the external SLAM system in a separate thread 1439 1439 - extern "C" void * 1440 1440 - t_slam_run(void *ptr) 1441 1441 - { 1442 1442 - auto &t = *(TrackerSlam *)ptr; 1443 1443 - SLAM_DEBUG("SLAM tracker starting"); 1444 1444 - t.slam->start(); 1445 1445 - return NULL; 1539 1539 + t_ptr->vit.tracker_destroy(t_ptr->tracker); 1540 1540 + t_vit_bundle_unload(&t_ptr->vit); 1541 1541 + 1542 1542 + delete t_ptr; 1446 1543 } 1447 1544 1448 1448 - //! Starts t_slam_run 1449 1545 extern "C" int 1450 1546 t_slam_start(struct xrt_tracked_slam *xts) 1451 1547 { 1452 1548 auto &t = *container_of(xts, TrackerSlam, base); 1453 1453 - int ret = os_thread_helper_start(&t.oth, t_slam_run, &t); 1454 1454 - SLAM_ASSERT(ret == 0, "Unable to start thread"); 1549 1549 + vit_result_t vres = t.vit.tracker_start(t.tracker); 1550 1550 + if (vres != VIT_SUCCESS) { 1551 1551 + SLAM_ERROR("Failed to start VIT tracker"); 1552 1552 + return -1; 1553 1553 + } 1554 1554 + 1455 1555 SLAM_DEBUG("SLAM tracker started"); 1456 1456 - return ret; 1556 1556 + return 0; 1457 1557 } 1458 1558 1459 1559 extern "C" void 1460 1560 t_slam_fill_default_config(struct t_slam_tracker_config *config) 1461 1561 { 1462 1562 config->log_level = debug_get_log_option_slam_log(); 1563 1563 + config->vit_system_library_path = debug_get_option_vit_system_library_path(); 1463 1564 config->slam_config = debug_get_option_slam_config(); 1464 1565 config->slam_ui = debug_get_bool_option_slam_ui(); 1465 1566 config->submit_from_start = debug_get_bool_option_slam_submit_from_start(); ··· 1487 1588 1488 1589 enum u_logging_level log_level = config->log_level; 1489 1590 1490 1490 - // Check that the external SLAM system built is compatible 1491 1491 - int ima = IMPLEMENTATION_VERSION_MAJOR; 1492 1492 - int imi = IMPLEMENTATION_VERSION_MINOR; 1493 1493 - int ipa = IMPLEMENTATION_VERSION_PATCH; 1494 1494 - int hma = HEADER_VERSION_MAJOR; 1495 1495 - int hmi = HEADER_VERSION_MINOR; 1496 1496 - int hpa = HEADER_VERSION_PATCH; 1497 1497 - U_LOG_IFL_I(log_level, "External SLAM system built %d.%d.%d, expected %d.%d.%d.", ima, imi, ipa, hma, hmi, hpa); 1498 1498 - if (IMPLEMENTATION_VERSION_MAJOR != HEADER_VERSION_MAJOR) { 1499 1499 - U_LOG_IFL_E(log_level, "Incompatible external SLAM system found."); 1591 1591 + std::unique_ptr<TrackerSlam> t_ptr = std::make_unique<TrackerSlam>(); 1592 1592 + TrackerSlam &t = *t_ptr; 1593 1593 + 1594 1594 + t.log_level = log_level; 1595 1595 + 1596 1596 + if (config->vit_system_library_path == NULL) { 1597 1597 + SLAM_WARN("No VIT system library set, use VIT_SYSTEM_LIBRARY_PATH to set a tracker"); 1598 1598 + SLAM_WARN("Attempting to load 'libbasalt.so' from system"); 1599 1599 + config->vit_system_library_path = "libbasalt.so"; 1600 1600 + } 1601 1601 + 1602 1602 + SLAM_INFO("Loading VIT system library from '%s'", config->vit_system_library_path); 1603 1603 + 1604 1604 + if (!t_vit_bundle_load(&t.vit, config->vit_system_library_path)) { 1605 1605 + SLAM_ERROR("Failed to load VIT system library from '%s'", config->vit_system_library_path); 1500 1606 return -1; 1501 1607 } 1502 1502 - U_LOG_IFL_I(log_level, "Initializing compatible external SLAM system."); 1503 1608 1504 1609 // Check the user has provided a SLAM_CONFIG file 1505 1610 const char *config_file = config->slam_config; 1506 1611 bool some_calib = config->slam_calib != nullptr; 1507 1612 if (!config_file && !some_calib) { 1508 1508 - U_LOG_IFL_W(log_level, "Unable to determine sensor calibration, did you forget to set SLAM_CONFIG?"); 1613 1613 + SLAM_WARN("Unable to determine sensor calibration, did you forget to set SLAM_CONFIG?"); 1509 1614 return -1; 1510 1615 } 1511 1616 1512 1512 - auto &t = *(new TrackerSlam{}); 1513 1513 - t.log_level = log_level; 1514 1514 - t.cv_wrapper = new MatFrame(); 1515 1515 - 1516 1516 - t.base.get_tracked_pose = t_slam_get_tracked_pose; 1517 1517 - 1518 1518 - slam_config system_config = {}; 1519 1519 - system_config.config_file = config_file ? make_shared<string>(config_file) : nullptr; 1617 1617 + struct vit_config system_config = {}; 1618 1618 + system_config.file = config_file; 1520 1619 system_config.cam_count = config->cam_count; 1521 1620 system_config.show_ui = config->slam_ui; 1522 1522 - t.slam = new slam_tracker{system_config}; 1621 1621 + 1622 1622 + vit_result_t vres = t.vit.tracker_create(&system_config, &t.tracker); 1623 1623 + if (vres != VIT_SUCCESS) { 1624 1624 + SLAM_ERROR("Failed to create VIT tracker"); 1625 1625 + return -1; 1626 1626 + } 1627 1627 + 1628 1628 + vres = t.vit.tracker_get_pose_capabilities(t.tracker, &t.caps); 1629 1629 + if (vres != VIT_SUCCESS) { 1630 1630 + SLAM_ERROR("Failed to get VIT tracker pose capabilities"); 1631 1631 + return -1; 1632 1632 + } 1633 1633 + 1634 1634 + t.base.get_tracked_pose = t_slam_get_tracked_pose; 1523 1635 1524 1636 if (!config_file) { 1525 1637 SLAM_INFO("Using calibration from driver and default pipeline settings"); ··· 1527 1639 } else { 1528 1640 SLAM_INFO("Using sensor calibration provided by the SLAM_CONFIG file"); 1529 1641 } 1530 1530 - 1531 1531 - t.slam->initialize(); 1532 1642 1533 1643 SLAM_ASSERT(t_slam_receive_cam[ARRAY_SIZE(t_slam_receive_cam) - 1] != nullptr, "See `cam_sink_push` docs"); 1534 1644 t.sinks.cam_count = config->cam_count; ··· 1549 1659 t.node.break_apart = t_slam_node_break_apart; 1550 1660 t.node.destroy = t_slam_node_destroy; 1551 1661 1552 1552 - int ret = os_thread_helper_init(&t.oth); 1553 1553 - SLAM_ASSERT(ret == 0, "Unable to initialize thread"); 1554 1554 - 1555 1662 xrt_frame_context_add(xfctx, &t.node); 1556 1663 1557 1664 t.euroc_recorder = euroc_recorder_create(xfctx, NULL, t.cam_count, false); ··· 1566 1673 1567 1674 t.gt.trajectory = new Trajectory{}; 1568 1675 1569 1569 - // Setup timing extension 1570 1570 - 1571 1571 - // Probe for timing extension. 1572 1572 - bool has_timing_extension = t.slam->supports_feature(F_ENABLE_POSE_EXT_TIMING); 1573 1573 - t.timing.ext_available = has_timing_extension; 1574 1574 - 1575 1575 - // We provide two timing columns by default, even if there is no extension support 1576 1576 - t.timing.columns = {"sampled", "received_by_monado"}; 1577 1577 - 1578 1578 - if (has_timing_extension) { 1579 1579 - bool enable_timing_extension = config->timing_stat; 1580 1580 - 1581 1581 - const auto params = make_shared<FPARAMS_EPET>(enable_timing_extension); 1582 1582 - shared_ptr<void> result; 1583 1583 - t.slam->use_feature(F_ENABLE_POSE_EXT_TIMING, params, result); 1584 1584 - vector<string> cols = *std::static_pointer_cast<FRESULT_EPET>(result); 1585 1585 - 1586 1586 - t.timing.columns.insert(t.timing.columns.begin() + 1, cols.begin(), cols.end()); 1587 1587 - t.timing.ext_enabled = enable_timing_extension; 1588 1588 - } 1589 1589 - 1590 1590 - // Setup features extension 1591 1591 - bool has_features_extension = t.slam->supports_feature(F_ENABLE_POSE_EXT_FEATURES); 1592 1592 - t.features.ext_available = has_features_extension; 1593 1593 - if (has_features_extension) { 1594 1594 - bool enable_features_extension = config->features_stat; 1595 1595 - 1596 1596 - const auto params = make_shared<FPARAMS_EPET>(enable_features_extension); 1597 1597 - shared_ptr<void> _; 1598 1598 - t.slam->use_feature(F_ENABLE_POSE_EXT_FEATURES, params, _); 1599 1599 - 1600 1600 - t.features.ext_enabled = enable_features_extension; 1601 1601 - } 1602 1602 - 1603 1676 // Setup CSV files 1604 1677 bool write_csvs = config->write_csvs; 1605 1678 string dir = config->csv_path; ··· 1621 1694 } 1622 1695 } 1623 1696 1624 1624 - *out_xts = &t.base; 1625 1625 - *out_sink = &t.sinks; 1697 1697 + // Get ownership 1698 1698 + TrackerSlam *tracker = t_ptr.release(); 1699 1699 + 1700 1700 + *out_xts = &tracker->base; 1701 1701 + *out_sink = &tracker->sinks; 1626 1702 1627 1703 SLAM_DEBUG("SLAM tracker created"); 1628 1704 return 0;

+7 -6

src/xrt/auxiliary/tracking/t_tracking.h

reviewed

··· 646 646 */ 647 647 struct t_slam_tracker_config 648 648 { 649 649 - enum u_logging_level log_level; //!< SLAM tracking logging level 650 650 - const char *slam_config; //!< Config file path, format is specific to the SLAM implementation in use 651 651 - int cam_count; //!< Number of cameras in use 652 652 - bool slam_ui; //!< Whether to open the external UI of the external SLAM system 653 653 - bool submit_from_start; //!< Whether to submit data to the SLAM tracker without user action 654 654 - int openvr_groundtruth_device; //!< If >0, use lighthouse as groundtruth, see @ref openvr_device 649 649 + enum u_logging_level log_level; //!< SLAM tracking logging level 650 650 + const char *vit_system_library_path; //!< Path to the VIT system library 651 651 + const char *slam_config; //!< Config file path, format is specific to the SLAM implementation in use 652 652 + int cam_count; //!< Number of cameras in use 653 653 + bool slam_ui; //!< Whether to open the external UI of the external SLAM system 654 654 + bool submit_from_start; //!< Whether to submit data to the SLAM tracker without user action 655 655 + int openvr_groundtruth_device; //!< If >0, use lighthouse as groundtruth, see @ref openvr_device 655 656 enum t_slam_prediction_type prediction; //!< Which level of prediction to use 656 657 bool write_csvs; //!< Whether to enable CSV writers from the start for later analysis 657 658 const char *csv_path; //!< Path to write CSVs to