mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-02-10 15:58:06 +00:00
a/tracking: Migrate C++ APIs to the official namespaces now that we have some.
This commit is contained in:
parent
e008e4f319
commit
d88aefafbc
|
@ -27,7 +27,7 @@ DEBUG_GET_ONCE_BOOL_OPTION(hsv_filter, "T_DEBUG_HSV_FILTER", false)
|
|||
DEBUG_GET_ONCE_BOOL_OPTION(hsv_picker, "T_DEBUG_HSV_PICKER", false)
|
||||
DEBUG_GET_ONCE_BOOL_OPTION(hsv_viewer, "T_DEBUG_HSV_VIEWER", false)
|
||||
|
||||
|
||||
namespace xrt::auxiliary::tracking {
|
||||
/*
|
||||
*
|
||||
* Structs
|
||||
|
@ -1416,3 +1416,5 @@ NormalizedCoordsCache::getNormalizedVector(cv::Point2f origCoords) const
|
|||
auto z = -std::sqrt(1.f - pt.dot(pt));
|
||||
return {pt[0], pt[1], z};
|
||||
}
|
||||
|
||||
} // namespace xrt::auxiliary::tracking
|
||||
|
|
|
@ -20,6 +20,7 @@
|
|||
#include <opencv2/opencv.hpp>
|
||||
#include <sys/stat.h>
|
||||
|
||||
namespace xrt::auxiliary::tracking {
|
||||
|
||||
/*!
|
||||
* @brief Essential calibration data wrapped for C++.
|
||||
|
@ -290,3 +291,5 @@ private:
|
|||
cv::Mat_<float> cacheX_;
|
||||
cv::Mat_<float> cacheY_;
|
||||
};
|
||||
|
||||
} // namespace xrt::auxiliary::tracking
|
||||
|
|
|
@ -13,6 +13,7 @@
|
|||
#include "util/u_misc.h"
|
||||
#include "util/u_logging.h"
|
||||
|
||||
|
||||
/*
|
||||
*
|
||||
* Pre-declar functions.
|
||||
|
@ -31,7 +32,7 @@ write_cv_mat(FILE *f, cv::Mat *m);
|
|||
* Refine and create functions.
|
||||
*
|
||||
*/
|
||||
|
||||
namespace xrt::auxiliary::tracking {
|
||||
RemapPair
|
||||
calibration_get_undistort_map(t_camera_calibration &calib,
|
||||
cv::InputArray rectify_transform_optional,
|
||||
|
@ -165,6 +166,7 @@ StereoRectificationMaps::StereoRectificationMaps(t_stereo_camera_calibration *da
|
|||
view[0].rectify = calibration_get_undistort_map(data->view[0], view[0].rotation_mat, view[0].projection_mat);
|
||||
view[1].rectify = calibration_get_undistort_map(data->view[1], view[1].rotation_mat, view[1].projection_mat);
|
||||
}
|
||||
} // namespace xrt::auxiliary::tracking
|
||||
|
||||
/*
|
||||
*
|
||||
|
@ -175,6 +177,7 @@ StereoRectificationMaps::StereoRectificationMaps(t_stereo_camera_calibration *da
|
|||
extern "C" bool
|
||||
t_stereo_camera_calibration_load_v1(FILE *calib_file, struct t_stereo_camera_calibration **out_data)
|
||||
{
|
||||
using xrt::auxiliary::tracking::StereoCameraCalibrationWrapper;
|
||||
t_stereo_camera_calibration *data_ptr = NULL;
|
||||
t_stereo_camera_calibration_alloc(&data_ptr);
|
||||
StereoCameraCalibrationWrapper wrapped(data_ptr);
|
||||
|
@ -255,6 +258,7 @@ t_stereo_camera_calibration_load_v1(FILE *calib_file, struct t_stereo_camera_cal
|
|||
extern "C" bool
|
||||
t_stereo_camera_calibration_save_v1(FILE *calib_file, struct t_stereo_camera_calibration *data)
|
||||
{
|
||||
using xrt::auxiliary::tracking::StereoCameraCalibrationWrapper;
|
||||
StereoCameraCalibrationWrapper wrapped(data);
|
||||
// Dummy matrix
|
||||
cv::Mat dummy;
|
||||
|
|
|
@ -22,7 +22,8 @@
|
|||
#include "flexkalman/PoseState.h"
|
||||
|
||||
|
||||
namespace xrt_fusion {
|
||||
namespace xrt::auxiliary::tracking {
|
||||
|
||||
namespace types = flexkalman::types;
|
||||
using flexkalman::types::Vector;
|
||||
|
||||
|
@ -227,4 +228,5 @@ private:
|
|||
MeasurementVector knownLocationInBodySpace_;
|
||||
MeasurementSquareMatrix covariance_;
|
||||
};
|
||||
} // namespace xrt_fusion
|
||||
|
||||
} // namespace xrt::auxiliary::tracking
|
||||
|
|
|
@ -16,6 +16,7 @@
|
|||
#include <opencv2/opencv.hpp>
|
||||
#include "util/u_frame.h"
|
||||
|
||||
namespace xrt::auxiliary::tracking {
|
||||
|
||||
struct HelperDebugSink
|
||||
{
|
||||
|
@ -124,3 +125,5 @@ public:
|
|||
xrt_frame_reference(&frame, NULL);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace xrt::auxiliary::tracking
|
||||
|
|
|
@ -16,13 +16,14 @@
|
|||
|
||||
#include <memory>
|
||||
|
||||
using xrt::auxiliary::tracking::SimpleIMUFusion;
|
||||
|
||||
struct imu_fusion
|
||||
{
|
||||
public:
|
||||
uint64_t time_ns{0};
|
||||
|
||||
xrt_fusion::SimpleIMUFusion simple_fusion;
|
||||
SimpleIMUFusion simple_fusion;
|
||||
|
||||
|
||||
public:
|
||||
|
|
|
@ -33,7 +33,11 @@ DEBUG_GET_ONCE_LOG_OPTION(simple_imu_log, "SIMPLE_IMU_LOG", U_LOGGING_WARN)
|
|||
#define SIMPLE_IMU_WARN(...) U_LOG_IFL_W(ll, __VA_ARGS__)
|
||||
#define SIMPLE_IMU_ERROR(...) U_LOG_IFL_E(ll, __VA_ARGS__)
|
||||
|
||||
namespace xrt_fusion {
|
||||
namespace xrt::auxiliary::tracking {
|
||||
|
||||
/*!
|
||||
* @brief A simple IMU fusion class.
|
||||
*/
|
||||
class SimpleIMUFusion
|
||||
{
|
||||
public:
|
||||
|
@ -304,4 +308,4 @@ SimpleIMUFusion::handleAccel(Eigen::Vector3d const &accel, timepoint_ns timestam
|
|||
return true;
|
||||
}
|
||||
|
||||
} // namespace xrt_fusion
|
||||
} // namespace xrt::auxiliary::tracking
|
||||
|
|
|
@ -20,8 +20,9 @@
|
|||
#include <type_traits>
|
||||
|
||||
|
||||
namespace xrt_fusion {
|
||||
namespace implementation {
|
||||
namespace xrt::auxiliary::tracking {
|
||||
|
||||
namespace detail {
|
||||
/*!
|
||||
* The shared implementation (between vector and scalar versions) of an
|
||||
* IIR low-pass filter.
|
||||
|
@ -99,7 +100,7 @@ namespace implementation {
|
|||
bool initialized{false};
|
||||
timepoint_ns filter_timestamp_ns{0};
|
||||
};
|
||||
} // namespace implementation
|
||||
} // namespace detail
|
||||
|
||||
/*!
|
||||
* A very simple low-pass filter, using a "one-pole infinite impulse response"
|
||||
|
@ -172,7 +173,7 @@ public:
|
|||
}
|
||||
|
||||
private:
|
||||
implementation::LowPassIIR<Scalar, Scalar> impl_;
|
||||
detail::LowPassIIR<Scalar, Scalar> impl_;
|
||||
};
|
||||
|
||||
} // namespace xrt_fusion
|
||||
} // namespace xrt::auxiliary::tracking
|
||||
|
|
|
@ -18,7 +18,7 @@
|
|||
#include <Eigen/Core>
|
||||
|
||||
|
||||
namespace xrt_fusion {
|
||||
namespace xrt::auxiliary::tracking {
|
||||
|
||||
/*!
|
||||
* A very simple low-pass filter, using a "one-pole infinite impulse response"
|
||||
|
@ -95,7 +95,7 @@ public:
|
|||
}
|
||||
|
||||
private:
|
||||
implementation::LowPassIIR<Vector, Scalar> impl_;
|
||||
detail::LowPassIIR<Vector, Scalar> impl_;
|
||||
};
|
||||
|
||||
} // namespace xrt_fusion
|
||||
} // namespace xrt::auxiliary::tracking
|
||||
|
|
|
@ -19,11 +19,15 @@
|
|||
#include "t_tracking.h"
|
||||
#include "tracking/t_calibration_opencv.hpp"
|
||||
|
||||
using namespace xrt::auxiliary::tracking;
|
||||
|
||||
//! Namespace for hand tracker implementation
|
||||
namespace xrt::auxiliary::tracking::hand {
|
||||
|
||||
/*!
|
||||
* Single camera.
|
||||
*
|
||||
* @see TrackerPSMV
|
||||
* @see TrackerHand
|
||||
*/
|
||||
struct View
|
||||
{
|
||||
|
@ -236,6 +240,11 @@ run(TrackerHand &t)
|
|||
|
||||
os_thread_helper_unlock(&t.oth);
|
||||
}
|
||||
|
||||
} // namespace xrt::auxiliary::tracking::hand
|
||||
|
||||
using xrt::auxiliary::tracking::hand::TrackerHand;
|
||||
|
||||
extern "C" void *
|
||||
t_ht_run(void *ptr)
|
||||
{
|
||||
|
|
|
@ -30,6 +30,10 @@
|
|||
#include <assert.h>
|
||||
#include <pthread.h>
|
||||
|
||||
using namespace xrt::auxiliary::tracking;
|
||||
|
||||
//! Namespace for PS Move tracking implementation
|
||||
namespace xrt::auxiliary::tracking::psmv {
|
||||
|
||||
/*!
|
||||
* Single camera.
|
||||
|
@ -106,7 +110,7 @@ struct TrackerPSMV
|
|||
|
||||
cv::Ptr<cv::SimpleBlobDetector> sbd;
|
||||
|
||||
std::unique_ptr<xrt_fusion::PSMVFusionInterface> filter;
|
||||
std::unique_ptr<PSMVFusionInterface> filter;
|
||||
|
||||
xrt_vec3 tracked_object_position;
|
||||
};
|
||||
|
@ -431,6 +435,9 @@ break_apart(TrackerPSMV &t)
|
|||
os_thread_helper_stop(&t.oth);
|
||||
}
|
||||
|
||||
} // namespace xrt::auxiliary::tracking::psmv
|
||||
|
||||
using xrt::auxiliary::tracking::psmv::TrackerPSMV;
|
||||
|
||||
/*
|
||||
*
|
||||
|
@ -534,7 +541,7 @@ t_psmv_create(struct xrt_frame_context *xfctx,
|
|||
t.fusion.rot.y = 0.0f;
|
||||
t.fusion.rot.z = 0.0f;
|
||||
t.fusion.rot.w = 1.0f;
|
||||
t.filter = xrt_fusion::PSMVFusionInterface::create();
|
||||
t.filter = PSMVFusionInterface::create();
|
||||
|
||||
ret = os_thread_helper_init(&t.oth);
|
||||
if (ret != 0) {
|
||||
|
|
|
@ -28,17 +28,19 @@
|
|||
#include "flexkalman/PoseState.h"
|
||||
|
||||
|
||||
using State = flexkalman::pose_externalized_rotation::State;
|
||||
using ProcessModel = flexkalman::PoseSeparatelyDampedConstantVelocityProcessModel<State>;
|
||||
namespace xrt::auxiliary::tracking {
|
||||
|
||||
namespace xrt_fusion {
|
||||
|
||||
struct TrackingInfo
|
||||
{
|
||||
bool valid{false};
|
||||
bool tracked{false};
|
||||
};
|
||||
//! Anonymous namespace to hide implementation names
|
||||
namespace {
|
||||
|
||||
using State = flexkalman::pose_externalized_rotation::State;
|
||||
using ProcessModel = flexkalman::PoseSeparatelyDampedConstantVelocityProcessModel<State>;
|
||||
|
||||
struct TrackingInfo
|
||||
{
|
||||
bool valid{false};
|
||||
bool tracked{false};
|
||||
};
|
||||
class PSMVFusion : public PSMVFusionInterface
|
||||
{
|
||||
public:
|
||||
|
@ -70,7 +72,7 @@ namespace {
|
|||
State filter_state;
|
||||
ProcessModel process_model;
|
||||
|
||||
xrt_fusion::SimpleIMUFusion imu;
|
||||
xrt::auxiliary::tracking::SimpleIMUFusion imu;
|
||||
|
||||
timepoint_ns filter_time_ns{0};
|
||||
bool tracked{false};
|
||||
|
@ -162,8 +164,8 @@ namespace {
|
|||
if (lever_arm_optional) {
|
||||
lever_arm = map_vec3(*lever_arm_optional).cast<double>();
|
||||
}
|
||||
auto measurement =
|
||||
xrt_fusion::AbsolutePositionLeverArmMeasurement{pos.cast<double>(), lever_arm, variance};
|
||||
auto measurement = xrt::auxiliary::tracking::AbsolutePositionLeverArmMeasurement{pos.cast<double>(),
|
||||
lever_arm, variance};
|
||||
double resid = measurement.getResidual(filter_state).norm();
|
||||
|
||||
if (resid > residual_limit) {
|
||||
|
@ -233,4 +235,4 @@ PSMVFusionInterface::create()
|
|||
auto ret = std::make_unique<PSMVFusion>();
|
||||
return ret;
|
||||
}
|
||||
} // namespace xrt_fusion
|
||||
} // namespace xrt::auxiliary::tracking
|
||||
|
|
|
@ -23,7 +23,8 @@
|
|||
#include <memory>
|
||||
|
||||
|
||||
namespace xrt_fusion {
|
||||
namespace xrt::auxiliary::tracking {
|
||||
|
||||
class PSMVFusionInterface
|
||||
{
|
||||
public:
|
||||
|
@ -52,4 +53,4 @@ public:
|
|||
virtual void
|
||||
get_prediction(timepoint_ns when_ns, struct xrt_space_relation *out_relation) = 0;
|
||||
};
|
||||
} // namespace xrt_fusion
|
||||
} // namespace xrt::auxiliary::tracking
|
||||
|
|
|
@ -109,6 +109,11 @@ DEBUG_GET_ONCE_LOG_OPTION(psvr_log, "PSVR_TRACKING_LOG", U_LOGGING_WARN)
|
|||
//#define PSVR_DUMP_FOR_OFFLINE_ANALYSIS
|
||||
//#define PSVR_DUMP_IMU_FOR_OFFLINE_ANALYSIS
|
||||
|
||||
using namespace xrt::auxiliary::tracking;
|
||||
|
||||
//! Namespace for PSVR tracking implementation
|
||||
namespace xrt::auxiliary::tracking::psvr {
|
||||
|
||||
typedef enum blob_type
|
||||
{
|
||||
BLOB_TYPE_UNKNOWN,
|
||||
|
@ -1945,6 +1950,9 @@ break_apart(TrackerPSVR &t)
|
|||
os_thread_helper_stop(&t.oth);
|
||||
}
|
||||
|
||||
} // namespace xrt::auxiliary::tracking::psvr
|
||||
|
||||
using xrt::auxiliary::tracking::psvr::TrackerPSVR;
|
||||
|
||||
/*
|
||||
*
|
||||
|
@ -2042,6 +2050,8 @@ t_psvr_create(struct xrt_frame_context *xfctx,
|
|||
PSVR_INFO("%s", __func__);
|
||||
int ret;
|
||||
|
||||
using xrt::auxiliary::tracking::psvr::init_filter;
|
||||
|
||||
for (uint32_t i = 0; i < PSVR_NUM_LEDS; i++) {
|
||||
init_filter(t.track_filters[i], PSVR_BLOB_PROCESS_NOISE, PSVR_BLOB_MEASUREMENT_NOISE, 1.0f);
|
||||
}
|
||||
|
|
|
@ -548,6 +548,19 @@ t_debug_hsv_filter_create(struct xrt_frame_context *xfctx,
|
|||
struct xrt_frame_sink *passthrough,
|
||||
struct xrt_frame_sink **out_sink);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
|
||||
namespace xrt::auxiliary {
|
||||
/*!
|
||||
* @brief Namespace used by C++ interfaces in the auxiliary tracking library code.
|
||||
*/
|
||||
namespace tracking {
|
||||
|
||||
} // namespace tracking
|
||||
} // namespace xrt::auxiliary
|
||||
#endif
|
||||
|
||||
/*!
|
||||
* @}
|
||||
*/
|
||||
|
|
Loading…
Reference in a new issue