[libcamera-devel,01/10] ipa: raspberrypi: Introduce an autofocus algorithm

16210 diff mbox series

diff --git a/src/ipa/raspberrypi/controller/af_algorithm.hpp b/src/ipa/raspberrypi/controller/af_algorithm.hpp
new file mode 100644
index 00000000..1e7aaa45
--- /dev/null
+++ b/src/ipa/raspberrypi/controller/af_algorithm.hpp
@@ -0,0 +1,33 @@ 
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2022, Raspberry Pi (Trading) Limited
+ *
+ * af_algorithm.hpp - autofocus control algorithm interface
+ */
+#pragma once
+
+#include <libcamera/geometry.h>
+
+#include "algorithm.hpp"
+
+namespace RPiController {
+
+class AfAlgorithm : public Algorithm
+{
+public:
+	AfAlgorithm(Controller *controller) : Algorithm(controller) {}
+	// An af algorithm must provide the following:
+	virtual void SetMode(const uint32_t &mode) = 0;
+	// start a cycle (in auto mode)
+	virtual void Trigger() = 0;
+	// cancel a cycle (in auto mode)
+	virtual void Cancel() = 0;
+	// set AF windows
+	virtual void SetWindows(const libcamera::Rectangle &afWindows) = 0;
+	// set AF range
+	virtual void SetRange(const uint32_t &range) = 0;
+	// set AF speed
+	virtual void setSpeed(const uint32_t &speed) = 0;
+};
+
+} // namespace RPiController
diff --git a/src/ipa/raspberrypi/controller/af_status.h b/src/ipa/raspberrypi/controller/af_status.h
new file mode 100644
index 00000000..f8cb1301
--- /dev/null
+++ b/src/ipa/raspberrypi/controller/af_status.h
@@ -0,0 +1,21 @@ 
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2022, Raspberry Pi (Trading) Limited
+ * Copyright (C) 2022, Ideas On Board
+ *
+ * af_status.h - autofocus measurement status
+ */
+#pragma once
+
+#include <libcamera/geometry.h>
+
+/*
+ * The focus algorithm should post the following structure into the image's
+ * "af.status" metadata.
+ */
+
+struct AfStatus {
+	uint32_t lensPosition;
+	uint32_t state;
+	libcamera::Rectangle windows;
+};
diff --git a/src/ipa/raspberrypi/controller/iob/af.cpp b/src/ipa/raspberrypi/controller/iob/af.cpp
new file mode 100644
index 00000000..12d41c81
--- /dev/null
+++ b/src/ipa/raspberrypi/controller/iob/af.cpp
@@ -0,0 +1,212 @@ 
+/* SPDX-License-Identifier: LGPL-2.1-or-later */
+/*
+ * Copyright (C) 2021, Red Hat
+ * Copyright (C) 2022, Ideas On Board
+ *
+ * af.cpp - automatic contrast-based focus algorithm
+ */
+#include <cmath>
+
+#include <stdint.h>
+
+#include <libcamera/base/log.h>
+
+#include "af.h"
+
+using namespace RPiController;
+using namespace libcamera;
+
+LOG_DEFINE_CATEGORY(IoBAf)
+
+#define NAME "iob.af"
+
+/*
+ * 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;
+
+Af::Af(Controller *controller)
+	: AfAlgorithm(controller), focus_(0), bestFocus_(0),
+	  currentContrast_(0.0), previousContrast_(0.0), maxContrast_(0.0),
+	  maxStep_(0), coarseCompleted_(false), fineCompleted_(false),
+	  mode_(0)
+{
+}
+
+char const *Af::Name() const
+{
+	return NAME;
+}
+
+void Af::SetMode([[maybe_unused]] const uint32_t &mode)
+{
+	mode_ = mode;
+}
+
+void Af::Trigger()
+{
+}
+
+void Af::Cancel()
+{
+}
+
+void Af::SetWindows([[maybe_unused]] const libcamera::Rectangle &afWindows)
+{
+}
+
+void Af::SetRange([[maybe_unused]] const uint32_t &range)
+{
+}
+
+void Af::setSpeed([[maybe_unused]] const uint32_t &speed)
+{
+}
+
+void Af::Initialise()
+{
+	status_.lensPosition = 0.0;
+	maxContrast_ = 0.0;
+	status_.state = 1;
+}
+
+void Af::Prepare(Metadata *image_metadata)
+{
+	image_metadata->Set("af.status", status_);
+}
+
+void Af::afCoarseScan()
+{
+	if (coarseCompleted_)
+		return;
+
+	if (afScan(kCoarseSearchStep)) {
+		coarseCompleted_ = true;
+		maxContrast_ = 0;
+		focus_ = status_.lensPosition - (status_.lensPosition * kFineRange);
+		status_.lensPosition = focus_;
+		previousContrast_ = 0;
+		maxStep_ = std::clamp(focus_ + static_cast<uint32_t>((focus_ * kFineRange)),
+				      0U, kMaxFocusSteps);
+	}
+}
+
+void Af::afFineScan()
+{
+	if (!coarseCompleted_)
+		return;
+
+	if (afScan(kFineSearchStep)) {
+		LOG(IoBAf, Debug) << "AF found the best focus position !";
+		status_.state = 2;
+		fineCompleted_ = true;
+	}
+}
+
+bool Af::afScan(uint32_t minSteps)
+{
+	if (focus_ > maxStep_) {
+		/* If the max step is reached, move lens to the position. */
+		status_.lensPosition = bestFocus_;
+		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.
+			 */
+			bestFocus_ = focus_;
+			focus_ += minSteps;
+			maxContrast_ = currentContrast_;
+			status_.lensPosition = focus_;
+		} 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.
+			 */
+			status_.lensPosition = bestFocus_;
+			return true;
+		}
+	}
+
+	previousContrast_ = currentContrast_;
+	LOG(IoBAf, Debug) << " Previous step is "
+			  << bestFocus_
+			  << " Current step is "
+			  << focus_;
+	return false;
+}
+
+void Af::afReset()
+{
+	LOG(IoBAf, Debug) << "Reset AF parameters";
+	status_.lensPosition = 0;
+	focus_ = 0;
+	status_.state = 0;
+	previousContrast_ = 0.0;
+	coarseCompleted_ = false;
+	fineCompleted_ = false;
+	maxStep_ = kMaxFocusSteps;
+	maxContrast_ = 0.0;
+}
+
+bool Af::afIsOutOfFocus()
+{
+	const uint32_t diff_var = std::abs(currentContrast_ -
+					   maxContrast_);
+	const double var_ratio = diff_var / maxContrast_;
+	LOG(IoBAf, Debug) << "Variance change rate: "
+			  << var_ratio
+			  << " Current VCM step: "
+			  << status_.lensPosition;
+	if (var_ratio > kMaxChange)
+		return true;
+	else
+		return false;
+}
+
+void Af::Process(StatisticsPtr &stats, [[maybe_unused]] Metadata *image_metadata)
+{
+	unsigned int i;
+	currentContrast_ = 0.0;
+
+	/* Use the second filter results only, and cache those. */
+	for (i = 0; i < FOCUS_REGIONS; i++)
+		currentContrast_ += stats->focus_stats[i].contrast_val[1][1]
+				  / stats->focus_stats[i].contrast_val_num[1][1];
+
+	if (status_.state != 2) {
+		afCoarseScan();
+		afFineScan();
+	} else {
+		if (afIsOutOfFocus())
+			afReset();
+	}
+}
+
+/* Register algorithm with the system. */
+static Algorithm *Create(Controller *controller)
+{
+	return new Af(controller);
+}
+static RegisterAlgorithm reg(NAME, &Create);
diff --git a/src/ipa/raspberrypi/controller/iob/af.h b/src/ipa/raspberrypi/controller/iob/af.h
new file mode 100644
index 00000000..4ed50cfb
--- /dev/null
+++ b/src/ipa/raspberrypi/controller/iob/af.h
@@ -0,0 +1,62 @@ 
+/* SPDX-License-Identifier: LGPL-2.1-or-later */
+/*
+ * Copyright (C) 2021, Red Hat
+ * Copyright (C) 2022, Ideas On Board
+ *
+ * af.h - automatic contrast-based focus algorithm
+ */
+#pragma once
+
+#include <libcamera/geometry.h>
+
+#include "../af_algorithm.hpp"
+#include "../af_status.h"
+#include "../metadata.hpp"
+
+namespace RPiController {
+
+class Af : public AfAlgorithm
+{
+public:
+	Af(Controller *controller);
+	char const *Name() const override;
+	void Initialise() override;
+	void Prepare(Metadata *image_metadata) override;
+	void Process(StatisticsPtr &stats, Metadata *image_metadata) override;
+	void SetMode(const uint32_t &mode) override;
+	void Trigger() override;
+	void Cancel() override;
+	void SetWindows(const libcamera::Rectangle &afWindows) override;
+	void SetRange(const uint32_t &range) override;
+	void setSpeed(const uint32_t &speed) override;
+private:
+	bool afNeedIgnoreFrame();
+	void afCoarseScan();
+	void afFineScan();
+	bool afScan(uint32_t minSteps);
+	void afReset();
+	bool afIsOutOfFocus();
+
+	AfStatus status_;
+
+	/* VCM step configuration. It is the current setting of the VCM step. */
+	uint32_t focus_;
+	/* The best VCM step. It is a local optimum VCM step during scanning. */
+	uint32_t bestFocus_;
+
+	/* 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 mode_;
+};
+
+} /* namespace RPiController */
diff --git a/src/ipa/raspberrypi/meson.build b/src/ipa/raspberrypi/meson.build
index 32897e07..37068ecc 100644
--- a/src/ipa/raspberrypi/meson.build
+++ b/src/ipa/raspberrypi/meson.build
@@ -28,6 +28,7 @@  rpi_ipa_sources = files([
     'controller/controller.cpp',
     'controller/histogram.cpp',
     'controller/algorithm.cpp',
+    'controller/iob/af.cpp',
     'controller/rpi/alsc.cpp',
     'controller/rpi/awb.cpp',
     'controller/rpi/sharpen.cpp',