00001
00002
00003
00004 #ifndef LIBREALSENSE_RSUTIL_H
00005 #define LIBREALSENSE_RSUTIL_H
00006
00007 #include "rs.h"
00008 #include "assert.h"
00009
00010
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);
00014 assert(intrin->model != RS_DISTORTION_FTHETA);
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
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);
00036 assert(intrin->model != RS_DISTORTION_FTHETA);
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
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
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},
00079 {5, 5, 0, 0, 1023, 0, 0, 0, 0, 2047},
00080 {5, 5, 115, 1, 512, 6, 18, 25, 3, 24},
00081 {5, 5, 185, 5, 505, 6, 35, 45, 45, 14},
00082 {5, 5, 175, 24, 430, 6, 48, 47, 24, 12},
00083 {5, 5, 235, 27, 420, 8, 80, 70, 90, 12},
00084 };
00085 rs_set_device_options(device, depth_control_options, 10, depth_control_presets[preset], 0);
00086 }
00087
00088
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},
00111 {1, 0, 303, 605, 303, -1, -1, -1, 1250, 975, 1, 1, 7, 0, -1},
00112 {0, 0, -1, -1, -1, -1, -1, -1, -1, -1, 16, 1, 6, 2, 22},
00113 {1, 1, 100, 179, 100, 2, 16, -1, 1000, 450, 1, 1, 6, 3, -1},
00114 {0, 1, -1, -1, -1, 2, 16, 16, 1000, 450, 1, 1, 3, 1, 9},
00115 {0, 0, -1, -1, -1, -1, -1, -1, -1, -1, 16, 1, 5, 1, 22},
00116 {2, 0, 40, 1600, 800, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1},
00117 {1, 1, 100, 179, 179, 2, 16, -1, 1000, 450, 1, 1, 6, 1, -1},
00118 {0, 0, -1, -1, -1, -1, -1, -1, -1, -1, 16, 1, 5, 3, 9},
00119 {1, 1, 180, 605, 303, 2, 16, -1, 1250, 650, 1, 1, 5, 1, -1},
00120 {2, 0, 40, 1600, 800, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1}
00121 };
00122
00123
00124
00125
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