The open source OpenXR runtime
0
fork

Configure Feed

Select the types of activity you want to include in your feed.

h/mercury: Dynamically allocate arrays for stereographic projection

Also factor out stereographic_unprojection into its own file because we're gonna need it elsewhere

+110 -73
+82 -73
src/xrt/tracking/hand/mercury/hg_image_distorter.cpp
··· 29 29 #include <iostream> 30 30 #include <opencv2/opencv.hpp> 31 31 32 - //!@todo This is the wrong way of doing it; we should probably be allocating some space up front for each 33 - //! keypoint estimator to scratch into. Patches welcome! 34 - #define EIGEN_STACK_ALLOCATION_LIMIT 128 * 128 * 4 * 3 35 - 36 32 #include "math/m_eigen_interop.hpp" 37 33 #include "hg_sync.hpp" 34 + #include "hg_stereographic_unprojection.hpp" 38 35 39 36 namespace xrt::tracking::hand::mercury { 40 37 ··· 43 40 template <typename T> using OutputSizedArray = Eigen::Array<T, wsize, wsize, Eigen::RowMajor>; 44 41 using OutputSizedFloatArray = OutputSizedArray<float>; 45 42 43 + #define ARRAY_STACK_SIZE 20 44 + 45 + struct ArrayStack 46 + { 47 + OutputSizedFloatArray arrays[ARRAY_STACK_SIZE]; 48 + size_t array_idx = 0; 49 + 50 + OutputSizedFloatArray & 51 + get() 52 + { 53 + if (array_idx == ARRAY_STACK_SIZE) { 54 + abort(); 55 + } 56 + return this->arrays[this->array_idx++]; 57 + }; 58 + 59 + void 60 + dropAll() 61 + { 62 + this->array_idx = 0; 63 + } 64 + }; 65 + 46 66 struct projection_state 47 67 { 48 68 cv::Mat &input; ··· 51 71 52 72 const projection_instructions &instructions; 53 73 74 + ArrayStack stack = {}; 75 + 76 + OutputSizedArray<int16_t> image_x = {}; 77 + OutputSizedArray<int16_t> image_y = {}; 78 + 54 79 projection_state(const projection_instructions &instructions, cv::Mat &input, cv::Mat &output) 55 80 : input(input), distorted_image_eigen(output.data, 128, 128), instructions(instructions){}; 56 81 }; ··· 58 83 59 84 // A private, purpose-optimized version of the Kannalla-Brandt projection function. 60 85 static void 61 - project_kb4(const t_camera_model_params &dist, // 62 - const OutputSizedFloatArray &x, // 63 - const OutputSizedFloatArray &y, // 64 - const OutputSizedFloatArray &z, // 65 - OutputSizedFloatArray &out_x, // 86 + project_kb4(projection_state &mi, 87 + const OutputSizedFloatArray &x, // 88 + const OutputSizedFloatArray &y, // 89 + const OutputSizedFloatArray &z, // 90 + OutputSizedFloatArray &out_x, // 66 91 OutputSizedFloatArray &out_y) 67 92 { 93 + const t_camera_model_params &dist = mi.dist; 94 + OutputSizedFloatArray r2 = mi.stack.get(); 95 + OutputSizedFloatArray r = mi.stack.get(); 68 96 69 - const OutputSizedFloatArray r2 = x * x + y * y; 70 - const OutputSizedFloatArray r = sqrt(r2); 97 + r2 = x * x + y * y; 98 + r = sqrt(r2); 71 99 72 100 #if 0 73 101 const I theta = atan2(r, z); ··· 81 109 // If neither of these were true we'd definitely need atan2. 82 110 // 83 111 // Grrr, we really need a good library for fast approximations of trigonometric functions. 84 - 85 - const OutputSizedFloatArray theta = atan(r / z); 112 + OutputSizedFloatArray theta = mi.stack.get(); 113 + theta = atan(r / z); 86 114 #endif 87 115 88 - const OutputSizedFloatArray theta2 = theta * theta; 116 + OutputSizedFloatArray theta2 = mi.stack.get(); 117 + theta2 = theta * theta; 89 118 90 119 91 120 #if 0 ··· 101 130 #else 102 131 // This version gives the compiler more options to do FMAs and avoid temporaries. Down to floating point 103 132 // precision this should give the same result as the above. 104 - OutputSizedFloatArray r_theta = 133 + OutputSizedFloatArray r_theta = mi.stack.get(); 134 + r_theta = 105 135 (((((dist.fisheye.k4 * theta2) + dist.fisheye.k3) * theta2 + dist.fisheye.k2) * theta2 + dist.fisheye.k1) * 106 136 theta2 + 107 137 1) * 108 138 theta; 109 139 #endif 110 140 111 - const OutputSizedFloatArray mx = x * r_theta / r; 112 - const OutputSizedFloatArray my = y * r_theta / r; 141 + OutputSizedFloatArray mx = mi.stack.get(); 142 + mx = x * r_theta / r; 143 + OutputSizedFloatArray my = mi.stack.get(); 144 + my = y * r_theta / r; 113 145 114 146 out_x = dist.fx * mx + dist.cx; 115 147 out_y = dist.fy * my + dist.cy; 116 148 } 117 149 118 - 119 - 120 - Eigen::Vector3f 121 - stereographic_to_direction(float sg_x, float sg_y) 122 - { 123 - float X = sg_x; 124 - float Y = sg_y; 125 - 126 - float denom = (1 + X * X + Y * Y); 127 - 128 - float x = (2 * X) / denom; 129 - float y = (2 * Y) / denom; 130 - float z = (-1 + X * X + Y * Y) / denom; 131 - 132 - // forward is -z 133 - return {x, y, z}; 134 - // return {x / -z, y / -z}; 135 - } 136 - 137 - 138 - 139 150 template <typename T> 140 151 T 141 152 map_ranges(T value, T from_low, T from_high, T to_low, T to_high) ··· 177 188 { 178 189 XRT_TRACE_MARKER(); 179 190 180 - OutputSizedFloatArray sg_x; 181 - OutputSizedFloatArray sg_y; 191 + OutputSizedFloatArray &sg_x = mi.stack.get(); 192 + OutputSizedFloatArray &sg_y = mi.stack.get(); 182 193 183 194 // Please vectorize me? 184 195 if (mi.instructions.flip) { ··· 205 216 // STEREOGRAPHIC DIRECTION TO 3D DIRECTION 206 217 // Note: we do not normalize the direction, because we don't need to. :) 207 218 208 - OutputSizedFloatArray dir_x; 209 - OutputSizedFloatArray dir_y; 210 - OutputSizedFloatArray dir_z; 219 + OutputSizedFloatArray &dir_x = mi.stack.get(); 220 + OutputSizedFloatArray &dir_y = mi.stack.get(); 221 + OutputSizedFloatArray &dir_z = mi.stack.get(); 211 222 212 223 213 224 #if 0 ··· 224 235 // END STEREOGRAPHIC DIRECTION TO 3D DIRECTION 225 236 226 237 // QUATERNION ROTATING VECTOR 227 - Eigen::Matrix<OutputSizedFloatArray, 3, 1> rot_dir; 238 + OutputSizedFloatArray &rot_dir_x = mi.stack.get(); 239 + OutputSizedFloatArray &rot_dir_y = mi.stack.get(); 240 + OutputSizedFloatArray &rot_dir_z = mi.stack.get(); 228 241 229 - OutputSizedFloatArray uv0; 230 - OutputSizedFloatArray uv1; 231 - OutputSizedFloatArray uv2; 242 + OutputSizedFloatArray &uv0 = mi.stack.get(); 243 + OutputSizedFloatArray &uv1 = mi.stack.get(); 244 + OutputSizedFloatArray &uv2 = mi.stack.get(); 232 245 233 246 const Eigen::Quaternionf &q = mi.instructions.rot_quat; 234 247 ··· 240 253 uv1 += uv1; 241 254 uv2 += uv2; 242 255 243 - rot_dir.x() = dir_x + q.w() * uv0; 244 - rot_dir.y() = dir_y + q.w() * uv1; 245 - rot_dir.z() = dir_z + q.w() * uv2; 256 + rot_dir_x = dir_x + q.w() * uv0; 257 + rot_dir_y = dir_y + q.w() * uv1; 258 + rot_dir_z = dir_z + q.w() * uv2; 246 259 247 - rot_dir.x() += q.y() * uv2 - q.z() * uv1; 248 - rot_dir.y() += q.z() * uv0 - q.x() * uv2; 249 - rot_dir.z() += q.x() * uv1 - q.y() * uv0; 260 + rot_dir_x += q.y() * uv2 - q.z() * uv1; 261 + rot_dir_y += q.z() * uv0 - q.x() * uv2; 262 + rot_dir_z += q.x() * uv1 - q.y() * uv0; 250 263 // END QUATERNION ROTATING VECTOR 251 264 252 265 253 266 254 - OutputSizedArray<int16_t> image_x; 255 - OutputSizedArray<int16_t> image_y; 256 - 257 - OutputSizedFloatArray image_x_f; 258 - OutputSizedFloatArray image_y_f; 267 + OutputSizedFloatArray &image_x_f = mi.stack.get(); 268 + OutputSizedFloatArray &image_y_f = mi.stack.get(); 259 269 260 270 261 271 //!@todo optimize 262 - rot_dir.y() *= -1; 263 - rot_dir.z() *= -1; 272 + rot_dir_y *= -1; 273 + rot_dir_z *= -1; 264 274 265 275 { 266 276 XRT_TRACE_IDENT(camera_projection); ··· 268 278 switch (mi.dist.model) { 269 279 case T_DISTORTION_FISHEYE_KB4: 270 280 // This takes 250us vs 500 because of the removed atan2. 271 - project_kb4(mi.dist, rot_dir.x(), rot_dir.y(), rot_dir.z(), image_x_f, image_y_f); 281 + project_kb4(mi, rot_dir_x, rot_dir_y, rot_dir_z, image_x_f, image_y_f); 272 282 break; 273 283 case T_DISTORTION_OPENCV_RADTAN_8: 274 284 // Regular C is plenty fast for radtan :) 275 285 for (int i = 0; i < image_x_f.rows(); i++) { 276 286 for (int j = 0; j < image_x_f.cols(); j++) { 277 - t_camera_models_project(&mi.dist, rot_dir.x()(i, j), rot_dir.y()(i, j), 278 - rot_dir.z()(i, j), &image_x_f(i, j), &image_y_f(i, j)); 287 + t_camera_models_project(&mi.dist, rot_dir_x(i, j), rot_dir_y(i, j), 288 + rot_dir_z(i, j), &image_x_f(i, j), &image_y_f(i, j)); 279 289 } 280 290 } 281 291 break; ··· 283 293 } 284 294 } 285 295 286 - image_x = image_x_f.cast<int16_t>(); 287 - image_y = image_y_f.cast<int16_t>(); 296 + mi.image_x = image_x_f.cast<int16_t>(); 297 + mi.image_y = image_y_f.cast<int16_t>(); 288 298 289 - naive_remap(image_x, image_y, mi.input, mi.distorted_image_eigen); 299 + naive_remap(mi.image_x, mi.image_y, mi.input, mi.distorted_image_eigen); 290 300 } 291 301 292 302 ··· 300 310 float sg_y = 301 311 map_ranges<float>(y, 0, wsize, mi.instructions.stereographic_radius, -mi.instructions.stereographic_radius); 302 312 303 - Eigen::Vector3f dir = stereographic_to_direction(sg_x, sg_y); 313 + Eigen::Vector3f dir = stereographic_unprojection(sg_x, sg_y); 304 314 305 315 dir = mi.instructions.rot_quat * dir; 306 316 ··· 519 529 float r = fmax(center.x - min.x, center.y - min.y); 520 530 out_instructions.stereographic_radius = r; 521 531 522 - Eigen::Vector3f new_direction = stereographic_to_direction(center.x, center.y); 532 + Eigen::Vector3f new_direction = stereographic_unprojection(center.x, center.y); 523 533 524 534 new_direction = out_instructions.rot_quat * new_direction; 525 535 ··· 598 608 599 609 { 600 610 out = cv::Mat(cv::Size(wsize, wsize), CV_8U); 601 - projection_state mi(instructions, input_image, out); 611 + projection_state *mi_ptr = new projection_state(instructions, input_image, out); 612 + projection_state &mi = *mi_ptr; 613 + 602 614 mi.dist = dist; 603 - // Why am I doing it like this???? 604 - // mi.stereographic_radius = instructions.stereographic_radius; 605 - // mi.rot_quat = instructions.rot_quat; 606 - // mi.flip = instructions.flip; 607 615 608 616 StereographicDistort(mi); 609 617 610 618 if (debug_image) { 611 619 draw_boundary(mi, boundary_color, *debug_image); 612 620 } 621 + delete mi_ptr; 613 622 } 614 623 } // namespace xrt::tracking::hand::mercury
+28
src/xrt/tracking/hand/mercury/hg_stereographic_unprojection.hpp
··· 1 + // Copyright 2023, Collabora, Ltd. 2 + // SPDX-License-Identifier: BSL-1.0 3 + /*! 4 + * @file 5 + * @brief Stereographic unprojection 6 + * @author Moshi Turner <moses@collabora.com> 7 + * @ingroup tracking 8 + */ 9 + 10 + #pragma once 11 + #include "math/m_eigen_interop.hpp" 12 + 13 + static inline Eigen::Vector3f 14 + stereographic_unprojection(float sg_x, float sg_y) 15 + { 16 + float X = sg_x; 17 + float Y = sg_y; 18 + 19 + float denom = (1 + X * X + Y * Y); 20 + 21 + float x = (2 * X) / denom; 22 + float y = (2 * Y) / denom; 23 + float z = (-1 + X * X + Y * Y) / denom; 24 + 25 + // forward is -z 26 + return {x, y, z}; 27 + // return {x / -z, y / -z}; 28 + }