Intel® RealSense™ Cross Platform API
Intel Realsense Cross-platform API
Loading...
Searching...
No Matches
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
16namespace rsimpl
17{
18
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
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) +
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
159 }
160 };
161
163 {
167 };
169 {
172 };
173#pragma pack(pop)
174
175
176
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;
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);
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);
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:
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);
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
Definition: zr300.h:206
auto_exposure_algorithm(fisheye_auto_exposure_state auto_exposure_state)
bool analyze_image(const rs_frame_ref *image)
void update_options(const fisheye_auto_exposure_state &options)
void modify_exposure(float &exposure_value, bool &exp_modified, float &gain_value, bool &gain_modified)
Definition: zr300.h:250
void add_frame(rs_frame_ref *frame, std::shared_ptr< rsimpl::frame_archive > archive)
auto_exposure_mechanism(zr300_camera *dev, fisheye_auto_exposure_state auto_exposure_state)
void update_auto_exposure_state(fisheye_auto_exposure_state &auto_exposure_state)
Definition: types.h:629
Definition: ds-device.h:31
void set_auto_exposure_state(rs_option option, double value)
unsigned get_auto_exposure_state(rs_option option) const
fisheye_auto_exposure_state()
Definition: zr300.h:186
Definition: motion-module.h:177
Definition: zr300.h:288
void stop(rs_source source) override
rs_motion_intrinsics get_motion_intrinsics() const override
bool supports_option(rs_option option) const override
void toggle_motion_module_power(bool bOn)
std::timed_mutex usbMutex
Definition: zr300.h:300
void on_before_callback(rs_stream, rs_frame_ref *, std::shared_ptr< rsimpl::frame_archive >) override
void toggle_motion_module_events(bool bOn)
rs_stream select_key_stream(const std::vector< rsimpl::subdevice_mode_selection > &selected_modes) override
void get_options(const rs_option options[], size_t count, double values[]) override
zr300_camera(std::shared_ptr< uvc::device > device, const static_device_info &info, motion_module_calibration fe_intrinsic, calibration_validator validator)
void stop_motion_tracking() override
void start(rs_source source) override
void get_option_range(rs_option option, double &min, double &max, double &step, double &def) override
void set_options(const rs_option options[], size_t count, const double values[]) override
rs_extrinsics get_motion_extrinsics_from(rs_stream from) const override
unsigned long long get_frame_counter_by_usb_cmd()
void start_motion_tracking() override
void send_blob_to_device(rs_blob_type type, void *data, int size) override
Definition: archive.h:13
auto_exposure_modes
Definition: zr300.h:177
motion_module_calibration read_fisheye_intrinsic(uvc::device &device)
std::shared_ptr< rs_device > make_zr300_device(std::shared_ptr< uvc::device > device)
bool check_not_all_zeros(std::vector< byte > data)
Definition: types.h:643
rs_stream
Streams are different types of data provided by RealSense devices.
Definition: rs.h:34
@ RS_DISTORTION_FTHETA
Definition: rs.h:104
rs_option
Defines general configuration controls.
Definition: rs.h:129
@ RS_OPTION_FISHEYE_AUTO_EXPOSURE_SKIP_FRAMES
Definition: rs.h:194
rs_blob_type
Proprietary formats for direct communication with device firmware.
Definition: rs.h:228
rs_source
Source: allows you to choose between available hardware subdevices.
Definition: rs.h:91
Cross-stream extrinsics: encode the topology describing how the different devices are connected.
Definition: rs.h:333
Definition: rscore.hpp:45
Video stream intrinsics.
Definition: rs.h:301
Motion device intrinsics: scale, bias, and variances.
Definition: rs.h:314
float noise_variances[3]
Definition: rs.h:320
float data[3][4]
Definition: rs.h:318
float bias_variances[3]
Definition: rs.h:321
Motion module intrinsics: includes accelerometer and gyroscope intrinsics structs of type rs_motion_d...
Definition: rs.h:326
Definition: zr300.h:81
mm_extrinsic fe_to_depth
Definition: zr300.h:84
int get_data_size() const
Definition: zr300.h:89
mm_extrinsic rgb_to_imu
Definition: zr300.h:85
byte reserved[55]
Definition: zr300.h:87
mm_extrinsic fe_to_imu
Definition: zr300.h:83
IMU_version ver
Definition: zr300.h:82
mm_extrinsic depth_to_imu
Definition: zr300.h:86
Definition: zr300.h:106
float acc_bias_variance[3]
Definition: zr300.h:110
IMU_version ver
Definition: zr300.h:107
rs_motion_device_intrinsic get_acc_intrinsic() const
Definition: zr300.h:146
MM_intrinsics gyro_intrinsic
Definition: zr300.h:109
byte reserved[103]
Definition: zr300.h:114
float acc_noise_variance[3]
Definition: zr300.h:111
int get_data_size() const
Definition: zr300.h:116
float gyro_bias_variance[3]
Definition: zr300.h:112
rs_motion_device_intrinsic convert(const MM_intrinsics &intrin, const float bias_variance[3], const float noises_variance[3]) const
Definition: zr300.h:127
MM_intrinsics acc_intrinsic
Definition: zr300.h:108
float gyro_noise_variance[3]
Definition: zr300.h:113
bool has_data() const
Definition: zr300.h:122
rs_motion_device_intrinsic get_gyro_intrinsic() const
Definition: zr300.h:151
Definition: zr300.h:20
byte ver[4]
Definition: zr300.h:21
byte CRC32[4]
Definition: zr300.h:23
byte size
Definition: zr300.h:22
Definition: zr300.h:96
float val[3][4]
Definition: zr300.h:101
exposure_and_frame_counter(double exposure, unsigned long long frame_counter)
Definition: zr300.h:258
unsigned long long frame_counter
Definition: zr300.h:260
Definition: zr300.h:163
IMU_extrinsic mm_extrinsic
Definition: zr300.h:165
IMU_intrinsic imu_intrinsic
Definition: zr300.h:166
fisheye_intrinsic fe_intrinsic
Definition: zr300.h:164
Definition: zr300.h:35
bool has_data() const
Definition: zr300.h:46
int get_data_size() const
Definition: zr300.h:41
float distf[5]
Definition: zr300.h:38
IMU_version ver
Definition: zr300.h:36
float kf[9]
Definition: zr300.h:37
byte reserved[191]
Definition: zr300.h:39
Definition: zr300.h:59
float rotation[9]
Definition: zr300.h:60
bool has_data() const
Definition: zr300.h:75
int get_data_size() const
Definition: zr300.h:70
float translation[3]
Definition: zr300.h:61
Definition: motion-module.h:88
Definition: zr300.h:169
calibration calib
Definition: zr300.h:171
serial_number sn
Definition: zr300.h:170
Definition: zr300.h:27
byte reserved[235]
Definition: zr300.h:31
byte DS4_s_n[6]
Definition: zr300.h:30
byte MM_s_n[6]
Definition: zr300.h:29
IMU_version ver
Definition: zr300.h:28
Definition: types.h:269