rsutil.h
Go to the documentation of this file.
00001 /* License: Apache 2.0. See LICENSE file in root directory.
00002    Copyright(c) 2015 Intel Corporation. All Rights Reserved. */
00003 
00004 #ifndef LIBREALSENSE_RSUTIL_H
00005 #define LIBREALSENSE_RSUTIL_H
00006 
00007 #include "rs.h"
00008 #include "assert.h"
00009 
00010 /* 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 */
00011 static void rs_project_point_to_pixel(float pixel[2], const struct rs_intrinsics * intrin, const float point[3])
00012 {
00013     assert(intrin->model != RS_DISTORTION_INVERSE_BROWN_CONRADY); // Cannot project to an inverse-distorted image
00014     assert(intrin->model != RS_DISTORTION_FTHETA); // Cannot project to an ftheta image
00015 
00016     float x = point[0] / point[2], y = point[1] / point[2];
00017     if(intrin->model == RS_DISTORTION_MODIFIED_BROWN_CONRADY)
00018     {
00019         float r2  = x*x + y*y;
00020         float f = 1 + intrin->coeffs[0]*r2 + intrin->coeffs[1]*r2*r2 + intrin->coeffs[4]*r2*r2*r2;
00021         x *= f;
00022         y *= f;
00023         float dx = x + 2*intrin->coeffs[2]*x*y + intrin->coeffs[3]*(r2 + 2*x*x);
00024         float dy = y + 2*intrin->coeffs[3]*x*y + intrin->coeffs[2]*(r2 + 2*y*y);
00025         x = dx;
00026         y = dy;
00027     }
00028     pixel[0] = x * intrin->fx + intrin->ppx;
00029     pixel[1] = y * intrin->fy + intrin->ppy;
00030 }
00031 
00032 /* 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 */
00033 static void rs_deproject_pixel_to_point(float point[3], const struct rs_intrinsics * intrin, const float pixel[2], float depth)
00034 {
00035     assert(intrin->model != RS_DISTORTION_MODIFIED_BROWN_CONRADY); // Cannot deproject from a forward-distorted image
00036     assert(intrin->model != RS_DISTORTION_FTHETA); // Cannot deproject to an ftheta image
00037 
00038     float x = (pixel[0] - intrin->ppx) / intrin->fx;
00039     float y = (pixel[1] - intrin->ppy) / intrin->fy;
00040     if(intrin->model == RS_DISTORTION_INVERSE_BROWN_CONRADY)
00041     {
00042         float r2  = x*x + y*y;
00043         float f = 1 + intrin->coeffs[0]*r2 + intrin->coeffs[1]*r2*r2 + intrin->coeffs[4]*r2*r2*r2;
00044         float ux = x*f + 2*intrin->coeffs[2]*x*y + intrin->coeffs[3]*(r2 + 2*x*x);
00045         float uy = y*f + 2*intrin->coeffs[3]*x*y + intrin->coeffs[2]*(r2 + 2*y*y);
00046         x = ux;
00047         y = uy;
00048     }
00049     point[0] = depth * x;
00050     point[1] = depth * y;
00051     point[2] = depth;
00052 }
00053 
00054 /* Transform 3D coordinates relative to one sensor to 3D coordinates relative to another viewpoint */
00055 static void rs_transform_point_to_point(float to_point[3], const struct rs_extrinsics * extrin, const float from_point[3])
00056 {
00057     to_point[0] = extrin->rotation[0] * from_point[0] + extrin->rotation[3] * from_point[1] + extrin->rotation[6] * from_point[2] + extrin->translation[0];
00058     to_point[1] = extrin->rotation[1] * from_point[0] + extrin->rotation[4] * from_point[1] + extrin->rotation[7] * from_point[2] + extrin->translation[1];
00059     to_point[2] = extrin->rotation[2] * from_point[0] + extrin->rotation[5] * from_point[1] + extrin->rotation[8] * from_point[2] + extrin->translation[2];
00060 }
00061 
00062 /* Provide access to several recommend sets of depth control parameters */
00063 static void rs_apply_depth_control_preset(rs_device * device, int preset)
00064 {
00065     static const rs_option depth_control_options[10] = {
00066         RS_OPTION_R200_DEPTH_CONTROL_ESTIMATE_MEDIAN_DECREMENT,
00067         RS_OPTION_R200_DEPTH_CONTROL_ESTIMATE_MEDIAN_INCREMENT,
00068         RS_OPTION_R200_DEPTH_CONTROL_MEDIAN_THRESHOLD,
00069         RS_OPTION_R200_DEPTH_CONTROL_SCORE_MINIMUM_THRESHOLD,
00070         RS_OPTION_R200_DEPTH_CONTROL_SCORE_MAXIMUM_THRESHOLD,
00071         RS_OPTION_R200_DEPTH_CONTROL_TEXTURE_COUNT_THRESHOLD, 
00072         RS_OPTION_R200_DEPTH_CONTROL_TEXTURE_DIFFERENCE_THRESHOLD,
00073         RS_OPTION_R200_DEPTH_CONTROL_SECOND_PEAK_THRESHOLD,
00074         RS_OPTION_R200_DEPTH_CONTROL_NEIGHBOR_THRESHOLD,
00075         RS_OPTION_R200_DEPTH_CONTROL_LR_THRESHOLD
00076     };
00077     double depth_control_presets[6][10] = {
00078         {5, 5, 192,  1,  512, 6, 24, 27,  7,   24}, /* (DEFAULT)   Default settings on chip. Similar to the medium setting and best for outdoors. */
00079         {5, 5,   0,  0, 1023, 0,  0,  0,  0, 2047}, /* (OFF)       Disable almost all hardware-based outlier removal */
00080         {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. */
00081         {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. */
00082         {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. */
00083         {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. */
00084     };
00085     rs_set_device_options(device, depth_control_options, 10, depth_control_presets[preset], 0);
00086 }
00087 
00088 /* Provide access to several recommend sets of option presets for ivcam */
00089 static void rs_apply_ivcam_preset(rs_device * device, rs_ivcam_preset preset)
00090 {
00091     const rs_option arr_options[] = {
00092         RS_OPTION_SR300_AUTO_RANGE_ENABLE_MOTION_VERSUS_RANGE,
00093         RS_OPTION_SR300_AUTO_RANGE_ENABLE_LASER,
00094         RS_OPTION_SR300_AUTO_RANGE_MIN_MOTION_VERSUS_RANGE,
00095         RS_OPTION_SR300_AUTO_RANGE_MAX_MOTION_VERSUS_RANGE,
00096         RS_OPTION_SR300_AUTO_RANGE_START_MOTION_VERSUS_RANGE,
00097         RS_OPTION_SR300_AUTO_RANGE_MIN_LASER,
00098         RS_OPTION_SR300_AUTO_RANGE_MAX_LASER,
00099         RS_OPTION_SR300_AUTO_RANGE_START_LASER,
00100         RS_OPTION_SR300_AUTO_RANGE_UPPER_THRESHOLD,
00101         RS_OPTION_SR300_AUTO_RANGE_LOWER_THRESHOLD,
00102         RS_OPTION_F200_LASER_POWER,
00103         RS_OPTION_F200_ACCURACY,
00104         RS_OPTION_F200_FILTER_OPTION,
00105         RS_OPTION_F200_CONFIDENCE_THRESHOLD,
00106         RS_OPTION_F200_MOTION_RANGE
00107     };
00108 
00109     const double arr_values[][15] = {
00110         {1,     1, 180,  303,  180,   2,  16,  -1, 1000, 450,  1,  1,  5,  1, -1}, /* ShortRange                */
00111         {1,     0, 303,  605,  303,  -1,  -1,  -1, 1250, 975,  1,  1,  7,  0, -1}, /* LongRange                 */
00112         {0,     0,  -1,   -1,   -1,  -1,  -1,  -1,   -1,  -1, 16,  1,  6,  2, 22}, /* BackgroundSegmentation    */
00113         {1,     1, 100,  179,  100,   2,  16,  -1, 1000, 450,  1,  1,  6,  3, -1}, /* GestureRecognition        */
00114         {0,     1,  -1,   -1,   -1,   2,  16,  16, 1000, 450,  1,  1,  3,  1,  9}, /* ObjectScanning            */
00115         {0,     0,  -1,   -1,   -1,  -1,  -1,  -1,   -1,  -1, 16,  1,  5,  1, 22}, /* FaceAnalytics             */
00116         {2,     0,  40, 1600,  800,  -1,  -1,  -1,   -1,  -1,  1, -1, -1, -1, -1}, /* FaceLogin                 */
00117         {1,     1, 100,  179,  179,   2,  16,  -1, 1000, 450,  1,  1,  6,  1, -1}, /* GRCursor                  */
00118         {0,     0,  -1,   -1,   -1,  -1,  -1,  -1,   -1,  -1, 16,  1,  5,  3,  9}, /* Default                   */
00119         {1,     1, 180,  605,  303,   2,  16,  -1, 1250, 650,  1,  1,  5,  1, -1}, /* MidRange                  */
00120         {2,     0,  40, 1600,  800,  -1,  -1,  -1,   -1,  -1,  1, -1, -1, -1, -1}  /* IROnly                    */
00121     };
00122 
00123     // The Default preset is handled differntly from all the rest,
00124     // When the user applies the Default preset the camera is expected to return to
00125     // Default values of depth options:
00126     if(preset == RS_IVCAM_PRESET_DEFAULT)
00127     {
00128         rs_reset_device_options_to_default(device, arr_options, 15, 0);
00129     }
00130     else
00131     {
00132         if(arr_values[preset][14] != -1) rs_set_device_options(device, arr_options, 15, arr_values[preset], 0);
00133         if(arr_values[preset][13] != -1) rs_set_device_options(device, arr_options, 14, arr_values[preset], 0);
00134         else rs_set_device_options(device, arr_options, 11, arr_values[preset], 0);
00135     }
00136 }
00137 
00138 #endif


librealsense
Author(s): Sergey Dorodnicov , Mark Horn , Reagan Lopez
autogenerated on Tue Jun 25 2019 19:54:39