matrixfurry.com
The open source OpenXR runtime
1/*
2 * Copyright 2022 Jan Schmidt
3 * SPDX-License-Identifier: BSL-1.0
4 *
5 */
6/*!
7 * @file
8 * @brief Driver code for Oculus Rift S headsets
9 *
10 * Utility functions for generating a stereo camera calibration,
11 * and converting the Rift S Fisheye62 distortion parameters into
12 * OpenCV-compatible Kannala-Brandt parameters
13 *
14 * @author Jan Schmidt <jan@centricular.com>
15 * @ingroup drv_rift_s
16 */
17#include <stdbool.h>
18#include <math.h>
19
20#include "tinyceres/tiny_solver.hpp"
21#include "tinyceres/tiny_solver_autodiff_function.hpp"
22
23#include "rift_s_util.h"
24
25using ceres::TinySolver;
26using ceres::TinySolverAutoDiffFunction;
27
28const int N_KB4_DISTORT_PARAMS = 4;
29
30template <typename T>
31bool
32fisheye62_undistort_func(struct t_camera_calibration *calib,
33 const double *distortion_params,
34 const T point[2],
35 T *out_point)
36{
37 const T x = point[0];
38 const T y = point[1];
39
40 const T r2 = x * x + y * y;
41 const T r = sqrt(r2);
42
43 const double fx = calib->intrinsics[0][0];
44 const double fy = calib->intrinsics[1][1];
45
46 const double cx = calib->intrinsics[0][2];
47 const double cy = calib->intrinsics[1][2];
48
49 if (r < 1e-8) {
50 out_point[0] = fx * x + cx;
51 out_point[1] = fy * y + cy;
52 return true;
53 }
54
55 const T theta = atan(r);
56 const T theta2 = theta * theta;
57
58 const T xp = x * theta / r;
59 const T yp = y * theta / r;
60
61 const double k1 = distortion_params[0];
62 const double k2 = distortion_params[1];
63 const double k3 = distortion_params[2];
64 const double k4 = distortion_params[3];
65 const double k5 = distortion_params[4];
66 const double k6 = distortion_params[5];
67 const double p1 = distortion_params[6];
68 const double p2 = distortion_params[7];
69
70 /* 1 + k1 * theta^2 + k2 * theta^4 + k3 * theta^6 + k4 * theta^8 + k5 * theta^10 + k6 * theta^12 */
71 T r_theta = k6 * theta2;
72 r_theta += k5;
73 r_theta *= theta2;
74 r_theta += k4;
75 r_theta *= theta2;
76 r_theta += k3;
77 r_theta *= theta2;
78 r_theta += k2;
79 r_theta *= theta2;
80 r_theta += k1;
81 r_theta *= theta2;
82 r_theta += 1;
83
84 T delta_x = 2 * p1 * xp * yp + p2 * (theta2 + xp * xp * 2.0);
85 T delta_y = 2 * p2 * xp * yp + p1 * (theta2 + yp * yp * 2.0);
86
87 const T mx = xp * r_theta + delta_x;
88 const T my = yp * r_theta + delta_y;
89
90 out_point[0] = fx * mx + cx;
91 out_point[1] = fy * my + cy;
92
93 return true;
94}
95
96struct UndistortCostFunctor
97{
98 UndistortCostFunctor(struct t_camera_calibration *calib, double *distortion_params, double point[2])
99 : m_calib(calib), m_distortion_params(distortion_params)
100 {
101 m_point[0] = point[0];
102 m_point[1] = point[1];
103 }
104
105 struct t_camera_calibration *m_calib;
106 double *m_distortion_params;
107 double m_point[2];
108
109 template <typename T>
110 bool
111 operator()(const T *const x, T *residual) const
112 {
113 T out_point[2];
114
115 if (!fisheye62_undistort_func(m_calib, m_distortion_params, x, out_point))
116 return false;
117
118 residual[0] = out_point[0] - m_point[0];
119 residual[1] = out_point[1] - m_point[1];
120 return true;
121 }
122};
123
124template <typename T>
125bool
126kb4_distort_func(struct t_camera_calibration *calib, const T *distortion_params, const double point[2], T *out_point)
127{
128 const double x = point[0];
129 const double y = point[1];
130
131 const double r2 = x * x + y * y;
132 const double r = sqrt(r2);
133
134 const double fx = calib->intrinsics[0][0];
135 const double fy = calib->intrinsics[1][1];
136
137 const double cx = calib->intrinsics[0][2];
138 const double cy = calib->intrinsics[1][2];
139
140 if (r < 1e-8) {
141 out_point[0] = T(fx * x + cx);
142 out_point[1] = T(fy * y + cy);
143 return true;
144 }
145
146 const double theta = atan(r);
147 const double theta2 = theta * theta;
148
149 const T k1 = distortion_params[0];
150 const T k2 = distortion_params[1];
151 const T k3 = distortion_params[2];
152 const T k4 = distortion_params[3];
153
154 T r_theta = k4 * theta2;
155
156 r_theta += k3;
157 r_theta *= theta2;
158 r_theta += k2;
159 r_theta *= theta2;
160 r_theta += k1;
161 r_theta *= theta2;
162 r_theta += 1;
163 r_theta *= theta;
164
165 const T mx = x * r_theta / r;
166 const T my = y * r_theta / r;
167
168 out_point[0] = fx * mx + cx;
169 out_point[1] = fy * my + cy;
170
171 return true;
172}
173
174struct TargetPoint
175{
176 double point[2];
177 double distorted[2];
178};
179
180struct DistortParamKB4CostFunctor
181{
182 DistortParamKB4CostFunctor(struct t_camera_calibration *calib, int nSteps, TargetPoint *targetPointGrid)
183 : m_calib(calib), m_nSteps(nSteps), m_targetPointGrid(targetPointGrid)
184 {}
185
186 struct t_camera_calibration *m_calib;
187 int m_nSteps;
188 TargetPoint *m_targetPointGrid;
189
190 template <typename T>
191 bool
192 operator()(const T *const distort_params, T *residual) const
193 {
194 T out_point[2];
195
196 for (int y_index = 0; y_index < m_nSteps; y_index++) {
197 for (int x_index = 0; x_index < m_nSteps; x_index++) {
198 int residual_index = 2 * (y_index * m_nSteps + x_index);
199 TargetPoint *p = &m_targetPointGrid[(y_index * m_nSteps) + x_index];
200
201 if (!kb4_distort_func<T>(m_calib, distort_params, p->point, out_point))
202 return false;
203
204 residual[residual_index + 0] = out_point[0] - p->distorted[0];
205 residual[residual_index + 1] = out_point[1] - p->distorted[1];
206 }
207 }
208
209 return true;
210 }
211};
212
213#define STEPS 21
214struct t_camera_calibration
215rift_s_get_cam_calib(struct rift_s_camera_calibration_block *camera_calibration, enum rift_s_camera_id cam_id)
216{
217 struct t_camera_calibration tcc;
218
219 struct rift_s_camera_calibration *rift_s_cam = &camera_calibration->cameras[cam_id];
220 tcc.image_size_pixels.h = rift_s_cam->roi.extent.h;
221 tcc.image_size_pixels.w = rift_s_cam->roi.extent.w;
222 tcc.intrinsics[0][0] = rift_s_cam->projection.fx;
223 tcc.intrinsics[1][1] = rift_s_cam->projection.fy;
224 tcc.intrinsics[0][2] = rift_s_cam->projection.cx;
225 tcc.intrinsics[1][2] = rift_s_cam->projection.cy;
226 tcc.intrinsics[2][2] = 1.0;
227 tcc.distortion_model = T_DISTORTION_FISHEYE_KB4;
228
229 TargetPoint xy[STEPS * STEPS];
230
231 /* Convert fisheye62 params to KB4: */
232 double fisheye62_distort_params[8];
233 for (int i = 0; i < 6; i++) {
234 fisheye62_distort_params[i] = rift_s_cam->distortion.k[i];
235 }
236 fisheye62_distort_params[6] = rift_s_cam->distortion.p1;
237 fisheye62_distort_params[7] = rift_s_cam->distortion.p2;
238
239 /* Calculate Fisheye62 distortion grid by finding the viewplane coordinates that
240 * project onto the points of grid spaced across the pixel image plane */
241 for (int y_index = 0; y_index < STEPS; y_index++) {
242 for (int x_index = 0; x_index < STEPS; x_index++) {
243 int x = x_index * (tcc.image_size_pixels.w - 1) / (STEPS - 1);
244 int y = y_index * (tcc.image_size_pixels.h - 1) / (STEPS - 1);
245 TargetPoint *p = &xy[(y_index * STEPS) + x_index];
246
247 p->distorted[0] = x;
248 p->distorted[1] = y;
249
250 Eigen::Matrix<double, 2, 1> result(0, 0);
251
252 using AutoDiffUndistortFunction = TinySolverAutoDiffFunction<UndistortCostFunctor, 2, 2>;
253 UndistortCostFunctor undistort_functor(&tcc, fisheye62_distort_params, p->distorted);
254 AutoDiffUndistortFunction f(undistort_functor);
255
256 TinySolver<AutoDiffUndistortFunction> solver;
257 solver.Solve(f, &result);
258
259 p->point[0] = result[0];
260 p->point[1] = result[1];
261 }
262 }
263
264 /* Use the calculated distortion grid to solve for kb4 params */
265 {
266 Eigen::Matrix<double, N_KB4_DISTORT_PARAMS, 1> kb4_distort_params{0, 0, 0, 0};
267
268 using AutoDiffDistortParamKB4Function =
269 TinySolverAutoDiffFunction<DistortParamKB4CostFunctor, 2 * STEPS * STEPS, N_KB4_DISTORT_PARAMS>;
270 DistortParamKB4CostFunctor distort_param_kb4_functor(&tcc, STEPS, xy);
271 AutoDiffDistortParamKB4Function f(distort_param_kb4_functor);
272
273 TinySolver<AutoDiffDistortParamKB4Function> solver;
274 solver.Solve(f, &kb4_distort_params);
275
276 tcc.kb4.k1 = kb4_distort_params[0];
277 tcc.kb4.k2 = kb4_distort_params[1];
278 tcc.kb4.k3 = kb4_distort_params[2];
279 tcc.kb4.k4 = kb4_distort_params[3];
280
281 return tcc;
282 }
283}
284
285/*!
286 * Allocate and populate an OpenCV-compatible @ref t_stereo_camera_calibration pointer from
287 * the Rift S config.
288 *
289 * This requires fitting a KB4 fisheye polynomial to the 6 radial + 2 tangential 'Fisheye62'
290 * parameters provided by the Rift S.
291 *
292 */
293struct t_stereo_camera_calibration *
294rift_s_create_stereo_camera_calib_rotated(struct rift_s_camera_calibration_block *camera_calibration)
295{
296 struct t_stereo_camera_calibration *calib = NULL;
297 t_stereo_camera_calibration_alloc(&calib, T_DISTORTION_FISHEYE_KB4);
298
299 struct rift_s_camera_calibration *left = &camera_calibration->cameras[RIFT_S_CAMERA_FRONT_LEFT];
300 struct rift_s_camera_calibration *right = &camera_calibration->cameras[RIFT_S_CAMERA_FRONT_RIGHT];
301
302 // Intrinsics
303 for (int view = 0; view < 2; view++) {
304 enum rift_s_camera_id cam_id = view == 0 ? RIFT_S_CAMERA_FRONT_LEFT : RIFT_S_CAMERA_FRONT_RIGHT;
305 calib->view[view] = rift_s_get_cam_calib(camera_calibration, cam_id);
306 }
307
308 struct xrt_pose device_from_left, device_from_right;
309 struct xrt_pose right_from_device, right_from_left;
310
311 struct xrt_matrix_3x3 right_from_left_rot;
312
313 /* Compute the transform from the left eye to the right eye
314 * by using the config provided camera->device transform
315 */
316 math_pose_from_isometry(&left->device_from_camera, &device_from_left);
317 math_pose_from_isometry(&right->device_from_camera, &device_from_right);
318
319 math_pose_invert(&device_from_right, &right_from_device);
320 math_pose_transform(&device_from_left, &right_from_device, &right_from_left);
321
322 math_matrix_3x3_from_quat(&right_from_left.orientation, &right_from_left_rot);
323
324 /* Rotate the position in the camera extrinsics 90° to
325 * compensate for the front cams being rotated. That means hand poses
326 * are detected and come out rotated too, so need correcting
327 * in the tracking override offset */
328 calib->camera_translation[0] = -right_from_left.position.y;
329 calib->camera_translation[1] = right_from_left.position.x;
330 calib->camera_translation[2] = right_from_left.position.z;
331
332 calib->camera_rotation[0][0] = right_from_left_rot.v[0];
333 calib->camera_rotation[0][1] = right_from_left_rot.v[1];
334 calib->camera_rotation[0][2] = right_from_left_rot.v[2];
335 calib->camera_rotation[1][0] = right_from_left_rot.v[3];
336 calib->camera_rotation[1][1] = right_from_left_rot.v[4];
337 calib->camera_rotation[1][2] = right_from_left_rot.v[5];
338 calib->camera_rotation[2][0] = right_from_left_rot.v[6];
339 calib->camera_rotation[2][1] = right_from_left_rot.v[7];
340 calib->camera_rotation[2][2] = right_from_left_rot.v[8];
341
342 return calib;
343}