Robot Control Library
|
advanced linear algebra functions
Functions | |
int | rc_algebra_lup_decomp (rc_matrix_t A, rc_matrix_t *L, rc_matrix_t *U, rc_matrix_t *P) |
Performs LUP decomposition on matrix A with partial pivoting. More... | |
int | rc_algebra_qr_decomp (rc_matrix_t A, rc_matrix_t *Q, rc_matrix_t *R) |
Calculate the QR decomposition of matrix A. More... | |
int | rc_algebra_invert_matrix (rc_matrix_t A, rc_matrix_t *Ainv) |
Inverts matrix A via LUP decomposition method. More... | |
int | rc_algebra_invert_matrix_inplace (rc_matrix_t *A) |
Inverts matrix A in place. More... | |
int | rc_algebra_lin_system_solve (rc_matrix_t A, rc_vector_t b, rc_vector_t *x) |
Solves Ax=b for given matrix A and vector b. More... | |
void | rc_algebra_set_zero_tolerance (double tol) |
Sets the zero tolerance for detecting singular matrices. More... | |
int | rc_algebra_lin_system_solve_qr (rc_matrix_t A, rc_vector_t b, rc_vector_t *x) |
Finds a least-squares solution to the system Ax=b for non-square A using QR decomposition method. More... | |
int | rc_algebra_fit_ellipsoid (rc_matrix_t points, rc_vector_t *center, rc_vector_t *lengths) |
Fits an ellipsoid to a set of points in 3D space. More... | |
int rc_algebra_lup_decomp | ( | rc_matrix_t | A, |
rc_matrix_t * | L, | ||
rc_matrix_t * | U, | ||
rc_matrix_t * | P | ||
) |
Performs LUP decomposition on matrix A with partial pivoting.
Places the result in matrices L,U,&P. Matrix A remains untouched and the original contents of LUP (if any) are freed and LUP are resized appropriately.
[in] | A | input matrix |
[out] | L | lower triangular |
[out] | U | upper triangular |
[out] | P | permutation matrix |
int rc_algebra_qr_decomp | ( | rc_matrix_t | A, |
rc_matrix_t * | Q, | ||
rc_matrix_t * | R | ||
) |
Calculate the QR decomposition of matrix A.
Uses householder reflection method. Matrix A remains untouched and the original contents of Q&R (if any) are freed and resized appropriately.
[in] | A | input matrix |
[out] | Q | orthogonal matrix output |
[out] | R | upper triangular matrix output |
int rc_algebra_invert_matrix | ( | rc_matrix_t | A, |
rc_matrix_t * | Ainv | ||
) |
Inverts matrix A via LUP decomposition method.
Places the result in matrix Ainv. Any existing memory allocated for Ainv is freed if necessary and its contents are overwritten. Returns -1 if matrix is not invertible.
[in] | A | input matrix |
[out] | Ainv | resulting inverted matrix |
int rc_algebra_invert_matrix_inplace | ( | rc_matrix_t * | A | ) |
Inverts matrix A in place.
The original contents of A are lost. Returns -1 if A is not invertible.
A | matrix to be inverted |
int rc_algebra_lin_system_solve | ( | rc_matrix_t | A, |
rc_vector_t | b, | ||
rc_vector_t * | x | ||
) |
Solves Ax=b for given matrix A and vector b.
Places the result in vector x. existing contents of x are freed and new memory is allocated if necessary.
[in] | A | matrix A |
[in] | b | column vector b |
[out] | x | solution column vector |
void rc_algebra_set_zero_tolerance | ( | double | tol | ) |
Sets the zero tolerance for detecting singular matrices.
When inverting matrices or solving a linear system, this library first checks that the determinant of the matrix is non-zero. Due to the rounding errors that come from float-point math, we cannot check if the determinant is exactly zero. Instead, it is checked to be smaller in magnitude than the zero-tolerance.
The default value is 10^-8 but it can be changed here if the user is dealing with unusually small or large floating point values.
This only effects the operation of rc_algebra_invert_matrix, rc_algebra_invert_matrix_inplace, and rc_algebra_lin_system_solve.
[in] | tol | The zero-tolerance |
int rc_algebra_lin_system_solve_qr | ( | rc_matrix_t | A, |
rc_vector_t | b, | ||
rc_vector_t * | x | ||
) |
Finds a least-squares solution to the system Ax=b for non-square A using QR decomposition method.
Places the solution in x.
[in] | A | matrix A |
[in] | b | column vector b |
[out] | x | solution column vector |
int rc_algebra_fit_ellipsoid | ( | rc_matrix_t | points, |
rc_vector_t * | center, | ||
rc_vector_t * | lengths | ||
) |
Fits an ellipsoid to a set of points in 3D space.
The principle axes of the fitted ellipsoid align with the global coordinate system. Therefore there are 6 degrees of freedom defining the ellipsoid: the x,y,z coordinates of the centroid and the lengths from the centroid to the surface in each of the 3 directions.
rc_matrix_t 'points' is a tall matrix with 3 columns and at least 6 rows. Each row must contain the x,y&z components of each individual point to be fit. If only 6 rows are provided, the resulting ellipsoid will be an exact fit. Otherwise the result is a least-squares fit to the over-defined dataset.
The final x,y,z position of the centroid will be placed in vector 'center' and the lengths or radius from the centroid to the surface along each axis will be placed in the vector 'lengths'
[in] | points | datapoints to fit |
[out] | center | center of ellipse |
[out] | lengths | lengths along principle axis |