diff options
Diffstat (limited to 'src/ipa/rpi/controller/rpi')
39 files changed, 6992 insertions, 0 deletions
diff --git a/src/ipa/rpi/controller/rpi/af.cpp b/src/ipa/rpi/controller/rpi/af.cpp new file mode 100644 index 00000000..c5fd8482 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/af.cpp @@ -0,0 +1,797 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2022-2023, Raspberry Pi Ltd + * + * 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); + } +} + +/* Af Algorithm class */ + +static constexpr unsigned MaxWindows = 10; + +Af::Af(Controller *controller) + : AfAlgorithm(controller), + cfg_(), + range_(AfRangeNormal), + speed_(AfSpeedNormal), + mode_(AfAlgorithm::AfModeManual), + pauseFlag_(false), + statsRegion_(0, 0, 0, 0), + windows_(), + useWindows_(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) +{ + /* + * Reserve space for data, to reduce memory fragmentation. It's too early + * to query the size of the PDAF (from camera) and Contrast (from ISP) + * statistics, but these are plausible upper bounds. + */ + phaseWeights_.w.reserve(16 * 12); + contrastWeights_.w.reserve(getHardwareConfig().focusRegions.width * + getHardwareConfig().focusRegions.height); + scanData_.reserve(32); +} + +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; + + /* Assume that PDAF and Focus stats grids cover the visible area */ + statsRegion_.x = (int)cameraMode.cropX; + statsRegion_.y = (int)cameraMode.cropY; + statsRegion_.width = (unsigned)(cameraMode.width * cameraMode.scaleX); + statsRegion_.height = (unsigned)(cameraMode.height * cameraMode.scaleY); + LOG(RPiAf, Debug) << "switchMode: statsRegion: " + << statsRegion_.x << ',' + << statsRegion_.y << ',' + << statsRegion_.width << ',' + << statsRegion_.height; + invalidateWeights(); + + 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; +} + +void Af::computeWeights(RegionWeights *wgts, unsigned rows, unsigned cols) +{ + wgts->rows = rows; + wgts->cols = cols; + wgts->sum = 0; + wgts->w.resize(rows * cols); + std::fill(wgts->w.begin(), wgts->w.end(), 0); + + if (rows > 0 && cols > 0 && useWindows_ && + statsRegion_.height >= rows && statsRegion_.width >= cols) { + /* + * Here we just merge all of the given windows, weighted by area. + * \todo Perhaps a better approach might be to find the phase in each + * window and choose either the closest or the highest-confidence one? + * Ensure weights sum to less than (1<<16). 46080 is a "round number" + * below 65536, for better rounding when window size is a simple + * fraction of image dimensions. + */ + const unsigned maxCellWeight = 46080u / (MaxWindows * rows * cols); + const unsigned cellH = statsRegion_.height / rows; + const unsigned cellW = statsRegion_.width / cols; + const unsigned cellA = cellH * cellW; + + for (auto &w : windows_) { + for (unsigned r = 0; r < rows; ++r) { + int y0 = std::max(statsRegion_.y + (int)(cellH * r), w.y); + int y1 = std::min(statsRegion_.y + (int)(cellH * (r + 1)), + w.y + (int)(w.height)); + if (y0 >= y1) + continue; + y1 -= y0; + for (unsigned c = 0; c < cols; ++c) { + int x0 = std::max(statsRegion_.x + (int)(cellW * c), w.x); + int x1 = std::min(statsRegion_.x + (int)(cellW * (c + 1)), + w.x + (int)(w.width)); + if (x0 >= x1) + continue; + unsigned a = y1 * (x1 - x0); + a = (maxCellWeight * a + cellA - 1) / cellA; + wgts->w[r * cols + c] += a; + wgts->sum += a; + } + } + } + } + + if (wgts->sum == 0) { + /* Default AF window is the middle 1/2 width of the middle 1/3 height */ + for (unsigned r = rows / 3; r < rows - rows / 3; ++r) { + for (unsigned c = cols / 4; c < cols - cols / 4; ++c) { + wgts->w[r * cols + c] = 1; + wgts->sum += 1; + } + } + } +} + +void Af::invalidateWeights() +{ + phaseWeights_.sum = 0; + contrastWeights_.sum = 0; +} + +bool Af::getPhase(PdafRegions const ®ions, double &phase, double &conf) +{ + libcamera::Size size = regions.size(); + if (size.height != phaseWeights_.rows || size.width != phaseWeights_.cols || + phaseWeights_.sum == 0) { + LOG(RPiAf, Debug) << "Recompute Phase weights " << size.width << 'x' << size.height; + computeWeights(&phaseWeights_, size.height, size.width); + } + + uint32_t sumWc = 0; + int64_t sumWcp = 0; + for (unsigned i = 0; i < regions.numRegions(); ++i) { + unsigned w = phaseWeights_.w[i]; + if (w) { + const PdafData &data = regions.get(i).val; + unsigned c = data.conf; + if (c >= cfg_.confThresh) { + if (c > cfg_.confClip) + c = cfg_.confClip; + c -= (cfg_.confThresh >> 2); + sumWc += w * c; + c -= (cfg_.confThresh >> 2); + sumWcp += (int64_t)(w * c) * (int64_t)data.phase; + } + } + } + + if (0 < phaseWeights_.sum && phaseWeights_.sum <= sumWc) { + phase = (double)sumWcp / (double)sumWc; + conf = (double)sumWc / (double)phaseWeights_.sum; + return true; + } else { + phase = 0.0; + conf = 0.0; + return false; + } +} + +double Af::getContrast(const FocusRegions &focusStats) +{ + libcamera::Size size = focusStats.size(); + if (size.height != contrastWeights_.rows || + size.width != contrastWeights_.cols || contrastWeights_.sum == 0) { + LOG(RPiAf, Debug) << "Recompute Contrast weights " + << size.width << 'x' << size.height; + computeWeights(&contrastWeights_, size.height, size.width); + } + + uint64_t sumWc = 0; + for (unsigned i = 0; i < focusStats.numRegions(); ++i) + sumWc += contrastWeights_.w[i] * focusStats.get(i).val; + + return (contrastWeights_.sum > 0) ? ((double)sumWc / (double)contrastWeights_.sum) : 0.0; +} + +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. + */ + phase *= conf / (conf + cfg_.confEpsilon); + if (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch) { + double a = phase / cfg_.speeds[speed_].pdafSquelch; + phase *= a * a; + } + } 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 (0.0 <= dropLo && dropLo < dropHi) { + double param = 0.3125 * (1.0 - dropLo / dropHi) * (1.6 - dropLo / dropHi); + f += param * (scanData_[i - 1].focus - f); + } else if (0.0 <= dropHi && 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 sensor 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_ ? 1.0 : 0.25) * cfg_.confEpsilon) { + 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 (scanState_ >= ScanState::Pdaf) { + ftarget_ = std::clamp(ftarget_, + cfg_.ranges[range_].focusMin, + cfg_.ranges[range_].focusMax); + } + + 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; + } +} + +void Af::startAF() +{ + /* Use PDAF if the tuning file allows it; else CDAF. */ + if (cfg_.speeds[speed_].dropoutFrames > 0 && + (mode_ == AfModeContinuous || cfg_.speeds[speed_].pdafFrames > 0)) { + if (!initted_) { + ftarget_ = cfg_.ranges[range_].focusDefault; + updateLensPosition(); + } + stepCount_ = (mode_ == AfModeContinuous) ? 0 : cfg_.speeds[speed_].pdafFrames; + 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(); +} + +/* + * 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) +{ + /* Initialize for triggered scan or start of CAF mode */ + if (scanState_ == ScanState::Trigger) + startAF(); + + if (initted_) { + /* Get PDAF from the embedded metadata, and run AF algorithm core */ + PdafRegions regions; + double phase = 0.0, conf = 0.0; + double oldFt = ftarget_; + double oldFs = fsmooth_; + ScanState oldSs = scanState_; + uint32_t oldSt = stepCount_; + if (imageMetadata->get("pdaf.regions", regions) == 0) + getPhase(regions, 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); +} + +void Af::process(StatisticsPtr &stats, [[maybe_unused]] Metadata *imageMetadata) +{ + (void)imageMetadata; + prevContrast_ = getContrast(stats->focusRegions); +} + +/* 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) +{ + if (useWindows_ != mode) { + useWindows_ = mode; + invalidateWeights(); + } +} + +void Af::setWindows(libcamera::Span<libcamera::Rectangle const> const &wins) +{ + windows_.clear(); + for (auto &w : wins) { + LOG(RPiAf, Debug) << "Window: " + << w.x << ", " + << w.y << ", " + << w.width << ", " + << w.height; + windows_.push_back(w); + if (windows_.size() >= MaxWindows) + break; + } + + if (useWindows_) + invalidateWeights(); +} + +bool Af::setLensPosition(double dioptres, int *hwpos) +{ + bool changed = false; + + if (mode_ == AfModeManual) { + LOG(RPiAf, Debug) << "setLensPosition: " << dioptres; + ftarget_ = cfg_.map.domain().clip(dioptres); + changed = !(initted_ && fsmooth_ == ftarget_); + 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::cancelScan() +{ + LOG(RPiAf, Debug) << "cancelScan"; + if (mode_ == AfModeAuto) + goIdle(); +} + +void Af::triggerScan() +{ + LOG(RPiAf, Debug) << "triggerScan"; + if (mode_ == AfModeAuto && scanState_ == ScanState::Idle) + scanState_ = ScanState::Trigger; +} + +void Af::setMode(AfAlgorithm::AfMode mode) +{ + LOG(RPiAf, Debug) << "setMode: " << (unsigned)mode; + if (mode_ != mode) { + mode_ = mode; + pauseFlag_ = false; + if (mode == AfModeContinuous) + scanState_ = ScanState::Trigger; + else if (mode != AfModeAuto || scanState_ < ScanState::Coarse) + 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_) { + pauseFlag_ = false; + if (scanState_ < ScanState::Coarse) + scanState_ = ScanState::Trigger; + } else if (pause != AfPauseResume && !pauseFlag_) { + pauseFlag_ = true; + if (pause == AfPauseImmediate || scanState_ < ScanState::Coarse) + goIdle(); + } + } +} + +// 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/rpi/controller/rpi/af.h b/src/ipa/rpi/controller/rpi/af.h new file mode 100644 index 00000000..2617e2ac --- /dev/null +++ b/src/ipa/rpi/controller/rpi/af.h @@ -0,0 +1,165 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2022-2023, Raspberry Pi Ltd + * + * 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. + * + */ + +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 = 0, + Trigger, + 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; + }; + + struct RegionWeights { + unsigned rows; + unsigned cols; + uint32_t sum; + std::vector<uint16_t> w; + + RegionWeights() + : rows(0), cols(0), sum(0), w() {} + }; + + void computeWeights(RegionWeights *wgts, unsigned rows, unsigned cols); + void invalidateWeights(); + bool getPhase(PdafRegions const ®ions, double &phase, double &conf); + double getContrast(const FocusRegions &focusStats); + void doPDAF(double phase, double conf); + bool earlyTerminationByPhase(double phase); + double findPeak(unsigned index) const; + void doScan(double contrast, double phase, double conf); + void doAF(double contrast, double phase, double conf); + void updateLensPosition(); + void startAF(); + void startProgrammedScan(); + void goIdle(); + + /* Configuration and settings */ + CfgParams cfg_; + AfRange range_; + AfSpeed speed_; + AfMode mode_; + bool pauseFlag_; + libcamera::Rectangle statsRegion_; + std::vector<libcamera::Rectangle> windows_; + bool useWindows_; + RegionWeights phaseWeights_; + RegionWeights contrastWeights_; + + /* Working state. */ + ScanState scanState_; + bool initted_; + double ftarget_, fsmooth_; + double prevContrast_; + unsigned skipCount_, stepCount_, dropCount_; + unsigned scanMaxIndex_; + double scanMaxContrast_, scanMinContrast_; + std::vector<ScanRecord> scanData_; + AfState reportState_; +}; + +} // namespace RPiController diff --git a/src/ipa/rpi/controller/rpi/agc.cpp b/src/ipa/rpi/controller/rpi/agc.cpp new file mode 100644 index 00000000..fcf7aec9 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/agc.cpp @@ -0,0 +1,338 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * AGC/AEC control algorithm + */ + +#include "agc.h" + +#include <libcamera/base/log.h> + +#include "../metadata.h" + +using namespace RPiController; +using namespace libcamera; +using libcamera::utils::Duration; +using namespace std::literals::chrono_literals; + +LOG_DEFINE_CATEGORY(RPiAgc) + +#define NAME "rpi.agc" + +Agc::Agc(Controller *controller) + : AgcAlgorithm(controller), + activeChannels_({ 0 }), index_(0) +{ +} + +char const *Agc::name() const +{ + return NAME; +} + +int Agc::read(const libcamera::YamlObject ¶ms) +{ + /* + * When there is only a single channel we can read the old style syntax. + * Otherwise we expect a "channels" keyword followed by a list of configurations. + */ + if (!params.contains("channels")) { + LOG(RPiAgc, Debug) << "Single channel only"; + channelTotalExposures_.resize(1, 0s); + channelData_.emplace_back(); + return channelData_.back().channel.read(params, getHardwareConfig()); + } + + const auto &channels = params["channels"].asList(); + for (auto ch = channels.begin(); ch != channels.end(); ch++) { + LOG(RPiAgc, Debug) << "Read AGC channel"; + channelData_.emplace_back(); + int ret = channelData_.back().channel.read(*ch, getHardwareConfig()); + if (ret) + return ret; + } + + LOG(RPiAgc, Debug) << "Read " << channelData_.size() << " channel(s)"; + if (channelData_.empty()) { + LOG(RPiAgc, Error) << "No AGC channels provided"; + return -1; + } + + channelTotalExposures_.resize(channelData_.size(), 0s); + + return 0; +} + +int Agc::checkChannel(unsigned int channelIndex) const +{ + if (channelIndex >= channelData_.size()) { + LOG(RPiAgc, Warning) << "AGC channel " << channelIndex << " not available"; + return -1; + } + + return 0; +} + +void Agc::disableAuto() +{ + LOG(RPiAgc, Debug) << "disableAuto"; + + /* All channels are enabled/disabled together. */ + for (auto &data : channelData_) + data.channel.disableAuto(); +} + +void Agc::enableAuto() +{ + LOG(RPiAgc, Debug) << "enableAuto"; + + /* All channels are enabled/disabled together. */ + for (auto &data : channelData_) + data.channel.enableAuto(); +} + +unsigned int Agc::getConvergenceFrames() const +{ + /* If there are n channels, it presumably takes n times as long to converge. */ + return channelData_[0].channel.getConvergenceFrames() * activeChannels_.size(); +} + +std::vector<double> const &Agc::getWeights() const +{ + /* + * In future the metering weights may be determined differently, making it + * difficult to associate different sets of weight with different channels. + * Therefore we shall impose a limitation, at least for now, that all + * channels will use the same weights. + */ + return channelData_[0].channel.getWeights(); +} + +void Agc::setEv(unsigned int channelIndex, double ev) +{ + if (checkChannel(channelIndex)) + return; + + LOG(RPiAgc, Debug) << "setEv " << ev << " for channel " << channelIndex; + channelData_[channelIndex].channel.setEv(ev); +} + +void Agc::setFlickerPeriod(Duration flickerPeriod) +{ + LOG(RPiAgc, Debug) << "setFlickerPeriod " << flickerPeriod; + + /* Flicker period will be the same across all channels. */ + for (auto &data : channelData_) + data.channel.setFlickerPeriod(flickerPeriod); +} + +void Agc::setMaxShutter(Duration maxShutter) +{ + /* Frame durations will be the same across all channels too. */ + for (auto &data : channelData_) + data.channel.setMaxShutter(maxShutter); +} + +void Agc::setFixedShutter(unsigned int channelIndex, Duration fixedShutter) +{ + if (checkChannel(channelIndex)) + return; + + LOG(RPiAgc, Debug) << "setFixedShutter " << fixedShutter + << " for channel " << channelIndex; + channelData_[channelIndex].channel.setFixedShutter(fixedShutter); +} + +void Agc::setFixedAnalogueGain(unsigned int channelIndex, double fixedAnalogueGain) +{ + if (checkChannel(channelIndex)) + return; + + LOG(RPiAgc, Debug) << "setFixedAnalogueGain " << fixedAnalogueGain + << " for channel " << channelIndex; + channelData_[channelIndex].channel.setFixedAnalogueGain(fixedAnalogueGain); +} + +void Agc::setMeteringMode(std::string const &meteringModeName) +{ + /* Metering modes will be the same across all channels too. */ + for (auto &data : channelData_) + data.channel.setMeteringMode(meteringModeName); +} + +void Agc::setExposureMode(std::string const &exposureModeName) +{ + LOG(RPiAgc, Debug) << "setExposureMode " << exposureModeName; + + /* Exposure mode will be the same across all channels. */ + for (auto &data : channelData_) + data.channel.setExposureMode(exposureModeName); +} + +void Agc::setConstraintMode(std::string const &constraintModeName) +{ + LOG(RPiAgc, Debug) << "setConstraintMode " << constraintModeName; + + /* Constraint mode will be the same across all channels. */ + for (auto &data : channelData_) + data.channel.setConstraintMode(constraintModeName); +} + +template<typename T> +std::ostream &operator<<(std::ostream &os, const std::vector<T> &v) +{ + os << "{"; + for (const auto &e : v) + os << " " << e; + os << " }"; + return os; +} + +void Agc::setActiveChannels(const std::vector<unsigned int> &activeChannels) +{ + if (activeChannels.empty()) { + LOG(RPiAgc, Warning) << "No active AGC channels supplied"; + return; + } + + for (auto index : activeChannels) + if (checkChannel(index)) + return; + + LOG(RPiAgc, Debug) << "setActiveChannels " << activeChannels; + activeChannels_ = activeChannels; + index_ = 0; +} + +void Agc::switchMode(CameraMode const &cameraMode, + Metadata *metadata) +{ + /* + * We run switchMode on every channel, and then we're going to start over + * with the first active channel again which means that this channel's + * status needs to be the one we leave in the metadata. + */ + AgcStatus status; + + for (unsigned int channelIndex = 0; channelIndex < channelData_.size(); channelIndex++) { + LOG(RPiAgc, Debug) << "switchMode for channel " << channelIndex; + channelData_[channelIndex].channel.switchMode(cameraMode, metadata); + if (channelIndex == activeChannels_[0]) + metadata->get("agc.status", status); + } + + status.channel = activeChannels_[0]; + metadata->set("agc.status", status); + index_ = 0; +} + +static void getDelayedChannelIndex(Metadata *metadata, const char *message, unsigned int &channelIndex) +{ + std::unique_lock<RPiController::Metadata> lock(*metadata); + AgcStatus *status = metadata->getLocked<AgcStatus>("agc.delayed_status"); + if (status) + channelIndex = status->channel; + else { + /* This does happen at startup, otherwise it would be a Warning or Error. */ + LOG(RPiAgc, Debug) << message; + } +} + +static libcamera::utils::Duration +setCurrentChannelIndexGetExposure(Metadata *metadata, const char *message, unsigned int channelIndex) +{ + std::unique_lock<RPiController::Metadata> lock(*metadata); + AgcStatus *status = metadata->getLocked<AgcStatus>("agc.status"); + libcamera::utils::Duration dur = 0s; + + if (status) { + status->channel = channelIndex; + dur = status->totalExposureValue; + } else { + /* This does happen at startup, otherwise it would be a Warning or Error. */ + LOG(RPiAgc, Debug) << message; + } + + return dur; +} + +void Agc::prepare(Metadata *imageMetadata) +{ + /* + * The DeviceStatus in the metadata should be correct for the image we + * are processing. The delayed status should tell us what channel this frame + * was from, so we will use that channel's prepare method. + * + * \todo To be honest, there's not much that's stateful in the prepare methods + * so we should perhaps re-evaluate whether prepare even needs to be done + * "per channel". + */ + unsigned int channelIndex = activeChannels_[0]; + getDelayedChannelIndex(imageMetadata, "prepare: no delayed status", channelIndex); + + LOG(RPiAgc, Debug) << "prepare for channel " << channelIndex; + channelData_[channelIndex].channel.prepare(imageMetadata); +} + +void Agc::process(StatisticsPtr &stats, Metadata *imageMetadata) +{ + /* + * We want to generate values for the next channel in round robin fashion + * (i.e. the channel at location index_ in the activeChannel list), even though + * the statistics we have will be for a different channel (which we find + * again from the delayed status). + */ + + /* Generate updated AGC values for channel for new channel that we are requesting. */ + unsigned int channelIndex = activeChannels_[index_]; + AgcChannelData &channelData = channelData_[channelIndex]; + /* The stats that arrived with this image correspond to the following channel. */ + unsigned int statsIndex = 0; + getDelayedChannelIndex(imageMetadata, "process: no delayed status for stats", statsIndex); + LOG(RPiAgc, Debug) << "process for channel " << channelIndex; + + /* + * We keep a cache of the most recent DeviceStatus and stats for each channel, + * so that we can invoke the next channel's process method with the most up to date + * values. + */ + LOG(RPiAgc, Debug) << "Save DeviceStatus and stats for channel " << statsIndex; + DeviceStatus deviceStatus; + if (imageMetadata->get<DeviceStatus>("device.status", deviceStatus) == 0) + channelData_[statsIndex].deviceStatus = deviceStatus; + else + /* Every frame should have a DeviceStatus. */ + LOG(RPiAgc, Error) << "process: no device status found"; + channelData_[statsIndex].statistics = stats; + + /* + * Finally fetch the most recent DeviceStatus and stats for the new channel, if both + * exist, and call process(). We must make the agc.status metadata record correctly + * which channel this is. + */ + StatisticsPtr *statsPtr = &stats; + if (channelData.statistics && channelData.deviceStatus) { + deviceStatus = *channelData.deviceStatus; + statsPtr = &channelData.statistics; + } else { + /* Can also happen when new channels start. */ + LOG(RPiAgc, Debug) << "process: channel " << channelIndex << " not seen yet"; + } + + channelData.channel.process(*statsPtr, deviceStatus, imageMetadata, channelTotalExposures_); + auto dur = setCurrentChannelIndexGetExposure(imageMetadata, "process: no AGC status found", + channelIndex); + if (dur) + channelTotalExposures_[channelIndex] = dur; + + /* And onto the next channel for the next call. */ + index_ = (index_ + 1) % activeChannels_.size(); +} + +/* Register algorithm with the system. */ +static Algorithm *create(Controller *controller) +{ + return (Algorithm *)new Agc(controller); +} +static RegisterAlgorithm reg(NAME, &create); diff --git a/src/ipa/rpi/controller/rpi/agc.h b/src/ipa/rpi/controller/rpi/agc.h new file mode 100644 index 00000000..5d056f02 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/agc.h @@ -0,0 +1,58 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * AGC/AEC control algorithm + */ +#pragma once + +#include <optional> +#include <string> +#include <vector> + +#include "../agc_algorithm.h" + +#include "agc_channel.h" + +namespace RPiController { + +struct AgcChannelData { + AgcChannel channel; + std::optional<DeviceStatus> deviceStatus; + StatisticsPtr statistics; +}; + +class Agc : public AgcAlgorithm +{ +public: + Agc(Controller *controller); + char const *name() const override; + int read(const libcamera::YamlObject ¶ms) override; + unsigned int getConvergenceFrames() const override; + std::vector<double> const &getWeights() const override; + void setEv(unsigned int channel, double ev) override; + void setFlickerPeriod(libcamera::utils::Duration flickerPeriod) override; + void setMaxShutter(libcamera::utils::Duration maxShutter) override; + void setFixedShutter(unsigned int channelIndex, + libcamera::utils::Duration fixedShutter) override; + void setFixedAnalogueGain(unsigned int channelIndex, + double fixedAnalogueGain) override; + void setMeteringMode(std::string const &meteringModeName) override; + void setExposureMode(std::string const &exposureModeName) override; + void setConstraintMode(std::string const &contraintModeName) override; + void enableAuto() override; + void disableAuto() override; + void switchMode(CameraMode const &cameraMode, Metadata *metadata) override; + void prepare(Metadata *imageMetadata) override; + void process(StatisticsPtr &stats, Metadata *imageMetadata) override; + void setActiveChannels(const std::vector<unsigned int> &activeChannels) override; + +private: + int checkChannel(unsigned int channel) const; + std::vector<AgcChannelData> channelData_; + std::vector<unsigned int> activeChannels_; + unsigned int index_; /* index into the activeChannels_ */ + AgcChannelTotalExposures channelTotalExposures_; +}; + +} /* namespace RPiController */ diff --git a/src/ipa/rpi/controller/rpi/agc_channel.cpp b/src/ipa/rpi/controller/rpi/agc_channel.cpp new file mode 100644 index 00000000..a77ccec3 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/agc_channel.cpp @@ -0,0 +1,1022 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2023, Raspberry Pi Ltd + * + * AGC/AEC control algorithm + */ + +#include "agc_channel.h" + +#include <algorithm> +#include <tuple> + +#include <libcamera/base/log.h> + +#include "../awb_status.h" +#include "../device_status.h" +#include "../histogram.h" +#include "../lux_status.h" +#include "../metadata.h" + +using namespace RPiController; +using namespace libcamera; +using libcamera::utils::Duration; +using namespace std::literals::chrono_literals; + +LOG_DECLARE_CATEGORY(RPiAgc) + +int AgcMeteringMode::read(const libcamera::YamlObject ¶ms) +{ + const YamlObject &yamlWeights = params["weights"]; + + for (const auto &p : yamlWeights.asList()) { + auto value = p.get<double>(); + if (!value) + return -EINVAL; + weights.push_back(*value); + } + + return 0; +} + +static std::tuple<int, std::string> +readMeteringModes(std::map<std::string, AgcMeteringMode> &metering_modes, + const libcamera::YamlObject ¶ms) +{ + std::string first; + int ret; + + for (const auto &[key, value] : params.asDict()) { + AgcMeteringMode meteringMode; + ret = meteringMode.read(value); + if (ret) + return { ret, {} }; + + metering_modes[key] = std::move(meteringMode); + if (first.empty()) + first = key; + } + + return { 0, first }; +} + +int AgcExposureMode::read(const libcamera::YamlObject ¶ms) +{ + auto value = params["shutter"].getList<double>(); + if (!value) + return -EINVAL; + std::transform(value->begin(), value->end(), std::back_inserter(shutter), + [](double v) { return v * 1us; }); + + value = params["gain"].getList<double>(); + if (!value) + return -EINVAL; + gain = std::move(*value); + + if (shutter.size() < 2 || gain.size() < 2) { + LOG(RPiAgc, Error) + << "AgcExposureMode: must have at least two entries in exposure profile"; + return -EINVAL; + } + + if (shutter.size() != gain.size()) { + LOG(RPiAgc, Error) + << "AgcExposureMode: expect same number of exposure and gain entries in exposure profile"; + return -EINVAL; + } + + return 0; +} + +static std::tuple<int, std::string> +readExposureModes(std::map<std::string, AgcExposureMode> &exposureModes, + const libcamera::YamlObject ¶ms) +{ + std::string first; + int ret; + + for (const auto &[key, value] : params.asDict()) { + AgcExposureMode exposureMode; + ret = exposureMode.read(value); + if (ret) + return { ret, {} }; + + exposureModes[key] = std::move(exposureMode); + if (first.empty()) + first = key; + } + + return { 0, first }; +} + +int AgcConstraint::read(const libcamera::YamlObject ¶ms) +{ + std::string boundString = params["bound"].get<std::string>(""); + transform(boundString.begin(), boundString.end(), + boundString.begin(), ::toupper); + if (boundString != "UPPER" && boundString != "LOWER") { + LOG(RPiAgc, Error) << "AGC constraint type should be UPPER or LOWER"; + return -EINVAL; + } + bound = boundString == "UPPER" ? Bound::UPPER : Bound::LOWER; + + auto value = params["q_lo"].get<double>(); + if (!value) + return -EINVAL; + qLo = *value; + + value = params["q_hi"].get<double>(); + if (!value) + return -EINVAL; + qHi = *value; + + return yTarget.read(params["y_target"]); +} + +static std::tuple<int, AgcConstraintMode> +readConstraintMode(const libcamera::YamlObject ¶ms) +{ + AgcConstraintMode mode; + int ret; + + for (const auto &p : params.asList()) { + AgcConstraint constraint; + ret = constraint.read(p); + if (ret) + return { ret, {} }; + + mode.push_back(std::move(constraint)); + } + + return { 0, mode }; +} + +static std::tuple<int, std::string> +readConstraintModes(std::map<std::string, AgcConstraintMode> &constraintModes, + const libcamera::YamlObject ¶ms) +{ + std::string first; + int ret; + + for (const auto &[key, value] : params.asDict()) { + std::tie(ret, constraintModes[key]) = readConstraintMode(value); + if (ret) + return { ret, {} }; + + if (first.empty()) + first = key; + } + + return { 0, first }; +} + +int AgcChannelConstraint::read(const libcamera::YamlObject ¶ms) +{ + auto channelValue = params["channel"].get<unsigned int>(); + if (!channelValue) { + LOG(RPiAgc, Error) << "AGC channel constraint must have a channel"; + return -EINVAL; + } + channel = *channelValue; + + std::string boundString = params["bound"].get<std::string>(""); + transform(boundString.begin(), boundString.end(), + boundString.begin(), ::toupper); + if (boundString != "UPPER" && boundString != "LOWER") { + LOG(RPiAgc, Error) << "AGC channel constraint type should be UPPER or LOWER"; + return -EINVAL; + } + bound = boundString == "UPPER" ? Bound::UPPER : Bound::LOWER; + + auto factorValue = params["factor"].get<double>(); + if (!factorValue) { + LOG(RPiAgc, Error) << "AGC channel constraint must have a factor"; + return -EINVAL; + } + factor = *factorValue; + + return 0; +} + +static int readChannelConstraints(std::vector<AgcChannelConstraint> &channelConstraints, + const libcamera::YamlObject ¶ms) +{ + for (const auto &p : params.asList()) { + AgcChannelConstraint constraint; + int ret = constraint.read(p); + if (ret) + return ret; + + channelConstraints.push_back(constraint); + } + + return 0; +} + +int AgcConfig::read(const libcamera::YamlObject ¶ms) +{ + LOG(RPiAgc, Debug) << "AgcConfig"; + int ret; + + std::tie(ret, defaultMeteringMode) = + readMeteringModes(meteringModes, params["metering_modes"]); + if (ret) + return ret; + std::tie(ret, defaultExposureMode) = + readExposureModes(exposureModes, params["exposure_modes"]); + if (ret) + return ret; + std::tie(ret, defaultConstraintMode) = + readConstraintModes(constraintModes, params["constraint_modes"]); + if (ret) + return ret; + + if (params.contains("channel_constraints")) { + ret = readChannelConstraints(channelConstraints, params["channel_constraints"]); + if (ret) + return ret; + } + + ret = yTarget.read(params["y_target"]); + if (ret) + return ret; + + speed = params["speed"].get<double>(0.2); + startupFrames = params["startup_frames"].get<uint16_t>(10); + convergenceFrames = params["convergence_frames"].get<unsigned int>(6); + fastReduceThreshold = params["fast_reduce_threshold"].get<double>(0.4); + baseEv = params["base_ev"].get<double>(1.0); + + /* Start with quite a low value as ramping up is easier than ramping down. */ + defaultExposureTime = params["default_exposure_time"].get<double>(1000) * 1us; + defaultAnalogueGain = params["default_analogue_gain"].get<double>(1.0); + + stableRegion = params["stable_region"].get<double>(0.02); + + desaturate = params["desaturate"].get<int>(1); + + return 0; +} + +AgcChannel::ExposureValues::ExposureValues() + : shutter(0s), analogueGain(0), + totalExposure(0s), totalExposureNoDG(0s) +{ +} + +AgcChannel::AgcChannel() + : meteringMode_(nullptr), exposureMode_(nullptr), constraintMode_(nullptr), + frameCount_(0), lockCount_(0), + lastTargetExposure_(0s), ev_(1.0), flickerPeriod_(0s), + maxShutter_(0s), fixedShutter_(0s), fixedAnalogueGain_(0.0) +{ + /* Set AWB default values in case early frames have no updates in metadata. */ + awb_.gainR = 1.0; + awb_.gainG = 1.0; + awb_.gainB = 1.0; + + /* + * Setting status_.totalExposureValue_ to zero initially tells us + * it's not been calculated yet (i.e. Process hasn't yet run). + */ + status_ = {}; + status_.ev = ev_; +} + +int AgcChannel::read(const libcamera::YamlObject ¶ms, + const Controller::HardwareConfig &hardwareConfig) +{ + int ret = config_.read(params); + if (ret) + return ret; + + const Size &size = hardwareConfig.agcZoneWeights; + for (auto const &modes : config_.meteringModes) { + if (modes.second.weights.size() != size.width * size.height) { + LOG(RPiAgc, Error) << "AgcMeteringMode: Incorrect number of weights"; + return -EINVAL; + } + } + + /* + * Set the config's defaults (which are the first ones it read) as our + * current modes, until someone changes them. (they're all known to + * exist at this point) + */ + meteringModeName_ = config_.defaultMeteringMode; + meteringMode_ = &config_.meteringModes[meteringModeName_]; + exposureModeName_ = config_.defaultExposureMode; + exposureMode_ = &config_.exposureModes[exposureModeName_]; + constraintModeName_ = config_.defaultConstraintMode; + constraintMode_ = &config_.constraintModes[constraintModeName_]; + /* Set up the "last shutter/gain" values, in case AGC starts "disabled". */ + status_.shutterTime = config_.defaultExposureTime; + status_.analogueGain = config_.defaultAnalogueGain; + return 0; +} + +void AgcChannel::disableAuto() +{ + fixedShutter_ = status_.shutterTime; + fixedAnalogueGain_ = status_.analogueGain; +} + +void AgcChannel::enableAuto() +{ + fixedShutter_ = 0s; + fixedAnalogueGain_ = 0; +} + +unsigned int AgcChannel::getConvergenceFrames() const +{ + /* + * If shutter and gain have been explicitly set, there is no + * convergence to happen, so no need to drop any frames - return zero. + */ + if (fixedShutter_ && fixedAnalogueGain_) + return 0; + else + return config_.convergenceFrames; +} + +std::vector<double> const &AgcChannel::getWeights() const +{ + /* + * In case someone calls setMeteringMode and then this before the + * algorithm has run and updated the meteringMode_ pointer. + */ + auto it = config_.meteringModes.find(meteringModeName_); + if (it == config_.meteringModes.end()) + return meteringMode_->weights; + return it->second.weights; +} + +void AgcChannel::setEv(double ev) +{ + ev_ = ev; +} + +void AgcChannel::setFlickerPeriod(Duration flickerPeriod) +{ + flickerPeriod_ = flickerPeriod; +} + +void AgcChannel::setMaxShutter(Duration maxShutter) +{ + maxShutter_ = maxShutter; +} + +void AgcChannel::setFixedShutter(Duration fixedShutter) +{ + fixedShutter_ = fixedShutter; + /* Set this in case someone calls disableAuto() straight after. */ + status_.shutterTime = limitShutter(fixedShutter_); +} + +void AgcChannel::setFixedAnalogueGain(double fixedAnalogueGain) +{ + fixedAnalogueGain_ = fixedAnalogueGain; + /* Set this in case someone calls disableAuto() straight after. */ + status_.analogueGain = limitGain(fixedAnalogueGain); +} + +void AgcChannel::setMeteringMode(std::string const &meteringModeName) +{ + meteringModeName_ = meteringModeName; +} + +void AgcChannel::setExposureMode(std::string const &exposureModeName) +{ + exposureModeName_ = exposureModeName; +} + +void AgcChannel::setConstraintMode(std::string const &constraintModeName) +{ + constraintModeName_ = constraintModeName; +} + +void AgcChannel::switchMode(CameraMode const &cameraMode, + Metadata *metadata) +{ + /* AGC expects the mode sensitivity always to be non-zero. */ + ASSERT(cameraMode.sensitivity); + + housekeepConfig(); + + /* + * Store the mode in the local state. We must cache the sensitivity of + * of the previous mode for the calculations below. + */ + double lastSensitivity = mode_.sensitivity; + mode_ = cameraMode; + + Duration fixedShutter = limitShutter(fixedShutter_); + if (fixedShutter && fixedAnalogueGain_) { + /* We're going to reset the algorithm here with these fixed values. */ + fetchAwbStatus(metadata); + double minColourGain = std::min({ awb_.gainR, awb_.gainG, awb_.gainB, 1.0 }); + ASSERT(minColourGain != 0.0); + + /* This is the equivalent of computeTargetExposure and applyDigitalGain. */ + target_.totalExposureNoDG = fixedShutter_ * fixedAnalogueGain_; + target_.totalExposure = target_.totalExposureNoDG / minColourGain; + + /* Equivalent of filterExposure. This resets any "history". */ + filtered_ = target_; + + /* Equivalent of divideUpExposure. */ + filtered_.shutter = fixedShutter; + filtered_.analogueGain = fixedAnalogueGain_; + } else if (status_.totalExposureValue) { + /* + * On a mode switch, various things could happen: + * - the exposure profile might change + * - a fixed exposure or gain might be set + * - the new mode's sensitivity might be different + * We cope with the last of these by scaling the target values. After + * that we just need to re-divide the exposure/gain according to the + * current exposure profile, which takes care of everything else. + */ + + double ratio = lastSensitivity / cameraMode.sensitivity; + target_.totalExposureNoDG *= ratio; + target_.totalExposure *= ratio; + filtered_.totalExposureNoDG *= ratio; + filtered_.totalExposure *= ratio; + + divideUpExposure(); + } else { + /* + * We come through here on startup, when at least one of the shutter + * or gain has not been fixed. We must still write those values out so + * that they will be applied immediately. We supply some arbitrary defaults + * for any that weren't set. + */ + + /* Equivalent of divideUpExposure. */ + filtered_.shutter = fixedShutter ? fixedShutter : config_.defaultExposureTime; + filtered_.analogueGain = fixedAnalogueGain_ ? fixedAnalogueGain_ : config_.defaultAnalogueGain; + } + + writeAndFinish(metadata, false); +} + +void AgcChannel::prepare(Metadata *imageMetadata) +{ + Duration totalExposureValue = status_.totalExposureValue; + AgcStatus delayedStatus; + AgcPrepareStatus prepareStatus; + + /* Fetch the AWB status now because AWB also sets it in the prepare method. */ + fetchAwbStatus(imageMetadata); + + if (!imageMetadata->get("agc.delayed_status", delayedStatus)) + totalExposureValue = delayedStatus.totalExposureValue; + + prepareStatus.digitalGain = 1.0; + prepareStatus.locked = false; + + if (status_.totalExposureValue) { + /* Process has run, so we have meaningful values. */ + DeviceStatus deviceStatus; + if (imageMetadata->get("device.status", deviceStatus) == 0) { + Duration actualExposure = deviceStatus.shutterSpeed * + deviceStatus.analogueGain; + if (actualExposure) { + double digitalGain = totalExposureValue / actualExposure; + LOG(RPiAgc, Debug) << "Want total exposure " << totalExposureValue; + /* + * Never ask for a gain < 1.0, and also impose + * some upper limit. Make it customisable? + */ + prepareStatus.digitalGain = std::max(1.0, std::min(digitalGain, 4.0)); + LOG(RPiAgc, Debug) << "Actual exposure " << actualExposure; + LOG(RPiAgc, Debug) << "Use digitalGain " << prepareStatus.digitalGain; + LOG(RPiAgc, Debug) << "Effective exposure " + << actualExposure * prepareStatus.digitalGain; + /* Decide whether AEC/AGC has converged. */ + prepareStatus.locked = updateLockStatus(deviceStatus); + } + } else + LOG(RPiAgc, Warning) << "AgcChannel: no device metadata"; + imageMetadata->set("agc.prepare_status", prepareStatus); + } +} + +void AgcChannel::process(StatisticsPtr &stats, DeviceStatus const &deviceStatus, + Metadata *imageMetadata, + const AgcChannelTotalExposures &channelTotalExposures) +{ + frameCount_++; + /* + * First a little bit of housekeeping, fetching up-to-date settings and + * configuration, that kind of thing. + */ + housekeepConfig(); + /* Get the current exposure values for the frame that's just arrived. */ + fetchCurrentExposure(deviceStatus); + /* Compute the total gain we require relative to the current exposure. */ + double gain, targetY; + computeGain(stats, imageMetadata, gain, targetY); + /* Now compute the target (final) exposure which we think we want. */ + computeTargetExposure(gain); + /* The results have to be filtered so as not to change too rapidly. */ + filterExposure(); + /* + * We may be asked to limit the exposure using other channels. If another channel + * determines our upper bound we may want to know this later. + */ + bool channelBound = applyChannelConstraints(channelTotalExposures); + /* + * Some of the exposure has to be applied as digital gain, so work out + * what that is. It also tells us whether it's trying to desaturate the image + * more quickly, which can only happen when another channel is not limiting us. + */ + bool desaturate = applyDigitalGain(gain, targetY, channelBound); + /* + * The last thing is to divide up the exposure value into a shutter time + * and analogue gain, according to the current exposure mode. + */ + divideUpExposure(); + /* Finally advertise what we've done. */ + writeAndFinish(imageMetadata, desaturate); +} + +bool AgcChannel::updateLockStatus(DeviceStatus const &deviceStatus) +{ + const double errorFactor = 0.10; /* make these customisable? */ + const int maxLockCount = 5; + /* Reset "lock count" when we exceed this multiple of errorFactor */ + const double resetMargin = 1.5; + + /* Add 200us to the exposure time error to allow for line quantisation. */ + Duration exposureError = lastDeviceStatus_.shutterSpeed * errorFactor + 200us; + double gainError = lastDeviceStatus_.analogueGain * errorFactor; + Duration targetError = lastTargetExposure_ * errorFactor; + + /* + * Note that we don't know the exposure/gain limits of the sensor, so + * the values we keep requesting may be unachievable. For this reason + * we only insist that we're close to values in the past few frames. + */ + if (deviceStatus.shutterSpeed > lastDeviceStatus_.shutterSpeed - exposureError && + deviceStatus.shutterSpeed < lastDeviceStatus_.shutterSpeed + exposureError && + deviceStatus.analogueGain > lastDeviceStatus_.analogueGain - gainError && + deviceStatus.analogueGain < lastDeviceStatus_.analogueGain + gainError && + status_.targetExposureValue > lastTargetExposure_ - targetError && + status_.targetExposureValue < lastTargetExposure_ + targetError) + lockCount_ = std::min(lockCount_ + 1, maxLockCount); + else if (deviceStatus.shutterSpeed < lastDeviceStatus_.shutterSpeed - resetMargin * exposureError || + deviceStatus.shutterSpeed > lastDeviceStatus_.shutterSpeed + resetMargin * exposureError || + deviceStatus.analogueGain < lastDeviceStatus_.analogueGain - resetMargin * gainError || + deviceStatus.analogueGain > lastDeviceStatus_.analogueGain + resetMargin * gainError || + status_.targetExposureValue < lastTargetExposure_ - resetMargin * targetError || + status_.targetExposureValue > lastTargetExposure_ + resetMargin * targetError) + lockCount_ = 0; + + lastDeviceStatus_ = deviceStatus; + lastTargetExposure_ = status_.targetExposureValue; + + LOG(RPiAgc, Debug) << "Lock count updated to " << lockCount_; + return lockCount_ == maxLockCount; +} + +void AgcChannel::housekeepConfig() +{ + /* First fetch all the up-to-date settings, so no one else has to do it. */ + status_.ev = ev_; + status_.fixedShutter = limitShutter(fixedShutter_); + status_.fixedAnalogueGain = fixedAnalogueGain_; + status_.flickerPeriod = flickerPeriod_; + LOG(RPiAgc, Debug) << "ev " << status_.ev << " fixedShutter " + << status_.fixedShutter << " fixedAnalogueGain " + << status_.fixedAnalogueGain; + /* + * Make sure the "mode" pointers point to the up-to-date things, if + * they've changed. + */ + if (meteringModeName_ != status_.meteringMode) { + auto it = config_.meteringModes.find(meteringModeName_); + if (it == config_.meteringModes.end()) { + LOG(RPiAgc, Warning) << "No metering mode " << meteringModeName_; + meteringModeName_ = status_.meteringMode; + } else { + meteringMode_ = &it->second; + status_.meteringMode = meteringModeName_; + } + } + if (exposureModeName_ != status_.exposureMode) { + auto it = config_.exposureModes.find(exposureModeName_); + if (it == config_.exposureModes.end()) { + LOG(RPiAgc, Warning) << "No exposure profile " << exposureModeName_; + exposureModeName_ = status_.exposureMode; + } else { + exposureMode_ = &it->second; + status_.exposureMode = exposureModeName_; + } + } + if (constraintModeName_ != status_.constraintMode) { + auto it = config_.constraintModes.find(constraintModeName_); + if (it == config_.constraintModes.end()) { + LOG(RPiAgc, Warning) << "No constraint list " << constraintModeName_; + constraintModeName_ = status_.constraintMode; + } else { + constraintMode_ = &it->second; + status_.constraintMode = constraintModeName_; + } + } + LOG(RPiAgc, Debug) << "exposureMode " + << exposureModeName_ << " constraintMode " + << constraintModeName_ << " meteringMode " + << meteringModeName_; +} + +void AgcChannel::fetchCurrentExposure(DeviceStatus const &deviceStatus) +{ + current_.shutter = deviceStatus.shutterSpeed; + current_.analogueGain = deviceStatus.analogueGain; + current_.totalExposure = 0s; /* this value is unused */ + current_.totalExposureNoDG = current_.shutter * current_.analogueGain; +} + +void AgcChannel::fetchAwbStatus(Metadata *imageMetadata) +{ + if (imageMetadata->get("awb.status", awb_) != 0) + LOG(RPiAgc, Debug) << "No AWB status found"; +} + +static double computeInitialY(StatisticsPtr &stats, AwbStatus const &awb, + std::vector<double> &weights, double gain) +{ + constexpr uint64_t maxVal = 1 << Statistics::NormalisationFactorPow2; + + /* + * If we have no AGC region stats, but do have a a Y histogram, use that + * directly to caluclate the mean Y value of the image. + */ + if (!stats->agcRegions.numRegions() && stats->yHist.bins()) { + /* + * When the gain is applied to the histogram, anything below minBin + * will scale up directly with the gain, but anything above that + * will saturate into the top bin. + */ + auto &hist = stats->yHist; + double minBin = std::min(1.0, 1.0 / gain) * hist.bins(); + double binMean = hist.interBinMean(0.0, minBin); + double numUnsaturated = hist.cumulativeFreq(minBin); + /* This term is from all the pixels that won't saturate. */ + double ySum = binMean * gain * numUnsaturated; + /* And add the ones that will saturate. */ + ySum += (hist.total() - numUnsaturated) * hist.bins(); + return ySum / hist.total() / hist.bins(); + } + + ASSERT(weights.size() == stats->agcRegions.numRegions()); + + /* + * Note that the weights are applied by the IPA to the statistics directly, + * before they are given to us here. + */ + double rSum = 0, gSum = 0, bSum = 0, pixelSum = 0; + for (unsigned int i = 0; i < stats->agcRegions.numRegions(); i++) { + auto ®ion = stats->agcRegions.get(i); + rSum += std::min<double>(region.val.rSum * gain, (maxVal - 1) * region.counted); + gSum += std::min<double>(region.val.gSum * gain, (maxVal - 1) * region.counted); + bSum += std::min<double>(region.val.bSum * gain, (maxVal - 1) * region.counted); + pixelSum += region.counted; + } + if (pixelSum == 0.0) { + LOG(RPiAgc, Warning) << "computeInitialY: pixelSum is zero"; + return 0; + } + + double ySum; + /* Factor in the AWB correction if needed. */ + if (stats->agcStatsPos == Statistics::AgcStatsPos::PreWb) { + ySum = rSum * awb.gainR * .299 + + gSum * awb.gainG * .587 + + bSum * awb.gainB * .114; + } else + ySum = rSum * .299 + gSum * .587 + bSum * .114; + + return ySum / pixelSum / (1 << 16); +} + +/* + * We handle extra gain through EV by adjusting our Y targets. However, you + * simply can't monitor histograms once they get very close to (or beyond!) + * saturation, so we clamp the Y targets to this value. It does mean that EV + * increases don't necessarily do quite what you might expect in certain + * (contrived) cases. + */ + +static constexpr double EvGainYTargetLimit = 0.9; + +static double constraintComputeGain(AgcConstraint &c, const Histogram &h, double lux, + double evGain, double &targetY) +{ + targetY = c.yTarget.eval(c.yTarget.domain().clip(lux)); + targetY = std::min(EvGainYTargetLimit, targetY * evGain); + double iqm = h.interQuantileMean(c.qLo, c.qHi); + return (targetY * h.bins()) / iqm; +} + +void AgcChannel::computeGain(StatisticsPtr &statistics, Metadata *imageMetadata, + double &gain, double &targetY) +{ + struct LuxStatus lux = {}; + lux.lux = 400; /* default lux level to 400 in case no metadata found */ + if (imageMetadata->get("lux.status", lux) != 0) + LOG(RPiAgc, Warning) << "No lux level found"; + const Histogram &h = statistics->yHist; + double evGain = status_.ev * config_.baseEv; + /* + * The initial gain and target_Y come from some of the regions. After + * that we consider the histogram constraints. + */ + targetY = config_.yTarget.eval(config_.yTarget.domain().clip(lux.lux)); + targetY = std::min(EvGainYTargetLimit, targetY * evGain); + + /* + * Do this calculation a few times as brightness increase can be + * non-linear when there are saturated regions. + */ + gain = 1.0; + for (int i = 0; i < 8; i++) { + double initialY = computeInitialY(statistics, awb_, meteringMode_->weights, gain); + double extraGain = std::min(10.0, targetY / (initialY + .001)); + gain *= extraGain; + LOG(RPiAgc, Debug) << "Initial Y " << initialY << " target " << targetY + << " gives gain " << gain; + if (extraGain < 1.01) /* close enough */ + break; + } + + for (auto &c : *constraintMode_) { + double newTargetY; + double newGain = constraintComputeGain(c, h, lux.lux, evGain, newTargetY); + LOG(RPiAgc, Debug) << "Constraint has target_Y " + << newTargetY << " giving gain " << newGain; + if (c.bound == AgcConstraint::Bound::LOWER && newGain > gain) { + LOG(RPiAgc, Debug) << "Lower bound constraint adopted"; + gain = newGain; + targetY = newTargetY; + } else if (c.bound == AgcConstraint::Bound::UPPER && newGain < gain) { + LOG(RPiAgc, Debug) << "Upper bound constraint adopted"; + gain = newGain; + targetY = newTargetY; + } + } + LOG(RPiAgc, Debug) << "Final gain " << gain << " (target_Y " << targetY << " ev " + << status_.ev << " base_ev " << config_.baseEv + << ")"; +} + +void AgcChannel::computeTargetExposure(double gain) +{ + if (status_.fixedShutter && status_.fixedAnalogueGain) { + /* + * When ag and shutter are both fixed, we need to drive the + * total exposure so that we end up with a digital gain of at least + * 1/minColourGain. Otherwise we'd desaturate channels causing + * white to go cyan or magenta. + */ + double minColourGain = std::min({ awb_.gainR, awb_.gainG, awb_.gainB, 1.0 }); + ASSERT(minColourGain != 0.0); + target_.totalExposure = + status_.fixedShutter * status_.fixedAnalogueGain / minColourGain; + } else { + /* + * The statistics reflect the image without digital gain, so the final + * total exposure we're aiming for is: + */ + target_.totalExposure = current_.totalExposureNoDG * gain; + /* The final target exposure is also limited to what the exposure mode allows. */ + Duration maxShutter = status_.fixedShutter + ? status_.fixedShutter + : exposureMode_->shutter.back(); + maxShutter = limitShutter(maxShutter); + Duration maxTotalExposure = + maxShutter * + (status_.fixedAnalogueGain != 0.0 + ? status_.fixedAnalogueGain + : exposureMode_->gain.back()); + target_.totalExposure = std::min(target_.totalExposure, maxTotalExposure); + } + LOG(RPiAgc, Debug) << "Target totalExposure " << target_.totalExposure; +} + +bool AgcChannel::applyChannelConstraints(const AgcChannelTotalExposures &channelTotalExposures) +{ + bool channelBound = false; + LOG(RPiAgc, Debug) + << "Total exposure before channel constraints " << filtered_.totalExposure; + + for (const auto &constraint : config_.channelConstraints) { + LOG(RPiAgc, Debug) + << "Check constraint: channel " << constraint.channel << " bound " + << (constraint.bound == AgcChannelConstraint::Bound::UPPER ? "UPPER" : "LOWER") + << " factor " << constraint.factor; + if (constraint.channel >= channelTotalExposures.size() || + !channelTotalExposures[constraint.channel]) { + LOG(RPiAgc, Debug) << "no such channel or no exposure available- skipped"; + continue; + } + + libcamera::utils::Duration limitExposure = + channelTotalExposures[constraint.channel] * constraint.factor; + LOG(RPiAgc, Debug) << "Limit exposure " << limitExposure; + if ((constraint.bound == AgcChannelConstraint::Bound::UPPER && + filtered_.totalExposure > limitExposure) || + (constraint.bound == AgcChannelConstraint::Bound::LOWER && + filtered_.totalExposure < limitExposure)) { + filtered_.totalExposure = limitExposure; + LOG(RPiAgc, Debug) << "Constraint applies"; + channelBound = true; + } else + LOG(RPiAgc, Debug) << "Constraint does not apply"; + } + + LOG(RPiAgc, Debug) + << "Total exposure after channel constraints " << filtered_.totalExposure; + + return channelBound; +} + +bool AgcChannel::applyDigitalGain(double gain, double targetY, bool channelBound) +{ + double minColourGain = std::min({ awb_.gainR, awb_.gainG, awb_.gainB, 1.0 }); + ASSERT(minColourGain != 0.0); + double dg = 1.0 / minColourGain; + /* + * I think this pipeline subtracts black level and rescales before we + * get the stats, so no need to worry about it. + */ + LOG(RPiAgc, Debug) << "after AWB, target dg " << dg << " gain " << gain + << " target_Y " << targetY; + /* + * Finally, if we're trying to reduce exposure but the target_Y is + * "close" to 1.0, then the gain computed for that constraint will be + * only slightly less than one, because the measured Y can never be + * larger than 1.0. When this happens, demand a large digital gain so + * that the exposure can be reduced, de-saturating the image much more + * quickly (and we then approach the correct value more quickly from + * below). + */ + bool desaturate = false; + if (config_.desaturate) + desaturate = !channelBound && + targetY > config_.fastReduceThreshold && gain < sqrt(targetY); + if (desaturate) + dg /= config_.fastReduceThreshold; + LOG(RPiAgc, Debug) << "Digital gain " << dg << " desaturate? " << desaturate; + filtered_.totalExposureNoDG = filtered_.totalExposure / dg; + LOG(RPiAgc, Debug) << "Target totalExposureNoDG " << filtered_.totalExposureNoDG; + return desaturate; +} + +void AgcChannel::filterExposure() +{ + double speed = config_.speed; + double stableRegion = config_.stableRegion; + + /* + * AGC adapts instantly if both shutter and gain are directly specified + * or we're in the startup phase. + */ + if ((status_.fixedShutter && status_.fixedAnalogueGain) || + frameCount_ <= config_.startupFrames) + speed = 1.0; + if (!filtered_.totalExposure) { + filtered_.totalExposure = target_.totalExposure; + } else if (filtered_.totalExposure * (1.0 - stableRegion) < target_.totalExposure && + filtered_.totalExposure * (1.0 + stableRegion) > target_.totalExposure) { + /* Total exposure must change by more than this or we leave it alone. */ + } else { + /* + * If close to the result go faster, to save making so many + * micro-adjustments on the way. (Make this customisable?) + */ + if (filtered_.totalExposure < 1.2 * target_.totalExposure && + filtered_.totalExposure > 0.8 * target_.totalExposure) + speed = sqrt(speed); + filtered_.totalExposure = speed * target_.totalExposure + + filtered_.totalExposure * (1.0 - speed); + } + LOG(RPiAgc, Debug) << "After filtering, totalExposure " << filtered_.totalExposure + << " no dg " << filtered_.totalExposureNoDG; +} + +void AgcChannel::divideUpExposure() +{ + /* + * Sending the fixed shutter/gain cases through the same code may seem + * unnecessary, but it will make more sense when extend this to cover + * variable aperture. + */ + Duration exposureValue = filtered_.totalExposureNoDG; + Duration shutterTime; + double analogueGain; + shutterTime = status_.fixedShutter ? status_.fixedShutter + : exposureMode_->shutter[0]; + shutterTime = limitShutter(shutterTime); + analogueGain = status_.fixedAnalogueGain != 0.0 ? status_.fixedAnalogueGain + : exposureMode_->gain[0]; + analogueGain = limitGain(analogueGain); + if (shutterTime * analogueGain < exposureValue) { + for (unsigned int stage = 1; + stage < exposureMode_->gain.size(); stage++) { + if (!status_.fixedShutter) { + Duration stageShutter = + limitShutter(exposureMode_->shutter[stage]); + if (stageShutter * analogueGain >= exposureValue) { + shutterTime = exposureValue / analogueGain; + break; + } + shutterTime = stageShutter; + } + if (status_.fixedAnalogueGain == 0.0) { + if (exposureMode_->gain[stage] * shutterTime >= exposureValue) { + analogueGain = exposureValue / shutterTime; + break; + } + analogueGain = exposureMode_->gain[stage]; + analogueGain = limitGain(analogueGain); + } + } + } + LOG(RPiAgc, Debug) << "Divided up shutter and gain are " << shutterTime << " and " + << analogueGain; + /* + * Finally adjust shutter time for flicker avoidance (require both + * shutter and gain not to be fixed). + */ + if (!status_.fixedShutter && !status_.fixedAnalogueGain && + status_.flickerPeriod) { + int flickerPeriods = shutterTime / status_.flickerPeriod; + if (flickerPeriods) { + Duration newShutterTime = flickerPeriods * status_.flickerPeriod; + analogueGain *= shutterTime / newShutterTime; + /* + * We should still not allow the ag to go over the + * largest value in the exposure mode. Note that this + * may force more of the total exposure into the digital + * gain as a side-effect. + */ + analogueGain = std::min(analogueGain, exposureMode_->gain.back()); + analogueGain = limitGain(analogueGain); + shutterTime = newShutterTime; + } + LOG(RPiAgc, Debug) << "After flicker avoidance, shutter " + << shutterTime << " gain " << analogueGain; + } + filtered_.shutter = shutterTime; + filtered_.analogueGain = analogueGain; +} + +void AgcChannel::writeAndFinish(Metadata *imageMetadata, bool desaturate) +{ + status_.totalExposureValue = filtered_.totalExposure; + status_.targetExposureValue = desaturate ? 0s : target_.totalExposure; + status_.shutterTime = filtered_.shutter; + status_.analogueGain = filtered_.analogueGain; + /* + * Write to metadata as well, in case anyone wants to update the camera + * immediately. + */ + imageMetadata->set("agc.status", status_); + LOG(RPiAgc, Debug) << "Output written, total exposure requested is " + << filtered_.totalExposure; + LOG(RPiAgc, Debug) << "Camera exposure update: shutter time " << filtered_.shutter + << " analogue gain " << filtered_.analogueGain; +} + +Duration AgcChannel::limitShutter(Duration shutter) +{ + /* + * shutter == 0 is a special case for fixed shutter values, and must pass + * through unchanged + */ + if (!shutter) + return shutter; + + shutter = std::clamp(shutter, mode_.minShutter, maxShutter_); + return shutter; +} + +double AgcChannel::limitGain(double gain) const +{ + /* + * Only limit the lower bounds of the gain value to what the sensor limits. + * The upper bound on analogue gain will be made up with additional digital + * gain applied by the ISP. + * + * gain == 0.0 is a special case for fixed shutter values, and must pass + * through unchanged + */ + if (!gain) + return gain; + + gain = std::max(gain, mode_.minAnalogueGain); + return gain; +} diff --git a/src/ipa/rpi/controller/rpi/agc_channel.h b/src/ipa/rpi/controller/rpi/agc_channel.h new file mode 100644 index 00000000..99033e23 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/agc_channel.h @@ -0,0 +1,153 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2023, Raspberry Pi Ltd + * + * AGC/AEC control algorithm + */ +#pragma once + +#include <map> +#include <string> +#include <vector> + +#include <libcamera/base/utils.h> + +#include "../agc_status.h" +#include "../awb_status.h" +#include "../controller.h" +#include "../pwl.h" + +/* This is our implementation of AGC. */ + +namespace RPiController { + +using AgcChannelTotalExposures = std::vector<libcamera::utils::Duration>; + +struct AgcMeteringMode { + std::vector<double> weights; + int read(const libcamera::YamlObject ¶ms); +}; + +struct AgcExposureMode { + std::vector<libcamera::utils::Duration> shutter; + std::vector<double> gain; + int read(const libcamera::YamlObject ¶ms); +}; + +struct AgcConstraint { + enum class Bound { LOWER = 0, + UPPER = 1 }; + Bound bound; + double qLo; + double qHi; + Pwl yTarget; + int read(const libcamera::YamlObject ¶ms); +}; + +typedef std::vector<AgcConstraint> AgcConstraintMode; + +struct AgcChannelConstraint { + enum class Bound { LOWER = 0, + UPPER = 1 }; + Bound bound; + unsigned int channel; + double factor; + int read(const libcamera::YamlObject ¶ms); +}; + +struct AgcConfig { + int read(const libcamera::YamlObject ¶ms); + std::map<std::string, AgcMeteringMode> meteringModes; + std::map<std::string, AgcExposureMode> exposureModes; + std::map<std::string, AgcConstraintMode> constraintModes; + std::vector<AgcChannelConstraint> channelConstraints; + Pwl yTarget; + double speed; + uint16_t startupFrames; + unsigned int convergenceFrames; + double maxChange; + double minChange; + double fastReduceThreshold; + double speedUpThreshold; + std::string defaultMeteringMode; + std::string defaultExposureMode; + std::string defaultConstraintMode; + double baseEv; + libcamera::utils::Duration defaultExposureTime; + double defaultAnalogueGain; + double stableRegion; + bool desaturate; +}; + +class AgcChannel +{ +public: + AgcChannel(); + int read(const libcamera::YamlObject ¶ms, + const Controller::HardwareConfig &hardwareConfig); + unsigned int getConvergenceFrames() const; + std::vector<double> const &getWeights() const; + void setEv(double ev); + void setFlickerPeriod(libcamera::utils::Duration flickerPeriod); + void setMaxShutter(libcamera::utils::Duration maxShutter); + void setFixedShutter(libcamera::utils::Duration fixedShutter); + void setFixedAnalogueGain(double fixedAnalogueGain); + void setMeteringMode(std::string const &meteringModeName); + void setExposureMode(std::string const &exposureModeName); + void setConstraintMode(std::string const &contraintModeName); + void enableAuto(); + void disableAuto(); + void switchMode(CameraMode const &cameraMode, Metadata *metadata); + void prepare(Metadata *imageMetadata); + void process(StatisticsPtr &stats, DeviceStatus const &deviceStatus, Metadata *imageMetadata, + const AgcChannelTotalExposures &channelTotalExposures); + +private: + bool updateLockStatus(DeviceStatus const &deviceStatus); + AgcConfig config_; + void housekeepConfig(); + void fetchCurrentExposure(DeviceStatus const &deviceStatus); + void fetchAwbStatus(Metadata *imageMetadata); + void computeGain(StatisticsPtr &statistics, Metadata *imageMetadata, + double &gain, double &targetY); + void computeTargetExposure(double gain); + void filterExposure(); + bool applyChannelConstraints(const AgcChannelTotalExposures &channelTotalExposures); + bool applyDigitalGain(double gain, double targetY, bool channelBound); + void divideUpExposure(); + void writeAndFinish(Metadata *imageMetadata, bool desaturate); + libcamera::utils::Duration limitShutter(libcamera::utils::Duration shutter); + double limitGain(double gain) const; + AgcMeteringMode *meteringMode_; + AgcExposureMode *exposureMode_; + AgcConstraintMode *constraintMode_; + CameraMode mode_; + uint64_t frameCount_; + AwbStatus awb_; + struct ExposureValues { + ExposureValues(); + + libcamera::utils::Duration shutter; + double analogueGain; + libcamera::utils::Duration totalExposure; + libcamera::utils::Duration totalExposureNoDG; /* without digital gain */ + }; + ExposureValues current_; /* values for the current frame */ + ExposureValues target_; /* calculate the values we want here */ + ExposureValues filtered_; /* these values are filtered towards target */ + AgcStatus status_; + int lockCount_; + DeviceStatus lastDeviceStatus_; + libcamera::utils::Duration lastTargetExposure_; + /* Below here the "settings" that applications can change. */ + std::string meteringModeName_; + std::string exposureModeName_; + std::string constraintModeName_; + double ev_; + libcamera::utils::Duration flickerPeriod_; + libcamera::utils::Duration maxShutter_; + libcamera::utils::Duration fixedShutter_; + double fixedAnalogueGain_; +}; + +} /* namespace RPiController */ diff --git a/src/ipa/rpi/controller/rpi/alsc.cpp b/src/ipa/rpi/controller/rpi/alsc.cpp new file mode 100644 index 00000000..67029fc3 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/alsc.cpp @@ -0,0 +1,867 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * ALSC (auto lens shading correction) control algorithm + */ + +#include <algorithm> +#include <functional> +#include <math.h> +#include <numeric> + +#include <libcamera/base/log.h> +#include <libcamera/base/span.h> + +#include "../awb_status.h" +#include "alsc.h" + +/* Raspberry Pi ALSC (Auto Lens Shading Correction) algorithm. */ + +using namespace RPiController; +using namespace libcamera; + +LOG_DEFINE_CATEGORY(RPiAlsc) + +#define NAME "rpi.alsc" + +static const double InsufficientData = -1.0; + +Alsc::Alsc(Controller *controller) + : Algorithm(controller) +{ + asyncAbort_ = asyncStart_ = asyncStarted_ = asyncFinished_ = false; + asyncThread_ = std::thread(std::bind(&Alsc::asyncFunc, this)); +} + +Alsc::~Alsc() +{ + { + std::lock_guard<std::mutex> lock(mutex_); + asyncAbort_ = true; + } + asyncSignal_.notify_one(); + asyncThread_.join(); +} + +char const *Alsc::name() const +{ + return NAME; +} + +static int generateLut(Array2D<double> &lut, const libcamera::YamlObject ¶ms) +{ + /* These must be signed ints for the co-ordinate calculations below. */ + int X = lut.dimensions().width, Y = lut.dimensions().height; + double cstrength = params["corner_strength"].get<double>(2.0); + if (cstrength <= 1.0) { + LOG(RPiAlsc, Error) << "corner_strength must be > 1.0"; + return -EINVAL; + } + + double asymmetry = params["asymmetry"].get<double>(1.0); + if (asymmetry < 0) { + LOG(RPiAlsc, Error) << "asymmetry must be >= 0"; + return -EINVAL; + } + + double f1 = cstrength - 1, f2 = 1 + sqrt(cstrength); + double R2 = X * Y / 4 * (1 + asymmetry * asymmetry); + int num = 0; + for (int y = 0; y < Y; y++) { + for (int x = 0; x < X; x++) { + double dy = y - Y / 2 + 0.5, + dx = (x - X / 2 + 0.5) * asymmetry; + double r2 = (dx * dx + dy * dy) / R2; + lut[num++] = + (f1 * r2 + f2) * (f1 * r2 + f2) / + (f2 * f2); /* this reproduces the cos^4 rule */ + } + } + return 0; +} + +static int readLut(Array2D<double> &lut, const libcamera::YamlObject ¶ms) +{ + if (params.size() != lut.size()) { + LOG(RPiAlsc, Error) << "Invalid number of entries in LSC table"; + return -EINVAL; + } + + int num = 0; + for (const auto &p : params.asList()) { + auto value = p.get<double>(); + if (!value) + return -EINVAL; + lut[num++] = *value; + } + + return 0; +} + +static int readCalibrations(std::vector<AlscCalibration> &calibrations, + const libcamera::YamlObject ¶ms, + std::string const &name, const Size &size) +{ + if (params.contains(name)) { + double lastCt = 0; + for (const auto &p : params[name].asList()) { + auto value = p["ct"].get<double>(); + if (!value) + return -EINVAL; + double ct = *value; + if (ct <= lastCt) { + LOG(RPiAlsc, Error) + << "Entries in " << name << " must be in increasing ct order"; + return -EINVAL; + } + AlscCalibration calibration; + calibration.ct = lastCt = ct; + + const libcamera::YamlObject &table = p["table"]; + if (table.size() != size.width * size.height) { + LOG(RPiAlsc, Error) + << "Incorrect number of values for ct " + << ct << " in " << name; + return -EINVAL; + } + + int num = 0; + calibration.table.resize(size); + for (const auto &elem : table.asList()) { + value = elem.get<double>(); + if (!value) + return -EINVAL; + calibration.table[num++] = *value; + } + + calibrations.push_back(std::move(calibration)); + LOG(RPiAlsc, Debug) + << "Read " << name << " calibration for ct " << ct; + } + } + return 0; +} + +int Alsc::read(const libcamera::YamlObject ¶ms) +{ + config_.tableSize = getHardwareConfig().awbRegions; + config_.framePeriod = params["frame_period"].get<uint16_t>(12); + config_.startupFrames = params["startup_frames"].get<uint16_t>(10); + config_.speed = params["speed"].get<double>(0.05); + double sigma = params["sigma"].get<double>(0.01); + config_.sigmaCr = params["sigma_Cr"].get<double>(sigma); + config_.sigmaCb = params["sigma_Cb"].get<double>(sigma); + config_.minCount = params["min_count"].get<double>(10.0); + config_.minG = params["min_G"].get<uint16_t>(50); + config_.omega = params["omega"].get<double>(1.3); + config_.nIter = params["n_iter"].get<uint32_t>(config_.tableSize.width + config_.tableSize.height); + config_.luminanceStrength = + params["luminance_strength"].get<double>(1.0); + + config_.luminanceLut.resize(config_.tableSize, 1.0); + int ret = 0; + + if (params.contains("corner_strength")) + ret = generateLut(config_.luminanceLut, params); + else if (params.contains("luminance_lut")) + ret = readLut(config_.luminanceLut, params["luminance_lut"]); + else + LOG(RPiAlsc, Warning) + << "no luminance table - assume unity everywhere"; + if (ret) + return ret; + + ret = readCalibrations(config_.calibrationsCr, params, "calibrations_Cr", + config_.tableSize); + if (ret) + return ret; + ret = readCalibrations(config_.calibrationsCb, params, "calibrations_Cb", + config_.tableSize); + if (ret) + return ret; + + config_.defaultCt = params["default_ct"].get<double>(4500.0); + config_.threshold = params["threshold"].get<double>(1e-3); + config_.lambdaBound = params["lambda_bound"].get<double>(0.05); + + return 0; +} + +static double getCt(Metadata *metadata, double defaultCt); +static void getCalTable(double ct, std::vector<AlscCalibration> const &calibrations, + Array2D<double> &calTable); +static void resampleCalTable(const Array2D<double> &calTableIn, CameraMode const &cameraMode, + Array2D<double> &calTableOut); +static void compensateLambdasForCal(const Array2D<double> &calTable, + const Array2D<double> &oldLambdas, + Array2D<double> &newLambdas); +static void addLuminanceToTables(std::array<Array2D<double>, 3> &results, + const Array2D<double> &lambdaR, double lambdaG, + const Array2D<double> &lambdaB, + const Array2D<double> &luminanceLut, + double luminanceStrength); + +void Alsc::initialise() +{ + frameCount2_ = frameCount_ = framePhase_ = 0; + firstTime_ = true; + ct_ = config_.defaultCt; + + const size_t XY = config_.tableSize.width * config_.tableSize.height; + + for (auto &r : syncResults_) + r.resize(config_.tableSize); + for (auto &r : prevSyncResults_) + r.resize(config_.tableSize); + for (auto &r : asyncResults_) + r.resize(config_.tableSize); + + luminanceTable_.resize(config_.tableSize); + asyncLambdaR_.resize(config_.tableSize); + asyncLambdaB_.resize(config_.tableSize); + /* The lambdas are initialised in the SwitchMode. */ + lambdaR_.resize(config_.tableSize); + lambdaB_.resize(config_.tableSize); + + /* Temporaries for the computations, but sensible to allocate this up-front! */ + for (auto &c : tmpC_) + c.resize(config_.tableSize); + for (auto &m : tmpM_) + m.resize(XY); +} + +void Alsc::waitForAysncThread() +{ + if (asyncStarted_) { + asyncStarted_ = false; + std::unique_lock<std::mutex> lock(mutex_); + syncSignal_.wait(lock, [&] { + return asyncFinished_; + }); + asyncFinished_ = false; + } +} + +static bool compareModes(CameraMode const &cm0, CameraMode const &cm1) +{ + /* + * Return true if the modes crop from the sensor significantly differently, + * or if the user transform has changed. + */ + if (cm0.transform != cm1.transform) + return true; + int leftDiff = abs(cm0.cropX - cm1.cropX); + int topDiff = abs(cm0.cropY - cm1.cropY); + int rightDiff = fabs(cm0.cropX + cm0.scaleX * cm0.width - + cm1.cropX - cm1.scaleX * cm1.width); + int bottomDiff = fabs(cm0.cropY + cm0.scaleY * cm0.height - + cm1.cropY - cm1.scaleY * cm1.height); + /* + * These thresholds are a rather arbitrary amount chosen to trigger + * when carrying on with the previously calculated tables might be + * worse than regenerating them (but without the adaptive algorithm). + */ + int thresholdX = cm0.sensorWidth >> 4; + int thresholdY = cm0.sensorHeight >> 4; + return leftDiff > thresholdX || rightDiff > thresholdX || + topDiff > thresholdY || bottomDiff > thresholdY; +} + +void Alsc::switchMode(CameraMode const &cameraMode, + [[maybe_unused]] Metadata *metadata) +{ + /* + * We're going to start over with the tables if there's any "significant" + * change. + */ + bool resetTables = firstTime_ || compareModes(cameraMode_, cameraMode); + + /* Believe the colour temperature from the AWB, if there is one. */ + ct_ = getCt(metadata, ct_); + + /* Ensure the other thread isn't running while we do this. */ + waitForAysncThread(); + + cameraMode_ = cameraMode; + + /* + * We must resample the luminance table like we do the others, but it's + * fixed so we can simply do it up front here. + */ + resampleCalTable(config_.luminanceLut, cameraMode_, luminanceTable_); + + if (resetTables) { + /* + * Upon every "table reset", arrange for something sensible to be + * generated. Construct the tables for the previous recorded colour + * temperature. In order to start over from scratch we initialise + * the lambdas, but the rest of this code then echoes the code in + * doAlsc, without the adaptive algorithm. + */ + std::fill(lambdaR_.begin(), lambdaR_.end(), 1.0); + std::fill(lambdaB_.begin(), lambdaB_.end(), 1.0); + Array2D<double> &calTableR = tmpC_[0], &calTableB = tmpC_[1], &calTableTmp = tmpC_[2]; + getCalTable(ct_, config_.calibrationsCr, calTableTmp); + resampleCalTable(calTableTmp, cameraMode_, calTableR); + getCalTable(ct_, config_.calibrationsCb, calTableTmp); + resampleCalTable(calTableTmp, cameraMode_, calTableB); + compensateLambdasForCal(calTableR, lambdaR_, asyncLambdaR_); + compensateLambdasForCal(calTableB, lambdaB_, asyncLambdaB_); + addLuminanceToTables(syncResults_, asyncLambdaR_, 1.0, asyncLambdaB_, + luminanceTable_, config_.luminanceStrength); + prevSyncResults_ = syncResults_; + framePhase_ = config_.framePeriod; /* run the algo again asap */ + firstTime_ = false; + } +} + +void Alsc::fetchAsyncResults() +{ + LOG(RPiAlsc, Debug) << "Fetch ALSC results"; + asyncFinished_ = false; + asyncStarted_ = false; + syncResults_ = asyncResults_; +} + +double getCt(Metadata *metadata, double defaultCt) +{ + AwbStatus awbStatus; + awbStatus.temperatureK = defaultCt; /* in case nothing found */ + if (metadata->get("awb.status", awbStatus) != 0) + LOG(RPiAlsc, Debug) << "no AWB results found, using " + << awbStatus.temperatureK; + else + LOG(RPiAlsc, Debug) << "AWB results found, using " + << awbStatus.temperatureK; + return awbStatus.temperatureK; +} + +static void copyStats(RgbyRegions ®ions, StatisticsPtr &stats, + std::array<Array2D<double>, 3> &prevSyncResults) +{ + if (!regions.numRegions()) + regions.init(stats->awbRegions.size()); + + const std::vector<double> &rTable = prevSyncResults[0].data(); //status.r; + const std::vector<double> &gTable = prevSyncResults[1].data(); //status.g; + const std::vector<double> &bTable = prevSyncResults[2].data(); //status.b; + for (unsigned int i = 0; i < stats->awbRegions.numRegions(); i++) { + auto r = stats->awbRegions.get(i); + if (stats->colourStatsPos == Statistics::ColourStatsPos::PostLsc) { + r.val.rSum = static_cast<uint64_t>(r.val.rSum / rTable[i]); + r.val.gSum = static_cast<uint64_t>(r.val.gSum / gTable[i]); + r.val.bSum = static_cast<uint64_t>(r.val.bSum / bTable[i]); + } + regions.set(i, r); + } +} + +void Alsc::restartAsync(StatisticsPtr &stats, Metadata *imageMetadata) +{ + LOG(RPiAlsc, Debug) << "Starting ALSC calculation"; + /* + * Get the current colour temperature. It's all we need from the + * metadata. Default to the last CT value (which could be the default). + */ + ct_ = getCt(imageMetadata, ct_); + /* + * We have to copy the statistics here, dividing out our best guess of + * the LSC table that the pipeline applied to them which we get from + * prevSyncResults_. + */ + copyStats(statistics_, stats, prevSyncResults_); + framePhase_ = 0; + asyncStarted_ = true; + { + std::lock_guard<std::mutex> lock(mutex_); + asyncStart_ = true; + } + asyncSignal_.notify_one(); +} + +void Alsc::prepare(Metadata *imageMetadata) +{ + /* + * Count frames since we started, and since we last poked the async + * thread. + */ + if (frameCount_ < (int)config_.startupFrames) + frameCount_++; + double speed = frameCount_ < (int)config_.startupFrames + ? 1.0 + : config_.speed; + LOG(RPiAlsc, Debug) + << "frame count " << frameCount_ << " speed " << speed; + { + std::unique_lock<std::mutex> lock(mutex_); + if (asyncStarted_ && asyncFinished_) + fetchAsyncResults(); + } + /* Apply IIR filter to results and program into the pipeline. */ + for (unsigned int j = 0; j < syncResults_.size(); j++) { + for (unsigned int i = 0; i < syncResults_[j].size(); i++) + prevSyncResults_[j][i] = speed * syncResults_[j][i] + (1.0 - speed) * prevSyncResults_[j][i]; + } + /* Put output values into status metadata. */ + AlscStatus status; + status.r = prevSyncResults_[0].data(); + status.g = prevSyncResults_[1].data(); + status.b = prevSyncResults_[2].data(); + imageMetadata->set("alsc.status", status); + /* + * Put the results in the global metadata as well. This will be used by + * AWB to factor in the colour shading correction. + */ + getGlobalMetadata().set("alsc.status", status); +} + +void Alsc::process(StatisticsPtr &stats, Metadata *imageMetadata) +{ + /* + * Count frames since we started, and since we last poked the async + * thread. + */ + if (framePhase_ < (int)config_.framePeriod) + framePhase_++; + if (frameCount2_ < (int)config_.startupFrames) + frameCount2_++; + LOG(RPiAlsc, Debug) << "frame_phase " << framePhase_; + if (framePhase_ >= (int)config_.framePeriod || + frameCount2_ < (int)config_.startupFrames) { + if (asyncStarted_ == false) + restartAsync(stats, imageMetadata); + } +} + +void Alsc::asyncFunc() +{ + while (true) { + { + std::unique_lock<std::mutex> lock(mutex_); + asyncSignal_.wait(lock, [&] { + return asyncStart_ || asyncAbort_; + }); + asyncStart_ = false; + if (asyncAbort_) + break; + } + doAlsc(); + { + std::lock_guard<std::mutex> lock(mutex_); + asyncFinished_ = true; + } + syncSignal_.notify_one(); + } +} + +void getCalTable(double ct, std::vector<AlscCalibration> const &calibrations, + Array2D<double> &calTable) +{ + if (calibrations.empty()) { + std::fill(calTable.begin(), calTable.end(), 1.0); + LOG(RPiAlsc, Debug) << "no calibrations found"; + } else if (ct <= calibrations.front().ct) { + calTable = calibrations.front().table; + LOG(RPiAlsc, Debug) << "using calibration for " + << calibrations.front().ct; + } else if (ct >= calibrations.back().ct) { + calTable = calibrations.back().table; + LOG(RPiAlsc, Debug) << "using calibration for " + << calibrations.back().ct; + } else { + int idx = 0; + while (ct > calibrations[idx + 1].ct) + idx++; + double ct0 = calibrations[idx].ct, ct1 = calibrations[idx + 1].ct; + LOG(RPiAlsc, Debug) + << "ct is " << ct << ", interpolating between " + << ct0 << " and " << ct1; + for (unsigned int i = 0; i < calTable.size(); i++) + calTable[i] = + (calibrations[idx].table[i] * (ct1 - ct) + + calibrations[idx + 1].table[i] * (ct - ct0)) / + (ct1 - ct0); + } +} + +void resampleCalTable(const Array2D<double> &calTableIn, + CameraMode const &cameraMode, + Array2D<double> &calTableOut) +{ + int X = calTableIn.dimensions().width; + int Y = calTableIn.dimensions().height; + + /* + * Precalculate and cache the x sampling locations and phases to save + * recomputing them on every row. + */ + int xLo[X], xHi[X]; + double xf[X]; + double scaleX = cameraMode.sensorWidth / + (cameraMode.width * cameraMode.scaleX); + double xOff = cameraMode.cropX / (double)cameraMode.sensorWidth; + double x = .5 / scaleX + xOff * X - .5; + double xInc = 1 / scaleX; + for (int i = 0; i < X; i++, x += xInc) { + xLo[i] = floor(x); + xf[i] = x - xLo[i]; + xHi[i] = std::min(xLo[i] + 1, X - 1); + xLo[i] = std::max(xLo[i], 0); + if (!!(cameraMode.transform & libcamera::Transform::HFlip)) { + xLo[i] = X - 1 - xLo[i]; + xHi[i] = X - 1 - xHi[i]; + } + } + /* Now march over the output table generating the new values. */ + double scaleY = cameraMode.sensorHeight / + (cameraMode.height * cameraMode.scaleY); + double yOff = cameraMode.cropY / (double)cameraMode.sensorHeight; + double y = .5 / scaleY + yOff * Y - .5; + double yInc = 1 / scaleY; + for (int j = 0; j < Y; j++, y += yInc) { + int yLo = floor(y); + double yf = y - yLo; + int yHi = std::min(yLo + 1, Y - 1); + yLo = std::max(yLo, 0); + if (!!(cameraMode.transform & libcamera::Transform::VFlip)) { + yLo = Y - 1 - yLo; + yHi = Y - 1 - yHi; + } + double const *rowAbove = calTableIn.ptr() + X * yLo; + double const *rowBelow = calTableIn.ptr() + X * yHi; + double *out = calTableOut.ptr() + X * j; + for (int i = 0; i < X; i++) { + double above = rowAbove[xLo[i]] * (1 - xf[i]) + + rowAbove[xHi[i]] * xf[i]; + double below = rowBelow[xLo[i]] * (1 - xf[i]) + + rowBelow[xHi[i]] * xf[i]; + *(out++) = above * (1 - yf) + below * yf; + } + } +} + +/* Calculate chrominance statistics (R/G and B/G) for each region. */ +static void calculateCrCb(const RgbyRegions &awbRegion, Array2D<double> &cr, + Array2D<double> &cb, uint32_t minCount, uint16_t minG) +{ + for (unsigned int i = 0; i < cr.size(); i++) { + auto s = awbRegion.get(i); + + /* Do not return unreliable, or zero, colour ratio statistics. */ + if (s.counted <= minCount || s.val.gSum / s.counted <= minG || + s.val.rSum / s.counted <= minG || s.val.bSum / s.counted <= minG) { + cr[i] = cb[i] = InsufficientData; + continue; + } + + cr[i] = s.val.rSum / (double)s.val.gSum; + cb[i] = s.val.bSum / (double)s.val.gSum; + } +} + +static void applyCalTable(const Array2D<double> &calTable, Array2D<double> &C) +{ + for (unsigned int i = 0; i < C.size(); i++) + if (C[i] != InsufficientData) + C[i] *= calTable[i]; +} + +void compensateLambdasForCal(const Array2D<double> &calTable, + const Array2D<double> &oldLambdas, + Array2D<double> &newLambdas) +{ + double minNewLambda = std::numeric_limits<double>::max(); + for (unsigned int i = 0; i < newLambdas.size(); i++) { + newLambdas[i] = oldLambdas[i] * calTable[i]; + minNewLambda = std::min(minNewLambda, newLambdas[i]); + } + for (unsigned int i = 0; i < newLambdas.size(); i++) + newLambdas[i] /= minNewLambda; +} + +[[maybe_unused]] static void printCalTable(const Array2D<double> &C) +{ + const Size &size = C.dimensions(); + printf("table: [\n"); + for (unsigned int j = 0; j < size.height; j++) { + for (unsigned int i = 0; i < size.width; i++) { + printf("%5.3f", 1.0 / C[j * size.width + i]); + if (i != size.width - 1 || j != size.height - 1) + printf(","); + } + printf("\n"); + } + printf("]\n"); +} + +/* + * Compute weight out of 1.0 which reflects how similar we wish to make the + * colours of these two regions. + */ +static double computeWeight(double Ci, double Cj, double sigma) +{ + if (Ci == InsufficientData || Cj == InsufficientData) + return 0; + double diff = (Ci - Cj) / sigma; + return exp(-diff * diff / 2); +} + +/* Compute all weights. */ +static void computeW(const Array2D<double> &C, double sigma, + SparseArray<double> &W) +{ + size_t XY = C.size(); + size_t X = C.dimensions().width; + + for (unsigned int i = 0; i < XY; i++) { + /* Start with neighbour above and go clockwise. */ + W[i][0] = i >= X ? computeWeight(C[i], C[i - X], sigma) : 0; + W[i][1] = i % X < X - 1 ? computeWeight(C[i], C[i + 1], sigma) : 0; + W[i][2] = i < XY - X ? computeWeight(C[i], C[i + X], sigma) : 0; + W[i][3] = i % X ? computeWeight(C[i], C[i - 1], sigma) : 0; + } +} + +/* Compute M, the large but sparse matrix such that M * lambdas = 0. */ +static void constructM(const Array2D<double> &C, + const SparseArray<double> &W, + SparseArray<double> &M) +{ + size_t XY = C.size(); + size_t X = C.dimensions().width; + + double epsilon = 0.001; + for (unsigned int i = 0; i < XY; i++) { + /* + * Note how, if C[i] == INSUFFICIENT_DATA, the weights will all + * be zero so the equation is still set up correctly. + */ + int m = !!(i >= X) + !!(i % X < X - 1) + !!(i < XY - X) + + !!(i % X); /* total number of neighbours */ + /* we'll divide the diagonal out straight away */ + double diagonal = (epsilon + W[i][0] + W[i][1] + W[i][2] + W[i][3]) * C[i]; + M[i][0] = i >= X ? (W[i][0] * C[i - X] + epsilon / m * C[i]) / diagonal : 0; + M[i][1] = i % X < X - 1 ? (W[i][1] * C[i + 1] + epsilon / m * C[i]) / diagonal : 0; + M[i][2] = i < XY - X ? (W[i][2] * C[i + X] + epsilon / m * C[i]) / diagonal : 0; + M[i][3] = i % X ? (W[i][3] * C[i - 1] + epsilon / m * C[i]) / diagonal : 0; + } +} + +/* + * In the compute_lambda_ functions, note that the matrix coefficients for the + * left/right neighbours are zero down the left/right edges, so we don't need + * need to test the i value to exclude them. + */ +static double computeLambdaBottom(int i, const SparseArray<double> &M, + Array2D<double> &lambda) +{ + return M[i][1] * lambda[i + 1] + M[i][2] * lambda[i + lambda.dimensions().width] + + M[i][3] * lambda[i - 1]; +} +static double computeLambdaBottomStart(int i, const SparseArray<double> &M, + Array2D<double> &lambda) +{ + return M[i][1] * lambda[i + 1] + M[i][2] * lambda[i + lambda.dimensions().width]; +} +static double computeLambdaInterior(int i, const SparseArray<double> &M, + Array2D<double> &lambda) +{ + return M[i][0] * lambda[i - lambda.dimensions().width] + M[i][1] * lambda[i + 1] + + M[i][2] * lambda[i + lambda.dimensions().width] + M[i][3] * lambda[i - 1]; +} +static double computeLambdaTop(int i, const SparseArray<double> &M, + Array2D<double> &lambda) +{ + return M[i][0] * lambda[i - lambda.dimensions().width] + M[i][1] * lambda[i + 1] + + M[i][3] * lambda[i - 1]; +} +static double computeLambdaTopEnd(int i, const SparseArray<double> &M, + Array2D<double> &lambda) +{ + return M[i][0] * lambda[i - lambda.dimensions().width] + M[i][3] * lambda[i - 1]; +} + +/* Gauss-Seidel iteration with over-relaxation. */ +static double gaussSeidel2Sor(const SparseArray<double> &M, double omega, + Array2D<double> &lambda, double lambdaBound) +{ + int XY = lambda.size(); + int X = lambda.dimensions().width; + const double min = 1 - lambdaBound, max = 1 + lambdaBound; + Array2D<double> oldLambda = lambda; + int i; + lambda[0] = computeLambdaBottomStart(0, M, lambda); + lambda[0] = std::clamp(lambda[0], min, max); + for (i = 1; i < X; i++) { + lambda[i] = computeLambdaBottom(i, M, lambda); + lambda[i] = std::clamp(lambda[i], min, max); + } + for (; i < XY - X; i++) { + lambda[i] = computeLambdaInterior(i, M, lambda); + lambda[i] = std::clamp(lambda[i], min, max); + } + for (; i < XY - 1; i++) { + lambda[i] = computeLambdaTop(i, M, lambda); + lambda[i] = std::clamp(lambda[i], min, max); + } + lambda[i] = computeLambdaTopEnd(i, M, lambda); + lambda[i] = std::clamp(lambda[i], min, max); + /* + * Also solve the system from bottom to top, to help spread the updates + * better. + */ + lambda[i] = computeLambdaTopEnd(i, M, lambda); + lambda[i] = std::clamp(lambda[i], min, max); + for (i = XY - 2; i >= XY - X; i--) { + lambda[i] = computeLambdaTop(i, M, lambda); + lambda[i] = std::clamp(lambda[i], min, max); + } + for (; i >= X; i--) { + lambda[i] = computeLambdaInterior(i, M, lambda); + lambda[i] = std::clamp(lambda[i], min, max); + } + for (; i >= 1; i--) { + lambda[i] = computeLambdaBottom(i, M, lambda); + lambda[i] = std::clamp(lambda[i], min, max); + } + lambda[0] = computeLambdaBottomStart(0, M, lambda); + lambda[0] = std::clamp(lambda[0], min, max); + double maxDiff = 0; + for (i = 0; i < XY; i++) { + lambda[i] = oldLambda[i] + (lambda[i] - oldLambda[i]) * omega; + if (fabs(lambda[i] - oldLambda[i]) > fabs(maxDiff)) + maxDiff = lambda[i] - oldLambda[i]; + } + return maxDiff; +} + +/* Normalise the values so that the smallest value is 1. */ +static void normalise(Array2D<double> &results) +{ + double minval = *std::min_element(results.begin(), results.end()); + std::for_each(results.begin(), results.end(), + [minval](double val) { return val / minval; }); +} + +/* Rescale the values so that the average value is 1. */ +static void reaverage(Array2D<double> &data) +{ + double sum = std::accumulate(data.begin(), data.end(), 0.0); + double ratio = 1 / (sum / data.size()); + std::for_each(data.begin(), data.end(), + [ratio](double val) { return val * ratio; }); +} + +static void runMatrixIterations(const Array2D<double> &C, + Array2D<double> &lambda, + const SparseArray<double> &W, + SparseArray<double> &M, double omega, + unsigned int nIter, double threshold, double lambdaBound) +{ + constructM(C, W, M); + double lastMaxDiff = std::numeric_limits<double>::max(); + for (unsigned int i = 0; i < nIter; i++) { + double maxDiff = fabs(gaussSeidel2Sor(M, omega, lambda, lambdaBound)); + if (maxDiff < threshold) { + LOG(RPiAlsc, Debug) + << "Stop after " << i + 1 << " iterations"; + break; + } + /* + * this happens very occasionally (so make a note), though + * doesn't seem to matter + */ + if (maxDiff > lastMaxDiff) + LOG(RPiAlsc, Debug) + << "Iteration " << i << ": maxDiff gone up " + << lastMaxDiff << " to " << maxDiff; + lastMaxDiff = maxDiff; + } + /* We're going to normalise the lambdas so the total average is 1. */ + reaverage(lambda); +} + +static void addLuminanceRb(Array2D<double> &result, const Array2D<double> &lambda, + const Array2D<double> &luminanceLut, + double luminanceStrength) +{ + for (unsigned int i = 0; i < result.size(); i++) + result[i] = lambda[i] * ((luminanceLut[i] - 1) * luminanceStrength + 1); +} + +static void addLuminanceG(Array2D<double> &result, double lambda, + const Array2D<double> &luminanceLut, + double luminanceStrength) +{ + for (unsigned int i = 0; i < result.size(); i++) + result[i] = lambda * ((luminanceLut[i] - 1) * luminanceStrength + 1); +} + +void addLuminanceToTables(std::array<Array2D<double>, 3> &results, + const Array2D<double> &lambdaR, + double lambdaG, const Array2D<double> &lambdaB, + const Array2D<double> &luminanceLut, + double luminanceStrength) +{ + addLuminanceRb(results[0], lambdaR, luminanceLut, luminanceStrength); + addLuminanceG(results[1], lambdaG, luminanceLut, luminanceStrength); + addLuminanceRb(results[2], lambdaB, luminanceLut, luminanceStrength); + for (auto &r : results) + normalise(r); +} + +void Alsc::doAlsc() +{ + Array2D<double> &cr = tmpC_[0], &cb = tmpC_[1], &calTableR = tmpC_[2], + &calTableB = tmpC_[3], &calTableTmp = tmpC_[4]; + SparseArray<double> &wr = tmpM_[0], &wb = tmpM_[1], &M = tmpM_[2]; + + /* + * Calculate our R/B ("Cr"/"Cb") colour statistics, and assess which are + * usable. + */ + calculateCrCb(statistics_, cr, cb, config_.minCount, config_.minG); + /* + * Fetch the new calibrations (if any) for this CT. Resample them in + * case the camera mode is not full-frame. + */ + getCalTable(ct_, config_.calibrationsCr, calTableTmp); + resampleCalTable(calTableTmp, cameraMode_, calTableR); + getCalTable(ct_, config_.calibrationsCb, calTableTmp); + resampleCalTable(calTableTmp, cameraMode_, calTableB); + /* + * You could print out the cal tables for this image here, if you're + * tuning the algorithm... + * Apply any calibration to the statistics, so the adaptive algorithm + * makes only the extra adjustments. + */ + applyCalTable(calTableR, cr); + applyCalTable(calTableB, cb); + /* Compute weights between zones. */ + computeW(cr, config_.sigmaCr, wr); + computeW(cb, config_.sigmaCb, wb); + /* Run Gauss-Seidel iterations over the resulting matrix, for R and B. */ + runMatrixIterations(cr, lambdaR_, wr, M, config_.omega, config_.nIter, + config_.threshold, config_.lambdaBound); + runMatrixIterations(cb, lambdaB_, wb, M, config_.omega, config_.nIter, + config_.threshold, config_.lambdaBound); + /* + * Fold the calibrated gains into our final lambda values. (Note that on + * the next run, we re-start with the lambda values that don't have the + * calibration gains included.) + */ + compensateLambdasForCal(calTableR, lambdaR_, asyncLambdaR_); + compensateLambdasForCal(calTableB, lambdaB_, asyncLambdaB_); + /* Fold in the luminance table at the appropriate strength. */ + addLuminanceToTables(asyncResults_, asyncLambdaR_, 1.0, + asyncLambdaB_, luminanceTable_, + config_.luminanceStrength); +} + +/* Register algorithm with the system. */ +static Algorithm *create(Controller *controller) +{ + return (Algorithm *)new Alsc(controller); +} +static RegisterAlgorithm reg(NAME, &create); diff --git a/src/ipa/rpi/controller/rpi/alsc.h b/src/ipa/rpi/controller/rpi/alsc.h new file mode 100644 index 00000000..31087982 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/alsc.h @@ -0,0 +1,174 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * ALSC (auto lens shading correction) control algorithm + */ +#pragma once + +#include <array> +#include <mutex> +#include <condition_variable> +#include <thread> +#include <vector> + +#include <libcamera/geometry.h> + +#include "../algorithm.h" +#include "../alsc_status.h" +#include "../statistics.h" + +namespace RPiController { + +/* Algorithm to generate automagic LSC (Lens Shading Correction) tables. */ + +/* + * The Array2D class is a very thin wrapper round std::vector so that it can + * be used in exactly the same way in the code but carries its correct width + * and height ("dimensions") with it. + */ + +template<typename T> +class Array2D +{ +public: + using Size = libcamera::Size; + + const Size &dimensions() const { return dimensions_; } + + size_t size() const { return data_.size(); } + + const std::vector<T> &data() const { return data_; } + + void resize(const Size &dims) + { + dimensions_ = dims; + data_.resize(dims.width * dims.height); + } + + void resize(const Size &dims, const T &value) + { + resize(dims); + std::fill(data_.begin(), data_.end(), value); + } + + T &operator[](int index) { return data_[index]; } + + const T &operator[](int index) const { return data_[index]; } + + T *ptr() { return data_.data(); } + + const T *ptr() const { return data_.data(); } + + auto begin() { return data_.begin(); } + auto end() { return data_.end(); } + +private: + Size dimensions_; + std::vector<T> data_; +}; + +/* + * We'll use the term SparseArray for the large sparse matrices that are + * XY tall but have only 4 non-zero elements on each row. + */ + +template<typename T> +using SparseArray = std::vector<std::array<T, 4>>; + +struct AlscCalibration { + double ct; + Array2D<double> table; +}; + +struct AlscConfig { + /* Only repeat the ALSC calculation every "this many" frames */ + uint16_t framePeriod; + /* number of initial frames for which speed taken as 1.0 (maximum) */ + uint16_t startupFrames; + /* IIR filter speed applied to algorithm results */ + double speed; + double sigmaCr; + double sigmaCb; + double minCount; + uint16_t minG; + double omega; + uint32_t nIter; + Array2D<double> luminanceLut; + double luminanceStrength; + std::vector<AlscCalibration> calibrationsCr; + std::vector<AlscCalibration> calibrationsCb; + double defaultCt; /* colour temperature if no metadata found */ + double threshold; /* iteration termination threshold */ + double lambdaBound; /* upper/lower bound for lambda from a value of 1 */ + libcamera::Size tableSize; +}; + +class Alsc : public Algorithm +{ +public: + Alsc(Controller *controller = NULL); + ~Alsc(); + char const *name() const override; + void initialise() override; + void switchMode(CameraMode const &cameraMode, Metadata *metadata) override; + int read(const libcamera::YamlObject ¶ms) override; + void prepare(Metadata *imageMetadata) override; + void process(StatisticsPtr &stats, Metadata *imageMetadata) override; + +private: + /* configuration is read-only, and available to both threads */ + AlscConfig config_; + bool firstTime_; + CameraMode cameraMode_; + Array2D<double> luminanceTable_; + std::thread asyncThread_; + void asyncFunc(); /* asynchronous thread function */ + std::mutex mutex_; + /* condvar for async thread to wait on */ + std::condition_variable asyncSignal_; + /* condvar for synchronous thread to wait on */ + std::condition_variable syncSignal_; + /* for sync thread to check if async thread finished (requires mutex) */ + bool asyncFinished_; + /* for async thread to check if it's been told to run (requires mutex) */ + bool asyncStart_; + /* for async thread to check if it's been told to quit (requires mutex) */ + bool asyncAbort_; + + /* + * The following are only for the synchronous thread to use: + * for sync thread to note its has asked async thread to run + */ + bool asyncStarted_; + /* counts up to framePeriod before restarting the async thread */ + int framePhase_; + /* counts up to startupFrames */ + int frameCount_; + /* counts up to startupFrames for Process function */ + int frameCount2_; + std::array<Array2D<double>, 3> syncResults_; + std::array<Array2D<double>, 3> prevSyncResults_; + void waitForAysncThread(); + /* + * The following are for the asynchronous thread to use, though the main + * thread can set/reset them if the async thread is known to be idle: + */ + void restartAsync(StatisticsPtr &stats, Metadata *imageMetadata); + /* copy out the results from the async thread so that it can be restarted */ + void fetchAsyncResults(); + double ct_; + RgbyRegions statistics_; + std::array<Array2D<double>, 3> asyncResults_; + Array2D<double> asyncLambdaR_; + Array2D<double> asyncLambdaB_; + void doAlsc(); + Array2D<double> lambdaR_; + Array2D<double> lambdaB_; + + /* Temporaries for the computations */ + std::array<Array2D<double>, 5> tmpC_; + std::array<SparseArray<double>, 3> tmpM_; +}; + +} /* namespace RPiController */ diff --git a/src/ipa/rpi/controller/rpi/awb.cpp b/src/ipa/rpi/controller/rpi/awb.cpp new file mode 100644 index 00000000..abe5906e --- /dev/null +++ b/src/ipa/rpi/controller/rpi/awb.cpp @@ -0,0 +1,751 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * AWB control algorithm + */ + +#include <assert.h> +#include <functional> + +#include <libcamera/base/log.h> + +#include "../lux_status.h" + +#include "alsc_status.h" +#include "awb.h" + +using namespace RPiController; +using namespace libcamera; + +LOG_DEFINE_CATEGORY(RPiAwb) + +#define NAME "rpi.awb" + +/* + * todo - the locking in this algorithm needs some tidying up as has been done + * elsewhere (ALSC and AGC). + */ + +int AwbMode::read(const libcamera::YamlObject ¶ms) +{ + auto value = params["lo"].get<double>(); + if (!value) + return -EINVAL; + ctLo = *value; + + value = params["hi"].get<double>(); + if (!value) + return -EINVAL; + ctHi = *value; + + return 0; +} + +int AwbPrior::read(const libcamera::YamlObject ¶ms) +{ + auto value = params["lux"].get<double>(); + if (!value) + return -EINVAL; + lux = *value; + + return prior.read(params["prior"]); +} + +static int readCtCurve(Pwl &ctR, Pwl &ctB, const libcamera::YamlObject ¶ms) +{ + if (params.size() % 3) { + LOG(RPiAwb, Error) << "AwbConfig: incomplete CT curve entry"; + return -EINVAL; + } + + if (params.size() < 6) { + LOG(RPiAwb, Error) << "AwbConfig: insufficient points in CT curve"; + return -EINVAL; + } + + const auto &list = params.asList(); + + for (auto it = list.begin(); it != list.end(); it++) { + auto value = it->get<double>(); + if (!value) + return -EINVAL; + double ct = *value; + + assert(it == list.begin() || ct != ctR.domain().end); + + value = (++it)->get<double>(); + if (!value) + return -EINVAL; + ctR.append(ct, *value); + + value = (++it)->get<double>(); + if (!value) + return -EINVAL; + ctB.append(ct, *value); + } + + return 0; +} + +int AwbConfig::read(const libcamera::YamlObject ¶ms) +{ + int ret; + + bayes = params["bayes"].get<int>(1); + framePeriod = params["frame_period"].get<uint16_t>(10); + startupFrames = params["startup_frames"].get<uint16_t>(10); + convergenceFrames = params["convergence_frames"].get<unsigned int>(3); + speed = params["speed"].get<double>(0.05); + + if (params.contains("ct_curve")) { + ret = readCtCurve(ctR, ctB, params["ct_curve"]); + if (ret) + return ret; + /* We will want the inverse functions of these too. */ + ctRInverse = ctR.inverse(); + ctBInverse = ctB.inverse(); + } + + if (params.contains("priors")) { + for (const auto &p : params["priors"].asList()) { + AwbPrior prior; + ret = prior.read(p); + if (ret) + return ret; + if (!priors.empty() && prior.lux <= priors.back().lux) { + LOG(RPiAwb, Error) << "AwbConfig: Prior must be ordered in increasing lux value"; + return -EINVAL; + } + priors.push_back(prior); + } + if (priors.empty()) { + LOG(RPiAwb, Error) << "AwbConfig: no AWB priors configured"; + return ret; + } + } + if (params.contains("modes")) { + for (const auto &[key, value] : params["modes"].asDict()) { + ret = modes[key].read(value); + if (ret) + return ret; + if (defaultMode == nullptr) + defaultMode = &modes[key]; + } + if (defaultMode == nullptr) { + LOG(RPiAwb, Error) << "AwbConfig: no AWB modes configured"; + return -EINVAL; + } + } + + minPixels = params["min_pixels"].get<double>(16.0); + minG = params["min_G"].get<uint16_t>(32); + minRegions = params["min_regions"].get<uint32_t>(10); + deltaLimit = params["delta_limit"].get<double>(0.2); + coarseStep = params["coarse_step"].get<double>(0.2); + transversePos = params["transverse_pos"].get<double>(0.01); + transverseNeg = params["transverse_neg"].get<double>(0.01); + if (transversePos <= 0 || transverseNeg <= 0) { + LOG(RPiAwb, Error) << "AwbConfig: transverse_pos/neg must be > 0"; + return -EINVAL; + } + + sensitivityR = params["sensitivity_r"].get<double>(1.0); + sensitivityB = params["sensitivity_b"].get<double>(1.0); + + if (bayes) { + if (ctR.empty() || ctB.empty() || priors.empty() || + defaultMode == nullptr) { + LOG(RPiAwb, Warning) + << "Bayesian AWB mis-configured - switch to Grey method"; + bayes = false; + } + } + fast = params[fast].get<int>(bayes); /* default to fast for Bayesian, otherwise slow */ + whitepointR = params["whitepoint_r"].get<double>(0.0); + whitepointB = params["whitepoint_b"].get<double>(0.0); + if (bayes == false) + sensitivityR = sensitivityB = 1.0; /* nor do sensitivities make any sense */ + return 0; +} + +Awb::Awb(Controller *controller) + : AwbAlgorithm(controller) +{ + asyncAbort_ = asyncStart_ = asyncStarted_ = asyncFinished_ = false; + mode_ = nullptr; + manualR_ = manualB_ = 0.0; + asyncThread_ = std::thread(std::bind(&Awb::asyncFunc, this)); +} + +Awb::~Awb() +{ + { + std::lock_guard<std::mutex> lock(mutex_); + asyncAbort_ = true; + } + asyncSignal_.notify_one(); + asyncThread_.join(); +} + +char const *Awb::name() const +{ + return NAME; +} + +int Awb::read(const libcamera::YamlObject ¶ms) +{ + return config_.read(params); +} + +void Awb::initialise() +{ + frameCount_ = framePhase_ = 0; + /* + * Put something sane into the status that we are filtering towards, + * just in case the first few frames don't have anything meaningful in + * them. + */ + if (!config_.ctR.empty() && !config_.ctB.empty()) { + syncResults_.temperatureK = config_.ctR.domain().clip(4000); + syncResults_.gainR = 1.0 / config_.ctR.eval(syncResults_.temperatureK); + syncResults_.gainG = 1.0; + syncResults_.gainB = 1.0 / config_.ctB.eval(syncResults_.temperatureK); + } else { + /* random values just to stop the world blowing up */ + syncResults_.temperatureK = 4500; + syncResults_.gainR = syncResults_.gainG = syncResults_.gainB = 1.0; + } + prevSyncResults_ = syncResults_; + asyncResults_ = syncResults_; +} + +void Awb::initialValues(double &gainR, double &gainB) +{ + gainR = syncResults_.gainR; + gainB = syncResults_.gainB; +} + +void Awb::disableAuto() +{ + /* Freeze the most recent values, and treat them as manual gains */ + manualR_ = syncResults_.gainR = prevSyncResults_.gainR; + manualB_ = syncResults_.gainB = prevSyncResults_.gainB; + syncResults_.gainG = prevSyncResults_.gainG; + syncResults_.temperatureK = prevSyncResults_.temperatureK; +} + +void Awb::enableAuto() +{ + manualR_ = 0.0; + manualB_ = 0.0; +} + +unsigned int Awb::getConvergenceFrames() const +{ + /* + * If not in auto mode, there is no convergence + * to happen, so no need to drop any frames - return zero. + */ + if (!isAutoEnabled()) + return 0; + else + return config_.convergenceFrames; +} + +void Awb::setMode(std::string const &modeName) +{ + modeName_ = modeName; +} + +void Awb::setManualGains(double manualR, double manualB) +{ + /* If any of these are 0.0, we swich back to auto. */ + manualR_ = manualR; + manualB_ = manualB; + /* + * If not in auto mode, set these values into the syncResults which + * means that Prepare() will adopt them immediately. + */ + if (!isAutoEnabled()) { + syncResults_.gainR = prevSyncResults_.gainR = manualR_; + syncResults_.gainG = prevSyncResults_.gainG = 1.0; + syncResults_.gainB = prevSyncResults_.gainB = manualB_; + if (config_.bayes) { + /* Also estimate the best corresponding colour temperature from the curves. */ + double ctR = config_.ctRInverse.eval(config_.ctRInverse.domain().clip(1 / manualR_)); + double ctB = config_.ctBInverse.eval(config_.ctBInverse.domain().clip(1 / manualB_)); + prevSyncResults_.temperatureK = (ctR + ctB) / 2; + syncResults_.temperatureK = prevSyncResults_.temperatureK; + } + } +} + +void Awb::switchMode([[maybe_unused]] CameraMode const &cameraMode, + Metadata *metadata) +{ + /* Let other algorithms know the current white balance values. */ + metadata->set("awb.status", prevSyncResults_); +} + +bool Awb::isAutoEnabled() const +{ + return manualR_ == 0.0 || manualB_ == 0.0; +} + +void Awb::fetchAsyncResults() +{ + LOG(RPiAwb, Debug) << "Fetch AWB results"; + asyncFinished_ = false; + asyncStarted_ = false; + /* + * It's possible manual gains could be set even while the async + * thread was running, so only copy the results if still in auto mode. + */ + if (isAutoEnabled()) + syncResults_ = asyncResults_; +} + +void Awb::restartAsync(StatisticsPtr &stats, double lux) +{ + LOG(RPiAwb, Debug) << "Starting AWB calculation"; + /* this makes a new reference which belongs to the asynchronous thread */ + statistics_ = stats; + /* store the mode as it could technically change */ + auto m = config_.modes.find(modeName_); + mode_ = m != config_.modes.end() + ? &m->second + : (mode_ == nullptr ? config_.defaultMode : mode_); + lux_ = lux; + framePhase_ = 0; + asyncStarted_ = true; + size_t len = modeName_.copy(asyncResults_.mode, + sizeof(asyncResults_.mode) - 1); + asyncResults_.mode[len] = '\0'; + { + std::lock_guard<std::mutex> lock(mutex_); + asyncStart_ = true; + } + asyncSignal_.notify_one(); +} + +void Awb::prepare(Metadata *imageMetadata) +{ + if (frameCount_ < (int)config_.startupFrames) + frameCount_++; + double speed = frameCount_ < (int)config_.startupFrames + ? 1.0 + : config_.speed; + LOG(RPiAwb, Debug) + << "frame_count " << frameCount_ << " speed " << speed; + { + std::unique_lock<std::mutex> lock(mutex_); + if (asyncStarted_ && asyncFinished_) + fetchAsyncResults(); + } + /* Finally apply IIR filter to results and put into metadata. */ + memcpy(prevSyncResults_.mode, syncResults_.mode, + sizeof(prevSyncResults_.mode)); + prevSyncResults_.temperatureK = speed * syncResults_.temperatureK + + (1.0 - speed) * prevSyncResults_.temperatureK; + prevSyncResults_.gainR = speed * syncResults_.gainR + + (1.0 - speed) * prevSyncResults_.gainR; + prevSyncResults_.gainG = speed * syncResults_.gainG + + (1.0 - speed) * prevSyncResults_.gainG; + prevSyncResults_.gainB = speed * syncResults_.gainB + + (1.0 - speed) * prevSyncResults_.gainB; + imageMetadata->set("awb.status", prevSyncResults_); + LOG(RPiAwb, Debug) + << "Using AWB gains r " << prevSyncResults_.gainR << " g " + << prevSyncResults_.gainG << " b " + << prevSyncResults_.gainB; +} + +void Awb::process(StatisticsPtr &stats, Metadata *imageMetadata) +{ + /* Count frames since we last poked the async thread. */ + if (framePhase_ < (int)config_.framePeriod) + framePhase_++; + LOG(RPiAwb, Debug) << "frame_phase " << framePhase_; + /* We do not restart the async thread if we're not in auto mode. */ + if (isAutoEnabled() && + (framePhase_ >= (int)config_.framePeriod || + frameCount_ < (int)config_.startupFrames)) { + /* Update any settings and any image metadata that we need. */ + struct LuxStatus luxStatus = {}; + luxStatus.lux = 400; /* in case no metadata */ + if (imageMetadata->get("lux.status", luxStatus) != 0) + LOG(RPiAwb, Debug) << "No lux metadata found"; + LOG(RPiAwb, Debug) << "Awb lux value is " << luxStatus.lux; + + if (asyncStarted_ == false) + restartAsync(stats, luxStatus.lux); + } +} + +void Awb::asyncFunc() +{ + while (true) { + { + std::unique_lock<std::mutex> lock(mutex_); + asyncSignal_.wait(lock, [&] { + return asyncStart_ || asyncAbort_; + }); + asyncStart_ = false; + if (asyncAbort_) + break; + } + doAwb(); + { + std::lock_guard<std::mutex> lock(mutex_); + asyncFinished_ = true; + } + syncSignal_.notify_one(); + } +} + +static void generateStats(std::vector<Awb::RGB> &zones, + StatisticsPtr &stats, double minPixels, + double minG, Metadata &globalMetadata) +{ + std::scoped_lock<RPiController::Metadata> l(globalMetadata); + + for (unsigned int i = 0; i < stats->awbRegions.numRegions(); i++) { + Awb::RGB zone; + auto ®ion = stats->awbRegions.get(i); + if (region.counted >= minPixels) { + zone.G = region.val.gSum / region.counted; + if (zone.G < minG) + continue; + zone.R = region.val.rSum / region.counted; + zone.B = region.val.bSum / region.counted; + /* Factor in the ALSC applied colour shading correction if required. */ + const AlscStatus *alscStatus = globalMetadata.getLocked<AlscStatus>("alsc.status"); + if (stats->colourStatsPos == Statistics::ColourStatsPos::PreLsc && alscStatus) { + zone.R *= alscStatus->r[i]; + zone.G *= alscStatus->g[i]; + zone.B *= alscStatus->b[i]; + } + zones.push_back(zone); + } + } +} + +void Awb::prepareStats() +{ + zones_.clear(); + /* + * LSC has already been applied to the stats in this pipeline, so stop + * any LSC compensation. We also ignore config_.fast in this version. + */ + generateStats(zones_, statistics_, config_.minPixels, + config_.minG, getGlobalMetadata()); + /* + * apply sensitivities, so values appear to come from our "canonical" + * sensor. + */ + for (auto &zone : zones_) { + zone.R *= config_.sensitivityR; + zone.B *= config_.sensitivityB; + } +} + +double Awb::computeDelta2Sum(double gainR, double gainB) +{ + /* + * Compute the sum of the squared colour error (non-greyness) as it + * appears in the log likelihood equation. + */ + double delta2Sum = 0; + for (auto &z : zones_) { + double deltaR = gainR * z.R - 1 - config_.whitepointR; + double deltaB = gainB * z.B - 1 - config_.whitepointB; + double delta2 = deltaR * deltaR + deltaB * deltaB; + /* LOG(RPiAwb, Debug) << "deltaR " << deltaR << " deltaB " << deltaB << " delta2 " << delta2; */ + delta2 = std::min(delta2, config_.deltaLimit); + delta2Sum += delta2; + } + return delta2Sum; +} + +Pwl Awb::interpolatePrior() +{ + /* + * Interpolate the prior log likelihood function for our current lux + * value. + */ + if (lux_ <= config_.priors.front().lux) + return config_.priors.front().prior; + else if (lux_ >= config_.priors.back().lux) + return config_.priors.back().prior; + else { + int idx = 0; + /* find which two we lie between */ + while (config_.priors[idx + 1].lux < lux_) + idx++; + double lux0 = config_.priors[idx].lux, + lux1 = config_.priors[idx + 1].lux; + return Pwl::combine(config_.priors[idx].prior, + config_.priors[idx + 1].prior, + [&](double /*x*/, double y0, double y1) { + return y0 + (y1 - y0) * + (lux_ - lux0) / (lux1 - lux0); + }); + } +} + +static double interpolateQuadatric(Pwl::Point const &a, Pwl::Point const &b, + Pwl::Point const &c) +{ + /* + * Given 3 points on a curve, find the extremum of the function in that + * interval by fitting a quadratic. + */ + const double eps = 1e-3; + Pwl::Point ca = c - a, ba = b - a; + double denominator = 2 * (ba.y * ca.x - ca.y * ba.x); + if (abs(denominator) > eps) { + double numerator = ba.y * ca.x * ca.x - ca.y * ba.x * ba.x; + double result = numerator / denominator + a.x; + return std::max(a.x, std::min(c.x, result)); + } + /* has degenerated to straight line segment */ + return a.y < c.y - eps ? a.x : (c.y < a.y - eps ? c.x : b.x); +} + +double Awb::coarseSearch(Pwl const &prior) +{ + points_.clear(); /* assume doesn't deallocate memory */ + size_t bestPoint = 0; + double t = mode_->ctLo; + int spanR = 0, spanB = 0; + /* Step down the CT curve evaluating log likelihood. */ + while (true) { + double r = config_.ctR.eval(t, &spanR); + double b = config_.ctB.eval(t, &spanB); + double gainR = 1 / r, gainB = 1 / b; + double delta2Sum = computeDelta2Sum(gainR, gainB); + double priorLogLikelihood = prior.eval(prior.domain().clip(t)); + double finalLogLikelihood = delta2Sum - priorLogLikelihood; + LOG(RPiAwb, Debug) + << "t: " << t << " gain R " << gainR << " gain B " + << gainB << " delta2_sum " << delta2Sum + << " prior " << priorLogLikelihood << " final " + << finalLogLikelihood; + points_.push_back(Pwl::Point(t, finalLogLikelihood)); + if (points_.back().y < points_[bestPoint].y) + bestPoint = points_.size() - 1; + if (t == mode_->ctHi) + break; + /* for even steps along the r/b curve scale them by the current t */ + t = std::min(t + t / 10 * config_.coarseStep, mode_->ctHi); + } + t = points_[bestPoint].x; + LOG(RPiAwb, Debug) << "Coarse search found CT " << t; + /* + * We have the best point of the search, but refine it with a quadratic + * interpolation around its neighbours. + */ + if (points_.size() > 2) { + unsigned long bp = std::min(bestPoint, points_.size() - 2); + bestPoint = std::max(1UL, bp); + t = interpolateQuadatric(points_[bestPoint - 1], + points_[bestPoint], + points_[bestPoint + 1]); + LOG(RPiAwb, Debug) + << "After quadratic refinement, coarse search has CT " + << t; + } + return t; +} + +void Awb::fineSearch(double &t, double &r, double &b, Pwl const &prior) +{ + int spanR = -1, spanB = -1; + config_.ctR.eval(t, &spanR); + config_.ctB.eval(t, &spanB); + double step = t / 10 * config_.coarseStep * 0.1; + int nsteps = 5; + double rDiff = config_.ctR.eval(t + nsteps * step, &spanR) - + config_.ctR.eval(t - nsteps * step, &spanR); + double bDiff = config_.ctB.eval(t + nsteps * step, &spanB) - + config_.ctB.eval(t - nsteps * step, &spanB); + Pwl::Point transverse(bDiff, -rDiff); + if (transverse.len2() < 1e-6) + return; + /* + * unit vector orthogonal to the b vs. r function (pointing outwards + * with r and b increasing) + */ + transverse = transverse / transverse.len(); + double bestLogLikelihood = 0, bestT = 0, bestR = 0, bestB = 0; + double transverseRange = config_.transverseNeg + config_.transversePos; + const int maxNumDeltas = 12; + /* a transverse step approximately every 0.01 r/b units */ + int numDeltas = floor(transverseRange * 100 + 0.5) + 1; + numDeltas = numDeltas < 3 ? 3 : (numDeltas > maxNumDeltas ? maxNumDeltas : numDeltas); + /* + * Step down CT curve. March a bit further if the transverse range is + * large. + */ + nsteps += numDeltas; + for (int i = -nsteps; i <= nsteps; i++) { + double tTest = t + i * step; + double priorLogLikelihood = + prior.eval(prior.domain().clip(tTest)); + double rCurve = config_.ctR.eval(tTest, &spanR); + double bCurve = config_.ctB.eval(tTest, &spanB); + /* x will be distance off the curve, y the log likelihood there */ + Pwl::Point points[maxNumDeltas]; + int bestPoint = 0; + /* Take some measurements transversely *off* the CT curve. */ + for (int j = 0; j < numDeltas; j++) { + points[j].x = -config_.transverseNeg + + (transverseRange * j) / (numDeltas - 1); + Pwl::Point rbTest = Pwl::Point(rCurve, bCurve) + + transverse * points[j].x; + double rTest = rbTest.x, bTest = rbTest.y; + double gainR = 1 / rTest, gainB = 1 / bTest; + double delta2Sum = computeDelta2Sum(gainR, gainB); + points[j].y = delta2Sum - priorLogLikelihood; + LOG(RPiAwb, Debug) + << "At t " << tTest << " r " << rTest << " b " + << bTest << ": " << points[j].y; + if (points[j].y < points[bestPoint].y) + bestPoint = j; + } + /* + * We have NUM_DELTAS points transversely across the CT curve, + * now let's do a quadratic interpolation for the best result. + */ + bestPoint = std::max(1, std::min(bestPoint, numDeltas - 2)); + Pwl::Point rbTest = Pwl::Point(rCurve, bCurve) + + transverse * interpolateQuadatric(points[bestPoint - 1], + points[bestPoint], + points[bestPoint + 1]); + double rTest = rbTest.x, bTest = rbTest.y; + double gainR = 1 / rTest, gainB = 1 / bTest; + double delta2Sum = computeDelta2Sum(gainR, gainB); + double finalLogLikelihood = delta2Sum - priorLogLikelihood; + LOG(RPiAwb, Debug) + << "Finally " + << tTest << " r " << rTest << " b " << bTest << ": " + << finalLogLikelihood + << (finalLogLikelihood < bestLogLikelihood ? " BEST" : ""); + if (bestT == 0 || finalLogLikelihood < bestLogLikelihood) + bestLogLikelihood = finalLogLikelihood, + bestT = tTest, bestR = rTest, bestB = bTest; + } + t = bestT, r = bestR, b = bestB; + LOG(RPiAwb, Debug) + << "Fine search found t " << t << " r " << r << " b " << b; +} + +void Awb::awbBayes() +{ + /* + * May as well divide out G to save computeDelta2Sum from doing it over + * and over. + */ + for (auto &z : zones_) + z.R = z.R / (z.G + 1), z.B = z.B / (z.G + 1); + /* + * Get the current prior, and scale according to how many zones are + * valid... not entirely sure about this. + */ + Pwl prior = interpolatePrior(); + prior *= zones_.size() / (double)(statistics_->awbRegions.numRegions()); + prior.map([](double x, double y) { + LOG(RPiAwb, Debug) << "(" << x << "," << y << ")"; + }); + double t = coarseSearch(prior); + double r = config_.ctR.eval(t); + double b = config_.ctB.eval(t); + LOG(RPiAwb, Debug) + << "After coarse search: r " << r << " b " << b << " (gains r " + << 1 / r << " b " << 1 / b << ")"; + /* + * Not entirely sure how to handle the fine search yet. Mostly the + * estimated CT is already good enough, but the fine search allows us to + * wander transverely off the CT curve. Under some illuminants, where + * there may be more or less green light, this may prove beneficial, + * though I probably need more real datasets before deciding exactly how + * this should be controlled and tuned. + */ + fineSearch(t, r, b, prior); + LOG(RPiAwb, Debug) + << "After fine search: r " << r << " b " << b << " (gains r " + << 1 / r << " b " << 1 / b << ")"; + /* + * Write results out for the main thread to pick up. Remember to adjust + * the gains from the ones that the "canonical sensor" would require to + * the ones needed by *this* sensor. + */ + asyncResults_.temperatureK = t; + asyncResults_.gainR = 1.0 / r * config_.sensitivityR; + asyncResults_.gainG = 1.0; + asyncResults_.gainB = 1.0 / b * config_.sensitivityB; +} + +void Awb::awbGrey() +{ + LOG(RPiAwb, Debug) << "Grey world AWB"; + /* + * Make a separate list of the derivatives for each of red and blue, so + * that we can sort them to exclude the extreme gains. We could + * consider some variations, such as normalising all the zones first, or + * doing an L2 average etc. + */ + std::vector<RGB> &derivsR(zones_); + std::vector<RGB> derivsB(derivsR); + std::sort(derivsR.begin(), derivsR.end(), + [](RGB const &a, RGB const &b) { + return a.G * b.R < b.G * a.R; + }); + std::sort(derivsB.begin(), derivsB.end(), + [](RGB const &a, RGB const &b) { + return a.G * b.B < b.G * a.B; + }); + /* Average the middle half of the values. */ + int discard = derivsR.size() / 4; + RGB sumR(0, 0, 0), sumB(0, 0, 0); + for (auto ri = derivsR.begin() + discard, + bi = derivsB.begin() + discard; + ri != derivsR.end() - discard; ri++, bi++) + sumR += *ri, sumB += *bi; + double gainR = sumR.G / (sumR.R + 1), + gainB = sumB.G / (sumB.B + 1); + asyncResults_.temperatureK = 4500; /* don't know what it is */ + asyncResults_.gainR = gainR; + asyncResults_.gainG = 1.0; + asyncResults_.gainB = gainB; +} + +void Awb::doAwb() +{ + prepareStats(); + LOG(RPiAwb, Debug) << "Valid zones: " << zones_.size(); + if (zones_.size() > config_.minRegions) { + if (config_.bayes) + awbBayes(); + else + awbGrey(); + LOG(RPiAwb, Debug) + << "CT found is " + << asyncResults_.temperatureK + << " with gains r " << asyncResults_.gainR + << " and b " << asyncResults_.gainB; + } + /* + * we're done with these; we may as well relinquish our hold on the + * pointer. + */ + statistics_.reset(); +} + +/* Register algorithm with the system. */ +static Algorithm *create(Controller *controller) +{ + return (Algorithm *)new Awb(controller); +} +static RegisterAlgorithm reg(NAME, &create); diff --git a/src/ipa/rpi/controller/rpi/awb.h b/src/ipa/rpi/controller/rpi/awb.h new file mode 100644 index 00000000..499b4519 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/awb.h @@ -0,0 +1,192 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * AWB control algorithm + */ +#pragma once + +#include <mutex> +#include <condition_variable> +#include <thread> + +#include "../awb_algorithm.h" +#include "../pwl.h" +#include "../awb_status.h" +#include "../statistics.h" + +namespace RPiController { + +/* Control algorithm to perform AWB calculations. */ + +struct AwbMode { + int read(const libcamera::YamlObject ¶ms); + double ctLo; /* low CT value for search */ + double ctHi; /* high CT value for search */ +}; + +struct AwbPrior { + int read(const libcamera::YamlObject ¶ms); + double lux; /* lux level */ + Pwl prior; /* maps CT to prior log likelihood for this lux level */ +}; + +struct AwbConfig { + AwbConfig() : defaultMode(nullptr) {} + int read(const libcamera::YamlObject ¶ms); + /* Only repeat the AWB calculation every "this many" frames */ + uint16_t framePeriod; + /* number of initial frames for which speed taken as 1.0 (maximum) */ + uint16_t startupFrames; + unsigned int convergenceFrames; /* approx number of frames to converge */ + double speed; /* IIR filter speed applied to algorithm results */ + bool fast; /* "fast" mode uses a 16x16 rather than 32x32 grid */ + Pwl ctR; /* function maps CT to r (= R/G) */ + Pwl ctB; /* function maps CT to b (= B/G) */ + Pwl ctRInverse; /* inverse of ctR */ + Pwl ctBInverse; /* inverse of ctB */ + /* table of illuminant priors at different lux levels */ + std::vector<AwbPrior> priors; + /* AWB "modes" (determines the search range) */ + std::map<std::string, AwbMode> modes; + AwbMode *defaultMode; /* mode used if no mode selected */ + /* + * minimum proportion of pixels counted within AWB region for it to be + * "useful" + */ + double minPixels; + /* minimum G value of those pixels, to be regarded a "useful" */ + uint16_t minG; + /* + * number of AWB regions that must be "useful" in order to do the AWB + * calculation + */ + uint32_t minRegions; + /* clamp on colour error term (so as not to penalise non-grey excessively) */ + double deltaLimit; + /* step size control in coarse search */ + double coarseStep; + /* how far to wander off CT curve towards "more purple" */ + double transversePos; + /* how far to wander off CT curve towards "more green" */ + double transverseNeg; + /* + * red sensitivity ratio (set to canonical sensor's R/G divided by this + * sensor's R/G) + */ + double sensitivityR; + /* + * blue sensitivity ratio (set to canonical sensor's B/G divided by this + * sensor's B/G) + */ + double sensitivityB; + /* The whitepoint (which we normally "aim" for) can be moved. */ + double whitepointR; + double whitepointB; + bool bayes; /* use Bayesian algorithm */ +}; + +class Awb : public AwbAlgorithm +{ +public: + Awb(Controller *controller = NULL); + ~Awb(); + char const *name() const override; + void initialise() override; + int read(const libcamera::YamlObject ¶ms) override; + unsigned int getConvergenceFrames() const override; + void initialValues(double &gainR, double &gainB) override; + void setMode(std::string const &name) override; + void setManualGains(double manualR, double manualB) override; + void enableAuto() override; + void disableAuto() override; + void switchMode(CameraMode const &cameraMode, Metadata *metadata) override; + void prepare(Metadata *imageMetadata) override; + void process(StatisticsPtr &stats, Metadata *imageMetadata) override; + struct RGB { + RGB(double r = 0, double g = 0, double b = 0) + : R(r), G(g), B(b) + { + } + double R, G, B; + RGB &operator+=(RGB const &other) + { + R += other.R, G += other.G, B += other.B; + return *this; + } + }; + +private: + bool isAutoEnabled() const; + /* configuration is read-only, and available to both threads */ + AwbConfig config_; + std::thread asyncThread_; + void asyncFunc(); /* asynchronous thread function */ + std::mutex mutex_; + /* condvar for async thread to wait on */ + std::condition_variable asyncSignal_; + /* condvar for synchronous thread to wait on */ + std::condition_variable syncSignal_; + /* for sync thread to check if async thread finished (requires mutex) */ + bool asyncFinished_; + /* for async thread to check if it's been told to run (requires mutex) */ + bool asyncStart_; + /* for async thread to check if it's been told to quit (requires mutex) */ + bool asyncAbort_; + + /* + * The following are only for the synchronous thread to use: + * for sync thread to note its has asked async thread to run + */ + bool asyncStarted_; + /* counts up to framePeriod before restarting the async thread */ + int framePhase_; + int frameCount_; /* counts up to startup_frames */ + AwbStatus syncResults_; + AwbStatus prevSyncResults_; + std::string modeName_; + /* + * The following are for the asynchronous thread to use, though the main + * thread can set/reset them if the async thread is known to be idle: + */ + void restartAsync(StatisticsPtr &stats, double lux); + /* copy out the results from the async thread so that it can be restarted */ + void fetchAsyncResults(); + StatisticsPtr statistics_; + AwbMode *mode_; + double lux_; + AwbStatus asyncResults_; + void doAwb(); + void awbBayes(); + void awbGrey(); + void prepareStats(); + double computeDelta2Sum(double gainR, double gainB); + Pwl interpolatePrior(); + double coarseSearch(Pwl const &prior); + void fineSearch(double &t, double &r, double &b, Pwl const &prior); + std::vector<RGB> zones_; + std::vector<Pwl::Point> points_; + /* manual r setting */ + double manualR_; + /* manual b setting */ + double manualB_; +}; + +static inline Awb::RGB operator+(Awb::RGB const &a, Awb::RGB const &b) +{ + return Awb::RGB(a.R + b.R, a.G + b.G, a.B + b.B); +} +static inline Awb::RGB operator-(Awb::RGB const &a, Awb::RGB const &b) +{ + return Awb::RGB(a.R - b.R, a.G - b.G, a.B - b.B); +} +static inline Awb::RGB operator*(double d, Awb::RGB const &rgb) +{ + return Awb::RGB(d * rgb.R, d * rgb.G, d * rgb.B); +} +static inline Awb::RGB operator*(Awb::RGB const &rgb, double d) +{ + return d * rgb; +} + +} /* namespace RPiController */ diff --git a/src/ipa/rpi/controller/rpi/black_level.cpp b/src/ipa/rpi/controller/rpi/black_level.cpp new file mode 100644 index 00000000..ea991df9 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/black_level.cpp @@ -0,0 +1,74 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * black level control algorithm + */ + +#include <math.h> +#include <stdint.h> + +#include <libcamera/base/log.h> + +#include "../black_level_status.h" + +#include "black_level.h" + +using namespace RPiController; +using namespace libcamera; + +LOG_DEFINE_CATEGORY(RPiBlackLevel) + +#define NAME "rpi.black_level" + +BlackLevel::BlackLevel(Controller *controller) + : BlackLevelAlgorithm(controller) +{ +} + +char const *BlackLevel::name() const +{ + return NAME; +} + +int BlackLevel::read(const libcamera::YamlObject ¶ms) +{ + /* 64 in 10 bits scaled to 16 bits */ + uint16_t blackLevel = params["black_level"].get<uint16_t>(4096); + blackLevelR_ = params["black_level_r"].get<uint16_t>(blackLevel); + blackLevelG_ = params["black_level_g"].get<uint16_t>(blackLevel); + blackLevelB_ = params["black_level_b"].get<uint16_t>(blackLevel); + LOG(RPiBlackLevel, Debug) + << " Read black levels red " << blackLevelR_ + << " green " << blackLevelG_ + << " blue " << blackLevelB_; + return 0; +} + +void BlackLevel::initialValues(uint16_t &blackLevelR, uint16_t &blackLevelG, + uint16_t &blackLevelB) +{ + blackLevelR = blackLevelR_; + blackLevelG = blackLevelG_; + blackLevelB = blackLevelB_; +} + +void BlackLevel::prepare(Metadata *imageMetadata) +{ + /* + * Possibly we should think about doing this in a switchMode or + * something? + */ + struct BlackLevelStatus status; + status.blackLevelR = blackLevelR_; + status.blackLevelG = blackLevelG_; + status.blackLevelB = blackLevelB_; + imageMetadata->set("black_level.status", status); +} + +/* Register algorithm with the system. */ +static Algorithm *create(Controller *controller) +{ + return new BlackLevel(controller); +} +static RegisterAlgorithm reg(NAME, &create); diff --git a/src/ipa/rpi/controller/rpi/black_level.h b/src/ipa/rpi/controller/rpi/black_level.h new file mode 100644 index 00000000..f50729db --- /dev/null +++ b/src/ipa/rpi/controller/rpi/black_level.h @@ -0,0 +1,32 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * black level control algorithm + */ +#pragma once + +#include "../black_level_algorithm.h" +#include "../black_level_status.h" + +/* This is our implementation of the "black level algorithm". */ + +namespace RPiController { + +class BlackLevel : public BlackLevelAlgorithm +{ +public: + BlackLevel(Controller *controller); + char const *name() const override; + int read(const libcamera::YamlObject ¶ms) override; + void initialValues(uint16_t &blackLevelR, uint16_t &blackLevelG, + uint16_t &blackLevelB) override; + void prepare(Metadata *imageMetadata) override; + +private: + double blackLevelR_; + double blackLevelG_; + double blackLevelB_; +}; + +} /* namespace RPiController */ diff --git a/src/ipa/rpi/controller/rpi/cac.cpp b/src/ipa/rpi/controller/rpi/cac.cpp new file mode 100644 index 00000000..17779ad5 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/cac.cpp @@ -0,0 +1,107 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2023 Raspberry Pi Ltd + * + * Chromatic Aberration Correction algorithm + */ +#include "cac.h" + +#include <libcamera/base/log.h> + +#include "cac_status.h" + +using namespace RPiController; +using namespace libcamera; + +LOG_DEFINE_CATEGORY(RPiCac) + +#define NAME "rpi.cac" + +Cac::Cac(Controller *controller) + : Algorithm(controller) +{ +} + +char const *Cac::name() const +{ + return NAME; +} + +static bool arrayToSet(const libcamera::YamlObject ¶ms, std::vector<double> &inputArray, const Size &size) +{ + int num = 0; + int max_num = (size.width + 1) * (size.height + 1); + inputArray.resize(max_num); + + for (const auto &p : params.asList()) { + if (num == max_num) + return false; + inputArray[num++] = p.get<double>(0); + } + + return num == max_num; +} + +static void setStrength(std::vector<double> &inputArray, std::vector<double> &outputArray, + double strengthFactor) +{ + int num = 0; + for (const auto &p : inputArray) { + outputArray[num++] = p * strengthFactor; + } +} + +int Cac::read(const libcamera::YamlObject ¶ms) +{ + config_.enabled = params.contains("lut_rx") && params.contains("lut_ry") && + params.contains("lut_bx") && params.contains("lut_by"); + if (!config_.enabled) + return 0; + + const Size &size = getHardwareConfig().cacRegions; + + if (!arrayToSet(params["lut_rx"], config_.lutRx, size)) { + LOG(RPiCac, Error) << "Bad CAC lut_rx table"; + return -EINVAL; + } + + if (!arrayToSet(params["lut_ry"], config_.lutRy, size)) { + LOG(RPiCac, Error) << "Bad CAC lut_ry table"; + return -EINVAL; + } + + if (!arrayToSet(params["lut_bx"], config_.lutBx, size)) { + LOG(RPiCac, Error) << "Bad CAC lut_bx table"; + return -EINVAL; + } + + if (!arrayToSet(params["lut_by"], config_.lutBy, size)) { + LOG(RPiCac, Error) << "Bad CAC lut_by table"; + return -EINVAL; + } + + double strength = params["strength"].get<double>(1); + cacStatus_.lutRx = config_.lutRx; + cacStatus_.lutRy = config_.lutRy; + cacStatus_.lutBx = config_.lutBx; + cacStatus_.lutBy = config_.lutBy; + setStrength(config_.lutRx, cacStatus_.lutRx, strength); + setStrength(config_.lutBx, cacStatus_.lutBx, strength); + setStrength(config_.lutRy, cacStatus_.lutRy, strength); + setStrength(config_.lutBy, cacStatus_.lutBy, strength); + + return 0; +} + +void Cac::prepare(Metadata *imageMetadata) +{ + if (config_.enabled) + imageMetadata->set("cac.status", cacStatus_); +} + +// Register algorithm with the system. +static Algorithm *Create(Controller *controller) +{ + return (Algorithm *)new Cac(controller); +} +static RegisterAlgorithm reg(NAME, &Create); diff --git a/src/ipa/rpi/controller/rpi/cac.h b/src/ipa/rpi/controller/rpi/cac.h new file mode 100644 index 00000000..a7b14c00 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/cac.h @@ -0,0 +1,35 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2023, Raspberry Pi Ltd + * + * cac.hpp - CAC control algorithm + */ +#pragma once + +#include "algorithm.h" +#include "cac_status.h" + +namespace RPiController { + +struct CacConfig { + bool enabled; + std::vector<double> lutRx; + std::vector<double> lutRy; + std::vector<double> lutBx; + std::vector<double> lutBy; +}; + +class Cac : public Algorithm +{ +public: + Cac(Controller *controller = NULL); + char const *name() const override; + int read(const libcamera::YamlObject ¶ms) override; + void prepare(Metadata *imageMetadata) override; + +private: + CacConfig config_; + CacStatus cacStatus_; +}; + +} // namespace RPiController diff --git a/src/ipa/rpi/controller/rpi/ccm.cpp b/src/ipa/rpi/controller/rpi/ccm.cpp new file mode 100644 index 00000000..c5588029 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/ccm.cpp @@ -0,0 +1,199 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * CCM (colour correction matrix) control algorithm + */ + +#include <libcamera/base/log.h> + +#include "../awb_status.h" +#include "../ccm_status.h" +#include "../lux_status.h" +#include "../metadata.h" + +#include "ccm.h" + +using namespace RPiController; +using namespace libcamera; + +LOG_DEFINE_CATEGORY(RPiCcm) + +/* + * This algorithm selects a CCM (Colour Correction Matrix) according to the + * colour temperature estimated by AWB (interpolating between known matricies as + * necessary). Additionally the amount of colour saturation can be controlled + * both according to the current estimated lux level and according to a + * saturation setting that is exposed to applications. + */ + +#define NAME "rpi.ccm" + +Matrix::Matrix() +{ + memset(m, 0, sizeof(m)); +} +Matrix::Matrix(double m0, double m1, double m2, double m3, double m4, double m5, + double m6, double m7, double m8) +{ + m[0][0] = m0, m[0][1] = m1, m[0][2] = m2, m[1][0] = m3, m[1][1] = m4, + m[1][2] = m5, m[2][0] = m6, m[2][1] = m7, m[2][2] = m8; +} +int Matrix::read(const libcamera::YamlObject ¶ms) +{ + double *ptr = (double *)m; + + if (params.size() != 9) { + LOG(RPiCcm, Error) << "Wrong number of values in CCM"; + return -EINVAL; + } + + for (const auto ¶m : params.asList()) { + auto value = param.get<double>(); + if (!value) + return -EINVAL; + *ptr++ = *value; + } + + return 0; +} + +Ccm::Ccm(Controller *controller) + : CcmAlgorithm(controller), saturation_(1.0) {} + +char const *Ccm::name() const +{ + return NAME; +} + +int Ccm::read(const libcamera::YamlObject ¶ms) +{ + int ret; + + if (params.contains("saturation")) { + ret = config_.saturation.read(params["saturation"]); + if (ret) + return ret; + } + + for (auto &p : params["ccms"].asList()) { + auto value = p["ct"].get<double>(); + if (!value) + return -EINVAL; + + CtCcm ctCcm; + ctCcm.ct = *value; + ret = ctCcm.ccm.read(p["ccm"]); + if (ret) + return ret; + + if (!config_.ccms.empty() && ctCcm.ct <= config_.ccms.back().ct) { + LOG(RPiCcm, Error) + << "CCM not in increasing colour temperature order"; + return -EINVAL; + } + + config_.ccms.push_back(std::move(ctCcm)); + } + + if (config_.ccms.empty()) { + LOG(RPiCcm, Error) << "No CCMs specified"; + return -EINVAL; + } + + return 0; +} + +void Ccm::setSaturation(double saturation) +{ + saturation_ = saturation; +} + +void Ccm::initialise() +{ +} + +template<typename T> +static bool getLocked(Metadata *metadata, std::string const &tag, T &value) +{ + T *ptr = metadata->getLocked<T>(tag); + if (ptr == nullptr) + return false; + value = *ptr; + return true; +} + +Matrix calculateCcm(std::vector<CtCcm> const &ccms, double ct) +{ + if (ct <= ccms.front().ct) + return ccms.front().ccm; + else if (ct >= ccms.back().ct) + return ccms.back().ccm; + else { + int i = 0; + for (; ct > ccms[i].ct; i++) + ; + double lambda = + (ct - ccms[i - 1].ct) / (ccms[i].ct - ccms[i - 1].ct); + return lambda * ccms[i].ccm + (1.0 - lambda) * ccms[i - 1].ccm; + } +} + +Matrix applySaturation(Matrix const &ccm, double saturation) +{ + Matrix RGB2Y(0.299, 0.587, 0.114, -0.169, -0.331, 0.500, 0.500, -0.419, + -0.081); + Matrix Y2RGB(1.000, 0.000, 1.402, 1.000, -0.345, -0.714, 1.000, 1.771, + 0.000); + Matrix S(1, 0, 0, 0, saturation, 0, 0, 0, saturation); + return Y2RGB * S * RGB2Y * ccm; +} + +void Ccm::prepare(Metadata *imageMetadata) +{ + bool awbOk = false, luxOk = false; + struct AwbStatus awb = {}; + awb.temperatureK = 4000; /* in case no metadata */ + struct LuxStatus lux = {}; + lux.lux = 400; /* in case no metadata */ + { + /* grab mutex just once to get everything */ + std::lock_guard<Metadata> lock(*imageMetadata); + awbOk = getLocked(imageMetadata, "awb.status", awb); + luxOk = getLocked(imageMetadata, "lux.status", lux); + } + if (!awbOk) + LOG(RPiCcm, Warning) << "no colour temperature found"; + if (!luxOk) + LOG(RPiCcm, Warning) << "no lux value found"; + Matrix ccm = calculateCcm(config_.ccms, awb.temperatureK); + double saturation = saturation_; + struct CcmStatus ccmStatus; + ccmStatus.saturation = saturation; + if (!config_.saturation.empty()) + saturation *= config_.saturation.eval( + config_.saturation.domain().clip(lux.lux)); + ccm = applySaturation(ccm, saturation); + for (int j = 0; j < 3; j++) + for (int i = 0; i < 3; i++) + ccmStatus.matrix[j * 3 + i] = + std::max(-8.0, std::min(7.9999, ccm.m[j][i])); + LOG(RPiCcm, Debug) + << "colour temperature " << awb.temperatureK << "K"; + LOG(RPiCcm, Debug) + << "CCM: " << ccmStatus.matrix[0] << " " << ccmStatus.matrix[1] + << " " << ccmStatus.matrix[2] << " " + << ccmStatus.matrix[3] << " " << ccmStatus.matrix[4] + << " " << ccmStatus.matrix[5] << " " + << ccmStatus.matrix[6] << " " << ccmStatus.matrix[7] + << " " << ccmStatus.matrix[8]; + imageMetadata->set("ccm.status", ccmStatus); +} + +/* Register algorithm with the system. */ +static Algorithm *create(Controller *controller) +{ + return (Algorithm *)new Ccm(controller); + ; +} +static RegisterAlgorithm reg(NAME, &create); diff --git a/src/ipa/rpi/controller/rpi/ccm.h b/src/ipa/rpi/controller/rpi/ccm.h new file mode 100644 index 00000000..b3abeddf --- /dev/null +++ b/src/ipa/rpi/controller/rpi/ccm.h @@ -0,0 +1,75 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * CCM (colour correction matrix) control algorithm + */ +#pragma once + +#include <vector> + +#include "../ccm_algorithm.h" +#include "../pwl.h" + +namespace RPiController { + +/* Algorithm to calculate colour matrix. Should be placed after AWB. */ + +struct Matrix { + Matrix(double m0, double m1, double m2, double m3, double m4, double m5, + double m6, double m7, double m8); + Matrix(); + double m[3][3]; + int read(const libcamera::YamlObject ¶ms); +}; +static inline Matrix operator*(double d, Matrix const &m) +{ + return Matrix(m.m[0][0] * d, m.m[0][1] * d, m.m[0][2] * d, + m.m[1][0] * d, m.m[1][1] * d, m.m[1][2] * d, + m.m[2][0] * d, m.m[2][1] * d, m.m[2][2] * d); +} +static inline Matrix operator*(Matrix const &m1, Matrix const &m2) +{ + Matrix m; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + m.m[i][j] = m1.m[i][0] * m2.m[0][j] + + m1.m[i][1] * m2.m[1][j] + + m1.m[i][2] * m2.m[2][j]; + return m; +} +static inline Matrix operator+(Matrix const &m1, Matrix const &m2) +{ + Matrix m; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + m.m[i][j] = m1.m[i][j] + m2.m[i][j]; + return m; +} + +struct CtCcm { + double ct; + Matrix ccm; +}; + +struct CcmConfig { + std::vector<CtCcm> ccms; + Pwl saturation; +}; + +class Ccm : public CcmAlgorithm +{ +public: + Ccm(Controller *controller = NULL); + char const *name() const override; + int read(const libcamera::YamlObject ¶ms) override; + void setSaturation(double saturation) override; + void initialise() override; + void prepare(Metadata *imageMetadata) override; + +private: + CcmConfig config_; + double saturation_; +}; + +} /* namespace RPiController */ diff --git a/src/ipa/rpi/controller/rpi/contrast.cpp b/src/ipa/rpi/controller/rpi/contrast.cpp new file mode 100644 index 00000000..9eef792d --- /dev/null +++ b/src/ipa/rpi/controller/rpi/contrast.cpp @@ -0,0 +1,192 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * contrast (gamma) control algorithm + */ +#include <stdint.h> + +#include <libcamera/base/log.h> + +#include "../contrast_status.h" +#include "../histogram.h" + +#include "contrast.h" + +using namespace RPiController; +using namespace libcamera; + +LOG_DEFINE_CATEGORY(RPiContrast) + +/* + * This is a very simple control algorithm which simply retrieves the results of + * AGC and AWB via their "status" metadata, and applies digital gain to the + * colour channels in accordance with those instructions. We take care never to + * apply less than unity gains, as that would cause fully saturated pixels to go + * off-white. + */ + +#define NAME "rpi.contrast" + +Contrast::Contrast(Controller *controller) + : ContrastAlgorithm(controller), brightness_(0.0), contrast_(1.0) +{ +} + +char const *Contrast::name() const +{ + return NAME; +} + +int Contrast::read(const libcamera::YamlObject ¶ms) +{ + // enable adaptive enhancement by default + config_.ceEnable = params["ce_enable"].get<int>(1); + ceEnable_ = config_.ceEnable; + // the point near the bottom of the histogram to move + config_.loHistogram = params["lo_histogram"].get<double>(0.01); + // where in the range to try and move it to + config_.loLevel = params["lo_level"].get<double>(0.015); + // but don't move by more than this + config_.loMax = params["lo_max"].get<double>(500); + // equivalent values for the top of the histogram... + config_.hiHistogram = params["hi_histogram"].get<double>(0.95); + config_.hiLevel = params["hi_level"].get<double>(0.95); + config_.hiMax = params["hi_max"].get<double>(2000); + return config_.gammaCurve.read(params["gamma_curve"]); +} + +void Contrast::setBrightness(double brightness) +{ + brightness_ = brightness; +} + +void Contrast::setContrast(double contrast) +{ + contrast_ = contrast; +} + +void Contrast::enableCe(bool enable) +{ + ceEnable_ = enable; +} + +void Contrast::restoreCe() +{ + ceEnable_ = config_.ceEnable; +} + +void Contrast::initialise() +{ + /* + * Fill in some default values as Prepare will run before Process gets + * called. + */ + status_.brightness = brightness_; + status_.contrast = contrast_; + status_.gammaCurve = config_.gammaCurve; +} + +void Contrast::prepare(Metadata *imageMetadata) +{ + imageMetadata->set("contrast.status", status_); +} + +Pwl computeStretchCurve(Histogram const &histogram, + ContrastConfig const &config) +{ + Pwl enhance; + enhance.append(0, 0); + /* + * If the start of the histogram is rather empty, try to pull it down a + * bit. + */ + double histLo = histogram.quantile(config.loHistogram) * + (65536 / histogram.bins()); + double levelLo = config.loLevel * 65536; + LOG(RPiContrast, Debug) + << "Move histogram point " << histLo << " to " << levelLo; + histLo = std::max(levelLo, + std::min(65535.0, std::min(histLo, levelLo + config.loMax))); + LOG(RPiContrast, Debug) + << "Final values " << histLo << " -> " << levelLo; + enhance.append(histLo, levelLo); + /* + * Keep the mid-point (median) in the same place, though, to limit the + * apparent amount of global brightness shift. + */ + double mid = histogram.quantile(0.5) * (65536 / histogram.bins()); + enhance.append(mid, mid); + + /* + * If the top to the histogram is empty, try to pull the pixel values + * there up. + */ + double histHi = histogram.quantile(config.hiHistogram) * + (65536 / histogram.bins()); + double levelHi = config.hiLevel * 65536; + LOG(RPiContrast, Debug) + << "Move histogram point " << histHi << " to " << levelHi; + histHi = std::min(levelHi, + std::max(0.0, std::max(histHi, levelHi - config.hiMax))); + LOG(RPiContrast, Debug) + << "Final values " << histHi << " -> " << levelHi; + enhance.append(histHi, levelHi); + enhance.append(65535, 65535); + return enhance; +} + +Pwl applyManualContrast(Pwl const &gammaCurve, double brightness, + double contrast) +{ + Pwl newGammaCurve; + LOG(RPiContrast, Debug) + << "Manual brightness " << brightness << " contrast " << contrast; + gammaCurve.map([&](double x, double y) { + newGammaCurve.append( + x, std::max(0.0, std::min(65535.0, + (y - 32768) * contrast + + 32768 + brightness))); + }); + return newGammaCurve; +} + +void Contrast::process(StatisticsPtr &stats, + [[maybe_unused]] Metadata *imageMetadata) +{ + Histogram &histogram = stats->yHist; + /* + * We look at the histogram and adjust the gamma curve in the following + * ways: 1. Adjust the gamma curve so as to pull the start of the + * histogram down, and possibly push the end up. + */ + Pwl gammaCurve = config_.gammaCurve; + if (ceEnable_) { + if (config_.loMax != 0 || config_.hiMax != 0) + gammaCurve = computeStretchCurve(histogram, config_).compose(gammaCurve); + /* + * We could apply other adjustments (e.g. partial equalisation) + * based on the histogram...? + */ + } + /* + * 2. Finally apply any manually selected brightness/contrast + * adjustment. + */ + if (brightness_ != 0 || contrast_ != 1.0) + gammaCurve = applyManualContrast(gammaCurve, brightness_, contrast_); + /* + * And fill in the status for output. Use more points towards the bottom + * of the curve. + */ + status_.brightness = brightness_; + status_.contrast = contrast_; + status_.gammaCurve = std::move(gammaCurve); +} + +/* Register algorithm with the system. */ +static Algorithm *create(Controller *controller) +{ + return (Algorithm *)new Contrast(controller); +} +static RegisterAlgorithm reg(NAME, &create); diff --git a/src/ipa/rpi/controller/rpi/contrast.h b/src/ipa/rpi/controller/rpi/contrast.h new file mode 100644 index 00000000..a9d9bbc9 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/contrast.h @@ -0,0 +1,54 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * contrast (gamma) control algorithm + */ +#pragma once + +#include <mutex> + +#include "../contrast_algorithm.h" +#include "../pwl.h" + +namespace RPiController { + +/* + * Back End algorithm to appaly correct digital gain. Should be placed after + * Back End AWB. + */ + +struct ContrastConfig { + bool ceEnable; + double loHistogram; + double loLevel; + double loMax; + double hiHistogram; + double hiLevel; + double hiMax; + Pwl gammaCurve; +}; + +class Contrast : public ContrastAlgorithm +{ +public: + Contrast(Controller *controller = NULL); + char const *name() const override; + int read(const libcamera::YamlObject ¶ms) override; + void setBrightness(double brightness) override; + void setContrast(double contrast) override; + void enableCe(bool enable) override; + void restoreCe() override; + void initialise() override; + void prepare(Metadata *imageMetadata) override; + void process(StatisticsPtr &stats, Metadata *imageMetadata) override; + +private: + ContrastConfig config_; + double brightness_; + double contrast_; + ContrastStatus status_; + double ceEnable_; +}; + +} /* namespace RPiController */ diff --git a/src/ipa/rpi/controller/rpi/denoise.cpp b/src/ipa/rpi/controller/rpi/denoise.cpp new file mode 100644 index 00000000..ba851658 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/denoise.cpp @@ -0,0 +1,198 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2022 Raspberry Pi Ltd + * + * Denoise (spatial, colour, temporal) control algorithm + */ +#include "denoise.h" + +#include <libcamera/base/log.h> + +#include "denoise_status.h" +#include "noise_status.h" + +using namespace RPiController; +using namespace libcamera; + +LOG_DEFINE_CATEGORY(RPiDenoise) + +// Calculate settings for the denoise blocks using the noise profile in +// the image metadata. + +#define NAME "rpi.denoise" + +int DenoiseConfig::read(const libcamera::YamlObject ¶ms) +{ + sdnEnable = params.contains("sdn"); + if (sdnEnable) { + auto &sdnParams = params["sdn"]; + sdnDeviation = sdnParams["deviation"].get<double>(3.2); + sdnStrength = sdnParams["strength"].get<double>(0.25); + sdnDeviation2 = sdnParams["deviation2"].get<double>(sdnDeviation); + sdnDeviationNoTdn = sdnParams["deviation_no_tdn"].get<double>(sdnDeviation); + sdnStrengthNoTdn = sdnParams["strength_no_tdn"].get<double>(sdnStrength); + sdnTdnBackoff = sdnParams["backoff"].get<double>(0.75); + } + + cdnEnable = params.contains("cdn"); + if (cdnEnable) { + auto &cdnParams = params["cdn"]; + cdnDeviation = cdnParams["deviation"].get<double>(120); + cdnStrength = cdnParams["strength"].get<double>(0.2); + } + + tdnEnable = params.contains("tdn"); + if (tdnEnable) { + auto &tdnParams = params["tdn"]; + tdnDeviation = tdnParams["deviation"].get<double>(0.5); + tdnThreshold = tdnParams["threshold"].get<double>(0.75); + } else if (sdnEnable) { + /* + * If SDN is enabled but TDN isn't, overwrite all the SDN settings + * with the "no TDN" versions. This makes it easier to enable or + * disable TDN in the tuning file without editing all the other + * parameters. + */ + sdnDeviation = sdnDeviation2 = sdnDeviationNoTdn; + sdnStrength = sdnStrengthNoTdn; + } + + return 0; +} + +Denoise::Denoise(Controller *controller) + : DenoiseAlgorithm(controller), mode_(DenoiseMode::ColourHighQuality) +{ +} + +char const *Denoise::name() const +{ + return NAME; +} + +int Denoise::read(const libcamera::YamlObject ¶ms) +{ + if (!params.contains("normal")) { + configs_["normal"].read(params); + currentConfig_ = &configs_["normal"]; + + return 0; + } + + for (const auto &[key, value] : params.asDict()) { + if (configs_[key].read(value)) { + LOG(RPiDenoise, Error) << "Failed to read denoise config " << key; + return -EINVAL; + } + } + + auto it = configs_.find("normal"); + if (it == configs_.end()) { + LOG(RPiDenoise, Error) << "No normal denoise settings found"; + return -EINVAL; + } + currentConfig_ = &it->second; + + return 0; +} + +void Denoise::initialise() +{ +} + +void Denoise::switchMode([[maybe_unused]] CameraMode const &cameraMode, + [[maybe_unused]] Metadata *metadata) +{ + /* A mode switch effectively resets temporal denoise and it has to start over. */ + currentSdnDeviation_ = currentConfig_->sdnDeviationNoTdn; + currentSdnStrength_ = currentConfig_->sdnStrengthNoTdn; + currentSdnDeviation2_ = currentConfig_->sdnDeviationNoTdn; +} + +void Denoise::prepare(Metadata *imageMetadata) +{ + struct NoiseStatus noiseStatus = {}; + noiseStatus.noiseSlope = 3.0; // in case no metadata + if (imageMetadata->get("noise.status", noiseStatus) != 0) + LOG(RPiDenoise, Warning) << "no noise profile found"; + + LOG(RPiDenoise, Debug) + << "Noise profile: constant " << noiseStatus.noiseConstant + << " slope " << noiseStatus.noiseSlope; + + if (mode_ == DenoiseMode::Off) + return; + + if (currentConfig_->sdnEnable) { + struct SdnStatus sdn; + sdn.noiseConstant = noiseStatus.noiseConstant * currentSdnDeviation_; + sdn.noiseSlope = noiseStatus.noiseSlope * currentSdnDeviation_; + sdn.noiseConstant2 = noiseStatus.noiseConstant * currentConfig_->sdnDeviation2; + sdn.noiseSlope2 = noiseStatus.noiseSlope * currentSdnDeviation2_; + sdn.strength = currentSdnStrength_; + imageMetadata->set("sdn.status", sdn); + LOG(RPiDenoise, Debug) + << "const " << sdn.noiseConstant + << " slope " << sdn.noiseSlope + << " str " << sdn.strength + << " const2 " << sdn.noiseConstant2 + << " slope2 " << sdn.noiseSlope2; + + /* For the next frame, we back off the SDN parameters as TDN ramps up. */ + double f = currentConfig_->sdnTdnBackoff; + currentSdnDeviation_ = f * currentSdnDeviation_ + (1 - f) * currentConfig_->sdnDeviation; + currentSdnStrength_ = f * currentSdnStrength_ + (1 - f) * currentConfig_->sdnStrength; + currentSdnDeviation2_ = f * currentSdnDeviation2_ + (1 - f) * currentConfig_->sdnDeviation2; + } + + if (currentConfig_->tdnEnable) { + struct TdnStatus tdn; + tdn.noiseConstant = noiseStatus.noiseConstant * currentConfig_->tdnDeviation; + tdn.noiseSlope = noiseStatus.noiseSlope * currentConfig_->tdnDeviation; + tdn.threshold = currentConfig_->tdnThreshold; + imageMetadata->set("tdn.status", tdn); + LOG(RPiDenoise, Debug) + << "programmed tdn threshold " << tdn.threshold + << " constant " << tdn.noiseConstant + << " slope " << tdn.noiseSlope; + } + + if (currentConfig_->cdnEnable && mode_ != DenoiseMode::ColourOff) { + struct CdnStatus cdn; + cdn.threshold = currentConfig_->cdnDeviation * noiseStatus.noiseSlope + noiseStatus.noiseConstant; + cdn.strength = currentConfig_->cdnStrength; + imageMetadata->set("cdn.status", cdn); + LOG(RPiDenoise, Debug) + << "programmed cdn threshold " << cdn.threshold + << " strength " << cdn.strength; + } +} + +void Denoise::setMode(DenoiseMode mode) +{ + // We only distinguish between off and all other modes. + mode_ = mode; +} + +void Denoise::setConfig(std::string const &name) +{ + auto it = configs_.find(name); + if (it == configs_.end()) { + /* + * Some platforms may have no need for different denoise settings, so we only issue + * a warning if there clearly are several configurations. + */ + if (configs_.size() > 1) + LOG(RPiDenoise, Warning) << "No denoise config found for " << name; + else + LOG(RPiDenoise, Debug) << "No denoise config found for " << name; + } else + currentConfig_ = &it->second; +} + +// Register algorithm with the system. +static Algorithm *Create(Controller *controller) +{ + return (Algorithm *)new Denoise(controller); +} +static RegisterAlgorithm reg(NAME, &Create); diff --git a/src/ipa/rpi/controller/rpi/denoise.h b/src/ipa/rpi/controller/rpi/denoise.h new file mode 100644 index 00000000..92ff4f93 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/denoise.h @@ -0,0 +1,59 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2022, Raspberry Pi Ltd + * + * denoise.hpp - Denoise (spatial, colour, temporal) control algorithm + */ +#pragma once + +#include <map> +#include <string> + +#include "algorithm.h" +#include "denoise_algorithm.h" + +namespace RPiController { + +// Algorithm to calculate correct denoise settings. + +struct DenoiseConfig { + double sdnDeviation; + double sdnStrength; + double sdnDeviation2; + double sdnDeviationNoTdn; + double sdnStrengthNoTdn; + double sdnTdnBackoff; + double cdnDeviation; + double cdnStrength; + double tdnDeviation; + double tdnThreshold; + bool tdnEnable; + bool sdnEnable; + bool cdnEnable; + int read(const libcamera::YamlObject ¶ms); +}; + +class Denoise : public DenoiseAlgorithm +{ +public: + Denoise(Controller *controller); + char const *name() const override; + int read(const libcamera::YamlObject ¶ms) override; + void initialise() override; + void switchMode(CameraMode const &cameraMode, Metadata *metadata) override; + void prepare(Metadata *imageMetadata) override; + void setMode(DenoiseMode mode) override; + void setConfig(std::string const &name) override; + +private: + std::map<std::string, DenoiseConfig> configs_; + DenoiseConfig *currentConfig_; + DenoiseMode mode_; + + /* SDN parameters attenuate over time if TDN is running. */ + double currentSdnDeviation_; + double currentSdnStrength_; + double currentSdnDeviation2_; +}; + +} // namespace RPiController diff --git a/src/ipa/rpi/controller/rpi/dpc.cpp b/src/ipa/rpi/controller/rpi/dpc.cpp new file mode 100644 index 00000000..8aac03f7 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/dpc.cpp @@ -0,0 +1,59 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * DPC (defective pixel correction) control algorithm + */ + +#include <libcamera/base/log.h> + +#include "dpc.h" + +using namespace RPiController; +using namespace libcamera; + +LOG_DEFINE_CATEGORY(RPiDpc) + +/* + * We use the lux status so that we can apply stronger settings in darkness (if + * necessary). + */ + +#define NAME "rpi.dpc" + +Dpc::Dpc(Controller *controller) + : Algorithm(controller) +{ +} + +char const *Dpc::name() const +{ + return NAME; +} + +int Dpc::read(const libcamera::YamlObject ¶ms) +{ + config_.strength = params["strength"].get<int>(1); + if (config_.strength < 0 || config_.strength > 2) { + LOG(RPiDpc, Error) << "Bad strength value"; + return -EINVAL; + } + + return 0; +} + +void Dpc::prepare(Metadata *imageMetadata) +{ + DpcStatus dpcStatus = {}; + /* Should we vary this with lux level or analogue gain? TBD. */ + dpcStatus.strength = config_.strength; + LOG(RPiDpc, Debug) << "strength " << dpcStatus.strength; + imageMetadata->set("dpc.status", dpcStatus); +} + +/* Register algorithm with the system. */ +static Algorithm *create(Controller *controller) +{ + return (Algorithm *)new Dpc(controller); +} +static RegisterAlgorithm reg(NAME, &create); diff --git a/src/ipa/rpi/controller/rpi/dpc.h b/src/ipa/rpi/controller/rpi/dpc.h new file mode 100644 index 00000000..9cefb06d --- /dev/null +++ b/src/ipa/rpi/controller/rpi/dpc.h @@ -0,0 +1,32 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * DPC (defective pixel correction) control algorithm + */ +#pragma once + +#include "../algorithm.h" +#include "../dpc_status.h" + +namespace RPiController { + +/* Back End algorithm to apply appropriate GEQ settings. */ + +struct DpcConfig { + int strength; +}; + +class Dpc : public Algorithm +{ +public: + Dpc(Controller *controller); + char const *name() const override; + int read(const libcamera::YamlObject ¶ms) override; + void prepare(Metadata *imageMetadata) override; + +private: + DpcConfig config_; +}; + +} /* namespace RPiController */ diff --git a/src/ipa/rpi/controller/rpi/focus.h b/src/ipa/rpi/controller/rpi/focus.h new file mode 100644 index 00000000..ee014be9 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/focus.h @@ -0,0 +1,28 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2020, Raspberry Pi Ltd + * + * focus algorithm + */ +#pragma once + +#include "../algorithm.h" +#include "../metadata.h" + +/* + * The "focus" algorithm. All it does it print out a version of the + * focus contrast measure; there is no actual auto-focus mechanism to + * control. + */ + +namespace RPiController { + +class Focus : public Algorithm +{ +public: + Focus(Controller *controller); + char const *name() const override; + void process(StatisticsPtr &stats, Metadata *imageMetadata) override; +}; + +} /* namespace RPiController */ diff --git a/src/ipa/rpi/controller/rpi/geq.cpp b/src/ipa/rpi/controller/rpi/geq.cpp new file mode 100644 index 00000000..fb539d1f --- /dev/null +++ b/src/ipa/rpi/controller/rpi/geq.cpp @@ -0,0 +1,89 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * GEQ (green equalisation) control algorithm + */ + +#include <libcamera/base/log.h> + +#include "../device_status.h" +#include "../lux_status.h" +#include "../pwl.h" + +#include "geq.h" + +using namespace RPiController; +using namespace libcamera; + +LOG_DEFINE_CATEGORY(RPiGeq) + +/* + * We use the lux status so that we can apply stronger settings in darkness (if + * necessary). + */ + +#define NAME "rpi.geq" + +Geq::Geq(Controller *controller) + : Algorithm(controller) +{ +} + +char const *Geq::name() const +{ + return NAME; +} + +int Geq::read(const libcamera::YamlObject ¶ms) +{ + config_.offset = params["offset"].get<uint16_t>(0); + config_.slope = params["slope"].get<double>(0.0); + if (config_.slope < 0.0 || config_.slope >= 1.0) { + LOG(RPiGeq, Error) << "Bad slope value"; + return -EINVAL; + } + + if (params.contains("strength")) { + int ret = config_.strength.read(params["strength"]); + if (ret) + return ret; + } + + return 0; +} + +void Geq::prepare(Metadata *imageMetadata) +{ + LuxStatus luxStatus = {}; + luxStatus.lux = 400; + if (imageMetadata->get("lux.status", luxStatus)) + LOG(RPiGeq, Warning) << "no lux data found"; + DeviceStatus deviceStatus; + deviceStatus.analogueGain = 1.0; /* in case not found */ + if (imageMetadata->get("device.status", deviceStatus)) + LOG(RPiGeq, Warning) + << "no device metadata - use analogue gain of 1x"; + GeqStatus geqStatus = {}; + double strength = config_.strength.empty() + ? 1.0 + : config_.strength.eval(config_.strength.domain().clip(luxStatus.lux)); + strength *= deviceStatus.analogueGain; + double offset = config_.offset * strength; + double slope = config_.slope * strength; + geqStatus.offset = std::min(65535.0, std::max(0.0, offset)); + geqStatus.slope = std::min(.99999, std::max(0.0, slope)); + LOG(RPiGeq, Debug) + << "offset " << geqStatus.offset << " slope " + << geqStatus.slope << " (analogue gain " + << deviceStatus.analogueGain << " lux " + << luxStatus.lux << ")"; + imageMetadata->set("geq.status", geqStatus); +} + +/* Register algorithm with the system. */ +static Algorithm *create(Controller *controller) +{ + return (Algorithm *)new Geq(controller); +} +static RegisterAlgorithm reg(NAME, &create); diff --git a/src/ipa/rpi/controller/rpi/geq.h b/src/ipa/rpi/controller/rpi/geq.h new file mode 100644 index 00000000..2c8400c2 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/geq.h @@ -0,0 +1,34 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * GEQ (green equalisation) control algorithm + */ +#pragma once + +#include "../algorithm.h" +#include "../geq_status.h" + +namespace RPiController { + +/* Back End algorithm to apply appropriate GEQ settings. */ + +struct GeqConfig { + uint16_t offset; + double slope; + Pwl strength; /* lux to strength factor */ +}; + +class Geq : public Algorithm +{ +public: + Geq(Controller *controller); + char const *name() const override; + int read(const libcamera::YamlObject ¶ms) override; + void prepare(Metadata *imageMetadata) override; + +private: + GeqConfig config_; +}; + +} /* namespace RPiController */ diff --git a/src/ipa/rpi/controller/rpi/hdr.cpp b/src/ipa/rpi/controller/rpi/hdr.cpp new file mode 100644 index 00000000..34cf360e --- /dev/null +++ b/src/ipa/rpi/controller/rpi/hdr.cpp @@ -0,0 +1,417 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2023 Raspberry Pi Ltd + * + * HDR control algorithm + */ + +#include "hdr.h" + +#include <cmath> + +#include <libcamera/base/log.h> + +#include "../agc_status.h" +#include "../alsc_status.h" +#include "../stitch_status.h" +#include "../tonemap_status.h" + +using namespace RPiController; +using namespace libcamera; + +LOG_DEFINE_CATEGORY(RPiHdr) + +#define NAME "rpi.hdr" + +void HdrConfig::read(const libcamera::YamlObject ¶ms, const std::string &modeName) +{ + name = modeName; + + if (!params.contains("cadence")) + LOG(RPiHdr, Fatal) << "No cadence for HDR mode " << name; + cadence = params["cadence"].getList<unsigned int>().value(); + if (cadence.empty()) + LOG(RPiHdr, Fatal) << "Empty cadence in HDR mode " << name; + + /* + * In the JSON file it's easier to use the channel name as the key, but + * for us it's convenient to swap them over. + */ + for (const auto &[k, v] : params["channel_map"].asDict()) + channelMap[v.get<unsigned int>().value()] = k; + + /* Lens shading related parameters. */ + if (params.contains("spatial_gain_curve")) { + spatialGainCurve.read(params["spatial_gain_curve"]); + } else if (params.contains("spatial_gain")) { + double spatialGain = params["spatial_gain"].get<double>(2.0); + spatialGainCurve.append(0.0, spatialGain); + spatialGainCurve.append(0.01, spatialGain); + spatialGainCurve.append(0.06, 1.0); /* maybe make this programmable? */ + spatialGainCurve.append(1.0, 1.0); + } + + diffusion = params["diffusion"].get<unsigned int>(3); + /* Clip to an arbitrary limit just to stop typos from killing the system! */ + const unsigned int MAX_DIFFUSION = 15; + if (diffusion > MAX_DIFFUSION) { + diffusion = MAX_DIFFUSION; + LOG(RPiHdr, Warning) << "Diffusion value clipped to " << MAX_DIFFUSION; + } + + /* Read any tonemap parameters. */ + tonemapEnable = params["tonemap_enable"].get<int>(0); + detailConstant = params["detail_constant"].get<uint16_t>(0); + detailSlope = params["detail_slope"].get<double>(0.0); + iirStrength = params["iir_strength"].get<double>(8.0); + strength = params["strength"].get<double>(1.5); + if (tonemapEnable) + tonemap.read(params["tonemap"]); + speed = params["speed"].get<double>(1.0); + if (params.contains("hi_quantile_targets")) { + hiQuantileTargets = params["hi_quantile_targets"].getList<double>().value(); + if (hiQuantileTargets.empty() || hiQuantileTargets.size() % 2) + LOG(RPiHdr, Fatal) << "hi_quantile_targets much be even and non-empty"; + } else + hiQuantileTargets = { 0.95, 0.65, 0.5, 0.28, 0.3, 0.25 }; + hiQuantileMaxGain = params["hi_quantile_max_gain"].get<double>(1.6); + if (params.contains("quantile_targets")) { + quantileTargets = params["quantile_targets"].getList<double>().value(); + if (quantileTargets.empty() || quantileTargets.size() % 2) + LOG(RPiHdr, Fatal) << "quantile_targets much be even and non-empty"; + } else + quantileTargets = { 0.2, 0.03, 1.0, 0.15 }; + powerMin = params["power_min"].get<double>(0.65); + powerMax = params["power_max"].get<double>(1.0); + if (params.contains("contrast_adjustments")) { + contrastAdjustments = params["contrast_adjustments"].getList<double>().value(); + } else + contrastAdjustments = { 0.5, 0.75 }; + + /* Read any stitch parameters. */ + stitchEnable = params["stitch_enable"].get<int>(0); + thresholdLo = params["threshold_lo"].get<uint16_t>(50000); + motionThreshold = params["motion_threshold"].get<double>(0.005); + diffPower = params["diff_power"].get<uint8_t>(13); + if (diffPower > 15) + LOG(RPiHdr, Fatal) << "Bad diff_power value in HDR mode " << name; +} + +Hdr::Hdr(Controller *controller) + : HdrAlgorithm(controller) +{ + regions_ = controller->getHardwareConfig().awbRegions; + numRegions_ = regions_.width * regions_.height; + gains_[0].resize(numRegions_, 1.0); + gains_[1].resize(numRegions_, 1.0); +} + +char const *Hdr::name() const +{ + return NAME; +} + +int Hdr::read(const libcamera::YamlObject ¶ms) +{ + /* Make an "HDR off" mode by default so that tuning files don't have to. */ + HdrConfig &offMode = config_["Off"]; + offMode.name = "Off"; + offMode.cadence = { 0 }; + offMode.channelMap[0] = "None"; + status_.mode = offMode.name; + delayedStatus_.mode = offMode.name; + + /* + * But we still allow the tuning file to override the "Off" mode if it wants. + * For example, maybe an application will make channel 0 be the "short" + * channel, in order to apply other AGC controls to it. + */ + for (const auto &[key, value] : params.asDict()) + config_[key].read(value, key); + + return 0; +} + +int Hdr::setMode(std::string const &mode) +{ + /* Always validate the mode, so it can be used later without checking. */ + auto it = config_.find(mode); + if (it == config_.end()) { + LOG(RPiHdr, Warning) << "No such HDR mode " << mode; + return -1; + } + + status_.mode = it->second.name; + + return 0; +} + +std::vector<unsigned int> Hdr::getChannels() const +{ + return config_.at(status_.mode).cadence; +} + +void Hdr::updateAgcStatus(Metadata *metadata) +{ + std::scoped_lock lock(*metadata); + AgcStatus *agcStatus = metadata->getLocked<AgcStatus>("agc.status"); + if (agcStatus) { + HdrConfig &hdrConfig = config_[status_.mode]; + auto it = hdrConfig.channelMap.find(agcStatus->channel); + if (it != hdrConfig.channelMap.end()) { + status_.channel = it->second; + agcStatus->hdr = status_; + } else + LOG(RPiHdr, Warning) << "Channel " << agcStatus->channel + << " not found in mode " << status_.mode; + } else + LOG(RPiHdr, Warning) << "No agc.status found"; +} + +void Hdr::switchMode([[maybe_unused]] CameraMode const &cameraMode, Metadata *metadata) +{ + updateAgcStatus(metadata); + delayedStatus_ = status_; +} + +void Hdr::prepare(Metadata *imageMetadata) +{ + AgcStatus agcStatus; + if (!imageMetadata->get<AgcStatus>("agc.delayed_status", agcStatus)) + delayedStatus_ = agcStatus.hdr; + + auto it = config_.find(delayedStatus_.mode); + if (it == config_.end()) { + /* Shouldn't be possible. There would be nothing we could do. */ + LOG(RPiHdr, Warning) << "Unexpected HDR mode " << delayedStatus_.mode; + return; + } + + HdrConfig &config = it->second; + if (config.spatialGainCurve.empty()) + return; + + AlscStatus alscStatus{}; /* some compilers seem to require the braces */ + if (imageMetadata->get<AlscStatus>("alsc.status", alscStatus)) { + LOG(RPiHdr, Warning) << "No ALSC status"; + return; + } + + /* The final gains ended up in the odd or even array, according to diffusion. */ + std::vector<double> &gains = gains_[config.diffusion & 1]; + for (unsigned int i = 0; i < numRegions_; i++) { + alscStatus.r[i] *= gains[i]; + alscStatus.g[i] *= gains[i]; + alscStatus.b[i] *= gains[i]; + } + imageMetadata->set("alsc.status", alscStatus); +} + +bool Hdr::updateTonemap([[maybe_unused]] StatisticsPtr &stats, HdrConfig &config) +{ + /* When there's a change of HDR mode we start over with a new tonemap curve. */ + if (delayedStatus_.mode != previousMode_) { + previousMode_ = delayedStatus_.mode; + tonemap_ = Pwl(); + } + + /* No tonemapping. No need to output a tonemap.status. */ + if (!config.tonemapEnable) + return false; + + /* If an explicit tonemap was given, use it. */ + if (!config.tonemap.empty()) { + tonemap_ = config.tonemap; + return true; + } + + /* + * We wouldn't update the tonemap on short frames when in multi-exposure mode. But + * we still need to output the most recent tonemap. Possibly we should make the + * config indicate the channels for which we should update the tonemap? + */ + if (delayedStatus_.mode == "MultiExposure" && delayedStatus_.channel != "short") + return true; + + /* + * Create a tonemap dynamically. We have three ingredients. + * + * 1. We have a list of "hi quantiles" and "targets". We use these to judge if + * the image does seem to be reasonably saturated. If it isn't, we calculate + * a gain that we will feed as a linear factor into the tonemap generation. + * This prevents unsaturated images from beoming quite so "flat". + * + * 2. We have a list of quantile/target pairs for the bottom of the histogram. + * We use these to calculate how much gain we must apply to the bottom of the + * tonemap. We apply this gain as a power curve so as not to blow out the top + * end. + * + * 3. Finally, when we generate the tonemap, we have some contrast adjustments + * for the bottom because we know that power curves can start quite steeply and + * cause a washed-out look. + */ + + /* Compute the linear gain from the headroom for saturation at the top. */ + double gain = 10; /* arbitrary, but hiQuantileMaxGain will clamp it later */ + for (unsigned int i = 0; i < config.hiQuantileTargets.size(); i += 2) { + double quantile = config.hiQuantileTargets[i]; + double target = config.hiQuantileTargets[i + 1]; + double value = stats->yHist.interQuantileMean(quantile, 1.0) / 1024.0; + double newGain = target / (value + 0.01); + gain = std::min(gain, newGain); + } + gain = std::clamp(gain, 1.0, config.hiQuantileMaxGain); + + /* Compute the power curve from the amount of gain needed at the bottom. */ + double min_power = 2; /* arbitrary, but config.powerMax will clamp it later */ + for (unsigned int i = 0; i < config.quantileTargets.size(); i += 2) { + double quantile = config.quantileTargets[i]; + double target = config.quantileTargets[i + 1]; + double value = stats->yHist.interQuantileMean(0, quantile) / 1024.0; + value = std::min(value * gain, 1.0); + double power = log(target + 1e-6) / log(value + 1e-6); + min_power = std::min(min_power, power); + } + double power = std::clamp(min_power, config.powerMin, config.powerMax); + + /* Generate the tonemap, including the contrast adjustment factors. */ + Pwl tonemap; + tonemap.append(0, 0); + for (unsigned int i = 0; i <= 6; i++) { + double x = 1 << (i + 9); /* x loops from 512 to 32768 inclusive */ + double y = pow(std::min(x * gain, 65535.0) / 65536.0, power) * 65536; + if (i < config.contrastAdjustments.size()) + y *= config.contrastAdjustments[i]; + if (!tonemap_.empty()) + y = y * config.speed + tonemap_.eval(x) * (1 - config.speed); + tonemap.append(x, y); + } + tonemap.append(65535, 65535); + tonemap_ = tonemap; + + return true; +} + +static void averageGains(std::vector<double> &src, std::vector<double> &dst, const Size &size) +{ +#define IDX(y, x) ((y)*size.width + (x)) + unsigned int lastCol = size.width - 1; /* index of last column */ + unsigned int preLastCol = lastCol - 1; /* and the column before that */ + unsigned int lastRow = size.height - 1; /* index of last row */ + unsigned int preLastRow = lastRow - 1; /* and the row before that */ + + /* Corners first. */ + dst[IDX(0, 0)] = (src[IDX(0, 0)] + src[IDX(0, 1)] + src[IDX(1, 0)]) / 3; + dst[IDX(0, lastCol)] = (src[IDX(0, lastCol)] + src[IDX(0, preLastCol)] + src[IDX(1, lastCol)]) / 3; + dst[IDX(lastRow, 0)] = (src[IDX(lastRow, 0)] + src[IDX(lastRow, 1)] + src[IDX(preLastRow, 0)]) / 3; + dst[IDX(lastRow, lastCol)] = (src[IDX(lastRow, lastCol)] + src[IDX(lastRow, preLastCol)] + + src[IDX(preLastRow, lastCol)]) / + 3; + + /* Now the edges. */ + for (unsigned int i = 1; i < lastCol; i++) { + dst[IDX(0, i)] = (src[IDX(0, i - 1)] + src[IDX(0, i)] + src[IDX(0, i + 1)] + src[IDX(1, i)]) / 4; + dst[IDX(lastRow, i)] = (src[IDX(lastRow, i - 1)] + src[IDX(lastRow, i)] + + src[IDX(lastRow, i + 1)] + src[IDX(preLastRow, i)]) / + 4; + } + + for (unsigned int i = 1; i < lastRow; i++) { + dst[IDX(i, 0)] = (src[IDX(i - 1, 0)] + src[IDX(i, 0)] + src[IDX(i + 1, 0)] + src[IDX(i, 1)]) / 4; + dst[IDX(i, 31)] = (src[IDX(i - 1, lastCol)] + src[IDX(i, lastCol)] + + src[IDX(i + 1, lastCol)] + src[IDX(i, preLastCol)]) / + 4; + } + + /* Finally the interior. */ + for (unsigned int j = 1; j < lastRow; j++) { + for (unsigned int i = 1; i < lastCol; i++) { + dst[IDX(j, i)] = (src[IDX(j - 1, i)] + src[IDX(j, i - 1)] + src[IDX(j, i)] + + src[IDX(j, i + 1)] + src[IDX(j + 1, i)]) / + 5; + } + } +} + +void Hdr::updateGains(StatisticsPtr &stats, HdrConfig &config) +{ + if (config.spatialGainCurve.empty()) + return; + + /* When alternating exposures, only compute these gains for the short frame. */ + if (delayedStatus_.mode == "MultiExposure" && delayedStatus_.channel != "short") + return; + + for (unsigned int i = 0; i < numRegions_; i++) { + auto ®ion = stats->awbRegions.get(i); + unsigned int counted = region.counted; + counted += (counted == 0); /* avoid div by zero */ + double r = region.val.rSum / counted; + double g = region.val.gSum / counted; + double b = region.val.bSum / counted; + double brightness = std::max({ r, g, b }) / 65535; + gains_[0][i] = config.spatialGainCurve.eval(brightness); + } + + /* Ping-pong between the two gains_ buffers. */ + for (unsigned int i = 0; i < config.diffusion; i++) + averageGains(gains_[i & 1], gains_[(i & 1) ^ 1], regions_); +} + +void Hdr::process(StatisticsPtr &stats, Metadata *imageMetadata) +{ + /* Note what HDR channel this frame will be once it comes back to us. */ + updateAgcStatus(imageMetadata); + + /* + * Now figure out what HDR channel this frame is. It should be available in the + * agc.delayed_status, unless this is an early frame after a mode switch, in which + * case delayedStatus_ should be right. + */ + AgcStatus agcStatus; + if (!imageMetadata->get<AgcStatus>("agc.delayed_status", agcStatus)) + delayedStatus_ = agcStatus.hdr; + + auto it = config_.find(delayedStatus_.mode); + if (it == config_.end()) { + /* Shouldn't be possible. There would be nothing we could do. */ + LOG(RPiHdr, Warning) << "Unexpected HDR mode " << delayedStatus_.mode; + return; + } + + HdrConfig &config = it->second; + + /* Update the spatially varying gains. They get written in prepare(). */ + updateGains(stats, config); + + if (updateTonemap(stats, config)) { + /* Add tonemap.status metadata. */ + TonemapStatus tonemapStatus; + + tonemapStatus.detailConstant = config.detailConstant; + tonemapStatus.detailSlope = config.detailSlope; + tonemapStatus.iirStrength = config.iirStrength; + tonemapStatus.strength = config.strength; + tonemapStatus.tonemap = tonemap_; + + imageMetadata->set("tonemap.status", tonemapStatus); + } + + if (config.stitchEnable) { + /* Add stitch.status metadata. */ + StitchStatus stitchStatus; + + stitchStatus.diffPower = config.diffPower; + stitchStatus.motionThreshold = config.motionThreshold; + stitchStatus.thresholdLo = config.thresholdLo; + + imageMetadata->set("stitch.status", stitchStatus); + } +} + +/* Register algorithm with the system. */ +static Algorithm *create(Controller *controller) +{ + return (Algorithm *)new Hdr(controller); +} +static RegisterAlgorithm reg(NAME, &create); diff --git a/src/ipa/rpi/controller/rpi/hdr.h b/src/ipa/rpi/controller/rpi/hdr.h new file mode 100644 index 00000000..9b7327f8 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/hdr.h @@ -0,0 +1,84 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2023, Raspberry Pi Ltd + * + * HDR control algorithm + */ +#pragma once + +#include <map> +#include <string> +#include <vector> + +#include <libcamera/geometry.h> + +#include "../hdr_algorithm.h" +#include "../hdr_status.h" +#include "../pwl.h" + +/* This is our implementation of an HDR algorithm. */ + +namespace RPiController { + +struct HdrConfig { + std::string name; + std::vector<unsigned int> cadence; + std::map<unsigned int, std::string> channelMap; + + /* Lens shading related parameters. */ + Pwl spatialGainCurve; /* Brightness to gain curve for different image regions. */ + unsigned int diffusion; /* How much to diffuse the gain spatially. */ + + /* Tonemap related parameters. */ + bool tonemapEnable; + uint16_t detailConstant; + double detailSlope; + double iirStrength; + double strength; + Pwl tonemap; + /* These relate to adaptive tonemap calculation. */ + double speed; + std::vector<double> hiQuantileTargets; /* quantiles to check for unsaturated images */ + double hiQuantileMaxGain; /* the max gain we'll apply when unsaturated */ + std::vector<double> quantileTargets; /* target values for histogram quantiles */ + double powerMin; /* minimum tonemap power */ + double powerMax; /* maximum tonemap power */ + std::vector<double> contrastAdjustments; /* any contrast adjustment factors */ + + /* Stitch related parameters. */ + bool stitchEnable; + uint16_t thresholdLo; + uint8_t diffPower; + double motionThreshold; + + void read(const libcamera::YamlObject ¶ms, const std::string &name); +}; + +class Hdr : public HdrAlgorithm +{ +public: + Hdr(Controller *controller); + char const *name() const override; + void switchMode(CameraMode const &cameraMode, Metadata *metadata) override; + int read(const libcamera::YamlObject ¶ms) override; + void prepare(Metadata *imageMetadata) override; + void process(StatisticsPtr &stats, Metadata *imageMetadata) override; + int setMode(std::string const &mode) override; + std::vector<unsigned int> getChannels() const override; + +private: + void updateAgcStatus(Metadata *metadata); + void updateGains(StatisticsPtr &stats, HdrConfig &config); + bool updateTonemap(StatisticsPtr &stats, HdrConfig &config); + + std::map<std::string, HdrConfig> config_; + HdrStatus status_; /* track the current HDR mode and channel */ + HdrStatus delayedStatus_; /* track the delayed HDR mode and channel */ + std::string previousMode_; + Pwl tonemap_; + libcamera::Size regions_; /* stats regions */ + unsigned int numRegions_; /* total number of stats regions */ + std::vector<double> gains_[2]; +}; + +} /* namespace RPiController */ diff --git a/src/ipa/rpi/controller/rpi/lux.cpp b/src/ipa/rpi/controller/rpi/lux.cpp new file mode 100644 index 00000000..7b31faab --- /dev/null +++ b/src/ipa/rpi/controller/rpi/lux.cpp @@ -0,0 +1,115 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * Lux control algorithm + */ +#include <math.h> + +#include <libcamera/base/log.h> + +#include "../device_status.h" + +#include "lux.h" + +using namespace RPiController; +using namespace libcamera; +using namespace std::literals::chrono_literals; + +LOG_DEFINE_CATEGORY(RPiLux) + +#define NAME "rpi.lux" + +Lux::Lux(Controller *controller) + : Algorithm(controller) +{ + /* + * Put in some defaults as there will be no meaningful values until + * Process has run. + */ + status_.aperture = 1.0; + status_.lux = 400; +} + +char const *Lux::name() const +{ + return NAME; +} + +int Lux::read(const libcamera::YamlObject ¶ms) +{ + auto value = params["reference_shutter_speed"].get<double>(); + if (!value) + return -EINVAL; + referenceShutterSpeed_ = *value * 1.0us; + + value = params["reference_gain"].get<double>(); + if (!value) + return -EINVAL; + referenceGain_ = *value; + + referenceAperture_ = params["reference_aperture"].get<double>(1.0); + + value = params["reference_Y"].get<double>(); + if (!value) + return -EINVAL; + referenceY_ = *value; + + value = params["reference_lux"].get<double>(); + if (!value) + return -EINVAL; + referenceLux_ = *value; + + currentAperture_ = referenceAperture_; + return 0; +} + +void Lux::setCurrentAperture(double aperture) +{ + currentAperture_ = aperture; +} + +void Lux::prepare(Metadata *imageMetadata) +{ + std::unique_lock<std::mutex> lock(mutex_); + imageMetadata->set("lux.status", status_); +} + +void Lux::process(StatisticsPtr &stats, Metadata *imageMetadata) +{ + DeviceStatus deviceStatus; + if (imageMetadata->get("device.status", deviceStatus) == 0) { + double currentGain = deviceStatus.analogueGain; + double currentAperture = deviceStatus.aperture.value_or(currentAperture_); + double currentY = stats->yHist.interQuantileMean(0, 1); + double gainRatio = referenceGain_ / currentGain; + double shutterSpeedRatio = + referenceShutterSpeed_ / deviceStatus.shutterSpeed; + double apertureRatio = referenceAperture_ / currentAperture; + double yRatio = currentY * (65536 / stats->yHist.bins()) / referenceY_; + double estimatedLux = shutterSpeedRatio * gainRatio * + apertureRatio * apertureRatio * + yRatio * referenceLux_; + LuxStatus status; + status.lux = estimatedLux; + status.aperture = currentAperture; + LOG(RPiLux, Debug) << ": estimated lux " << estimatedLux; + { + std::unique_lock<std::mutex> lock(mutex_); + status_ = status; + } + /* + * Overwrite the metadata here as well, so that downstream + * algorithms get the latest value. + */ + imageMetadata->set("lux.status", status); + } else + LOG(RPiLux, Warning) << ": no device metadata"; +} + +/* Register algorithm with the system. */ +static Algorithm *create(Controller *controller) +{ + return (Algorithm *)new Lux(controller); +} +static RegisterAlgorithm reg(NAME, &create); diff --git a/src/ipa/rpi/controller/rpi/lux.h b/src/ipa/rpi/controller/rpi/lux.h new file mode 100644 index 00000000..89f441fc --- /dev/null +++ b/src/ipa/rpi/controller/rpi/lux.h @@ -0,0 +1,45 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * Lux control algorithm + */ +#pragma once + +#include <mutex> + +#include <libcamera/base/utils.h> + +#include "../lux_status.h" +#include "../algorithm.h" + +/* This is our implementation of the "lux control algorithm". */ + +namespace RPiController { + +class Lux : public Algorithm +{ +public: + Lux(Controller *controller); + char const *name() const override; + int read(const libcamera::YamlObject ¶ms) override; + void prepare(Metadata *imageMetadata) override; + void process(StatisticsPtr &stats, Metadata *imageMetadata) override; + void setCurrentAperture(double aperture); + +private: + /* + * These values define the conditions of the reference image, against + * which we compare the new image. + */ + libcamera::utils::Duration referenceShutterSpeed_; + double referenceGain_; + double referenceAperture_; /* units of 1/f */ + double referenceY_; /* out of 65536 */ + double referenceLux_; + double currentAperture_; + LuxStatus status_; + std::mutex mutex_; +}; + +} /* namespace RPiController */ diff --git a/src/ipa/rpi/controller/rpi/noise.cpp b/src/ipa/rpi/controller/rpi/noise.cpp new file mode 100644 index 00000000..3f1c62cf --- /dev/null +++ b/src/ipa/rpi/controller/rpi/noise.cpp @@ -0,0 +1,89 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * Noise control algorithm + */ + +#include <math.h> + +#include <libcamera/base/log.h> + +#include "../device_status.h" +#include "../noise_status.h" + +#include "noise.h" + +using namespace RPiController; +using namespace libcamera; + +LOG_DEFINE_CATEGORY(RPiNoise) + +#define NAME "rpi.noise" + +Noise::Noise(Controller *controller) + : Algorithm(controller), modeFactor_(1.0) +{ +} + +char const *Noise::name() const +{ + return NAME; +} + +void Noise::switchMode(CameraMode const &cameraMode, + [[maybe_unused]] Metadata *metadata) +{ + /* + * For example, we would expect a 2x2 binned mode to have a "noise + * factor" of sqrt(2x2) = 2. (can't be less than one, right?) + */ + modeFactor_ = std::max(1.0, cameraMode.noiseFactor); +} + +int Noise::read(const libcamera::YamlObject ¶ms) +{ + auto value = params["reference_constant"].get<double>(); + if (!value) + return -EINVAL; + referenceConstant_ = *value; + + value = params["reference_slope"].get<double>(); + if (!value) + return -EINVAL; + referenceSlope_ = *value; + + return 0; +} + +void Noise::prepare(Metadata *imageMetadata) +{ + struct DeviceStatus deviceStatus; + deviceStatus.analogueGain = 1.0; /* keep compiler calm */ + if (imageMetadata->get("device.status", deviceStatus) == 0) { + /* + * There is a slight question as to exactly how the noise + * profile, specifically the constant part of it, scales. For + * now we assume it all scales the same, and we'll revisit this + * if it proves substantially wrong. NOTE: we may also want to + * make some adjustments based on the camera mode (such as + * binning), if we knew how to discover it... + */ + double factor = sqrt(deviceStatus.analogueGain) / modeFactor_; + struct NoiseStatus status; + status.noiseConstant = referenceConstant_ * factor; + status.noiseSlope = referenceSlope_ * factor; + imageMetadata->set("noise.status", status); + LOG(RPiNoise, Debug) + << "constant " << status.noiseConstant + << " slope " << status.noiseSlope; + } else + LOG(RPiNoise, Warning) << " no metadata"; +} + +/* Register algorithm with the system. */ +static Algorithm *create(Controller *controller) +{ + return new Noise(controller); +} +static RegisterAlgorithm reg(NAME, &create); diff --git a/src/ipa/rpi/controller/rpi/noise.h b/src/ipa/rpi/controller/rpi/noise.h new file mode 100644 index 00000000..6deae1f0 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/noise.h @@ -0,0 +1,32 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * Noise control algorithm + */ +#pragma once + +#include "../algorithm.h" +#include "../noise_status.h" + +/* This is our implementation of the "noise algorithm". */ + +namespace RPiController { + +class Noise : public Algorithm +{ +public: + Noise(Controller *controller); + char const *name() const override; + void switchMode(CameraMode const &cameraMode, Metadata *metadata) override; + int read(const libcamera::YamlObject ¶ms) override; + void prepare(Metadata *imageMetadata) override; + +private: + /* the noise profile for analogue gain of 1.0 */ + double referenceConstant_; + double referenceSlope_; + double modeFactor_; +}; + +} /* namespace RPiController */ diff --git a/src/ipa/rpi/controller/rpi/saturation.cpp b/src/ipa/rpi/controller/rpi/saturation.cpp new file mode 100644 index 00000000..b83c5887 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/saturation.cpp @@ -0,0 +1,57 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2022 Raspberry Pi Ltd + * + * Saturation control algorithm + */ +#include "saturation.h" + +#include <libcamera/base/log.h> + +#include "saturation_status.h" + +using namespace RPiController; +using namespace libcamera; + +LOG_DEFINE_CATEGORY(RPiSaturation) + +#define NAME "rpi.saturation" + +Saturation::Saturation(Controller *controller) + : Algorithm(controller) +{ +} + +char const *Saturation::name() const +{ + return NAME; +} + +int Saturation::read(const libcamera::YamlObject ¶ms) +{ + config_.shiftR = params["shift_r"].get<uint8_t>(0); + config_.shiftG = params["shift_g"].get<uint8_t>(0); + config_.shiftB = params["shift_b"].get<uint8_t>(0); + return 0; +} + +void Saturation::initialise() +{ +} + +void Saturation::prepare(Metadata *imageMetadata) +{ + SaturationStatus saturation; + + saturation.shiftR = config_.shiftR; + saturation.shiftG = config_.shiftG; + saturation.shiftB = config_.shiftB; + imageMetadata->set("saturation.status", saturation); +} + +// Register algorithm with the system. +static Algorithm *Create(Controller *controller) +{ + return (Algorithm *)new Saturation(controller); +} +static RegisterAlgorithm reg(NAME, &Create); diff --git a/src/ipa/rpi/controller/rpi/saturation.h b/src/ipa/rpi/controller/rpi/saturation.h new file mode 100644 index 00000000..97da412a --- /dev/null +++ b/src/ipa/rpi/controller/rpi/saturation.h @@ -0,0 +1,32 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2022, Raspberry Pi Ltd + * + * saturation.hpp - Saturation control algorithm + */ +#pragma once + +#include "algorithm.h" + +namespace RPiController { + +struct SaturationConfig { + uint8_t shiftR; + uint8_t shiftG; + uint8_t shiftB; +}; + +class Saturation : public Algorithm +{ +public: + Saturation(Controller *controller = NULL); + char const *name() const override; + int read(const libcamera::YamlObject ¶ms) override; + void initialise() override; + void prepare(Metadata *imageMetadata) override; + +private: + SaturationConfig config_; +}; + +} // namespace RPiController diff --git a/src/ipa/rpi/controller/rpi/sdn.cpp b/src/ipa/rpi/controller/rpi/sdn.cpp new file mode 100644 index 00000000..619178a8 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/sdn.cpp @@ -0,0 +1,83 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019-2021, Raspberry Pi Ltd + * + * SDN (spatial denoise) control algorithm + */ + +#include <libcamera/base/log.h> +#include <libcamera/base/utils.h> + +#include "../denoise_status.h" +#include "../noise_status.h" + +#include "sdn.h" + +using namespace RPiController; +using namespace libcamera; + +LOG_DEFINE_CATEGORY(RPiSdn) + +/* + * Calculate settings for the spatial denoise block using the noise profile in + * the image metadata. + */ + +#define NAME "rpi.sdn" + +Sdn::Sdn(Controller *controller) + : DenoiseAlgorithm(controller), mode_(DenoiseMode::ColourOff) +{ +} + +char const *Sdn::name() const +{ + return NAME; +} + +int Sdn::read(const libcamera::YamlObject ¶ms) +{ + LOG(RPiSdn, Warning) + << "Using legacy SDN tuning - please consider moving SDN inside rpi.denoise"; + deviation_ = params["deviation"].get<double>(3.2); + strength_ = params["strength"].get<double>(0.75); + return 0; +} + +void Sdn::initialise() +{ +} + +void Sdn::prepare(Metadata *imageMetadata) +{ + struct NoiseStatus noiseStatus = {}; + noiseStatus.noiseSlope = 3.0; /* in case no metadata */ + if (imageMetadata->get("noise.status", noiseStatus) != 0) + LOG(RPiSdn, Warning) << "no noise profile found"; + LOG(RPiSdn, Debug) + << "Noise profile: constant " << noiseStatus.noiseConstant + << " slope " << noiseStatus.noiseSlope; + struct DenoiseStatus status; + status.noiseConstant = noiseStatus.noiseConstant * deviation_; + status.noiseSlope = noiseStatus.noiseSlope * deviation_; + status.strength = strength_; + status.mode = utils::to_underlying(mode_); + imageMetadata->set("denoise.status", status); + LOG(RPiSdn, Debug) + << "programmed constant " << status.noiseConstant + << " slope " << status.noiseSlope + << " strength " << status.strength; +} + +void Sdn::setMode(DenoiseMode mode) +{ + /* We only distinguish between off and all other modes. */ + mode_ = mode; +} + +/* Register algorithm with the system. */ +static Algorithm *create(Controller *controller) +{ + return (Algorithm *)new Sdn(controller); +} +static RegisterAlgorithm reg(NAME, &create); diff --git a/src/ipa/rpi/controller/rpi/sdn.h b/src/ipa/rpi/controller/rpi/sdn.h new file mode 100644 index 00000000..cb226de8 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/sdn.h @@ -0,0 +1,32 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * SDN (spatial denoise) control algorithm + */ +#pragma once + +#include "../algorithm.h" +#include "../denoise_algorithm.h" + +namespace RPiController { + +/* Algorithm to calculate correct spatial denoise (SDN) settings. */ + +class Sdn : public DenoiseAlgorithm +{ +public: + Sdn(Controller *controller = NULL); + char const *name() const override; + int read(const libcamera::YamlObject ¶ms) override; + void initialise() override; + void prepare(Metadata *imageMetadata) override; + void setMode(DenoiseMode mode) override; + +private: + double deviation_; + double strength_; + DenoiseMode mode_; +}; + +} /* namespace RPiController */ diff --git a/src/ipa/rpi/controller/rpi/sharpen.cpp b/src/ipa/rpi/controller/rpi/sharpen.cpp new file mode 100644 index 00000000..39537f4a --- /dev/null +++ b/src/ipa/rpi/controller/rpi/sharpen.cpp @@ -0,0 +1,92 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * sharpening control algorithm + */ + +#include <math.h> + +#include <libcamera/base/log.h> + +#include "../sharpen_status.h" + +#include "sharpen.h" + +using namespace RPiController; +using namespace libcamera; + +LOG_DEFINE_CATEGORY(RPiSharpen) + +#define NAME "rpi.sharpen" + +Sharpen::Sharpen(Controller *controller) + : SharpenAlgorithm(controller), userStrength_(1.0) +{ +} + +char const *Sharpen::name() const +{ + return NAME; +} + +void Sharpen::switchMode(CameraMode const &cameraMode, + [[maybe_unused]] Metadata *metadata) +{ + /* can't be less than one, right? */ + modeFactor_ = std::max(1.0, cameraMode.noiseFactor); +} + +int Sharpen::read(const libcamera::YamlObject ¶ms) +{ + threshold_ = params["threshold"].get<double>(1.0); + strength_ = params["strength"].get<double>(1.0); + limit_ = params["limit"].get<double>(1.0); + LOG(RPiSharpen, Debug) + << "Read threshold " << threshold_ + << " strength " << strength_ + << " limit " << limit_; + return 0; +} + +void Sharpen::setStrength(double strength) +{ + /* + * Note that this function is how an application sets the overall + * sharpening "strength". We call this the "user strength" field + * as there already is a strength_ field - being an internal gain + * parameter that gets passed to the ISP control code. Negative + * values are not allowed - coerce them to zero (no sharpening). + */ + userStrength_ = std::max(0.0, strength); +} + +void Sharpen::prepare(Metadata *imageMetadata) +{ + /* + * The userStrength_ affects the algorithm's internal gain directly, but + * we adjust the limit and threshold less aggressively. Using a sqrt + * function is an arbitrary but gentle way of accomplishing this. + */ + double userStrengthSqrt = sqrt(userStrength_); + struct SharpenStatus status; + /* + * Binned modes seem to need the sharpening toned down with this + * pipeline, thus we use the modeFactor_ here. Also avoid + * divide-by-zero with the userStrengthSqrt. + */ + status.threshold = threshold_ * modeFactor_ / + std::max(0.01, userStrengthSqrt); + status.strength = strength_ / modeFactor_ * userStrength_; + status.limit = limit_ / modeFactor_ * userStrengthSqrt; + /* Finally, report any application-supplied parameters that were used. */ + status.userStrength = userStrength_; + imageMetadata->set("sharpen.status", status); +} + +/* Register algorithm with the system. */ +static Algorithm *create(Controller *controller) +{ + return new Sharpen(controller); +} +static RegisterAlgorithm reg(NAME, &create); diff --git a/src/ipa/rpi/controller/rpi/sharpen.h b/src/ipa/rpi/controller/rpi/sharpen.h new file mode 100644 index 00000000..96ccd609 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/sharpen.h @@ -0,0 +1,34 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2019, Raspberry Pi Ltd + * + * sharpening control algorithm + */ +#pragma once + +#include "../sharpen_algorithm.h" +#include "../sharpen_status.h" + +/* This is our implementation of the "sharpen algorithm". */ + +namespace RPiController { + +class Sharpen : public SharpenAlgorithm +{ +public: + Sharpen(Controller *controller); + char const *name() const override; + void switchMode(CameraMode const &cameraMode, Metadata *metadata) override; + int read(const libcamera::YamlObject ¶ms) override; + void setStrength(double strength) override; + void prepare(Metadata *imageMetadata) override; + +private: + double threshold_; + double strength_; + double limit_; + double modeFactor_; + double userStrength_; +}; + +} /* namespace RPiController */ diff --git a/src/ipa/rpi/controller/rpi/tonemap.cpp b/src/ipa/rpi/controller/rpi/tonemap.cpp new file mode 100644 index 00000000..0426e972 --- /dev/null +++ b/src/ipa/rpi/controller/rpi/tonemap.cpp @@ -0,0 +1,61 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2022 Raspberry Pi Ltd + * + * Tonemap control algorithm + */ +#include "tonemap.h" + +#include <libcamera/base/log.h> + +#include "tonemap_status.h" + +using namespace RPiController; +using namespace libcamera; + +LOG_DEFINE_CATEGORY(RPiTonemap) + +#define NAME "rpi.tonemap" + +Tonemap::Tonemap(Controller *controller) + : Algorithm(controller) +{ +} + +char const *Tonemap::name() const +{ + return NAME; +} + +int Tonemap::read(const libcamera::YamlObject ¶ms) +{ + config_.detailConstant = params["detail_constant"].get<uint16_t>(0); + config_.detailSlope = params["detail_slope"].get<double>(0.1); + config_.iirStrength = params["iir_strength"].get<double>(1.0); + config_.strength = params["strength"].get<double>(1.0); + config_.tonemap.read(params["tone_curve"]); + return 0; +} + +void Tonemap::initialise() +{ +} + +void Tonemap::prepare(Metadata *imageMetadata) +{ + TonemapStatus tonemapStatus; + + tonemapStatus.detailConstant = config_.detailConstant; + tonemapStatus.detailSlope = config_.detailSlope; + tonemapStatus.iirStrength = config_.iirStrength; + tonemapStatus.strength = config_.strength; + tonemapStatus.tonemap = config_.tonemap; + imageMetadata->set("tonemap.status", tonemapStatus); +} + +// Register algorithm with the system. +static Algorithm *Create(Controller *controller) +{ + return (Algorithm *)new Tonemap(controller); +} +static RegisterAlgorithm reg(NAME, &Create); diff --git a/src/ipa/rpi/controller/rpi/tonemap.h b/src/ipa/rpi/controller/rpi/tonemap.h new file mode 100644 index 00000000..f25aa47f --- /dev/null +++ b/src/ipa/rpi/controller/rpi/tonemap.h @@ -0,0 +1,35 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2022, Raspberry Pi Ltd + * + * tonemap.hpp - Tonemap control algorithm + */ +#pragma once + +#include "algorithm.h" +#include "pwl.h" + +namespace RPiController { + +struct TonemapConfig { + uint16_t detailConstant; + double detailSlope; + double iirStrength; + double strength; + Pwl tonemap; +}; + +class Tonemap : public Algorithm +{ +public: + Tonemap(Controller *controller = NULL); + char const *name() const override; + int read(const libcamera::YamlObject ¶ms) override; + void initialise() override; + void prepare(Metadata *imageMetadata) override; + +private: + TonemapConfig config_; +}; + +} // namespace RPiController |