Intel® RealSense™ Cross Platform API
Intel Realsense Cross-platform API
zr300.h
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2016 Intel Corporation. All Rights Reserved.
3 
4 #pragma once
5 #ifndef LIBREALSENSE_ZR300_H
6 #define LIBREALSENSE_ZR300_H
7 
8 #include "motion-module.h"
9 #include "ds-device.h"
10 #include "sync.h"
11 
12 #include <deque>
13 #include <cmath>
14 #include <math.h>
15 
16 namespace rsimpl
17 {
18 
19  struct IMU_version
20  {
21  byte ver[4];
22  byte size;
23  byte CRC32[4];
24  };
25 #pragma pack(push, 1)
27  {
29  byte MM_s_n[6];
30  byte DS4_s_n[6];
31  byte reserved[235];
32  };
33 
35  {
37  float kf[9];
38  float distf[5];
39  byte reserved[191];
40 
41  int get_data_size() const
42  {
43  return sizeof(kf) + sizeof(distf);
44  };
45 
46  bool has_data() const
47  {
48  return check_not_all_zeros({(byte*)&kf, ((byte*)&kf) + get_data_size()});
49  }
50 
51  operator rs_intrinsics () const
52  {
53  return{ 640, 480, kf[2], kf[5], kf[0], kf[4], RS_DISTORTION_FTHETA, { distf[0], 0, 0, 0, 0 } };
54  }
55  };
56 
57 
58  struct mm_extrinsic
59  {
60  float rotation[9];
61  float translation[3];
62 
63  operator rs_extrinsics () const {
64  return{ { rotation[0], rotation[1], rotation[2],
65  rotation[3], rotation[4], rotation[5],
66  rotation[6], rotation[7], rotation[8] },
67  { translation[0], translation[1], translation[2] } };
68  }
69 
70  int get_data_size() const
71  {
72  return sizeof(rotation) + sizeof(translation);
73  }
74 
75  bool has_data() const
76  {
77  return check_not_all_zeros({ (byte*)&rotation, ((byte*)&rotation) + get_data_size() });
78  }
79  };
81  {
87  byte reserved[55];
88 
89  int get_data_size() const
90  {
91  return sizeof(fe_to_imu) + sizeof(fe_to_depth) + sizeof(rgb_to_imu) + sizeof(depth_to_imu);
92  };
93  };
94 
96  {
97  // Scale X cross axis cross axis Bias X
98  // cross axis Scale Y cross axis Bias Y
99  // cross axis cross axis Scale Z Bias Z
100 
101  float val[3][4];
102  };
103 
104 
106  {
114  byte reserved[103];
115 
116  int get_data_size() const
117  {
118  return sizeof(acc_intrinsic) + sizeof(gyro_intrinsic) +
119  sizeof(acc_bias_variance) + sizeof(acc_noise_variance) + sizeof(gyro_bias_variance) + sizeof(gyro_noise_variance);
120  };
121 
122  bool has_data() const
123  {
124  return check_not_all_zeros({(byte*)&acc_intrinsic, ((byte*)&acc_intrinsic) + get_data_size()});
125  }
126 
127  rs_motion_device_intrinsic convert(const MM_intrinsics& intrin, const float bias_variance[3], const float noises_variance[3]) const
128  {
130 
131  for (auto i = 0; i < 3; i++)
132  {
133  for (auto j = 0; j < 4; j++)
134  {
135  res.data[i][j] = intrin.val[i][j];
136  }
137  }
138 
139  for (auto i = 0; i < 3; i++)
140  {
141  res.noise_variances[i] = noises_variance[i];
142  res.bias_variances[i] = bias_variance[i];
143  }
144  return res;
145  }
147  {
149  }
150 
152  {
154  }
155 
156  operator rs_motion_intrinsics() const{
157 
158  return{ get_acc_intrinsic(), get_gyro_intrinsic() };
159  }
160  };
161 
162  struct calibration
163  {
167  };
169  {
172  };
173 #pragma pack(pop)
174 
175 
176 
177  enum class auto_exposure_modes {
181  };
182 
184  {
185  public:
187  is_auto_exposure(true),
189  rate(60),
190  sample_rate(1),
191  skip_frames(2)
192  {}
193 
194  unsigned get_auto_exposure_state(rs_option option) const;
195  void set_auto_exposure_state(rs_option option, double value);
196 
197  private:
198  bool is_auto_exposure;
199  auto_exposure_modes mode;
200  unsigned rate;
201  unsigned sample_rate;
202  unsigned skip_frames;
203  };
204 
205  class zr300_camera;
207  public:
208  void modify_exposure(float& exposure_value, bool& exp_modified, float& gain_value, bool& gain_modified); // exposure_value in milliseconds
209  bool analyze_image(const rs_frame_ref* image);
211  void update_options(const fisheye_auto_exposure_state& options);
212 
213  private:
214  struct histogram_metric { int under_exposure_count; int over_exposure_count; int shadow_limit; int highlight_limit; int lower_q; int upper_q; float main_mean; float main_std; };
215  enum class rounding_mode_type { round, ceil, floor };
216 
217  inline void im_hist(const uint8_t* data, const int width, const int height, const int rowStep, int h[]);
218  void increase_exposure_target(float mult, float& target_exposure);
219  void decrease_exposure_target(float mult, float& target_exposure);
220  void increase_exposure_gain(const float& target_exposure, const float& target_exposure0, float& exposure, float& gain);
221  void decrease_exposure_gain(const float& target_exposure, const float& target_exposure0, float& exposure, float& gain);
222  void static_increase_exposure_gain(const float& target_exposure, const float& target_exposure0, float& exposure, float& gain);
223  void static_decrease_exposure_gain(const float& target_exposure, const float& target_exposure0, float& exposure, float& gain);
224  void anti_flicker_increase_exposure_gain(const float& target_exposure, const float& target_exposure0, float& exposure, float& gain);
225  void anti_flicker_decrease_exposure_gain(const float& target_exposure, const float& target_exposure0, float& exposure, float& gain);
226  void hybrid_increase_exposure_gain(const float& target_exposure, const float& target_exposure0, float& exposure, float& gain);
227  void hybrid_decrease_exposure_gain(const float& target_exposure, const float& target_exposure0, float& exposure, float& gain);
228 
229 #if defined(_WINDOWS) || defined(WIN32) || defined(WIN64)
230  inline float round(float x) { return std::round(x); }
231 #else
232  inline float round(float x) { return x < 0.0 ? std::ceil(x - 0.5f) : std::floor(x + 0.5f); }
233 #endif
234 
235  float exposure_to_value(float exp_ms, rounding_mode_type rounding_mode);
236  float gain_to_value(float gain, rounding_mode_type rounding_mode);
237  template <typename T> inline T sqr(const T& x) { return (x*x); }
238  void histogram_score(std::vector<int>& h, const int total_weight, histogram_metric& score);
239 
240 
241  float minimal_exposure = 0.1f, maximal_exposure = 20.f, base_gain = 2.0f, gain_limit = 15.0f;
242  float exposure = 10.0f, gain = 2.0f, target_exposure = 0.0f;
243  uint8_t under_exposure_limit = 5, over_exposure_limit = 250; int under_exposure_noise_limit = 50, over_exposure_noise_limit = 50;
244  int direction = 0, prev_direction = 0; float hysteresis = 0.075f;// 05;
245  float eps = 0.01f, exposure_step = 0.1f, minimal_exposure_step = 0.01f;
246  fisheye_auto_exposure_state state; float flicker_cycle; bool anti_flicker_mode = true;
247  std::recursive_mutex state_mutex;
248  };
249 
251  public:
254  void add_frame(rs_frame_ref* frame, std::shared_ptr<rsimpl::frame_archive> archive);
255  void update_auto_exposure_state(fisheye_auto_exposure_state& auto_exposure_state);
259  double exposure;
260  unsigned long long frame_counter;
261  };
262 
263  private:
264  void push_back_data(rs_frame_ref* data);
265  bool try_pop_front_data(rs_frame_ref** data);
266  size_t get_queue_size();
267  void clear_queue();
268  unsigned get_skip_frames(const fisheye_auto_exposure_state& auto_exposure_state) { return auto_exposure_state.get_auto_exposure_state(RS_OPTION_FISHEYE_AUTO_EXPOSURE_SKIP_FRAMES); }
269  void push_back_exp_and_cnt(exposure_and_frame_counter exp_and_cnt);
270  bool try_get_exp_by_frame_cnt(double& exposure, const unsigned long long frame_counter);
271 
272  const std::size_t max_size_of_exp_and_cnt_queue = 10;
273  zr300_camera* device;
274  auto_exposure_algorithm auto_exposure_algo;
275  std::shared_ptr<rsimpl::frame_archive> sync_archive;
276  std::shared_ptr<std::thread> exposure_thread;
277  std::condition_variable cv;
278  std::atomic<bool> keep_alive;
279  std::deque<rs_frame_ref*> data_queue;
280  std::mutex queue_mtx;
281  std::atomic<unsigned> frames_counter;
282  std::atomic<unsigned> skip_frames;
283  std::deque<exposure_and_frame_counter> exposure_and_frame_counter_queue;
284  std::mutex exp_and_cnt_queue_mtx;
285  };
286 
287  class zr300_camera final : public ds::ds_device
288  {
289  motion_module::motion_module_control motion_module_ctrl;
290  motion_module::mm_config motion_module_configuration;
291  fisheye_auto_exposure_state auto_exposure_state;
292  std::shared_ptr<auto_exposure_mechanism> auto_exposure;
293  std::atomic<bool> to_add_frames;
294 
295  protected:
296  void toggle_motion_module_power(bool bOn);
297  void toggle_motion_module_events(bool bOn);
298  void on_before_callback(rs_stream , rs_frame_ref *, std::shared_ptr<rsimpl::frame_archive>) override;
299 
300  std::timed_mutex usbMutex;
301 
302  public:
303  zr300_camera(std::shared_ptr<uvc::device> device, const static_device_info & info, motion_module_calibration fe_intrinsic, calibration_validator validator);
304  ~zr300_camera();
305 
306  void get_option_range(rs_option option, double & min, double & max, double & step, double & def) override;
307  void set_options(const rs_option options[], size_t count, const double values[]) override;
308  void get_options(const rs_option options[], size_t count, double values[]) override;
309  void send_blob_to_device(rs_blob_type type, void * data, int size) override;
310  bool supports_option(rs_option option) const override;
311 
312  void start_motion_tracking() override;
313  void stop_motion_tracking() override;
314 
315  void start(rs_source source) override;
316  void stop(rs_source source) override;
317 
318  rs_stream select_key_stream(const std::vector<rsimpl::subdevice_mode_selection> & selected_modes) override;
319 
322  unsigned long long get_frame_counter_by_usb_cmd();
323 
324 
325  private:
326  unsigned get_auto_exposure_state(rs_option option);
327  void set_auto_exposure_state(rs_option option, double value);
328  void set_fw_logger_option(double value);
329  unsigned get_fw_logger_option();
330 
331  bool validate_motion_extrinsics(rs_stream) const;
332  bool validate_motion_intrinsics() const;
333 
334  motion_module_calibration fe_intrinsic;
335  };
337  std::shared_ptr<rs_device> make_zr300_device(std::shared_ptr<uvc::device> device);
338 }
339 
340 #endif
rs_extrinsics
struct rs_extrinsics rs_extrinsics
Cross-stream extrinsics: encode the topology describing how the different devices are connected.
rsimpl::IMU_intrinsic::has_data
bool has_data() const
Definition: zr300.h:122
RS_DISTORTION_FTHETA
@ RS_DISTORTION_FTHETA
Definition: rs.h:104
rs_motion_device_intrinsic::data
float data[3][4]
Definition: rs.h:318
rs_motion_device_intrinsic::bias_variances
float bias_variances[3]
Definition: rs.h:321
rsimpl::serial_number::DS4_s_n
byte DS4_s_n[6]
Definition: zr300.h:30
rs_stream
rs_stream
Streams are different types of data provided by RealSense devices.
Definition: rs.h:33
rsimpl::motion_module::motion_module_control
Definition: motion-module.h:176
rsimpl::IMU_intrinsic::ver
IMU_version ver
Definition: zr300.h:107
rsimpl::mm_extrinsic::has_data
bool has_data() const
Definition: zr300.h:75
rs_motion_device_intrinsic::noise_variances
float noise_variances[3]
Definition: rs.h:320
rsimpl::zr300_camera::toggle_motion_module_power
void toggle_motion_module_power(bool bOn)
rsimpl::zr300_camera::get_frame_counter_by_usb_cmd
unsigned long long get_frame_counter_by_usb_cmd()
rsimpl::IMU_version::CRC32
byte CRC32[4]
Definition: zr300.h:23
sync.h
rsimpl::auto_exposure_mechanism::exposure_and_frame_counter::exposure_and_frame_counter
exposure_and_frame_counter(double exposure, unsigned long long frame_counter)
Definition: zr300.h:258
rsimpl::calibration::imu_intrinsic
IMU_intrinsic imu_intrinsic
Definition: zr300.h:166
rsimpl::fisheye_intrinsic::kf
float kf[9]
Definition: zr300.h:37
rsimpl::auto_exposure_mechanism::update_auto_exposure_state
void update_auto_exposure_state(fisheye_auto_exposure_state &auto_exposure_state)
rsimpl::zr300_camera::stop_motion_tracking
void stop_motion_tracking() override
rsimpl::auto_exposure_mechanism::exposure_and_frame_counter
Definition: zr300.h:256
rsimpl::fisheye_intrinsic
Definition: zr300.h:34
motion-module.h
rsimpl::fisheye_auto_exposure_state::set_auto_exposure_state
void set_auto_exposure_state(rs_option option, double value)
rs_motion_intrinsics
struct rs_motion_intrinsics rs_motion_intrinsics
Motion module intrinsics: includes accelerometer and gyroscope intrinsics structs of type rs_motion_d...
rsimpl::auto_exposure_mechanism::add_frame
void add_frame(rs_frame_ref *frame, std::shared_ptr< rsimpl::frame_archive > archive)
rsimpl::zr300_camera::get_options
void get_options(const rs_option options[], size_t count, double values[]) override
rsimpl::zr300_camera::set_options
void set_options(const rs_option options[], size_t count, const double values[]) override
rsimpl::IMU_intrinsic::get_gyro_intrinsic
rs_motion_device_intrinsic get_gyro_intrinsic() const
Definition: zr300.h:151
rsimpl::mm_extrinsic
Definition: zr300.h:58
rsimpl::static_device_info
Definition: types.h:268
rsimpl::IMU_version::ver
byte ver[4]
Definition: zr300.h:21
ds-device.h
rsimpl::mm_extrinsic::rotation
float rotation[9]
Definition: zr300.h:60
rsimpl::fisheye_auto_exposure_state::fisheye_auto_exposure_state
fisheye_auto_exposure_state()
Definition: zr300.h:186
rs_motion_intrinsics
Motion module intrinsics: includes accelerometer and gyroscope intrinsics structs of type rs_motion_d...
Definition: rs.h:325
rsimpl::IMU_intrinsic::acc_intrinsic
MM_intrinsics acc_intrinsic
Definition: zr300.h:108
rsimpl::auto_exposure_mechanism::exposure_and_frame_counter::exposure_and_frame_counter
exposure_and_frame_counter()
Definition: zr300.h:257
rsimpl::serial_number::ver
IMU_version ver
Definition: zr300.h:28
rsimpl::IMU_intrinsic::gyro_intrinsic
MM_intrinsics gyro_intrinsic
Definition: zr300.h:109
rsimpl::fisheye_intrinsic::ver
IMU_version ver
Definition: zr300.h:36
rsimpl::IMU_intrinsic::get_acc_intrinsic
rs_motion_device_intrinsic get_acc_intrinsic() const
Definition: zr300.h:146
rsimpl::IMU_extrinsic::ver
IMU_version ver
Definition: zr300.h:82
rsimpl::MM_intrinsics
Definition: zr300.h:95
rsimpl::auto_exposure_algorithm::update_options
void update_options(const fisheye_auto_exposure_state &options)
rsimpl::IMU_extrinsic::depth_to_imu
mm_extrinsic depth_to_imu
Definition: zr300.h:86
rsimpl::zr300_camera::select_key_stream
rs_stream select_key_stream(const std::vector< rsimpl::subdevice_mode_selection > &selected_modes) override
rsimpl::IMU_intrinsic::gyro_noise_variance
float gyro_noise_variance[3]
Definition: zr300.h:113
rsimpl::mm_extrinsic::get_data_size
int get_data_size() const
Definition: zr300.h:70
rsimpl::motion_module_calibration::calib
calibration calib
Definition: zr300.h:171
rsimpl
Definition: archive.h:12
rs::source
source
Allows the user to choose between available hardware subdevices.
Definition: rs.hpp:219
rsimpl::zr300_camera::~zr300_camera
~zr300_camera()
rsimpl::check_not_all_zeros
bool check_not_all_zeros(std::vector< byte > data)
Definition: types.h:643
rsimpl::IMU_extrinsic::reserved
byte reserved[55]
Definition: zr300.h:87
rsimpl::zr300_camera::on_before_callback
void on_before_callback(rs_stream, rs_frame_ref *, std::shared_ptr< rsimpl::frame_archive >) override
rsimpl::IMU_extrinsic::get_data_size
int get_data_size() const
Definition: zr300.h:89
rsimpl::auto_exposure_algorithm::analyze_image
bool analyze_image(const rs_frame_ref *image)
rsimpl::calibration
Definition: zr300.h:162
rsimpl::zr300_camera::toggle_motion_module_events
void toggle_motion_module_events(bool bOn)
rsimpl::serial_number::reserved
byte reserved[235]
Definition: zr300.h:31
rs_motion_device_intrinsic
Motion device intrinsics: scale, bias, and variances.
Definition: rs.h:313
rsimpl::IMU_intrinsic::acc_noise_variance
float acc_noise_variance[3]
Definition: zr300.h:111
rsimpl::motion_module_calibration::sn
serial_number sn
Definition: zr300.h:170
rsimpl::IMU_intrinsic::acc_bias_variance
float acc_bias_variance[3]
Definition: zr300.h:110
rsimpl::zr300_camera::stop
void stop(rs_source source) override
rsimpl::auto_exposure_mechanism::exposure_and_frame_counter::exposure
double exposure
Definition: zr300.h:259
rsimpl::IMU_version::size
byte size
Definition: zr300.h:22
rs_extrinsics
Cross-stream extrinsics: encode the topology describing how the different devices are connected.
Definition: rs.h:332
rsimpl::auto_exposure_modes
auto_exposure_modes
Definition: zr300.h:177
rsimpl::fisheye_auto_exposure_state
Definition: zr300.h:183
rsimpl::auto_exposure_modes::static_auto_exposure
@ static_auto_exposure
rsimpl::auto_exposure_modes::auto_exposure_anti_flicker
@ auto_exposure_anti_flicker
rs_intrinsics
struct rs_intrinsics rs_intrinsics
Video stream intrinsics.
rsimpl::zr300_camera
Definition: zr300.h:287
rsimpl::fisheye_intrinsic::get_data_size
int get_data_size() const
Definition: zr300.h:41
rsimpl::serial_number::MM_s_n
byte MM_s_n[6]
Definition: zr300.h:29
rs_source
rs_source
Source: allows you to choose between available hardware subdevices.
Definition: rs.h:90
rsimpl::zr300_camera::send_blob_to_device
void send_blob_to_device(rs_blob_type type, void *data, int size) override
rs::option
option
Defines general configuration controls.
Definition: rs.hpp:87
rsimpl::auto_exposure_mechanism::~auto_exposure_mechanism
~auto_exposure_mechanism()
RS_OPTION_FISHEYE_AUTO_EXPOSURE_SKIP_FRAMES
@ RS_OPTION_FISHEYE_AUTO_EXPOSURE_SKIP_FRAMES
Definition: rs.h:194
rsimpl::zr300_camera::start_motion_tracking
void start_motion_tracking() override
rsimpl::motion_module::mm_config
Definition: motion-module.h:87
rsimpl::IMU_intrinsic
Definition: zr300.h:105
rs_blob_type
rs_blob_type
Proprietary formats for direct communication with device firmware.
Definition: rs.h:228
rsimpl::IMU_extrinsic::fe_to_depth
mm_extrinsic fe_to_depth
Definition: zr300.h:84
rsimpl::zr300_camera::zr300_camera
zr300_camera(std::shared_ptr< uvc::device > device, const static_device_info &info, motion_module_calibration fe_intrinsic, calibration_validator validator)
rs_option
rs_option
Defines general configuration controls.
Definition: rs.h:128
rsimpl::fisheye_intrinsic::reserved
byte reserved[191]
Definition: zr300.h:39
rsimpl::MM_intrinsics::val
float val[3][4]
Definition: zr300.h:101
rsimpl::calibration_validator
Definition: types.h:628
rsimpl::make_zr300_device
std::shared_ptr< rs_device > make_zr300_device(std::shared_ptr< uvc::device > device)
rsimpl::auto_exposure_mechanism::auto_exposure_mechanism
auto_exposure_mechanism(zr300_camera *dev, fisheye_auto_exposure_state auto_exposure_state)
rsimpl::serial_number
Definition: zr300.h:26
rsimpl::ds::ds_device
Definition: ds-device.h:30
rsimpl::zr300_camera::get_option_range
void get_option_range(rs_option option, double &min, double &max, double &step, double &def) override
rsimpl::auto_exposure_algorithm::modify_exposure
void modify_exposure(float &exposure_value, bool &exp_modified, float &gain_value, bool &gain_modified)
rsimpl::fisheye_intrinsic::distf
float distf[5]
Definition: zr300.h:38
rsimpl::read_fisheye_intrinsic
motion_module_calibration read_fisheye_intrinsic(uvc::device &device)
rsimpl::mm_extrinsic::translation
float translation[3]
Definition: zr300.h:61
rs_frame_ref
Definition: rscore.hpp:44
rsimpl::IMU_intrinsic::reserved
byte reserved[103]
Definition: zr300.h:114
rsimpl::calibration::mm_extrinsic
IMU_extrinsic mm_extrinsic
Definition: zr300.h:165
rsimpl::IMU_intrinsic::get_data_size
int get_data_size() const
Definition: zr300.h:116
rsimpl::auto_exposure_mechanism
Definition: zr300.h:250
rsimpl::motion_module_calibration
Definition: zr300.h:168
rsimpl::zr300_camera::get_motion_intrinsics
rs_motion_intrinsics get_motion_intrinsics() const override
rsimpl::IMU_extrinsic::rgb_to_imu
mm_extrinsic rgb_to_imu
Definition: zr300.h:85
rsimpl::auto_exposure_mechanism::exposure_and_frame_counter::frame_counter
unsigned long long frame_counter
Definition: zr300.h:260
rsimpl::auto_exposure_algorithm::auto_exposure_algorithm
auto_exposure_algorithm(fisheye_auto_exposure_state auto_exposure_state)
rsimpl::fisheye_auto_exposure_state::get_auto_exposure_state
unsigned get_auto_exposure_state(rs_option option) const
rsimpl::IMU_extrinsic::fe_to_imu
mm_extrinsic fe_to_imu
Definition: zr300.h:83
rsimpl::IMU_intrinsic::convert
rs_motion_device_intrinsic convert(const MM_intrinsics &intrin, const float bias_variance[3], const float noises_variance[3]) const
Definition: zr300.h:127
rsimpl::IMU_intrinsic::gyro_bias_variance
float gyro_bias_variance[3]
Definition: zr300.h:112
rsimpl::auto_exposure_algorithm
Definition: zr300.h:206
rsimpl::zr300_camera::usbMutex
std::timed_mutex usbMutex
Definition: zr300.h:300
rsimpl::auto_exposure_modes::auto_exposure_hybrid
@ auto_exposure_hybrid
rsimpl::zr300_camera::get_motion_extrinsics_from
rs_extrinsics get_motion_extrinsics_from(rs_stream from) const override
rsimpl::calibration::fe_intrinsic
fisheye_intrinsic fe_intrinsic
Definition: zr300.h:164
rsimpl::fisheye_intrinsic::has_data
bool has_data() const
Definition: zr300.h:46
rsimpl::IMU_version
Definition: zr300.h:19
rsimpl::IMU_extrinsic
Definition: zr300.h:80
rsimpl::zr300_camera::supports_option
bool supports_option(rs_option option) const override
rsimpl::zr300_camera::start
void start(rs_source source) override