diff --git a/src/ipa/libipa/algorithms/af_hill_climbing.cpp b/src/ipa/libipa/algorithms/af_hill_climbing.cpp new file mode 100644 index 00000000..244b8803 --- /dev/null +++ b/src/ipa/libipa/algorithms/af_hill_climbing.cpp @@ -0,0 +1,374 @@ +/* SPDX-License-Identifier: LGPL-2.1-or-later */ +/* + * Copyright (C) 2021, Red Hat + * Copyright (C) 2022, Ideas On Board + * Copyright (C) 2023, Theobroma Systems + * + * af_hill_climbing.cpp - AF contrast based hill climbing common algorithm + */ + +#include "af_hill_climbing.h" + +#include "libcamera/internal/yaml_parser.h" + +/** + * \file af_hill_climbing.h + * \brief AF contrast based hill climbing common algorithm + */ + +namespace libcamera { + +namespace ipa::algorithms { + +LOG_DEFINE_CATEGORY(Af) + +/** + * \class AfHillClimbing + * \brief Contrast based hill climbing auto focus control algorithm + * implementation + * + * Control part of the auto focus algorithm. It calculates the lens position + * based on the contrast measure supplied by the platform-specific + * implementation. This way it is independent from the platform. + * + * Platform layer that use this algorithm should call process() function + * for each each frame and set the lens to the calculated position. + * + * Focus search is divided into two phases: + * 1. coarse search, + * 2. fine search. + * + * In the first phase, the lens is moved with bigger steps to quickly find + * a rough position for the best focus. Then, based on the outcome of coarse + * search, the second phase is executed. Lens is moved with smaller steps + * in a limited range within the rough position to find the exact position + * for best focus. + * + * Tuning file parameters: + * - **coarse-search-step:** The value by which the lens position will change + * in one step in the *coarse search* phase. Unit is lens specific. + * - **fine-search-step:** The value by which the lens position will change + * in one step in the *fine search* phase. Unit is lens specific. + * - **fine-search-range:** Search range in the *fine search* phase, expressed + * as a percentage of the coarse search result. Valid values are + * in the [0, 100] interval. Value 5 means 5%. If coarse search stopped + * at value 200, the fine search range will be [190, 210]. + * - **max-variance-change:** ratio of contrast variance change in the + * *continuous mode* needed for triggering the focus change. When the variance + * change exceeds this value, focus search will be triggered. Valid values are + * in the [0.0, 1.0] interval. + * . + * + * \todo Search range in the *fine search* phase should depend on the lens + * movement range rather than coarse search result. + * \todo Implement setRange. + * \todo Implement setSpeed. + * \todo Implement setMeteringMode. + * \todo Implement setWindows. + * \todo Implement the AfPauseDeferred mode. + */ + +/** + * \brief Initialize the AfHillClimbing with lens configuration and tuning data + * \param[in] minFocusPosition Minimum position supported by camera lens + * \param[in] maxFocusPosition Maximum position supported by camera lens + * \param[in] tuningData The tuning data for the algorithm + * + * This method should be called in the libcamera::ipa::Algorithm::init() + * method of the platform layer. + * + * \return 0 if successful, an error code otherwise + */ +int AfHillClimbing::init(int32_t minFocusPosition, int32_t maxFocusPosition, + const YamlObject &tuningData) +{ + minLensPosition_ = minFocusPosition; + maxLensPosition_ = maxFocusPosition; + + coarseSearchStep_ = tuningData["coarse-search-step"].get(30); + fineSearchStep_ = tuningData["fine-search-step"].get(1); + fineRange_ = tuningData["fine-search-range"].get(5); + fineRange_ /= 100; + maxChange_ = tuningData["max-variance-change"].get(0.5); + + LOG(Af, Debug) << "coarseSearchStep_: " << coarseSearchStep_ + << ", fineSearchStep_: " << fineSearchStep_ + << ", fineRange_: " << fineRange_ + << ", maxChange_: " << maxChange_; + + return 0; +} + +/** + * \brief Run the auto focus algorithm loop + * \param[in] currentContrast New value of contrast measured for current frame + * + * This method should be called in the libcamera::ipa::Algorithm::process() + * method of the platform layer for each frame. + * + * Contrast value supplied in the \p currentContrast parameter can be platform + * specific. The only requirement is the contrast value must increase with + * the increasing image focus. Contrast value must be highest when image is in + * focus. + * + * \return New lens position calculated by the AF algorithm + */ +int32_t AfHillClimbing::process(double currentContrast) +{ + currentContrast_ = currentContrast; + + if (shouldSkipFrame()) + return lensPosition_; + + switch (mode_) { + case controls::AfModeManual: + /* Nothing to process. */ + break; + case controls::AfModeAuto: + processAutoMode(); + break; + case controls::AfModeContinuous: + processContinuousMode(); + break; + } + + return lensPosition_; +} + +void AfHillClimbing::processAutoMode() +{ + if (state_ == controls::AfStateScanning) { + afCoarseScan(); + afFineScan(); + } +} + +void AfHillClimbing::processContinuousMode() +{ + /* If we are in a paused state, we won't process the stats. */ + if (pauseState_ == controls::AfPauseStatePaused) + return; + + if (state_ == controls::AfStateScanning) { + afCoarseScan(); + afFineScan(); + return; + } + + /* + * AF scan can be started at any moment in AfModeContinuous, + * except when the state is already AfStateScanning. + */ + if (afIsOutOfFocus()) + afReset(); +} + +/** + * \brief Request AF to skip n frames + * \param[in] n Number of frames to be skipped + * + * For the requested number of frames, the AF calculation will be skipped + * and lens position will not change. The platform layer still needs to + * call process() function for each frame during this time. + * This function can be used by the platform layer if the hardware needs more + * time for some operations. + * + * The number of the requested frames (\p n) will be applied only if \p n has + * higher value than the number of frames already requested to be skipped. + * For example, if *skipFrames(5)* was already called for the current frame, + * then calling *skipFrames(3)* will not override the previous request + * and 5 frames will be skipped. + */ +void AfHillClimbing::skipFrames(uint32_t n) +{ + if (n > framesToSkip_) + framesToSkip_ = n; +} + +void AfHillClimbing::setMode(controls::AfModeEnum mode) +{ + if (mode == mode_) + return; + + LOG(Af, Debug) << "Switched AF mode from " << mode_ << " to " << mode; + mode_ = mode; + + state_ = controls::AfStateIdle; + pauseState_ = controls::AfPauseStateRunning; + + if (mode_ == controls::AfModeContinuous) + afReset(); +} + +void AfHillClimbing::setRange([[maybe_unused]] controls::AfRangeEnum range) +{ + LOG(Af, Error) << "setRange() not implemented!"; +} + +void AfHillClimbing::setSpeed([[maybe_unused]] controls::AfSpeedEnum speed) +{ + LOG(Af, Error) << "setSpeed() not implemented!"; +} + +void AfHillClimbing::setMeteringMode([[maybe_unused]] controls::AfMeteringEnum metering) +{ + LOG(Af, Error) << "setMeteringMode() not implemented!"; +} + +void AfHillClimbing::setWindows([[maybe_unused]] Span windows) +{ + LOG(Af, Error) << "setWindows() not implemented!"; +} + +void AfHillClimbing::setTrigger(controls::AfTriggerEnum trigger) +{ + if (mode_ != controls::AfModeAuto) { + LOG(Af, Warning) + << "setTrigger() not valid in mode " << mode_; + return; + } + + LOG(Af, Debug) << "Trigger called with " << trigger; + + switch (trigger) { + case controls::AfTriggerStart: + afReset(); + break; + case controls::AfTriggerCancel: + state_ = controls::AfStateIdle; + break; + } +} + +void AfHillClimbing::setPause(controls::AfPauseEnum pause) +{ + if (mode_ != controls::AfModeContinuous) { + LOG(Af, Warning) + << "setPause() not valid in mode " << mode_; + return; + } + + switch (pause) { + case controls::AfPauseImmediate: + pauseState_ = controls::AfPauseStatePaused; + break; + case controls::AfPauseDeferred: + LOG(Af, Warning) << "AfPauseDeferred is not supported!"; + break; + case controls::AfPauseResume: + pauseState_ = controls::AfPauseStateRunning; + break; + } +} + +void AfHillClimbing::setLensPosition(float lensPosition) +{ + if (mode_ != controls::AfModeManual) { + LOG(Af, Warning) + << "setLensPosition() not valid in mode " << mode_; + return; + } + + lensPosition_ = static_cast(lensPosition); + + LOG(Af, Debug) << "Requesting lens position " << lensPosition_; +} + +void AfHillClimbing::afCoarseScan() +{ + if (coarseCompleted_) + return; + + if (afScan(coarseSearchStep_)) { + coarseCompleted_ = true; + maxContrast_ = 0; + const auto diff = static_cast( + std::abs(lensPosition_) * fineRange_); + lensPosition_ = std::max(lensPosition_ - diff, minLensPosition_); + maxStep_ = std::min(lensPosition_ + diff, maxLensPosition_); + } +} + +void AfHillClimbing::afFineScan() +{ + if (!coarseCompleted_) + return; + + if (afScan(fineSearchStep_)) { + LOG(Af, Debug) << "AF found the best focus position!"; + state_ = controls::AfStateFocused; + } +} + +bool AfHillClimbing::afScan(uint32_t steps) +{ + if (lensPosition_ + static_cast(steps) > maxStep_) { + /* If the max step is reached, move lens to the position. */ + lensPosition_ = bestPosition_; + return true; + } + + /* + * Find the maximum of the variance by estimating its derivative. + * If the direction changes, it means we have passed a maximum one step + * before. + */ + if ((currentContrast_ - maxContrast_) >= -(maxContrast_ * 0.1)) { + /* + * Positive and zero derivative: + * The variance is still increasing. The focus could be + * increased for the next comparison. Also, the max variance + * and previous focus value are updated. + */ + bestPosition_ = lensPosition_; + lensPosition_ += static_cast(steps); + maxContrast_ = currentContrast_; + } else { + /* + * Negative derivative: + * The variance starts to decrease, which means the maximum + * variance is found. Set focus step to previous good one, + * then return immediately. + */ + lensPosition_ = bestPosition_; + return true; + } + + LOG(Af, Debug) << "Previous step is " << bestPosition_ + << ", Current step is " << lensPosition_; + return false; +} + +void AfHillClimbing::afReset() +{ + LOG(Af, Debug) << "Reset AF parameters"; + lensPosition_ = minLensPosition_; + maxStep_ = maxLensPosition_; + state_ = controls::AfStateScanning; + coarseCompleted_ = false; + maxContrast_ = 0.0; + skipFrames(1); +} + +bool AfHillClimbing::afIsOutOfFocus() const +{ + const double diff_var = std::abs(currentContrast_ - maxContrast_); + const double var_ratio = diff_var / maxContrast_; + LOG(Af, Debug) << "Variance change rate: " << var_ratio + << ", Current lens step: " << lensPosition_; + return var_ratio > maxChange_; +} + +bool AfHillClimbing::shouldSkipFrame() +{ + if (framesToSkip_ > 0) { + framesToSkip_--; + return true; + } + + return false; +} + +} /* namespace ipa::algorithms */ + +} /* namespace libcamera */ diff --git a/src/ipa/libipa/algorithms/af_hill_climbing.h b/src/ipa/libipa/algorithms/af_hill_climbing.h new file mode 100644 index 00000000..2147939b --- /dev/null +++ b/src/ipa/libipa/algorithms/af_hill_climbing.h @@ -0,0 +1,91 @@ +/* SPDX-License-Identifier: LGPL-2.1-or-later */ +/* + * Copyright (C) 2021, Red Hat + * Copyright (C) 2022, Ideas On Board + * Copyright (C) 2023, Theobroma Systems + * + * af_hill_climbing.h - AF contrast based hill climbing common algorithm + */ + +#pragma once + +#include + +#include "af.h" + +namespace libcamera { + +class YamlObject; + +namespace ipa::algorithms { + +LOG_DECLARE_CATEGORY(Af) + +class AfHillClimbing : public Af +{ +public: + int init(int32_t minFocusPosition, int32_t maxFocusPosition, + const YamlObject &tuningData); + int32_t process(double currentContrast); + void skipFrames(uint32_t n); + + controls::AfStateEnum state() override { return state_; } + controls::AfPauseStateEnum pauseState() override { return pauseState_; } + +private: + void setMode(controls::AfModeEnum mode) override; + void setRange(controls::AfRangeEnum range) override; + void setSpeed(controls::AfSpeedEnum speed) override; + void setMeteringMode(controls::AfMeteringEnum metering) override; + void setWindows(Span windows) override; + void setTrigger(controls::AfTriggerEnum trigger) override; + void setPause(controls::AfPauseEnum pause) override; + void setLensPosition(float lensPosition) override; + + void processAutoMode(); + void processContinuousMode(); + void afCoarseScan(); + void afFineScan(); + bool afScan(uint32_t steps); + void afReset(); + [[nodiscard]] bool afIsOutOfFocus() const; + bool shouldSkipFrame(); + + controls::AfModeEnum mode_ = controls::AfModeManual; + controls::AfStateEnum state_ = controls::AfStateIdle; + controls::AfPauseStateEnum pauseState_ = controls::AfPauseStateRunning; + + /* Current focus lens position. */ + int32_t lensPosition_ = 0; + /* Local optimum focus lens position during scanning. */ + int32_t bestPosition_ = 0; + + /* Current AF statistic contrast. */ + double currentContrast_ = 0; + /* It is used to determine the derivative during scanning */ + double maxContrast_ = 0; + /* The designated maximum range of focus scanning. */ + int32_t maxStep_ = 0; + /* If the coarse scan completes, it is set to true. */ + bool coarseCompleted_ = false; + + uint32_t framesToSkip_ = 0; + + /* Position limits of the focus lens. */ + int32_t minLensPosition_; + int32_t maxLensPosition_; + + /* Minimum position step for searching appropriate focus. */ + uint32_t coarseSearchStep_; + uint32_t fineSearchStep_; + + /* Fine scan range 0 < fineRange_ < 1. */ + double fineRange_; + + /* Max ratio of variance change, 0.0 < maxChange_ < 1.0. */ + double maxChange_; +}; + +} /* namespace ipa::algorithms */ + +} /* namespace libcamera */ diff --git a/src/ipa/libipa/algorithms/meson.build b/src/ipa/libipa/algorithms/meson.build index 3df4798f..20e437fc 100644 --- a/src/ipa/libipa/algorithms/meson.build +++ b/src/ipa/libipa/algorithms/meson.build @@ -2,8 +2,10 @@ libipa_algorithms_headers = files([ 'af.h', + 'af_hill_climbing.h', ]) libipa_algorithms_sources = files([ 'af.cpp', + 'af_hill_climbing.cpp', ])