4 #ifndef LIBREALSENSE_RSUTIL2_H 5 #define LIBREALSENSE_RSUTIL2_H 21 float x = point[0] / point[2],
y = point[1] / point[2];
31 float dx = x + 2*intrin->
coeffs[2]*x*y + intrin->
coeffs[3]*(r2 + 2*x*
x);
32 float dy = y + 2*intrin->
coeffs[3]*x*y + intrin->
coeffs[2]*(r2 + 2*y*
y);
39 float r2 = x * x + y *
y;
40 float f = 1 + intrin->
coeffs[0] * r2 + intrin->
coeffs[1] * r2*r2 + intrin->
coeffs[4] * r2*r2*r2;
45 float dx = xf + 2 * intrin->
coeffs[2] * x*y + intrin->
coeffs[3] * (r2 + 2 * x*
x);
46 float dy = yf + 2 * intrin->
coeffs[3] * x*y + intrin->
coeffs[2] * (r2 + 2 * y*
y);
54 float r = sqrtf(x*x + y*y);
59 float rd = (float)(1.0
f / intrin->
coeffs[0] * atan(2 * r* tan(intrin->
coeffs[0] / 2.0f)));
65 float r = sqrtf(x*x + y*y);
70 float theta = atan(r);
71 float theta2 = theta*theta;
72 float series = 1 + theta2*(intrin->
coeffs[0] + theta2*(intrin->
coeffs[1] + theta2*(intrin->
coeffs[2] + theta2*intrin->
coeffs[3])));
73 float rd = theta*series;
78 pixel[0] = x * intrin->
fx + intrin->
ppx;
79 pixel[1] = y * intrin->
fy + intrin->
ppy;
88 float x = (pixel[0] - intrin->
ppx) / intrin->
fx;
89 float y = (pixel[1] - intrin->
ppy) / intrin->
fy;
98 for (
int i = 0;
i < 10;
i++)
100 float r2 = x * x +
y *
y;
101 float icdist = (float)1 / (
float)(1 + ((intrin->
coeffs[4] * r2 + intrin->
coeffs[1])*r2 + intrin->
coeffs[0])*r2);
102 float xq = x / icdist;
103 float yq = y / icdist;
104 float delta_x = 2 * intrin->
coeffs[2] * xq*yq + intrin->
coeffs[3] * (r2 + 2 * xq*xq);
105 float delta_y = 2 * intrin->
coeffs[3] * xq*yq + intrin->
coeffs[2] * (r2 + 2 * yq*yq);
106 x = (xo - delta_x)*icdist;
107 y = (yo - delta_y)*icdist;
114 for (
int i = 0;
i < 10;
i++)
116 float r2 = x * x +
y *
y;
117 float icdist = (float)1 / (
float)(1 + ((intrin->
coeffs[4] * r2 + intrin->
coeffs[1])*r2 + intrin->
coeffs[0])*r2);
118 float delta_x = 2 * intrin->
coeffs[2] * x*y + intrin->
coeffs[3] * (r2 + 2 * x*
x);
119 float delta_y = 2 * intrin->
coeffs[3] * x*y + intrin->
coeffs[2] * (r2 + 2 * y*
y);
120 x = (xo - delta_x)*icdist;
121 y = (yo - delta_y)*icdist;
127 float rd = sqrtf(x*x +
y*
y);
128 if (rd < FLT_EPSILON)
134 float theta2 = rd*rd;
135 for (
int i = 0;
i < 4;
i++)
137 float f = theta*(1 + theta2*(intrin->
coeffs[0] + theta2*(intrin->
coeffs[1] + theta2*(intrin->
coeffs[2] + theta2*intrin->
coeffs[3])))) - rd;
138 if (fabs(f) < FLT_EPSILON)
142 float df = 1 + theta2*(3 * intrin->
coeffs[0] + theta2*(5 * intrin->
coeffs[1] + theta2*(7 * intrin->
coeffs[2] + 9 * theta2*intrin->
coeffs[3])));
144 theta2 = theta*theta;
146 float r = tan(theta);
152 float rd = sqrtf(x*x +
y*
y);
153 if (rd < FLT_EPSILON)
157 float r = (float)(tan(intrin->
coeffs[0] * rd) / atan(2 * tan(intrin->
coeffs[0] / 2.0f)));
162 point[0] = depth *
x;
163 point[1] = depth *
y;
178 to_fov[0] = (atan2f(intrin->
ppx + 0.5f, intrin->
fx) + atan2f(intrin->
width - (intrin->
ppx + 0.5f), intrin->
fx)) * 57.2957795
f;
179 to_fov[1] = (atan2f(intrin->
ppy + 0.5f, intrin->
fy) + atan2f(intrin->
height - (intrin->
ppy + 0.5f), intrin->
fy)) * 57.2957795
f;
184 float line_slope = (end[1] - start[1]) / (end[0] - start[0]);
185 if (fabs(end[0] - curr[0]) > fabs(end[1] - curr[1]))
187 curr[0] = end[0] > curr[0] ? curr[0] + 1 : curr[0] - 1;
188 curr[1] = end[1] - line_slope * (end[0] - curr[0]);
192 curr[1] = end[1] > curr[1] ? curr[1] + 1 : curr[1] - 1;
193 curr[0] = end[0] - ((end[1] + curr[1]) / line_slope);
199 return ((end[0] >= start[0] && end[0] >= curr[0] && curr[0] >= start[0]) || (end[0] <= start[0] && end[0] <= curr[0] && curr[0] <= start[0])) &&
200 ((end[1] >= start[1] && end[1] >= curr[1] && curr[1] >= start[1]) || (end[1] <= start[1] && end[1] <= curr[1] && curr[1] <= start[1]));
205 if (p[0] < 0) p[0] = 0;
206 if (p[0] > width) p[0] = (float)width;
207 if (p[1] < 0) p[1] = 0;
208 if (p[1] > height) p[1] = (float)height;
214 float depth_min,
float depth_max,
219 const float from_pixel[2])
222 float start_pixel[2] = { 0 }, min_point[3] = { 0 }, min_transformed_point[3] = { 0 };
229 float end_pixel[2] = { 0 }, max_point[3] = { 0 }, max_transformed_point[3] = { 0 };
239 float depth = depth_scale * data[(int)
p[1] * depth_intrin->
width + (
int)
p[0]];
243 float projected_pixel[2] = { 0 },
point[3] = { 0 }, transformed_point[3] = { 0 };
248 float new_dist = (float)(pow((projected_pixel[1] - from_pixel[1]), 2) + pow((projected_pixel[0] - from_pixel[0]), 2));
249 if (new_dist < min_dist || min_dist < 0)
static void rs2_transform_point_to_point(float to_point[3], const struct rs2_extrinsics *extrin, const float from_point[3])
static void rs2_fov(const struct rs2_intrinsics *intrin, float to_fov[2])
GLint GLint GLsizei GLsizei GLsizei depth
Exposes librealsense functionality for C compilers.
Exposes RealSense frame functionality for C compilers.
static void rs2_deproject_pixel_to_point(float point[3], const struct rs2_intrinsics *intrin, const float pixel[2], float depth)
Exposes RealSense structs.
static void adjust_2D_point_to_boundary(float p[2], int width, int height)
GLint GLsizei GLsizei height
Exposes RealSense sensor functionality for C compilers.
static void next_pixel_in_line(float curr[2], const float start[2], const float end[2])
Cross-stream extrinsics: encodes the topology describing how the different devices are oriented...
static void rs2_project_point_to_pixel(float pixel[2], const struct rs2_intrinsics *intrin, const float point[3])
static bool is_pixel_in_line(const float curr[2], const float start[2], const float end[2])
static void rs2_project_color_pixel_to_depth_pixel(float to_pixel[2], const uint16_t *data, float depth_scale, float depth_min, float depth_max, const struct rs2_intrinsics *depth_intrin, const struct rs2_intrinsics *color_intrin, const struct rs2_extrinsics *color_to_depth, const struct rs2_extrinsics *depth_to_color, const float from_pixel[2])