22 motion_module_ctrl(device.get(), usbMutex),
23 auto_exposure(nullptr),
25 fe_intrinsic(in_fe_intrinsic)
47 std::vector<rs_option> base_opt;
48 std::vector<double> base_opt_val;
53 for (
size_t i = 0; i <
count; ++i)
74 default: base_opt.push_back(options[i]); base_opt_val.push_back(values[i]);
break;
80 ds_device::set_options(base_opt.data(), base_opt.size(), base_opt_val.data());
85 std::vector<rs_option> base_opt;
86 std::vector<size_t> base_opt_index;
87 std::vector<double> base_opt_val;
92 for (
size_t i = 0; i<
count; ++i)
114 default: base_opt.push_back(options[i]); base_opt_index.push_back(i);
break;
121 base_opt_val.resize(base_opt.size());
122 ds_device::get_options(base_opt.data(), base_opt.size(), base_opt_val.data());
126 for (
auto i : base_opt_index)
127 values[i] = base_opt_val[i];
180 if (auto_exposure_prev_state)
192 if (auto_exposure_prev_state)
228 ds_device::stop(source);
255 for(
const auto &
m : selected_modes)
257 for(
const auto & output :
m.get_outputs())
259 fps[output.first] =
m.mode.fps;
260 max_fps = std::max(max_fps,
m.mode.fps);
267 if(fps[
s] == max_fps)
return s;
276 throw std::runtime_error(
"Motion intrinsic is not valid");
284 std::vector<rs_option> auto_exposure_options = {
291 if (std::find(auto_exposure_options.begin(), auto_exposure_options.end(),
option) != auto_exposure_options.end())
296 return ds_device::supports_option(option);
303 throw std::runtime_error(
"Motion intrinsic is not valid");
314 throw std::runtime_error(
to_string() <<
"No motion extrinsics from "<<from );
332 ds_device::get_option_range(option, min, max, step, def);
338 hwmon_cmd cmd((
int)adaptor_board_command::FRCNT);
340 unsigned long long frame_counter = 0;
342 return frame_counter;
375 if (
res ==
false)
LOG_WARNING(
"Motion exntrinsics validation from " << from_stream <<
" failed, because the data is invalid");
388 LOG_ERROR(
"Motion intrinsics validation of failed, because the data is invalid");
399 get_raw_data(static_cast<uint8_t>(adaptor_board_command::MM_SNB), device, mutex, serial_number_raw, bufferLength);
402 memcpy(&sn, serial_number_raw, std::min(
sizeof(
serial_number), bufferLength));
409 get_raw_data(static_cast<uint8_t>(adaptor_board_command::MM_TRB), device, mutex, scalibration_raw, bufferLength);
412 memcpy(&calibration, scalibration_raw, std::min(
sizeof(calibration), bufferLength));
427 LOG_INFO(
"Connecting to Intel RealSense ZR300");
430 info.
name =
"Intel RealSense ZR300" ;
434 auto succeeded_to_read_fisheye_intrinsic =
false;
442 std::timed_mutex
mtx;
453 LOG_ERROR(
"Failed to get firmware version");
458 std::timed_mutex mutex;
460 succeeded_to_read_fisheye_intrinsic =
true;
464 LOG_ERROR(
"Couldn't query adapter board / motion module FW version!");
476 for (
auto &fps : { 30, 60})
504 ds_device::set_common_ds_config(
device, info, cam_info);
505 info.subdevice_modes.push_back({ 2, { 1920, 1080 },
pf_rw16, 30, cam_info.calibration.intrinsicsThird[0], { cam_info.calibration.modesThird[0][0] }, { 0 } });
507 if (succeeded_to_read_fisheye_intrinsic)
510 pose fisheye_to_depth = {
reinterpret_cast<float3x3 &
>(fe_extrinsic.fe_to_depth.rotation), reinterpret_cast<float3&>(fe_extrinsic.fe_to_depth.translation) };
511 auto depth_to_fisheye =
inverse(fisheye_to_depth);
519 LOG_WARNING(
"Motion module capabilities were disabled due to failure to acquire intrinsic");
528 if (!succeeded_to_read_fisheye_intrinsic)
530 LOG_WARNING(
"Intrinsics validation of "<<
stream<<
" failed, because the reading of calibration table failed");
540 LOG_WARNING(
"Intrinsics validation of " <<
stream <<
" failed, because the data is invalid");
552 if (!succeeded_to_read_fisheye_intrinsic)
554 LOG_WARNING(
"Exstrinsics validation of" << from_stream <<
" to "<< to_stream <<
" failed, because the reading of calibration table failed");
564 LOG_WARNING(
"Extrinsics validation of" << from_stream <<
" to "<<to_stream<<
" failed, because the data is invalid");
579 return (static_cast<unsigned>(is_auto_exposure));
582 return (static_cast<unsigned>(
mode));
585 return (static_cast<unsigned>(rate));
588 return (static_cast<unsigned>(sample_rate));
591 return (static_cast<unsigned>(skip_frames));
594 throw std::logic_error(
"Option unsupported");
604 is_auto_exposure = (value >= 1);
610 rate =
static_cast<unsigned>(
value);
613 sample_rate =
static_cast<unsigned>(
value);
616 skip_frames =
static_cast<unsigned>(
value);
619 throw std::logic_error(
"Option unsupported");
629 std::unique_lock<std::mutex> lk(
queue_mtx);
640 unsigned long long frame_counter;
665 double exp_by_frame_cnt;
668 auto exposure_value =
static_cast<float>((exp_and_cnt_sts)? exp_by_frame_cnt : values[0]);
669 auto gain_value =
static_cast<float>(2 + (values[1]-15) / 8.);
674 bool modify_exposure, modify_gain;
680 double value[] = { exposure_value * 10. };
690 double value[] = { (gain_value-2) * 8 +15. };
703 std::lock_guard<std::mutex> lk(
queue_mtx);
713 std::lock_guard<std::mutex> lk(
queue_mtx);
732 std::lock_guard<std::mutex> lk(
queue_mtx);
761 unsigned long long min = std::numeric_limits<uint64_t>::max();
765 unsigned int diff = std::abs(static_cast<int>(frame_counter - element.frame_counter));
769 exp = element.exposure;
777 exposure = it->exposure;
817 update_options(auto_exposure_state);
822 float total_exposure = exposure * gain;
823 LOG_DEBUG(
"TotalExposure " << total_exposure <<
", target_exposure " << target_exposure);
824 if (fabs(target_exposure - total_exposure) > eps)
828 if (target_exposure > total_exposure)
830 float target_exposure0 = total_exposure * (1.0f + exposure_step);
832 target_exposure0 = std::min(target_exposure0, target_exposure);
833 increase_exposure_gain(target_exposure0, target_exposure0, exposure, gain);
834 RoundingMode = rounding_mode_type::ceil;
835 LOG_DEBUG(
" ModifyExposure: IncreaseExposureGain: ");
836 LOG_DEBUG(
" target_exposure0 " << target_exposure0);
840 float target_exposure0 = total_exposure / (1.0f + exposure_step);
842 target_exposure0 = std::max(target_exposure0, target_exposure);
843 decrease_exposure_gain(target_exposure0, target_exposure0, exposure, gain);
844 RoundingMode = rounding_mode_type::floor;
845 LOG_DEBUG(
" ModifyExposure: DecreaseExposureGain: ");
846 LOG_DEBUG(
" target_exposure0 " << target_exposure0);
848 LOG_DEBUG(
" exposure " << exposure <<
", gain " << gain);
849 if (exposure_value != exposure)
852 exposure_value = exposure;
853 exposure_value = exposure_to_value(exposure_value, RoundingMode);
854 LOG_DEBUG(
"output exposure by algo = " << exposure_value);
856 if (gain_value != gain)
858 gain_modified =
true;
860 LOG_DEBUG(
"GainModified: gain = " << gain);
861 gain_value = gain_to_value(gain_value, RoundingMode);
872 const int number_of_pixels = cols * rows;
873 if (number_of_pixels == 0)
return false;
875 std::vector<int> H(256);
876 int total_weight = number_of_pixels;
881 histogram_score(H, total_weight, score);
884 float s1 = (score.main_mean - 128.0f) / 255.0
f;
887 s2 = (score.over_exposure_count - score.under_exposure_count) / (
float)total_weight;
889 float s = -0.3f * (s1 + 5.0f * s2);
895 increase_exposure_target(s, target_exposure);
899 LOG_DEBUG(
" AnalyzeImage: DecreaseExposure");
901 decrease_exposure_target(s, target_exposure);
904 if (fabs(1.0
f - (exposure * gain) / target_exposure) < hysteresis)
906 LOG_DEBUG(
" AnalyzeImage: Don't Modify (Hysteresis): " << target_exposure <<
" " << exposure * gain);
910 prev_direction = direction;
917 std::lock_guard<std::recursive_mutex> lock(state_mutex);
925 std::lock_guard<std::recursive_mutex> lock(state_mutex);
927 for (
int i = 0; i < 256; ++i) h[i] = 0;
928 const uint8_t* rowData =
data;
934 target_exposure = std::min((exposure * gain) * (1.0
f + mult), maximal_exposure * gain_limit);
939 target_exposure = std::max((exposure * gain) * (1.0
f + mult), minimal_exposure * base_gain);
944 std::lock_guard<std::recursive_mutex> lock(state_mutex);
955 std::lock_guard<std::recursive_mutex> lock(state_mutex);
966 exposure = std::max(minimal_exposure, std::min(target_exposure0 / base_gain, maximal_exposure));
967 gain = std::min(gain_limit, std::max(target_exposure0 / exposure, base_gain));
971 exposure = std::max(minimal_exposure, std::min(target_exposure0 / base_gain, maximal_exposure));
972 gain = std::min(gain_limit, std::max(target_exposure0 / exposure, base_gain));
976 std::vector< std::tuple<float, float, float> > exposure_gain_score;
978 for (
int i = 1; i < 4; ++i)
980 if (i * flicker_cycle >= maximal_exposure)
984 float exposure1 = std::max(std::min(i * flicker_cycle, maximal_exposure), flicker_cycle);
985 float gain1 = base_gain;
987 if ((exposure1 * gain1) != target_exposure)
989 gain1 = std::min(std::max(target_exposure / exposure1, base_gain), gain_limit);
991 float score1 = fabs(target_exposure - exposure1 * gain1);
992 exposure_gain_score.push_back(std::tuple<float, float, float>(score1, exposure1, gain1));
995 std::sort(exposure_gain_score.begin(), exposure_gain_score.end());
997 exposure = std::get<1>(exposure_gain_score.front());
998 gain = std::get<2>(exposure_gain_score.front());
1002 std::vector< std::tuple<float, float, float> > exposure_gain_score;
1004 for (
int i = 1; i < 4; ++i)
1006 if (i * flicker_cycle >= maximal_exposure)
1010 float exposure1 = std::max(std::min(i * flicker_cycle, maximal_exposure), flicker_cycle);
1011 float gain1 = base_gain;
1012 if ((exposure1 * gain1) != target_exposure)
1014 gain1 = std::min(std::max(target_exposure / exposure1, base_gain), gain_limit);
1016 float score1 = fabs(target_exposure - exposure1 * gain1);
1017 exposure_gain_score.push_back(std::tuple<float, float, float>(score1, exposure1, gain1));
1020 std::sort(exposure_gain_score.begin(), exposure_gain_score.end());
1022 exposure = std::get<1>(exposure_gain_score.front());
1023 gain = std::get<2>(exposure_gain_score.front());
1027 if (anti_flicker_mode)
1029 anti_flicker_increase_exposure_gain(target_exposure, target_exposure0, exposure, gain);
1033 static_increase_exposure_gain(target_exposure, target_exposure0, exposure, gain);
1034 LOG_DEBUG(
"HybridAutoExposure::IncreaseExposureGain: " << exposure * gain <<
" " << flicker_cycle * base_gain <<
" " << base_gain);
1035 if (target_exposure > 0.99 * flicker_cycle * base_gain)
1037 anti_flicker_mode =
true;
1038 anti_flicker_increase_exposure_gain(target_exposure, target_exposure0, exposure, gain);
1045 if (anti_flicker_mode)
1047 LOG_DEBUG(
"HybridAutoExposure::DecreaseExposureGain: " << exposure <<
" " << flicker_cycle <<
" " << gain <<
" " << base_gain);
1048 if ((target_exposure) <= 0.99 * (flicker_cycle * base_gain))
1050 anti_flicker_mode =
false;
1051 static_decrease_exposure_gain(target_exposure, target_exposure0, exposure, gain);
1056 anti_flicker_decrease_exposure_gain(target_exposure, target_exposure0, exposure, gain);
1061 static_decrease_exposure_gain(target_exposure, target_exposure0, exposure, gain);
1067 const float line_period_us = 19.33333333f;
1069 float ExposureTimeLine = (exp_ms * 1000.0f / line_period_us);
1070 if (rounding_mode == rounding_mode_type::ceil) ExposureTimeLine = std::ceil(ExposureTimeLine);
1071 else if (rounding_mode == rounding_mode_type::floor) ExposureTimeLine = std::floor(ExposureTimeLine);
1072 else ExposureTimeLine = round(ExposureTimeLine);
1073 return ((
float)ExposureTimeLine * line_period_us) / 1000.0f;
1079 if (gain < base_gain) {
return base_gain; }
1080 else if (gain > 16.0
f) {
return 16.0f; }
1082 if (rounding_mode == rounding_mode_type::ceil)
return std::ceil(gain * 8.0
f) / 8.0f;
1083 else if (rounding_mode == rounding_mode_type::floor)
return std::floor(gain * 8.0f) / 8.0f;
1084 else return round(gain * 8.0f) / 8.0f;
1088 template <
typename T>
inline T
sqr(
const T&
x) {
return (x*x); }
1094 for (
size_t i = 0; i <= under_exposure_limit; ++i)
1102 for (
size_t i = under_exposure_limit + 1; i <= over_exposure_limit; ++i)
1104 if (h[i] > under_exposure_noise_limit)
1112 for (
size_t i = under_exposure_limit + 1; i <= over_exposure_limit; ++i)
1115 if (lower_q > total_weight / 4)
1123 for (
size_t i = over_exposure_limit; i <= 255; ++i)
1132 for (
size_t i = over_exposure_limit; i >= under_exposure_limit; --i)
1134 if (h[i] > over_exposure_noise_limit)
1141 score.
upper_q = over_exposure_limit;
1142 for (
size_t i = over_exposure_limit; i >= under_exposure_limit; --i)
1145 if (upper_q > total_weight / 4)
1159 nn = (double)total_weight;
1160 for (
int i = 0; i <= 255; ++i)
1163 m2 += h[i] *
sqr(i);
1168 for (
int i = under_exposure_limit + 1; i < over_exposure_limit; ++i)
1171 m2 += h[i] *
sqr(i);
1174 score.
main_mean = (float)((
double)m1 / nn);
1175 double Var = (double)m2 / nn -
sqr((
double)m1 / nn);
void set_fw_logger_option(double value)
void toggle_motion_module_power(bool bOn)
size_t receivedCommandDataLength
void decrease_exposure_target(float mult, float &target_exposure)
std::shared_ptr< rs_device > make_zr300_device(std::shared_ptr< uvc::device > device)
uint16_t get_fisheye_exposure(const uvc::device &device)
rsimpl::device_config config
~auto_exposure_mechanism()
void anti_flicker_decrease_exposure_gain(const float &target_exposure, const float &target_exposure0, float &exposure, float &gain)
virtual bool supports(rs_capabilities capability) const override
const std::shared_ptr< rsimpl::uvc::device > device
std::deque< rs_frame_ref * > data_queue
std::vector< rs_frame_metadata > supported_metadata_vector
void get_pu_control_range(const device &device, int subdevice, rs_option option, int *min, int *max, int *step, int *def)
virtual void stop_motion_tracking()
void modify_exposure(float &exposure_value, bool &exp_modified, float &gain_value, bool &gain_modified)
const rsimpl::uvc::device & get_device() const
float exposure_to_value(float exp_ms, rounding_mode_type rounding_mode)
int get_data_size() const
virtual int get_frame_bpp() const =0
void toggle_motion_module_power(bool on)
std::mutex exp_and_cnt_queue_mtx
void set_fisheye_external_trigger(uvc::device &device, uint8_t ext_trig)
GLint GLint GLsizei GLsizei height
void increase_exposure_target(float mult, float &target_exposure)
rs_blob_type
Proprietary formats for direct communication with device firmware.
void increase_exposure_gain(const float &target_exposure, const float &target_exposure0, float &exposure, float &gain)
auto_exposure_algorithm(fisheye_auto_exposure_state auto_exposure_state)
void stop_motion_tracking() override
auto_exposure_mechanism(zr300_camera *dev, fisheye_auto_exposure_state auto_exposure_state)
bool is_device_connected(device &device, int vid, int pid)
bool analyze_image(const rs_frame_ref *image)
virtual const uint8_t * get_frame_data() const =0
unsigned long long get_frame_counter_by_usb_cmd()
void get_options(const rs_option options[], size_t count, double values[]) override
GLenum GLenum GLsizei void * image
const uint16_t VID_INTEL_CAMERA
GLsizei const GLchar *const * string
void anti_flicker_increase_exposure_gain(const float &target_exposure, const float &target_exposure0, float &exposure, float &gain)
float gain_to_value(float gain, rounding_mode_type rounding_mode)
virtual void stop_fw_logger() override
bool validate_motion_intrinsics() const
bool try_get_exp_by_frame_cnt(double &exposure, const unsigned long long frame_counter)
uint8_t receivedCommandData[HW_MONITOR_BUFFER_SIZE]
rs_option
Defines general configuration controls.
uint8_t get_fisheye_external_trigger(const uvc::device &device)
GLfloat GLfloat GLfloat GLfloat h
std::condition_variable cv
void set_options(const rs_option options[], size_t count, const double values[]) override
std::vector< subdevice_mode > subdevice_modes
void claim_motion_module_interface(uvc::device &device)
stream_request presets[RS_STREAM_NATIVE_COUNT][RS_PRESET_COUNT]
bool is_fisheye_uvc_control(rs_option option)
void perform_and_send_monitor_command(uvc::device &device, std::timed_mutex &mutex, hwmon_cmd &newCommand)
void set_fisheye_exposure(uvc::device &device, uint16_t exposure)
std::shared_ptr< std::thread > exposure_thread
void update_options(const fisheye_auto_exposure_state &options)
void histogram_score(std::vector< int > &h, const int total_weight, histogram_metric &score)
virtual double get_frame_metadata(rs_frame_metadata frame_metadata) const =0
const uint8_t RS_STREAM_NATIVE_COUNT
void set_auto_exposure_state(rs_option option, double value)
std::vector< supported_capability > capabilities_vector
virtual void send_blob_to_device(rs_blob_type, void *, int)
int stream_subdevices[RS_STREAM_NATIVE_COUNT]
calibration read_calibration(uvc::device &device, std::timed_mutex &mutex)
void update_auto_exposure_state(fisheye_auto_exposure_state &auto_exposure_state)
IMU_intrinsic imu_intrinsic
void push_back_data(rs_frame_ref *data)
serial_number read_serial_number(uvc::device &device, std::timed_mutex &mutex)
GLuint GLuint GLsizei count
std::shared_ptr< rsimpl::frame_archive > sync_archive
std::atomic< unsigned > frames_counter
stream_request requests[RS_STREAM_NATIVE_COUNT]
Motion module intrinsics: includes accelerometer and gyroscope intrinsics structs of type rs_motion_d...
uint8_t get_fisheye_strobe(const uvc::device &device)
rs_stream select_key_stream(const std::vector< rsimpl::subdevice_mode_selection > &selected_modes) override
typedef int(WINAPI *PFNWGLRELEASEPBUFFERDCARBPROC)(HPBUFFERARB hPbuffer
std::deque< exposure_and_frame_counter > exposure_and_frame_counter_queue
GLint GLenum GLsizei GLsizei GLsizei GLint GLsizei const void * data
virtual unsigned long long get_frame_number() const =0
std::atomic< bool > keep_fw_logger_alive
virtual int get_frame_width() const =0
unsigned get_fw_logger_option()
std::map< rs_camera_info, std::string > camera_info
void toggle_motion_module_events(bool on)
auto_exposure_algorithm auto_exposure_algo
void on_before_callback(rs_stream, rs_frame_ref *, std::shared_ptr< rsimpl::frame_archive >) override
bool validate_motion_extrinsics(rs_stream) const
void toggle_motion_module_events(bool bOn)
const std::size_t max_size_of_exp_and_cnt_queue
std::vector< supported_option > options
unsigned get_auto_exposure_state(rs_option option) const
IMU_extrinsic mm_extrinsic
std::shared_ptr< rsimpl::syncronizing_archive > archive
GLsizei const GLfloat * value
void send_blob_to_device(rs_blob_type type, void *data, int size) override
ds_info read_camera_info(uvc::device &device)
void stop(rs_source source) override
void hybrid_increase_exposure_gain(const float &target_exposure, const float &target_exposure0, float &exposure, float &gain)
const uint16_t HW_MONITOR_BUFFER_SIZE
rs_source
Source: allows you to choose between available hardware subdevices.
int get_pu_control(const device &device, int subdevice, rs_option option)
motion_module::motion_module_control motion_module_ctrl
Cross-stream extrinsics: encode the topology describing how the different devices are connected...
const native_pixel_format pf_rw16
void get_firmware_version_string(uvc::device &device, std::timed_mutex &mutex, std::string &version, int gvd_cmd, int offset)
#define FISHEYE_PRODUCT_ID
void set_pu_control_with_retry(device &device, int subdevice, rs_option option, int value)
GLenum GLsizei GLsizei GLint * values
bool is_fisheye_xu_control(rs_option option)
pose inverse(const pose &a)
void static_decrease_exposure_gain(const float &target_exposure, const float &target_exposure0, float &exposure, float &gain)
void start_motion_tracking() override
rs_stream
Streams are different types of data provided by RealSense devices.
void get_option_range(rs_option option, double &min, double &max, double &step, double &def) override
void push_back_exp_and_cnt(exposure_and_frame_counter exp_and_cnt)
GLint GLint GLsizei width
void set_auto_exposure_state(rs_option option, double value)
std::atomic< bool > keep_alive
zr300_camera(std::shared_ptr< uvc::device > device, const static_device_info &info, motion_module_calibration fe_intrinsic, calibration_validator validator)
GLsizei GLsizei GLchar * source
static firmware_version any()
std::atomic< unsigned > skip_frames
std::shared_ptr< auto_exposure_mechanism > auto_exposure
void firmware_upgrade(void *data, int size)
bool try_pop_front_data(rs_frame_ref **data)
void get_raw_data(uint8_t opcode, uvc::device &device, std::timed_mutex &mutex, uint8_t *data, size_t &bytesReturned)
void im_hist(const uint8_t *data, const int width, const int height, const int rowStep, int h[])
fisheye_intrinsic fe_intrinsic
rs_motion_intrinsics get_motion_intrinsics() const override
std::atomic< bool > to_add_frames
void add_frame(rs_frame_ref *frame, std::shared_ptr< rsimpl::frame_archive > archive)
GLuint GLfloat GLfloat GLfloat GLfloat GLfloat GLfloat GLfloat GLfloat s1
void static_increase_exposure_gain(const float &target_exposure, const float &target_exposure0, float &exposure, float &gain)
motion_module_calibration fe_intrinsic
virtual void start_motion_tracking()
int get_data_size() const
void decrease_exposure_gain(const float &target_exposure, const float &target_exposure0, float &exposure, float &gain)
rs_extrinsics get_motion_extrinsics_from(rs_stream from) const override
void set_fisheye_strobe(uvc::device &device, uint8_t strobe)
int get_data_size() const
unsigned get_auto_exposure_state(rs_option option)
fisheye_auto_exposure_state auto_exposure_state
bool supports_option(rs_option option) const override
motion_module_calibration read_fisheye_intrinsic(uvc::device &device, std::timed_mutex &mutex)
GLint GLint GLint GLint GLint x
virtual int get_frame_height() const =0
void start(rs_source source) override
const native_pixel_format pf_raw8
void hybrid_decrease_exposure_gain(const float &target_exposure, const float &target_exposure0, float &exposure, float &gain)
GLuint GLuint GLsizei GLenum type
virtual void start_fw_logger(char fw_log_op_code, int grab_rate_in_ms, std::timed_mutex &mutex) override
virtual bool supports_frame_metadata(rs_frame_metadata frame_metadata) const =0
rs_frame_ref * clone_frame(rs_frame_ref *frame) override
mm_extrinsic depth_to_imu
std::timed_mutex usbMutex