[v3,05/16] libcamera: matrix: Add inverse() function

23121 diff mbox series

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.

Changes inv3:
- Added stack allocated scratchBuffers to matrixInvert so that no heap
  allocations are necessary.
---
 include/libcamera/internal/matrix.h |  23 ++++
 src/libcamera/matrix.cpp            | 166 ++++++++++++++++++++++++++++
 2 files changed, 189 insertions(+)

Quoting Stefan Klug (2025-04-03 16:49:10)
> 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>
> 

Given there's a test structure added after this...


Reviewed-by: Kieran Bingham <kieran.bingham@ideasonboard.com>

Maybe that should be just acked by as I'm not spending a lot of time to
review the Gaussian Ellimination here - but I trust you and
Laurent and the results of some tests being added to the unit tests for
it so I'm fine with it ;-)

> ---
> 
> 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.
> 
> Changes inv3:
> - Added stack allocated scratchBuffers to matrixInvert so that no heap
>   allocations are necessary.
> ---
>  include/libcamera/internal/matrix.h |  23 ++++
>  src/libcamera/matrix.cpp            | 166 ++++++++++++++++++++++++++++
>  2 files changed, 189 insertions(+)
> 
> diff --git a/include/libcamera/internal/matrix.h b/include/libcamera/internal/matrix.h
> index 6d40567af0a0..a07a47701336 100644
> --- a/include/libcamera/internal/matrix.h
> +++ b/include/libcamera/internal/matrix.h
> @@ -19,6 +19,12 @@ namespace libcamera {
>  
>  LOG_DECLARE_CATEGORY(Matrix)
>  
> +#ifndef __DOXYGEN__
> +template<typename T>
> +bool matrixInvert(Span<const T> dataIn, Span<T> dataOut, unsigned int dim,
> +                 Span<T> scratchBuffer, Span<unsigned int> swapBuffer);
> +#endif /* __DOXYGEN__ */
> +
>  template<typename T, unsigned int Rows, unsigned int Cols>
>  class Matrix
>  {
> @@ -91,6 +97,23 @@ 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;
> +               std::array<T, Rows * Cols * 2> scratchBuffer;
> +               std::array<unsigned int, Rows> swapBuffer;
> +               bool res = matrixInvert(Span<const T>(data_),
> +                                       Span<T>(inverse.data_),
> +                                       Rows,
> +                                       Span<T>(scratchBuffer),
> +                                       Span<unsigned int>(swapBuffer));
> +               if (ok)
> +                       *ok = res;
> +               return inverse;
> +       }
> +
>  private:
>         /*
>          * \todo The initializer is only necessary for the constructor to be
> diff --git a/src/libcamera/matrix.cpp b/src/libcamera/matrix.cpp
> index 49e2aa3b4f2c..68fc1b7bd5ac 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,152 @@ LOG_DEFINE_CATEGORY(Matrix)
>   */
>  
>  #ifndef __DOXYGEN__
> +template<typename T>
> +bool matrixInvert(Span<const T> dataIn, Span<T> dataOut, unsigned int dim,
> +                 Span<T> scratchBuffer, Span<unsigned int> swapBuffer)
> +{
> +       /*
> +        * 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, Span<unsigned int> swapBuffer, unsigned int rows, unsigned int cols)
> +                       : data_(data), swap_(swapBuffer), rows_(rows), cols_(cols)
> +               {
> +                       ASSERT(swap_.size() == rows);
> +                       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_;
> +               Span<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.
> +        */
> +       ASSERT(scratchBuffer.size() == dim * dim * 2);
> +       MatrixAccessor matrix(scratchBuffer, swapBuffer, 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, j + dim) = T{ 0 };
> +               }
> +               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, Span<float> scratchBuffer,
> +                                 Span<unsigned int> swapBuffer);
> +template bool matrixInvert<double>(Span<const double> data, Span<double> dataOut,
> +                                  unsigned int dim, Span<double> scratchBuffer,
> +                                  Span<unsigned int> swapBuffer);
> +
>  /*
>   * 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
> -- 
> 2.43.0
>

On Fri, May 02, 2025 at 08:49:42AM +0100, Kieran Bingham wrote:
> Quoting Stefan Klug (2025-04-03 16:49:10)
> > 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>
> > 
> 
> Given there's a test structure added after this...
> 
> 
> Reviewed-by: Kieran Bingham <kieran.bingham@ideasonboard.com>
> 
> Maybe that should be just acked by as I'm not spending a lot of time to
> review the Gaussian Ellimination here - but I trust you and
> Laurent and the results of some tests being added to the unit tests for
> it so I'm fine with it ;-)

I went through and reviewed the Gaussian Elimination...

Reviewed-by: Paul Elder <paul.elder@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.
> > 
> > Changes inv3:
> > - Added stack allocated scratchBuffers to matrixInvert so that no heap
> >   allocations are necessary.
> > ---
> >  include/libcamera/internal/matrix.h |  23 ++++
> >  src/libcamera/matrix.cpp            | 166 ++++++++++++++++++++++++++++
> >  2 files changed, 189 insertions(+)
> > 
> > diff --git a/include/libcamera/internal/matrix.h b/include/libcamera/internal/matrix.h
> > index 6d40567af0a0..a07a47701336 100644
> > --- a/include/libcamera/internal/matrix.h
> > +++ b/include/libcamera/internal/matrix.h
> > @@ -19,6 +19,12 @@ namespace libcamera {
> >  
> >  LOG_DECLARE_CATEGORY(Matrix)
> >  
> > +#ifndef __DOXYGEN__
> > +template<typename T>
> > +bool matrixInvert(Span<const T> dataIn, Span<T> dataOut, unsigned int dim,
> > +                 Span<T> scratchBuffer, Span<unsigned int> swapBuffer);
> > +#endif /* __DOXYGEN__ */
> > +
> >  template<typename T, unsigned int Rows, unsigned int Cols>
> >  class Matrix
> >  {
> > @@ -91,6 +97,23 @@ 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;
> > +               std::array<T, Rows * Cols * 2> scratchBuffer;
> > +               std::array<unsigned int, Rows> swapBuffer;
> > +               bool res = matrixInvert(Span<const T>(data_),
> > +                                       Span<T>(inverse.data_),
> > +                                       Rows,
> > +                                       Span<T>(scratchBuffer),
> > +                                       Span<unsigned int>(swapBuffer));
> > +               if (ok)
> > +                       *ok = res;
> > +               return inverse;
> > +       }
> > +
> >  private:
> >         /*
> >          * \todo The initializer is only necessary for the constructor to be
> > diff --git a/src/libcamera/matrix.cpp b/src/libcamera/matrix.cpp
> > index 49e2aa3b4f2c..68fc1b7bd5ac 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,152 @@ LOG_DEFINE_CATEGORY(Matrix)
> >   */
> >  
> >  #ifndef __DOXYGEN__
> > +template<typename T>
> > +bool matrixInvert(Span<const T> dataIn, Span<T> dataOut, unsigned int dim,
> > +                 Span<T> scratchBuffer, Span<unsigned int> swapBuffer)
> > +{
> > +       /*
> > +        * 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, Span<unsigned int> swapBuffer, unsigned int rows, unsigned int cols)
> > +                       : data_(data), swap_(swapBuffer), rows_(rows), cols_(cols)
> > +               {
> > +                       ASSERT(swap_.size() == rows);
> > +                       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_;
> > +               Span<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.
> > +        */
> > +       ASSERT(scratchBuffer.size() == dim * dim * 2);
> > +       MatrixAccessor matrix(scratchBuffer, swapBuffer, 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, j + dim) = T{ 0 };
> > +               }
> > +               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, Span<float> scratchBuffer,
> > +                                 Span<unsigned int> swapBuffer);
> > +template bool matrixInvert<double>(Span<const double> data, Span<double> dataOut,
> > +                                  unsigned int dim, Span<double> scratchBuffer,
> > +                                  Span<unsigned int> swapBuffer);
> > +
> >  /*
> >   * 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
> > -- 
> > 2.43.0
> >