00001
00002
00003
00004 #include <climits>
00005 #include <algorithm>
00006
00007 #include "image.h"
00008 #include "ds-private.h"
00009 #include "zr300.h"
00010 #include "ivcam-private.h"
00011 #include "hw-monitor.h"
00012
00013 using namespace rsimpl;
00014 using namespace rsimpl::ds;
00015 using namespace rsimpl::motion_module;
00016 using namespace rsimpl::hw_monitor;
00017
00018 namespace rsimpl
00019 {
00020 zr300_camera::zr300_camera(std::shared_ptr<uvc::device> device, const static_device_info & info, motion_module_calibration in_fe_intrinsic, calibration_validator validator)
00021 : ds_device(device, info, validator),
00022 motion_module_ctrl(device.get(), usbMutex),
00023 auto_exposure(nullptr),
00024 to_add_frames((auto_exposure_state.get_auto_exposure_state(RS_OPTION_FISHEYE_ENABLE_AUTO_EXPOSURE) == 1)),
00025 fe_intrinsic(in_fe_intrinsic)
00026 {}
00027
00028 zr300_camera::~zr300_camera()
00029 {
00030
00031 }
00032
00033 bool is_fisheye_uvc_control(rs_option option)
00034 {
00035 return (option == RS_OPTION_FISHEYE_GAIN);
00036 }
00037
00038 bool is_fisheye_xu_control(rs_option option)
00039 {
00040 return (option == RS_OPTION_FISHEYE_STROBE) ||
00041 (option == RS_OPTION_FISHEYE_EXTERNAL_TRIGGER) ||
00042 (option == RS_OPTION_FISHEYE_EXPOSURE);
00043 }
00044
00045 void zr300_camera::set_options(const rs_option options[], size_t count, const double values[])
00046 {
00047 std::vector<rs_option> base_opt;
00048 std::vector<double> base_opt_val;
00049
00050 auto& dev = get_device();
00051
00052
00053 for (size_t i = 0; i < count; ++i)
00054 {
00055 if (is_fisheye_uvc_control(options[i]))
00056 {
00057 uvc::set_pu_control_with_retry(dev, 3, options[i], static_cast<int>(values[i]));
00058 continue;
00059 }
00060
00061 switch (options[i])
00062 {
00063 case RS_OPTION_FISHEYE_STROBE: zr300::set_fisheye_strobe(dev, static_cast<uint8_t>(values[i])); break;
00064 case RS_OPTION_FISHEYE_EXTERNAL_TRIGGER: zr300::set_fisheye_external_trigger(dev, static_cast<uint8_t>(values[i])); break;
00065 case RS_OPTION_FISHEYE_EXPOSURE: zr300::set_fisheye_exposure(dev, static_cast<uint16_t>(values[i])); break;
00066 case RS_OPTION_FISHEYE_ENABLE_AUTO_EXPOSURE: set_auto_exposure_state(RS_OPTION_FISHEYE_ENABLE_AUTO_EXPOSURE, values[i]); break;
00067 case RS_OPTION_FISHEYE_AUTO_EXPOSURE_MODE: set_auto_exposure_state(RS_OPTION_FISHEYE_AUTO_EXPOSURE_MODE, values[i]); break;
00068 case RS_OPTION_FISHEYE_AUTO_EXPOSURE_ANTIFLICKER_RATE: set_auto_exposure_state(RS_OPTION_FISHEYE_AUTO_EXPOSURE_ANTIFLICKER_RATE, values[i]); break;
00069 case RS_OPTION_FISHEYE_AUTO_EXPOSURE_PIXEL_SAMPLE_RATE: set_auto_exposure_state(RS_OPTION_FISHEYE_AUTO_EXPOSURE_PIXEL_SAMPLE_RATE, values[i]); break;
00070 case RS_OPTION_FISHEYE_AUTO_EXPOSURE_SKIP_FRAMES: set_auto_exposure_state(RS_OPTION_FISHEYE_AUTO_EXPOSURE_SKIP_FRAMES, values[i]); break;
00071 case RS_OPTION_HARDWARE_LOGGER_ENABLED: set_fw_logger_option(values[i]); break;
00072
00073
00074 default: base_opt.push_back(options[i]); base_opt_val.push_back(values[i]); break;
00075 }
00076 }
00077
00078
00079 if (base_opt.size())
00080 ds_device::set_options(base_opt.data(), base_opt.size(), base_opt_val.data());
00081 }
00082
00083 void zr300_camera::get_options(const rs_option options[], size_t count, double values[])
00084 {
00085 std::vector<rs_option> base_opt;
00086 std::vector<size_t> base_opt_index;
00087 std::vector<double> base_opt_val;
00088
00089 auto & dev = get_device();
00090
00091
00092 for (size_t i = 0; i<count; ++i)
00093 {
00094 if (is_fisheye_uvc_control(options[i]))
00095 {
00096 values[i] = uvc::get_pu_control(dev, 3, options[i]);
00097 continue;
00098 }
00099
00100 switch(options[i])
00101 {
00102
00103 case RS_OPTION_FISHEYE_STROBE: values[i] = zr300::get_fisheye_strobe (dev); break;
00104 case RS_OPTION_FISHEYE_EXTERNAL_TRIGGER: values[i] = zr300::get_fisheye_external_trigger (dev); break;
00105 case RS_OPTION_FISHEYE_EXPOSURE: values[i] = zr300::get_fisheye_exposure (dev); break;
00106 case RS_OPTION_FISHEYE_ENABLE_AUTO_EXPOSURE: values[i] = get_auto_exposure_state(RS_OPTION_FISHEYE_ENABLE_AUTO_EXPOSURE); break;
00107 case RS_OPTION_FISHEYE_AUTO_EXPOSURE_MODE: values[i] = get_auto_exposure_state(RS_OPTION_FISHEYE_AUTO_EXPOSURE_MODE); break;
00108 case RS_OPTION_FISHEYE_AUTO_EXPOSURE_ANTIFLICKER_RATE: values[i] = get_auto_exposure_state(RS_OPTION_FISHEYE_AUTO_EXPOSURE_ANTIFLICKER_RATE); break;
00109 case RS_OPTION_FISHEYE_AUTO_EXPOSURE_PIXEL_SAMPLE_RATE: values[i] = get_auto_exposure_state(RS_OPTION_FISHEYE_AUTO_EXPOSURE_PIXEL_SAMPLE_RATE); break;
00110 case RS_OPTION_FISHEYE_AUTO_EXPOSURE_SKIP_FRAMES: values[i] = get_auto_exposure_state(RS_OPTION_FISHEYE_AUTO_EXPOSURE_SKIP_FRAMES); break;
00111 case RS_OPTION_HARDWARE_LOGGER_ENABLED: values[i] = get_fw_logger_option(); break;
00112
00113
00114 default: base_opt.push_back(options[i]); base_opt_index.push_back(i); break;
00115 }
00116 }
00117
00118
00119 if (base_opt.size())
00120 {
00121 base_opt_val.resize(base_opt.size());
00122 ds_device::get_options(base_opt.data(), base_opt.size(), base_opt_val.data());
00123 }
00124
00125
00126 for (auto i : base_opt_index)
00127 values[i] = base_opt_val[i];
00128 }
00129
00130 void zr300_camera::send_blob_to_device(rs_blob_type type, void * data, int size)
00131 {
00132 switch(type)
00133 {
00134 case RS_BLOB_TYPE_MOTION_MODULE_FIRMWARE_UPDATE:
00135 motion_module_ctrl.firmware_upgrade(data, size);
00136 break;
00137 default: rs_device_base::send_blob_to_device(type, data, size);
00138 }
00139 }
00140
00141 unsigned zr300_camera::get_auto_exposure_state(rs_option option)
00142 {
00143 return auto_exposure_state.get_auto_exposure_state(option);
00144 }
00145
00146 void zr300_camera::on_before_callback(rs_stream stream, rs_frame_ref * frame, std::shared_ptr<rsimpl::frame_archive> archive)
00147 {
00148 if (!to_add_frames || stream != RS_STREAM_FISHEYE)
00149 return;
00150
00151 auto_exposure->add_frame(clone_frame(frame), archive);
00152 }
00153
00154 void zr300_camera::set_fw_logger_option(double value)
00155 {
00156 if (value >= 1)
00157 {
00158 if (!rs_device_base::keep_fw_logger_alive)
00159 start_fw_logger(char(adaptor_board_command::GLD), 100, usbMutex);
00160 }
00161 else
00162 {
00163 if (rs_device_base::keep_fw_logger_alive)
00164 stop_fw_logger();
00165 }
00166 }
00167
00168 unsigned zr300_camera::get_fw_logger_option()
00169 {
00170 return rs_device_base::keep_fw_logger_alive;
00171 }
00172
00173 void zr300_camera::set_auto_exposure_state(rs_option option, double value)
00174 {
00175 auto auto_exposure_prev_state = auto_exposure_state.get_auto_exposure_state(RS_OPTION_FISHEYE_ENABLE_AUTO_EXPOSURE);
00176 auto_exposure_state.set_auto_exposure_state(option, value);
00177
00178 if (auto_exposure_state.get_auto_exposure_state(RS_OPTION_FISHEYE_ENABLE_AUTO_EXPOSURE))
00179 {
00180 if (auto_exposure_prev_state)
00181 {
00182 if (auto_exposure)
00183 auto_exposure->update_auto_exposure_state(auto_exposure_state);
00184 }
00185 else
00186 {
00187 to_add_frames = true;
00188 }
00189 }
00190 else
00191 {
00192 if (auto_exposure_prev_state)
00193 {
00194 to_add_frames = false;
00195 }
00196 }
00197 }
00198
00199 void zr300_camera::toggle_motion_module_power(bool on)
00200 {
00201 motion_module_ctrl.toggle_motion_module_power(on);
00202 }
00203
00204 void zr300_camera::toggle_motion_module_events(bool on)
00205 {
00206 motion_module_ctrl.toggle_motion_module_events(on);
00207 motion_module_ready = on;
00208 }
00209
00210
00211 void zr300_camera::start(rs_source source)
00212 {
00213 if ((supports(RS_CAPABILITIES_FISH_EYE)) && ((config.requests[RS_STREAM_FISHEYE].enabled)))
00214 toggle_motion_module_power(true);
00215
00216 if (supports(RS_CAPABILITIES_FISH_EYE))
00217 auto_exposure = std::make_shared<auto_exposure_mechanism>(this, auto_exposure_state);
00218
00219 ds_device::start(source);
00220 }
00221
00222
00223 void zr300_camera::stop(rs_source source)
00224 {
00225 if ((supports(RS_CAPABILITIES_FISH_EYE)) && ((config.requests[RS_STREAM_FISHEYE].enabled)))
00226 toggle_motion_module_power(false);
00227
00228 ds_device::stop(source);
00229 if (supports(RS_CAPABILITIES_FISH_EYE))
00230 auto_exposure.reset();
00231 }
00232
00233
00234 void zr300_camera::start_motion_tracking()
00235 {
00236 rs_device_base::start_motion_tracking();
00237 if (supports(RS_CAPABILITIES_MOTION_EVENTS))
00238 toggle_motion_module_events(true);
00239 }
00240
00241
00242 void zr300_camera::stop_motion_tracking()
00243 {
00244 if (supports(RS_CAPABILITIES_MOTION_EVENTS))
00245 toggle_motion_module_events(false);
00246 rs_device_base::stop_motion_tracking();
00247 }
00248
00249 rs_stream zr300_camera::select_key_stream(const std::vector<rsimpl::subdevice_mode_selection> & selected_modes)
00250 {
00251
00252
00253
00254 int fps[RS_STREAM_NATIVE_COUNT] = {}, max_fps = 0;
00255 for(const auto & m : selected_modes)
00256 {
00257 for(const auto & output : m.get_outputs())
00258 {
00259 fps[output.first] = m.mode.fps;
00260 max_fps = std::max(max_fps, m.mode.fps);
00261 }
00262 }
00263
00264
00265 for(auto s : {RS_STREAM_COLOR, RS_STREAM_INFRARED2, RS_STREAM_INFRARED, RS_STREAM_FISHEYE})
00266 {
00267 if(fps[s] == max_fps) return s;
00268 }
00269 return RS_STREAM_DEPTH;
00270 }
00271
00272 rs_motion_intrinsics zr300_camera::get_motion_intrinsics() const
00273 {
00274 if (!validate_motion_intrinsics())
00275 {
00276 throw std::runtime_error("Motion intrinsic is not valid");
00277 }
00278 return fe_intrinsic.calib.imu_intrinsic;
00279 }
00280
00281 bool zr300_camera::supports_option(rs_option option) const
00282 {
00283
00284 std::vector<rs_option> auto_exposure_options = {
00285 RS_OPTION_R200_AUTO_EXPOSURE_KP_EXPOSURE,
00286 RS_OPTION_R200_AUTO_EXPOSURE_KP_GAIN,
00287 RS_OPTION_R200_AUTO_EXPOSURE_KP_DARK_THRESHOLD,
00288 RS_OPTION_R200_AUTO_EXPOSURE_BRIGHT_RATIO_SET_POINT,
00289 };
00290
00291 if (std::find(auto_exposure_options.begin(), auto_exposure_options.end(), option) != auto_exposure_options.end())
00292 {
00293 return false;
00294 }
00295
00296 return ds_device::supports_option(option);
00297 }
00298
00299 rs_extrinsics zr300_camera::get_motion_extrinsics_from(rs_stream from) const
00300 {
00301 if (!validate_motion_extrinsics(from))
00302 {
00303 throw std::runtime_error("Motion intrinsic is not valid");
00304 }
00305 switch (from)
00306 {
00307 case RS_STREAM_DEPTH:
00308 return fe_intrinsic.calib.mm_extrinsic.depth_to_imu;
00309 case RS_STREAM_COLOR:
00310 return fe_intrinsic.calib.mm_extrinsic.rgb_to_imu;
00311 case RS_STREAM_FISHEYE:
00312 return fe_intrinsic.calib.mm_extrinsic.fe_to_imu;
00313 default:
00314 throw std::runtime_error(to_string() << "No motion extrinsics from "<<from );
00315 }
00316 }
00317
00318 void zr300_camera::get_option_range(rs_option option, double & min, double & max, double & step, double & def)
00319 {
00320 if (is_fisheye_uvc_control(option))
00321 {
00322 int mn, mx, stp, df;
00323 uvc::get_pu_control_range(get_device(), 3, option, &mn, &mx, &stp, &df);
00324 min = mn;
00325 max = mx;
00326 step = stp;
00327 def = df;
00328 }
00329 else
00330 {
00331
00332 ds_device::get_option_range(option, min, max, step, def);
00333 }
00334 }
00335
00336 unsigned long long zr300_camera::get_frame_counter_by_usb_cmd()
00337 {
00338 hwmon_cmd cmd((int)adaptor_board_command::FRCNT);
00339 perform_and_send_monitor_command(this->get_device(), usbMutex, cmd);
00340 unsigned long long frame_counter = 0;
00341 memcpy(&frame_counter, cmd.receivedCommandData, cmd.receivedCommandDataLength);
00342 return frame_counter;
00343 }
00344
00345 bool zr300_camera::validate_motion_extrinsics(rs_stream from_stream) const
00346 {
00347 if (fe_intrinsic.calib.mm_extrinsic.ver.size != fe_intrinsic.calib.mm_extrinsic.get_data_size())
00348 {
00349 LOG_WARNING("Motion exntrinsics validation from "<< from_stream <<" failed, ver.size = " << fe_intrinsic.calib.mm_extrinsic.ver.size << " real size = " << fe_intrinsic.calib.mm_extrinsic.get_data_size());
00350 return false;
00351 }
00352
00353 auto res = true;
00354 switch (from_stream)
00355 {
00356 case RS_STREAM_DEPTH:
00357 if (!fe_intrinsic.calib.mm_extrinsic.depth_to_imu.has_data())
00358 res = false;
00359
00360 break;
00361 case RS_STREAM_COLOR:
00362 if (!fe_intrinsic.calib.mm_extrinsic.rgb_to_imu.has_data())
00363 res = false;
00364
00365 break;
00366 case RS_STREAM_FISHEYE:
00367 if (!fe_intrinsic.calib.mm_extrinsic.fe_to_imu.has_data())
00368 res = false;
00369
00370 break;
00371 default:
00372 res = false;
00373 }
00374
00375 if (res == false) LOG_WARNING("Motion exntrinsics validation from " << from_stream << " failed, because the data is invalid");
00376 return res;
00377 }
00378
00379 bool zr300_camera::validate_motion_intrinsics() const
00380 {
00381 if (fe_intrinsic.calib.imu_intrinsic.ver.size != fe_intrinsic.calib.imu_intrinsic.get_data_size())
00382 {
00383 LOG_ERROR("Motion intrinsics validation of failed, ver.size = " << fe_intrinsic.calib.imu_intrinsic.ver.size << " real size = " << fe_intrinsic.calib.imu_intrinsic.get_data_size());
00384 return false;
00385 }
00386 if(!fe_intrinsic.calib.imu_intrinsic.has_data())
00387 {
00388 LOG_ERROR("Motion intrinsics validation of failed, because the data is invalid");
00389 return false;
00390 }
00391
00392 return true;
00393 }
00394
00395 serial_number read_serial_number(uvc::device & device, std::timed_mutex & mutex)
00396 {
00397 uint8_t serial_number_raw[HW_MONITOR_BUFFER_SIZE];
00398 size_t bufferLength = HW_MONITOR_BUFFER_SIZE;
00399 get_raw_data(static_cast<uint8_t>(adaptor_board_command::MM_SNB), device, mutex, serial_number_raw, bufferLength);
00400
00401 serial_number sn;
00402 memcpy(&sn, serial_number_raw, std::min(sizeof(serial_number), bufferLength));
00403 return sn;
00404 }
00405 calibration read_calibration(uvc::device & device, std::timed_mutex & mutex)
00406 {
00407 uint8_t scalibration_raw[HW_MONITOR_BUFFER_SIZE];
00408 size_t bufferLength = HW_MONITOR_BUFFER_SIZE;
00409 get_raw_data(static_cast<uint8_t>(adaptor_board_command::MM_TRB), device, mutex, scalibration_raw, bufferLength);
00410
00411 calibration calibration;
00412 memcpy(&calibration, scalibration_raw, std::min(sizeof(calibration), bufferLength));
00413 return calibration;
00414 }
00415 motion_module_calibration read_fisheye_intrinsic(uvc::device & device, std::timed_mutex & mutex)
00416 {
00417 motion_module_calibration intrinsic;
00418 intrinsic.calib = read_calibration(device, mutex);
00419 intrinsic.sn = read_serial_number(device, mutex);
00420
00421 return intrinsic;
00422 }
00423
00424
00425 std::shared_ptr<rs_device> make_zr300_device(std::shared_ptr<uvc::device> device)
00426 {
00427 LOG_INFO("Connecting to Intel RealSense ZR300");
00428
00429 static_device_info info;
00430 info.name = "Intel RealSense ZR300" ;
00431 auto cam_info = ds::read_camera_info(*device);
00432
00433 motion_module_calibration fisheye_intrinsic;
00434 auto succeeded_to_read_fisheye_intrinsic = false;
00435
00436
00437
00438 if (uvc::is_device_connected(*device, VID_INTEL_CAMERA, FISHEYE_PRODUCT_ID))
00439 {
00440
00441 zr300::claim_motion_module_interface(*device);
00442 std::timed_mutex mtx;
00443 try
00444 {
00445 std::string version_string;
00446 ivcam::get_firmware_version_string(*device, mtx, version_string, (int)adaptor_board_command::GVD);
00447 info.camera_info[RS_CAMERA_INFO_ADAPTER_BOARD_FIRMWARE_VERSION] = version_string;
00448 ivcam::get_firmware_version_string(*device, mtx, version_string, (int)adaptor_board_command::GVD, 4);
00449 info.camera_info[RS_CAMERA_INFO_MOTION_MODULE_FIRMWARE_VERSION] = version_string;
00450 }
00451 catch (...)
00452 {
00453 LOG_ERROR("Failed to get firmware version");
00454 }
00455
00456 try
00457 {
00458 std::timed_mutex mutex;
00459 fisheye_intrinsic = read_fisheye_intrinsic(*device, mutex);
00460 succeeded_to_read_fisheye_intrinsic = true;
00461 }
00462 catch (...)
00463 {
00464 LOG_ERROR("Couldn't query adapter board / motion module FW version!");
00465 }
00466 rs_intrinsics rs_intrinsics = fisheye_intrinsic.calib.fe_intrinsic;
00467
00468 info.capabilities_vector.push_back(RS_CAPABILITIES_MOTION_MODULE_FW_UPDATE);
00469 info.capabilities_vector.push_back(RS_CAPABILITIES_ADAPTER_BOARD);
00470
00471 info.stream_subdevices[RS_STREAM_FISHEYE] = 3;
00472 info.presets[RS_STREAM_FISHEYE][RS_PRESET_BEST_QUALITY] =
00473 info.presets[RS_STREAM_FISHEYE][RS_PRESET_LARGEST_IMAGE] =
00474 info.presets[RS_STREAM_FISHEYE][RS_PRESET_HIGHEST_FRAMERATE] = { true, 640, 480, RS_FORMAT_RAW8, 60 };
00475
00476 for (auto &fps : { 30, 60})
00477 info.subdevice_modes.push_back({ 3, { 640, 480 }, pf_raw8, fps, rs_intrinsics, { }, { 0 } });
00478
00479 if (info.camera_info.find(RS_CAMERA_INFO_ADAPTER_BOARD_FIRMWARE_VERSION) != info.camera_info.end())
00480 {
00481 firmware_version ver(info.camera_info[RS_CAMERA_INFO_ADAPTER_BOARD_FIRMWARE_VERSION]);
00482 if (ver >= firmware_version("1.25.0.0") && ver < firmware_version("1.27.2.90"))
00483 info.options.push_back({ RS_OPTION_FISHEYE_EXPOSURE, 40, 331, 1, 40 });
00484 else if (ver >= firmware_version("1.27.2.90"))
00485 {
00486 info.supported_metadata_vector.push_back(RS_FRAME_METADATA_ACTUAL_EXPOSURE);
00487 info.options.push_back({ RS_OPTION_FISHEYE_EXPOSURE, 2, 320, 1, 4 });
00488 }
00489 }
00490
00491 info.supported_metadata_vector.push_back(RS_FRAME_METADATA_ACTUAL_FPS);
00492
00493 info.options.push_back({ RS_OPTION_FISHEYE_GAIN, 0, 0, 0, 0 });
00494 info.options.push_back({ RS_OPTION_FISHEYE_STROBE, 0, 1, 1, 0 });
00495 info.options.push_back({ RS_OPTION_FISHEYE_EXTERNAL_TRIGGER, 0, 1, 1, 0 });
00496 info.options.push_back({ RS_OPTION_FISHEYE_ENABLE_AUTO_EXPOSURE, 0, 1, 1, 1 });
00497 info.options.push_back({ RS_OPTION_FISHEYE_AUTO_EXPOSURE_MODE, 0, 2, 1, 0 });
00498 info.options.push_back({ RS_OPTION_FISHEYE_AUTO_EXPOSURE_ANTIFLICKER_RATE, 50, 60, 10, 60 });
00499 info.options.push_back({ RS_OPTION_FISHEYE_AUTO_EXPOSURE_PIXEL_SAMPLE_RATE, 1, 3, 1, 1 });
00500 info.options.push_back({ RS_OPTION_FISHEYE_AUTO_EXPOSURE_SKIP_FRAMES, 2, 3, 1, 2 });
00501 info.options.push_back({ RS_OPTION_HARDWARE_LOGGER_ENABLED, 0, 1, 1, 0 });
00502 }
00503
00504 ds_device::set_common_ds_config(device, info, cam_info);
00505 info.subdevice_modes.push_back({ 2, { 1920, 1080 }, pf_rw16, 30, cam_info.calibration.intrinsicsThird[0], { cam_info.calibration.modesThird[0][0] }, { 0 } });
00506
00507 if (succeeded_to_read_fisheye_intrinsic)
00508 {
00509 auto fe_extrinsic = fisheye_intrinsic.calib.mm_extrinsic;
00510 pose fisheye_to_depth = { reinterpret_cast<float3x3 &>(fe_extrinsic.fe_to_depth.rotation), reinterpret_cast<float3&>(fe_extrinsic.fe_to_depth.translation) };
00511 auto depth_to_fisheye = inverse(fisheye_to_depth);
00512 info.stream_poses[RS_STREAM_FISHEYE] = depth_to_fisheye;
00513
00514 info.capabilities_vector.push_back({ RS_CAPABILITIES_FISH_EYE, { 1, 15, 5, 0 }, firmware_version::any(), RS_CAMERA_INFO_MOTION_MODULE_FIRMWARE_VERSION });
00515 info.capabilities_vector.push_back({ RS_CAPABILITIES_MOTION_EVENTS, { 1, 15, 5, 0 }, firmware_version::any(), RS_CAMERA_INFO_MOTION_MODULE_FIRMWARE_VERSION });
00516 }
00517 else
00518 {
00519 LOG_WARNING("Motion module capabilities were disabled due to failure to acquire intrinsic");
00520 }
00521
00522 auto fisheye_intrinsics_validator = [fisheye_intrinsic, succeeded_to_read_fisheye_intrinsic](rs_stream stream)
00523 {
00524 if (stream != RS_STREAM_FISHEYE)
00525 {
00526 return true;
00527 }
00528 if (!succeeded_to_read_fisheye_intrinsic)
00529 {
00530 LOG_WARNING("Intrinsics validation of "<<stream<<" failed, because the reading of calibration table failed");
00531 return false;
00532 }
00533 if (fisheye_intrinsic.calib.fe_intrinsic.ver.size != fisheye_intrinsic.calib.fe_intrinsic.get_data_size())
00534 {
00535 LOG_WARNING("Intrinsics validation of " << stream << " failed, ver.size param. = " << (int)fisheye_intrinsic.calib.fe_intrinsic.ver.size << "; actual size = " << fisheye_intrinsic.calib.fe_intrinsic.get_data_size());
00536 return false;
00537 }
00538 if (!fisheye_intrinsic.calib.fe_intrinsic.has_data())
00539 {
00540 LOG_WARNING("Intrinsics validation of " << stream <<" failed, because the data is invalid");
00541 return false;
00542 }
00543 return true;
00544 };
00545
00546 auto fisheye_extrinsics_validator = [fisheye_intrinsic, succeeded_to_read_fisheye_intrinsic](rs_stream from_stream, rs_stream to_stream)
00547 {
00548 if (from_stream != RS_STREAM_FISHEYE && to_stream != RS_STREAM_FISHEYE)
00549 {
00550 return true;
00551 }
00552 if (!succeeded_to_read_fisheye_intrinsic)
00553 {
00554 LOG_WARNING("Exstrinsics validation of" << from_stream <<" to "<< to_stream << " failed, because the reading of calibration table failed");
00555 return false;
00556 }
00557 if (fisheye_intrinsic.calib.mm_extrinsic.ver.size != fisheye_intrinsic.calib.mm_extrinsic.get_data_size())
00558 {
00559 LOG_WARNING("Extrinsics validation of" << from_stream <<" to "<<to_stream<< " failed, ver.size = " << fisheye_intrinsic.calib.fe_intrinsic.ver.size << " real size = " << fisheye_intrinsic.calib.fe_intrinsic.get_data_size());
00560 return false;
00561 }
00562 if(!fisheye_intrinsic.calib.mm_extrinsic.fe_to_depth.has_data())
00563 {
00564 LOG_WARNING("Extrinsics validation of" << from_stream <<" to "<<to_stream<< " failed, because the data is invalid");
00565 return false;
00566 }
00567
00568 return true;
00569 };
00570
00571 return std::make_shared<zr300_camera>(device, info, fisheye_intrinsic, calibration_validator(fisheye_extrinsics_validator, fisheye_intrinsics_validator));
00572 }
00573
00574 unsigned fisheye_auto_exposure_state::get_auto_exposure_state(rs_option option) const
00575 {
00576 switch (option)
00577 {
00578 case RS_OPTION_FISHEYE_ENABLE_AUTO_EXPOSURE:
00579 return (static_cast<unsigned>(is_auto_exposure));
00580 break;
00581 case RS_OPTION_FISHEYE_AUTO_EXPOSURE_MODE:
00582 return (static_cast<unsigned>(mode));
00583 break;
00584 case RS_OPTION_FISHEYE_AUTO_EXPOSURE_ANTIFLICKER_RATE:
00585 return (static_cast<unsigned>(rate));
00586 break;
00587 case RS_OPTION_FISHEYE_AUTO_EXPOSURE_PIXEL_SAMPLE_RATE:
00588 return (static_cast<unsigned>(sample_rate));
00589 break;
00590 case RS_OPTION_FISHEYE_AUTO_EXPOSURE_SKIP_FRAMES:
00591 return (static_cast<unsigned>(skip_frames));
00592 break;
00593 default:
00594 throw std::logic_error("Option unsupported");
00595 break;
00596 }
00597 }
00598
00599 void fisheye_auto_exposure_state::set_auto_exposure_state(rs_option option, double value)
00600 {
00601 switch (option)
00602 {
00603 case RS_OPTION_FISHEYE_ENABLE_AUTO_EXPOSURE:
00604 is_auto_exposure = (value >= 1);
00605 break;
00606 case RS_OPTION_FISHEYE_AUTO_EXPOSURE_MODE:
00607 mode = static_cast<auto_exposure_modes>((int)value);
00608 break;
00609 case RS_OPTION_FISHEYE_AUTO_EXPOSURE_ANTIFLICKER_RATE:
00610 rate = static_cast<unsigned>(value);
00611 break;
00612 case RS_OPTION_FISHEYE_AUTO_EXPOSURE_PIXEL_SAMPLE_RATE:
00613 sample_rate = static_cast<unsigned>(value);
00614 break;
00615 case RS_OPTION_FISHEYE_AUTO_EXPOSURE_SKIP_FRAMES:
00616 skip_frames = static_cast<unsigned>(value);
00617 break;
00618 default:
00619 throw std::logic_error("Option unsupported");
00620 break;
00621 }
00622 }
00623
00624 auto_exposure_mechanism::auto_exposure_mechanism(zr300_camera* dev, fisheye_auto_exposure_state auto_exposure_state) : device(dev), auto_exposure_algo(auto_exposure_state), sync_archive(nullptr), keep_alive(true), frames_counter(0), skip_frames(get_skip_frames(auto_exposure_state))
00625 {
00626 exposure_thread = std::make_shared<std::thread>([this]() {
00627 while (keep_alive)
00628 {
00629 std::unique_lock<std::mutex> lk(queue_mtx);
00630 cv.wait(lk, [&] {return (get_queue_size() || !keep_alive); });
00631 if (!keep_alive)
00632 return;
00633
00634
00635 rs_frame_ref* frame_ref = nullptr;
00636 auto frame_sts = try_pop_front_data(&frame_ref);
00637 lk.unlock();
00638
00639 double values[2] = {};
00640 unsigned long long frame_counter;
00641 try {
00642 if (frame_ref->supports_frame_metadata(RS_FRAME_METADATA_ACTUAL_EXPOSURE))
00643 {
00644 double gain[1] = {};
00645 rs_option options[] = { RS_OPTION_FISHEYE_GAIN };
00646 device->get_options(options, 1, gain);
00647 values[0] = frame_ref->get_frame_metadata(RS_FRAME_METADATA_ACTUAL_EXPOSURE);
00648 values[1] = gain[0];
00649 }
00650 else
00651 {
00652 rs_option options[] = { RS_OPTION_FISHEYE_EXPOSURE, RS_OPTION_FISHEYE_GAIN };
00653 device->get_options(options, 2, values);
00654 }
00655
00656 values[0] /= 10.;
00657 frame_counter = device->get_frame_counter_by_usb_cmd();
00658 push_back_exp_and_cnt(exposure_and_frame_counter(values[0], frame_counter));
00659 }
00660 catch (...) {};
00661
00662 if (frame_sts)
00663 {
00664 unsigned long long frame_counter = frame_ref->get_frame_number();
00665 double exp_by_frame_cnt;
00666 auto exp_and_cnt_sts = try_get_exp_by_frame_cnt(exp_by_frame_cnt, frame_counter);
00667
00668 auto exposure_value = static_cast<float>((exp_and_cnt_sts)? exp_by_frame_cnt : values[0]);
00669 auto gain_value = static_cast<float>(2 + (values[1]-15) / 8.);
00670
00671 bool sts = auto_exposure_algo.analyze_image(frame_ref);
00672 if (sts)
00673 {
00674 bool modify_exposure, modify_gain;
00675 auto_exposure_algo.modify_exposure(exposure_value, modify_exposure, gain_value, modify_gain);
00676
00677 if (modify_exposure)
00678 {
00679 rs_option option[] = { RS_OPTION_FISHEYE_EXPOSURE };
00680 double value[] = { exposure_value * 10. };
00681 if (value[0] < 1)
00682 value[0] = 1;
00683
00684 device->set_options(option, 1, value);
00685 }
00686
00687 if (modify_gain)
00688 {
00689 rs_option option[] = { RS_OPTION_FISHEYE_GAIN };
00690 double value[] = { (gain_value-2) * 8 +15. };
00691 device->set_options(option, 1, value);
00692 }
00693 }
00694 }
00695 sync_archive->release_frame_ref((rsimpl::frame_archive::frame_ref *)frame_ref);
00696 }
00697 });
00698 }
00699
00700 auto_exposure_mechanism::~auto_exposure_mechanism()
00701 {
00702 {
00703 std::lock_guard<std::mutex> lk(queue_mtx);
00704 keep_alive = false;
00705 clear_queue();
00706 }
00707 cv.notify_one();
00708 exposure_thread->join();
00709 }
00710
00711 void auto_exposure_mechanism::update_auto_exposure_state(fisheye_auto_exposure_state& auto_exposure_state)
00712 {
00713 std::lock_guard<std::mutex> lk(queue_mtx);
00714 skip_frames = auto_exposure_state.get_auto_exposure_state(RS_OPTION_FISHEYE_AUTO_EXPOSURE_SKIP_FRAMES);
00715 auto_exposure_algo.update_options(auto_exposure_state);
00716 }
00717
00718 void auto_exposure_mechanism::add_frame(rs_frame_ref* frame, std::shared_ptr<rsimpl::frame_archive> archive)
00719 {
00720 if (!keep_alive || (skip_frames && (frames_counter++) != skip_frames))
00721 {
00722 archive->release_frame_ref((rsimpl::frame_archive::frame_ref *)frame);
00723 return;
00724 }
00725
00726 frames_counter = 0;
00727
00728 if (!sync_archive)
00729 sync_archive = archive;
00730
00731 {
00732 std::lock_guard<std::mutex> lk(queue_mtx);
00733 if (data_queue.size() > 1)
00734 {
00735 sync_archive->release_frame_ref((rsimpl::frame_archive::frame_ref *)data_queue.front());
00736 data_queue.pop_front();
00737 }
00738
00739 push_back_data(frame);
00740 }
00741 cv.notify_one();
00742 }
00743
00744 void auto_exposure_mechanism::push_back_exp_and_cnt(exposure_and_frame_counter exp_and_cnt)
00745 {
00746 std::lock_guard<std::mutex> lk(exp_and_cnt_queue_mtx);
00747
00748 if (exposure_and_frame_counter_queue.size() > max_size_of_exp_and_cnt_queue)
00749 exposure_and_frame_counter_queue.pop_front();
00750
00751 exposure_and_frame_counter_queue.push_back(exp_and_cnt);
00752 }
00753
00754 bool auto_exposure_mechanism::try_get_exp_by_frame_cnt(double& exposure, const unsigned long long frame_counter)
00755 {
00756 std::lock_guard<std::mutex> lk(exp_and_cnt_queue_mtx);
00757
00758 if (!exposure_and_frame_counter_queue.size())
00759 return false;
00760
00761 unsigned long long min = std::numeric_limits<uint64_t>::max();
00762 double exp;
00763 auto it = std::find_if(exposure_and_frame_counter_queue.begin(), exposure_and_frame_counter_queue.end(),
00764 [&](const exposure_and_frame_counter& element) {
00765 unsigned int diff = std::abs(static_cast<int>(frame_counter - element.frame_counter));
00766 if (diff < min)
00767 {
00768 min = diff;
00769 exp = element.exposure;
00770 return false;
00771 }
00772 return true;
00773 });
00774
00775 if (it != exposure_and_frame_counter_queue.end())
00776 {
00777 exposure = it->exposure;
00778 exposure_and_frame_counter_queue.erase(it);
00779 return true;
00780 }
00781
00782 return false;
00783 }
00784
00785 void auto_exposure_mechanism::push_back_data(rs_frame_ref* data)
00786 {
00787 data_queue.push_back(data);
00788 }
00789
00790 bool auto_exposure_mechanism::try_pop_front_data(rs_frame_ref** data)
00791 {
00792 if (!data_queue.size())
00793 return false;
00794
00795 *data = data_queue.front();
00796 data_queue.pop_front();
00797
00798 return true;
00799 }
00800
00801 size_t auto_exposure_mechanism::get_queue_size()
00802 {
00803 return data_queue.size();
00804 }
00805
00806 void auto_exposure_mechanism::clear_queue()
00807 {
00808 rs_frame_ref* frame_ref = nullptr;
00809 while (try_pop_front_data(&frame_ref))
00810 {
00811 sync_archive->release_frame_ref((rsimpl::frame_archive::frame_ref *)frame_ref);
00812 }
00813 }
00814
00815 auto_exposure_algorithm::auto_exposure_algorithm(fisheye_auto_exposure_state auto_exposure_state)
00816 {
00817 update_options(auto_exposure_state);
00818 }
00819
00820 void auto_exposure_algorithm::modify_exposure(float& exposure_value, bool& exp_modified, float& gain_value, bool& gain_modified)
00821 {
00822 float total_exposure = exposure * gain;
00823 LOG_DEBUG("TotalExposure " << total_exposure << ", target_exposure " << target_exposure);
00824 if (fabs(target_exposure - total_exposure) > eps)
00825 {
00826 rounding_mode_type RoundingMode;
00827
00828 if (target_exposure > total_exposure)
00829 {
00830 float target_exposure0 = total_exposure * (1.0f + exposure_step);
00831
00832 target_exposure0 = std::min(target_exposure0, target_exposure);
00833 increase_exposure_gain(target_exposure0, target_exposure0, exposure, gain);
00834 RoundingMode = rounding_mode_type::ceil;
00835 LOG_DEBUG(" ModifyExposure: IncreaseExposureGain: ");
00836 LOG_DEBUG(" target_exposure0 " << target_exposure0);
00837 }
00838 else
00839 {
00840 float target_exposure0 = total_exposure / (1.0f + exposure_step);
00841
00842 target_exposure0 = std::max(target_exposure0, target_exposure);
00843 decrease_exposure_gain(target_exposure0, target_exposure0, exposure, gain);
00844 RoundingMode = rounding_mode_type::floor;
00845 LOG_DEBUG(" ModifyExposure: DecreaseExposureGain: ");
00846 LOG_DEBUG(" target_exposure0 " << target_exposure0);
00847 }
00848 LOG_DEBUG(" exposure " << exposure << ", gain " << gain);
00849 if (exposure_value != exposure)
00850 {
00851 exp_modified = true;
00852 exposure_value = exposure;
00853 exposure_value = exposure_to_value(exposure_value, RoundingMode);
00854 LOG_DEBUG("output exposure by algo = " << exposure_value);
00855 }
00856 if (gain_value != gain)
00857 {
00858 gain_modified = true;
00859 gain_value = gain;
00860 LOG_DEBUG("GainModified: gain = " << gain);
00861 gain_value = gain_to_value(gain_value, RoundingMode);
00862 LOG_DEBUG(" rounded to: " << gain);
00863 }
00864 }
00865 }
00866
00867 bool auto_exposure_algorithm::analyze_image(const rs_frame_ref* image)
00868 {
00869 int cols = image->get_frame_width();
00870 int rows = image->get_frame_height();
00871
00872 const int number_of_pixels = cols * rows;
00873 if (number_of_pixels == 0) return false;
00874
00875 std::vector<int> H(256);
00876 int total_weight = number_of_pixels;
00877
00878 im_hist((uint8_t*)image->get_frame_data(), cols, rows, image->get_frame_bpp() / 8 * cols, &H[0]);
00879
00880 histogram_metric score = {};
00881 histogram_score(H, total_weight, score);
00882
00884 float s1 = (score.main_mean - 128.0f) / 255.0f;
00885 float s2 = 0;
00886
00887 s2 = (score.over_exposure_count - score.under_exposure_count) / (float)total_weight;
00888
00889 float s = -0.3f * (s1 + 5.0f * s2);
00890 LOG_DEBUG(" AnalyzeImage Score: " << s);
00891
00892 if (s > 0)
00893 {
00894 direction = +1;
00895 increase_exposure_target(s, target_exposure);
00896 }
00897 else
00898 {
00899 LOG_DEBUG(" AnalyzeImage: DecreaseExposure");
00900 direction = -1;
00901 decrease_exposure_target(s, target_exposure);
00902 }
00903
00904 if (fabs(1.0f - (exposure * gain) / target_exposure) < hysteresis)
00905 {
00906 LOG_DEBUG(" AnalyzeImage: Don't Modify (Hysteresis): " << target_exposure << " " << exposure * gain);
00907 return false;
00908 }
00909
00910 prev_direction = direction;
00911 LOG_DEBUG(" AnalyzeImage: Modify");
00912 return true;
00913 }
00914
00915 void auto_exposure_algorithm::update_options(const fisheye_auto_exposure_state& options)
00916 {
00917 std::lock_guard<std::recursive_mutex> lock(state_mutex);
00918
00919 state = options;
00920 flicker_cycle = 1000.0f / (state.get_auto_exposure_state(RS_OPTION_FISHEYE_AUTO_EXPOSURE_ANTIFLICKER_RATE) * 2.0f);
00921 }
00922
00923 void auto_exposure_algorithm::im_hist(const uint8_t* data, const int width, const int height, const int rowStep, int h[])
00924 {
00925 std::lock_guard<std::recursive_mutex> lock(state_mutex);
00926
00927 for (int i = 0; i < 256; ++i) h[i] = 0;
00928 const uint8_t* rowData = data;
00929 for (int i = 0; i < height; ++i, rowData += rowStep) for (int j = 0; j < width; j+=state.get_auto_exposure_state(RS_OPTION_FISHEYE_AUTO_EXPOSURE_PIXEL_SAMPLE_RATE)) ++h[rowData[j]];
00930 }
00931
00932 void auto_exposure_algorithm::increase_exposure_target(float mult, float& target_exposure)
00933 {
00934 target_exposure = std::min((exposure * gain) * (1.0f + mult), maximal_exposure * gain_limit);
00935 }
00937 void auto_exposure_algorithm::decrease_exposure_target(float mult, float& target_exposure)
00938 {
00939 target_exposure = std::max((exposure * gain) * (1.0f + mult), minimal_exposure * base_gain);
00940 }
00941
00942 void auto_exposure_algorithm::increase_exposure_gain(const float& target_exposure, const float& target_exposure0, float& exposure, float& gain)
00943 {
00944 std::lock_guard<std::recursive_mutex> lock(state_mutex);
00945
00946 switch (state.get_auto_exposure_state(RS_OPTION_FISHEYE_AUTO_EXPOSURE_MODE))
00947 {
00948 case int(auto_exposure_modes::static_auto_exposure): static_increase_exposure_gain(target_exposure, target_exposure0, exposure, gain); break;
00949 case int(auto_exposure_modes::auto_exposure_anti_flicker): anti_flicker_increase_exposure_gain(target_exposure, target_exposure0, exposure, gain); break;
00950 case int(auto_exposure_modes::auto_exposure_hybrid): hybrid_increase_exposure_gain(target_exposure, target_exposure0, exposure, gain); break;
00951 }
00952 }
00953 void auto_exposure_algorithm::decrease_exposure_gain(const float& target_exposure, const float& target_exposure0, float& exposure, float& gain)
00954 {
00955 std::lock_guard<std::recursive_mutex> lock(state_mutex);
00956
00957 switch (state.get_auto_exposure_state(RS_OPTION_FISHEYE_AUTO_EXPOSURE_MODE))
00958 {
00959 case int(auto_exposure_modes::static_auto_exposure): static_decrease_exposure_gain(target_exposure, target_exposure0, exposure, gain); break;
00960 case int(auto_exposure_modes::auto_exposure_anti_flicker): anti_flicker_decrease_exposure_gain(target_exposure, target_exposure0, exposure, gain); break;
00961 case int(auto_exposure_modes::auto_exposure_hybrid): hybrid_decrease_exposure_gain(target_exposure, target_exposure0, exposure, gain); break;
00962 }
00963 }
00964 void auto_exposure_algorithm::static_increase_exposure_gain(const float& , const float& target_exposure0, float& exposure, float& gain)
00965 {
00966 exposure = std::max(minimal_exposure, std::min(target_exposure0 / base_gain, maximal_exposure));
00967 gain = std::min(gain_limit, std::max(target_exposure0 / exposure, base_gain));
00968 }
00969 void auto_exposure_algorithm::static_decrease_exposure_gain(const float& , const float& target_exposure0, float& exposure, float& gain)
00970 {
00971 exposure = std::max(minimal_exposure, std::min(target_exposure0 / base_gain, maximal_exposure));
00972 gain = std::min(gain_limit, std::max(target_exposure0 / exposure, base_gain));
00973 }
00974 void auto_exposure_algorithm::anti_flicker_increase_exposure_gain(const float& target_exposure, const float& , float& exposure, float& gain)
00975 {
00976 std::vector< std::tuple<float, float, float> > exposure_gain_score;
00977
00978 for (int i = 1; i < 4; ++i)
00979 {
00980 if (i * flicker_cycle >= maximal_exposure)
00981 {
00982 continue;
00983 }
00984 float exposure1 = std::max(std::min(i * flicker_cycle, maximal_exposure), flicker_cycle);
00985 float gain1 = base_gain;
00986
00987 if ((exposure1 * gain1) != target_exposure)
00988 {
00989 gain1 = std::min(std::max(target_exposure / exposure1, base_gain), gain_limit);
00990 }
00991 float score1 = fabs(target_exposure - exposure1 * gain1);
00992 exposure_gain_score.push_back(std::tuple<float, float, float>(score1, exposure1, gain1));
00993 }
00994
00995 std::sort(exposure_gain_score.begin(), exposure_gain_score.end());
00996
00997 exposure = std::get<1>(exposure_gain_score.front());
00998 gain = std::get<2>(exposure_gain_score.front());
00999 }
01000 void auto_exposure_algorithm::anti_flicker_decrease_exposure_gain(const float& target_exposure, const float& , float& exposure, float& gain)
01001 {
01002 std::vector< std::tuple<float, float, float> > exposure_gain_score;
01003
01004 for (int i = 1; i < 4; ++i)
01005 {
01006 if (i * flicker_cycle >= maximal_exposure)
01007 {
01008 continue;
01009 }
01010 float exposure1 = std::max(std::min(i * flicker_cycle, maximal_exposure), flicker_cycle);
01011 float gain1 = base_gain;
01012 if ((exposure1 * gain1) != target_exposure)
01013 {
01014 gain1 = std::min(std::max(target_exposure / exposure1, base_gain), gain_limit);
01015 }
01016 float score1 = fabs(target_exposure - exposure1 * gain1);
01017 exposure_gain_score.push_back(std::tuple<float, float, float>(score1, exposure1, gain1));
01018 }
01019
01020 std::sort(exposure_gain_score.begin(), exposure_gain_score.end());
01021
01022 exposure = std::get<1>(exposure_gain_score.front());
01023 gain = std::get<2>(exposure_gain_score.front());
01024 }
01025 void auto_exposure_algorithm::hybrid_increase_exposure_gain(const float& target_exposure, const float& target_exposure0, float& exposure, float& gain)
01026 {
01027 if (anti_flicker_mode)
01028 {
01029 anti_flicker_increase_exposure_gain(target_exposure, target_exposure0, exposure, gain);
01030 }
01031 else
01032 {
01033 static_increase_exposure_gain(target_exposure, target_exposure0, exposure, gain);
01034 LOG_DEBUG("HybridAutoExposure::IncreaseExposureGain: " << exposure * gain << " " << flicker_cycle * base_gain << " " << base_gain);
01035 if (target_exposure > 0.99 * flicker_cycle * base_gain)
01036 {
01037 anti_flicker_mode = true;
01038 anti_flicker_increase_exposure_gain(target_exposure, target_exposure0, exposure, gain);
01039 LOG_DEBUG("anti_flicker_mode = true");
01040 }
01041 }
01042 }
01043 void auto_exposure_algorithm::hybrid_decrease_exposure_gain(const float& target_exposure, const float& target_exposure0, float& exposure, float& gain)
01044 {
01045 if (anti_flicker_mode)
01046 {
01047 LOG_DEBUG("HybridAutoExposure::DecreaseExposureGain: " << exposure << " " << flicker_cycle << " " << gain << " " << base_gain);
01048 if ((target_exposure) <= 0.99 * (flicker_cycle * base_gain))
01049 {
01050 anti_flicker_mode = false;
01051 static_decrease_exposure_gain(target_exposure, target_exposure0, exposure, gain);
01052 LOG_DEBUG("anti_flicker_mode = false");
01053 }
01054 else
01055 {
01056 anti_flicker_decrease_exposure_gain(target_exposure, target_exposure0, exposure, gain);
01057 }
01058 }
01059 else
01060 {
01061 static_decrease_exposure_gain(target_exposure, target_exposure0, exposure, gain);
01062 }
01063 }
01064
01065 float auto_exposure_algorithm::exposure_to_value(float exp_ms, rounding_mode_type rounding_mode)
01066 {
01067 const float line_period_us = 19.33333333f;
01068
01069 float ExposureTimeLine = (exp_ms * 1000.0f / line_period_us);
01070 if (rounding_mode == rounding_mode_type::ceil) ExposureTimeLine = std::ceil(ExposureTimeLine);
01071 else if (rounding_mode == rounding_mode_type::floor) ExposureTimeLine = std::floor(ExposureTimeLine);
01072 else ExposureTimeLine = round(ExposureTimeLine);
01073 return ((float)ExposureTimeLine * line_period_us) / 1000.0f;
01074 }
01075
01076 float auto_exposure_algorithm::gain_to_value(float gain, rounding_mode_type rounding_mode)
01077 {
01078
01079 if (gain < base_gain) { return base_gain; }
01080 else if (gain > 16.0f) { return 16.0f; }
01081 else {
01082 if (rounding_mode == rounding_mode_type::ceil) return std::ceil(gain * 8.0f) / 8.0f;
01083 else if (rounding_mode == rounding_mode_type::floor) return std::floor(gain * 8.0f) / 8.0f;
01084 else return round(gain * 8.0f) / 8.0f;
01085 }
01086 }
01087
01088 template <typename T> inline T sqr(const T& x) { return (x*x); }
01089 void auto_exposure_algorithm::histogram_score(std::vector<int>& h, const int total_weight, histogram_metric& score)
01090 {
01091 score.under_exposure_count = 0;
01092 score.over_exposure_count = 0;
01093
01094 for (size_t i = 0; i <= under_exposure_limit; ++i)
01095 {
01096 score.under_exposure_count += h[i];
01097 }
01098 score.shadow_limit = 0;
01099
01100 {
01101 score.shadow_limit = under_exposure_limit;
01102 for (size_t i = under_exposure_limit + 1; i <= over_exposure_limit; ++i)
01103 {
01104 if (h[i] > under_exposure_noise_limit)
01105 {
01106 break;
01107 }
01108 score.shadow_limit++;
01109 }
01110 int lower_q = 0;
01111 score.lower_q = 0;
01112 for (size_t i = under_exposure_limit + 1; i <= over_exposure_limit; ++i)
01113 {
01114 lower_q += h[i];
01115 if (lower_q > total_weight / 4)
01116 {
01117 break;
01118 }
01119 score.lower_q++;
01120 }
01121 }
01122
01123 for (size_t i = over_exposure_limit; i <= 255; ++i)
01124 {
01125 score.over_exposure_count += h[i];
01126 }
01127
01128 score.highlight_limit = 255;
01129
01130 {
01131 score.highlight_limit = over_exposure_limit;
01132 for (size_t i = over_exposure_limit; i >= under_exposure_limit; --i)
01133 {
01134 if (h[i] > over_exposure_noise_limit)
01135 {
01136 break;
01137 }
01138 score.highlight_limit--;
01139 }
01140 int upper_q = 0;
01141 score.upper_q = over_exposure_limit;
01142 for (size_t i = over_exposure_limit; i >= under_exposure_limit; --i)
01143 {
01144 upper_q += h[i];
01145 if (upper_q > total_weight / 4)
01146 {
01147 break;
01148 }
01149 score.upper_q--;
01150 }
01151
01152 }
01153 int32_t m1 = 0;
01154 int64_t m2 = 0;
01155
01156 double nn = (double)total_weight - score.under_exposure_count - score.over_exposure_count;
01157 if (nn == 0)
01158 {
01159 nn = (double)total_weight;
01160 for (int i = 0; i <= 255; ++i)
01161 {
01162 m1 += h[i] * i;
01163 m2 += h[i] * sqr(i);
01164 }
01165 }
01166 else
01167 {
01168 for (int i = under_exposure_limit + 1; i < over_exposure_limit; ++i)
01169 {
01170 m1 += h[i] * i;
01171 m2 += h[i] * sqr(i);
01172 }
01173 }
01174 score.main_mean = (float)((double)m1 / nn);
01175 double Var = (double)m2 / nn - sqr((double)m1 / nn);
01176 if (Var > 0)
01177 {
01178 score.main_std = (float)sqrt(Var);
01179 }
01180 else
01181 {
01182 score.main_std = 0.0f;
01183 }
01184 }
01185 }