[libcamera-devel] [PATCH v2 02/11] ipa: Add class that implements base AF control algorithm

[Jacopo Mondi]

Kate in cc for real this time!

On Thu, Jul 14, 2022 at 06:17:23PM +0200, Jacopo Mondi via libcamera-devel wrote:
> Hi Daniel
>
> On Wed, Jul 13, 2022 at 10:43:08AM +0200, Daniel Semkowicz via libcamera-devel wrote:
> > Move the code that was common for IPU3 and RPi AF algorithms to
> > a separate class 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 is exactly the purpose of having algorithms in libipa. I'm
> excited it's the first "generic" one we have, or that I know of at
> least!
>
> This mean the very IPU3 implementation this algorithm is based on
> (Kate in cc) can now be generalized, and the same for the RPi one we
> have on the list!
>
> > Signed-off-by: Daniel Semkowicz <dse at thaumatec.com>
> > ---
> >  .../libipa/algorithms/af_hill_climbing.cpp    |  89 +++++++
> >  src/ipa/libipa/algorithms/af_hill_climbing.h  | 251 ++++++++++++++++++
> >  src/ipa/libipa/algorithms/meson.build         |   2 +
> >  3 files changed, 342 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..f666c6c2
> > --- /dev/null
> > +++ b/src/ipa/libipa/algorithms/af_hill_climbing.cpp
> > @@ -0,0 +1,89 @@
> > +/* 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.cpp - AF Hill Climbing common algorithm
> > + */
>
> Any particular reason why the implementation is in the .h file ?
>
> > +
> > +#include "af_hill_climbing.h"
> > +
> > +/**
> > + * \file af_hill_climbing.h
> > + * \brief AF Hill Climbing common algorithm
> > + */
> > +
> > +namespace libcamera {
> > +
> > +LOG_DEFINE_CATEGORY(Af)
> > +
> > +namespace ipa::common::algorithms {
> > +
> > +/**
> > + * \class AfHillClimbing
> > + * \brief The base class implementing hill climbing AF control algorithm
> > + * \tparam Module The IPA module type for this class of algorithms
> > + *
> > + * Control part of auto focus algorithm. It calculates the lens position basing
> > + * on contrast measure supplied by the higher level. This way it is independent
> > + * from the platform.
> > + *
> > + * Derived class should call processAutofocus() for each measured contrast value
> > + * and set the lens to the calculated position.
> > + */
> > +
> > +/**
> > + * \fn AfHillClimbing::setMode()
> > + * \copydoc libcamera::ipa::common::algorithms::AfAlgorithm::setMode
> > + */
> > +
> > +/**
> > + * \fn AfHillClimbing::setRange()
> > + * \copydoc libcamera::ipa::common::algorithms::AfAlgorithm::setRange
> > + */
> > +
> > +/**
> > + * \fn AfHillClimbing::setSpeed()
> > + * \copydoc libcamera::ipa::common::algorithms::AfAlgorithm::setSpeed
> > + */
> > +
> > +/**
> > + * \fn AfHillClimbing::setMetering()
> > + * \copydoc libcamera::ipa::common::algorithms::AfAlgorithm::setMetering
> > + */
> > +
> > +/**
> > + * \fn AfHillClimbing::setWindows()
> > + * \copydoc libcamera::ipa::common::algorithms::AfAlgorithm::setWindows
> > + */
> > +
> > +/**
> > + * \fn AfHillClimbing::setTrigger()
> > + * \copydoc libcamera::ipa::common::algorithms::AfAlgorithm::setTrigger
> > + */
> > +
> > +/**
> > + * \fn AfHillClimbing::setPause()
> > + * \copydoc libcamera::ipa::common::algorithms::AfAlgorithm::setPause
> > + */
> > +
> > +/**
> > + * \fn AfHillClimbing::setLensPosition()
> > + * \copydoc libcamera::ipa::common::algorithms::AfAlgorithm::setLensPosition
> > + */
> > +
> > +/**
> > + * \fn AfHillClimbing::processAutofocus()
> > + * \brief Run the auto focus algorithm loop
> > + * \param[in] currentContrast New value of contrast measured for current frame
> > + *
> > + * This method should be called for each new contrast value that was measured,
> > + * usually in the process() method.
> > + *
> > + * \return New lens position calculated by AF algorithm
> > + */
> > +
> > +} /* namespace ipa::common::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..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)
> > +	{
> > +	}
>
> Let's look at the implementation details later but I have one question
>
> > +
> > +	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();
>
> This seems out of place..
> Whenever I trigger an autofocus scan I get the lens reset to 0 causing
> a not very nice effect. Why are you resetting the lens position here?
>
> > +			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_;
>
> Let me recap one point. We are still missing a generic definition for
> the lens position value. As you can see we use
> V4L2_CID_FOCUS_ABSOLUTE which transports the actual value to be set in
> the v4l2 control. This can't work here and we are planning to remove
> V4L2 controls from the IPA in general, but while for other controls we
> have defined generic ranges, for the lens position we are still
> missing a way to compute a "generic" value in the IPA, send it to the
> pipeline handler by using libcamera::controls::LensPosition and have
> the CameraLens class translate it to the device-specific value.
>
> I'm a bit lazy and I'm not chasing how lensPosition_ is here computed,
> but have you considered how this could be generalized already ? As an
> example, it seems to me the values I get are in the 0-180 range, while
> in example the lens I'm using ranges from 0-4096...
>
> > +	}
> > +
> > +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 */
> > diff --git a/src/ipa/libipa/algorithms/meson.build b/src/ipa/libipa/algorithms/meson.build
> > index ab8da13a..860dc199 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_algorithm.h',
> > +    'af_hill_climbing.h',
> >  ])
> >
> >  common_ipa_algorithms_sources = files([
> >      'af_algorithm.cpp',
> > +    'af_hill_climbing.cpp',
> >  ])
> > --
> > 2.34.1
> >