algo.h
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2015 Intel Corporation. All Rights Reserved.
3 
4 #pragma once
5 #include "sensor.h"
6 #include "concurrency.h"
7 #include "types.h"
8 
9 #include <stdint.h>
10 #include <vector>
11 #include <mutex>
12 #include <deque>
13 #include <cmath>
14 #include <memory>
15 
16 namespace librealsense
17 {
18  static const float ae_step_default_value = 0.5f;
19 
20  enum class auto_exposure_modes {
24  };
25 
27  {
28  public:
30  is_auto_exposure(true),
31  mode(auto_exposure_modes::auto_exposure_hybrid),
32  rate(60),
33  step(ae_step_default_value)
34  {}
35 
36  bool get_enable_auto_exposure() const;
37  auto_exposure_modes get_auto_exposure_mode() const;
38  unsigned get_auto_exposure_antiflicker_rate() const;
39  float get_auto_exposure_step() const;
40 
41  void set_enable_auto_exposure(bool value);
42  void set_auto_exposure_mode(auto_exposure_modes value);
43  void set_auto_exposure_antiflicker_rate(unsigned value);
44  void set_auto_exposure_step(float value);
45 
46  static const unsigned sample_rate = 1;
47  static const unsigned skip_frames = 2;
48 
49  private:
52  unsigned rate;
53  float step;
54  };
55 
56 
58  public:
59  void modify_exposure(float& exposure_value, bool& exp_modified, float& gain_value, bool& gain_modified); // exposure_value in milliseconds
60  bool analyze_image(const frame_interface* image);
62  void update_options(const auto_exposure_state& options);
63  void update_roi(const region_of_interest& ae_roi);
64 
65  private:
66  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; };
67  enum class rounding_mode_type { round, ceil, floor };
68 
69  inline void im_hist(const uint8_t* data, const region_of_interest& image_roi, const int rowStep, int h[]);
70  void increase_exposure_target(float mult, float& target_exposure);
71  void decrease_exposure_target(float mult, float& target_exposure);
72  void increase_exposure_gain(const float& target_exposure, const float& target_exposure0, float& exposure, float& gain);
73  void decrease_exposure_gain(const float& target_exposure, const float& target_exposure0, float& exposure, float& gain);
74  void static_increase_exposure_gain(const float& target_exposure, const float& target_exposure0, float& exposure, float& gain);
75  void static_decrease_exposure_gain(const float& target_exposure, const float& target_exposure0, float& exposure, float& gain);
76  void anti_flicker_increase_exposure_gain(const float& target_exposure, const float& target_exposure0, float& exposure, float& gain);
77  void anti_flicker_decrease_exposure_gain(const float& target_exposure, const float& target_exposure0, float& exposure, float& gain);
78  void hybrid_increase_exposure_gain(const float& target_exposure, const float& target_exposure0, float& exposure, float& gain);
79  void hybrid_decrease_exposure_gain(const float& target_exposure, const float& target_exposure0, float& exposure, float& gain);
80 
81  #if defined(_WINDOWS) || defined(WIN32) || defined(WIN64)
82  inline float round(float x) { return std::round(x); }
83  #else
84  inline float round(float x) { return x < 0.0 ? std::ceil(x - 0.5f) : std::floor(x + 0.5f); }
85  #endif
86 
87  float exposure_to_value(float exp_ms, rounding_mode_type rounding_mode);
88  float gain_to_value(float gain, rounding_mode_type rounding_mode);
89  template <typename T> inline T sqr(const T& x) { return (x*x); }
90  void histogram_score(std::vector<int>& h, const int total_weight, histogram_metric& score);
91 
92 
93  float minimal_exposure = 0.2f, maximal_exposure = 20.f, base_gain = 2.0f, gain_limit = 15.0f;
94  float exposure = 10.0f, gain = 2.0f, target_exposure = 0.0f;
95  uint8_t under_exposure_limit = 5, over_exposure_limit = 250; int under_exposure_noise_limit = 50, over_exposure_noise_limit = 50;
96  int direction = 0, prev_direction = 0; float hysteresis = 0.075f;// 05;
97  float eps = 0.01f;
98  std::atomic<float> exposure_step;
99  auto_exposure_state state; float flicker_cycle; bool anti_flicker_mode = true;
101  bool is_roi_initialized = false;
102  std::recursive_mutex state_mutex;
103  };
104 
106  public:
109  void add_frame(frame_holder frame);
110  void update_auto_exposure_state(const auto_exposure_state& auto_exposure_state);
111  void update_auto_exposure_roi(const region_of_interest& roi);
112 
115  : exposure(0), frame_counter(0)
116  {}
117 
118  exposure_and_frame_counter(double exposure, unsigned long long frame_counter)
119  : exposure(exposure), frame_counter(frame_counter)
120  {}
121 
122  double exposure;
123  unsigned long long frame_counter;
124  };
125 
126  private:
127  static const int queue_size = 2;
131  std::shared_ptr<std::thread> _exposure_thread;
132  std::condition_variable _cv;
133  std::atomic<bool> _keep_alive;
135  std::mutex _queue_mtx;
136  std::atomic<unsigned> _frames_counter;
137  std::atomic<unsigned> _skip_frames;
138  };
139 
140  // Interface for target calculator
142  {
143  public:
144  virtual bool calculate(const uint8_t* img, float* target_dims, unsigned int target_dims_size) = 0;
145  virtual ~target_calculator_interface() = default;
146  };
147 
149  {
150  public:
153  bool calculate(const uint8_t* img, float* target_dims, unsigned int target_dims_size) override;
154 
157 
160 
161  protected:
162  void normalize(const uint8_t* img);
163  void calculate_ncc();
164 
165  bool find_corners();
166  void refine_corners();
167  bool validate_corners(const uint8_t* img);
168 
169  void calculate_rect_sides(float* rect_sides);
170 
171  void minimize_x(const double* p, int s, double* f, double& x);
172  void minimize_y(const double* p, int s, double* f, double& y);
173  double subpixel_agj(double* f, int s);
174 
175  public:
176  static const int _frame_num = 25; // number of frames used to smooth the result
177 
178  protected:
179  const int _tsize = 28; // template size
180  const int _htsize = _tsize >> 1;
181  const int _tsize2 = _tsize * _tsize;
182  std::vector<double> _imgt;
183 
184  const std::vector<double> _template
185  {
186  -0.02870443, -0.02855973, -0.02855973, -0.02841493, -0.02827013, -0.02812543, -0.02783583, -0.02769113, -0.02725683, -0.02696723, -0.02667773, -0.02624343, -0.02609863, -0.02595393, -0.02580913, -0.02595393, -0.02609863, -0.02624343, -0.02667773, -0.02696723, -0.02725683, -0.02769113, -0.02783583, -0.02812543, -0.02827013, -0.02841493, -0.02855973, -0.02855973,
187  -0.02855973, -0.02855973, -0.02841493, -0.02827013, -0.02798063, -0.02769113, -0.02740153, -0.02682253, -0.02624343, -0.02566433, -0.02508533, -0.02465103, -0.02421673, -0.02392713, -0.02378243, -0.02392713, -0.02421673, -0.02465103, -0.02508533, -0.02566433, -0.02624343, -0.02682253, -0.02740153, -0.02769113, -0.02798063, -0.02827013, -0.02841493, -0.02855973,
188  -0.02855973, -0.02841493, -0.02827013, -0.02798063, -0.02754633, -0.02711203, -0.02638823, -0.02566433, -0.02479573, -0.02378243, -0.02276903, -0.02190043, -0.02117663, -0.02074233, -0.02059753, -0.02074233, -0.02117663, -0.02190043, -0.02276903, -0.02378243, -0.02479573, -0.02566433, -0.02638823, -0.02711203, -0.02754633, -0.02798063, -0.02827013, -0.02841493,
189  -0.02841493, -0.02827013, -0.02798063, -0.02754633, -0.02696723, -0.02609863, -0.02508533, -0.02392713, -0.02247953, -0.02088703, -0.01929463, -0.01799173, -0.01683363, -0.01610973, -0.01582023, -0.01610973, -0.01683363, -0.01799173, -0.01929463, -0.02088703, -0.02247953, -0.02392713, -0.02508533, -0.02609863, -0.02696723, -0.02754633, -0.02798063, -0.02827013,
190  -0.02827013, -0.02798063, -0.02754633, -0.02696723, -0.02609863, -0.02479573, -0.02320333, -0.02132133, -0.01914983, -0.01683363, -0.01451733, -0.01234583, -0.01060863, -0.00945043, -0.00916093, -0.00945043, -0.01060863, -0.01234583, -0.01451733, -0.01683363, -0.01914983, -0.02132133, -0.02320333, -0.02479573, -0.02609863, -0.02696723, -0.02754633, -0.02798063,
191  -0.02812543, -0.02769113, -0.02711203, -0.02609863, -0.02479573, -0.02305853, -0.02074233, -0.01799173, -0.01480683, -0.01133243, -0.00785803, -0.00481793, -0.00221213, -0.00061973, -0.00004063, -0.00061973, -0.00221213, -0.00481793, -0.00785803, -0.01133243, -0.01480683, -0.01799173, -0.02074233, -0.02305853, -0.02479573, -0.02609863, -0.02711203, -0.02769113,
192  -0.02783583, -0.02740153, -0.02638823, -0.02508533, -0.02320333, -0.02074233, -0.01755743, -0.01364873, -0.00916093, -0.00423883, 0.00053847, 0.00488147, 0.00850057, 0.01081687, 0.01154077, 0.01081687, 0.00850057, 0.00488147, 0.00053847, -0.00423883, -0.00916093, -0.01364873, -0.01755743, -0.02074233, -0.02320333, -0.02508533, -0.02638823, -0.02740153,
193  -0.02769113, -0.02682253, -0.02566433, -0.02392713, -0.02132133, -0.01799173, -0.01364873, -0.00829233, -0.00221213, 0.00430237, 0.01081687, 0.01660757, 0.02138487, 0.02456977, 0.02558307, 0.02456977, 0.02138487, 0.01660757, 0.01081687, 0.00430237, -0.00221213, -0.00829233, -0.01364873, -0.01799173, -0.02132133, -0.02392713, -0.02566433, -0.02682253,
194  -0.02725683, -0.02624343, -0.02479573, -0.02247953, -0.01914983, -0.01480683, -0.00916093, -0.00221213, 0.00560527, 0.01400177, 0.02239827, 0.03021567, 0.03629587, 0.04034937, 0.04179697, 0.04034937, 0.03629587, 0.03021567, 0.02239827, 0.01400177, 0.00560527, -0.00221213, -0.00916093, -0.01480683, -0.01914983, -0.02247953, -0.02479573, -0.02624343,
195  -0.02696723, -0.02566433, -0.02378243, -0.02088703, -0.01683363, -0.01133243, -0.00423883, 0.00430237, 0.01400177, 0.02456977, 0.03499297, 0.04469237, 0.05236497, 0.05743187, 0.05916907, 0.05743187, 0.05236497, 0.04469237, 0.03499297, 0.02456977, 0.01400177, 0.00430237, -0.00423883, -0.01133243, -0.01683363, -0.02088703, -0.02378243, -0.02566433,
196  -0.02667773, -0.02508533, -0.02276903, -0.01929463, -0.01451733, -0.00785803, 0.00053847, 0.01081687, 0.02239827, 0.03499297, 0.04758767, 0.05916907, 0.06828937, 0.07436957, 0.07639627, 0.07436957, 0.06828937, 0.05916907, 0.04758767, 0.03499297, 0.02239827, 0.01081687, 0.00053847, -0.00785803, -0.01451733, -0.01929463, -0.02276903, -0.02508533,
197  -0.02624343, -0.02465103, -0.02190043, -0.01799173, -0.01234583, -0.00481793, 0.00488147, 0.01660757, 0.03021567, 0.04469237, 0.05916907, 0.07234287, 0.08291078, 0.08985968, 0.09217588, 0.08985968, 0.08291078, 0.07234287, 0.05916907, 0.04469237, 0.03021567, 0.01660757, 0.00488147, -0.00481793, -0.01234583, -0.01799173, -0.02190043, -0.02465103,
198  -0.02609863, -0.02421673, -0.02117663, -0.01683363, -0.01060863, -0.00221213, 0.00850057, 0.02138487, 0.03629587, 0.05236497, 0.06828937, 0.08291078, 0.09463698, 0.10230958, 0.10491538, 0.10230958, 0.09463698, 0.08291078, 0.06828937, 0.05236497, 0.03629587, 0.02138487, 0.00850057, -0.00221213, -0.01060863, -0.01683363, -0.02117663, -0.02421673,
199  -0.02595393, -0.02392713, -0.02074233, -0.01610973, -0.00945043, -0.00061973, 0.01081687, 0.02456977, 0.04034937, 0.05743187, 0.07436957, 0.08985968, 0.10230958, 0.11041658, 0.11316708, 0.11041658, 0.10230958, 0.08985968, 0.07436957, 0.05743187, 0.04034937, 0.02456977, 0.01081687, -0.00061973, -0.00945043, -0.01610973, -0.02074233, -0.02392713,
200  -0.02580913, -0.02378243, -0.02059753, -0.01582023, -0.00916093, -0.00004063, 0.01154077, 0.02558307, 0.04179697, 0.05916907, 0.07639627, 0.09217588, 0.10491538, 0.11316708, 0.11606248, 0.11316708, 0.10491538, 0.09217588, 0.07639627, 0.05916907, 0.04179697, 0.02558307, 0.01154077, -0.00004063, -0.00916093, -0.01582023, -0.02059753, -0.02378243,
201  -0.02595393, -0.02392713, -0.02074233, -0.01610973, -0.00945043, -0.00061973, 0.01081687, 0.02456977, 0.04034937, 0.05743187, 0.07436957, 0.08985968, 0.10230958, 0.11041658, 0.11316708, 0.11041658, 0.10230958, 0.08985968, 0.07436957, 0.05743187, 0.04034937, 0.02456977, 0.01081687, -0.00061973, -0.00945043, -0.01610973, -0.02074233, -0.02392713,
202  -0.02609863, -0.02421673, -0.02117663, -0.01683363, -0.01060863, -0.00221213, 0.00850057, 0.02138487, 0.03629587, 0.05236497, 0.06828937, 0.08291078, 0.09463698, 0.10230958, 0.10491538, 0.10230958, 0.09463698, 0.08291078, 0.06828937, 0.05236497, 0.03629587, 0.02138487, 0.00850057, -0.00221213, -0.01060863, -0.01683363, -0.02117663, -0.02421673,
203  -0.02624343, -0.02465103, -0.02190043, -0.01799173, -0.01234583, -0.00481793, 0.00488147, 0.01660757, 0.03021567, 0.04469237, 0.05916907, 0.07234287, 0.08291078, 0.08985968, 0.09217588, 0.08985968, 0.08291078, 0.07234287, 0.05916907, 0.04469237, 0.03021567, 0.01660757, 0.00488147, -0.00481793, -0.01234583, -0.01799173, -0.02190043, -0.02465103,
204  -0.02667773, -0.02508533, -0.02276903, -0.01929463, -0.01451733, -0.00785803, 0.00053847, 0.01081687, 0.02239827, 0.03499297, 0.04758767, 0.05916907, 0.06828937, 0.07436957, 0.07639627, 0.07436957, 0.06828937, 0.05916907, 0.04758767, 0.03499297, 0.02239827, 0.01081687, 0.00053847, -0.00785803, -0.01451733, -0.01929463, -0.02276903, -0.02508533,
205  -0.02696723, -0.02566433, -0.02378243, -0.02088703, -0.01683363, -0.01133243, -0.00423883, 0.00430237, 0.01400177, 0.02456977, 0.03499297, 0.04469237, 0.05236497, 0.05743187, 0.05916907, 0.05743187, 0.05236497, 0.04469237, 0.03499297, 0.02456977, 0.01400177, 0.00430237, -0.00423883, -0.01133243, -0.01683363, -0.02088703, -0.02378243, -0.02566433,
206  -0.02725683, -0.02624343, -0.02479573, -0.02247953, -0.01914983, -0.01480683, -0.00916093, -0.00221213, 0.00560527, 0.01400177, 0.02239827, 0.03021567, 0.03629587, 0.04034937, 0.04179697, 0.04034937, 0.03629587, 0.03021567, 0.02239827, 0.01400177, 0.00560527, -0.00221213, -0.00916093, -0.01480683, -0.01914983, -0.02247953, -0.02479573, -0.02624343,
207  -0.02769113, -0.02682253, -0.02566433, -0.02392713, -0.02132133, -0.01799173, -0.01364873, -0.00829233, -0.00221213, 0.00430237, 0.01081687, 0.01660757, 0.02138487, 0.02456977, 0.02558307, 0.02456977, 0.02138487, 0.01660757, 0.01081687, 0.00430237, -0.00221213, -0.00829233, -0.01364873, -0.01799173, -0.02132133, -0.02392713, -0.02566433, -0.02682253,
208  -0.02783583, -0.02740153, -0.02638823, -0.02508533, -0.02320333, -0.02074233, -0.01755743, -0.01364873, -0.00916093, -0.00423883, 0.00053847, 0.00488147, 0.00850057, 0.01081687, 0.01154077, 0.01081687, 0.00850057, 0.00488147, 0.00053847, -0.00423883, -0.00916093, -0.01364873, -0.01755743, -0.02074233, -0.02320333, -0.02508533, -0.02638823, -0.02740153,
209  -0.02812543, -0.02769113, -0.02711203, -0.02609863, -0.02479573, -0.02305853, -0.02074233, -0.01799173, -0.01480683, -0.01133243, -0.00785803, -0.00481793, -0.00221213, -0.00061973, -0.00004063, -0.00061973, -0.00221213, -0.00481793, -0.00785803, -0.01133243, -0.01480683, -0.01799173, -0.02074233, -0.02305853, -0.02479573, -0.02609863, -0.02711203, -0.02769113,
210  -0.02827013, -0.02798063, -0.02754633, -0.02696723, -0.02609863, -0.02479573, -0.02320333, -0.02132133, -0.01914983, -0.01683363, -0.01451733, -0.01234583, -0.01060863, -0.00945043, -0.00916093, -0.00945043, -0.01060863, -0.01234583, -0.01451733, -0.01683363, -0.01914983, -0.02132133, -0.02320333, -0.02479573, -0.02609863, -0.02696723, -0.02754633, -0.02798063,
211  -0.02841493, -0.02827013, -0.02798063, -0.02754633, -0.02696723, -0.02609863, -0.02508533, -0.02392713, -0.02247953, -0.02088703, -0.01929463, -0.01799173, -0.01683363, -0.01610973, -0.01582023, -0.01610973, -0.01683363, -0.01799173, -0.01929463, -0.02088703, -0.02247953, -0.02392713, -0.02508533, -0.02609863, -0.02696723, -0.02754633, -0.02798063, -0.02827013,
212  -0.02855973, -0.02841493, -0.02827013, -0.02798063, -0.02754633, -0.02711203, -0.02638823, -0.02566433, -0.02479573, -0.02378243, -0.02276903, -0.02190043, -0.02117663, -0.02074233, -0.02059753, -0.02074233, -0.02117663, -0.02190043, -0.02276903, -0.02378243, -0.02479573, -0.02566433, -0.02638823, -0.02711203, -0.02754633, -0.02798063, -0.02827013, -0.02841493,
213  -0.02855973, -0.02855973, -0.02841493, -0.02827013, -0.02798063, -0.02769113, -0.02740153, -0.02682253, -0.02624343, -0.02566433, -0.02508533, -0.02465103, -0.02421673, -0.02392713, -0.02378243, -0.02392713, -0.02421673, -0.02465103, -0.02508533, -0.02566433, -0.02624343, -0.02682253, -0.02740153, -0.02769113, -0.02798063, -0.02827013, -0.02841493, -0.02855973,
214  };
215 
216  const double _thresh = 0.7; // used internally, range from 0 to 1 for normalized image ma
217  const int _patch_size = 20; // in pixels
218  std::vector<double> _buf;
219 
220  std::vector<double> _img;
221  std::vector<double> _ncc;
222  int _width = 0;
223  int _height = 0;
224  int _size = 0;
225 
226  int _wt = 0;
227  int _ht = 0;
228  int _hwidth;
229  int _hheight;
230 
231  template <typename T>
232  struct point
233  {
234  T x;
235  T y;
236  };
237 
238  point<double> _corners[4];
239  point<int> _pts[4];
240  };
241 }
std::shared_ptr< std::thread > _exposure_thread
Definition: algo.h:131
float3 mult(const float3x3 &a, const float3 &b)
Definition: test-pose.cpp:78
GLint y
std::atomic< unsigned > _frames_counter
Definition: algo.h:136
std::recursive_mutex state_mutex
Definition: algo.h:102
auto_exposure_modes
Definition: algo.h:20
GLdouble s
static const float ae_step_default_value
Definition: algo.h:18
GLfloat GLfloat p
Definition: glext.h:12687
GLfloat value
exposure_and_frame_counter(double exposure, unsigned long long frame_counter)
Definition: algo.h:118
std::atomic< float > exposure_step
Definition: algo.h:98
GLfloat GLfloat GLfloat GLfloat h
Definition: glext.h:1960
unsigned char uint8_t
Definition: stdint.h:78
auto_exposure_algorithm _auto_exposure_algo
Definition: algo.h:130
direction
Definition: rs-align.cpp:25
GLenum GLenum GLsizei void * image
GLdouble f
GLenum mode
std::atomic< bool > _keep_alive
Definition: algo.h:133
auto_exposure_state state
Definition: algo.h:99
GLuint GLenum * rate
Definition: glext.h:10991
std::atomic< unsigned > _skip_frames
Definition: algo.h:137
GLdouble x
auto_exposure_modes mode
Definition: algo.h:51
GLint GLsizei GLsizei height
rs2_intrinsics normalize(const rs2_intrinsics &intr)
Definition: l500-color.cpp:272
GLint void * img
single_consumer_queue< frame_holder > _data_queue
Definition: algo.h:134
Definition: parser.hpp:150
GLint GLsizei width
std::condition_variable _cv
Definition: algo.h:132


librealsense2
Author(s): Sergey Dorodnicov , Doron Hirshberg , Mark Horn , Reagan Lopez , Itay Carpis
autogenerated on Mon May 3 2021 02:45:06