Go to the documentation of this file.
51 : _gain_option(gain_option), _exposure_option(exposure_option),
53 _keep_alive(true), _data_queue(queue_size), _frames_counter(0),
74 LOG_ERROR(
"After waiting on data_queue signalled there are no frames on queue");
79 auto frame = std::move(f_holder);
85 ?
static_cast< double >( actual_exposure_md )
89 ?
static_cast< double >( gain_level_md )
95 auto exposure_value =
static_cast<float>(
values[0]);
96 auto gain_value =
static_cast<float>(2. + (
values[1] - 15.) / 8.);
101 bool modify_exposure, modify_gain;
106 auto value = exposure_value * 1000.f;
115 auto value = (gain_value - 2.f) * 8.
f + 15.
f;
120 catch (
const std::exception& ex)
122 LOG_ERROR(
"Error during Auto-Exposure loop: " << ex.what());
126 LOG_ERROR(
"Unknown error during Auto-Exposure loop!");
188 float target_exposure0 = total_exposure * (1.0f +
exposure_step);
193 LOG_DEBUG(
" ModifyExposure: IncreaseExposureGain: ");
194 LOG_DEBUG(
" target_exposure0 " << target_exposure0);
198 float target_exposure0 = total_exposure / (1.0f +
exposure_step);
203 LOG_DEBUG(
" ModifyExposure: DecreaseExposureGain: ");
204 LOG_DEBUG(
" target_exposure0 " << target_exposure0);
212 LOG_DEBUG(
"output exposure by algo = " << exposure_value);
214 if (gain_value !=
gain)
216 gain_modified =
true;
228 auto number_of_pixels = (image_roi.
max_x - image_roi.
min_x + 1)*(image_roi.
max_y - image_roi.
min_y + 1);
229 if (number_of_pixels == 0)
245 std::vector<int>
H(256);
246 auto total_weight = number_of_pixels;
248 auto cols =
frame->get_width();
255 float s1 = (
score.main_mean - 128.0f) / 255.0
f;
258 s2 = (
score.over_exposure_count -
score.under_exposure_count) / (
float)total_weight;
260 float s = -0.3f * (
s1 + 5.0f *
s2);
270 LOG_DEBUG(
" AnalyzeImage: DecreaseExposure");
306 for (
int i = 0;
i < 256; ++
i)
310 for (
int i = image_roi.
min_y;
i < image_roi.
max_y; ++
i, rowData += rowStep)
329 switch ((
int)(
state.get_auto_exposure_mode()))
340 switch ((
int)(
state.get_auto_exposure_mode()))
359 std::vector< std::tuple<float, float, float> > exposure_gain_score;
361 for (
int i = 1;
i < 4; ++
i)
375 exposure_gain_score.push_back(std::tuple<float, float, float>(score1, exposure1, gain1));
378 std::sort(exposure_gain_score.begin(), exposure_gain_score.end());
380 exposure = std::get<1>(exposure_gain_score.front());
381 gain = std::get<2>(exposure_gain_score.front());
385 std::vector< std::tuple<float, float, float> > exposure_gain_score;
387 for (
int i = 1;
i < 4; ++
i)
400 exposure_gain_score.push_back(std::tuple<float, float, float>(score1, exposure1, gain1));
403 std::sort(exposure_gain_score.begin(), exposure_gain_score.end());
405 exposure = std::get<1>(exposure_gain_score.front());
406 gain = std::get<2>(exposure_gain_score.front());
450 const float line_period_us = 19.33333333f;
452 float ExposureTimeLine = (exp_ms * 1000.0f / line_period_us);
455 else ExposureTimeLine =
round(ExposureTimeLine);
456 return ((
float)ExposureTimeLine * line_period_us) / 1000.0f;
463 else if (
gain > 16.0
f) {
return 16.0f; }
471 template <
typename T>
inline T
sqr(
const T&
x) {
return (
x*
x); }
474 score.under_exposure_count = 0;
475 score.over_exposure_count = 0;
479 score.under_exposure_count +=
h[
i];
481 score.shadow_limit = 0;
491 score.shadow_limit++;
498 if (lower_q > total_weight / 4)
508 score.over_exposure_count +=
h[
i];
511 score.highlight_limit = 255;
521 score.highlight_limit--;
528 if (upper_q > total_weight / 4)
539 double nn = (double)total_weight -
score.under_exposure_count -
score.over_exposure_count;
542 nn = (double)total_weight;
543 for (
int i = 0;
i <= 255; ++
i)
557 score.main_mean = (float)((
double)m1 / nn);
558 double Var = (double)m2 / nn -
sqr((
double)m1 / nn);
561 score.main_std = (float)sqrt(Var);
565 score.main_std = 0.0f;
570 : _full_width(
width), _full_height(
height), _roi_start_x(roi_start_x), _roi_start_y(roi_start_y), _width(roi_width), _height(roi_height)
582 memset(
_ncc.data(), 0,
_size *
sizeof(
double));
594 if (target_dims_size < 4)
605 if (target_dims_size == 4)
607 else if (target_dims_size == 8)
610 for (
int i = 0;
i < 4; ++
i)
644 if (max_val > min_val)
646 double factor = 1.0 / (max_val - min_val);
649 double*
q =
_img.data();
654 *
q++ = 1.0
f - (*
p++ - min_val) * factor;
665 double* pit =
_imgt.data();
667 const double* pt =
nullptr;
668 const double* qi =
nullptr;
674 double min_val = 2.0;
675 double max_val = -2.0;
678 for (
int j = 0;
j <
_ht; ++
j)
680 for (
int i = 0;
i <
_wt; ++
i)
713 for (
int k = 0; k <
_tsize2; ++k)
714 sum += *pit++ * *pt++;
731 if (max_val > min_val)
733 double factor = 1.0 / (max_val - min_val);
738 tmp = (*pncc - min_val) * factor;
746 static const int edge = 20;
851 double*
f =
_buf.data();
895 static const int pos_diff_threshold = 4;
911 rect_sides[0] =
static_cast<float>(sqrt(lx * lx + ly * ly));
915 rect_sides[1] =
static_cast<float>(sqrt(lx * lx + ly * ly));
919 rect_sides[2] =
static_cast<float>(sqrt(lx * lx + ly * ly));
923 rect_sides[3] =
static_cast<float>(sqrt(lx * lx + ly * ly));
930 for (
int i = 0;
i <
s; ++
i)
933 for (
int j = 0;
j <
s; ++
j)
935 for (
int i = 0;
i <
s; ++
i)
947 for (
int i = 0;
i <
s; ++
i)
950 for (
int j = 0;
j <
s; ++
j)
952 for (
int i = 0;
i <
s; ++
i)
964 for (
int i = 1;
i <
s; ++
i)
973 double half_mv = 0.5f * mv;
977 for (
int i = 0;
i <
s; ++
i)
986 double left_mv = 0.0f;
990 left_mv = x_0 + (half_mv -
f[x_0]) / (
f[x_1] -
f[x_0]);
993 left_mv =
static_cast<double>(0);
996 for (
int i =
s - 1;
i >= 0; --
i)
1005 double right_mv = 0.0f;
1007 right_mv =
static_cast<double>(
s) - 1;
1011 right_mv = x_0 + (half_mv -
f[x_0]) / (
f[x_1] -
f[x_0]);
1014 return (left_mv + right_mv) / 2;
1024 int dim_size =
_roi ? 4 : 8;
bool calculate(const uint8_t *img, float *target_dims, unsigned int target_dims_size) override
@ auto_exposure_anti_flicker
void minimize_x(const double *p, int s, double *f, double &x)
@ RS2_CALIB_TARGET_ROI_RECT_GAUSSIAN_DOT_VERTICES
~auto_exposure_mechanism()
void static_decrease_exposure_gain(const float &target_exposure, const float &target_exposure0, float &exposure, float &gain)
void add_frame(frame_holder frame)
std::vector< double > _img
std::vector< double > _buf
auto_exposure_mechanism(option &gain_option, option &exposure_option, const auto_exposure_state &auto_exposure_state)
void minimize_y(const double *p, int s, double *f, double &y)
void hybrid_increase_exposure_gain(const float &target_exposure, const float &target_exposure0, float &exposure, float &gain)
void calculate_rect_sides(float *rect_sides)
const std::vector< double > _template
unsigned get_auto_exposure_antiflicker_rate() const
struct rs2_frame rs2_frame
std::atomic< float > exposure_step
GLdouble GLdouble GLdouble q
void set_auto_exposure_step(float value)
uint8_t over_exposure_limit
bool validate_corners(const uint8_t *img)
float3 mult(const float3x3 &a, const float3 &b)
void increase_exposure_gain(const float &target_exposure, const float &target_exposure0, float &exposure, float &gain)
#define RS2_DEFAULT_TIMEOUT
auto_exposure_algorithm(const auto_exposure_state &auto_exposure_state)
void set_enable_auto_exposure(bool value)
rect_gaussian_dots_target_calculator(int width, int height, int roi_start_x, int roi_start_y, int roi_width, int roi_height)
void anti_flicker_decrease_exposure_gain(const float &target_exposure, const float &target_exposure0, float &exposure, float &gain)
void update_auto_exposure_roi(const region_of_interest &roi)
void update_roi(const region_of_interest &ae_roi)
void im_hist(const uint8_t *data, const region_of_interest &image_roi, const int rowStep, int h[])
bool find_metadata(rs2_frame_metadata_value, rs2_metadata_type *p_output_value) const override
float exposure_to_value(float exp_ms, rounding_mode_type rounding_mode)
float gain_to_value(float gain, rounding_mode_type rounding_mode)
GLint GLsizei GLsizei height
void increase_exposure_target(float mult, float &target_exposure)
virtual ~rect_gaussian_dots_target_calculator()
std::shared_ptr< std::thread > _exposure_thread
std::atomic< unsigned > _frames_counter
long long rs2_metadata_type
auto_exposure_modes get_auto_exposure_mode() const
std::condition_variable _cv
rs2_calib_target_type
Calibration target type.
void normalize(const uint8_t *img)
void decrease_exposure_gain(const float &target_exposure, const float &target_exposure0, float &exposure, float &gain)
double subpixel_agj(double *f, int s)
static const unsigned skip_frames
void hybrid_decrease_exposure_gain(const float &target_exposure, const float &target_exposure0, float &exposure, float &gain)
uint8_t under_exposure_limit
bool analyze_image(const frame_interface *image)
std::vector< double > _ncc
int extract_target_dims(const rs2_frame *frame_ref, float4 &rect_sides)
void update_auto_exposure_state(const auto_exposure_state &auto_exposure_state)
void decrease_exposure_target(float mult, float &target_exposure)
void set_auto_exposure_antiflicker_rate(unsigned value)
virtual void set(float value)=0
void set_auto_exposure_mode(auto_exposure_modes value)
point< double > _corners[4]
auto_exposure_algorithm _auto_exposure_algo
@ RS2_FRAME_METADATA_GAIN_LEVEL
option & _exposure_option
void anti_flicker_increase_exposure_gain(const float &target_exposure, const float &target_exposure0, float &exposure, float &gain)
void sort(sort_type m_sort_type, const std::string &in, const std::string &out)
int under_exposure_noise_limit
@ RS2_FRAME_METADATA_ACTUAL_EXPOSURE
void histogram_score(std::vector< int > &h, const int total_weight, histogram_metric &score)
std::recursive_mutex state_mutex
int over_exposure_noise_limit
const uint8_t * get_frame_data() const override
GLenum GLenum GLsizei void * image
std::atomic< bool > _keep_alive
void update_options(const auto_exposure_state &options)
void static_increase_exposure_gain(const float &target_exposure, const float &target_exposure0, float &exposure, float &gain)
static const textual_icon lock
std::atomic< unsigned > _skip_frames
float get_auto_exposure_step() const
virtual float query() const =0
std::vector< double > _imgt
const std::string options
single_consumer_queue< frame_holder > _data_queue
GLsizei const GLfloat * values
@ RS2_CALIB_TARGET_RECT_GAUSSIAN_DOT_VERTICES
bool get_enable_auto_exposure() const
void rs2_extract_target_dimensions(const rs2_frame *frame, rs2_calib_target_type calib_type, float *target_dims, unsigned int target_dims_size, rs2_error **error)
static const float ae_step_default_value
void modify_exposure(float &exposure_value, bool &exp_modified, float &gain_value, bool &gain_modified)
librealsense2
Author(s): LibRealSense ROS Team
autogenerated on Fri Aug 2 2024 08:30:00