a/tracking: Migrate C++ APIs to the official namespaces now that we have some.

This commit is contained in:
Ryan Pavlik 2021-04-30 16:49:20 -05:00
parent e008e4f319
commit d88aefafbc
15 changed files with 95 additions and 33 deletions

View file

@ -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

View file

@ -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

View file

@ -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;

View file

@ -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

View file

@ -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

View file

@ -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:

View file

@ -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

View file

@ -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

View file

@ -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

View file

@ -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)
{

View file

@ -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) {

View file

@ -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

View file

@ -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

View file

@ -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);
}

View file

@ -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
/*!
* @}
*/