summaryrefslogtreecommitdiff
path: root/src/ipa/libipa/matrix_interpolator.h
blob: afbce5386b06b9f83b87dec5ebd3af430af97b76 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
/* SPDX-License-Identifier: LGPL-2.1-or-later */
/*
 * Copyright (C) 2024, Paul Elder <paul.elder@ideasonboard.com>
 *
 * Helper class for interpolating maps of matrices
 */

#pragma once

#include <map>
#include <string>

#include <libcamera/base/log.h>

#include "libcamera/internal/yaml_parser.h"

#include "matrix.h"

namespace libcamera {

LOG_DECLARE_CATEGORY(MatrixInterpolator)

namespace ipa {

#ifndef __DOXYGEN__
template<typename T, unsigned int R, unsigned int C,
	 std::enable_if_t<std::is_arithmetic_v<T>> * = nullptr>
#else
template<typename T, unsigned int R, unsigned int C>
#endif /* __DOXYGEN__ */
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()
	{
		matrices_.clear();
		matrices_[0] = Matrix<T, R, C>::identity();
	}

	int readYaml(const libcamera::YamlObject &yaml,
		     const std::string &key_name,
		     const std::string &matrix_name)
	{
		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>(""));
			std::optional<Matrix<T, R, C>> matrix =
				value[matrix_name].get<Matrix<T, R, C>>();
			if (!matrix) {
				LOG(MatrixInterpolator, Error) << "Failed to read matrix";
				return -EINVAL;
			}

			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_.find(ct) != matrices_.end())
			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);
		Matrix<T, R, C> ret =
			lambda * matrices_[ctUpper] + (1.0 - lambda) * matrices_[ctLower];
		return ret;
	}

private:
	std::map<unsigned int, Matrix<T, R, C>> matrices_;
};

} /* namespace ipa */

} /* namespace libcamera */