From patchwork Tue Mar 14 14:48:28 2023 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: Daniel Semkowicz X-Patchwork-Id: 18396 Return-Path: X-Original-To: parsemail@patchwork.libcamera.org Delivered-To: parsemail@patchwork.libcamera.org Received: from lancelot.ideasonboard.com (lancelot.ideasonboard.com [92.243.16.209]) by patchwork.libcamera.org (Postfix) with ESMTPS id 1FA4FC329C for ; Tue, 14 Mar 2023 14:50:04 +0000 (UTC) Received: from lancelot.ideasonboard.com (localhost [IPv6:::1]) by lancelot.ideasonboard.com (Postfix) with ESMTP id 07C0662709; Tue, 14 Mar 2023 15:50:03 +0100 (CET) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/simple; d=libcamera.org; s=mail; t=1678805403; bh=1bstBxppEM9TyGd4rPlh5RFkJCmHXVsZ3O5DHBFeUaA=; h=To:Date:In-Reply-To:References:Subject:List-Id:List-Unsubscribe: List-Archive:List-Post:List-Help:List-Subscribe:From:Reply-To:Cc: From; b=O1ErC9y59eAUhOKQ5HLlELY8gQ5+Pm1oE7r2whZQptr5tysL/MpnsQooDsg+RKMJU F54Hzf425NedElKs6cY/gucRzlVfRt9HwZW3S4kbKd4rPndpgFW2VUCl6du70iIzGs 3Q80MnBfnIMkzoOaQupThbigDuI13XsIbNRTwzrUsXZdBDeIyg6wO4DajAwFOGsQPH 5CqpPtHBdYmiYBpn5nh8T/YjVWCOD9GgCHb62qzbqEe8xB4D8doHkbUHAyjsBOk2fM SR+3t60XNjfYFHFBDix69XpTKY52nQMhIV6i3W5U2svu0ROqpsUfXg64eCiDDUkVEh wNUWdq2MU9WNQ== Received: from mail-lj1-x22b.google.com (mail-lj1-x22b.google.com [IPv6:2a00:1450:4864:20::22b]) by lancelot.ideasonboard.com (Postfix) with ESMTPS id E63A062710 for ; Tue, 14 Mar 2023 15:49:53 +0100 (CET) Authentication-Results: lancelot.ideasonboard.com; dkim=pass (2048-bit key; unprotected) header.d=thaumatec-com.20210112.gappssmtp.com header.i=@thaumatec-com.20210112.gappssmtp.com header.b="s0QTt5Ib"; dkim-atps=neutral Received: by mail-lj1-x22b.google.com with SMTP id a32so16293423ljq.1 for ; Tue, 14 Mar 2023 07:49:53 -0700 (PDT) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=thaumatec-com.20210112.gappssmtp.com; s=20210112; t=1678805393; h=content-transfer-encoding:mime-version:references:in-reply-to :message-id:date:subject:cc:to:from:from:to:cc:subject:date :message-id:reply-to; bh=vS1Kbj5O9OP4CxUMem8zYbMA/KhxmOSW/DP3/vIXaOw=; b=s0QTt5Ibi10mwdn/8i354u2fJvdXpQG9YLQZfxJYI6KC/3S5koov8ZIbLJjKDE755e i/fKeaKm13gxcGJOBgGfpN+MrUye5zBm3KJwqVT9Qx7z4xKCeO084mEMUORGdNRM6pYn R5kcmTXiDEyMyQUQDEFWB5Ze5BsdePYhC8dkf25mT71SaZg5KsdysKKFPRg46dQSMrsw P4Bo3ijF6Qa1dfRop3jx5r0FdsN47NEK01+KUz70iysjgHYG3GXlvhdkk2tMM0ikESi5 iNiwA7+VdokjmBPb6nbFrm90Qhh+5JI5h04U0DCXBgs5mYQicZRPM5027m/JI05GwKFi UUmg== X-Google-DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=1e100.net; s=20210112; t=1678805393; h=content-transfer-encoding:mime-version:references:in-reply-to :message-id:date:subject:cc:to:from:x-gm-message-state:from:to:cc :subject:date:message-id:reply-to; bh=vS1Kbj5O9OP4CxUMem8zYbMA/KhxmOSW/DP3/vIXaOw=; b=YZeSPWPV5z1ufGdoxRMzaZ+Ki2Hn1VvetV8BbUvBdPodajvjBO++6xtRc9lwK5O1sI GFt4U0IjpPDlO6+9BLdbb2TTWgFlrdTICN6xEiHkLI8uWeEkmiAjqjUY2hlvaSm2/H/7 LPsd4ipkn9sIx+8Xqt33HG/v9FmhMXoRN32W9O3FED3yWDT4onLABh2WYWF43ncYCmsR vm5trVNDyCZ24HLvvd5ne8rUYotxWsvlVHT22hVm1omi+82HZSVrVG4W1YazTjCtyWIt UboEiRwEwizzhDnCsM2d6rUFDi6KKJcD3V3B5HgT3mCegm7icP0qGh5U/j/n1g1kLbcC cGDA== X-Gm-Message-State: AO0yUKU1vZxMTk5jUvZvcvsrZy9nlNGDZVlNbQx15tVl/FjHD7VBTBCz O0ozQ/qmW8t6TudYfBhS+Gx023Zj4ZSEkMDT+VU= X-Google-Smtp-Source: AK7set8Ubo3GF8quTODs1UwgQ6b1Ih7tzgAyLvxZwzEOV3gwdtOGpCBaTkNEH3sFTPS8scew1GGSSg== X-Received: by 2002:a2e:a586:0:b0:298:39fa:b04d with SMTP id m6-20020a2ea586000000b0029839fab04dmr13988860ljp.33.1678805393018; Tue, 14 Mar 2023 07:49:53 -0700 (PDT) Received: from localhost.localdomain ([91.90.166.178]) by smtp.gmail.com with ESMTPSA id q25-20020a2eb4b9000000b00295a35f30e1sm470961ljm.115.2023.03.14.07.49.52 (version=TLS1_3 cipher=TLS_AES_256_GCM_SHA384 bits=256/256); Tue, 14 Mar 2023 07:49:52 -0700 (PDT) To: libcamera-devel@lists.libcamera.org Date: Tue, 14 Mar 2023 15:48:28 +0100 Message-Id: <20230314144834.85193-5-dse@thaumatec.com> X-Mailer: git-send-email 2.39.2 In-Reply-To: <20230314144834.85193-1-dse@thaumatec.com> References: <20230314144834.85193-1-dse@thaumatec.com> MIME-Version: 1.0 Subject: [libcamera-devel] [PATCH v4 04/10] ipa: Add common contrast based AF implementation X-BeenThere: libcamera-devel@lists.libcamera.org X-Mailman-Version: 2.1.29 Precedence: list List-Id: List-Unsubscribe: , List-Archive: List-Post: List-Help: List-Subscribe: , X-Patchwork-Original-From: Daniel Semkowicz via libcamera-devel From: Daniel Semkowicz Reply-To: Daniel Semkowicz Cc: jacopo.mondi@ideasonboard.com Errors-To: libcamera-devel-bounces@lists.libcamera.org Sender: "libcamera-devel" Create a new class with contrast based Auto Focus implementation using hill climbing algorithm. This common implementation is independent of platform specific code. This way, each platform can just implement contrast calculation and run the AF control loop basing on this class. This implementation is based on the code that was common for IPU3 and RPi AF algorithms. Signed-off-by: Daniel Semkowicz --- .../libipa/algorithms/af_hill_climbing.cpp | 358 ++++++++++++++++++ src/ipa/libipa/algorithms/af_hill_climbing.h | 93 +++++ src/ipa/libipa/algorithms/meson.build | 2 + 3 files changed, 453 insertions(+) create mode 100644 src/ipa/libipa/algorithms/af_hill_climbing.cpp create mode 100644 src/ipa/libipa/algorithms/af_hill_climbing.h 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..2b99afef --- /dev/null +++ b/src/ipa/libipa/algorithms/af_hill_climbing.cpp @@ -0,0 +1,358 @@ +/* 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::ipa::common::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 + * basing on the contrast measure supplied by the higher level. This way it is + * independent from the platform. + * + * Platform layer that use this algorithm should call process() method + * for each measured contrast value 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, lens are moved with bigger steps to quickly find a rough + * position for the best focus. Then, basing on the outcome of coarse search, + * the second phase is executed. Lens are 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 position will change in one + * step in the *coarse search* phase. + * - **fine-search-step:** The value by which the position will change in one + * step in the *fine search* phase. + * - **fine-search-range:** Search range in the *fine search* phase. + * Valid values are in the [0.0, 1.0] interval. Value 0.05 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. + */ + +/** + * \brief Initialize the AfHillClimbing with tuning data + * \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(const YamlObject &tuningData) +{ + coarseSearchStep_ = tuningData["coarse-search-step"].get(30); + fineSearchStep_ = tuningData["fine-search-step"].get(1); + fineRange_ = tuningData["fine-search-range"].get(0.05); + maxChange_ = tuningData["max-variance-change"].get(0.5); + + LOG(Af, Debug) << "coarseSearchStep_: " << coarseSearchStep_ + << ", fineSearchStep_: " << fineSearchStep_ + << ", fineRange_: " << fineRange_ + << ", maxChange_: " << maxChange_; + + return 0; +} + +/** + * \brief Configure the AfHillClimbing with sensor and lens information + * \param[in] minFocusPosition Minimum position supported by camera lens + * \param[in] maxFocusPosition Maximum position supported by camera lens + * + * This method should be called in the libcamera::ipa::Algorithm::configure() + * method of the platform layer. + */ +void AfHillClimbing::configure(int32_t minFocusPosition, + int32_t maxFocusPosition) +{ + minLensPosition_ = minFocusPosition; + maxLensPosition_ = maxFocusPosition; +} + +/** + * \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 new contrast value that was measured. + * + * \return New lens position calculated by 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: + processContinousMode(); + break; + default: + break; + } + + return lensPosition_; +} + +void AfHillClimbing::processAutoMode() +{ + if (state_ == controls::AfStateScanning) { + afCoarseScan(); + afFineScan(); + } +} + +void AfHillClimbing::processContinousMode() +{ + /* 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; + } + + /* We can re-start the scan at any moment in AfModeContinuous */ + if (afIsOutOfFocus()) { + afReset(); + } +} + +/** + * \brief Request AF to skip n frames + * \param[in] n Number of frames to be skipped + * + * Requested number of frames will not be used for AF calculation. + */ +void AfHillClimbing::setFramesToSkip(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) << __FUNCTION__ << " not implemented!"; +} + +void AfHillClimbing::setSpeed([[maybe_unused]] controls::AfSpeedEnum speed) +{ + LOG(Af, Error) << __FUNCTION__ << " not implemented!"; +} + +void AfHillClimbing::setMeteringMode([[maybe_unused]] controls::AfMeteringEnum metering) +{ + LOG(Af, Error) << __FUNCTION__ << " not implemented!"; +} + +void AfHillClimbing::setWindows([[maybe_unused]] Span windows) +{ + LOG(Af, Error) << __FUNCTION__ << " not implemented!"; +} + +void AfHillClimbing::setTrigger(controls::AfTriggerEnum trigger) +{ + if (mode_ != controls::AfModeAuto) { + LOG(Af, Warning) + << __FUNCTION__ << " not possible in mode " << mode_; + return; + } + + LOG(Af, Debug) << "Trigger called with " << trigger; + + if (trigger == controls::AfTriggerStart) + afReset(); + else + state_ = controls::AfStateIdle; +} + +void AfHillClimbing::setPause(controls::AfPauseEnum pause) +{ + if (mode_ != controls::AfModeContinuous) { + LOG(Af, Warning) + << __FUNCTION__ << " not possible in mode " << mode_; + return; + } + + switch (pause) { + case controls::AfPauseImmediate: + pauseState_ = controls::AfPauseStatePaused; + break; + case controls::AfPauseDeferred: + /* \todo: add the AfPauseDeferred mode */ + LOG(Af, Warning) << "AfPauseDeferred is not supported!"; + break; + case controls::AfPauseResume: + pauseState_ = controls::AfPauseStateRunning; + break; + default: + break; + } +} + +void AfHillClimbing::setLensPosition(float lensPosition) +{ + if (mode_ != controls::AfModeManual) { + LOG(Af, Warning) + << __FUNCTION__ << " not possible in mode " << mode_; + return; + } + + lensPosition_ = lensPosition; + + LOG(Af, Debug) << "Requesting lens position " << lensPosition_; +} + +void AfHillClimbing::afCoarseScan() +{ + if (coarseCompleted_) + return; + + if (afScan(coarseSearchStep_)) { + coarseCompleted_ = true; + maxContrast_ = 0; + int32_t diff = std::abs(lensPosition_) * fineRange_; + lensPosition_ = std::max(lensPosition_ - diff, minLensPosition_); + maxStep_ = std::min(lensPosition_ + diff, maxLensPosition_); + previousContrast_ = 0; + } +} + +void AfHillClimbing::afFineScan() +{ + if (!coarseCompleted_) + return; + + if (afScan(fineSearchStep_)) { + LOG(Af, Debug) << "AF found the best focus position!"; + state_ = controls::AfStateFocused; + fineCompleted_ = true; + } +} + +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; + } 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_ += 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; + } + } + + previousContrast_ = currentContrast_; + 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; + previousContrast_ = 0.0; + coarseCompleted_ = false; + fineCompleted_ = false; + maxContrast_ = 0.0; + setFramesToSkip(1); +} + +bool AfHillClimbing::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 lens step: " << lensPosition_; + if (var_ratio > maxChange_) + return true; + else + return false; +} + +bool AfHillClimbing::shouldSkipFrame() +{ + if (framesToSkip_ > 0) { + framesToSkip_--; + return true; + } + + return false; +} + +} /* namespace libcamera::ipa::common::algorithms */ 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..b361e5a1 --- /dev/null +++ b/src/ipa/libipa/algorithms/af_hill_climbing.h @@ -0,0 +1,93 @@ +/* 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::common::algorithms { + +LOG_DECLARE_CATEGORY(Af) + +class AfHillClimbing : public Af +{ +public: + int init(const YamlObject &tuningData); + void configure(int32_t minFocusPosition, int32_t maxFocusPosition); + int32_t process(double currentContrast); + void setFramesToSkip(uint32_t n); + + controls::AfStateEnum state() final { return state_; } + controls::AfPauseStateEnum pauseState() final { return pauseState_; } + +private: + void setMode(controls::AfModeEnum mode) final; + void setRange(controls::AfRangeEnum range) final; + void setSpeed(controls::AfSpeedEnum speed) final; + void setMeteringMode(controls::AfMeteringEnum metering) final; + void setWindows(Span windows) final; + void setTrigger(controls::AfTriggerEnum trigger) final; + void setPause(controls::AfPauseEnum pause) final; + void setLensPosition(float lensPosition) final; + + void processAutoMode(); + void processContinousMode(); + void afCoarseScan(); + void afFineScan(); + bool afScan(uint32_t steps); + void afReset(); + bool afIsOutOfFocus(); + bool shouldSkipFrame(); + + controls::AfModeEnum mode_ = controls::AfModeManual; + controls::AfStateEnum state_ = controls::AfStateIdle; + controls::AfPauseStateEnum pauseState_ = controls::AfPauseStateRunning; + + /* VCM step configuration. It is the current setting of the VCM step. */ + int32_t lensPosition_ = 0; + /* The best VCM step. It is a local optimum VCM step during scanning. */ + int32_t bestPosition_ = 0; + + /* Current AF statistic contrast. */ + double currentContrast_ = 0; + /* It is used to determine the derivative during scanning */ + double previousContrast_ = 0; + 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; + /* If the fine scan completes, it is set to true. */ + bool fineCompleted_ = false; + + uint32_t framesToSkip_ = 0; + + /* Focus steps range of the lens */ + int32_t minLensPosition_; + int32_t maxLensPosition_; + + /* Minimum focus 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::common::algorithms */ +} /* namespace libcamera */ diff --git a/src/ipa/libipa/algorithms/meson.build b/src/ipa/libipa/algorithms/meson.build index 7602976c..fa4b9564 100644 --- a/src/ipa/libipa/algorithms/meson.build +++ b/src/ipa/libipa/algorithms/meson.build @@ -2,8 +2,10 @@ common_ipa_algorithms_headers = files([ 'af.h', + 'af_hill_climbing.h', ]) common_ipa_algorithms_sources = files([ 'af.cpp', + 'af_hill_climbing.cpp', ])