The open source OpenXR runtime
d/wmr: Properly compute hand tracking boundary circle
authored by
Mateo de Mayo
and committed by
Simon Zeni
a10644c8
a9b85c99
+56
-7
+56
-7
src/xrt/drivers/wmr/wmr_hmd.c
+56
-7
src/xrt/drivers/wmr/wmr_hmd.c
···
1415
1415
return res;
1416
1416
}
1417
1417
1418
+
XRT_MAYBE_UNUSED static struct xrt_vec2
1419
+
wmr_hmd_camera_project(struct wmr_hmd *wh, struct xrt_vec3 p3d)
1420
+
{
1421
+
float w = wh->config.cams[0].roi.extent.w;
1422
+
float h = wh->config.cams[0].roi.extent.h;
1423
+
float fx = wh->config.cams[0].distortion6KT.params.fx * w;
1424
+
float fy = wh->config.cams[0].distortion6KT.params.fy * h;
1425
+
float cx = wh->config.cams[0].distortion6KT.params.cx * w;
1426
+
float cy = wh->config.cams[0].distortion6KT.params.cy * h;
1427
+
float k1 = wh->config.cams[0].distortion6KT.params.k[0];
1428
+
float k2 = wh->config.cams[0].distortion6KT.params.k[1];
1429
+
float p1 = wh->config.cams[0].distortion6KT.params.p1;
1430
+
float p2 = wh->config.cams[0].distortion6KT.params.p2;
1431
+
float k3 = wh->config.cams[0].distortion6KT.params.k[2];
1432
+
float k4 = wh->config.cams[0].distortion6KT.params.k[3];
1433
+
float k5 = wh->config.cams[0].distortion6KT.params.k[4];
1434
+
float k6 = wh->config.cams[0].distortion6KT.params.k[5];
1435
+
1436
+
float x = p3d.x;
1437
+
float y = p3d.y;
1438
+
float z = p3d.z;
1439
+
1440
+
float xp = x / z;
1441
+
float yp = y / z;
1442
+
float rp2 = xp * xp + yp * yp;
1443
+
float cdist = (1 + rp2 * (k1 + rp2 * (k2 + rp2 * k3))) / (1 + rp2 * (k4 + rp2 * (k5 + rp2 * k6)));
1444
+
// If we were using OpenCV's camera model we would do
1445
+
// float deltaX = 2 * p1 * xp * yp + p2 * (rp2 + 2 * xp * xp);
1446
+
// float deltaY = 2 * p2 * xp * yp + p1 * (rp2 + 2 * yp * yp);
1447
+
// But instead we use Azure Kinect model (see comment in wmr_hmd_create_stereo_camera_calib)
1448
+
float deltaX = p1 * xp * yp + p2 * (rp2 + 2 * xp * xp);
1449
+
float deltaY = p2 * xp * yp + p1 * (rp2 + 2 * yp * yp);
1450
+
float xpp = xp * cdist + deltaX;
1451
+
float ypp = yp * cdist + deltaY;
1452
+
float u = fx * xpp + cx;
1453
+
float v = fy * ypp + cy;
1454
+
1455
+
struct xrt_vec2 p2d = {u, v};
1456
+
return p2d;
1457
+
}
1458
+
1459
+
1418
1460
/*!
1419
1461
* Creates an OpenCV-compatible @ref t_stereo_camera_calibration pointer from
1420
1462
* the WMR config.
···
1670
1712
wh->hmd_desc->hmd_type == WMR_HEADSET_REVERB_G2) {
1671
1713
ori_guess = wmr_hmd_guess_camera_orientation(wh);
1672
1714
}
1673
-
extra_camera_info.views[0].camera_orientation = ori_guess;
1674
-
extra_camera_info.views[1].camera_orientation = ori_guess;
1675
1715
1676
-
//!@todo Turning it off is okay for now, but we should plug metric_radius (or whatever it's called) in, at some
1677
-
//! point.
1678
-
// TODO@mateosss: do it now
1679
-
extra_camera_info.views[0].boundary_type = HT_IMAGE_BOUNDARY_NONE;
1680
-
extra_camera_info.views[1].boundary_type = HT_IMAGE_BOUNDARY_NONE;
1716
+
for (int i = 0; i < 2; i++) {
1717
+
extra_camera_info.views[i].camera_orientation = ori_guess;
1718
+
extra_camera_info.views[i].boundary_type = HT_IMAGE_BOUNDARY_CIRCLE;
1719
+
float w = wh->config.cams[i].roi.extent.w;
1720
+
float h = wh->config.cams[i].roi.extent.h;
1721
+
float cx = wh->config.cams[i].distortion6KT.params.cx * w;
1722
+
float cy = wh->config.cams[i].distortion6KT.params.cy * h;
1723
+
float rpmax = wh->config.cams[i].distortion6KT.params.metric_radius;
1724
+
struct xrt_vec3 p3d = {rpmax, 0, 1}; // Right-most border of the metric_radius circle in the Z=1 plane
1725
+
struct xrt_vec2 p2d = wmr_hmd_camera_project(wh, p3d);
1726
+
float radius = (p2d.x - cx) / w;
1727
+
extra_camera_info.views[i].boundary.circle.normalized_center = (struct xrt_vec2){cx / w, cy / h};
1728
+
extra_camera_info.views[i].boundary.circle.normalized_radius = radius;
1729
+
}
1681
1730
1682
1731
struct t_hand_tracking_create_info create_info = {.cams_info = extra_camera_info, .masks_sink = masks_sink};
1683
1732