diff options
Diffstat (limited to 'src/ipa/libipa/algorithms/af_hill_climbing.h')
-rw-r--r-- | src/ipa/libipa/algorithms/af_hill_climbing.h | 251 |
1 files changed, 251 insertions, 0 deletions
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..db9fc058 --- /dev/null +++ b/src/ipa/libipa/algorithms/af_hill_climbing.h @@ -0,0 +1,251 @@ +/* SPDX-License-Identifier: LGPL-2.1-or-later */ +/* + * Copyright (C) 2021, Red Hat + * Copyright (C) 2022, Ideas On Board + * Copyright (C) 2022, Theobroma Systems + * + * af_hill_climbing.h - AF Hill Climbing common algorithm + */ + +#pragma once + +#include <libcamera/base/log.h> + +#include "af_algorithm.h" + +namespace libcamera { + +LOG_DECLARE_CATEGORY(Af) + +namespace ipa::common::algorithms { + +template<typename Module> +class AfHillClimbing : public AfAlgorithm<Module> +{ +public: + AfHillClimbing() + : mode_(controls::AfModeAuto), state_(controls::AfStateIdle), + pauseState_(controls::AfPauseStateRunning), + lensPosition_(0), bestPosition_(0), currentContrast_(0.0), + previousContrast_(0.0), maxContrast_(0.0), maxStep_(0), + coarseCompleted_(false), fineCompleted_(false), + lowStep_(0), highStep_(kMaxFocusSteps) + { + } + + virtual ~AfHillClimbing() {} + + void setMode(controls::AfModeEnum mode) final + { + if (mode != mode_) { + LOG(Af, Debug) << "Switched AF mode from " << mode_ + << " to " << mode; + pauseState_ = libcamera::controls::AfPauseStateRunning; + mode_ = mode; + } + } + + void setRange([[maybe_unused]] controls::AfRangeEnum range) final + { + LOG(Af, Error) << __FUNCTION__ << " not implemented!"; + } + + void setSpeed([[maybe_unused]] controls::AfSpeedEnum speed) final + { + LOG(Af, Error) << __FUNCTION__ << " not implemented!"; + } + + void setTrigger(controls::AfTriggerEnum trigger) final + { + LOG(Af, Debug) << "Trigger called in mode " << mode_ + << " with " << trigger; + if (mode_ == libcamera::controls::AfModeAuto) { + if (trigger == libcamera::controls::AfTriggerStart) + afReset(); + else + state_ = libcamera::controls::AfStateIdle; + } + } + + void setPause(controls::AfPauseEnum pause) final + { + /* \todo: add the AfPauseDeferred mode */ + if (mode_ == libcamera::controls::AfModeContinuous) { + if (pause == libcamera::controls::AfPauseImmediate) + pauseState_ = libcamera::controls::AfPauseStatePaused; + else if (pause == libcamera::controls::AfPauseResume) + pauseState_ = libcamera::controls::AfPauseStateRunning; + } + } + + void setLensPosition([[maybe_unused]] float lensPosition) final + { + LOG(Af, Error) << __FUNCTION__ << " not implemented!"; + } + + /* These methods should be implemented by derived class */ + virtual void setMetering(controls::AfMeteringEnum metering) = 0; + virtual void setWindows(Span<const Rectangle> windows) = 0; + +protected: + uint32_t processAutofocus(double currentContrast) + { + currentContrast_ = currentContrast; + + /* If we are in a paused state, we won't process the stats */ + if (pauseState_ == libcamera::controls::AfPauseStatePaused) + return lensPosition_; + + /* Depending on the mode, we may or may not process the stats */ + if (state_ == libcamera::controls::AfStateIdle) + return lensPosition_; + + if (state_ != libcamera::controls::AfStateFocused) { + afCoarseScan(); + afFineScan(); + } else { + /* We can re-start the scan at any moment in AfModeContinuous */ + if (mode_ == libcamera::controls::AfModeContinuous) + if (afIsOutOfFocus()) + afReset(); + } + + return lensPosition_; + } + +private: + void afCoarseScan() + { + if (coarseCompleted_) + return; + + if (afScan(kCoarseSearchStep)) { + coarseCompleted_ = true; + maxContrast_ = 0; + lensPosition_ = lensPosition_ - (lensPosition_ * kFineRange); + previousContrast_ = 0; + maxStep_ = std::clamp(lensPosition_ + static_cast<uint32_t>((lensPosition_ * kFineRange)), + 0U, highStep_); + } + } + + void afFineScan() + { + if (!coarseCompleted_) + return; + + if (afScan(kFineSearchStep)) { + LOG(Af, Debug) << "AF found the best focus position !"; + state_ = libcamera::controls::AfStateFocused; + fineCompleted_ = true; + } + } + + bool afScan(uint32_t minSteps) + { + if (lensPosition_ + minSteps > maxStep_) { + /* If the max step is reached, move lens to the position. */ + lensPosition_ = bestPosition_; + return true; + } else { + /* + * Find the maximum of the variance by estimating its + * derivative. If the direction changes, it means we have passed + * a maximum one step before. + */ + if ((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_ += minSteps; + 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; + } + } + + previousContrast_ = currentContrast_; + LOG(Af, Debug) << " Previous step is " << bestPosition_ + << " Current step is " << lensPosition_; + return false; + } + + void afReset() + { + LOG(Af, Debug) << "Reset AF parameters"; + lensPosition_ = lowStep_; + maxStep_ = highStep_; + state_ = libcamera::controls::AfStateScanning; + previousContrast_ = 0.0; + coarseCompleted_ = false; + fineCompleted_ = false; + maxContrast_ = 0.0; + } + + bool afIsOutOfFocus() + { + const uint32_t diff_var = std::abs(currentContrast_ - + maxContrast_); + const double var_ratio = diff_var / maxContrast_; + LOG(Af, Debug) << "Variance change rate: " << var_ratio + << " Current VCM step: " << lensPosition_; + if (var_ratio > kMaxChange) + return true; + else + return false; + } + + controls::AfModeEnum mode_; + controls::AfStateEnum state_; + controls::AfPauseStateEnum pauseState_; + + /* VCM step configuration. It is the current setting of the VCM step. */ + uint32_t lensPosition_; + /* The best VCM step. It is a local optimum VCM step during scanning. */ + uint32_t bestPosition_; + + /* Current AF statistic contrast. */ + double currentContrast_; + /* It is used to determine the derivative during scanning */ + double previousContrast_; + double maxContrast_; + /* The designated maximum range of focus scanning. */ + uint32_t maxStep_; + /* If the coarse scan completes, it is set to true. */ + bool coarseCompleted_; + /* If the fine scan completes, it is set to true. */ + bool fineCompleted_; + + uint32_t lowStep_; + uint32_t highStep_; + + /* + * Maximum focus steps of the VCM control + * \todo should be obtained from the VCM driver + */ + static constexpr uint32_t kMaxFocusSteps = 1023; + + /* Minimum focus step for searching appropriate focus */ + static constexpr uint32_t kCoarseSearchStep = 30; + static constexpr uint32_t kFineSearchStep = 1; + + /* Max ratio of variance change, 0.0 < kMaxChange < 1.0 */ + static constexpr double kMaxChange = 0.5; + + /* Fine scan range 0 < kFineRange < 1 */ + static constexpr double kFineRange = 0.05; +}; + +} /* namespace ipa::common::algorithms */ +} /* namespace libcamera */ |