[libcamera-devel] [PATCH v1 11/14] ipa: raspberrypi: First version of autofocus algorithm using PDAF
Kieran Bingham
kieran.bingham at ideasonboard.com
Fri Jan 20 17:39:12 CET 2023
Hi Nick, / Naush,
Quoting Nick Hollinghurst via libcamera-devel (2023-01-19 13:07:44)
> Hi all,
>
> This patch needs a tweak to handle the case where setWindows is called
> before configure -- see Af::Af(Controller *controller) constructor
> below.
>
> On Thu, 19 Jan 2023 at 10:46, Naushir Patuck <naush at raspberrypi.com> wrote:
> >
> > From: Nick Hollinghurst <nick.hollinghurst at raspberrypi.com>
> >
> > Provide the first version of the Raspberry Pi autofocus algorithm. This
> > implementation uses a hybrid of contrast detect autofocus (CDAF) and phase
> > detect autofocus (PDAF) statistics. PDAF is always preferred over CDAF due to
> > having less "hunting" behavior.
> >
> > Signed-off-by: Nick Hollinghurst <nick.hollinghurst at raspberrypi.com>
> > Signed-off-by: Naushir Patuck <naush at raspberrypi.com>
> > Reviewed-by: Naushir Patuck <naush at raspberrypi.com>
> > Reviewed-by: David Plowman <david.plowman at raspberrypi.com>
> > ---
> > src/ipa/raspberrypi/controller/rpi/af.cpp | 755 ++++++++++++++++++++++
> > src/ipa/raspberrypi/controller/rpi/af.h | 153 +++++
> > src/ipa/raspberrypi/meson.build | 1 +
> > 3 files changed, 909 insertions(+)
> > create mode 100644 src/ipa/raspberrypi/controller/rpi/af.cpp
> > create mode 100644 src/ipa/raspberrypi/controller/rpi/af.h
> >
> > diff --git a/src/ipa/raspberrypi/controller/rpi/af.cpp b/src/ipa/raspberrypi/controller/rpi/af.cpp
> > new file mode 100644
> > index 000000000000..7e2e8961085a
> > --- /dev/null
> > +++ b/src/ipa/raspberrypi/controller/rpi/af.cpp
> > @@ -0,0 +1,755 @@
> > +/* SPDX-License-Identifier: BSD-2-Clause */
> > +/*
> > + * Copyright (C) 2022-2023, Raspberry Pi Ltd
> > + *
> > + * af.cpp - Autofocus control algorithm
> > + */
> > +
> > +#include "af.h"
> > +
> > +#include <iomanip>
> > +#include <math.h>
> > +#include <stdlib.h>
> > +
> > +#include <libcamera/base/log.h>
> > +
> > +#include <libcamera/control_ids.h>
> > +
> > +using namespace RPiController;
> > +using namespace libcamera;
> > +
> > +LOG_DEFINE_CATEGORY(RPiAf)
> > +
> > +#define NAME "rpi.af"
> > +
> > +/*
> > + * Default values for parameters. All may be overridden in the tuning file.
> > + * Many of these values are sensor- or module-dependent; the defaults here
> > + * assume IMX708 in a Raspberry Pi V3 camera with the standard lens.
> > + *
> > + * Here all focus values are in dioptres (1/m). They are converted to hardware
> > + * units when written to status.lensSetting or returned from setLensPosition().
> > + *
> > + * Gain and delay values are relative to the update rate, since much (not all)
> > + * of the delay is in the sensor and (for CDAF) ISP, not the lens mechanism;
> > + * but note that algorithms are updated at no more than 30 Hz.
> > + */
> > +
> > +Af::RangeDependentParams::RangeDependentParams()
> > + : focusMin(0.0),
> > + focusMax(12.0),
> > + focusDefault(1.0)
> > +{
> > +}
> > +
> > +Af::SpeedDependentParams::SpeedDependentParams()
> > + : stepCoarse(1.0),
> > + stepFine(0.25),
> > + contrastRatio(0.75),
> > + pdafGain(-0.02),
> > + pdafSquelch(0.125),
> > + maxSlew(2.0),
> > + pdafFrames(20),
> > + dropoutFrames(6),
> > + stepFrames(4)
> > +{
> > +}
> > +
> > +Af::CfgParams::CfgParams()
> > + : confEpsilon(8),
> > + confThresh(16),
> > + confClip(512),
> > + skipFrames(5),
> > + map()
> > +{
> > +}
> > +
> > +template<typename T>
> > +static void readNumber(T &dest, const libcamera::YamlObject ¶ms, char const *name)
> > +{
> > + auto value = params[name].get<T>();
> > + if (value)
> > + dest = *value;
> > + else
> > + LOG(RPiAf, Warning) << "Missing parameter \"" << name << "\"";
> > +}
> > +
> > +void Af::RangeDependentParams::read(const libcamera::YamlObject ¶ms)
> > +{
> > +
> > + readNumber<double>(focusMin, params, "min");
> > + readNumber<double>(focusMax, params, "max");
> > + readNumber<double>(focusDefault, params, "default");
> > +}
> > +
> > +void Af::SpeedDependentParams::read(const libcamera::YamlObject ¶ms)
> > +{
> > + readNumber<double>(stepCoarse, params, "step_coarse");
> > + readNumber<double>(stepFine, params, "step_fine");
> > + readNumber<double>(contrastRatio, params, "contrast_ratio");
> > + readNumber<double>(pdafGain, params, "pdaf_gain");
> > + readNumber<double>(pdafSquelch, params, "pdaf_squelch");
> > + readNumber<double>(maxSlew, params, "max_slew");
> > + readNumber<uint32_t>(pdafFrames, params, "pdaf_frames");
> > + readNumber<uint32_t>(dropoutFrames, params, "dropout_frames");
> > + readNumber<uint32_t>(stepFrames, params, "step_frames");
> > +}
> > +
> > +int Af::CfgParams::read(const libcamera::YamlObject ¶ms)
> > +{
> > + if (params.contains("ranges")) {
> > + auto &rr = params["ranges"];
> > +
> > + if (rr.contains("normal"))
> > + ranges[AfRangeNormal].read(rr["normal"]);
> > + else
> > + LOG(RPiAf, Warning) << "Missing range \"normal\"";
> > +
> > + ranges[AfRangeMacro] = ranges[AfRangeNormal];
> > + if (rr.contains("macro"))
> > + ranges[AfRangeMacro].read(rr["macro"]);
> > +
> > + ranges[AfRangeFull].focusMin = std::min(ranges[AfRangeNormal].focusMin,
> > + ranges[AfRangeMacro].focusMin);
> > + ranges[AfRangeFull].focusMax = std::max(ranges[AfRangeNormal].focusMax,
> > + ranges[AfRangeMacro].focusMax);
> > + ranges[AfRangeFull].focusDefault = ranges[AfRangeNormal].focusDefault;
> > + if (rr.contains("full"))
> > + ranges[AfRangeFull].read(rr["full"]);
> > + } else
> > + LOG(RPiAf, Warning) << "No ranges defined";
> > +
> > + if (params.contains("speeds")) {
> > + auto &ss = params["speeds"];
> > +
> > + if (ss.contains("normal"))
> > + speeds[AfSpeedNormal].read(ss["normal"]);
> > + else
> > + LOG(RPiAf, Warning) << "Missing speed \"normal\"";
> > +
> > + speeds[AfSpeedFast] = speeds[AfSpeedNormal];
> > + if (ss.contains("fast"))
> > + speeds[AfSpeedFast].read(ss["fast"]);
> > + } else
> > + LOG(RPiAf, Warning) << "No speeds defined";
> > +
> > + readNumber<uint32_t>(confEpsilon, params, "conf_epsilon");
> > + readNumber<uint32_t>(confThresh, params, "conf_thresh");
> > + readNumber<uint32_t>(confClip, params, "conf_clip");
> > + readNumber<uint32_t>(skipFrames, params, "skip_frames");
> > +
> > + if (params.contains("map"))
> > + map.read(params["map"]);
> > + else
> > + LOG(RPiAf, Warning) << "No map defined";
> > +
> > + return 0;
> > +}
> > +
> > +void Af::CfgParams::initialise()
> > +{
> > + if (map.empty()) {
> > + /* Default mapping from dioptres to hardware setting */
> > + static constexpr double DefaultMapX0 = 0.0;
> > + static constexpr double DefaultMapY0 = 445.0;
> > + static constexpr double DefaultMapX1 = 15.0;
> > + static constexpr double DefaultMapY1 = 925.0;
> > +
> > + map.append(DefaultMapX0, DefaultMapY0);
> > + map.append(DefaultMapX1, DefaultMapY1);
> > +
> > + LOG(RPiAf, Warning) << "af.map is not defined, ";
> > + }
> > +}
> > +
> > +/* Af Algorithm class */
> > +
> > +Af::Af(Controller *controller)
> > + : AfAlgorithm(controller),
> > + cfg_(),
> > + range_(AfRangeNormal),
> > + speed_(AfSpeedNormal),
> > + mode_(AfAlgorithm::AfModeManual),
> > + pauseFlag_(false),
> > + sensorSize_{ 0, 0 },
>
> sensorSize_ needs to be set before setWindows() is called. For this
> version it suffices to initialize it to
>
> sensorSize_ { 4608, 2502 },
>
> a more general fix may follow later.
Can you send a tested update to this patch please? No need to send the
whole series again.
If that can include the fix I highlighted, (Remove pdafSeen_) I believe
I can merge this whole series.
--
Kieran
>
>
> > + useWeights_(false),
> > + phaseWeights_{},
> > + contrastWeights_{},
> > + scanState_(ScanState::Idle),
> > + initted_(false),
> > + ftarget_(-1.0),
> > + fsmooth_(-1.0),
> > + prevContrast_(0.0),
> > + skipCount_(0),
> > + stepCount_(0),
> > + dropCount_(0),
> > + scanMaxContrast_(0.0),
> > + scanMinContrast_(1.0e9),
> > + scanData_(),
> > + reportState_(AfState::Idle)
> > +{
> > + scanData_.reserve(24);
> > +}
> > +
> > +Af::~Af()
> > +{
> > +}
> > +
> > +char const *Af::name() const
> > +{
> > + return NAME;
> > +}
> > +
> > +int Af::read(const libcamera::YamlObject ¶ms)
> > +{
> > + return cfg_.read(params);
> > +}
> > +
> > +void Af::initialise()
> > +{
> > + cfg_.initialise();
> > +}
> > +
> > +void Af::switchMode(CameraMode const &cameraMode, [[maybe_unused]] Metadata *metadata)
> > +{
> > + (void)metadata;
> > + sensorSize_.width = cameraMode.sensorWidth;
> > + sensorSize_.height = cameraMode.sensorHeight;
> > +
> > + if (scanState_ >= ScanState::Coarse && scanState_ < ScanState::Settle) {
> > + /*
> > + * If a scan was in progress, re-start it, as CDAF statistics
> > + * may have changed. Though if the application is just about
> > + * to take a still picture, this will not help...
> > + */
> > + startProgrammedScan();
> > + }
> > + skipCount_ = cfg_.skipFrames;
> > +}
> > +
> > +bool Af::getPhase(PdafData const &data, double &phase, double &conf) const
> > +{
> > + static const uint8_t defaultWeights[PDAF_DATA_ROWS][PDAF_DATA_COLS] = {
> > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
> > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
> > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
> > + { 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 },
> > + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },
> > + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },
> > + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },
> > + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },
> > + { 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 },
> > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
> > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
> > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }
> > + };
> > + int32_t sumW = 0;
> > + int32_t sumWc = 0;
> > + int32_t sumWcp = 0;
> > + auto wgts = useWeights_ ? phaseWeights_ : defaultWeights;
> > +
> > + for (unsigned i = 0; i < PDAF_DATA_ROWS; ++i) {
> > + for (unsigned j = 0; j < PDAF_DATA_COLS; ++j) {
> > + if (wgts[i][j]) {
> > + uint32_t c = data.conf[i][j];
> > + if (c >= cfg_.confThresh) {
> > + if (c > cfg_.confClip)
> > + c = cfg_.confClip;
> > + sumWc += wgts[i][j] * (int32_t)c;
> > + c -= (cfg_.confThresh >> 1);
> > + sumWcp += wgts[i][j] * data.phase[i][j] * (int32_t)c;
> > + }
> > + sumW += wgts[i][j];
> > + }
> > + }
> > + }
> > +
> > + if (sumWc > 0) {
> > + phase = (double)sumWcp / (double)sumWc;
> > + conf = (double)sumWc / (double)sumW;
> > + return true;
> > + } else {
> > + phase = 0.0;
> > + conf = 0.0;
> > + return false;
> > + }
> > +}
> > +
> > +void Af::doPDAF(double phase, double conf)
> > +{
> > + /* Apply loop gain */
> > + phase *= cfg_.speeds[speed_].pdafGain;
> > +
> > + if (mode_ == AfModeContinuous) {
> > + /*
> > + * PDAF in Continuous mode. Scale down lens movement when
> > + * delta is small or confidence is low, to suppress wobble.
> > + */
> > + if (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch) {
> > + double a = phase / cfg_.speeds[speed_].pdafSquelch;
> > + phase *= a * a;
> > + }
> > + phase *= conf / (conf + cfg_.confEpsilon);
> > + } else {
> > + /*
> > + * PDAF in triggered-auto mode. Allow early termination when
> > + * phase delta is small; scale down lens movements towards
> > + * the end of the sequence, to ensure a stable image.
> > + */
> > + if (stepCount_ >= cfg_.speeds[speed_].stepFrames) {
> > + if (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch)
> > + stepCount_ = cfg_.speeds[speed_].stepFrames;
> > + } else
> > + phase *= stepCount_ / cfg_.speeds[speed_].stepFrames;
> > + }
> > +
> > + /* Apply slew rate limit. Report failure if out of bounds. */
> > + if (phase < -cfg_.speeds[speed_].maxSlew) {
> > + phase = -cfg_.speeds[speed_].maxSlew;
> > + reportState_ = (ftarget_ <= cfg_.ranges[range_].focusMin) ? AfState::Failed
> > + : AfState::Scanning;
> > + } else if (phase > cfg_.speeds[speed_].maxSlew) {
> > + phase = cfg_.speeds[speed_].maxSlew;
> > + reportState_ = (ftarget_ >= cfg_.ranges[range_].focusMax) ? AfState::Failed
> > + : AfState::Scanning;
> > + } else
> > + reportState_ = AfState::Focused;
> > +
> > + ftarget_ = fsmooth_ + phase;
> > +}
> > +
> > +bool Af::earlyTerminationByPhase(double phase)
> > +{
> > + if (scanData_.size() > 0 &&
> > + scanData_[scanData_.size() - 1].conf >= cfg_.confEpsilon) {
> > + double oldFocus = scanData_[scanData_.size() - 1].focus;
> > + double oldPhase = scanData_[scanData_.size() - 1].phase;
> > +
> > + /*
> > + * Check that the gradient is finite and has the expected sign;
> > + * Interpolate/extrapolate the lens position for zero phase.
> > + * Check that the extrapolation is well-conditioned.
> > + */
> > + if ((ftarget_ - oldFocus) * (phase - oldPhase) > 0.0) {
> > + double param = phase / (phase - oldPhase);
> > + if (-3.0 <= param && param <= 3.5) {
> > + ftarget_ += param * (oldFocus - ftarget_);
> > + LOG(RPiAf, Debug) << "ETBP: param=" << param;
> > + return true;
> > + }
> > + }
> > + }
> > +
> > + return false;
> > +}
> > +
> > +double Af::findPeak(unsigned i) const
> > +{
> > + double f = scanData_[i].focus;
> > +
> > + if (i > 0 && i + 1 < scanData_.size()) {
> > + double dropLo = scanData_[i].contrast - scanData_[i - 1].contrast;
> > + double dropHi = scanData_[i].contrast - scanData_[i + 1].contrast;
> > + if (dropLo < dropHi) {
> > + double param = 0.3125 * (1.0 - dropLo / dropHi) * (1.6 - dropLo / dropHi);
> > + f += param * (scanData_[i - 1].focus - f);
> > + } else if (dropHi < dropLo) {
> > + double param = 0.3125 * (1.0 - dropHi / dropLo) * (1.6 - dropHi / dropLo);
> > + f += param * (scanData_[i + 1].focus - f);
> > + }
> > + }
> > +
> > + LOG(RPiAf, Debug) << "FindPeak: " << f;
> > + return f;
> > +}
> > +
> > +void Af::doScan(double contrast, double phase, double conf)
> > +{
> > + /* Record lens position, contrast and phase values for the current scan */
> > + if (scanData_.empty() || contrast > scanMaxContrast_) {
> > + scanMaxContrast_ = contrast;
> > + scanMaxIndex_ = scanData_.size();
> > + }
> > + if (contrast < scanMinContrast_)
> > + scanMinContrast_ = contrast;
> > + scanData_.emplace_back(ScanRecord{ ftarget_, contrast, phase, conf });
> > +
> > + if (scanState_ == ScanState::Coarse) {
> > + if (ftarget_ >= cfg_.ranges[range_].focusMax ||
> > + contrast < cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) {
> > + /*
> > + * Finished course scan, or termination based on contrast.
> > + * Jump to just after max contrast and start fine scan.
> > + */
> > + ftarget_ = std::min(ftarget_, findPeak(scanMaxIndex_) +
> > + 2.0 * cfg_.speeds[speed_].stepFine);
> > + scanState_ = ScanState::Fine;
> > + scanData_.clear();
> > + } else
> > + ftarget_ += cfg_.speeds[speed_].stepCoarse;
> > + } else { /* ScanState::Fine */
> > + if (ftarget_ <= cfg_.ranges[range_].focusMin || scanData_.size() >= 5 ||
> > + contrast < cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) {
> > + /*
> > + * Finished fine scan, or termination based on contrast.
> > + * Use quadratic peak-finding to find best contrast position.
> > + */
> > + ftarget_ = findPeak(scanMaxIndex_);
> > + scanState_ = ScanState::Settle;
> > + } else
> > + ftarget_ -= cfg_.speeds[speed_].stepFine;
> > + }
> > +
> > + stepCount_ = (ftarget_ == fsmooth_) ? 0 : cfg_.speeds[speed_].stepFrames;
> > +}
> > +
> > +void Af::doAF(double contrast, double phase, double conf)
> > +{
> > + /* Skip frames at startup and after mode change */
> > + if (skipCount_ > 0) {
> > + LOG(RPiAf, Debug) << "SKIP";
> > + skipCount_--;
> > + return;
> > + }
> > +
> > + if (scanState_ == ScanState::Pdaf) {
> > + /*
> > + * Use PDAF closed-loop control whenever available, in both CAF
> > + * mode and (for a limited number of iterations) when triggered.
> > + * If PDAF fails (due to poor contrast, noise or large defocus),
> > + * fall back to a CDAF-based scan. To avoid "nuisance" scans,
> > + * scan only after a number of frames with low PDAF confidence.
> > + */
> > + if (conf > (dropCount_ ? cfg_.confEpsilon : 0.0)) {
> > + doPDAF(phase, conf);
> > + if (stepCount_ > 0)
> > + stepCount_--;
> > + else if (mode_ != AfModeContinuous)
> > + scanState_ = ScanState::Idle;
> > + dropCount_ = 0;
> > + } else if (++dropCount_ == cfg_.speeds[speed_].dropoutFrames)
> > + startProgrammedScan();
> > + } else if (scanState_ >= ScanState::Coarse && fsmooth_ == ftarget_) {
> > + /*
> > + * Scanning sequence. This means PDAF has become unavailable.
> > + * Allow a delay between steps for CDAF FoM statistics to be
> > + * updated, and a "settling time" at the end of the sequence.
> > + * [A coarse or fine scan can be abandoned if two PDAF samples
> > + * allow direct interpolation of the zero-phase lens position.]
> > + */
> > + if (stepCount_ > 0)
> > + stepCount_--;
> > + else if (scanState_ == ScanState::Settle) {
> > + if (prevContrast_ >= cfg_.speeds[speed_].contrastRatio * scanMaxContrast_ &&
> > + scanMinContrast_ <= cfg_.speeds[speed_].contrastRatio * scanMaxContrast_)
> > + reportState_ = AfState::Focused;
> > + else
> > + reportState_ = AfState::Failed;
> > + if (mode_ == AfModeContinuous && !pauseFlag_ &&
> > + cfg_.speeds[speed_].dropoutFrames > 0)
> > + scanState_ = ScanState::Pdaf;
> > + else
> > + scanState_ = ScanState::Idle;
> > + scanData_.clear();
> > + } else if (conf >= cfg_.confEpsilon && earlyTerminationByPhase(phase)) {
> > + scanState_ = ScanState::Settle;
> > + stepCount_ = (mode_ == AfModeContinuous) ? 0
> > + : cfg_.speeds[speed_].stepFrames;
> > + } else
> > + doScan(contrast, phase, conf);
> > + }
> > +}
> > +
> > +void Af::updateLensPosition()
> > +{
> > + if (mode_ != AfModeManual) {
> > + ftarget_ = std::clamp(ftarget_,
> > + cfg_.ranges[range_].focusMin,
> > + cfg_.ranges[range_].focusMax);
> > + }
> > +
> > + /* \todo Add a clip for manual lens position to be within the PWL limits. */
> > +
> > + if (initted_) {
> > + /* from a known lens position: apply slew rate limit */
> > + fsmooth_ = std::clamp(ftarget_,
> > + fsmooth_ - cfg_.speeds[speed_].maxSlew,
> > + fsmooth_ + cfg_.speeds[speed_].maxSlew);
> > + } else {
> > + /* from an unknown position: go straight to target, but add delay */
> > + fsmooth_ = ftarget_;
> > + initted_ = true;
> > + skipCount_ = cfg_.skipFrames;
> > + }
> > +}
> > +
> > +/*
> > + * PDAF phase data are available in prepare(), but CDAF statistics are not
> > + * available until process(). We are gambling on the availability of PDAF.
> > + * To expedite feedback control using PDAF, issue the V4L2 lens control from
> > + * prepare(). Conversely, during scans, we must allow an extra frame delay
> > + * between steps, to retrieve CDAF statistics from the previous process()
> > + * so we can terminate the scan early without having to change our minds.
> > + */
> > +
> > +void Af::prepare(Metadata *imageMetadata)
> > +{
> > + if (initted_) {
> > + /* Get PDAF from the embedded metadata, and run AF algorithm core */
> > + PdafData data;
> > + double phase = 0.0, conf = 0.0;
> > + double oldFt = ftarget_;
> > + double oldFs = fsmooth_;
> > + ScanState oldSs = scanState_;
> > + uint32_t oldSt = stepCount_;
> > + if (imageMetadata->get("pdaf.data", data) == 0)
> > + getPhase(data, phase, conf);
> > + doAF(prevContrast_, phase, conf);
> > + updateLensPosition();
> > + LOG(RPiAf, Debug) << std::fixed << std::setprecision(2)
> > + << static_cast<unsigned int>(reportState_)
> > + << " sst" << static_cast<unsigned int>(oldSs)
> > + << "->" << static_cast<unsigned int>(scanState_)
> > + << " stp" << oldSt << "->" << stepCount_
> > + << " ft" << oldFt << "->" << ftarget_
> > + << " fs" << oldFs << "->" << fsmooth_
> > + << " cont=" << (int)prevContrast_
> > + << " phase=" << (int)phase << " conf=" << (int)conf;
> > + }
> > +
> > + /* Report status and produce new lens setting */
> > + AfStatus status;
> > + if (pauseFlag_)
> > + status.pauseState = (scanState_ == ScanState::Idle) ? AfPauseState::Paused
> > + : AfPauseState::Pausing;
> > + else
> > + status.pauseState = AfPauseState::Running;
> > +
> > + if (mode_ == AfModeAuto && scanState_ != ScanState::Idle)
> > + status.state = AfState::Scanning;
> > + else
> > + status.state = reportState_;
> > + status.lensSetting = initted_ ? std::optional<int>(cfg_.map.eval(fsmooth_))
> > + : std::nullopt;
> > + imageMetadata->set("af.status", status);
> > +}
> > +
> > +double Af::getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const
> > +{
> > + uint32_t totW = 0, totWc = 0;
> > +
> > + if (useWeights_) {
> > + for (unsigned i = 0; i < FOCUS_REGIONS; ++i) {
> > + unsigned w = contrastWeights_[i];
> > + totW += w;
> > + totWc += w * (focus_stats[i].contrast_val[1][1] >> 10);
> > + }
> > + }
> > + if (totW == 0) {
> > + totW = 2;
> > + totWc = (focus_stats[5].contrast_val[1][1] >> 10) +
> > + (focus_stats[6].contrast_val[1][1] >> 10);
> > + }
> > +
> > + return (double)totWc / (double)totW;
> > +}
> > +
> > +void Af::process(StatisticsPtr &stats, [[maybe_unused]] Metadata *imageMetadata)
> > +{
> > + (void)imageMetadata;
> > + prevContrast_ = getContrast(stats->focus_stats);
> > +}
> > +
> > +/* Controls */
> > +
> > +void Af::setRange(AfRange r)
> > +{
> > + LOG(RPiAf, Debug) << "setRange: " << (unsigned)r;
> > + if (r < AfAlgorithm::AfRangeMax)
> > + range_ = r;
> > +}
> > +
> > +void Af::setSpeed(AfSpeed s)
> > +{
> > + LOG(RPiAf, Debug) << "setSpeed: " << (unsigned)s;
> > + if (s < AfAlgorithm::AfSpeedMax) {
> > + if (scanState_ == ScanState::Pdaf &&
> > + cfg_.speeds[s].pdafFrames > cfg_.speeds[speed_].pdafFrames)
> > + stepCount_ += cfg_.speeds[s].pdafFrames - cfg_.speeds[speed_].pdafFrames;
> > + speed_ = s;
> > + }
> > +}
> > +
> > +void Af::setMetering(bool mode)
> > +{
> > + useWeights_ = mode;
> > +}
> > +
> > +void Af::setWindows(libcamera::Span<libcamera::Rectangle const> const &wins)
> > +{
> > + /*
> > + * Here we just merge all of the given windows, weighted by area.
> > + * If there are more than 15 overlapping windows, overflow can occur.
> > + * TODO: A better approach might be to find the phase in each window
> > + * and choose either the closest or the highest-confidence one?
> > + *
> > + * Using mostly "int" arithmetic, because Rectangle has signed x, y
> > + */
> > + ASSERT(sensorSize_.width > 0 && sensorSize_.height > 0);
> > + int gridY = (int)(sensorSize_.height / PDAF_DATA_ROWS);
> > + int gridX = (int)(sensorSize_.width / PDAF_DATA_COLS);
> > + int gridA = gridY * gridX;
> > +
> > + for (int i = 0; i < PDAF_DATA_ROWS; ++i)
> > + std::fill(phaseWeights_[i], phaseWeights_[i] + PDAF_DATA_COLS, 0);
> > + std::fill(contrastWeights_, contrastWeights_ + FOCUS_REGIONS, 0);
> > +
> > + for (auto &w : wins) {
> > + for (int i = 0; i < PDAF_DATA_ROWS; ++i) {
> > + int y0 = std::max(gridY * i, w.y);
> > + int y1 = std::min(gridY * (i + 1), w.y + (int)(w.height));
> > + if (y0 >= y1)
> > + continue;
> > + y1 -= y0;
> > + for (int j = 0; j < PDAF_DATA_COLS; ++j) {
> > + int x0 = std::max(gridX * j, w.x);
> > + int x1 = std::min(gridX * (j + 1), w.x + (int)(w.width));
> > + if (x0 >= x1)
> > + continue;
> > + int a = y1 * (x1 - x0);
> > + a = (16 * a + gridA - 1) / gridA;
> > + phaseWeights_[i][j] += a;
> > + contrastWeights_[4 * ((3 * i) / PDAF_DATA_ROWS) + ((4 * j) / PDAF_DATA_COLS)] += a;
> > + }
> > + }
> > + }
> > +}
> > +
> > +bool Af::setLensPosition(double dioptres, int *hwpos)
> > +{
> > + bool changed = false;
> > +
> > + if (mode_ == AfModeManual) {
> > + LOG(RPiAf, Debug) << "setLensPosition: " << dioptres;
> > + changed = !(initted_ && fsmooth_ == dioptres);
> > + ftarget_ = dioptres;
> > + updateLensPosition();
> > + }
> > +
> > + if (hwpos)
> > + *hwpos = cfg_.map.eval(fsmooth_);
> > +
> > + return changed;
> > +}
> > +
> > +std::optional<double> Af::getLensPosition() const
> > +{
> > + /*
> > + * \todo We ought to perform some precise timing here to determine
> > + * the current lens position.
> > + */
> > + return initted_ ? std::optional<double>(fsmooth_) : std::nullopt;
> > +}
> > +
> > +void Af::startCAF()
> > +{
> > + /* Try PDAF if the tuning file permits it for CAF; else CDAF */
> > + if (cfg_.speeds[speed_].dropoutFrames > 0) {
> > + if (!initted_) {
> > + ftarget_ = cfg_.ranges[range_].focusDefault;
> > + updateLensPosition();
> > + }
> > + scanState_ = ScanState::Pdaf;
> > + scanData_.clear();
> > + dropCount_ = 0;
> > + reportState_ = AfState::Scanning;
> > + } else {
> > + startProgrammedScan();
> > + }
> > +}
> > +
> > +void Af::startProgrammedScan()
> > +{
> > + ftarget_ = cfg_.ranges[range_].focusMin;
> > + updateLensPosition();
> > + scanState_ = ScanState::Coarse;
> > + scanMaxContrast_ = 0.0;
> > + scanMinContrast_ = 1.0e9;
> > + scanMaxIndex_ = 0;
> > + scanData_.clear();
> > + stepCount_ = cfg_.speeds[speed_].stepFrames;
> > + reportState_ = AfState::Scanning;
> > +}
> > +
> > +void Af::goIdle()
> > +{
> > + scanState_ = ScanState::Idle;
> > + reportState_ = AfState::Idle;
> > + scanData_.clear();
> > +}
> > +
> > +void Af::cancelScan()
> > +{
> > + LOG(RPiAf, Debug) << "cancelScan";
> > + if (mode_ == AfModeAuto)
> > + goIdle();
> > +}
> > +
> > +void Af::triggerScan()
> > +{
> > + LOG(RPiAf, Debug) << "triggerScan";
> > + if (mode_ == AfModeAuto && scanState_ == ScanState::Idle) {
> > + /* Try PDAF if the tuning file permits it for Auto; else CDAF */
> > + if (cfg_.speeds[speed_].pdafFrames > 0 && cfg_.speeds[speed_].dropoutFrames > 0) {
> > + if (!initted_) {
> > + ftarget_ = cfg_.ranges[range_].focusDefault;
> > + updateLensPosition();
> > + }
> > + stepCount_ = cfg_.speeds[speed_].pdafFrames;
> > + scanState_ = ScanState::Pdaf;
> > + dropCount_ = 0;
> > + } else
> > + startProgrammedScan();
> > + reportState_ = AfState::Scanning;
> > + }
> > +}
> > +
> > +void Af::setMode(AfAlgorithm::AfMode mode)
> > +{
> > + LOG(RPiAf, Debug) << "setMode: " << (unsigned)mode;
> > + if (mode_ != mode) {
> > + mode_ = mode;
> > + pauseFlag_ = false;
> > + if (mode == AfModeContinuous)
> > + startCAF();
> > + else if (mode != AfModeAuto)
> > + goIdle();
> > + }
> > +}
> > +
> > +AfAlgorithm::AfMode Af::getMode() const
> > +{
> > + return mode_;
> > +}
> > +
> > +void Af::pause(AfAlgorithm::AfPause pause)
> > +{
> > + LOG(RPiAf, Debug) << "pause: " << (unsigned)pause;
> > + if (mode_ == AfModeContinuous) {
> > + if (pause == AfPauseResume && pauseFlag_) {
> > + startCAF();
> > + pauseFlag_ = false;
> > + } else if (pause != AfPauseResume && !pauseFlag_) {
> > + if (pause == AfPauseImmediate || scanState_ == ScanState::Pdaf)
> > + goIdle();
> > + pauseFlag_ = true;
> > + }
> > + }
> > +}
> > +
> > +// Register algorithm with the system.
> > +static Algorithm *create(Controller *controller)
> > +{
> > + return (Algorithm *)new Af(controller);
> > +}
> > +static RegisterAlgorithm reg(NAME, &create);
> > diff --git a/src/ipa/raspberrypi/controller/rpi/af.h b/src/ipa/raspberrypi/controller/rpi/af.h
> > new file mode 100644
> > index 000000000000..0431bd70ae29
> > --- /dev/null
> > +++ b/src/ipa/raspberrypi/controller/rpi/af.h
> > @@ -0,0 +1,153 @@
> > +/* SPDX-License-Identifier: BSD-2-Clause */
> > +/*
> > + * Copyright (C) 2022-2023, Raspberry Pi Ltd
> > + *
> > + * af.h - Autofocus control algorithm
> > + */
> > +#pragma once
> > +
> > +#include "../af_algorithm.h"
> > +#include "../af_status.h"
> > +#include "../pdaf_data.h"
> > +#include "../pwl.h"
> > +
> > +/*
> > + * This algorithm implements a hybrid of CDAF and PDAF, favouring PDAF.
> > + *
> > + * Whenever PDAF is available, it is used in a continuous feedback loop.
> > + * When triggered in auto mode, we simply enable AF for a limited number
> > + * of frames (it may terminate early if the delta becomes small enough).
> > + *
> > + * When PDAF confidence is low (due e.g. to low contrast or extreme defocus)
> > + * or PDAF data are absent, fall back to CDAF with a programmed scan pattern.
> > + * A coarse and fine scan are performed, using ISP's CDAF focus FoM to
> > + * estimate the lens position with peak contrast. This is slower due to
> > + * extra latency in the ISP, and requires a settling time between steps.
> > + *
> > + * Some hysteresis is applied to the switch between PDAF and CDAF, to avoid
> > + * "nuisance" scans. During each interval where PDAF is not working, only
> > + * ONE scan will be performed; CAF cannot track objects using CDAF alone.
> > + *
> > + * This algorithm is unrelated to "rpi.focus" which merely reports CDAF FoM.
> > + */
> > +
> > +namespace RPiController {
> > +
> > +class Af : public AfAlgorithm
> > +{
> > +public:
> > + Af(Controller *controller = NULL);
> > + ~Af();
> > + char const *name() const override;
> > + int read(const libcamera::YamlObject ¶ms) override;
> > + void initialise() override;
> > +
> > + /* IPA calls */
> > + void switchMode(CameraMode const &cameraMode, Metadata *metadata) override;
> > + void prepare(Metadata *imageMetadata) override;
> > + void process(StatisticsPtr &stats, Metadata *imageMetadata) override;
> > +
> > + /* controls */
> > + void setRange(AfRange range) override;
> > + void setSpeed(AfSpeed speed) override;
> > + void setMetering(bool use_windows) override;
> > + void setWindows(libcamera::Span<libcamera::Rectangle const> const &wins) override;
> > + void setMode(AfMode mode) override;
> > + AfMode getMode() const override;
> > + bool setLensPosition(double dioptres, int32_t *hwpos) override;
> > + std::optional<double> getLensPosition() const override;
> > + void triggerScan() override;
> > + void cancelScan() override;
> > + void pause(AfPause pause) override;
> > +
> > +private:
> > + enum class ScanState {
> > + Idle,
> > + Pdaf,
> > + Coarse,
> > + Fine,
> > + Settle
> > + };
> > +
> > + struct RangeDependentParams {
> > + double focusMin; /* lower (far) limit in dipotres */
> > + double focusMax; /* upper (near) limit in dioptres */
> > + double focusDefault; /* default setting ("hyperfocal") */
> > +
> > + RangeDependentParams();
> > + void read(const libcamera::YamlObject ¶ms);
> > + };
> > +
> > + struct SpeedDependentParams {
> > + double stepCoarse; /* used for scans */
> > + double stepFine; /* used for scans */
> > + double contrastRatio; /* used for scan termination and reporting */
> > + double pdafGain; /* coefficient for PDAF feedback loop */
> > + double pdafSquelch; /* PDAF stability parameter (device-specific) */
> > + double maxSlew; /* limit for lens movement per frame */
> > + uint32_t pdafFrames; /* number of iterations when triggered */
> > + uint32_t dropoutFrames; /* number of non-PDAF frames to switch to CDAF */
> > + uint32_t stepFrames; /* frames to skip in between steps of a scan */
> > +
> > + SpeedDependentParams();
> > + void read(const libcamera::YamlObject ¶ms);
> > + };
> > +
> > + struct CfgParams {
> > + RangeDependentParams ranges[AfRangeMax];
> > + SpeedDependentParams speeds[AfSpeedMax];
> > + uint32_t confEpsilon; /* PDAF hysteresis threshold (sensor-specific) */
> > + uint32_t confThresh; /* PDAF confidence cell min (sensor-specific) */
> > + uint32_t confClip; /* PDAF confidence cell max (sensor-specific) */
> > + uint32_t skipFrames; /* frames to skip at start or modeswitch */
> > + Pwl map; /* converts dioptres -> lens driver position */
> > +
> > + CfgParams();
> > + int read(const libcamera::YamlObject ¶ms);
> > + void initialise();
> > + };
> > +
> > + struct ScanRecord {
> > + double focus;
> > + double contrast;
> > + double phase;
> > + double conf;
> > + };
> > +
> > + bool getPhase(PdafData const &data, double &phase, double &conf) const;
> > + double getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const;
> > + void doPDAF(double phase, double conf);
> > + bool earlyTerminationByPhase(double phase);
> > + void doScan(double contrast, double phase, double conf);
> > + double findPeak(unsigned index) const;
> > + void doAF(double contrast, double phase, double conf);
> > + void updateLensPosition();
> > + void startProgrammedScan();
> > + void startCAF();
> > + void goIdle();
> > +
> > + /* Configuration and settings */
> > + CfgParams cfg_;
> > + AfRange range_;
> > + AfSpeed speed_;
> > + AfMode mode_;
> > + bool pauseFlag_;
> > + libcamera::Size sensorSize_;
> > + bool useWeights_;
> > + uint8_t phaseWeights_[PDAF_DATA_ROWS][PDAF_DATA_COLS];
> > + uint16_t contrastWeights_[FOCUS_REGIONS];
> > +
> > + /* Working state. */
> > + ScanState scanState_;
> > + bool initted_;
> > + double ftarget_, fsmooth_;
> > + double prevContrast_;
> > + bool pdafSeen_;
> > + unsigned skipCount_, stepCount_, dropCount_;
> > + unsigned scanMaxIndex_;
> > + double scanMaxContrast_, scanMinContrast_;
> > + std::vector<ScanRecord> scanData_;
> > + AfState reportState_;
> > +};
> > +
> > +} // namespace RPiController
> > diff --git a/src/ipa/raspberrypi/meson.build b/src/ipa/raspberrypi/meson.build
> > index 517d815bb98c..4e2689536257 100644
> > --- a/src/ipa/raspberrypi/meson.build
> > +++ b/src/ipa/raspberrypi/meson.build
> > @@ -27,6 +27,7 @@ rpi_ipa_sources = files([
> > 'controller/controller.cpp',
> > 'controller/histogram.cpp',
> > 'controller/algorithm.cpp',
> > + 'controller/rpi/af.cpp',
> > 'controller/rpi/alsc.cpp',
> > 'controller/rpi/awb.cpp',
> > 'controller/rpi/sharpen.cpp',
> > --
> > 2.25.1
> >
More information about the libcamera-devel
mailing list