mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-02-13 17:20:09 +00:00
external/slam: Update header to 4.0.0
1. Add feature info pose extension 2. Make pose extensions toggleable on runtime 3. Add timestats helper for external system to keep track of info for pose extensions
This commit is contained in:
parent
49cd45b24e
commit
536001e2ad
src
93
src/external/slam_tracker/slam_tracker.hpp
vendored
93
src/external/slam_tracker/slam_tracker.hpp
vendored
|
@ -19,15 +19,17 @@
|
|||
#include <opencv2/core/mat.hpp>
|
||||
|
||||
#include <cstdint>
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <chrono>
|
||||
|
||||
namespace xrt::auxiliary::tracking::slam {
|
||||
|
||||
// For implementation: same as IMPLEMENTATION_VERSION_*
|
||||
// For user: expected IMPLEMENTATION_VERSION_*. Should be checked in runtime.
|
||||
constexpr int HEADER_VERSION_MAJOR = 3; //!< API Breakages
|
||||
constexpr int HEADER_VERSION_MAJOR = 4; //!< API Breakages
|
||||
constexpr int HEADER_VERSION_MINOR = 0; //!< Backwards compatible API changes
|
||||
constexpr int HEADER_VERSION_PATCH = 0; //!< Backw. comp. .h-implemented changes
|
||||
|
||||
|
@ -199,7 +201,7 @@ struct cam_calibration {
|
|||
double fx, fy; //<! Focal point
|
||||
double cx, cy; //<! Principal point
|
||||
std::string distortion_model; //!< Models like: none, rt4, rt5, rt8, kb4
|
||||
std::vector<double> distortion; //!< Parameters for the distortion_model
|
||||
std::vector<double> distortion{}; //!< Parameters for the distortion_model
|
||||
cv::Matx<double, 4, 4> t_imu_cam; //!< Transformation from IMU to camera
|
||||
};
|
||||
|
||||
|
@ -254,10 +256,17 @@ DEFINE_FEATURE(ADD_IMU_CALIBRATION, AIC, 2, imu_calibration, void)
|
|||
/*!
|
||||
* Feature ENABLE_POSE_EXT_TIMING
|
||||
*
|
||||
* Use it after constructor but before `start()`.
|
||||
* Enable/disable adding internal timestamps to the estimated poses.
|
||||
* Returns a vector with names for the timestamps in `pose_ext_timing`.
|
||||
*/
|
||||
DEFINE_FEATURE(ENABLE_POSE_EXT_TIMING, EPET, 3, void, std::vector<std::string>)
|
||||
DEFINE_FEATURE(ENABLE_POSE_EXT_TIMING, EPET, 3, bool, std::vector<std::string>)
|
||||
|
||||
/*!
|
||||
* Feature ENABLE_POSE_EXT_FEATURES
|
||||
*
|
||||
* Enable/disable adding feature information to the estimated poses.
|
||||
*/
|
||||
DEFINE_FEATURE(ENABLE_POSE_EXT_FEATURES, EPEF, 4, bool, void)
|
||||
|
||||
/*
|
||||
* Pose extensions
|
||||
|
@ -272,6 +281,7 @@ DEFINE_FEATURE(ENABLE_POSE_EXT_TIMING, EPET, 3, void, std::vector<std::string>)
|
|||
enum class pose_ext_type : int {
|
||||
UNDEFINED = 0,
|
||||
TIMING = 1,
|
||||
FEATURES = 2,
|
||||
};
|
||||
|
||||
struct pose_extension {
|
||||
|
@ -290,12 +300,79 @@ pose::find_pose_extension(pose_ext_type required_type) const {
|
|||
return pe;
|
||||
}
|
||||
|
||||
struct pose_ext_timing : pose_extension {
|
||||
//! Internal pipeline timestamps of interest when generating the pose. In
|
||||
//! steady clock ns. Must have the same number of elements in the same run.
|
||||
std::vector<std::int64_t> timestamps{};
|
||||
// Timing pose extension
|
||||
struct pose_ext_timing_data {
|
||||
//! Internal pipeline stage timestamps of interest when generating the pose.
|
||||
//! In steady clock ns. Must have the same number of elements in the same run.
|
||||
std::vector<std::int64_t> timing{};
|
||||
|
||||
//! Names of each timing stage. Should point to static memory.
|
||||
const std::vector<std::string> *timing_titles = nullptr;
|
||||
};
|
||||
|
||||
struct pose_ext_timing : pose_extension, pose_ext_timing_data {
|
||||
pose_ext_timing() : pose_extension{pose_ext_type::TIMING} {}
|
||||
pose_ext_timing(const pose_ext_timing_data &petd)
|
||||
: pose_extension{pose_ext_type::TIMING}, pose_ext_timing_data{petd} {}
|
||||
};
|
||||
|
||||
// Features pose extension
|
||||
struct pose_ext_features_data {
|
||||
struct feature {
|
||||
std::int64_t id;
|
||||
float u;
|
||||
float v;
|
||||
float depth;
|
||||
};
|
||||
|
||||
std::vector<std::vector<feature>> features_per_cam{};
|
||||
};
|
||||
|
||||
struct pose_ext_features : pose_extension, pose_ext_features_data {
|
||||
pose_ext_features() : pose_extension{pose_ext_type::FEATURES} {}
|
||||
pose_ext_features(const pose_ext_features_data &pefd)
|
||||
: pose_extension{pose_ext_type::FEATURES}, pose_ext_features_data{pefd} {}
|
||||
};
|
||||
|
||||
/*!
|
||||
* Utility object to keep track of different stats for a particular timestamp.
|
||||
* Stats usually correspond with a particular pose extension.
|
||||
*/
|
||||
struct timestats : pose_ext_timing_data, pose_ext_features_data {
|
||||
using ptr = std::shared_ptr<timestats>;
|
||||
|
||||
std::int64_t ts = -1;
|
||||
bool timing_enabled = false;
|
||||
bool features_enabled = false;
|
||||
|
||||
void addTime(const char *name, int64_t ts = INT64_MIN) {
|
||||
if (!timing_enabled) {
|
||||
return;
|
||||
}
|
||||
if (timing_titles) {
|
||||
std::string expected = timing_titles->at(timing.size());
|
||||
if (expected != name) {
|
||||
std::cout << "Invalid timing stage\n";
|
||||
std::cout << "expected: " << expected;
|
||||
std::cout << ", got: " << name << std::endl;
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
}
|
||||
if (ts == INT64_MIN) {
|
||||
ts = std::chrono::steady_clock::now().time_since_epoch().count();
|
||||
}
|
||||
timing.push_back(ts);
|
||||
}
|
||||
|
||||
void addFeature(size_t cam, const feature &f) {
|
||||
if (!features_enabled) {
|
||||
return;
|
||||
}
|
||||
if (cam >= features_per_cam.size()) {
|
||||
features_per_cam.resize(cam + 1);
|
||||
}
|
||||
features_per_cam.at(cam).push_back(f);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace xrt::auxiliary::tracking::slam
|
||||
|
|
|
@ -444,7 +444,7 @@ timing_ui_push(TrackerSlam &t, const pose &p)
|
|||
shared_ptr<pose_extension> ext = p.find_pose_extension(pose_ext_type::TIMING);
|
||||
SLAM_DASSERT(ext != nullptr, "An enabled extension was null");
|
||||
pose_ext_timing pet = *std::static_pointer_cast<pose_ext_timing>(ext);
|
||||
tss.insert(tss.begin() + 1, pet.timestamps.begin(), pet.timestamps.end());
|
||||
tss.insert(tss.begin() + 1, pet.timing.begin(), pet.timing.end());
|
||||
}
|
||||
|
||||
// The two timestamps to compare in the graph
|
||||
|
@ -1144,8 +1144,9 @@ t_slam_create(struct xrt_frame_context *xfctx,
|
|||
t.timing.columns = {"sampled", "received_by_monado"};
|
||||
bool has_timing_extension = t.slam->supports_feature(F_ENABLE_POSE_EXT_TIMING);
|
||||
if (has_timing_extension) {
|
||||
const auto params = make_shared<FPARAMS_EPET>(true);
|
||||
shared_ptr<void> result;
|
||||
t.slam->use_feature(FID_EPET, nullptr, result);
|
||||
t.slam->use_feature(FID_EPET, params, result);
|
||||
t.timing.ext_enabled = true;
|
||||
|
||||
vector<string> cols = *std::static_pointer_cast<FRESULT_EPET>(result);
|
||||
|
|
Loading…
Reference in a new issue