@@ -19,6 +19,11 @@ namespace libcamera {
LOG_DECLARE_CATEGORY(Matrix)
+#ifndef __DOXYGEN__
+template<typename T>
+bool matrixInvert(Span<const T> dataIn, Span<T> dataOut, unsigned int dim);
+#endif /* __DOXYGEN__ */
+
template<typename T, unsigned int Rows, unsigned int Cols>
class Matrix
{
@@ -88,6 +93,17 @@ public:
return *this;
}
+ Matrix<T, Rows, Cols> inverse(bool *ok = nullptr) const
+ {
+ static_assert(Rows == Cols, "Matrix must be square");
+
+ Matrix<T, Rows, Cols> inverse;
+ bool res = matrixInvert(Span<const T>(data_), Span<T>(inverse.data_), Rows);
+ if (ok)
+ *ok = res;
+ return inverse;
+ }
+
private:
std::array<T, Rows * Cols> data_{};
};
@@ -7,6 +7,12 @@
#include "libcamera/internal/matrix.h"
+#include <algorithm>
+#include <assert.h>
+#include <cmath>
+#include <numeric>
+#include <vector>
+
#include <libcamera/base/log.h>
/**
@@ -87,6 +93,20 @@ LOG_DEFINE_CATEGORY(Matrix)
* \return Row \a i from the matrix, as a Span
*/
+/**
+ * \fn Matrix::inverse(bool *ok) const
+ * \param[out] ok Indicate if the matrix was successfully inverted
+ * \brief Compute the inverse of the matrix
+ *
+ * This function computes the inverse of the matrix. It is only implemented for
+ * matrices of float and double types. If \a ok is provided it will be set to a
+ * boolean value to indicate of the inversion was successful. This can be used
+ * to check if the matrix is singular, in which case the function will return
+ * an identity matrix.
+ *
+ * \return The inverse of the matrix
+ */
+
/**
* \fn Matrix::operator[](size_t i)
* \copydoc Matrix::operator[](size_t i) const
@@ -142,6 +162,146 @@ LOG_DEFINE_CATEGORY(Matrix)
*/
#ifndef __DOXYGEN__
+template<typename T>
+bool matrixInvert(Span<const T> dataIn, Span<T> dataOut, unsigned int dim)
+{
+ /*
+ * Convenience class to access matrix data, providing a row-major (i,j)
+ * element accessor through the call operator, and the ability to swap
+ * rows without modifying the backing storage.
+ */
+ class MatrixAccessor
+ {
+ public:
+ MatrixAccessor(Span<T> data, unsigned int rows, unsigned int cols)
+ : data_(data), swap_(rows), rows_(rows), cols_(cols)
+ {
+ std::iota(swap_.begin(), swap_.end(), T{ 0 });
+ }
+
+ T &operator()(unsigned int row, unsigned int col)
+ {
+ assert(row < rows_ && col < cols_);
+ return data_[index(row, col)];
+ }
+
+ void swap(unsigned int a, unsigned int b)
+ {
+ assert(a < rows_ && a < cols_);
+ std::swap(swap_[a], swap_[b]);
+ }
+
+ private:
+ unsigned int index(unsigned int row, unsigned int col) const
+ {
+ return swap_[row] * cols_ + col;
+ }
+
+ Span<T> data_;
+ std::vector<unsigned int> swap_;
+ unsigned int rows_;
+ unsigned int cols_;
+ };
+
+ /*
+ * Matrix inversion using Gaussian elimination.
+ *
+ * Start by augmenting the original matrix with an identiy matrix of
+ * the same size.
+ */
+ std::vector<T> data(dim * dim * 2);
+ MatrixAccessor matrix(data, dim, dim * 2);
+
+ for (unsigned int i = 0; i < dim; ++i) {
+ for (unsigned int j = 0; j < dim; ++j)
+ matrix(i, j) = dataIn[i * dim + j];
+ matrix(i, i + dim) = T{ 1 };
+ }
+
+ /* Start by triangularizing the input . */
+ for (unsigned int pivot = 0; pivot < dim; ++pivot) {
+ /*
+ * Locate the next pivot. To improve numerical stability, use
+ * the row with the largest value in the pivot's column.
+ */
+ unsigned int row;
+ T maxValue{ 0 };
+
+ for (unsigned int i = pivot; i < dim; ++i) {
+ T value = std::abs(matrix(i, pivot));
+ if (maxValue < value) {
+ maxValue = value;
+ row = i;
+ }
+ }
+
+ /*
+ * If no pivot is found in the column, the matrix is not
+ * invertible. Return an identity matrix.
+ */
+ if (maxValue == 0) {
+ std::fill(dataOut.begin(), dataOut.end(), T{ 0 });
+ for (unsigned int i = 0; i < dim; ++i)
+ dataOut[i * dim + i] = T{ 1 };
+ return false;
+ }
+
+ /* Swap rows to bring the pivot in the right location. */
+ matrix.swap(pivot, row);
+
+ /* Process all rows below the pivot to zero the pivot column. */
+ const T pivotValue = matrix(pivot, pivot);
+
+ for (unsigned int i = pivot + 1; i < dim; ++i) {
+ const T factor = matrix(i, pivot) / pivotValue;
+
+ /*
+ * We know the element in the pivot column will be 0,
+ * hardcode it instead of computing it.
+ */
+ matrix(i, pivot) = T{ 0 };
+
+ for (unsigned int j = pivot + 1; j < dim * 2; ++j)
+ matrix(i, j) -= matrix(pivot, j) * factor;
+ }
+ }
+
+ /*
+ * Then diagonalize the input, walking the diagonal backwards. There's
+ * no need to update the input matrix, as all the values we would write
+ * in the top-right triangle aren't used in further calculations (and
+ * would all by definition be zero).
+ */
+ for (unsigned int pivot = dim - 1; pivot > 0; --pivot) {
+ const T pivotValue = matrix(pivot, pivot);
+
+ for (unsigned int i = 0; i < pivot; ++i) {
+ const T factor = matrix(i, pivot) / pivotValue;
+
+ for (unsigned int j = dim; j < dim * 2; ++j)
+ matrix(i, j) -= matrix(pivot, j) * factor;
+ }
+ }
+
+ /*
+ * Finally, normalize the diagonal and store the result in the output
+ * data.
+ */
+ for (unsigned int i = 0; i < dim; ++i) {
+ const T factor = matrix(i, i);
+
+ for (unsigned int j = 0; j < dim; ++j)
+ dataOut[i * dim + j] = matrix(i, j + dim) / factor;
+ }
+
+ return true;
+}
+
+template bool matrixInvert<float>(Span<const float> dataIn, Span<float> dataOut,
+ unsigned int dim);
+template bool matrixInvert<double>(Span<const double> data, Span<double> dataOut,
+ unsigned int dim);
+
/*
* The YAML data shall be a list of numerical values. Its size shall be equal
* to the product of the number of rows and columns of the matrix (Rows x