mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-02-13 17:20:09 +00:00
u/aeg: Implement state machine to correct overshooting and avoid oscillations
This commit is contained in:
parent
bf311f3c2e
commit
85bdbc1817
doc/images
src/xrt
4
doc/images/autoexpgain.drawio.svg
Normal file
4
doc/images/autoexpgain.drawio.svg
Normal file
File diff suppressed because one or more lines are too long
After (image error) Size: 33 KiB |
3
doc/images/autoexpgain.drawio.svg.license
Normal file
3
doc/images/autoexpgain.drawio.svg.license
Normal file
|
@ -0,0 +1,3 @@
|
||||||
|
Copyright 2022, Collabora, Ltd.
|
||||||
|
|
||||||
|
SPDX-License-Identifier: BSL-1.0
|
|
@ -39,18 +39,48 @@ DEBUG_GET_ONCE_LOG_OPTION(aeg_log, "AEG_LOG", U_LOGGING_WARN)
|
||||||
|
|
||||||
#define LEVELS 256 //!< Possible pixel intensity values, only 8-bit supported
|
#define LEVELS 256 //!< Possible pixel intensity values, only 8-bit supported
|
||||||
#define INITIAL_BRIGHTNESS 0.5
|
#define INITIAL_BRIGHTNESS 0.5
|
||||||
#define INITIAL_MAX_BRIGHTNESS_STEP 0.05 //!< 0.1 is faster but introduces oscillations more often
|
#define INITIAL_MAX_BRIGHTNESS_STEP 0.1
|
||||||
#define INITIAL_THRESHOLD 0.1
|
#define INITIAL_THRESHOLD 0.1
|
||||||
#define GRID_COLS 40 //!< Amount of columns for the histogram sample grid
|
#define GRID_COLS 40 //!< Amount of columns for the histogram sample grid
|
||||||
|
|
||||||
|
//! AEG State machine states
|
||||||
|
enum u_aeg_state
|
||||||
|
{
|
||||||
|
IDLE,
|
||||||
|
BRIGHTEN,
|
||||||
|
STOP_BRIGHTEN, //!< Avoid oscillations by
|
||||||
|
DARKEN,
|
||||||
|
STOP_DARKEN, //!< Similar to STOP_BRIGHTEN
|
||||||
|
};
|
||||||
|
|
||||||
|
//! This actions are triggered when the image is too dark, bright or good enough
|
||||||
|
enum u_aeg_action
|
||||||
|
{
|
||||||
|
GOOD,
|
||||||
|
DARK,
|
||||||
|
BRIGHT,
|
||||||
|
};
|
||||||
|
|
||||||
//! Auto exposure and gain (AEG) adjustment algorithm state.
|
//! Auto exposure and gain (AEG) adjustment algorithm state.
|
||||||
struct u_autoexpgain
|
struct u_autoexpgain
|
||||||
{
|
{
|
||||||
bool enable; //!< Whether to enable auto exposure and gain adjustment
|
bool enable; //!< Whether to enable auto exposure and gain adjustment
|
||||||
|
|
||||||
|
//! AEG is a finite state machine. @see set_state.
|
||||||
|
enum u_aeg_state state;
|
||||||
|
|
||||||
enum u_logging_level log_level;
|
enum u_logging_level log_level;
|
||||||
|
|
||||||
//! Algorithm strategy that affects how score and brightness are computed
|
//! Counts how many times we've overshooted in the last brightness change.
|
||||||
|
//! It's then used for exponential backoff of the brightness step.
|
||||||
|
int overshoots;
|
||||||
|
|
||||||
|
//! There are buffer states that wait `frame_delay` frames to ensure we are
|
||||||
|
//! not overshooting. This field counts the remaining frames to wait.
|
||||||
|
//! @see set_state
|
||||||
|
int wait;
|
||||||
|
|
||||||
|
//! The selected strategy affects various targets of the algorithm.
|
||||||
enum u_aeg_strategy strategy;
|
enum u_aeg_strategy strategy;
|
||||||
struct u_var_combo strategy_combo; //!< UI combo box for selecting `strategy`
|
struct u_var_combo strategy_combo; //!< UI combo box for selecting `strategy`
|
||||||
|
|
||||||
|
@ -70,20 +100,146 @@ struct u_autoexpgain
|
||||||
//! images with a good enough `brightness` value.
|
//! images with a good enough `brightness` value.
|
||||||
float current_score;
|
float current_score;
|
||||||
|
|
||||||
//! Scores further than `threshold` from zero will trigger a `brightness` update.
|
//! Scores further than `threshold` from the target score will trigger a
|
||||||
|
//! `brightness` update.
|
||||||
float threshold;
|
float threshold;
|
||||||
|
|
||||||
uint32_t frame_counter; //!< Number of frames received
|
//! A camera might take a couple of frames until the new exposure/gain sets in
|
||||||
|
//! the image. Knowing how many (this variable) helps in avoiding overshooting
|
||||||
//! Every how many frames should we update `brightness`. Some cameras take a
|
//! brightness changes.
|
||||||
//! couple of frames until the new exposure/gain sets in and a new score can
|
int frame_delay;
|
||||||
//! be recomputed properly.
|
|
||||||
uint8_t update_every;
|
|
||||||
|
|
||||||
float exposure; //!< Currently computed exposure value to use
|
float exposure; //!< Currently computed exposure value to use
|
||||||
float gain; //!< Currently computed gain value to use
|
float gain; //!< Currently computed gain value to use
|
||||||
};
|
};
|
||||||
|
|
||||||
|
static const char *
|
||||||
|
state_to_string(enum u_aeg_state state)
|
||||||
|
{
|
||||||
|
if (state == IDLE) {
|
||||||
|
return "IDLE";
|
||||||
|
} else if (state == BRIGHTEN) {
|
||||||
|
return "BRIGHTEN";
|
||||||
|
} else if (state == STOP_BRIGHTEN) {
|
||||||
|
return "STOP_BRIGHTEN";
|
||||||
|
} else if (state == DARKEN) {
|
||||||
|
return "DARKEN";
|
||||||
|
} else if (state == STOP_DARKEN) {
|
||||||
|
return "STOP_DARKEN";
|
||||||
|
} else {
|
||||||
|
AEG_ASSERT_(false);
|
||||||
|
}
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
static const char *
|
||||||
|
action_to_string(enum u_aeg_action action)
|
||||||
|
{
|
||||||
|
if (action == DARK) {
|
||||||
|
return "DARK";
|
||||||
|
} else if (action == BRIGHT) {
|
||||||
|
return "BRIGHT";
|
||||||
|
} else if (action == GOOD) {
|
||||||
|
return "GOOD";
|
||||||
|
} else {
|
||||||
|
AEG_ASSERT_(false);
|
||||||
|
}
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*!
|
||||||
|
* Defines the AEG state machine transitions.
|
||||||
|
* The main idea is that if brightness needs to change then we go from `IDLE` to
|
||||||
|
* `BRIGHTEN`/`DARKEN`. To avoid oscillations we detect overshootings
|
||||||
|
* and exponentially backoff our brightness step. We only reset our `overshoots`
|
||||||
|
* counter after the image have been good for `frame_delay` frames, this delay
|
||||||
|
* is counted during `STOP_DARKEN`/`STOP_BRIGHTEN` states.
|
||||||
|
*
|
||||||
|
* A diagram of the state machine is below:
|
||||||
|
* ![AEG state machine](images/autoexpgain.drawio.svg)
|
||||||
|
*/
|
||||||
|
static void
|
||||||
|
set_state(struct u_autoexpgain *aeg, enum u_aeg_action action)
|
||||||
|
{
|
||||||
|
enum u_aeg_state new_state;
|
||||||
|
if (aeg->state == IDLE) {
|
||||||
|
if (action == DARK) {
|
||||||
|
new_state = BRIGHTEN;
|
||||||
|
} else if (action == BRIGHT) {
|
||||||
|
new_state = DARKEN;
|
||||||
|
} else if (action == GOOD) {
|
||||||
|
new_state = IDLE;
|
||||||
|
} else {
|
||||||
|
AEG_ASSERT_(false);
|
||||||
|
}
|
||||||
|
} else if (aeg->state == BRIGHTEN) {
|
||||||
|
if (action == DARK) {
|
||||||
|
new_state = BRIGHTEN;
|
||||||
|
} else if (action == BRIGHT) {
|
||||||
|
aeg->overshoots++;
|
||||||
|
new_state = DARKEN;
|
||||||
|
} else if (action == GOOD) {
|
||||||
|
new_state = STOP_BRIGHTEN;
|
||||||
|
} else {
|
||||||
|
AEG_ASSERT_(false);
|
||||||
|
}
|
||||||
|
} else if (aeg->state == STOP_BRIGHTEN) {
|
||||||
|
if (action == DARK) {
|
||||||
|
new_state = BRIGHTEN;
|
||||||
|
} else if (action == BRIGHT) {
|
||||||
|
aeg->overshoots++;
|
||||||
|
new_state = DARKEN;
|
||||||
|
} else if (action == GOOD) {
|
||||||
|
aeg->wait--;
|
||||||
|
new_state = aeg->wait == 0 ? IDLE : STOP_BRIGHTEN;
|
||||||
|
} else {
|
||||||
|
AEG_ASSERT_(false);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (new_state != STOP_BRIGHTEN) {
|
||||||
|
aeg->wait = aeg->frame_delay;
|
||||||
|
}
|
||||||
|
} else if (aeg->state == DARKEN) {
|
||||||
|
if (action == DARK) {
|
||||||
|
aeg->overshoots++;
|
||||||
|
new_state = BRIGHTEN;
|
||||||
|
} else if (action == BRIGHT) {
|
||||||
|
new_state = DARKEN;
|
||||||
|
} else if (action == GOOD) {
|
||||||
|
new_state = STOP_DARKEN;
|
||||||
|
} else {
|
||||||
|
AEG_ASSERT_(false);
|
||||||
|
}
|
||||||
|
} else if (aeg->state == STOP_DARKEN) {
|
||||||
|
if (action == DARK) {
|
||||||
|
aeg->overshoots++;
|
||||||
|
new_state = BRIGHTEN;
|
||||||
|
} else if (action == BRIGHT) {
|
||||||
|
new_state = DARKEN;
|
||||||
|
} else if (action == GOOD) {
|
||||||
|
aeg->wait--;
|
||||||
|
new_state = aeg->wait == 0 ? IDLE : STOP_DARKEN;
|
||||||
|
} else {
|
||||||
|
AEG_ASSERT_(false);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (new_state != STOP_DARKEN) {
|
||||||
|
aeg->wait = aeg->frame_delay;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
AEG_ASSERT_(false);
|
||||||
|
}
|
||||||
|
if (new_state == IDLE) {
|
||||||
|
aeg->overshoots = 0;
|
||||||
|
}
|
||||||
|
aeg->overshoots = CLAMP(aeg->overshoots, 0, 3);
|
||||||
|
|
||||||
|
AEG_TRACE("[%s] ---%s--> [%s] (overshoots=%d, wait=%d)", state_to_string(aeg->state), action_to_string(action),
|
||||||
|
state_to_string(new_state), aeg->overshoots, aeg->wait);
|
||||||
|
|
||||||
|
aeg->state = new_state;
|
||||||
|
}
|
||||||
|
|
||||||
//! Maps a `brightness` in [0, 1] to a pair of exposure and gain values based on
|
//! Maps a `brightness` in [0, 1] to a pair of exposure and gain values based on
|
||||||
//! a piecewise function.
|
//! a piecewise function.
|
||||||
static void
|
static void
|
||||||
|
@ -227,19 +383,33 @@ update_brightness(struct u_autoexpgain *aeg, struct xrt_frame *xf)
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
aeg->frame_counter++;
|
float target_score;
|
||||||
if (aeg->frame_counter % aeg->update_every != 0) {
|
if (aeg->strategy == U_AEG_STRATEGY_TRACKING) {
|
||||||
return;
|
target_score = -aeg->threshold; // Makes 0 the right bound of our "good enugh" range
|
||||||
|
} else if (aeg->strategy == U_AEG_STRATEGY_DYNAMIC_RANGE) {
|
||||||
|
target_score = 0;
|
||||||
|
} else {
|
||||||
|
AEG_ASSERT(false, "Unexpected strategy=%d", aeg->strategy);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool score_is_high = fabsf(score) > aeg->threshold;
|
enum u_aeg_action action; // State machine input action
|
||||||
if (!score_is_high) {
|
if (score > target_score + aeg->threshold) {
|
||||||
|
action = BRIGHT;
|
||||||
|
} else if (score < target_score - aeg->threshold) {
|
||||||
|
action = DARK;
|
||||||
|
} else {
|
||||||
|
action = GOOD;
|
||||||
|
}
|
||||||
|
|
||||||
|
set_state(aeg, action);
|
||||||
|
|
||||||
|
if (aeg->state != BRIGHTEN && aeg->state != DARKEN) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
float max_step = aeg->max_brightness_step;
|
float max_step = aeg->max_brightness_step;
|
||||||
float step = CLAMP(max_step * score, -max_step, max_step);
|
float step = max_step * score / powf(2.0f, aeg->overshoots);
|
||||||
aeg->brightness.val -= step;
|
aeg->brightness.val -= CLAMP(step, -max_step, max_step);
|
||||||
aeg->brightness.val = CLAMP(aeg->brightness.val, 0, 1);
|
aeg->brightness.val = CLAMP(aeg->brightness.val, 0, 1);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -250,12 +420,15 @@ update_brightness(struct u_autoexpgain *aeg, struct xrt_frame *xf)
|
||||||
*/
|
*/
|
||||||
|
|
||||||
struct u_autoexpgain *
|
struct u_autoexpgain *
|
||||||
u_autoexpgain_create(enum u_aeg_strategy strategy, bool enabled_from_start, uint8_t update_every)
|
u_autoexpgain_create(enum u_aeg_strategy strategy, bool enabled_from_start, int frame_delay)
|
||||||
{
|
{
|
||||||
struct u_autoexpgain *aeg = U_TYPED_CALLOC(struct u_autoexpgain);
|
struct u_autoexpgain *aeg = U_TYPED_CALLOC(struct u_autoexpgain);
|
||||||
|
|
||||||
aeg->enable = enabled_from_start;
|
aeg->enable = enabled_from_start;
|
||||||
aeg->log_level = debug_get_log_option_aeg_log();
|
aeg->log_level = debug_get_log_option_aeg_log();
|
||||||
|
aeg->state = IDLE;
|
||||||
|
aeg->wait = frame_delay;
|
||||||
|
aeg->overshoots = 0;
|
||||||
aeg->strategy = strategy;
|
aeg->strategy = strategy;
|
||||||
aeg->strategy_combo.count = U_AEG_STRATEGY_COUNT;
|
aeg->strategy_combo.count = U_AEG_STRATEGY_COUNT;
|
||||||
aeg->strategy_combo.options = "Tracking\0Dynamic Range\0\0";
|
aeg->strategy_combo.options = "Tracking\0Dynamic Range\0\0";
|
||||||
|
@ -272,8 +445,7 @@ u_autoexpgain_create(enum u_aeg_strategy strategy, bool enabled_from_start, uint
|
||||||
aeg->max_brightness_step = INITIAL_MAX_BRIGHTNESS_STEP;
|
aeg->max_brightness_step = INITIAL_MAX_BRIGHTNESS_STEP;
|
||||||
|
|
||||||
aeg->threshold = INITIAL_THRESHOLD;
|
aeg->threshold = INITIAL_THRESHOLD;
|
||||||
aeg->frame_counter = 0;
|
aeg->frame_delay = frame_delay;
|
||||||
aeg->update_every = update_every;
|
|
||||||
|
|
||||||
brightness_to_expgain(aeg, INITIAL_BRIGHTNESS, &aeg->exposure, &aeg->gain);
|
brightness_to_expgain(aeg, INITIAL_BRIGHTNESS, &aeg->exposure, &aeg->gain);
|
||||||
|
|
||||||
|
@ -284,7 +456,7 @@ void
|
||||||
u_autoexpgain_add_vars(struct u_autoexpgain *aeg, void *root)
|
u_autoexpgain_add_vars(struct u_autoexpgain *aeg, void *root)
|
||||||
{
|
{
|
||||||
u_var_add_bool(root, &aeg->enable, "Update brightness automatically");
|
u_var_add_bool(root, &aeg->enable, "Update brightness automatically");
|
||||||
u_var_add_u8(root, &aeg->update_every, "Update every X frames");
|
u_var_add_i32(root, &aeg->frame_delay, "Frame update delay");
|
||||||
u_var_add_combo(root, &aeg->strategy_combo, "Strategy");
|
u_var_add_combo(root, &aeg->strategy_combo, "Strategy");
|
||||||
u_var_add_draggable_f32(root, &aeg->brightness, "Brightness");
|
u_var_add_draggable_f32(root, &aeg->brightness, "Brightness");
|
||||||
u_var_add_f32(root, &aeg->threshold, "Score threshold");
|
u_var_add_f32(root, &aeg->threshold, "Score threshold");
|
||||||
|
|
|
@ -31,11 +31,11 @@ struct u_autoexpgain;
|
||||||
*
|
*
|
||||||
* @param strategy What objective is preferred for the algorithm.
|
* @param strategy What objective is preferred for the algorithm.
|
||||||
* @param enabled_from_start Update exposure/gain from the start.
|
* @param enabled_from_start Update exposure/gain from the start.
|
||||||
* @param update_every Every how many frames should we update exposure/gain
|
* @param frame_delay About how many frames does it take for exp and gain to settle in.
|
||||||
* @return struct u_autoexpgain* Created object
|
* @return struct u_autoexpgain* Created object
|
||||||
*/
|
*/
|
||||||
struct u_autoexpgain *
|
struct u_autoexpgain *
|
||||||
u_autoexpgain_create(enum u_aeg_strategy strategy, bool enabled_from_start, uint8_t update_every);
|
u_autoexpgain_create(enum u_aeg_strategy strategy, bool enabled_from_start, int frame_delay);
|
||||||
|
|
||||||
//! Setup UI for the AEG algorithm
|
//! Setup UI for the AEG algorithm
|
||||||
void
|
void
|
||||||
|
|
|
@ -447,8 +447,8 @@ wmr_camera_open(struct xrt_prober_device *dev_holo,
|
||||||
}
|
}
|
||||||
|
|
||||||
bool enable_aeg = debug_get_bool_option_wmr_autoexposure();
|
bool enable_aeg = debug_get_bool_option_wmr_autoexposure();
|
||||||
int aeg_update_every = 3; // WMR takes about three frames until the cmd changes the image
|
int frame_delay = 3; // WMR takes about three frames until the cmd changes the image
|
||||||
cam->aeg = u_autoexpgain_create(U_AEG_STRATEGY_TRACKING, enable_aeg, aeg_update_every);
|
cam->aeg = u_autoexpgain_create(U_AEG_STRATEGY_TRACKING, enable_aeg, frame_delay);
|
||||||
|
|
||||||
cam->exposure_ui.val = &cam->exposure;
|
cam->exposure_ui.val = &cam->exposure;
|
||||||
cam->exposure_ui.max = WMR_MAX_EXPOSURE;
|
cam->exposure_ui.max = WMR_MAX_EXPOSURE;
|
||||||
|
|
|
@ -55,7 +55,7 @@ extern "C" {
|
||||||
#define STR_TO_U32(s) ((uint32_t)(((s)[0]) | ((s)[1] << 8) | ((s)[2] << 16) | ((s)[3] << 24)))
|
#define STR_TO_U32(s) ((uint32_t)(((s)[0]) | ((s)[1] << 8) | ((s)[2] << 16) | ((s)[3] << 24)))
|
||||||
#define WMR_MAGIC STR_TO_U32("Dlo+")
|
#define WMR_MAGIC STR_TO_U32("Dlo+")
|
||||||
|
|
||||||
#define WMR_MIN_EXPOSURE 120
|
#define WMR_MIN_EXPOSURE 60
|
||||||
#define WMR_MAX_OBSERVED_EXPOSURE 6000
|
#define WMR_MAX_OBSERVED_EXPOSURE 6000
|
||||||
#define WMR_MAX_EXPOSURE 9000
|
#define WMR_MAX_EXPOSURE 9000
|
||||||
#define WMR_MIN_GAIN 16
|
#define WMR_MIN_GAIN 16
|
||||||
|
|
Loading…
Reference in a new issue