FancySafeBot 0.0.1
A safe robotics library
Loading...
Searching...
No Matches
Linear Algebra

Linear algebra utility for matrix and vector computations. More...

Macros

#define FSB_MAX(a, b)   (((a) > (b)) ? (a) : (b))
 
#define FSB_MIN(a, b)   (((a) < (b)) ? (a) : (b))
 

Enumerations

enum class  fsb::LinalgErrorType : uint8_t {
  LinalgErrorType::ERROR_NONE = 0 , LinalgErrorType::ERROR_INPUT = 1 , LinalgErrorType::ERROR_MEMORY = 2 , LinalgErrorType::ERROR_CONVERGE = 3 ,
  LinalgErrorType::ERROR_QUERY = 4 , LinalgErrorType::NOT_POSITIVE_DEFINITE = 5 , LinalgErrorType::SINGULAR = 6 , LinalgErrorType::ERROR_NOT_FULL_RANK = 7
}
 Error codes for linear algebra functions. More...
 

Functions

LinalgErrorType fsb::linalg_cholesky_decomposition (Span< const Real > mat, size_t dim, Span< Real > mat_chol)
 Cholesky factorization for a symmetric positive definite matrix.
 
LinalgErrorType fsb::linalg_cholesky_solve (Span< const Real > mat, size_t dim, Span< const Real > b_vec, size_t nrhs, Span< Real > work, Span< Real > x_vec)
 Cholesky solve.
 
bool fsb::linalg_is_posdef (Span< const Real > mat, size_t dim, Span< Real > work)
 Check if a symmetric matrix (lower triangular storage) is positive definite.
 
LinalgErrorType fsb::linalg_matrix_sqr_solve (Span< const Real > mat, size_t dim, Span< const Real > y_vec, size_t nrhs, Span< Real > work, Span< lapack_int > iwork, Span< Real > x_vec)
 Solve a square linear system A * x = b using LU factorization.
 
LinalgErrorType fsb::linalg_pseudoinverse (Span< const Real > mat, size_t rows, size_t cols, Span< Real > work, Span< Real > inv_mat)
 Moore-Penrose pseudoinverse via SVD.
 
LinalgErrorType fsb::linalg_leastsquares_solve (Span< const Real > mat, size_t rows, size_t cols, Span< const Real > b_vec, size_t nrhs, Span< Real > work, Span< Real > x_vec)
 Solve an overdetermined or underdetermined least-squares problem.
 
LinalgErrorType fsb::linalg_svd (Span< const Real > mat, size_t rows, size_t cols, bool u_full, bool v_full, Span< Real > work, Span< Real > unitary_u, Span< Real > sing_val, Span< Real > unitary_vt)
 SVD decomposition.
 
LinalgErrorType fsb::linalg_matrix_eig (Span< const Real > mat, size_t dim, Span< Real > work, Span< Real > val_real, Span< Real > val_imag, Span< Real > vec_real, Span< Real > vec_imag)
 Eigenvalue decomposition for a general square matrix.
 
LinalgErrorType fsb::linalg_sym_lt_eig (Span< const Real > mat, size_t dim, Span< Real > work, Span< Real > val, Span< Real > vec)
 Eigenvalue decomposition for a real symmetric matrix (lower triangular storage)
 
template<size_t MaxMat, size_t MaxWorkLen, size_t MaxU, size_t MaxS, size_t MaxVT>
LinalgErrorType fsb::linalg_svd_array (const Array< MaxMat > &mat, size_t rows, size_t cols, bool u_full, bool v_full, Array< MaxWorkLen > &work, size_t work_len, Array< MaxU > &unitary_u, Array< MaxS > &sing_val, size_t s_dim, Array< MaxVT > &unitary_vt)
 
template<size_t MaxMat, size_t MaxWorkLen, size_t MaxEig, size_t MaxVec>
LinalgErrorType fsb::linalg_matrix_eig_array (const Array< MaxMat > &mat, size_t dim, Array< MaxWorkLen > &work, size_t work_len, Array< MaxEig > &val_real, Array< MaxEig > &val_imag, Array< MaxVec > &vec_real, Array< MaxVec > &vec_imag)
 
template<size_t MaxMat, size_t MaxWorkLen, size_t MaxEig, size_t MaxVec>
LinalgErrorType fsb::linalg_sym_lt_eig_array (const Array< MaxMat > &mat, size_t dim, Array< MaxWorkLen > &work, size_t work_len, Array< MaxEig > &val, Array< MaxVec > &vec)
 
template<size_t MaxMat>
LinalgErrorType fsb::linalg_cholesky_decomposition_array (const Array< MaxMat > &mat, size_t dim, Array< MaxMat > &mat_chol)
 
template<size_t MaxMat, size_t MaxWorkLen>
bool fsb::linalg_is_posdef_array (const Array< MaxMat > &mat, size_t dim, Array< MaxWorkLen > &work, size_t work_len)
 
template<size_t MaxMat, size_t MaxVec, size_t MaxWorkLen>
LinalgErrorType fsb::linalg_cholesky_solve_array (const Array< MaxMat > &mat, size_t dim, const Array< MaxVec > &b_vec, size_t nrhs, Array< MaxWorkLen > &work, size_t work_len, Array< MaxVec > &x_vec)
 
template<size_t MaxMat, size_t MaxVec, size_t MaxWorkLen, size_t MaxIWorkLen>
LinalgErrorType fsb::linalg_matrix_sqr_solve_array (const Array< MaxMat > &mat, size_t dim, const Array< MaxVec > &y_vec, size_t nrhs, Array< MaxWorkLen > &work, size_t work_len, Array< MaxIWorkLen, lapack_int > &iwork, size_t iwork_len, Array< MaxVec > &x_vec)
 
template<size_t MaxMat, size_t MaxInv, size_t MaxWorkLen>
LinalgErrorType fsb::linalg_pseudoinverse_array (const Array< MaxMat > &mat, size_t rows, size_t cols, Array< MaxWorkLen > &work, size_t work_len, Array< MaxInv > &inv_mat)
 
template<size_t MaxMat, size_t MaxB, size_t MaxX, size_t MaxWorkLen>
LinalgErrorType fsb::linalg_leastsquares_solve_array (const Array< MaxMat > &mat, size_t rows, size_t cols, const Array< MaxB > &b_vec, size_t nrhs, Array< MaxWorkLen > &work, size_t work_len, Array< MaxX > &x_vec)
 

Detailed Description

Linear algebra utility for matrix and vector computations.

Enumeration Type Documentation

◆ LinalgErrorType

enum class fsb::LinalgErrorType : uint8_t
strong

Error codes for linear algebra functions.

Enumerator
ERROR_NONE 

No error.

ERROR_INPUT 

Input value error.

ERROR_MEMORY 

Not enough memory.

ERROR_CONVERGE 

Solution did not converge.

ERROR_QUERY 

Work query failed.

NOT_POSITIVE_DEFINITE 

Matrix not positive definite.

SINGULAR 

Input matrix is singular.

ERROR_NOT_FULL_RANK 

Matrix is not full rank.

Function Documentation

◆ linalg_cholesky_decomposition()

LinalgErrorType fsb::linalg_cholesky_decomposition ( Span< const Real >  mat,
size_t  dim,
Span< Real >  mat_chol 
)

Cholesky factorization for a symmetric positive definite matrix.

Computes the lower triangular Cholesky factorization A = L * L^T of a symmetric positive definite matrix stored in lower triangular form.

Parameters
[in]matInput symmetric positive definite matrix (dim x dim)
[in]dimMatrix dimension
[out]mat_cholLower triangular Cholesky factor L (dim x dim)
Returns
Error code; NOT_POSITIVE_DEFINITE if the matrix is not positive definite

◆ linalg_cholesky_solve()

LinalgErrorType fsb::linalg_cholesky_solve ( Span< const Real >  mat,
size_t  dim,
Span< const Real >  b_vec,
size_t  nrhs,
Span< Real >  work,
Span< Real >  x_vec 
)

Cholesky solve.

Solves the symmetric positive-definite system A * x = b using Cholesky factorization (DPOTRF / DPOTRS). work must hold at least dim × dim elements for the factorization buffer.

Parameters
[in]matInput symmetric positive-definite matrix (dim x dim)
[in]dimMatrix dimension
[in]b_vecRight-hand side matrix (dim x nrhs)
[in]nrhsNumber of right-hand side vectors
[in,out]workWork buffer (at least dim × dim elements)
[out]x_vecSolution matrix (dim x nrhs)
Returns
Error code; NOT_POSITIVE_DEFINITE if the matrix is not positive definite

◆ linalg_is_posdef()

bool fsb::linalg_is_posdef ( Span< const Real >  mat,
size_t  dim,
Span< Real >  work 
)

Check if a symmetric matrix (lower triangular storage) is positive definite.

Attempts a Cholesky factorization. work must hold at least dim × dim elements.

Parameters
[in]matInput lower triangular symmetric matrix (dim x dim)
[in]dimMatrix dimension
[in,out]workWork buffer (at least dim × dim elements)
Returns
true if the matrix is positive definite, false otherwise

◆ linalg_leastsquares_solve()

LinalgErrorType fsb::linalg_leastsquares_solve ( Span< const Real >  mat,
size_t  rows,
size_t  cols,
Span< const Real >  b_vec,
size_t  nrhs,
Span< Real >  work,
Span< Real >  x_vec 
)

Solve an overdetermined or underdetermined least-squares problem.

Solves min‖A·X − B‖₂ for X using DGELS (QR or LQ factorization). work must hold at least rows × cols + max(rows,cols) × nrhs elements, plus the additional LAPACK workspace returned by the internal work query.

Parameters
[in]matInput matrix A (rows x cols)
[in]rowsNumber of rows in A
[in]colsNumber of columns in A
[in]b_vecRight-hand side matrix B (rows x nrhs)
[in]nrhsNumber of right-hand side vectors
[in,out]workWork buffer (see size requirement above)
[out]x_vecSolution matrix X (cols x nrhs)
Returns
Error code; ERROR_NOT_FULL_RANK if A is rank-deficient

◆ linalg_matrix_eig()

LinalgErrorType fsb::linalg_matrix_eig ( Span< const Real >  mat,
size_t  dim,
Span< Real >  work,
Span< Real >  val_real,
Span< Real >  val_imag,
Span< Real >  vec_real,
Span< Real >  vec_imag 
)

Eigenvalue decomposition for a general square matrix.

Computes all eigenvalues and optionally right eigenvectors of a general dim x dim matrix using DGEEV.

Parameters
[in]matInput square matrix (dim x dim)
[in]dimMatrix dimension
[in,out]workWork buffer; must hold at least dim × dim elements plus the LAPACK workspace returned by the internal work query
[out]val_realReal parts of eigenvalues (dim x 1)
[out]val_imagImaginary parts of eigenvalues (dim x 1)
[out]vec_realReal parts of right eigenvectors, column-major (dim x dim)
[out]vec_imagImaginary parts of right eigenvectors, column-major (dim x dim)
Returns
Error code

◆ linalg_matrix_sqr_solve()

LinalgErrorType fsb::linalg_matrix_sqr_solve ( Span< const Real >  mat,
size_t  dim,
Span< const Real >  y_vec,
size_t  nrhs,
Span< Real >  work,
Span< lapack_int >  iwork,
Span< Real >  x_vec 
)

Solve a square linear system A * x = b using LU factorization.

Uses DGETRF (LU with partial pivoting) followed by DGETRS. work must hold at least dim × dim elements; iwork must hold at least dim elements.

Parameters
[in]matInput square matrix (dim x dim)
[in]dimMatrix dimension
[in]y_vecRight-hand side matrix (dim x nrhs)
[in]nrhsNumber of right-hand side vectors
[in,out]workReal work buffer (at least dim × dim elements)
[in,out]iworkInteger pivot buffer (at least dim elements)
[out]x_vecSolution matrix (dim x nrhs)
Returns
Error code; SINGULAR if the matrix is exactly singular

◆ linalg_pseudoinverse()

LinalgErrorType fsb::linalg_pseudoinverse ( Span< const Real >  mat,
size_t  rows,
size_t  cols,
Span< Real >  work,
Span< Real >  inv_mat 
)

Moore-Penrose pseudoinverse via SVD.

Computes the pseudoinverse of an arbitrary (rows × cols) matrix using thin SVD. work must hold at least rows × min(rows,cols) + min(rows,cols) + min(rows,cols) × cols elements, plus the additional workspace required by linalg_svd().

Parameters
[in]matInput matrix (rows x cols)
[in]rowsNumber of rows
[in]colsNumber of columns
[in,out]workWork buffer (see size requirement above)
[out]inv_matPseudoinverse matrix (cols x rows)
Returns
Error code

◆ linalg_svd()

LinalgErrorType fsb::linalg_svd ( Span< const Real >  mat,
size_t  rows,
size_t  cols,
bool  u_full,
bool  v_full,
Span< Real >  work,
Span< Real >  unitary_u,
Span< Real >  sing_val,
Span< Real >  unitary_vt 
)

SVD decomposition.

u_full and v_full affect the output matrices as follows: if (m < n) then s = m else s = n if (u_full, v_full) == (0, 0) then U(m,s), S(s,s), VT(s,n) if (u_full, v_full) == (1, 0) then U(m,m), S(m,s), VT(s,n) if (u_full, v_full) == (0, 1) then U(m,s), S(s,n), VT(n,n) if (u_full, v_full) == (1, 1) then U(m,m), S(m,n), VT(n,n)

Parameters
[in]matInput matrix to decompose (rows x cols)
[in]rowsNumber of rows
[in]colsNumber of columns
[in]u_fullCompute full U matrix (true) or thin U (false)
[in]v_fullCompute full V^T matrix (true) or thin V^T (false)
[in,out]workWork buffer; must hold at least rows × cols elements plus the LAPACK workspace returned by the internal work query
[out]unitary_uUnitary matrix U
[out]sing_valSingular values (s x 1)
[out]unitary_vtTranspose of unitary matrix V^T

◆ linalg_sym_lt_eig()

LinalgErrorType fsb::linalg_sym_lt_eig ( Span< const Real >  mat,
size_t  dim,
Span< Real >  work,
Span< Real >  val,
Span< Real >  vec 
)

Eigenvalue decomposition for a real symmetric matrix (lower triangular storage)

Computes all eigenvalues and eigenvectors of a real symmetric matrix stored in lower triangular form. On exit, vec contains the orthonormal eigenvectors as columns and val contains the corresponding eigenvalues in ascending order.

Parameters
[in]matInput symmetric matrix in lower triangular form (dim x dim)
[in]dimMatrix dimension
[in,out]workWork buffer; must be at least as large as the workspace size returned by the internal LAPACK work query
[out]valEigenvalues in ascending order (dim x 1)
[out]vecColumn eigenvectors (dim x dim)
Returns
Error code