aux/tracking: Port to u_logging.

This commit is contained in:
Lubosz Sarnecki 2020-12-18 13:08:29 +01:00 committed by Jakob Bornecrantz
parent e350e42ca9
commit 58ecf67515
9 changed files with 62 additions and 84 deletions

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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