[PATCH 03/10] libcamera: matrix: Add inverse() function

[Laurent Pinchart]

Hi Stefan,

Thank you for the patch.

On Mon, Feb 17, 2025 at 11:01:44AM +0100, Stefan Klug wrote:
> For calculations in upcoming algorithm patches, the inverse of a matrix
> is required. Add an implementation of the inverse() function for 3x3
> matrices.
> 
> Signed-off-by: Stefan Klug <stefan.klug at ideasonboard.com>
> ---
>  include/libcamera/internal/matrix.h | 24 ++++++++++++++++++++++++
>  src/libcamera/matrix.cpp            | 10 ++++++++++
>  2 files changed, 34 insertions(+)
> 
> diff --git a/include/libcamera/internal/matrix.h b/include/libcamera/internal/matrix.h
> index ca81dcda93c4..053336e9cfa4 100644
> --- a/include/libcamera/internal/matrix.h
> +++ b/include/libcamera/internal/matrix.h
> @@ -90,6 +90,30 @@ public:
>  		return *this;
>  	}
>  
> +	Matrix<T, Rows, Cols> inverse() const
> +	{
> +		static_assert(Rows == 3 && Cols == 3, "Matrix must be 3x3");

You know what I'll ask, right ? :-)

Inverting the matrix only makes sense when T is a floating point type.
I'd add a static assertion or enable_if for this.

> +
> +		const auto &m = *this;
> +		double det = m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]) -
> +			     m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]) +
> +			     m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);

Calculating the determinant should be moved to its own function, it's
useful by itself.

> +
> +		double invdet = 1 / det;

What if det is zero (or very close) ?

> +
> +		Matrix<T, Rows, Cols> ret;
> +		ret[0][0] = (m[1][1] * m[2][2] - m[2][1] * m[1][2]) * invdet;
> +		ret[0][1] = (m[0][2] * m[2][1] - m[0][1] * m[2][2]) * invdet;
> +		ret[0][2] = (m[0][1] * m[1][2] - m[0][2] * m[1][1]) * invdet;
> +		ret[1][0] = (m[1][2] * m[2][0] - m[1][0] * m[2][2]) * invdet;
> +		ret[1][1] = (m[0][0] * m[2][2] - m[0][2] * m[2][0]) * invdet;
> +		ret[1][2] = (m[1][0] * m[0][2] - m[0][0] * m[1][2]) * invdet;
> +		ret[2][0] = (m[1][0] * m[2][1] - m[2][0] * m[1][1]) * invdet;
> +		ret[2][1] = (m[2][0] * m[0][1] - m[0][0] * m[2][1]) * invdet;
> +		ret[2][2] = (m[0][0] * m[1][1] - m[1][0] * m[0][1]) * invdet;

That's a lot of calculation for an inline function, please move it to
the .cpp file.

> +		return ret;
> +	}
> +
>  	template<typename T2>
>  	Matrix<T2, Rows, Cols> cast() const
>  	{
> diff --git a/src/libcamera/matrix.cpp b/src/libcamera/matrix.cpp
> index 91a3f16405a3..b17a1938f1ba 100644
> --- a/src/libcamera/matrix.cpp
> +++ b/src/libcamera/matrix.cpp
> @@ -77,6 +77,16 @@ LOG_DEFINE_CATEGORY(Matrix)
>   * \return Row \a i from the matrix, as a Span
>   */
>  
> +/**
> + * \fn Matrix::inverse() const
> + * \brief Compute the inverse of the matrix
> + *
> + * This function computes the inverse of the matrix. It is only implemented for
> + * 3x3 matrices.
> + *
> + * \return The inverse of the matrix
> + */
> +
>  /**
>   * \fn template<typename T2> Matrix<T2, Rows, Cols> Matrix::cast() const
>   * \brief Cast the matrix to a different type

-- 
Regards,

Laurent Pinchart