mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-02-09 15:28:03 +00:00
aux/tracking: Port to u_logging.
This commit is contained in:
parent
e350e42ca9
commit
58ecf67515
|
@ -654,15 +654,15 @@ process_view_samples(class Calibration &c,
|
|||
cv::Mat distortion_fisheye_mat = {};
|
||||
|
||||
if (c.dump_measurements) {
|
||||
printf("...measured = (ArrayOfMeasurements){\n");
|
||||
U_LOG_RAW("...measured = (ArrayOfMeasurements){");
|
||||
for (MeasurementF32 &m : view.measured_f32) {
|
||||
printf(" {\n");
|
||||
U_LOG_RAW(" {");
|
||||
for (cv::Point2f &p : m) {
|
||||
printf(" {%+ff, %+ff},\n", p.x, p.y);
|
||||
U_LOG_RAW(" {%+ff, %+ff},", p.x, p.y);
|
||||
}
|
||||
printf(" },\n");
|
||||
U_LOG_RAW(" },");
|
||||
}
|
||||
printf("};\n");
|
||||
U_LOG_RAW("};");
|
||||
}
|
||||
|
||||
if (c.use_fisheye) {
|
||||
|
@ -1149,17 +1149,16 @@ process_load_image(class Calibration &c, struct xrt_frame *xf)
|
|||
c.gray = cv::imread(buf, cv::IMREAD_GRAYSCALE);
|
||||
|
||||
if (c.gray.rows == 0 || c.gray.cols == 0) {
|
||||
fprintf(stderr, "Could not find image '%s'!\n", buf);
|
||||
U_LOG_E("Could not find image '%s'!", buf);
|
||||
continue;
|
||||
}
|
||||
|
||||
if (c.gray.rows != (int)xf->height ||
|
||||
c.gray.cols != (int)xf->width) {
|
||||
fprintf(stderr,
|
||||
"Image size does not match frame size! Image: "
|
||||
"(%ix%i) Frame: (%ux%u)\n",
|
||||
c.gray.cols, c.gray.rows, xf->width,
|
||||
xf->height);
|
||||
U_LOG_E(
|
||||
"Image size does not match frame size! Image: "
|
||||
"(%ix%i) Frame: (%ux%u)",
|
||||
c.gray.cols, c.gray.rows, xf->width, xf->height);
|
||||
continue;
|
||||
}
|
||||
|
||||
|
|
|
@ -141,10 +141,7 @@ process_frame(class DebugHSVPicker &d, struct xrt_frame *xf)
|
|||
switch (xf->format) {
|
||||
case XRT_FORMAT_YUV888: process_frame_yuv(d, xf); break;
|
||||
case XRT_FORMAT_YUYV422: process_frame_yuyv(d, xf); break;
|
||||
default:
|
||||
fprintf(stderr, "ERROR: Bad format '%s'",
|
||||
u_format_str(xf->format));
|
||||
break;
|
||||
default: U_LOG_E("Bad format '%s'", u_format_str(xf->format)); break;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -11,7 +11,7 @@
|
|||
|
||||
#include "tracking/t_calibration_opencv.hpp"
|
||||
#include "util/u_misc.h"
|
||||
|
||||
#include "util/u_logging.h"
|
||||
|
||||
/*
|
||||
*
|
||||
|
@ -213,7 +213,7 @@ t_stereo_camera_calibration_load_v1(
|
|||
result = result && read_cv_mat(calib_file, &mat_image_size, "mat_image_size");
|
||||
|
||||
if (!result) {
|
||||
fprintf(stderr, "\tRe-run calibration!\n");
|
||||
U_LOG_W("Re-run calibration!");
|
||||
return false;
|
||||
}
|
||||
wrapped.view[0].image_size_pixels.w = uint32_t(mat_image_size(0, 0));
|
||||
|
@ -226,22 +226,22 @@ t_stereo_camera_calibration_load_v1(
|
|||
}
|
||||
|
||||
if (!read_cv_mat(calib_file, &wrapped.camera_translation_mat, "translation")) {
|
||||
fprintf(stderr, "\tRe-run calibration!\n");
|
||||
U_LOG_W("Re-run calibration!");
|
||||
}
|
||||
if (!read_cv_mat(calib_file, &wrapped.camera_rotation_mat, "rotation")) {
|
||||
fprintf(stderr, "\tRe-run calibration!\n");
|
||||
U_LOG_W("Re-run calibration!");
|
||||
}
|
||||
if (!read_cv_mat(calib_file, &wrapped.camera_essential_mat, "essential")) {
|
||||
fprintf(stderr, "\tRe-run calibration!\n");
|
||||
U_LOG_W("Re-run calibration!");
|
||||
}
|
||||
if (!read_cv_mat(calib_file, &wrapped.camera_fundamental_mat, "fundamental")) {
|
||||
fprintf(stderr, "\tRe-run calibration!\n");
|
||||
U_LOG_W("Re-run calibration!");
|
||||
}
|
||||
|
||||
cv::Mat_<float> mat_use_fisheye(1, 1);
|
||||
if (!read_cv_mat(calib_file, &mat_use_fisheye, "use_fisheye")) {
|
||||
wrapped.view[0].use_fisheye = false;
|
||||
fprintf(stderr, "\tRe-run calibration! (Assuming not fisheye)\n");
|
||||
U_LOG_W("Re-run calibration! (Assuming not fisheye)");
|
||||
} else {
|
||||
wrapped.view[0].use_fisheye = mat_use_fisheye(0, 0) != 0.0f;
|
||||
}
|
||||
|
@ -338,8 +338,8 @@ read_cv_mat(FILE *f, cv::Mat *m, const char *name)
|
|||
cv::Mat temp;
|
||||
read = fread(static_cast<void *>(header), sizeof(uint32_t), 3, f);
|
||||
if (read != 3) {
|
||||
printf("Failed to read mat header: '%i' '%s'\n", (int)read,
|
||||
name);
|
||||
U_LOG_E("Failed to read mat header: '%i' '%s'", (int)read,
|
||||
name);
|
||||
return false;
|
||||
}
|
||||
|
||||
|
@ -358,20 +358,20 @@ read_cv_mat(FILE *f, cv::Mat *m, const char *name)
|
|||
read = fread(static_cast<void *>(temp.data), header[0],
|
||||
header[1] * header[2], f);
|
||||
if (read != (header[1] * header[2])) {
|
||||
printf("Failed to read mat body: '%i' '%s'\n", (int)read, name);
|
||||
U_LOG_E("Failed to read mat body: '%i' '%s'", (int)read, name);
|
||||
return false;
|
||||
}
|
||||
if (m->empty()) {
|
||||
m->create(header[1], header[2], temp.type());
|
||||
}
|
||||
if (temp.type() != m->type()) {
|
||||
printf("Mat body type does not match: %i vs %i for '%s'\n",
|
||||
(int)temp.type(), (int)m->type(), name);
|
||||
U_LOG_E("Mat body type does not match: %i vs %i for '%s'",
|
||||
(int)temp.type(), (int)m->type(), name);
|
||||
return false;
|
||||
}
|
||||
if (temp.total() != m->total()) {
|
||||
printf("Mat total size does not match: %i vs %i for '%s'\n",
|
||||
(int)temp.total(), (int)m->total(), name);
|
||||
U_LOG_E("Mat total size does not match: %i vs %i for '%s'",
|
||||
(int)temp.total(), (int)m->total(), name);
|
||||
return false;
|
||||
}
|
||||
if (temp.size() == m->size()) {
|
||||
|
@ -381,12 +381,12 @@ read_cv_mat(FILE *f, cv::Mat *m, const char *name)
|
|||
}
|
||||
if (temp.size().width == m->size().height &&
|
||||
temp.size().height == m->size().width) {
|
||||
printf("Mat transposing on load: '%s'\n", name);
|
||||
U_LOG_W("Mat transposing on load: '%s'", name);
|
||||
// needs transpose
|
||||
cv::transpose(temp, *m);
|
||||
return true;
|
||||
}
|
||||
// highly unlikely so minimally-helpful error message.
|
||||
printf("Mat dimension unknown mismatch: '%s'\n", name);
|
||||
U_LOG_E("Mat dimension unknown mismatch: '%s'", name);
|
||||
return false;
|
||||
}
|
||||
|
|
|
@ -306,10 +306,7 @@ push_frame(struct xrt_frame_sink *xsink, struct xrt_frame *xf)
|
|||
ensure_buf_allocated(f, xf);
|
||||
process_frame_yuyv(f, xf);
|
||||
break;
|
||||
default:
|
||||
fprintf(stderr, "ERROR: Bad format '%s'",
|
||||
u_format_str(xf->format));
|
||||
return;
|
||||
default: U_LOG_E("Bad format '%s'", u_format_str(xf->format)); return;
|
||||
}
|
||||
|
||||
push_buf(f, xf, f->sinks[0], &f->frame0);
|
||||
|
|
|
@ -18,30 +18,20 @@
|
|||
#include "math/m_api.h"
|
||||
#include "util/u_time.h"
|
||||
#include "util/u_debug.h"
|
||||
#include "util/u_logging.h"
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
#include "flexkalman/EigenQuatExponentialMap.h"
|
||||
|
||||
DEBUG_GET_ONCE_BOOL_OPTION(simple_imu_debug, "SIMPLE_IMU_DEBUG", false)
|
||||
DEBUG_GET_ONCE_BOOL_OPTION(simple_imu_spew, "SIMPLE_IMU_SPEW", false)
|
||||
DEBUG_GET_ONCE_LOG_OPTION(simple_imu_log, "SIMPLE_IMU_LOG", U_LOGGING_WARN)
|
||||
|
||||
#define SIMPLE_IMU_DEBUG(MSG) \
|
||||
do { \
|
||||
if (debug_) { \
|
||||
printf("SimpleIMU(%p): " MSG "\n", \
|
||||
(const void *)this); \
|
||||
} \
|
||||
} while (0)
|
||||
|
||||
#define SIMPLE_IMU_SPEW(MSG) \
|
||||
do { \
|
||||
if (spew_) { \
|
||||
printf("SimpleIMU(%p): " MSG "\n", \
|
||||
(const void *)this); \
|
||||
} \
|
||||
} while (0)
|
||||
#define SIMPLE_IMU_TRACE(...) U_LOG_IFL_T(ll, __VA_ARGS__)
|
||||
#define SIMPLE_IMU_DEBUG(...) U_LOG_IFL_D(ll, __VA_ARGS__)
|
||||
#define SIMPLE_IMU_INFO(...) U_LOG_IFL_I(ll, __VA_ARGS__)
|
||||
#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 {
|
||||
class SimpleIMUFusion
|
||||
|
@ -54,8 +44,7 @@ public:
|
|||
*/
|
||||
explicit SimpleIMUFusion(double gravity_rate = 0.9)
|
||||
: gravity_scale_(gravity_rate),
|
||||
debug_(debug_get_bool_option_simple_imu_debug()),
|
||||
spew_(debug_get_bool_option_simple_imu_spew())
|
||||
ll(debug_get_log_option_simple_imu_log())
|
||||
{
|
||||
SIMPLE_IMU_DEBUG("Creating instance");
|
||||
}
|
||||
|
@ -212,8 +201,7 @@ private:
|
|||
uint64_t last_gyro_timestamp_{0};
|
||||
double gyro_min_squared_length_{1.e-8};
|
||||
bool started_{false};
|
||||
bool debug_{false};
|
||||
bool spew_{false};
|
||||
enum u_logging_level ll;
|
||||
};
|
||||
|
||||
inline Eigen::Quaterniond
|
||||
|
@ -254,7 +242,7 @@ SimpleIMUFusion::handleGyro(Eigen::Vector3d const &gyro, timepoint_ns timestamp)
|
|||
Eigen::Vector3d incRot = gyro * dt;
|
||||
// Crude handling of "approximately zero"
|
||||
if (incRot.squaredNorm() < gyro_min_squared_length_) {
|
||||
SIMPLE_IMU_SPEW(
|
||||
SIMPLE_IMU_TRACE(
|
||||
"Discarding gyro data that is approximately zero");
|
||||
return false;
|
||||
}
|
||||
|
@ -310,7 +298,7 @@ SimpleIMUFusion::handleAccel(Eigen::Vector3d const &accel,
|
|||
if (scale <= 0) {
|
||||
// Too far from gravity to be useful/trusted for orientation
|
||||
// purposes.
|
||||
SIMPLE_IMU_SPEW("Too far from gravity to be useful/trusted.");
|
||||
SIMPLE_IMU_TRACE("Too far from gravity to be useful/trusted.");
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
|
@ -163,9 +163,7 @@ process(TrackerHand &t, struct xrt_frame *xf)
|
|||
int rect_rows = t.view[0].undistort_rectify_map_x.rows;
|
||||
|
||||
if (cols != rect_cols || rows != rect_rows) {
|
||||
fprintf(stderr,
|
||||
"Error: %dx%d rectification matrix does not fit %dx%d "
|
||||
"Image\n",
|
||||
U_LOG_E("%dx%d rectification matrix does not fit %dx%d Image",
|
||||
rect_cols, rect_rows, cols, rows);
|
||||
return;
|
||||
}
|
||||
|
@ -347,7 +345,7 @@ t_hand_create(struct xrt_frame_context *xfctx,
|
|||
struct xrt_tracked_hand **out_xth,
|
||||
struct xrt_frame_sink **out_sink)
|
||||
{
|
||||
fprintf(stderr, "%s\n", __func__);
|
||||
U_LOG_D("Creating hand tracker.");
|
||||
|
||||
auto &t = *(new TrackerHand());
|
||||
int ret;
|
||||
|
|
|
@ -540,7 +540,7 @@ t_psmv_create(struct xrt_frame_context *xfctx,
|
|||
struct xrt_tracked_psmv **out_xtmv,
|
||||
struct xrt_frame_sink **out_sink)
|
||||
{
|
||||
fprintf(stderr, "%s\n", __func__);
|
||||
U_LOG_D("Creating PSMV tracker.");
|
||||
|
||||
auto &t = *(new TrackerPSMV());
|
||||
int ret;
|
||||
|
|
|
@ -142,18 +142,18 @@ namespace {
|
|||
orientation_state.tracked = true;
|
||||
orientation_state.valid = true;
|
||||
} else {
|
||||
fprintf(stderr,
|
||||
"Got non-finite something when filtering IMU - "
|
||||
"resetting filter and IMU fusion!\n");
|
||||
U_LOG_E(
|
||||
"Got non-finite something when filtering IMU - "
|
||||
"resetting filter and IMU fusion!");
|
||||
reset_filter_and_imu();
|
||||
}
|
||||
// 7200 deg/sec
|
||||
constexpr double max_rad_per_sec = 20 * double(EIGEN_PI) * 2;
|
||||
if (filter_state.angularVelocity().squaredNorm() >
|
||||
max_rad_per_sec * max_rad_per_sec) {
|
||||
fprintf(stderr,
|
||||
"Got excessive angular velocity when filtering "
|
||||
"IMU - resetting filter and IMU fusion!\n");
|
||||
U_LOG_E(
|
||||
"Got excessive angular velocity when filtering "
|
||||
"IMU - resetting filter and IMU fusion!");
|
||||
reset_filter_and_imu();
|
||||
}
|
||||
}
|
||||
|
@ -183,10 +183,9 @@ namespace {
|
|||
|
||||
if (resid > residual_limit) {
|
||||
// Residual arbitrarily "too large"
|
||||
fprintf(
|
||||
stderr,
|
||||
"Warning - measurement residual is %f, resetting "
|
||||
"filter state\n",
|
||||
U_LOG_W(
|
||||
"measurement residual is %f, resetting "
|
||||
"filter state",
|
||||
resid);
|
||||
reset_filter();
|
||||
return;
|
||||
|
@ -196,9 +195,9 @@ namespace {
|
|||
position_state.valid = true;
|
||||
position_state.tracked = true;
|
||||
} else {
|
||||
fprintf(stderr,
|
||||
"Got non-finite something when filtering "
|
||||
"tracker - resetting filter!\n");
|
||||
U_LOG_W(
|
||||
"Got non-finite something when filtering "
|
||||
"tracker - resetting filter!");
|
||||
reset_filter();
|
||||
}
|
||||
}
|
||||
|
|
|
@ -585,7 +585,7 @@ remove_outliers(std::vector<blob_point_t> *orig_points,
|
|||
sqrt((error_x * error_x) + (error_y * error_y) +
|
||||
(error_z * error_z));
|
||||
|
||||
// printf("ERROR: %f %f %f %f %f %f\n", temp_points[i].p.x,
|
||||
// U_LOG_D("%f %f %f %f %f %f", temp_points[i].p.x,
|
||||
// temp_points[i].p.y, temp_points[i].p.z, error_x,
|
||||
// error_y, error_z);
|
||||
if (rms_error < outlier_thresh) {
|
||||
|
@ -993,7 +993,7 @@ disambiguate(TrackerPSVR &t,
|
|||
solve_for_measurement(&t, measured_points, solved);
|
||||
float diff = last_diff(t, solved, &t.last_vertices);
|
||||
if (diff < PSVR_HOLD_THRESH) {
|
||||
// printf("diff from last: %f\n", diff);
|
||||
// U_LOG_D("diff from last: %f", diff);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
@ -1105,13 +1105,13 @@ disambiguate(TrackerPSVR &t,
|
|||
// 'bottom
|
||||
|
||||
if (has_bl && has_tl && bl_pos.y() > tl_pos.y()) {
|
||||
// printf("IGNORING BL > TL %f %f\n",
|
||||
// U_LOG_D("IGNORING BL > TL %f %f",
|
||||
// bl_pos.y(),
|
||||
// br_pos.y());
|
||||
// ignore = true;
|
||||
}
|
||||
if (has_br && has_tr && br_pos.y() > tr_pos.y()) {
|
||||
// printf("IGNORING TL > TR %f %f\n",
|
||||
// U_LOG_D("IGNORING TL > TR %f %f",
|
||||
// tl_pos.y(),
|
||||
// tr_pos.y());
|
||||
// ignore = true;
|
||||
|
@ -1127,9 +1127,9 @@ disambiguate(TrackerPSVR &t,
|
|||
}
|
||||
|
||||
// useful for debugging
|
||||
// printf(
|
||||
// U_LOG_D(
|
||||
// "match %d dist to last: %f dist to imu: %f "
|
||||
// "rmsError: %f squaredSum:%f %d\n",
|
||||
// "rmsError: %f squaredSum:%f %d",
|
||||
// i, prev_diff, imu_diff, avg_error, error_sum,
|
||||
// ignore);
|
||||
}
|
||||
|
@ -1143,7 +1143,7 @@ disambiguate(TrackerPSVR &t,
|
|||
}
|
||||
}
|
||||
|
||||
// printf("lowest_error %f\n", lowest_error);
|
||||
// U_LOG_D("lowest_error %f", lowest_error);
|
||||
if (best_model == -1) {
|
||||
PSVR_INFO("COULD NOT MATCH MODEL!");
|
||||
return Eigen::Matrix4f().Identity();
|
||||
|
@ -1698,7 +1698,7 @@ process(TrackerPSVR &t, struct xrt_frame *xf)
|
|||
cv::KeyPoint rkp = t.view[1].keypoints.at(r_index);
|
||||
t.l_blobs.push_back(lkp);
|
||||
t.r_blobs.push_back(rkp);
|
||||
// printf("2D coords: LX %f LY %f RX %f RY %f\n",
|
||||
// U_LOG_D("2D coords: LX %f LY %f RX %f RY %f",
|
||||
// lkp.pt.x,
|
||||
// lkp.pt.y, rkp.pt.x, rkp.pt.y);
|
||||
}
|
||||
|
|
Loading…
Reference in a new issue