diff --git a/src/ipa/raspberrypi/controller/af_algorithm.hpp b/src/ipa/raspberrypi/controller/af_algorithm.hpp new file mode 100644 index 00000000..553a37e1 --- /dev/null +++ b/src/ipa/raspberrypi/controller/af_algorithm.hpp @@ -0,0 +1,20 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi (Trading) Limited + * + * af_algorithm.hpp - autofocus control algorithm interface + */ +#pragma once + +#include "algorithm.hpp" + +namespace RPiController { + +class AfAlgorithm : public Algorithm +{ +public: + AfAlgorithm(Controller *controller) : Algorithm(controller) {} + // An af algorithm must provide the following: +}; + +} // namespace RPiController diff --git a/src/ipa/raspberrypi/controller/af_status.h b/src/ipa/raspberrypi/controller/af_status.h new file mode 100644 index 00000000..835e1e2f --- /dev/null +++ b/src/ipa/raspberrypi/controller/af_status.h @@ -0,0 +1,31 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2020, Raspberry Pi (Trading) Limited + * Copyright (C) 2022, Ideas On Board + * + * af_status.h - autofocus measurement status + */ +#pragma once + +#include + +/* + * The focus algorithm should post the following structure into the image's + * "af.status" metadata. + */ + +#ifdef __cplusplus +extern "C" { +#endif + +struct AfStatus { + unsigned int num; + uint32_t focus_measures[FOCUS_REGIONS]; + bool stable; + uint32_t focus; + double maxVariance; +}; + +#ifdef __cplusplus +} +#endif diff --git a/src/ipa/raspberrypi/controller/focus_status.h b/src/ipa/raspberrypi/controller/focus_status.h index ace2fe2c..8122df4b 100644 --- a/src/ipa/raspberrypi/controller/focus_status.h +++ b/src/ipa/raspberrypi/controller/focus_status.h @@ -19,6 +19,9 @@ extern "C" { struct FocusStatus { unsigned int num; uint32_t focus_measures[FOCUS_REGIONS]; + bool stable; + uint32_t focus; + double maxVariance; }; #ifdef __cplusplus diff --git a/src/ipa/raspberrypi/controller/iob/af.cpp b/src/ipa/raspberrypi/controller/iob/af.cpp new file mode 100644 index 00000000..dc5258ba --- /dev/null +++ b/src/ipa/raspberrypi/controller/iob/af.cpp @@ -0,0 +1,231 @@ +/* SPDX-License-Identifier: LGPL-2.1-or-later */ +/* + * Copyright (C) 2021, Red Hat + * Copyright (C) 2022, Ideas On Board + * + * af.cpp - automatic contrast-based focus algorithm + */ +#include + +#include + +#include + +#include "af.h" + +using namespace RPiController; +using namespace libcamera; + +LOG_DEFINE_CATEGORY(IoBAf) + +#define NAME "iob.af" + +/* + * Maximum focus steps of the VCM control + * \todo should be obtained from the VCM driver + */ +static constexpr uint32_t kMaxFocusSteps = 1023; + +/* Minimum focus step for searching appropriate focus */ +static constexpr uint32_t kCoarseSearchStep = 30; +static constexpr uint32_t kFineSearchStep = 1; + +/* Max ratio of variance change, 0.0 < kMaxChange < 1.0 */ +static constexpr double kMaxChange = 0.5; + +/* The numbers of frame to be ignored, before performing focus scan. */ +static constexpr uint32_t kIgnoreFrame = 10; + +/* Fine scan range 0 < kFineRange < 1 */ +static constexpr double kFineRange = 0.05; + +Af::Af(Controller *controller) + : AfAlgorithm(controller), focus_(0), bestFocus_(0), ignoreCounter_(0), + currentVariance_(0.0), previousVariance_(0.0), maxStep_(0), + coarseCompleted_(false), fineCompleted_(false) +{ +} + +char const *Af::Name() const +{ + return NAME; +} + +void Af::Initialise() +{ + status_.focus = 0.0; + status_.maxVariance = 0.0; + status_.stable = false; +} + +void Af::Prepare(Metadata *image_metadata) +{ + image_metadata->Set("af.status", status_); +} + +double Af::estimateVariance() +{ + unsigned int i; + double mean; + uint64_t total = 0; + double var_sum = 0.0; + + /* Compute the mean value. */ + for (i = 0; i < FOCUS_REGIONS; i++) + total += status_.focus_measures[i]; + mean = total / FOCUS_REGIONS; + + /* Compute the sum of the squared variance. */ + for (i = 0; i < FOCUS_REGIONS; i++) + var_sum += std::pow(status_.focus_measures[i] - mean, 2); + + return var_sum / FOCUS_REGIONS; +} + +bool Af::afNeedIgnoreFrame() +{ + if (ignoreCounter_ == 0) + return false; + else + ignoreCounter_--; + return true; +} + +void Af::afCoarseScan() +{ + if (coarseCompleted_) + return; + + if (afNeedIgnoreFrame()) + return; + + if (afScan(kCoarseSearchStep)) { + coarseCompleted_ = true; + status_.maxVariance = 0; + focus_ = status_.focus - (status_.focus * kFineRange); + status_.focus = focus_; + previousVariance_ = 0; + maxStep_ = std::clamp(focus_ + static_cast((focus_ * kFineRange)), + 0U, kMaxFocusSteps); + } +} + +void Af::afFineScan() +{ + if (!coarseCompleted_) + return; + + if (afNeedIgnoreFrame()) + return; + + if (afScan(kFineSearchStep)) { + status_.stable = true; + fineCompleted_ = true; + } +} + +bool Af::afScan(uint32_t minSteps) +{ + if (focus_ > maxStep_) { + /* If the max step is reached, move lens to the position. */ + status_.focus = bestFocus_; + return true; + } else { + /* + * Find the maximum of the variance by estimating its + * derivative. If the direction changes, it means we have passed + * a maximum one step before. + */ + if ((currentVariance_ - status_.maxVariance) >= + -(status_.maxVariance * 0.1)) { + /* + * Positive and zero derivative: + * The variance is still increasing. The focus could be + * increased for the next comparison. Also, the max + * variance and previous focus value are updated. + */ + bestFocus_ = focus_; + focus_ += minSteps; + status_.focus = focus_; + status_.maxVariance = currentVariance_; + } else { + /* + * Negative derivative: + * The variance starts to decrease which means the maximum + * variance is found. Set focus step to previous good one + * then return immediately. + */ + status_.focus = bestFocus_; + return true; + } + } + + previousVariance_ = currentVariance_; + LOG(IoBAf, Debug) << " Previous step is " + << bestFocus_ + << " Current step is " + << focus_; + return false; +} + +void Af::afReset() +{ + if (afNeedIgnoreFrame()) + return; + + status_.maxVariance = 0; + status_.focus = 0; + focus_ = 0; + status_.stable = false; + ignoreCounter_ = kIgnoreFrame; + previousVariance_ = 0.0; + coarseCompleted_ = false; + fineCompleted_ = false; + maxStep_ = kMaxFocusSteps; +} + +bool Af::afIsOutOfFocus() +{ + const uint32_t diff_var = std::abs(currentVariance_ - + status_.maxVariance); + const double var_ratio = diff_var / status_.maxVariance; + LOG(IoBAf, Debug) << "Variance change rate: " + << var_ratio + << " Current VCM step: " + << status_.focus; + if (var_ratio > kMaxChange) + return true; + else + return false; +} + +void Af::Process(StatisticsPtr &stats, Metadata *image_metadata) +{ + unsigned int i; + image_metadata->Get("af.status", status_); + + /* Use the second filter results only, and cache those. */ + for (i = 0; i < FOCUS_REGIONS; i++) + status_.focus_measures[i] = stats->focus_stats[i].contrast_val[1][1] + / stats->focus_stats[i].contrast_val_num[1][1]; + status_.num = i; + + currentVariance_ = estimateVariance(); + + if (!status_.stable) { + afCoarseScan(); + afFineScan(); + } else { + if (afIsOutOfFocus()) + afReset(); + else + ignoreCounter_ = kIgnoreFrame; + } +} + +/* Register algorithm with the system. */ +static Algorithm *Create(Controller *controller) +{ + return new Af(controller); +} +static RegisterAlgorithm reg(NAME, &Create); diff --git a/src/ipa/raspberrypi/controller/iob/af.h b/src/ipa/raspberrypi/controller/iob/af.h new file mode 100644 index 00000000..45c9711f --- /dev/null +++ b/src/ipa/raspberrypi/controller/iob/af.h @@ -0,0 +1,55 @@ +/* SPDX-License-Identifier: LGPL-2.1-or-later */ +/* + * Copyright (C) 2021, Red Hat + * Copyright (C) 2022, Ideas On Board + * + * af.h - automatic contrast-based focus algorithm + */ +#pragma once + +#include "../af_algorithm.hpp" +#include "../af_status.h" +#include "../metadata.hpp" + +namespace RPiController { + +class Af : public AfAlgorithm +{ +public: + Af(Controller *controller); + char const *Name() const override; + void Initialise() override; + void Prepare(Metadata *image_metadata) override; + void Process(StatisticsPtr &stats, Metadata *image_metadata) override; +private: + double estimateVariance(); + bool afNeedIgnoreFrame(); + void afCoarseScan(); + void afFineScan(); + bool afScan(uint32_t minSteps); + void afReset(); + bool afIsOutOfFocus(); + + AfStatus status_; + + /* VCM step configuration. It is the current setting of the VCM step. */ + uint32_t focus_; + /* The best VCM step. It is a local optimum VCM step during scanning. */ + uint32_t bestFocus_; + + /* The frames ignored before starting measuring. */ + uint32_t ignoreCounter_; + + /* Current AF statistic variance. */ + double currentVariance_; + /* It is used to determine the derivative during scanning */ + double previousVariance_; + /* The designated maximum range of focus scanning. */ + uint32_t maxStep_; + /* If the coarse scan completes, it is set to true. */ + bool coarseCompleted_; + /* If the fine scan completes, it is set to true. */ + bool fineCompleted_; +}; + +} /* namespace RPiController */ diff --git a/src/ipa/raspberrypi/meson.build b/src/ipa/raspberrypi/meson.build index 32897e07..37068ecc 100644 --- a/src/ipa/raspberrypi/meson.build +++ b/src/ipa/raspberrypi/meson.build @@ -28,6 +28,7 @@ rpi_ipa_sources = files([ 'controller/controller.cpp', 'controller/histogram.cpp', 'controller/algorithm.cpp', + 'controller/iob/af.cpp', 'controller/rpi/alsc.cpp', 'controller/rpi/awb.cpp', 'controller/rpi/sharpen.cpp',