[libcamera-devel,RFC,1/3] ipa: raspberrypi: Introduce an autofocus algorithm

15515 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..553a37e1
--- /dev/null
+++ b/src/ipa/raspberrypi/controller/af_algorithm.hpp
@@ -0,0 +1,20 @@ 
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2019, Raspberry Pi (Trading) Limited
+ *
+ * af_algorithm.hpp - autofocus control algorithm interface
+ */
+#pragma once
+
+#include "algorithm.hpp"
+
+namespace RPiController {
+
+class AfAlgorithm : public Algorithm
+{
+public:
+	AfAlgorithm(Controller *controller) : Algorithm(controller) {}
+	// An af algorithm must provide the following:
+};
+
+} // 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..835e1e2f
--- /dev/null
+++ b/src/ipa/raspberrypi/controller/af_status.h
@@ -0,0 +1,31 @@ 
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2020, Raspberry Pi (Trading) Limited
+ * Copyright (C) 2022, Ideas On Board
+ *
+ * af_status.h - autofocus measurement status
+ */
+#pragma once
+
+#include <linux/bcm2835-isp.h>
+
+/*
+ * The focus algorithm should post the following structure into the image's
+ * "af.status" metadata.
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct AfStatus {
+	unsigned int num;
+	uint32_t focus_measures[FOCUS_REGIONS];
+	bool stable;
+	uint32_t focus;
+	double maxVariance;
+};
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/src/ipa/raspberrypi/controller/focus_status.h b/src/ipa/raspberrypi/controller/focus_status.h
index ace2fe2c..8122df4b 100644
--- a/src/ipa/raspberrypi/controller/focus_status.h
+++ b/src/ipa/raspberrypi/controller/focus_status.h
@@ -19,6 +19,9 @@  extern "C" {
 struct FocusStatus {
 	unsigned int num;
 	uint32_t focus_measures[FOCUS_REGIONS];
+	bool stable;
+	uint32_t focus;
+	double maxVariance;
 };
 
 #ifdef __cplusplus
diff --git a/src/ipa/raspberrypi/controller/iob/af.cpp b/src/ipa/raspberrypi/controller/iob/af.cpp
new file mode 100644
index 00000000..dc5258ba
--- /dev/null
+++ b/src/ipa/raspberrypi/controller/iob/af.cpp
@@ -0,0 +1,231 @@ 
+/* 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;
+
+/* The numbers of frame to be ignored, before performing focus scan. */
+static constexpr uint32_t kIgnoreFrame = 10;
+
+/* Fine scan range 0 < kFineRange < 1 */
+static constexpr double kFineRange = 0.05;
+
+Af::Af(Controller *controller)
+	: AfAlgorithm(controller), focus_(0), bestFocus_(0), ignoreCounter_(0),
+	  currentVariance_(0.0), previousVariance_(0.0), maxStep_(0),
+	  coarseCompleted_(false), fineCompleted_(false)
+{
+}
+
+char const *Af::Name() const
+{
+	return NAME;
+}
+
+void Af::Initialise()
+{
+	status_.focus = 0.0;
+	status_.maxVariance = 0.0;
+	status_.stable = false;
+}
+
+void Af::Prepare(Metadata *image_metadata)
+{
+	image_metadata->Set("af.status", status_);
+}
+
+double Af::estimateVariance()
+{
+	unsigned int i;
+	double mean;
+	uint64_t total = 0;
+	double var_sum = 0.0;
+
+	/* Compute the mean value. */
+	for (i = 0; i < FOCUS_REGIONS; i++)
+		total += status_.focus_measures[i];
+	mean = total / FOCUS_REGIONS;
+
+	/* Compute the sum of the squared variance. */
+	for (i = 0; i < FOCUS_REGIONS; i++)
+		var_sum += std::pow(status_.focus_measures[i] - mean, 2);
+
+	return var_sum / FOCUS_REGIONS;
+}
+
+bool Af::afNeedIgnoreFrame()
+{
+	if (ignoreCounter_ == 0)
+		return false;
+	else
+		ignoreCounter_--;
+	return true;
+}
+
+void Af::afCoarseScan()
+{
+	if (coarseCompleted_)
+		return;
+
+	if (afNeedIgnoreFrame())
+		return;
+
+	if (afScan(kCoarseSearchStep)) {
+		coarseCompleted_ = true;
+		status_.maxVariance = 0;
+		focus_ = status_.focus - (status_.focus * kFineRange);
+		status_.focus = focus_;
+		previousVariance_ = 0;
+		maxStep_ = std::clamp(focus_ + static_cast<uint32_t>((focus_ * kFineRange)),
+				      0U, kMaxFocusSteps);
+	}
+}
+
+void Af::afFineScan()
+{
+	if (!coarseCompleted_)
+		return;
+
+	if (afNeedIgnoreFrame())
+		return;
+
+	if (afScan(kFineSearchStep)) {
+		status_.stable = true;
+		fineCompleted_ = true;
+	}
+}
+
+bool Af::afScan(uint32_t minSteps)
+{
+	if (focus_ > maxStep_) {
+		/* If the max step is reached, move lens to the position. */
+		status_.focus = 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 ((currentVariance_ - status_.maxVariance) >=
+		    -(status_.maxVariance * 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;
+			status_.focus = focus_;
+			status_.maxVariance = currentVariance_;
+		} 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_.focus = bestFocus_;
+			return true;
+		}
+	}
+
+	previousVariance_ = currentVariance_;
+	LOG(IoBAf, Debug) << " Previous step is "
+			  << bestFocus_
+			  << " Current step is "
+			  << focus_;
+	return false;
+}
+
+void Af::afReset()
+{
+	if (afNeedIgnoreFrame())
+		return;
+
+	status_.maxVariance = 0;
+	status_.focus = 0;
+	focus_ = 0;
+	status_.stable = false;
+	ignoreCounter_ = kIgnoreFrame;
+	previousVariance_ = 0.0;
+	coarseCompleted_ = false;
+	fineCompleted_ = false;
+	maxStep_ = kMaxFocusSteps;
+}
+
+bool Af::afIsOutOfFocus()
+{
+	const uint32_t diff_var = std::abs(currentVariance_ -
+					   status_.maxVariance);
+	const double var_ratio = diff_var / status_.maxVariance;
+	LOG(IoBAf, Debug) << "Variance change rate: "
+			  << var_ratio
+			  << " Current VCM step: "
+			  << status_.focus;
+	if (var_ratio > kMaxChange)
+		return true;
+	else
+		return false;
+}
+
+void Af::Process(StatisticsPtr &stats, Metadata *image_metadata)
+{
+	unsigned int i;
+	image_metadata->Get("af.status", status_);
+
+	/* Use the second filter results only, and cache those. */
+	for (i = 0; i < FOCUS_REGIONS; i++)
+		status_.focus_measures[i] = stats->focus_stats[i].contrast_val[1][1]
+					  / stats->focus_stats[i].contrast_val_num[1][1];
+	status_.num = i;
+
+	currentVariance_ = estimateVariance();
+
+	if (!status_.stable) {
+		afCoarseScan();
+		afFineScan();
+	} else {
+		if (afIsOutOfFocus())
+			afReset();
+		else
+			ignoreCounter_ = kIgnoreFrame;
+	}
+}
+
+/* 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..45c9711f
--- /dev/null
+++ b/src/ipa/raspberrypi/controller/iob/af.h
@@ -0,0 +1,55 @@ 
+/* 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 "../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;
+private:
+	double estimateVariance();
+	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_;
+
+	/* The frames ignored before starting measuring. */
+	uint32_t ignoreCounter_;
+
+	/* Current AF statistic variance. */
+	double currentVariance_;
+	/* It is used to determine the derivative during scanning */
+	double previousVariance_;
+	/* 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_;
+};
+
+} /* 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',