external/slam: Update header to 4.0.0 · matrixfurry.com/monado@536001e

+88 -10

2 changed files

expand all

src

external

slam_tracker

slam_tracker.hpp

xrt

auxiliary

tracking

t_tracker_slam.cpp

+85 -8

src/external/slam_tracker/slam_tracker.hpp

··· 19 19 #include <opencv2/core/mat.hpp> 20 20 21 21 #include <cstdint> 22 + #include <iostream> 22 23 #include <memory> 23 24 #include <string> 24 25 #include <vector> 26 + #include <chrono> 25 27 26 28 namespace xrt::auxiliary::tracking::slam { 27 29 28 30 // For implementation: same as IMPLEMENTATION_VERSION_* 29 31 // For user: expected IMPLEMENTATION_VERSION_*. Should be checked in runtime. 30 - constexpr int HEADER_VERSION_MAJOR = 3; //!< API Breakages 32 + constexpr int HEADER_VERSION_MAJOR = 4; //!< API Breakages 31 33 constexpr int HEADER_VERSION_MINOR = 0; //!< Backwards compatible API changes 32 34 constexpr int HEADER_VERSION_PATCH = 0; //!< Backw. comp. .h-implemented changes 33 35 ··· 199 201 double fx, fy; //<! Focal point 200 202 double cx, cy; //<! Principal point 201 203 std::string distortion_model; //!< Models like: none, rt4, rt5, rt8, kb4 202 - std::vector<double> distortion; //!< Parameters for the distortion_model 204 + std::vector<double> distortion{}; //!< Parameters for the distortion_model 203 205 cv::Matx<double, 4, 4> t_imu_cam; //!< Transformation from IMU to camera 204 206 }; 205 207 ··· 254 256 /*! 255 257 * Feature ENABLE_POSE_EXT_TIMING 256 258 * 257 - * Use it after constructor but before `start()`. 259 + * Enable/disable adding internal timestamps to the estimated poses. 258 260 * Returns a vector with names for the timestamps in `pose_ext_timing`. 259 261 */ 260 - DEFINE_FEATURE(ENABLE_POSE_EXT_TIMING, EPET, 3, void, std::vector<std::string>) 262 + DEFINE_FEATURE(ENABLE_POSE_EXT_TIMING, EPET, 3, bool, std::vector<std::string>) 263 + 264 + /*! 265 + * Feature ENABLE_POSE_EXT_FEATURES 266 + * 267 + * Enable/disable adding feature information to the estimated poses. 268 + */ 269 + DEFINE_FEATURE(ENABLE_POSE_EXT_FEATURES, EPEF, 4, bool, void) 261 270 262 271 /* 263 272 * Pose extensions ··· 272 281 enum class pose_ext_type : int { 273 282 UNDEFINED = 0, 274 283 TIMING = 1, 284 + FEATURES = 2, 275 285 }; 276 286 277 287 struct pose_extension { ··· 290 300 return pe; 291 301 } 292 302 293 - struct pose_ext_timing : pose_extension { 294 - //! Internal pipeline timestamps of interest when generating the pose. In 295 - //! steady clock ns. Must have the same number of elements in the same run. 296 - std::vector<std::int64_t> timestamps{}; 303 + // Timing pose extension 304 + struct pose_ext_timing_data { 305 + //! Internal pipeline stage timestamps of interest when generating the pose. 306 + //! In steady clock ns. Must have the same number of elements in the same run. 307 + std::vector<std::int64_t> timing{}; 308 + 309 + //! Names of each timing stage. Should point to static memory. 310 + const std::vector<std::string> *timing_titles = nullptr; 311 + }; 297 312 313 + struct pose_ext_timing : pose_extension, pose_ext_timing_data { 298 314 pose_ext_timing() : pose_extension{pose_ext_type::TIMING} {} 315 + pose_ext_timing(const pose_ext_timing_data &petd) 316 + : pose_extension{pose_ext_type::TIMING}, pose_ext_timing_data{petd} {} 317 + }; 318 + 319 + // Features pose extension 320 + struct pose_ext_features_data { 321 + struct feature { 322 + std::int64_t id; 323 + float u; 324 + float v; 325 + float depth; 326 + }; 327 + 328 + std::vector<std::vector<feature>> features_per_cam{}; 329 + }; 330 + 331 + struct pose_ext_features : pose_extension, pose_ext_features_data { 332 + pose_ext_features() : pose_extension{pose_ext_type::FEATURES} {} 333 + pose_ext_features(const pose_ext_features_data &pefd) 334 + : pose_extension{pose_ext_type::FEATURES}, pose_ext_features_data{pefd} {} 335 + }; 336 + 337 + /*! 338 + * Utility object to keep track of different stats for a particular timestamp. 339 + * Stats usually correspond with a particular pose extension. 340 + */ 341 + struct timestats : pose_ext_timing_data, pose_ext_features_data { 342 + using ptr = std::shared_ptr<timestats>; 343 + 344 + std::int64_t ts = -1; 345 + bool timing_enabled = false; 346 + bool features_enabled = false; 347 + 348 + void addTime(const char *name, int64_t ts = INT64_MIN) { 349 + if (!timing_enabled) { 350 + return; 351 + } 352 + if (timing_titles) { 353 + std::string expected = timing_titles->at(timing.size()); 354 + if (expected != name) { 355 + std::cout << "Invalid timing stage\n"; 356 + std::cout << "expected: " << expected; 357 + std::cout << ", got: " << name << std::endl; 358 + exit(EXIT_FAILURE); 359 + } 360 + } 361 + if (ts == INT64_MIN) { 362 + ts = std::chrono::steady_clock::now().time_since_epoch().count(); 363 + } 364 + timing.push_back(ts); 365 + } 366 + 367 + void addFeature(size_t cam, const feature &f) { 368 + if (!features_enabled) { 369 + return; 370 + } 371 + if (cam >= features_per_cam.size()) { 372 + features_per_cam.resize(cam + 1); 373 + } 374 + features_per_cam.at(cam).push_back(f); 375 + } 299 376 }; 300 377 301 378 } // namespace xrt::auxiliary::tracking::slam

+3 -2

src/xrt/auxiliary/tracking/t_tracker_slam.cpp

··· 444 444 shared_ptr<pose_extension> ext = p.find_pose_extension(pose_ext_type::TIMING); 445 445 SLAM_DASSERT(ext != nullptr, "An enabled extension was null"); 446 446 pose_ext_timing pet = *std::static_pointer_cast<pose_ext_timing>(ext); 447 - tss.insert(tss.begin() + 1, pet.timestamps.begin(), pet.timestamps.end()); 447 + tss.insert(tss.begin() + 1, pet.timing.begin(), pet.timing.end()); 448 448 } 449 449 450 450 // The two timestamps to compare in the graph ··· 1144 1144 t.timing.columns = {"sampled", "received_by_monado"}; 1145 1145 bool has_timing_extension = t.slam->supports_feature(F_ENABLE_POSE_EXT_TIMING); 1146 1146 if (has_timing_extension) { 1147 + const auto params = make_shared<FPARAMS_EPET>(true); 1147 1148 shared_ptr<void> result; 1148 - t.slam->use_feature(FID_EPET, nullptr, result); 1149 + t.slam->use_feature(FID_EPET, params, result); 1149 1150 t.timing.ext_enabled = true; 1150 1151 1151 1152 vector<string> cols = *std::static_pointer_cast<FRESULT_EPET>(result);

Configure Feed

Configure Feed