| Message ID | 20250319161152.63625-7-stefan.klug@ideasonboard.com |
|---|---|
| State | Superseded |
| Headers | show |
| Series |
|
| Related | show |
Hi 2025. 03. 19. 17:11 keltezéssel, Stefan Klug írta: > 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 <stefan.klug@ideasonboard.com> > Signed-off-by: Laurent Pinchart <laurent.pinchart@ideasonboard.com> > > --- > > 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<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 Returning `std::optional<...>`? Or `std::pair<Matrix<...>, bool>` if the returned matrix is in any way useful? > + { > + 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_{}; > }; > 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 <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) Sorry, but I think this is a bit of an overkill: (1) it is (most likely) slower than hard-coding the inversion of a 3x3 matrix (where it would be used); (2) it adds dynamic memory allocations. Or am I missing something? I suppose (2) could be addressed by providing a "scratch buffer" as well, but I think (1) still stands. Regards, Barnabás Pőcze > +{ > + /* > + * 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
I'm not sure what happens, this is a review of 05/17 but it cames as a reply to 06/17. On Wed, Mar 19, 2025 at 06:12:21PM +0100, Barnabás Pőcze wrote: > 2025. 03. 19. 17:11 keltezéssel, Stefan Klug írta: > > 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 <stefan.klug@ideasonboard.com> > > Signed-off-by: Laurent Pinchart <laurent.pinchart@ideasonboard.com> > > > > --- > > > > 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<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 > > Returning `std::optional<...>`? Or `std::pair<Matrix<...>, bool>` if the > returned matrix is in any way useful? > > > > + { > > + 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_{}; > > }; > > 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 <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) > > Sorry, but I think this is a bit of an overkill: > > (1) it is (most likely) slower than hard-coding the inversion of a 3x3 matrix > (where it would be used); Is this a real concern, given our usage patterns ? If so we could use template specialization for 3x3 matrices. > (2) it adds dynamic memory allocations. > > Or am I missing something? I suppose (2) could be addressed by providing a > "scratch buffer" as well, but I think (1) still stands. > > > +{ > > + /* > > + * 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
Hi 2025. 03. 31. 15:15 keltezéssel, Laurent Pinchart írta: > I'm not sure what happens, this is a review of 05/17 but it cames as a > reply to 06/17. Oops, you're right. I'm not sure what happened. > > On Wed, Mar 19, 2025 at 06:12:21PM +0100, Barnabás Pőcze wrote: >> 2025. 03. 19. 17:11 keltezéssel, Stefan Klug írta: >>> 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 <stefan.klug@ideasonboard.com> >>> Signed-off-by: Laurent Pinchart <laurent.pinchart@ideasonboard.com> >>> >>> --- >>> >>> 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<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 >> >> Returning `std::optional<...>`? Or `std::pair<Matrix<...>, bool>` if the >> returned matrix is in any way useful? >> >> >>> + { >>> + 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_{}; >>> }; >>> 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 <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) >> >> Sorry, but I think this is a bit of an overkill: >> >> (1) it is (most likely) slower than hard-coding the inversion of a 3x3 matrix >> (where it would be used); > > Is this a real concern, given our usage patterns ? If so we could use > template specialization for 3x3 matrices. The only user I can see is `libcamera::ipa::rkisp1::algorithms::Awb::calculateRgbMeans()`, and as far as I can tell that runs on every frame. So I don't think this is ideal, and while of course this in and of itself is not a big issue, I think many small things add up over time. Regards, Barnabás Pőcze > >> (2) it adds dynamic memory allocations. >> >> Or am I missing something? I suppose (2) could be addressed by providing a >> "scratch buffer" as well, but I think (1) still stands. >> >>> +{ >>> + /* >>> + * 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 >
On Mon, Mar 31, 2025 at 03:52:07PM +0200, Barnabás Pőcze wrote: > 2025. 03. 31. 15:15 keltezéssel, Laurent Pinchart írta: > > I'm not sure what happens, this is a review of 05/17 but it cames as a > > reply to 06/17. > > Oops, you're right. I'm not sure what happened. > > > On Wed, Mar 19, 2025 at 06:12:21PM +0100, Barnabás Pőcze wrote: > >> 2025. 03. 19. 17:11 keltezéssel, Stefan Klug írta: > >>> 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 <stefan.klug@ideasonboard.com> > >>> Signed-off-by: Laurent Pinchart <laurent.pinchart@ideasonboard.com> > >>> > >>> --- > >>> > >>> 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<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 > >> > >> Returning `std::optional<...>`? Or `std::pair<Matrix<...>, bool>` if the > >> returned matrix is in any way useful? > >> > >> > >>> + { > >>> + 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_{}; > >>> }; > >>> 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 <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) > >> > >> Sorry, but I think this is a bit of an overkill: > >> > >> (1) it is (most likely) slower than hard-coding the inversion of a 3x3 matrix > >> (where it would be used); > > > > Is this a real concern, given our usage patterns ? If so we could use > > template specialization for 3x3 matrices. > > The only user I can see is `libcamera::ipa::rkisp1::algorithms::Awb::calculateRgbMeans()`, > and as far as I can tell that runs on every frame. So I don't think this is ideal, and while > of course this in and of itself is not a big issue, I think many small things add up over time. We can easily add an optimized template specialization, right ? I'd be curious to know how big the improvement is. Small things do that up, but if none of them are significant by themselves, it means we'll have to optimize them all. Eventually, we should consider switching to an off-the-shelf linear algebra library. > >> (2) it adds dynamic memory allocations. > >> > >> Or am I missing something? I suppose (2) could be addressed by providing a > >> "scratch buffer" as well, but I think (1) still stands. I forgot to reply to this. We could easily allocate the memory on the stack, yes. > >>> +{ > >>> + /* > >>> + * 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
Quoting Stefan Klug (2025-03-19 16:11:11) > Add a few tests for the Matrix class. This is not full fledged but at > least a starter. > > Signed-off-by: Stefan Klug <stefan.klug@ideasonboard.com> > > --- > > Changes in v2: > - Added this patch > --- > test/matrix.cpp | 53 ++++++++++++++++++++++++++++++++++++++++++++++++ > test/meson.build | 1 + > 2 files changed, 54 insertions(+) > create mode 100644 test/matrix.cpp > > diff --git a/test/matrix.cpp b/test/matrix.cpp > new file mode 100644 > index 000000000000..93a3b95db2dc > --- /dev/null > +++ b/test/matrix.cpp > @@ -0,0 +1,53 @@ > +/* SPDX-License-Identifier: GPL-2.0-or-later */ > +/* > + * Copyright (C) 2024, Ideas on Board Oy > + * > + * Vector tests > + */ > + > +#include "libcamera/internal/matrix.h" > + > +#include <cmath> > +#include <iostream> > + > +#include "test.h" > + > +using namespace libcamera; > + > +#define ASSERT_EQ(a, b) \ > + if ((a) != (b)) { \ > + std::cout << #a " != " #b << " (line " << __LINE__ << ")" \ > + << std::endl; \ > + return TestFail; \ > + } > + > +class VectorTest : public Test > +{ > +protected: > + int run() > + { > + Matrix<double, 3, 3> m1; > + > + ASSERT_EQ(m1[0][0], 0.0); > + ASSERT_EQ(m1[0][1], 0.0); > + > + constexpr Matrix<float, 2, 2> m2 = Matrix<float, 2, 2>().identity(); > + ASSERT_EQ(m2[0][0], 1.0); > + ASSERT_EQ(m2[0][1], 0.0); > + ASSERT_EQ(m2[1][0], 0.0); > + ASSERT_EQ(m2[1][1], 1.0); > + > + Matrix<float, 2, 2> m3{ { 2.0, 0.0, 0.0, 2.0 } }; > + Matrix<float, 2, 2> m4 = m3.inverse(); > + > + Matrix<float, 2, 2> m5 = m3 * m4; > + ASSERT_EQ(m5[0][0], 1.0); > + ASSERT_EQ(m5[0][1], 0.0); > + ASSERT_EQ(m5[1][0], 0.0); > + ASSERT_EQ(m5[1][1], 1.0); > + > + return TestPass; > + } > +}; > + > +TEST_REGISTER(VectorTest) Not 'MatrixTest' ? Aside from that, I'd put a tag, as tests here are helpful ... I'll leave the implementation detail on how to implement the Matrix features to Barnabas+Laurent to continue to discuss.... > diff --git a/test/meson.build b/test/meson.build > index 4095664994fd..52f04364e4fc 100644 > --- a/test/meson.build > +++ b/test/meson.build > @@ -60,6 +60,7 @@ internal_tests = [ > {'name': 'file', 'sources': ['file.cpp']}, > {'name': 'flags', 'sources': ['flags.cpp']}, > {'name': 'hotplug-cameras', 'sources': ['hotplug-cameras.cpp']}, > + {'name': 'matrix', 'sources': ['matrix.cpp']}, > {'name': 'message', 'sources': ['message.cpp']}, > {'name': 'object', 'sources': ['object.cpp']}, > {'name': 'object-delete', 'sources': ['object-delete.cpp']}, > -- > 2.43.0 >
On Mon, Mar 31, 2025 at 06:07:24PM +0100, Kieran Bingham wrote: > Quoting Stefan Klug (2025-03-19 16:11:11) > > Add a few tests for the Matrix class. This is not full fledged but at > > least a starter. > > > > Signed-off-by: Stefan Klug <stefan.klug@ideasonboard.com> > > > > --- > > > > Changes in v2: > > - Added this patch > > --- > > test/matrix.cpp | 53 ++++++++++++++++++++++++++++++++++++++++++++++++ > > test/meson.build | 1 + > > 2 files changed, 54 insertions(+) > > create mode 100644 test/matrix.cpp > > > > diff --git a/test/matrix.cpp b/test/matrix.cpp > > new file mode 100644 > > index 000000000000..93a3b95db2dc > > --- /dev/null > > +++ b/test/matrix.cpp > > @@ -0,0 +1,53 @@ > > +/* SPDX-License-Identifier: GPL-2.0-or-later */ > > +/* > > + * Copyright (C) 2024, Ideas on Board Oy > > + * > > + * Vector tests > > + */ > > + > > +#include "libcamera/internal/matrix.h" > > + > > +#include <cmath> > > +#include <iostream> > > + > > +#include "test.h" > > + > > +using namespace libcamera; > > + > > +#define ASSERT_EQ(a, b) \ > > + if ((a) != (b)) { \ > > + std::cout << #a " != " #b << " (line " << __LINE__ << ")" \ > > + << std::endl; \ > > + return TestFail; \ > > + } > > + > > +class VectorTest : public Test > > +{ > > +protected: > > + int run() > > + { > > + Matrix<double, 3, 3> m1; > > + > > + ASSERT_EQ(m1[0][0], 0.0); > > + ASSERT_EQ(m1[0][1], 0.0); > > + > > + constexpr Matrix<float, 2, 2> m2 = Matrix<float, 2, 2>().identity(); > > + ASSERT_EQ(m2[0][0], 1.0); > > + ASSERT_EQ(m2[0][1], 0.0); > > + ASSERT_EQ(m2[1][0], 0.0); > > + ASSERT_EQ(m2[1][1], 1.0); > > + > > + Matrix<float, 2, 2> m3{ { 2.0, 0.0, 0.0, 2.0 } }; > > + Matrix<float, 2, 2> m4 = m3.inverse(); > > + > > + Matrix<float, 2, 2> m5 = m3 * m4; > > + ASSERT_EQ(m5[0][0], 1.0); > > + ASSERT_EQ(m5[0][1], 0.0); > > + ASSERT_EQ(m5[1][0], 0.0); > > + ASSERT_EQ(m5[1][1], 1.0); > > + > > + return TestPass; > > + } > > +}; > > + > > +TEST_REGISTER(VectorTest) > > Not 'MatrixTest' ? > > Aside from that, I'd put a tag, as tests here are helpful ... I'll leave > the implementation detail on how to implement the Matrix features to > Barnabas+Laurent to continue to discuss.... With the test class renamed, Reviewed-by: Laurent Pinchart <laurent.pinchart@ideasonboard.com> > > diff --git a/test/meson.build b/test/meson.build > > index 4095664994fd..52f04364e4fc 100644 > > --- a/test/meson.build > > +++ b/test/meson.build > > @@ -60,6 +60,7 @@ internal_tests = [ > > {'name': 'file', 'sources': ['file.cpp']}, > > {'name': 'flags', 'sources': ['flags.cpp']}, > > {'name': 'hotplug-cameras', 'sources': ['hotplug-cameras.cpp']}, > > + {'name': 'matrix', 'sources': ['matrix.cpp']}, > > {'name': 'message', 'sources': ['message.cpp']}, > > {'name': 'object', 'sources': ['object.cpp']}, > > {'name': 'object-delete', 'sources': ['object-delete.cpp']},
diff --git a/test/matrix.cpp b/test/matrix.cpp new file mode 100644 index 000000000000..93a3b95db2dc --- /dev/null +++ b/test/matrix.cpp @@ -0,0 +1,53 @@ +/* SPDX-License-Identifier: GPL-2.0-or-later */ +/* + * Copyright (C) 2024, Ideas on Board Oy + * + * Vector tests + */ + +#include "libcamera/internal/matrix.h" + +#include <cmath> +#include <iostream> + +#include "test.h" + +using namespace libcamera; + +#define ASSERT_EQ(a, b) \ + if ((a) != (b)) { \ + std::cout << #a " != " #b << " (line " << __LINE__ << ")" \ + << std::endl; \ + return TestFail; \ + } + +class VectorTest : public Test +{ +protected: + int run() + { + Matrix<double, 3, 3> m1; + + ASSERT_EQ(m1[0][0], 0.0); + ASSERT_EQ(m1[0][1], 0.0); + + constexpr Matrix<float, 2, 2> m2 = Matrix<float, 2, 2>().identity(); + ASSERT_EQ(m2[0][0], 1.0); + ASSERT_EQ(m2[0][1], 0.0); + ASSERT_EQ(m2[1][0], 0.0); + ASSERT_EQ(m2[1][1], 1.0); + + Matrix<float, 2, 2> m3{ { 2.0, 0.0, 0.0, 2.0 } }; + Matrix<float, 2, 2> m4 = m3.inverse(); + + Matrix<float, 2, 2> m5 = m3 * m4; + ASSERT_EQ(m5[0][0], 1.0); + ASSERT_EQ(m5[0][1], 0.0); + ASSERT_EQ(m5[1][0], 0.0); + ASSERT_EQ(m5[1][1], 1.0); + + return TestPass; + } +}; + +TEST_REGISTER(VectorTest) diff --git a/test/meson.build b/test/meson.build index 4095664994fd..52f04364e4fc 100644 --- a/test/meson.build +++ b/test/meson.build @@ -60,6 +60,7 @@ internal_tests = [ {'name': 'file', 'sources': ['file.cpp']}, {'name': 'flags', 'sources': ['flags.cpp']}, {'name': 'hotplug-cameras', 'sources': ['hotplug-cameras.cpp']}, + {'name': 'matrix', 'sources': ['matrix.cpp']}, {'name': 'message', 'sources': ['message.cpp']}, {'name': 'object', 'sources': ['object.cpp']}, {'name': 'object-delete', 'sources': ['object-delete.cpp']},
Add a few tests for the Matrix class. This is not full fledged but at least a starter. Signed-off-by: Stefan Klug <stefan.klug@ideasonboard.com> --- Changes in v2: - Added this patch --- test/matrix.cpp | 53 ++++++++++++++++++++++++++++++++++++++++++++++++ test/meson.build | 1 + 2 files changed, 54 insertions(+) create mode 100644 test/matrix.cpp