[PATCH v4 2/3] ipa: libipa: Add MatrixInterpolator class

[Paul Elder]

Add a class to encapsulate the functionality of fetching a matrix based
on an integer key, and interpolating if there is no exact match. This is
expected to be used by both color correction matrices / crosstalk
correction as well as lens shading correction.

Signed-off-by: Paul Elder <paul.elder at ideasonboard.com>

---
Changes in v4:
- remove stray semicolons
- read from the new yaml layout (which mirrors what we have at the
  moment for lsc), and add keys to make it more flexible

Changes in v3:
- add a constructor that takes a map of unsigned int -> matrix
- s/unit/identity
- clear matrices on reset and when reading from yaml
- add assert at get()

Changes in v2:
- initialize to identity matrix
- add a function to reset to identity matrix
- other minor fixes
---
 src/ipa/libipa/matrix_interpolator.cpp |  54 +++++++++++
 src/ipa/libipa/matrix_interpolator.h   | 119 +++++++++++++++++++++++++
 src/ipa/libipa/meson.build             |   2 +
 3 files changed, 175 insertions(+)
 create mode 100644 src/ipa/libipa/matrix_interpolator.cpp
 create mode 100644 src/ipa/libipa/matrix_interpolator.h

diff --git a/src/ipa/libipa/matrix_interpolator.cpp b/src/ipa/libipa/matrix_interpolator.cpp
new file mode 100644
index 000000000..fc1837e75
--- /dev/null
+++ b/src/ipa/libipa/matrix_interpolator.cpp
@@ -0,0 +1,54 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2019, Raspberry Pi Ltd
+ * Copyright (C) 2024, Paul Elder <paul.elder at ideasonboard.com>
+ *
+ * matrix_interpolator.cpp - Helper class for interpolating maps of matrices
+ */
+#include "matrix_interpolator.h"
+
+#include <algorithm>
+#include <string>
+
+#include <libcamera/base/log.h>
+
+#include "libcamera/internal/yaml_parser.h"
+
+#include "matrix.h"
+
+/**
+ * \file ccm.h
+ * \brief Helper class for interpolating maps of matrices
+ */
+
+namespace libcamera {
+
+LOG_DEFINE_CATEGORY(MatrixInterpolator)
+
+namespace ipa {
+
+/**
+ * \class MatrixInterpolator
+ * \brief Class for storing, retrieving, and interpolating matrices
+ */
+
+/**
+ * \fn int MatrixInterpolator<T, R, C>::readYaml(const libcamera::YamlObject &yaml)
+ * \brief Initialize an MatrixInterpolator instance from yaml
+ * \tparam T Type of data stored in the matrices
+ * \tparam R Number of rows of the matrices
+ * \tparam C Number of columns of the matrices
+ * \param[in] yaml The yaml object that contains the map of unsigned integers to matrices
+ * \return Zero on success, negative error code otherwise
+ */
+
+/**
+ * \fn Matrix<T, R, C> MatrixInterpolator<T, R, C>::get(unsigned int key)
+ * \brief Retrieve a matrix from the list of matrices, interpolating if necessary
+ * \param[in] key The unsigned integer key of the matrix to retrieve
+ * \return The matrix corresponding to the color temperature
+ */
+
+} /* namespace ipa */
+
+} /* namespace libcamera */
diff --git a/src/ipa/libipa/matrix_interpolator.h b/src/ipa/libipa/matrix_interpolator.h
new file mode 100644
index 000000000..fb017889d
--- /dev/null
+++ b/src/ipa/libipa/matrix_interpolator.h
@@ -0,0 +1,119 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2019, Raspberry Pi Ltd
+ * Copyright (C) 2024, Paul Elder <paul.elder at ideasonboard.com>
+ *
+ * matrix_interpolator.h - Helper class for interpolating maps of matrices
+ */
+
+#pragma once
+
+#include <algorithm>
+#include <map>
+#include <string>
+#include <tuple>
+
+#include <libcamera/base/log.h>
+
+#include "libcamera/internal/yaml_parser.h"
+
+#include "matrix.h"
+
+namespace libcamera {
+
+LOG_DECLARE_CATEGORY(MatrixInterpolator)
+
+namespace ipa {
+
+template<typename T, unsigned int R, unsigned int C,
+	 std::enable_if_t<std::is_arithmetic_v<T>> * = nullptr>
+class MatrixInterpolator
+{
+public:
+	MatrixInterpolator()
+	{
+		reset();
+	}
+
+	MatrixInterpolator(const std::map<unsigned int, Matrix<T, R, C>> &matrices)
+	{
+		for (const auto &pair : matrices)
+			matrices_[pair.first] = pair.second;
+	}
+
+	~MatrixInterpolator() {}
+
+	void reset()
+	{
+		Matrix<T, R, C> identity;
+		matrices_.clear();
+		matrices_[0] = identity;
+	}
+
+	int readYaml(const libcamera::YamlObject &yaml,
+		     const std::string &key_name,
+		     const std::string &matrix_name)
+	{
+		int ret;
+
+		matrices_.clear();
+
+		if (!yaml.isList()) {
+			LOG(MatrixInterpolator, Error) << "yaml object must be a list";
+			return -EINVAL;
+		}
+
+		for (const auto &value : yaml.asList()) {
+			unsigned int ct = std::stoul(value[key_name].get<std::string>(""));
+			Matrix<T, R, C> matrix;
+			if ((ret = matrix.readYaml(value[matrix_name])) < 0) {
+				LOG(MatrixInterpolator, Error) << "Failed to read matrix";
+				return ret;
+			}
+
+			matrices_[ct] = matrix;
+
+			LOG(MatrixInterpolator, Debug)
+				<< "Read matrix '" << matrix_name << "' for key '"
+				<< key_name << "' " << ct << ": "
+				<< matrices_[ct].toString();
+		}
+
+		if (matrices_.size() < 1) {
+			LOG(MatrixInterpolator, Error) << "Need at least one matrix";
+			return -EINVAL;
+		}
+
+		return 0;
+	}
+
+	Matrix<T, R, C> get(unsigned int ct)
+	{
+		ASSERT(matrices_.size() > 0);
+
+		if (matrices_.size() == 1 ||
+		    ct <= matrices_.begin()->first)
+			return matrices_.begin()->second;
+
+		if (ct >= matrices_.rbegin()->first)
+			return matrices_.rbegin()->second;
+
+		if (matrices_.count(ct))
+			return matrices_[ct];
+
+		/* The above four guarantee that this will succeed */
+		auto iter = matrices_.upper_bound(ct);
+		unsigned int ctUpper = iter->first;
+		unsigned int ctLower = (--iter)->first;
+
+		double lambda = (ct - ctLower) / static_cast<double>(ctUpper - ctLower);
+		return lambda * matrices_[ctUpper] + (1.0 - lambda) * matrices_[ctLower];
+	}
+
+private:
+	std::map<unsigned int, Matrix<T, R, C>> matrices_;
+};
+
+} /* namespace ipa */
+
+} /* namespace libcamera */
diff --git a/src/ipa/libipa/meson.build b/src/ipa/libipa/meson.build
index 1e34355fe..305acf641 100644
--- a/src/ipa/libipa/meson.build
+++ b/src/ipa/libipa/meson.build
@@ -8,6 +8,7 @@ libipa_headers = files([
     'fc_queue.h',
     'histogram.h',
     'matrix.h',
+    'matrix_interpolator.h',
     'module.h',
     'pwl.h',
 ])
@@ -20,6 +21,7 @@ libipa_sources = files([
     'fc_queue.cpp',
     'histogram.cpp',
     'matrix.cpp',
+    'matrix_interpolator.cpp',
     'module.cpp',
     'pwl.cpp'
 ])
-- 
2.39.2