From patchwork Wed Mar 19 16:11:10 2025 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: Stefan Klug X-Patchwork-Id: 22983 Return-Path: X-Original-To: parsemail@patchwork.libcamera.org Delivered-To: parsemail@patchwork.libcamera.org Received: from lancelot.ideasonboard.com (lancelot.ideasonboard.com [92.243.16.209]) by patchwork.libcamera.org (Postfix) with ESMTPS id 018E2C32FE for ; Wed, 19 Mar 2025 16:12:13 +0000 (UTC) Received: from lancelot.ideasonboard.com (localhost [IPv6:::1]) by lancelot.ideasonboard.com (Postfix) with ESMTP id 8A92B6895B; Wed, 19 Mar 2025 17:12:13 +0100 (CET) Authentication-Results: lancelot.ideasonboard.com; dkim=pass (1024-bit key; unprotected) header.d=ideasonboard.com header.i=@ideasonboard.com header.b="uvH2Li6K"; dkim-atps=neutral Received: from perceval.ideasonboard.com (perceval.ideasonboard.com [213.167.242.64]) by lancelot.ideasonboard.com (Postfix) with ESMTPS id DE4D268950 for ; Wed, 19 Mar 2025 17:12:11 +0100 (CET) Received: from ideasonboard.com (unknown [IPv6:2a00:6020:448c:6c00:760:e5ca:4814:99c7]) by perceval.ideasonboard.com (Postfix) with ESMTPSA id EC4588FA; Wed, 19 Mar 2025 17:10:28 +0100 (CET) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/simple; d=ideasonboard.com; s=mail; t=1742400629; bh=Gfx7x+E8Sx24+3cf5pWOkHZEsFcd/OJpxqWgEUGDWOQ=; h=From:To:Cc:Subject:Date:In-Reply-To:References:From; b=uvH2Li6KYtr+IhpDRImz0Kft4waOW8/iusEVnphGvEtIowZy6A1JOygQq54tJUmAg ZwtZMnjU95P3xEzKvjyBlx13xwWd0FdPgifZNbta1p9Pt0v8vB9UENBJPENtXIm1SQ GiDyuQO0sKUPPsPjTUId5nyCzeeazWIQkJFBHNRA= From: Stefan Klug To: libcamera-devel@lists.libcamera.org Cc: Stefan Klug , Laurent Pinchart Subject: [PATCH v2 05/17] libcamera: matrix: Add inverse() function Date: Wed, 19 Mar 2025 17:11:10 +0100 Message-ID: <20250319161152.63625-6-stefan.klug@ideasonboard.com> X-Mailer: git-send-email 2.43.0 In-Reply-To: <20250319161152.63625-1-stefan.klug@ideasonboard.com> References: <20250319161152.63625-1-stefan.klug@ideasonboard.com> MIME-Version: 1.0 X-BeenThere: libcamera-devel@lists.libcamera.org X-Mailman-Version: 2.1.29 Precedence: list List-Id: List-Unsubscribe: , List-Archive: List-Post: List-Help: List-Subscribe: , Errors-To: libcamera-devel-bounces@lists.libcamera.org Sender: "libcamera-devel" For calculations in upcoming algorithm patches, the inverse of a matrix is required. Add an implementation of the inverse() function for square matrices. Signed-off-by: Stefan Klug Signed-off-by: Laurent Pinchart --- Changes in v2: - Replaced the implementation by a generic one provided by Laurent that supports arbitrary square matrices instead of 2x2 and 3x3 only. - Moved the implementation into the cpp file. --- include/libcamera/internal/matrix.h | 16 +++ src/libcamera/matrix.cpp | 160 ++++++++++++++++++++++++++++ 2 files changed, 176 insertions(+) diff --git a/include/libcamera/internal/matrix.h b/include/libcamera/internal/matrix.h index 9b80521e3cb0..6e3c190286fe 100644 --- a/include/libcamera/internal/matrix.h +++ b/include/libcamera/internal/matrix.h @@ -19,6 +19,11 @@ namespace libcamera { LOG_DECLARE_CATEGORY(Matrix) +#ifndef __DOXYGEN__ +template +bool matrixInvert(Span dataIn, Span dataOut, unsigned int dim); +#endif /* __DOXYGEN__ */ + template class Matrix { @@ -88,6 +93,17 @@ public: return *this; } + Matrix inverse(bool *ok = nullptr) const + { + static_assert(Rows == Cols, "Matrix must be square"); + + Matrix inverse; + bool res = matrixInvert(Span(data_), Span(inverse.data_), Rows); + if (ok) + *ok = res; + return inverse; + } + private: std::array data_{}; }; diff --git a/src/libcamera/matrix.cpp b/src/libcamera/matrix.cpp index 6dca7498cab3..8590f8efeff3 100644 --- a/src/libcamera/matrix.cpp +++ b/src/libcamera/matrix.cpp @@ -7,6 +7,12 @@ #include "libcamera/internal/matrix.h" +#include +#include +#include +#include +#include + #include /** @@ -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 +bool matrixInvert(Span dataIn, Span 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 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 data_; + std::vector 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 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(Span dataIn, Span dataOut, + unsigned int dim); +template bool matrixInvert(Span data, Span 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