[v1,2/8] ipa: rpi: controller: Improve findPeak() function in AF algorithm
diff mbox series

Message ID 20250620124452.557855-3-naush@raspberrypi.com
State Accepted
Headers show
Series
  • Raspberry Pi: AF improvements
Related show

Commit Message

Naushir Patuck June 20, 2025, 12:42 p.m. UTC 
From: Nick Hollinghurst <nick.hollinghurst@raspberrypi.com>

Improve quadratic peak fitting in findPeak(). The old approximation
was good but only valid when points were equally spaced and the
MAX was not at one end of the series.

Signed-off-by: Nick Hollinghurst <nick.hollinghurst@raspberrypi.com>
Signed-off-by: Naushir Patuck <naush@raspberrypi.com>
Reviewed-by: Naushir Patuck <naush@raspberrypi.com>
---
 src/ipa/rpi/controller/rpi/af.cpp | 31 ++++++++++++++++++++++---------
 1 file changed, 22 insertions(+), 9 deletions(-)

Patch
diff mbox series 

diff --git a/src/ipa/rpi/controller/rpi/af.cpp b/src/ipa/rpi/controller/rpi/af.cpp
index 041cb51db277..8df614ed7b6b 100644
--- a/src/ipa/rpi/controller/rpi/af.cpp
+++ b/src/ipa/rpi/controller/rpi/af.cpp
@@ -436,15 +436,28 @@  double Af::findPeak(unsigned i) const
 {
 	double f = scanData_[i].focus;
 
-	if (i > 0 && i + 1 < scanData_.size()) {
-		double dropLo = scanData_[i].contrast - scanData_[i - 1].contrast;
-		double dropHi = scanData_[i].contrast - scanData_[i + 1].contrast;
-		if (0.0 <= dropLo && dropLo < dropHi) {
-			double param = 0.3125 * (1.0 - dropLo / dropHi) * (1.6 - dropLo / dropHi);
-			f += param * (scanData_[i - 1].focus - f);
-		} else if (0.0 <= dropHi && dropHi < dropLo) {
-			double param = 0.3125 * (1.0 - dropHi / dropLo) * (1.6 - dropHi / dropLo);
-			f += param * (scanData_[i + 1].focus - f);
+	if (scanData_.size() >= 3) {
+		/*
+		 * Given the sample with the highest contrast score and its two
+		 * neighbours either side (or same side if at the end of a scan),
+		 * solve for the best lens position by fitting a parabola.
+		 * Adapted from awb.cpp: interpolateQaudaratic()
+		 */
+
+		if (i == 0)
+			i++;
+		else if (i + 1 >= scanData_.size())
+			i--;
+
+		double abx = scanData_[i - 1].focus - scanData_[i].focus;
+		double aby = scanData_[i - 1].contrast - scanData_[i].contrast;
+		double cbx = scanData_[i + 1].focus - scanData_[i].focus;
+		double cby = scanData_[i + 1].contrast - scanData_[i].contrast;
+		double denom = 2.0 * (aby * cbx - cby * abx);
+		if (std::abs(denom) >= (1.0 / 64.0) && denom * abx > 0.0) {
+			f = (aby * cbx * cbx - cby * abx * abx) / denom;
+			f = std::clamp(f, std::min(abx, cbx), std::max(abx, cbx));
+			f += scanData_[i].focus;
 		}
 	}