+1 -9

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 interface 83 83 + # External VIT 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) 87 87 - target_include_directories( 88 88 - xrt-external-slam INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}/slam_tracker 89 89 - ) 90 90 - target_include_directories(xrt-external-slam SYSTEM INTERFACE ${SLAM_INCLUDE_DIRS}) 91 91 - target_link_libraries(xrt-external-slam INTERFACE ${SLAM_LDFLAGS}) 92 92 - 93 85 add_library(xrt-external-vit INTERFACE) 94 86 target_include_directories( 95 87 xrt-external-vit INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}/vit_includes

-396

src/external/slam_tracker/slam_tracker.hpp

reviewed

··· 1 1 - // Copyright 2021, Collabora, Ltd. 2 2 - // SPDX-License-Identifier: BSL-1.0 3 3 - /*! 4 4 - * @file 5 5 - * @brief SLAM tracker class header for usage in Monado. 6 6 - * @author Mateo de Mayo <mateo.demayo@collabora.com> 7 7 - * @ingroup aux_tracking 8 8 - * 9 9 - * This file contains the declaration of the @ref slam_tracker class. This 10 10 - * header is intended to appear in both Monado and an external SLAM system. The 11 11 - * implementation of `slam_tracker` is provided by the external system. 12 12 - * Additional data types are declared for the communication between Monado and 13 13 - * the system. 14 14 - * 15 15 - */ 16 16 - 17 17 - #pragma once 18 18 - 19 19 - #include <opencv2/core/mat.hpp> 20 20 - 21 21 - #include <cstdint> 22 22 - #include <iostream> 23 23 - #include <memory> 24 24 - #include <string> 25 25 - #include <vector> 26 26 - #include <chrono> 27 27 - 28 28 - namespace xrt::auxiliary::tracking::slam { 29 29 - 30 30 - // For implementation: same as IMPLEMENTATION_VERSION_* 31 31 - // For user: expected IMPLEMENTATION_VERSION_*. Should be checked in runtime. 32 32 - constexpr int HEADER_VERSION_MAJOR = 7; //!< API Breakages 33 33 - constexpr int HEADER_VERSION_MINOR = 0; //!< Backwards compatible API changes 34 34 - constexpr int HEADER_VERSION_PATCH = 0; //!< Backw. comp. .h-implemented changes 35 35 - 36 36 - // Which header version the external system is implementing. 37 37 - extern const int IMPLEMENTATION_VERSION_MAJOR; 38 38 - extern const int IMPLEMENTATION_VERSION_MINOR; 39 39 - extern const int IMPLEMENTATION_VERSION_PATCH; 40 40 - 41 41 - enum class pose_ext_type : int; 42 42 - 43 43 - /*! 44 44 - * @brief Standard pose type to communicate Monado with the external SLAM system 45 45 - */ 46 46 - struct pose { 47 47 - std::int64_t timestamp; //!< In same clock as input samples 48 48 - float px, py, pz; //!< Position vector 49 49 - float rx, ry, rz, rw = 1; //!< Orientation quaternion 50 50 - std::shared_ptr<struct pose_extension> next = nullptr; 51 51 - 52 52 - pose() = default; 53 53 - pose(std::int64_t timestamp, // 54 54 - float px, float py, float pz, // 55 55 - float rx, float ry, float rz, float rw) 56 56 - : timestamp(timestamp), // 57 57 - px(px), py(py), pz(pz), // 58 58 - rx(rx), ry(ry), rz(rz), rw(rw) {} 59 59 - 60 60 - std::shared_ptr<pose_extension> 61 61 - find_pose_extension(pose_ext_type required_type) const; 62 62 - }; 63 63 - 64 64 - struct rect { 65 65 - float x, y, w, h; 66 66 - }; 67 67 - 68 68 - /*! 69 69 - * @brief IMU Sample type to pass around between programs 70 70 - */ 71 71 - struct imu_sample { 72 72 - std::int64_t timestamp; //!< In nanoseconds 73 73 - double ax, ay, az; //!< Accel in meters per second squared (m / s^2) 74 74 - double wx, wy, wz; //!< Gyro in radians per second (rad / s) 75 75 - imu_sample() = default; 76 76 - imu_sample(std::int64_t timestamp, double ax, double ay, double az, double wx, 77 77 - double wy, double wz) 78 78 - : timestamp(timestamp), ax(ax), ay(ay), az(az), wx(wx), wy(wy), wz(wz) {} 79 79 - }; 80 80 - 81 81 - /*! 82 82 - * @brief Image sample type to pass around between programs. It is expected that 83 83 - * any SLAM system takes OpenCV matrices as input. 84 84 - */ 85 85 - struct img_sample { 86 86 - std::int64_t timestamp; 87 87 - cv::Mat img; 88 88 - int cam_index; 89 89 - std::vector<rect> masks{}; //!< Masks to ignore 90 90 - img_sample() = default; 91 91 - img_sample(std::int64_t timestamp, const cv::Mat &img, int cam_index) 92 92 - : timestamp(timestamp), img(img), cam_index(cam_index) {} 93 93 - }; 94 94 - 95 95 - /*! 96 96 - * @brief Parameters for creating the system pipeline. 97 97 - */ 98 98 - struct slam_config { 99 99 - //! Path to a implementation-specific config file. If null, use defaults. 100 100 - std::shared_ptr<std::string> config_file; 101 101 - 102 102 - //! Number of cameras to use. Required. 103 103 - int cam_count = -1; 104 104 - 105 105 - //! If supported, whether to open the system's UI. 106 106 - bool show_ui = false; 107 107 - }; 108 108 - 109 109 - /*! 110 110 - * @brief slam_tracker serves as an interface between Monado and external SLAM 111 111 - * systems. 112 112 - * 113 113 - * This class uses the pointer-to-implementation pattern, and its implementation 114 114 - * should be provided by an external SLAM system. 115 115 - */ 116 116 - struct slam_tracker { 117 117 - slam_tracker(const slam_config &config); 118 118 - ~slam_tracker(); 119 119 - 120 120 - slam_tracker(const slam_tracker &) = delete; 121 121 - slam_tracker &operator=(const slam_tracker &) = delete; 122 122 - 123 123 - void initialize(); 124 124 - void start(); 125 125 - bool is_running(); 126 126 - void stop(); 127 127 - void finalize(); 128 128 - 129 129 - /*! 130 130 - * @brief Push an IMU sample into the tracker. 131 131 - * 132 132 - * There must be a single producer thread pushing samples. 133 133 - * Samples must have monotonically increasing timestamps. 134 134 - * The implementation must be non-blocking. 135 135 - * Thus, a separate consumer thread should process the samples. 136 136 - */ 137 137 - void push_imu_sample(const imu_sample &sample); 138 138 - 139 139 - /*! 140 140 - * @brief Push an image sample into the tracker. 141 141 - * 142 142 - * Same conditions as @ref push_imu_sample apply. 143 143 - * When using N>1 cameras, the N frames must be pushed following cam_id order. 144 144 - * The bundle of N frames must have the same timestamps. 145 145 - */ 146 146 - void push_frame(const img_sample &sample); 147 147 - 148 148 - /*! 149 149 - * @brief Get the latest tracked pose from the SLAM system. 150 150 - * 151 151 - * There must be a single thread consuming this method. 152 152 - * 153 153 - * @param[out] out_pose Dequeued pose. 154 154 - * @return true If a new pose was dequeued into @p out_pose. 155 155 - * @return false If there was no pose to dequeue. 156 156 - */ 157 157 - bool try_dequeue_pose(pose &out_pose); 158 158 - 159 159 - //! Asks the SLAM system whether it supports a specific feature. 160 160 - bool supports_feature(int feature_id); 161 161 - 162 162 - /*! 163 163 - * @brief Use a special feature of the SLAM tracker. 164 164 - * 165 165 - * This method uses heap allocated objects for passing parameters and 166 166 - * obtaining the results. Use `std::static_pointer_cast` to shared pointers to 167 167 - * the expected types. 168 168 - * 169 169 - * @param feature_id Id of the special feature. 170 170 - * @param params Pointer to the parameter object for this feature. 171 171 - * @param result Pointer to the result produced by the feature call. 172 172 - * @return false if the feature was not supported, true otherwise. 173 173 - */ 174 174 - bool use_feature(int feature_id, const std::shared_ptr<void> &params, 175 175 - std::shared_ptr<void> &result); 176 176 - 177 177 - private: 178 178 - struct implementation; 179 179 - std::unique_ptr<implementation> impl; 180 180 - }; 181 181 - 182 182 - /* 183 183 - * Special features 184 184 - * 185 185 - * A special feature is comprised of an ID, a PARAMS type and a RESULT type. It 186 186 - * can be defined using DEFINE_FEATURE. Once defined, the definition should not 187 187 - * suffer future changes. 188 188 - * 189 189 - * One of the main concerns in the features interface is the ability to add new 190 190 - * features without being required to update the SLAM systems that are not 191 191 - * interested in implementing the feature. 192 192 - * 193 193 - */ 194 194 - 195 195 - #define DEFINE_FEATURE(NAME, SHORT_NAME, ID, PARAMS_TYPE, RESULT_TYPE) \ 196 196 - using FPARAMS_##SHORT_NAME = PARAMS_TYPE; \ 197 197 - using FRESULT_##SHORT_NAME = RESULT_TYPE; \ 198 198 - constexpr int FID_##SHORT_NAME = ID; \ 199 199 - constexpr int F_##NAME = ID; 200 200 - 201 201 - /*! 202 202 - * Container of parameters for a pinhole camera calibration (fx, fy, cx, cy) 203 203 - * with an optional distortion. 204 204 - * 205 205 - *`distortion_model` and its corresponding `distortion` parameters are not 206 206 - * standardized in this struct to facilitate implementation prototyping. 207 207 - */ 208 208 - struct cam_calibration { 209 209 - int cam_index; //!< For multi-camera setups. For stereo 0 ~ left, 1 ~ right. 210 210 - int width, height; //<! Resolution 211 211 - double frequency; //<! Frames per second 212 212 - double fx, fy; //<! Focal point 213 213 - double cx, cy; //<! Principal point 214 214 - std::string distortion_model; //!< Models like: none, rt4, rt5, rt8, kb4 215 215 - std::vector<double> distortion{}; //!< Parameters for the distortion_model 216 216 - cv::Matx<double, 4, 4> t_imu_cam; //!< Transformation from IMU to camera 217 217 - }; 218 218 - 219 219 - struct inertial_calibration { 220 220 - // Calibration intrinsics to apply to each raw measurement. 221 221 - 222 222 - //! This transform will be applied to raw measurements. 223 223 - cv::Matx<double, 3, 3> transform; 224 224 - 225 225 - //! Offset to add to raw measurements to; called bias in other contexts. 226 226 - cv::Matx<double, 3, 1> offset; 227 227 - 228 228 - // Parameters for the random processes that model this IMU. See section "2.1 229 229 - // Gyro Noise Model" of N. Trawny and S. I. Roumeliotis, "Indirect Kalman 230 230 - // Filter for 3D Attitude Estimation". Analogous for accelerometers. 231 231 - // http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf#page=15 232 232 - 233 233 - //! IMU internal bias ~ wiener process with steps N(0, σ²); this field is σ; 234 234 - //! [σ] = U / sqrt(sec³) with U = rad if gyroscope, U = m/s if accelerometer. 235 235 - cv::Matx<double, 3, 1> bias_std; 236 236 - 237 237 - //! IMU measurement noise ~ N(0, σ²); this field is σ. 238 238 - //! [σ] = U / sqrt(sec) with U = rad if gyroscope, U = m/s if accelerometer. 239 239 - cv::Matx<double, 3, 1> noise_std; 240 240 - 241 241 - inertial_calibration() : transform(cv::Matx<double, 3, 3>::eye()) {} 242 242 - }; 243 243 - 244 244 - struct imu_calibration { 245 245 - int imu_index; //!< For multi-imu setups. Usually just 0. 246 246 - double frequency; //!< Samples per second 247 247 - inertial_calibration accel; 248 248 - inertial_calibration gyro; 249 249 - }; 250 250 - 251 251 - /*! 252 252 - * Feature ADD_CAMERA_CALIBRATION 253 253 - * 254 254 - * Use it after constructor but before `start()` to write or overwrite camera 255 255 - * calibration data that might come from the system-specific config file. 256 256 - */ 257 257 - DEFINE_FEATURE(ADD_CAMERA_CALIBRATION, ACC, 1, cam_calibration, void) 258 258 - 259 259 - /*! 260 260 - * Feature ADD_IMU_CALIBRATION 261 261 - * 262 262 - * Use it after constructor but before `start()` to write or overwrite IMU 263 263 - * calibration data that might come from the system-specific config file. 264 264 - */ 265 265 - DEFINE_FEATURE(ADD_IMU_CALIBRATION, AIC, 2, imu_calibration, void) 266 266 - 267 267 - /*! 268 268 - * Feature ENABLE_POSE_EXT_TIMING 269 269 - * 270 270 - * Enable/disable adding internal timestamps to the estimated poses. 271 271 - * Returns a vector with names for the timestamps in `pose_ext_timing`. 272 272 - */ 273 273 - DEFINE_FEATURE(ENABLE_POSE_EXT_TIMING, EPET, 3, bool, std::vector<std::string>) 274 274 - 275 275 - /*! 276 276 - * Feature ENABLE_POSE_EXT_FEATURES 277 277 - * 278 278 - * Enable/disable adding feature information to the estimated poses. 279 279 - */ 280 280 - DEFINE_FEATURE(ENABLE_POSE_EXT_FEATURES, EPEF, 4, bool, void) 281 281 - 282 282 - /*! 283 283 - * Feature RESET_TRACKER_STATE 284 284 - * 285 285 - * Reset tracker state. 286 286 - */ 287 287 - DEFINE_FEATURE(RESET_TRACKER_STATE, RS, 5, void, void) 288 288 - 289 289 - /* 290 290 - * Pose extensions 291 291 - * 292 292 - * A pose extension is a struct that gets linked in the `pose.next` field. You 293 293 - * first ask if the implementation supports enabling such extension with a 294 294 - * `supports_feature()` call with the appropriate `ENABLE_POSE_EXT_*`. Then, it 295 295 - * can be enabled with the corresponding `use_feature()` call. 296 296 - * 297 297 - */ 298 298 - 299 299 - enum class pose_ext_type : int { 300 300 - UNDEFINED = 0, 301 301 - TIMING = 1, 302 302 - FEATURES = 2, 303 303 - }; 304 304 - 305 305 - struct pose_extension { 306 306 - pose_ext_type type = pose_ext_type::UNDEFINED; 307 307 - std::shared_ptr<pose_extension> next = nullptr; 308 308 - 309 309 - pose_extension(pose_ext_type type) : type(type) {} 310 310 - }; 311 311 - 312 312 - inline std::shared_ptr<pose_extension> 313 313 - pose::find_pose_extension(pose_ext_type required_type) const { 314 314 - std::shared_ptr<pose_extension> pe = next; 315 315 - while (pe != nullptr && pe->type != required_type) { 316 316 - pe = pe->next; 317 317 - } 318 318 - return pe; 319 319 - } 320 320 - 321 321 - // Timing pose extension 322 322 - struct pose_ext_timing_data { 323 323 - //! Internal pipeline stage timestamps of interest when generating the pose. 324 324 - //! In steady clock ns. Must have the same number of elements in the same run. 325 325 - std::vector<std::int64_t> timing{}; 326 326 - 327 327 - //! Names of each timing stage. Should point to static memory. 328 328 - const std::vector<std::string> *timing_titles = nullptr; 329 329 - }; 330 330 - 331 331 - struct pose_ext_timing : pose_extension, pose_ext_timing_data { 332 332 - pose_ext_timing() : pose_extension{pose_ext_type::TIMING} {} 333 333 - pose_ext_timing(const pose_ext_timing_data &petd) 334 334 - : pose_extension{pose_ext_type::TIMING}, pose_ext_timing_data{petd} {} 335 335 - }; 336 336 - 337 337 - // Features pose extension 338 338 - struct pose_ext_features_data { 339 339 - struct feature { 340 340 - std::int64_t id; 341 341 - float u; 342 342 - float v; 343 343 - float depth; 344 344 - }; 345 345 - 346 346 - std::vector<std::vector<feature>> features_per_cam{}; 347 347 - }; 348 348 - 349 349 - struct pose_ext_features : pose_extension, pose_ext_features_data { 350 350 - pose_ext_features() : pose_extension{pose_ext_type::FEATURES} {} 351 351 - pose_ext_features(const pose_ext_features_data &pefd) 352 352 - : pose_extension{pose_ext_type::FEATURES}, pose_ext_features_data{pefd} {} 353 353 - }; 354 354 - 355 355 - /*! 356 356 - * Utility object to keep track of different stats for a particular timestamp. 357 357 - * Stats usually correspond with a particular pose extension. 358 358 - */ 359 359 - struct timestats : pose_ext_timing_data, pose_ext_features_data { 360 360 - using ptr = std::shared_ptr<timestats>; 361 361 - 362 362 - std::int64_t ts = -1; 363 363 - bool timing_enabled = false; 364 364 - bool features_enabled = false; 365 365 - 366 366 - void addTime(const char *name, int64_t ts = INT64_MIN) { 367 367 - if (!timing_enabled) { 368 368 - return; 369 369 - } 370 370 - if (timing_titles) { 371 371 - std::string expected = timing_titles->at(timing.size()); 372 372 - if (expected != name) { 373 373 - std::cout << "Invalid timing stage\n"; 374 374 - std::cout << "expected: " << expected; 375 375 - std::cout << ", got: " << name << std::endl; 376 376 - exit(EXIT_FAILURE); 377 377 - } 378 378 - } 379 379 - if (ts == INT64_MIN) { 380 380 - ts = std::chrono::steady_clock::now().time_since_epoch().count(); 381 381 - } 382 382 - timing.push_back(ts); 383 383 - } 384 384 - 385 385 - void addFeature(size_t cam, const feature &f) { 386 386 - if (!features_enabled) { 387 387 - return; 388 388 - } 389 389 - if (cam >= features_per_cam.size()) { 390 390 - features_per_cam.resize(cam + 1); 391 391 - } 392 392 - features_per_cam.at(cam).push_back(f); 393 393 - } 394 394 - }; 395 395 - 396 396 - } // namespace xrt::auxiliary::tracking::slam

+2 -11

src/xrt/auxiliary/tracking/CMakeLists.txt

reviewed

··· 62 62 endif() 63 63 64 64 if(XRT_FEATURE_SLAM) 65 65 - target_sources(aux_tracking 66 66 - PRIVATE 67 67 - t_vit_loader.c 68 68 - t_tracker_slam.cpp 69 69 - ) 70 70 - target_link_libraries(aux_tracking 71 71 - PRIVATE 72 72 - xrt-external-slam 73 73 - xrt-external-vit 74 74 - ${CMAKE_DL_LIBS} 75 75 - ) 65 65 + target_sources(aux_tracking PRIVATE t_vit_loader.c t_tracker_slam.cpp) 66 66 + target_link_libraries(aux_tracking PRIVATE xrt-external-vit ${CMAKE_DL_LIBS}) 76 67 endif() 77 68 78 69 if(XRT_HAVE_OPENVR)