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"
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 
148  const int _roi_ws = 480;
149  const int _roi_we = 800;
150  const int _roi_hs = 240;
151  const int _roi_he = 480;
152  const int _patch_size = 20; // in pixels
153 
155  {
156  public:
157  rect_gaussian_dots_target_calculator(int width, int height, int roi_start_x, int roi_start_y, int roi_width, int roi_height);
159  bool calculate(const uint8_t* img, float* target_dims, unsigned int target_dims_size) override;
160 
163 
166 
167  protected:
168  void normalize(const uint8_t* img);
169  void calculate_ncc();
170 
171  bool find_corners();
172  void refine_corners();
173  bool validate_corners(const uint8_t* img);
174 
175  void calculate_rect_sides(float* rect_sides);
176 
177  void minimize_x(const double* p, int s, double* f, double& x);
178  void minimize_y(const double* p, int s, double* f, double& y);
179  double subpixel_agj(double* f, int s);
180 
181  protected:
182  const int _tsize = 28; // template size
183  const int _htsize = _tsize >> 1;
184  const int _tsize2 = _tsize * _tsize;
185  std::vector<double> _imgt;
186 
187  const std::vector<double> _template
188  {
189  -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,
190  -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,
191  -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,
192  -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,
193  -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,
194  -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,
195  -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,
196  -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,
197  -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,
198  -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,
199  -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,
200  -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,
201  -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,
202  -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,
203  -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,
204  -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,
205  -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,
206  -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,
207  -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,
208  -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,
209  -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,
210  -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,
211  -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,
212  -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,
213  -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,
214  -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,
215  -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,
216  -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,
217  };
218 
219  const double _thresh = 0.7; // used internally, range from 0 to 1 for normalized image ma
220  std::vector<double> _buf;
221 
222  std::vector<double> _img;
223  std::vector<double> _ncc;
224  int _width = 0;
225  int _height = 0;
226  int _size = 0;
227 
228  int _wt = 0;
229  int _ht = 0;
230  int _hwidth;
231  int _hheight;
232 
233  template <typename T>
234  struct point
235  {
236  T x;
237  T y;
238  };
239 
240  point<double> _corners[4];
241  point<int> _pts[4];
242 
247  };
248 
249  // Utility class for calculating the rectangle sides on the specific target
251  {
252  public:
253  rect_calculator(bool roi = false) : _roi(roi) {}
254  virtual ~rect_calculator() {}
255 
256  // return 1 if target found, zero otherwise
257  int extract_target_dims(const rs2_frame* frame_ref, float4& rect_sides);
258 
259  private:
260  bool _roi = false;
261  };
262 }
std::shared_ptr< std::thread > _exposure_thread
Definition: algo.h:131
float3 mult(const float3x3 &a, const float3 &b)
Definition: test-pose.cpp:72
GLint y
std::atomic< unsigned > _frames_counter
Definition: algo.h:136
const int _roi_we
Definition: algo.h:149
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 value
exposure_and_frame_counter(double exposure, unsigned long long frame_counter)
Definition: algo.h:118
const int _roi_ws
Definition: algo.h:148
std::atomic< float > exposure_step
Definition: algo.h:98
unsigned char uint8_t
Definition: stdint.h:78
auto_exposure_algorithm _auto_exposure_algo
Definition: algo.h:130
direction
Definition: rs-align.cpp:25
const int _patch_size
Definition: algo.h:152
GLenum mode
std::atomic< bool > _keep_alive
Definition: algo.h:133
auto_exposure_state state
Definition: algo.h:99
GLenum GLenum GLsizei void * image
std::atomic< unsigned > _skip_frames
Definition: algo.h:137
double p[GRIDW][GRIDH]
Definition: wave.c:109
const int _roi_hs
Definition: algo.h:150
GLdouble x
auto_exposure_modes mode
Definition: algo.h:51
GLint GLsizei GLsizei height
rect_calculator(bool roi=false)
Definition: algo.h:253
rs2_intrinsics normalize(const rs2_intrinsics &intr)
Definition: l500-color.cpp:271
GLint void * img
const int _roi_he
Definition: algo.h:151
single_consumer_queue< frame_holder > _data_queue
Definition: algo.h:134
GLdouble f
virtual ~rect_calculator()
Definition: algo.h:254
int h
Definition: sw.py:17
Definition: parser.hpp:153
struct rs2_frame rs2_frame
Definition: rs_types.h:262
GLint GLsizei width
std::condition_variable _cv
Definition: algo.h:132


librealsense2
Author(s): LibRealSense ROS Team
autogenerated on Thu Dec 22 2022 03:41:41