···39394040#define LEVELS 256 //!< Possible pixel intensity values, only 8-bit supported
4141#define INITIAL_BRIGHTNESS 0.5
4242-#define INITIAL_MAX_BRIGHTNESS_STEP 0.05 //!< 0.1 is faster but introduces oscillations more often
4242+#define INITIAL_MAX_BRIGHTNESS_STEP 0.1
4343#define INITIAL_THRESHOLD 0.1
4444#define GRID_COLS 40 //!< Amount of columns for the histogram sample grid
45454646+//! AEG State machine states
4747+enum u_aeg_state
4848+{
4949+ IDLE,
5050+ BRIGHTEN,
5151+ STOP_BRIGHTEN, //!< Avoid oscillations by
5252+ DARKEN,
5353+ STOP_DARKEN, //!< Similar to STOP_BRIGHTEN
5454+};
5555+5656+//! This actions are triggered when the image is too dark, bright or good enough
5757+enum u_aeg_action
5858+{
5959+ GOOD,
6060+ DARK,
6161+ BRIGHT,
6262+};
6363+4664//! Auto exposure and gain (AEG) adjustment algorithm state.
4765struct u_autoexpgain
4866{
4967 bool enable; //!< Whether to enable auto exposure and gain adjustment
50686969+ //! AEG is a finite state machine. @see set_state.
7070+ enum u_aeg_state state;
7171+5172 enum u_logging_level log_level;
52735353- //! Algorithm strategy that affects how score and brightness are computed
7474+ //! Counts how many times we've overshooted in the last brightness change.
7575+ //! It's then used for exponential backoff of the brightness step.
7676+ int overshoots;
7777+7878+ //! There are buffer states that wait `frame_delay` frames to ensure we are
7979+ //! not overshooting. This field counts the remaining frames to wait.
8080+ //! @see set_state
8181+ int wait;
8282+8383+ //! The selected strategy affects various targets of the algorithm.
5484 enum u_aeg_strategy strategy;
5585 struct u_var_combo strategy_combo; //!< UI combo box for selecting `strategy`
5686···70100 //! images with a good enough `brightness` value.
71101 float current_score;
721027373- //! Scores further than `threshold` from zero will trigger a `brightness` update.
103103+ //! Scores further than `threshold` from the target score will trigger a
104104+ //! `brightness` update.
74105 float threshold;
751067676- uint32_t frame_counter; //!< Number of frames received
7777-7878- //! Every how many frames should we update `brightness`. Some cameras take a
7979- //! couple of frames until the new exposure/gain sets in and a new score can
8080- //! be recomputed properly.
8181- uint8_t update_every;
107107+ //! A camera might take a couple of frames until the new exposure/gain sets in
108108+ //! the image. Knowing how many (this variable) helps in avoiding overshooting
109109+ //! brightness changes.
110110+ int frame_delay;
8211183112 float exposure; //!< Currently computed exposure value to use
84113 float gain; //!< Currently computed gain value to use
85114};
86115116116+static const char *
117117+state_to_string(enum u_aeg_state state)
118118+{
119119+ if (state == IDLE) {
120120+ return "IDLE";
121121+ } else if (state == BRIGHTEN) {
122122+ return "BRIGHTEN";
123123+ } else if (state == STOP_BRIGHTEN) {
124124+ return "STOP_BRIGHTEN";
125125+ } else if (state == DARKEN) {
126126+ return "DARKEN";
127127+ } else if (state == STOP_DARKEN) {
128128+ return "STOP_DARKEN";
129129+ } else {
130130+ AEG_ASSERT_(false);
131131+ }
132132+ return NULL;
133133+}
134134+135135+static const char *
136136+action_to_string(enum u_aeg_action action)
137137+{
138138+ if (action == DARK) {
139139+ return "DARK";
140140+ } else if (action == BRIGHT) {
141141+ return "BRIGHT";
142142+ } else if (action == GOOD) {
143143+ return "GOOD";
144144+ } else {
145145+ AEG_ASSERT_(false);
146146+ }
147147+ return NULL;
148148+}
149149+150150+/*!
151151+ * Defines the AEG state machine transitions.
152152+ * The main idea is that if brightness needs to change then we go from `IDLE` to
153153+ * `BRIGHTEN`/`DARKEN`. To avoid oscillations we detect overshootings
154154+ * and exponentially backoff our brightness step. We only reset our `overshoots`
155155+ * counter after the image have been good for `frame_delay` frames, this delay
156156+ * is counted during `STOP_DARKEN`/`STOP_BRIGHTEN` states.
157157+ *
158158+ * A diagram of the state machine is below:
159159+ * 
160160+ */
161161+static void
162162+set_state(struct u_autoexpgain *aeg, enum u_aeg_action action)
163163+{
164164+ enum u_aeg_state new_state;
165165+ if (aeg->state == IDLE) {
166166+ if (action == DARK) {
167167+ new_state = BRIGHTEN;
168168+ } else if (action == BRIGHT) {
169169+ new_state = DARKEN;
170170+ } else if (action == GOOD) {
171171+ new_state = IDLE;
172172+ } else {
173173+ AEG_ASSERT_(false);
174174+ }
175175+ } else if (aeg->state == BRIGHTEN) {
176176+ if (action == DARK) {
177177+ new_state = BRIGHTEN;
178178+ } else if (action == BRIGHT) {
179179+ aeg->overshoots++;
180180+ new_state = DARKEN;
181181+ } else if (action == GOOD) {
182182+ new_state = STOP_BRIGHTEN;
183183+ } else {
184184+ AEG_ASSERT_(false);
185185+ }
186186+ } else if (aeg->state == STOP_BRIGHTEN) {
187187+ if (action == DARK) {
188188+ new_state = BRIGHTEN;
189189+ } else if (action == BRIGHT) {
190190+ aeg->overshoots++;
191191+ new_state = DARKEN;
192192+ } else if (action == GOOD) {
193193+ aeg->wait--;
194194+ new_state = aeg->wait == 0 ? IDLE : STOP_BRIGHTEN;
195195+ } else {
196196+ AEG_ASSERT_(false);
197197+ }
198198+199199+ if (new_state != STOP_BRIGHTEN) {
200200+ aeg->wait = aeg->frame_delay;
201201+ }
202202+ } else if (aeg->state == DARKEN) {
203203+ if (action == DARK) {
204204+ aeg->overshoots++;
205205+ new_state = BRIGHTEN;
206206+ } else if (action == BRIGHT) {
207207+ new_state = DARKEN;
208208+ } else if (action == GOOD) {
209209+ new_state = STOP_DARKEN;
210210+ } else {
211211+ AEG_ASSERT_(false);
212212+ }
213213+ } else if (aeg->state == STOP_DARKEN) {
214214+ if (action == DARK) {
215215+ aeg->overshoots++;
216216+ new_state = BRIGHTEN;
217217+ } else if (action == BRIGHT) {
218218+ new_state = DARKEN;
219219+ } else if (action == GOOD) {
220220+ aeg->wait--;
221221+ new_state = aeg->wait == 0 ? IDLE : STOP_DARKEN;
222222+ } else {
223223+ AEG_ASSERT_(false);
224224+ }
225225+226226+ if (new_state != STOP_DARKEN) {
227227+ aeg->wait = aeg->frame_delay;
228228+ }
229229+ } else {
230230+ AEG_ASSERT_(false);
231231+ }
232232+ if (new_state == IDLE) {
233233+ aeg->overshoots = 0;
234234+ }
235235+ aeg->overshoots = CLAMP(aeg->overshoots, 0, 3);
236236+237237+ AEG_TRACE("[%s] ---%s--> [%s] (overshoots=%d, wait=%d)", state_to_string(aeg->state), action_to_string(action),
238238+ state_to_string(new_state), aeg->overshoots, aeg->wait);
239239+240240+ aeg->state = new_state;
241241+}
242242+87243//! Maps a `brightness` in [0, 1] to a pair of exposure and gain values based on
88244//! a piecewise function.
89245static void
···227383 return;
228384 }
229385230230- aeg->frame_counter++;
231231- if (aeg->frame_counter % aeg->update_every != 0) {
232232- return;
386386+ float target_score;
387387+ if (aeg->strategy == U_AEG_STRATEGY_TRACKING) {
388388+ target_score = -aeg->threshold; // Makes 0 the right bound of our "good enugh" range
389389+ } else if (aeg->strategy == U_AEG_STRATEGY_DYNAMIC_RANGE) {
390390+ target_score = 0;
391391+ } else {
392392+ AEG_ASSERT(false, "Unexpected strategy=%d", aeg->strategy);
233393 }
234394235235- bool score_is_high = fabsf(score) > aeg->threshold;
236236- if (!score_is_high) {
395395+ enum u_aeg_action action; // State machine input action
396396+ if (score > target_score + aeg->threshold) {
397397+ action = BRIGHT;
398398+ } else if (score < target_score - aeg->threshold) {
399399+ action = DARK;
400400+ } else {
401401+ action = GOOD;
402402+ }
403403+404404+ set_state(aeg, action);
405405+406406+ if (aeg->state != BRIGHTEN && aeg->state != DARKEN) {
237407 return;
238408 }
239409240410 float max_step = aeg->max_brightness_step;
241241- float step = CLAMP(max_step * score, -max_step, max_step);
242242- aeg->brightness.val -= step;
411411+ float step = max_step * score / powf(2.0f, aeg->overshoots);
412412+ aeg->brightness.val -= CLAMP(step, -max_step, max_step);
243413 aeg->brightness.val = CLAMP(aeg->brightness.val, 0, 1);
244414}
245415···250420 */
251421252422struct u_autoexpgain *
253253-u_autoexpgain_create(enum u_aeg_strategy strategy, bool enabled_from_start, uint8_t update_every)
423423+u_autoexpgain_create(enum u_aeg_strategy strategy, bool enabled_from_start, int frame_delay)
254424{
255425 struct u_autoexpgain *aeg = U_TYPED_CALLOC(struct u_autoexpgain);
256426257427 aeg->enable = enabled_from_start;
258428 aeg->log_level = debug_get_log_option_aeg_log();
429429+ aeg->state = IDLE;
430430+ aeg->wait = frame_delay;
431431+ aeg->overshoots = 0;
259432 aeg->strategy = strategy;
260433 aeg->strategy_combo.count = U_AEG_STRATEGY_COUNT;
261434 aeg->strategy_combo.options = "Tracking\0Dynamic Range\0\0";
···272445 aeg->max_brightness_step = INITIAL_MAX_BRIGHTNESS_STEP;
273446274447 aeg->threshold = INITIAL_THRESHOLD;
275275- aeg->frame_counter = 0;
276276- aeg->update_every = update_every;
448448+ aeg->frame_delay = frame_delay;
277449278450 brightness_to_expgain(aeg, INITIAL_BRIGHTNESS, &aeg->exposure, &aeg->gain);
279451···284456u_autoexpgain_add_vars(struct u_autoexpgain *aeg, void *root)
285457{
286458 u_var_add_bool(root, &aeg->enable, "Update brightness automatically");
287287- u_var_add_u8(root, &aeg->update_every, "Update every X frames");
459459+ u_var_add_i32(root, &aeg->frame_delay, "Frame update delay");
288460 u_var_add_combo(root, &aeg->strategy_combo, "Strategy");
289461 u_var_add_draggable_f32(root, &aeg->brightness, "Brightness");
290462 u_var_add_f32(root, &aeg->threshold, "Score threshold");
+2-2
src/xrt/auxiliary/util/u_autoexpgain.h
···3131 *
3232 * @param strategy What objective is preferred for the algorithm.
3333 * @param enabled_from_start Update exposure/gain from the start.
3434- * @param update_every Every how many frames should we update exposure/gain
3434+ * @param frame_delay About how many frames does it take for exp and gain to settle in.
3535 * @return struct u_autoexpgain* Created object
3636 */
3737struct u_autoexpgain *
3838-u_autoexpgain_create(enum u_aeg_strategy strategy, bool enabled_from_start, uint8_t update_every);
3838+u_autoexpgain_create(enum u_aeg_strategy strategy, bool enabled_from_start, int frame_delay);
39394040//! Setup UI for the AEG algorithm
4141void
+2-2
src/xrt/drivers/wmr/wmr_camera.c
···447447 }
448448449449 bool enable_aeg = debug_get_bool_option_wmr_autoexposure();
450450- int aeg_update_every = 3; // WMR takes about three frames until the cmd changes the image
451451- cam->aeg = u_autoexpgain_create(U_AEG_STRATEGY_TRACKING, enable_aeg, aeg_update_every);
450450+ int frame_delay = 3; // WMR takes about three frames until the cmd changes the image
451451+ cam->aeg = u_autoexpgain_create(U_AEG_STRATEGY_TRACKING, enable_aeg, frame_delay);
452452453453 cam->exposure_ui.val = &cam->exposure;
454454 cam->exposure_ui.max = WMR_MAX_EXPOSURE;