4 #ifndef LIBREALSENSE_RSUTIL2_H 5 #define LIBREALSENSE_RSUTIL2_H 19 static void rs2_project_point_to_pixel(
float pixel[2],
const struct rs2_intrinsics * intrin,
const float point[3])
21 float x = point[0] / point[2], y = point[1] / point[2];
28 float f = 1 + intrin->
coeffs[0]*r2 + intrin->
coeffs[1]*r2*r2 + intrin->
coeffs[4]*r2*r2*r2;
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);
38 float r = sqrtf(x*x + y*y);
43 float rd = (float)(1.0f / intrin->
coeffs[0] * atan(2 * r* tan(intrin->
coeffs[0] / 2.0f)));
49 float r = sqrtf(x*x + y*y);
54 float theta = atan(r);
55 float theta2 = theta*theta;
56 float series = 1 + theta2*(intrin->
coeffs[0] + theta2*(intrin->
coeffs[1] + theta2*(intrin->
coeffs[2] + theta2*intrin->
coeffs[3])));
57 float rd = theta*series;
62 pixel[0] = x * intrin->
fx + intrin->
ppx;
63 pixel[1] = y * intrin->
fy + intrin->
ppy;
67 static void rs2_deproject_pixel_to_point(
float point[3],
const struct rs2_intrinsics * intrin,
const float pixel[2],
float depth)
72 float x = (pixel[0] - intrin->
ppx) / intrin->
fx;
73 float y = (pixel[1] - intrin->
ppy) / intrin->
fy;
77 float f = 1 + intrin->
coeffs[0]*r2 + intrin->
coeffs[1]*r2*r2 + intrin->
coeffs[4]*r2*r2*r2;
78 float ux = x*f + 2*intrin->
coeffs[2]*x*y + intrin->
coeffs[3]*(r2 + 2*x*x);
79 float uy = y*f + 2*intrin->
coeffs[3]*x*y + intrin->
coeffs[2]*(r2 + 2*y*y);
85 float rd = sqrtf(x*x + y*y);
93 for (
int i = 0; i < 4; i++)
95 float f = theta*(1 + theta2*(intrin->
coeffs[0] + theta2*(intrin->
coeffs[1] + theta2*(intrin->
coeffs[2] + theta2*intrin->
coeffs[3])))) - rd;
96 if (abs(f) < FLT_EPSILON)
100 float df = 1 + theta2*(3 * intrin->
coeffs[0] + theta2*(5 * intrin->
coeffs[1] + theta2*(7 * intrin->
coeffs[2] + 9 * theta2*intrin->
coeffs[3])));
102 theta2 = theta*theta;
104 float r = tan(theta);
110 float rd = sqrtf(x*x + y*y);
111 if (rd < FLT_EPSILON)
115 float r = (float)(tan(intrin->
coeffs[0] * rd) / atan(2 * tan(intrin->
coeffs[0] / 2.0f)));
120 point[0] = depth * x;
121 point[1] = depth * y;
126 static void rs2_transform_point_to_point(
float to_point[3],
const struct rs2_extrinsics * extrin,
const float from_point[3])
134 static void rs2_fov(
const struct rs2_intrinsics * intrin,
float to_fov[2])
136 to_fov[0] = (atan2f(intrin->
ppx + 0.5f, intrin->
fx) + atan2f(intrin->
width - (intrin->
ppx + 0.5f), intrin->
fx)) * 57.2957795f;
137 to_fov[1] = (atan2f(intrin->
ppy + 0.5f, intrin->
fy) + atan2f(intrin->
height - (intrin->
ppy + 0.5f), intrin->
fy)) * 57.2957795f;
140 static void next_pixel_in_line(
float curr[2],
const float start[2],
const float end[2])
142 float line_slope = (end[1] - start[1]) / (end[0] - start[0]);
143 if (fabs(end[0] - curr[0]) > fabs(end[1] - curr[1]))
145 curr[0] = end[0] > curr[0] ? curr[0] + 1 : curr[0] - 1;
146 curr[1] = end[1] - line_slope * (end[0] - curr[0]);
150 curr[1] = end[1] > curr[1] ? curr[1] + 1 : curr[1] - 1;
151 curr[0] = end[0] - ((end[1] + curr[1]) / line_slope);
155 static bool is_pixel_in_line(
const float curr[2],
const float start[2],
const float end[2])
157 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])) &&
158 ((end[1] >= start[1] && end[1] >= curr[1] && curr[1] >= start[1]) || (end[1] <= start[1] && end[1] <= curr[1] && curr[1] <= start[1]));
161 static void adjust_2D_point_to_boundary(
float p[2],
int width,
int height)
163 if (p[0] < 0) p[0] = 0;
164 if (p[0] > width) p[0] = (float)width;
165 if (p[1] < 0) p[1] = 0;
166 if (p[1] > height) p[1] = (float)height;
170 static void rs2_project_color_pixel_to_depth_pixel(
float to_pixel[2],
171 const uint16_t* data,
float depth_scale,
172 float depth_min,
float depth_max,
177 const float from_pixel[2])
180 float start_pixel[2] = { 0 }, min_point[3] = { 0 }, min_transformed_point[3] = { 0 };
181 rs2_deproject_pixel_to_point(min_point, color_intrin, from_pixel, depth_min);
182 rs2_transform_point_to_point(min_transformed_point, color_to_depth, min_point);
183 rs2_project_point_to_pixel(start_pixel, depth_intrin, min_transformed_point);
184 adjust_2D_point_to_boundary(start_pixel, depth_intrin->
width, depth_intrin->
height);
187 float end_pixel[2] = { 0 }, max_point[3] = { 0 }, max_transformed_point[3] = { 0 };
188 rs2_deproject_pixel_to_point(max_point, color_intrin, from_pixel, depth_max);
189 rs2_transform_point_to_point(max_transformed_point, color_to_depth, max_point);
190 rs2_project_point_to_pixel(end_pixel, depth_intrin, max_transformed_point);
191 adjust_2D_point_to_boundary(end_pixel, depth_intrin->
width, depth_intrin->
height);
195 for (
float p[2] = { start_pixel[0], start_pixel[1] }; is_pixel_in_line(p, start_pixel, end_pixel); next_pixel_in_line(p, start_pixel, end_pixel))
197 float depth = depth_scale * data[(int)p[1] * depth_intrin->
width + (
int)p[0]];
201 float projected_pixel[2] = { 0 }, point[3] = { 0 }, transformed_point[3] = { 0 };
202 rs2_deproject_pixel_to_point(point, depth_intrin, p, depth);
203 rs2_transform_point_to_point(transformed_point, depth_to_color, point);
204 rs2_project_point_to_pixel(projected_pixel, color_intrin, transformed_point);
206 float new_dist = pow((projected_pixel[1] - from_pixel[1]), 2) + pow((projected_pixel[0] - from_pixel[0]), 2);
207 if (new_dist < min_dist || min_dist < 0)
Definition: rs_types.h:48
float translation[3]
Definition: rs_sensor.h:97
Definition: rs_types.h:52
float coeffs[5]
Definition: rs_types.h:67
Exposes librealsense functionality for C compilers.
float rotation[9]
Definition: rs_sensor.h:96
float ppx
Definition: rs_types.h:62
Exposes RealSense frame functionality for C compilers.
Exposes RealSense structs.
Definition: rs_types.h:50
int width
Definition: rs_types.h:60
Exposes RealSense sensor functionality for C compilers.
Definition: rs_types.h:49
Cross-stream extrinsics: encodes the topology describing how the different devices are oriented.
Definition: rs_sensor.h:94
rs2_distortion model
Definition: rs_types.h:66
float fy
Definition: rs_types.h:65
float fx
Definition: rs_types.h:64
Video stream intrinsics.
Definition: rs_types.h:58
int height
Definition: rs_types.h:61
float ppy
Definition: rs_types.h:63