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 leastsquares solution to the system Ax=b for nonsquare 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 nonzero. Due to the rounding errors that come from floatpoint math, we cannot check if the determinant is exactly zero. Instead, it is checked to be smaller in magnitude than the zerotolerance.
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 zerotolerance 
int rc_algebra_lin_system_solve_qr  (  rc_matrix_t  A, 
rc_vector_t  b,  
rc_vector_t *  x  
) 
Finds a leastsquares solution to the system Ax=b for nonsquare 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 leastsquares fit to the overdefined 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 