Intel® RealSense™ Cross Platform API
Intel Realsense Cross-platform API
rsutil.h
Go to the documentation of this file.
1 /* License: Apache 2.0. See LICENSE file in root directory.
2  Copyright(c) 2015 Intel Corporation. All Rights Reserved. */
3 
4 #ifndef LIBREALSENSE_RSUTIL_H
5 #define LIBREALSENSE_RSUTIL_H
6 
7 #include "rs.h"
8 #include "assert.h"
9 
10 /* Given a point in 3D space, compute the corresponding pixel coordinates in an image with no distortion or forward distortion coefficients produced by the same camera */
11 static void rs_project_point_to_pixel(float pixel[2], const struct rs_intrinsics * intrin, const float point[3])
12 {
13  assert(intrin->model != RS_DISTORTION_INVERSE_BROWN_CONRADY); // Cannot project to an inverse-distorted image
14  assert(intrin->model != RS_DISTORTION_FTHETA); // Cannot project to an ftheta image
15 
16  float x = point[0] / point[2], y = point[1] / point[2];
18  {
19  float r2 = x*x + y*y;
20  float f = 1 + intrin->coeffs[0]*r2 + intrin->coeffs[1]*r2*r2 + intrin->coeffs[4]*r2*r2*r2;
21  x *= f;
22  y *= f;
23  float dx = x + 2*intrin->coeffs[2]*x*y + intrin->coeffs[3]*(r2 + 2*x*x);
24  float dy = y + 2*intrin->coeffs[3]*x*y + intrin->coeffs[2]*(r2 + 2*y*y);
25  x = dx;
26  y = dy;
27  }
28  pixel[0] = x * intrin->fx + intrin->ppx;
29  pixel[1] = y * intrin->fy + intrin->ppy;
30 }
31 
32 /* Given pixel coordinates and depth in an image with no distortion or inverse distortion coefficients, compute the corresponding point in 3D space relative to the same camera */
33 static void rs_deproject_pixel_to_point(float point[3], const struct rs_intrinsics * intrin, const float pixel[2], float depth)
34 {
35  assert(intrin->model != RS_DISTORTION_MODIFIED_BROWN_CONRADY); // Cannot deproject from a forward-distorted image
36  assert(intrin->model != RS_DISTORTION_FTHETA); // Cannot deproject to an ftheta image
37 
38  float x = (pixel[0] - intrin->ppx) / intrin->fx;
39  float y = (pixel[1] - intrin->ppy) / intrin->fy;
41  {
42  float r2 = x*x + y*y;
43  float f = 1 + intrin->coeffs[0]*r2 + intrin->coeffs[1]*r2*r2 + intrin->coeffs[4]*r2*r2*r2;
44  float ux = x*f + 2*intrin->coeffs[2]*x*y + intrin->coeffs[3]*(r2 + 2*x*x);
45  float uy = y*f + 2*intrin->coeffs[3]*x*y + intrin->coeffs[2]*(r2 + 2*y*y);
46  x = ux;
47  y = uy;
48  }
49  point[0] = depth * x;
50  point[1] = depth * y;
51  point[2] = depth;
52 }
53 
54 /* Transform 3D coordinates relative to one sensor to 3D coordinates relative to another viewpoint */
55 static void rs_transform_point_to_point(float to_point[3], const struct rs_extrinsics * extrin, const float from_point[3])
56 {
57  to_point[0] = extrin->rotation[0] * from_point[0] + extrin->rotation[3] * from_point[1] + extrin->rotation[6] * from_point[2] + extrin->translation[0];
58  to_point[1] = extrin->rotation[1] * from_point[0] + extrin->rotation[4] * from_point[1] + extrin->rotation[7] * from_point[2] + extrin->translation[1];
59  to_point[2] = extrin->rotation[2] * from_point[0] + extrin->rotation[5] * from_point[1] + extrin->rotation[8] * from_point[2] + extrin->translation[2];
60 }
61 
62 /* Provide access to several recommend sets of depth control parameters */
63 static void rs_apply_depth_control_preset(rs_device * device, int preset)
64 {
65  static const rs_option depth_control_options[10] = {
76  };
77  double depth_control_presets[6][10] = {
78  {5, 5, 192, 1, 512, 6, 24, 27, 7, 24}, /* (DEFAULT) Default settings on chip. Similar to the medium setting and best for outdoors. */
79  {5, 5, 0, 0, 1023, 0, 0, 0, 0, 2047}, /* (OFF) Disable almost all hardware-based outlier removal */
80  {5, 5, 115, 1, 512, 6, 18, 25, 3, 24}, /* (LOW) Provide a depthmap with a lower number of outliers removed, which has minimal false negatives. */
81  {5, 5, 185, 5, 505, 6, 35, 45, 45, 14}, /* (MEDIUM) Provide a depthmap with a medium number of outliers removed, which has balanced approach. */
82  {5, 5, 175, 24, 430, 6, 48, 47, 24, 12}, /* (OPTIMIZED) Provide a depthmap with a medium/high number of outliers removed. Derived from an optimization function. */
83  {5, 5, 235, 27, 420, 8, 80, 70, 90, 12}, /* (HIGH) Provide a depthmap with a higher number of outliers removed, which has minimal false positives. */
84  };
85  rs_set_device_options(device, depth_control_options, 10, depth_control_presets[preset], 0);
86 }
87 
88 /* Provide access to several recommend sets of option presets for ivcam */
89 static void rs_apply_ivcam_preset(rs_device * device, rs_ivcam_preset preset)
90 {
91  const rs_option arr_options[] = {
107  };
108 
109  const double arr_values[][15] = {
110  {1, 1, 180, 303, 180, 2, 16, -1, 1000, 450, 1, 1, 5, 1, -1}, /* ShortRange */
111  {1, 0, 303, 605, 303, -1, -1, -1, 1250, 975, 1, 1, 7, 0, -1}, /* LongRange */
112  {0, 0, -1, -1, -1, -1, -1, -1, -1, -1, 16, 1, 6, 2, 22}, /* BackgroundSegmentation */
113  {1, 1, 100, 179, 100, 2, 16, -1, 1000, 450, 1, 1, 6, 3, -1}, /* GestureRecognition */
114  {0, 1, -1, -1, -1, 2, 16, 16, 1000, 450, 1, 1, 3, 1, 9}, /* ObjectScanning */
115  {0, 0, -1, -1, -1, -1, -1, -1, -1, -1, 16, 1, 5, 1, 22}, /* FaceAnalytics */
116  {2, 0, 40, 1600, 800, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1}, /* FaceLogin */
117  {1, 1, 100, 179, 179, 2, 16, -1, 1000, 450, 1, 1, 6, 1, -1}, /* GRCursor */
118  {0, 0, -1, -1, -1, -1, -1, -1, -1, -1, 16, 1, 5, 3, 9}, /* Default */
119  {1, 1, 180, 605, 303, 2, 16, -1, 1250, 650, 1, 1, 5, 1, -1}, /* MidRange */
120  {2, 0, 40, 1600, 800, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1} /* IROnly */
121  };
122 
123  // The Default preset is handled differntly from all the rest,
124  // When the user applies the Default preset the camera is expected to return to
125  // Default values of depth options:
127  {
128  rs_reset_device_options_to_default(device, arr_options, 15, 0);
129  }
130  else
131  {
132  if(arr_values[preset][14] != -1) rs_set_device_options(device, arr_options, 15, arr_values[preset], 0);
133  if(arr_values[preset][13] != -1) rs_set_device_options(device, arr_options, 14, arr_values[preset], 0);
134  else rs_set_device_options(device, arr_options, 11, arr_values[preset], 0);
135  }
136 }
137 
138 #endif
RS_DISTORTION_FTHETA
@ RS_DISTORTION_FTHETA
Definition: rs.h:104
RS_OPTION_SR300_AUTO_RANGE_MAX_MOTION_VERSUS_RANGE
@ RS_OPTION_SR300_AUTO_RANGE_MAX_MOTION_VERSUS_RANGE
Definition: rs.h:151
RS_OPTION_R200_DEPTH_CONTROL_TEXTURE_COUNT_THRESHOLD
@ RS_OPTION_R200_DEPTH_CONTROL_TEXTURE_COUNT_THRESHOLD
Definition: rs.h:181
rs::preset
preset
Presets: general preferences that are translated by librealsense into concrete resolution and FPS.
Definition: rs.hpp:68
RS_OPTION_SR300_AUTO_RANGE_MIN_MOTION_VERSUS_RANGE
@ RS_OPTION_SR300_AUTO_RANGE_MIN_MOTION_VERSUS_RANGE
Definition: rs.h:150
rs_extrinsics::rotation
float rotation[9]
Definition: rs.h:334
rs_intrinsics::model
rs_distortion model
Definition: rs.h:308
rs_intrinsics::fy
float fy
Definition: rs.h:307
RS_OPTION_SR300_AUTO_RANGE_START_MOTION_VERSUS_RANGE
@ RS_OPTION_SR300_AUTO_RANGE_START_MOTION_VERSUS_RANGE
Definition: rs.h:152
RS_OPTION_R200_DEPTH_CONTROL_SCORE_MAXIMUM_THRESHOLD
@ RS_OPTION_R200_DEPTH_CONTROL_SCORE_MAXIMUM_THRESHOLD
Definition: rs.h:180
rs_intrinsics::ppx
float ppx
Definition: rs.h:304
rs_intrinsics::ppy
float ppy
Definition: rs.h:305
RS_DISTORTION_MODIFIED_BROWN_CONRADY
@ RS_DISTORTION_MODIFIED_BROWN_CONRADY
Definition: rs.h:102
RS_OPTION_R200_DEPTH_CONTROL_SECOND_PEAK_THRESHOLD
@ RS_OPTION_R200_DEPTH_CONTROL_SECOND_PEAK_THRESHOLD
Definition: rs.h:183
rs_reset_device_options_to_default
void rs_reset_device_options_to_default(rs_device *device, const rs_option *options, int count, rs_error **error)
Efficiently resets the value of an arbitrary number of options to default.
RS_OPTION_R200_DEPTH_CONTROL_TEXTURE_DIFFERENCE_THRESHOLD
@ RS_OPTION_R200_DEPTH_CONTROL_TEXTURE_DIFFERENCE_THRESHOLD
Definition: rs.h:182
rs_intrinsics::fx
float fx
Definition: rs.h:306
rs::stream::depth
@ depth
rs_set_device_options
void rs_set_device_options(rs_device *device, const rs_option *options, unsigned int count, const double *values, rs_error **error)
Efficiently sets the value of an arbitrary number of options, using minimal hardware IO.
RS_OPTION_SR300_AUTO_RANGE_MIN_LASER
@ RS_OPTION_SR300_AUTO_RANGE_MIN_LASER
Definition: rs.h:153
RS_OPTION_R200_DEPTH_CONTROL_SCORE_MINIMUM_THRESHOLD
@ RS_OPTION_R200_DEPTH_CONTROL_SCORE_MINIMUM_THRESHOLD
Definition: rs.h:179
RS_OPTION_F200_CONFIDENCE_THRESHOLD
@ RS_OPTION_F200_CONFIDENCE_THRESHOLD
Definition: rs.h:146
RS_OPTION_SR300_AUTO_RANGE_MAX_LASER
@ RS_OPTION_SR300_AUTO_RANGE_MAX_LASER
Definition: rs.h:154
rs_intrinsics::coeffs
float coeffs[5]
Definition: rs.h:309
RS_OPTION_R200_DEPTH_CONTROL_ESTIMATE_MEDIAN_DECREMENT
@ RS_OPTION_R200_DEPTH_CONTROL_ESTIMATE_MEDIAN_DECREMENT
Definition: rs.h:176
RS_OPTION_SR300_AUTO_RANGE_ENABLE_MOTION_VERSUS_RANGE
@ RS_OPTION_SR300_AUTO_RANGE_ENABLE_MOTION_VERSUS_RANGE
Definition: rs.h:148
rs_device
Definition: rscore.hpp:64
RS_OPTION_SR300_AUTO_RANGE_ENABLE_LASER
@ RS_OPTION_SR300_AUTO_RANGE_ENABLE_LASER
Definition: rs.h:149
RS_OPTION_F200_LASER_POWER
@ RS_OPTION_F200_LASER_POWER
Definition: rs.h:142
RS_IVCAM_PRESET_DEFAULT
@ RS_IVCAM_PRESET_DEFAULT
Definition: rs.h:119
rs_extrinsics
Cross-stream extrinsics: encode the topology describing how the different devices are connected.
Definition: rs.h:332
RS_OPTION_F200_ACCURACY
@ RS_OPTION_F200_ACCURACY
Definition: rs.h:143
RS_DISTORTION_INVERSE_BROWN_CONRADY
@ RS_DISTORTION_INVERSE_BROWN_CONRADY
Definition: rs.h:103
RS_OPTION_R200_DEPTH_CONTROL_ESTIMATE_MEDIAN_INCREMENT
@ RS_OPTION_R200_DEPTH_CONTROL_ESTIMATE_MEDIAN_INCREMENT
Definition: rs.h:177
rs_intrinsics
Video stream intrinsics.
Definition: rs.h:300
rs_option
rs_option
Defines general configuration controls.
Definition: rs.h:128
RS_OPTION_R200_DEPTH_CONTROL_LR_THRESHOLD
@ RS_OPTION_R200_DEPTH_CONTROL_LR_THRESHOLD
Definition: rs.h:185
RS_OPTION_SR300_AUTO_RANGE_START_LASER
@ RS_OPTION_SR300_AUTO_RANGE_START_LASER
Definition: rs.h:155
rs.h
Exposes librealsense functionality for C compilers.
RS_OPTION_SR300_AUTO_RANGE_LOWER_THRESHOLD
@ RS_OPTION_SR300_AUTO_RANGE_LOWER_THRESHOLD
Definition: rs.h:157
rs_extrinsics::translation
float translation[3]
Definition: rs.h:335
rs_ivcam_preset
rs_ivcam_preset
For SR300 devices: provides optimized settings (presets) for specific types of usage.
Definition: rs.h:109
RS_OPTION_F200_FILTER_OPTION
@ RS_OPTION_F200_FILTER_OPTION
Definition: rs.h:145
RS_OPTION_R200_DEPTH_CONTROL_MEDIAN_THRESHOLD
@ RS_OPTION_R200_DEPTH_CONTROL_MEDIAN_THRESHOLD
Definition: rs.h:178
RS_OPTION_F200_MOTION_RANGE
@ RS_OPTION_F200_MOTION_RANGE
Definition: rs.h:144
RS_OPTION_R200_DEPTH_CONTROL_NEIGHBOR_THRESHOLD
@ RS_OPTION_R200_DEPTH_CONTROL_NEIGHBOR_THRESHOLD
Definition: rs.h:184
RS_OPTION_SR300_AUTO_RANGE_UPPER_THRESHOLD
@ RS_OPTION_SR300_AUTO_RANGE_UPPER_THRESHOLD
Definition: rs.h:156