00001
00002
00003
00004 #include <cstring>
00005 #include <algorithm>
00006 #include <thread>
00007 #include <cmath>
00008
00009 #include "hw-monitor.h"
00010 #include "ivcam-private.h"
00011
00012 using namespace rsimpl::hw_monitor;
00013 using namespace rsimpl::ivcam;
00014
00015 namespace rsimpl {
00016 namespace ivcam {
00017
00018 enum Property
00019 {
00020 IVCAM_PROPERTY_COLOR_EXPOSURE = 1,
00021 IVCAM_PROPERTY_COLOR_BRIGHTNESS = 2,
00022 IVCAM_PROPERTY_COLOR_CONTRAST = 3,
00023 IVCAM_PROPERTY_COLOR_SATURATION = 4,
00024 IVCAM_PROPERTY_COLOR_HUE = 5,
00025 IVCAM_PROPERTY_COLOR_GAMMA = 6,
00026 IVCAM_PROPERTY_COLOR_WHITE_BALANCE = 7,
00027 IVCAM_PROPERTY_COLOR_SHARPNESS = 8,
00028 IVCAM_PROPERTY_COLOR_BACK_LIGHT_COMPENSATION = 9,
00029 IVCAM_PROPERTY_COLOR_GAIN = 10,
00030 IVCAM_PROPERTY_COLOR_POWER_LINE_FREQUENCY = 11,
00031 IVCAM_PROPERTY_AUDIO_MIX_LEVEL = 12,
00032 IVCAM_PROPERTY_APERTURE = 13,
00033 IVCAM_PROPERTY_DISTORTION_CORRECTION_I = 202,
00034 IVCAM_PROPERTY_DISTORTION_CORRECTION_DPTH = 203,
00035 IVCAM_PROPERTY_DEPTH_MIRROR = 204,
00036 IVCAM_PROPERTY_COLOR_MIRROR = 205,
00037 IVCAM_PROPERTY_COLOR_FIELD_OF_VIEW = 207,
00038 IVCAM_PROPERTY_COLOR_SENSOR_RANGE = 209,
00039 IVCAM_PROPERTY_COLOR_FOCAL_LENGTH = 211,
00040 IVCAM_PROPERTY_COLOR_PRINCIPAL_POINT = 213,
00041 IVCAM_PROPERTY_DEPTH_FIELD_OF_VIEW = 215,
00042 IVCAM_PROPERTY_DEPTH_UNDISTORTED_FIELD_OF_VIEW = 223,
00043 IVCAM_PROPERTY_DEPTH_SENSOR_RANGE = 217,
00044 IVCAM_PROPERTY_DEPTH_FOCAL_LENGTH = 219,
00045 IVCAM_PROPERTY_DEPTH_UNDISTORTED_FOCAL_LENGTH = 225,
00046 IVCAM_PROPERTY_DEPTH_PRINCIPAL_POINT = 221,
00047 IVCAM_PROPERTY_MF_DEPTH_LOW_CONFIDENCE_VALUE = 5000,
00048 IVCAM_PROPERTY_MF_DEPTH_UNIT = 5001,
00049 IVCAM_PROPERTY_MF_CALIBRATION_DATA = 5003,
00050 IVCAM_PROPERTY_MF_LASER_POWER = 5004,
00051 IVCAM_PROPERTY_MF_ACCURACY = 5005,
00052 IVCAM_PROPERTY_MF_INTENSITY_IMAGE_TYPE = 5006,
00053 IVCAM_PROPERTY_MF_MOTION_VS_RANGE_TRADE = 5007,
00054 IVCAM_PROPERTY_MF_POWER_GEAR = 5008,
00055 IVCAM_PROPERTY_MF_FILTER_OPTION = 5009,
00056 IVCAM_PROPERTY_MF_VERSION = 5010,
00057 IVCAM_PROPERTY_MF_DEPTH_CONFIDENCE_THRESHOLD = 5013,
00058 IVCAM_PROPERTY_ACCELEROMETER_READING = 3000,
00059 IVCAM_PROPERTY_PROJECTION_SERIALIZABLE = 3003,
00060 IVCAM_PROPERTY_CUSTOMIZED = 0x04000000,
00061 };
00062
00064
00066
00067
00068
00069 void xu_read(const uvc::device & device, uint8_t xu_ctrl, void * buffer, uint32_t length)
00070 {
00071 uvc::get_control_with_retry(device, ivcam::depth_xu, static_cast<int>(xu_ctrl), buffer, length);
00072 }
00073
00074 void xu_write(uvc::device & device, uint8_t xu_ctrl, void * buffer, uint32_t length)
00075 {
00076 uvc::set_control_with_retry(device, ivcam::depth_xu, static_cast<int>(xu_ctrl), buffer, length);
00077 }
00078
00079 void get_laser_power(const uvc::device & device, uint8_t & laser_power)
00080 {
00081 xu_read(device, IVCAM_DEPTH_LASER_POWER, &laser_power, sizeof(laser_power));
00082 }
00083
00084 void set_laser_power(uvc::device & device, uint8_t laser_power)
00085 {
00086 xu_write(device, IVCAM_DEPTH_LASER_POWER, &laser_power, sizeof(laser_power));
00087 }
00088
00089 void get_accuracy(const uvc::device & device, uint8_t & accuracy)
00090 {
00091 xu_read(device, IVCAM_DEPTH_ACCURACY, &accuracy, sizeof(accuracy));
00092 }
00093
00094 void set_accuracy(uvc::device & device, uint8_t accuracy)
00095 {
00096 xu_write(device, IVCAM_DEPTH_ACCURACY, &accuracy, sizeof(accuracy));
00097 }
00098
00099 void get_motion_range(const uvc::device & device, uint8_t & motion_range)
00100 {
00101 xu_read(device, IVCAM_DEPTH_MOTION_RANGE, &motion_range, sizeof(motion_range));
00102 }
00103
00104 void set_motion_range(uvc::device & device, uint8_t motion_range)
00105 {
00106 xu_write(device, IVCAM_DEPTH_MOTION_RANGE, &motion_range, sizeof(motion_range));
00107 }
00108
00109 void get_filter_option(const uvc::device & device, uint8_t & filter_option)
00110 {
00111 xu_read(device, IVCAM_DEPTH_FILTER_OPTION, &filter_option, sizeof(filter_option));
00112 }
00113
00114 void set_filter_option(uvc::device & device, uint8_t filter_option)
00115 {
00116 xu_write(device, IVCAM_DEPTH_FILTER_OPTION, &filter_option, sizeof(filter_option));
00117 }
00118
00119 void get_confidence_threshold(const uvc::device & device, uint8_t & conf_thresh)
00120 {
00121 xu_read(device, IVCAM_DEPTH_CONFIDENCE_THRESH, &conf_thresh, sizeof(conf_thresh));
00122 }
00123
00124 void set_confidence_threshold(uvc::device & device, uint8_t conf_thresh)
00125 {
00126 xu_write(device, IVCAM_DEPTH_CONFIDENCE_THRESH, &conf_thresh, sizeof(conf_thresh));
00127 }
00128
00130
00132
00133 void claim_ivcam_interface(uvc::device & device)
00134 {
00135 const uvc::guid IVCAM_WIN_USB_DEVICE_GUID = { 0x175695CD, 0x30D9, 0x4F87, {0x8B, 0xE3, 0x5A, 0x82, 0x70, 0xF4, 0x9A, 0x31} };
00136 claim_interface(device, IVCAM_WIN_USB_DEVICE_GUID, IVCAM_MONITOR_INTERFACE);
00137 }
00138
00139 size_t prepare_usb_command(uint8_t * request, size_t & requestSize, uint32_t op, uint32_t p1, uint32_t p2, uint32_t p3, uint32_t p4, uint8_t * data, size_t dataLength)
00140 {
00141
00142 if (requestSize < IVCAM_MONITOR_HEADER_SIZE)
00143 return 0;
00144
00145 size_t index = sizeof(uint16_t);
00146 *(uint16_t *)(request + index) = IVCAM_MONITOR_MAGIC_NUMBER;
00147 index += sizeof(uint16_t);
00148 *(uint32_t *)(request + index) = op;
00149 index += sizeof(uint32_t);
00150 *(uint32_t *)(request + index) = p1;
00151 index += sizeof(uint32_t);
00152 *(uint32_t *)(request + index) = p2;
00153 index += sizeof(uint32_t);
00154 *(uint32_t *)(request + index) = p3;
00155 index += sizeof(uint32_t);
00156 *(uint32_t *)(request + index) = p4;
00157 index += sizeof(uint32_t);
00158
00159 if (dataLength)
00160 {
00161 memcpy(request + index, data, dataLength);
00162 index += dataLength;
00163 }
00164
00165
00166 *(uint16_t *)request = (uint16_t)(index - sizeof(uint32_t));
00167 requestSize = index;
00168 return index;
00169 }
00170
00171
00172
00173 void get_gvd(uvc::device & device, std::timed_mutex & mutex, size_t sz, char * gvd, int gvd_cmd)
00174 {
00175 hwmon_cmd cmd((uint8_t)gvd_cmd);
00176 perform_and_send_monitor_command(device, mutex, cmd);
00177 auto minSize = std::min(sz, cmd.receivedCommandDataLength);
00178 memcpy(gvd, cmd.receivedCommandData, minSize);
00179 }
00180
00181 void get_firmware_version_string(uvc::device & device, std::timed_mutex & mutex, std::string & version, int gvd_cmd, int offset)
00182 {
00183 std::vector<char> gvd(1024);
00184 get_gvd(device, mutex, 1024, gvd.data(), gvd_cmd);
00185 char fws[8];
00186 memcpy(fws, gvd.data() + offset, 8);
00187 version = std::string(std::to_string(fws[3]) + "." + std::to_string(fws[2]) + "." + std::to_string(fws[1]) + "." + std::to_string(fws[0]));
00188 }
00189
00190 void get_module_serial_string(uvc::device & device, std::timed_mutex & mutex, std::string & serial, int offset)
00191 {
00192 std::vector<char> gvd(1024);
00193 get_gvd(device, mutex, 1024, gvd.data());
00194 unsigned char ss[8];
00195 memcpy(ss, gvd.data() + offset, 8);
00196 char formattedBuffer[64];
00197 if (offset == 96)
00198 {
00199 sprintf(formattedBuffer, "%02X%02X%02X%02X%02X%02X", ss[0], ss[1], ss[2], ss[3], ss[4], ss[5]);
00200 serial = std::string(formattedBuffer);
00201 }
00202 else if (offset == 132)
00203 {
00204 sprintf(formattedBuffer, "%02X%02X%02X%02X%02X%-2X", ss[0], ss[1], ss[2], ss[3], ss[4], ss[5]);
00205 serial = std::string(formattedBuffer);
00206 }
00207 }
00208
00209 void force_hardware_reset(uvc::device & device, std::timed_mutex & mutex)
00210 {
00211 hwmon_cmd cmd((uint8_t)fw_cmd::HWReset);
00212 cmd.oneDirection = true;
00213 perform_and_send_monitor_command(device, mutex, cmd);
00214 }
00215
00216 void enable_timestamp(uvc::device & device, std::timed_mutex & mutex, bool colorEnable, bool depthEnable)
00217 {
00218 hwmon_cmd cmd((uint8_t)fw_cmd::TimeStampEnable);
00219 cmd.Param1 = depthEnable ? 1 : 0;
00220 cmd.Param2 = colorEnable ? 1 : 0;
00221 perform_and_send_monitor_command(device, mutex, cmd);
00222 }
00223
00224 void set_auto_range(uvc::device & device, std::timed_mutex & mutex, int enableMvR, int16_t minMvR, int16_t maxMvR, int16_t startMvR, int enableLaser, int16_t minLaser, int16_t maxLaser, int16_t startLaser, int16_t ARUpperTH, int16_t ARLowerTH)
00225 {
00226 hwmon_cmd CommandParameters((uint8_t)fw_cmd::SetAutoRange);
00227 CommandParameters.Param1 = enableMvR;
00228 CommandParameters.Param2 = enableLaser;
00229
00230 auto data = reinterpret_cast<int16_t *>(CommandParameters.data);
00231 data[0] = minMvR;
00232 data[1] = maxMvR;
00233 data[2] = startMvR;
00234 data[3] = minLaser;
00235 data[4] = maxLaser;
00236 data[5] = startLaser;
00237 auto size = 12;
00238
00239 if (ARUpperTH != -1)
00240 {
00241 data[6] = ARUpperTH;
00242 size += 2;
00243 }
00244
00245 if (ARLowerTH != -1)
00246 {
00247 data[7] = ARLowerTH;
00248 size += 2;
00249 }
00250
00251 CommandParameters.sizeOfSendCommandData = size;
00252 perform_and_send_monitor_command(device, mutex, CommandParameters);
00253 }
00254
00255 FirmwareError get_fw_last_error(uvc::device & device, std::timed_mutex & mutex)
00256 {
00257 hwmon_cmd cmd((uint8_t)fw_cmd::GetFWLastError);
00258 memset(cmd.data, 0, 4);
00259
00260 perform_and_send_monitor_command(device, mutex, cmd);
00261 return *reinterpret_cast<FirmwareError *>(cmd.receivedCommandData);
00262 }
00263
00264 }
00265 namespace f200
00266 {
00267 struct cam_asic_coefficients
00268 {
00269 float CoefValueArray[NUM_OF_CALIBRATION_COEFFS];
00270 };
00271
00272 struct OACOffsetData
00273 {
00274 int OACOffset1;
00275 int OACOffset2;
00276 int OACOffset3;
00277 int OACOffset4;
00278 };
00279
00280 struct IVCAMTesterData
00281 {
00282 int16_t TableValidation;
00283 int16_t TableVarsion;
00284 cam_temperature_data TemperatureData;
00285 OACOffsetData OACOffsetData_;
00286 thermal_loop_params ThermalLoopParams;
00287 };
00288
00290
00292
00293 void set_asic_coefficients(uvc::device & device, std::timed_mutex & mutex, const cam_asic_coefficients & coeffs)
00294 {
00295
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305 hwmon_cmd command((uint8_t)fw_cmd::UpdateCalib);
00306
00307 memcpy(command.data, coeffs.CoefValueArray, NUM_OF_CALIBRATION_COEFFS * sizeof(float));
00308 command.Param1 = 0;
00309 command.Param2 = 0;
00310 command.Param3 = 0;
00311 command.Param4 = 0;
00312 command.oneDirection = false;
00313 command.sizeOfSendCommandData = NUM_OF_CALIBRATION_COEFFS * sizeof(float);
00314 command.TimeOut = 5000;
00315
00316 perform_and_send_monitor_command(device, mutex, command);
00317 }
00318
00319 float read_mems_temp(uvc::device & device, std::timed_mutex & mutex)
00320 {
00321 hwmon_cmd command((uint8_t)fw_cmd::GetMEMSTemp);
00322 command.Param1 = 0;
00323 command.Param2 = 0;
00324 command.Param3 = 0;
00325 command.Param4 = 0;
00326 command.sizeOfSendCommandData = 0;
00327 command.TimeOut = 5000;
00328 command.oneDirection = false;
00329
00330 perform_and_send_monitor_command(device, mutex, command);
00331 int32_t t = *reinterpret_cast<int32_t *>(command.receivedCommandData);
00332 return static_cast<float>(t) / 100;
00333 }
00334
00335 int read_ir_temp(uvc::device & device, std::timed_mutex & mutex)
00336 {
00337 hwmon_cmd command((uint8_t)fw_cmd::GetIRTemp);
00338 command.Param1 = 0;
00339 command.Param2 = 0;
00340 command.Param3 = 0;
00341 command.Param4 = 0;
00342 command.sizeOfSendCommandData = 0;
00343 command.TimeOut = 5000;
00344 command.oneDirection = false;
00345
00346 perform_and_send_monitor_command(device, mutex, command);
00347 return static_cast<int8_t>(command.receivedCommandData[0]);
00348 }
00349
00350 void get_f200_calibration_raw_data(uvc::device & device, std::timed_mutex & usbMutex, uint8_t * data, size_t & bytesReturned)
00351 {
00352 uint8_t request[IVCAM_MONITOR_HEADER_SIZE];
00353 size_t requestSize = sizeof(request);
00354 uint32_t responseOp;
00355
00356 if (prepare_usb_command(request, requestSize, (uint32_t)fw_cmd::GetCalibrationTable) == 0)
00357 throw std::runtime_error("usb transfer to retrieve calibration data failed");
00358 execute_usb_command(device, usbMutex, request, requestSize, responseOp, data, bytesReturned);
00359 }
00360
00361 int bcdtoint(uint8_t * buf, int bufsize)
00362 {
00363 int r = 0;
00364 for (int i = 0; i < bufsize; i++)
00365 r = r * 10 + *buf++;
00366 return r;
00367 }
00368
00369 int get_version_of_calibration(uint8_t * validation, uint8_t * version)
00370 {
00371 uint8_t valid[2] = { 0X14, 0x0A };
00372 if (memcmp(valid, validation, 2) != 0) return 0;
00373 else return bcdtoint(version, 2);
00374 }
00375
00376 std::tuple<ivcam::camera_calib_params, cam_temperature_data, thermal_loop_params> get_f200_calibration(uint8_t * rawCalibData, size_t len)
00377 {
00378 uint8_t * bufParams = rawCalibData + 4;
00379
00380 ivcam::cam_calibration CalibrationData;
00381 IVCAMTesterData TesterData;
00382
00383 memset(&CalibrationData, 0, sizeof(cam_calibration));
00384
00385 int ver = get_version_of_calibration(bufParams, bufParams + 2);
00386
00387 if (ver > IVCAM_MIN_SUPPORTED_VERSION)
00388 {
00389 rawCalibData = rawCalibData + 4;
00390
00391 size_t size = (sizeof(cam_calibration) > len) ? len : sizeof(cam_calibration);
00392
00393 auto fixWithVersionInfo = [&](cam_calibration &d, int size, uint8_t * data)
00394 {
00395 memcpy((uint8_t*)&d + sizeof(int), data, size - sizeof(int));
00396 };
00397
00398 fixWithVersionInfo(CalibrationData, (int)size, rawCalibData);
00399
00400 ivcam::camera_calib_params calibration;
00401 memcpy(&calibration, &CalibrationData.CalibrationParameters, sizeof(ivcam::camera_calib_params));
00402 memcpy(&TesterData, rawCalibData, SIZE_OF_CALIB_HEADER_BYTES);
00403
00404
00405 int EndOfCalibratioData = SIZE_OF_CALIB_PARAM_BYTES + SIZE_OF_CALIB_HEADER_BYTES;
00406 memcpy((uint8_t*)&TesterData + SIZE_OF_CALIB_HEADER_BYTES, rawCalibData + EndOfCalibratioData, sizeof(IVCAMTesterData) - SIZE_OF_CALIB_HEADER_BYTES);
00407 return std::make_tuple(calibration, TesterData.TemperatureData, TesterData.ThermalLoopParams);
00408 }
00409
00410 if (ver == IVCAM_MIN_SUPPORTED_VERSION)
00411 {
00412 float *params = (float *)bufParams;
00413
00414 ivcam::camera_calib_params calibration;
00415 memcpy(&calibration, params + 1, sizeof(ivcam::camera_calib_params));
00416 memcpy(&TesterData, bufParams, SIZE_OF_CALIB_HEADER_BYTES);
00417
00418 memset((uint8_t*)&TesterData + SIZE_OF_CALIB_HEADER_BYTES, 0, sizeof(IVCAMTesterData) - SIZE_OF_CALIB_HEADER_BYTES);
00419 return std::make_tuple(calibration, TesterData.TemperatureData, TesterData.ThermalLoopParams);
00420 }
00421
00422 throw std::runtime_error("calibration table is not compatible with this API");
00423 }
00424
00425 std::tuple<ivcam::camera_calib_params, cam_temperature_data, thermal_loop_params> read_f200_calibration(uvc::device & device, std::timed_mutex & mutex)
00426 {
00427 uint8_t rawCalibrationBuffer[HW_MONITOR_BUFFER_SIZE];
00428 size_t bufferLength = HW_MONITOR_BUFFER_SIZE;
00429 get_f200_calibration_raw_data(device, mutex, rawCalibrationBuffer, bufferLength);
00430 return get_f200_calibration(rawCalibrationBuffer, bufferLength);
00431 }
00432
00433 void generate_asic_calibration_coefficients(const ivcam::camera_calib_params & compensated_calibration, std::vector<int> resolution, const bool isZMode, float * values)
00434 {
00435 auto params = compensated_calibration;
00436
00437
00438 bool isQres = resolution[0] == 640 && resolution[1] == 360;
00439
00440 if (isQres)
00441 {
00442 resolution[1] = 480;
00443 }
00444
00445
00446 int scale = 5;
00447
00448 float width = (float)resolution[0] * scale;
00449 float height = (float)resolution[1];
00450
00451 int PrecisionBits = 16;
00452 int CodeBits = 14;
00453 int TexturePrecisionBits = 12;
00454 float ypscale = (float)(1 << (CodeBits + 1 - 10));
00455 float ypoff = 0;
00456
00457 float s1 = (float)(1 << (PrecisionBits)) / 2047;
00458 float s2 = (float)(1 << (CodeBits)) - ypscale*0.5f;
00459
00460 float alpha = 2 / (width*params.Kc[0][0]);
00461 float beta = -(params.Kc[0][2] + 1) / params.Kc[0][0];
00462 float gamma = 2 / (height*params.Kc[1][1]);
00463 float delta = -(params.Kc[1][2] + 1) / params.Kc[1][1];
00464
00465 float a = alpha / gamma;
00466 float a1 = 1;
00467 float b = 0.5f*scale*a + beta / gamma;
00468 float c = 0.5f*a1 + delta / gamma;
00469
00470 float d0 = 1;
00471 float d1 = params.Invdistc[0] * pow(gamma, (float)2.0);
00472 float d2 = params.Invdistc[1] * pow(gamma, (float)4.0);
00473 float d5 = (float)((double)(params.Invdistc[4]) * pow((double)gamma, 6.0));
00474 float d3 = params.Invdistc[2] * gamma;
00475 float d4 = params.Invdistc[3] * gamma;
00476
00477 float q = 1 / pow(gamma, (float)2.0);
00478 float p1 = params.Pp[2][3] * s1;
00479 float p2 = -s1*s2*(params.Pp[1][3] + params.Pp[2][3]);
00480
00481 if (isZMode)
00482 {
00483 p1 = p1*sqrt(q);
00484 p2 = p2*sqrt(q);
00485 }
00486
00487 float p3 = -params.Pp[2][0];
00488 float p4 = -params.Pp[2][1];
00489 float p5 = -params.Pp[2][2] / gamma;
00490 float p6 = s2*(params.Pp[1][0] + params.Pp[2][0]);
00491 float p7 = s2*(params.Pp[1][1] + params.Pp[2][1]);
00492 float p8 = s2*(params.Pp[1][2] + params.Pp[2][2]) / gamma;
00493
00494
00495 float sreproj = 2;
00496 float ax = -(1 + params.Kp[0][2]) / params.Kp[0][0];
00497 float ay = -(1 + params.Kp[1][2]) / params.Kp[1][1];
00498
00499 float f0 = (params.Pp[0][1] + params.Pp[2][1]) / (params.Pp[0][0] + params.Pp[2][0]) / params.Kp[0][0];
00500 float f1 = (params.Pp[0][2] + params.Pp[2][2]) / (params.Pp[0][0] + params.Pp[2][0]) / gamma / params.Kp[0][0];
00501 float f2 = 0;
00502 float f3 = 0;
00503 float f4 = 0;
00504 float f5 = 2 * params.Pp[2][0] / (params.Pp[0][0] + params.Pp[2][0]) / sreproj;
00505 float f6 = 2 * params.Pp[2][1] / (params.Pp[0][0] + params.Pp[2][0]) / sreproj;
00506 float f7 = 2 * params.Pp[2][2] / (params.Pp[0][0] + params.Pp[2][0]) / sreproj / gamma;
00507 float f8 = (float)((double)(params.Pp[0][3] + params.Pp[2][3]) / (params.Pp[0][0] + params.Pp[2][0]) * s1 / params.Kp[0][0]);
00508 float f9 = (params.Pp[1][3] + params.Pp[2][3]) / (params.Pp[0][0] + params.Pp[2][0]) * s1 / params.Kp[1][1];
00509 float f10 = 2 * params.Pp[2][3] / (params.Pp[0][0] + params.Pp[2][0]) * s1 / sreproj;
00510 if (isZMode)
00511 {
00512 f8 = f8 * sqrt(q);
00513 f9 = 0;
00514 f10 = f10 * sqrt(q);
00515 }
00516 float f11 = 1 / params.Kp[0][0];
00517
00518
00519 f11 = f11 + ax*f5;
00520 f0 = f0 + ax*f6;
00521 f1 = f1 + ax*f7;
00522 f8 = f8 + ax*f10;
00523 f2 = f2 + ay*f5;
00524 f3 = f3 + ay*f6;
00525 f4 = f4 + ay*f7;
00526 f9 = f9 + ay*f10;
00527
00528
00529 float suv = (float)((1 << TexturePrecisionBits) - 1);
00530
00531 float h0 = (params.Pt[0][1] + params.Pt[2][1]) / (params.Pt[0][0] + params.Pt[2][0]);
00532 float h1 = (params.Pt[0][2] + params.Pt[2][2]) / (params.Pt[0][0] + params.Pt[2][0]) / gamma;
00533 float h2 = (params.Pt[1][0] + params.Pt[2][0]) / (params.Pt[0][0] + params.Pt[2][0]);
00534 float h3 = (params.Pt[1][1] + params.Pt[2][1]) / (params.Pt[0][0] + params.Pt[2][0]);
00535 float h4 = (params.Pt[1][2] + params.Pt[2][2]) / (params.Pt[0][0] + params.Pt[2][0]) / gamma;
00536 float h5 = 2 * params.Pt[2][0] / (params.Pt[0][0] + params.Pt[2][0]) / suv;
00537 float h6 = 2 * params.Pt[2][1] / (params.Pt[0][0] + params.Pt[2][0]) / suv;
00538 float h7 = 2 * params.Pt[2][2] / (params.Pt[0][0] + params.Pt[2][0]) / suv / gamma;
00539 float h8 = (params.Pt[0][3] + params.Pt[2][3]) / (params.Pt[0][0] + params.Pt[2][0]) * s1;
00540 float h9 = (params.Pt[1][3] + params.Pt[2][3]) / (params.Pt[0][0] + params.Pt[2][0]) * s1;
00541 float h10 = 2 * params.Pt[2][3] / (params.Pt[0][0] + params.Pt[2][0]) * s1 / suv;
00542 float h11 = 1;
00543
00544 if (isZMode)
00545 {
00546 h8 = h8 * sqrt(q);
00547 h9 = h9 * sqrt(q);
00548 h10 = h10 * sqrt(q);
00549 }
00550
00551 float o1 = (1 + params.Kp[0][2]) / params.Kp[0][0];
00552 float o2 = -(1 + params.Kp[1][2]) / params.Kp[1][1];
00553 float o3 = 1 / s2 / params.Kp[1][1];
00554 float o4 = 0;
00555
00556 float dp1 = params.Distp[0];
00557 float dp2 = params.Distp[1];
00558 float dp3 = params.Distp[2];
00559 float dp4 = params.Distp[3];
00560 float dp5 = params.Distp[4];
00561
00562 float ip0 = params.Kp[1][1] * s2;
00563 float ip1 = (s2*params.Invdistp[0] * params.Kp[1][1]) + s2*(1 + params.Kp[1][2]);
00564 float ip2 = (s2*params.Invdistp[1] * params.Kp[1][1]);
00565 float ip3 = (s2*params.Invdistp[2] * params.Kp[1][1]);
00566 float ip4 = (s2*params.Invdistp[3] * params.Kp[1][1]);
00567 float ip5 = (s2*params.Invdistp[4] * params.Kp[1][1]);
00568
00569 if (isQres)
00570 c *= 0.75;
00571
00572
00573 float coeffs[NUM_OF_CALIBRATION_COEFFS] = { 1.0f,3.0f,a,a1,b,c,d0,d1,d2,
00574 d3,d4,d5,q,p1,p2,p3,p4,p5,p6,p7,p8,h0,h1,h2,h3,h4,h5,h6,h7,h8,h9,h10,h11,f0,f1,
00575 f2,f3,f4,f5,f6,f7,f8,f9,f10,f11,o1,o2,o3,o4,dp1,dp2,dp3,dp4,dp5,ip0,ip1,ip2,ip3,
00576 ip4,ip5,ypscale,ypoff,0,0 };
00577
00578 memcpy(values, coeffs, NUM_OF_CALIBRATION_COEFFS * sizeof(float));
00579 return;
00580 }
00581
00582 void update_asic_coefficients(uvc::device & device, std::timed_mutex & mutex, const ivcam::camera_calib_params & compensated_params)
00583 {
00584 cam_asic_coefficients coeffs = {};
00585 generate_asic_calibration_coefficients(compensated_params, { 640, 480 }, true, coeffs.CoefValueArray);
00586 set_asic_coefficients(device, mutex, coeffs);
00587 }
00588
00589 void get_dynamic_fps(const uvc::device & device, uint8_t & dynamic_fps)
00590 {
00591 return xu_read(device, IVCAM_DEPTH_DYNAMIC_FPS, &dynamic_fps, sizeof(dynamic_fps));
00592 }
00593
00594 void set_dynamic_fps(uvc::device & device, uint8_t dynamic_fps)
00595 {
00596 return xu_write(device, IVCAM_DEPTH_DYNAMIC_FPS, &dynamic_fps, sizeof(dynamic_fps));
00597 }
00598 }
00599 namespace sr300 {
00600
00601 struct SR300RawCalibration
00602 {
00603 uint16_t tableVersion;
00604 uint16_t tableID;
00605 uint32_t dataSize;
00606 uint32_t reserved;
00607 int crc;
00608 ivcam::camera_calib_params CalibrationParameters;
00609 uint8_t reserved_1[176];
00610 uint8_t reserved21[148];
00611 };
00612
00613 enum class cam_data_source : uint32_t
00614 {
00615 TakeFromRO = 0,
00616 TakeFromRW = 1,
00617 TakeFromRAM = 2
00618 };
00619
00621
00623 void get_sr300_calibration_raw_data(uvc::device & device, std::timed_mutex & mutex, uint8_t * data, size_t & bytesReturned)
00624 {
00625 hwmon_cmd command((uint8_t)fw_cmd::GetCalibrationTable);
00626 command.Param1 = (uint32_t)cam_data_source::TakeFromRAM;
00627
00628 perform_and_send_monitor_command(device, mutex, command);
00629 memcpy(data, command.receivedCommandData, HW_MONITOR_BUFFER_SIZE);
00630 bytesReturned = command.receivedCommandDataLength;
00631 }
00632
00633 ivcam::camera_calib_params read_sr300_calibration(uvc::device & device, std::timed_mutex & mutex)
00634 {
00635 uint8_t rawCalibrationBuffer[HW_MONITOR_BUFFER_SIZE];
00636 size_t bufferLength = HW_MONITOR_BUFFER_SIZE;
00637 get_sr300_calibration_raw_data(device, mutex, rawCalibrationBuffer, bufferLength);
00638
00639 SR300RawCalibration rawCalib;
00640 memcpy(&rawCalib, rawCalibrationBuffer, std::min(sizeof(rawCalib), bufferLength));
00641 return rawCalib.CalibrationParameters;
00642 }
00643 }
00644 }