Robot Core Documentation
Loading...
Searching...
No Matches
edu.wpi.first.math.Matrix< R extends Num, C extends Num > Class Template Reference

Public Member Functions

 Matrix (Nat< R > rows, Nat< C > columns)
 
 Matrix (Nat< R > rows, Nat< C > columns, double[] storage)
 
 Matrix (SimpleMatrix storage)
 
 Matrix (Matrix< R, C > other)
 
SimpleMatrix getStorage ()
 
final int getNumCols ()
 
final int getNumRows ()
 
final double get (int row, int col)
 
final void set (int row, int col, double value)
 
final void setRow (int row, Matrix< N1, C > val)
 
final void setColumn (int column, Matrix< R, N1 > val)
 
void fill (double value)
 
final Matrix< R, C > diag ()
 
final double max ()
 
final double maxAbs ()
 
final double minInternal ()
 
final double mean ()
 
final< C2 extends Num > Matrix< R, C2 > times (Matrix< C, C2 > other)
 
Matrix< R, C > times (double value)
 
final Matrix< R, C > elementTimes (Matrix< R, C > other)
 
final Matrix< R, C > minus (double value)
 
final Matrix< R, C > minus (Matrix< R, C > value)
 
final Matrix< R, C > plus (double value)
 
final Matrix< R, C > plus (Matrix< R, C > value)
 
Matrix< R, C > div (int value)
 
Matrix< R, C > div (double value)
 
final Matrix< C, R > transpose ()
 
final Matrix< R, C > copy ()
 
final Matrix< R, C > inv ()
 
final< C2 extends Num > Matrix< C, C2 > solve (Matrix< R, C2 > b)
 
final< R2 extends Num, C2 extends Num > Matrix< C, C2 > solveFullPivHouseholderQr (Matrix< R2, C2 > other)
 
final Matrix< R, C > exp ()
 
final Matrix< R, C > pow (double exponent)
 
final double det ()
 
final double normF ()
 
final double normIndP1 ()
 
final double elementSum ()
 
final double trace ()
 
final Matrix< R, C > elementPower (double b)
 
final Matrix< R, C > elementPower (int b)
 
final Matrix< N1, C > extractRowVector (int row)
 
final Matrix< R, N1extractColumnVector (int column)
 
final< R2 extends Num, C2 extends Num > Matrix< R2, C2 > block (Nat< R2 > height, Nat< C2 > width, int startingRow, int startingCol)
 
final< R2 extends Num, C2 extends Num > Matrix< R2, C2 > block (int height, int width, int startingRow, int startingCol)
 
Matrix< R, C > lltDecompose (boolean lowerTriangular)
 
double[] getData ()
 
boolean isIdentical (Matrix<?, ?> other, double tolerance)
 
boolean isEqual (Matrix<?, ?> other, double tolerance)
 
void rankUpdate (Matrix< R, N1 > v, double sigma, boolean lowerTriangular)
 
boolean equals (Object other)
 

Static Public Member Functions

static< D extends Num > Matrix< D, D > eye (Nat< D > dim)
 
static< D extends Num > Matrix< D, D > eye (D dim)
 
static< R extends Num, C extends Num > MatBuilder< R, C > mat (Nat< R > rows, Nat< C > cols)
 
static< R1 extends Num, C1 extends Num > Matrix< R1, C1 > changeBoundsUnchecked (Matrix<?, ?> mat)
 

Protected Attributes

final SimpleMatrix m_storage
 

Detailed Description

A shape-safe wrapper over Efficient Java Matrix Library (EJML) matrices.

This class is intended to be used alongside the state space library.

Parameters
<R>The number of rows in this matrix.
<C>The number of columns in this matrix.

Constructor & Destructor Documentation

◆ Matrix() [1/4]

edu.wpi.first.math.Matrix< R extends Num, C extends Num >.Matrix ( Nat< R > rows,
Nat< C > columns )

Constructs an empty zero matrix of the given dimensions.

Parameters
rowsThe number of rows of the matrix.
columnsThe number of columns of the matrix.

◆ Matrix() [2/4]

edu.wpi.first.math.Matrix< R extends Num, C extends Num >.Matrix ( Nat< R > rows,
Nat< C > columns,
double[] storage )

Constructs a new Matrix with the given storage. Caller should make sure that the provided generic bounds match the shape of the provided Matrix.

Parameters
rowsThe number of rows of the matrix.
columnsThe number of columns of the matrix.
storageThe double array to back this value.

◆ Matrix() [3/4]

edu.wpi.first.math.Matrix< R extends Num, C extends Num >.Matrix ( SimpleMatrix< R extends Num, C extends Num > storage)

Constructs a new Matrix with the given storage. Caller should make sure that the provided generic bounds match the shape of the provided Matrix.

NOTE:It is not recommend to use this constructor unless the SimpleMatrix API is absolutely necessary due to the desired function not being accessible through the Matrix wrapper.

Parameters
storageThe SimpleMatrix to back this value.

◆ Matrix() [4/4]

edu.wpi.first.math.Matrix< R extends Num, C extends Num >.Matrix ( Matrix< R, C > other)

Constructs a new matrix with the storage of the supplied matrix.

Parameters
otherThe Matrix to copy the storage of.

Member Function Documentation

◆ block() [1/2]

final< R2 extends Num, C2 extends Num > Matrix< R2, C2 > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.block ( int height,
int width,
int startingRow,
int startingCol )

Extracts a matrix of a given size and start position with new underlying storage.

Parameters
<R2>Number of rows to extract.
<C2>Number of columns to extract.
heightThe number of rows of the extracted matrix.
widthThe number of columns of the extracted matrix.
startingRowThe starting row of the extracted matrix.
startingColThe starting column of the extracted matrix.
Returns
The extracted matrix.

◆ block() [2/2]

final< R2 extends Num, C2 extends Num > Matrix< R2, C2 > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.block ( Nat< R2 > height,
Nat< C2 > width,
int startingRow,
int startingCol )

Extracts a matrix of a given size and start position with new underlying storage.

Parameters
<R2>Number of rows to extract.
<C2>Number of columns to extract.
heightThe number of rows of the extracted matrix.
widthThe number of columns of the extracted matrix.
startingRowThe starting row of the extracted matrix.
startingColThe starting column of the extracted matrix.
Returns
The extracted matrix.

◆ changeBoundsUnchecked()

static< R1 extends Num, C1 extends Num > Matrix< R1, C1 > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.changeBoundsUnchecked ( Matrix<?, ?> mat)
static

Reassigns dimensions of a Matrix to allow for operations with other matrices that have wildcard dimensions.

Parameters
<R1>Row dimension to assign.
<C1>Column dimension to assign.
matThe Matrix to remove the dimensions from.
Returns
The matrix with reassigned dimensions.

◆ copy()

final Matrix< R, C > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.copy ( )

Returns a copy of this matrix.

Returns
A copy of this matrix.

◆ det()

final double edu.wpi.first.math.Matrix< R extends Num, C extends Num >.det ( )

Returns the determinant of this matrix.

Returns
The determinant of this matrix.

◆ diag()

final Matrix< R, C > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.diag ( )

Returns the diagonal elements inside a vector or square matrix.

If "this" Matrix is a vector then a square matrix is returned. If a "this" Matrix is a matrix then a vector of diagonal elements is returned.

Returns
The diagonal elements inside a vector or a square matrix.

◆ div() [1/2]

Matrix< R, C > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.div ( double value)

Divides all elements of this matrix by the given value.

Parameters
valueThe value to divide by.
Returns
The resultant matrix.

◆ div() [2/2]

Matrix< R, C > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.div ( int value)

Divides all elements of this matrix by the given value.

Parameters
valueThe value to divide by.
Returns
The resultant matrix.

◆ elementPower() [1/2]

final Matrix< R, C > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.elementPower ( double b)

Returns a matrix which is the result of an element by element power of "this" and b.

ci,j = ai,j ^ b

Parameters
bScalar.
Returns
The element by element power of "this" and b.

◆ elementPower() [2/2]

final Matrix< R, C > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.elementPower ( int b)

Returns a matrix which is the result of an element by element power of "this" and b.

ci,j = ai,j ^ b

Parameters
bScalar.
Returns
The element by element power of "this" and b.

◆ elementSum()

final double edu.wpi.first.math.Matrix< R extends Num, C extends Num >.elementSum ( )

Computes the sum of all the elements in the matrix.

Returns
Sum of all the elements.

◆ elementTimes()

final Matrix< R, C > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.elementTimes ( Matrix< R, C > other)

Returns a matrix which is the result of an element by element multiplication of "this" and other.

ci,j = ai,j*otheri,j

Parameters
otherThe other Matrix to perform element multiplication on.
Returns
The element by element multiplication of "this" and other.

◆ equals()

boolean edu.wpi.first.math.Matrix< R extends Num, C extends Num >.equals ( Object other)

Checks if an object is equal to this Matrix.

aij == bij

Parameters
otherThe Object to check against this Matrix.
Returns
true if the object supplied is a Matrix and is equal to this matrix.

◆ exp()

final Matrix< R, C > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.exp ( )

Computes the matrix exponential using Eigen's solver. This method only works for square matrices, and will otherwise throw an MatrixDimensionException.

Returns
The exponential of A.

◆ extractColumnVector()

final Matrix< R, N1 > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.extractColumnVector ( int column)

Extracts a given column into a column vector with new underlying storage.

Parameters
columnThe column to extract a vector from.
Returns
A column vector from the given column.

◆ extractRowVector()

final Matrix< N1, C > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.extractRowVector ( int row)

Extracts a given row into a row vector with new underlying storage.

Parameters
rowThe row to extract a vector from.
Returns
A row vector from the given row.

◆ eye() [1/2]

static< D extends Num > Matrix< D, D > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.eye ( D dim)
static

Creates the identity matrix of the given dimension.

Parameters
dimThe dimension of the desired matrix as a Num.
<D>The dimension of the desired matrix as a generic.
Returns
The DxD identity matrix.

◆ eye() [2/2]

static< D extends Num > Matrix< D, D > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.eye ( Nat< D > dim)
static

Creates the identity matrix of the given dimension.

Parameters
dimThe dimension of the desired matrix as a Nat.
<D>The dimension of the desired matrix as a generic.
Returns
The DxD identity matrix.

◆ fill()

void edu.wpi.first.math.Matrix< R extends Num, C extends Num >.fill ( double value)

Sets all the elements in "this" matrix equal to the specified value.

Parameters
valueThe value each element is set to.

◆ get()

final double edu.wpi.first.math.Matrix< R extends Num, C extends Num >.get ( int row,
int col )

Get an element of this matrix.

Parameters
rowThe row of the element.
colThe column of the element.
Returns
The element in this matrix at row,col.

◆ getData()

double[] edu.wpi.first.math.Matrix< R extends Num, C extends Num >.getData ( )

Returns the row major data of this matrix as a double array.

Returns
The row major data of this matrix as a double array.

◆ getNumCols()

final int edu.wpi.first.math.Matrix< R extends Num, C extends Num >.getNumCols ( )

Gets the number of columns in this matrix.

Returns
The number of columns, according to the internal storage.

◆ getNumRows()

final int edu.wpi.first.math.Matrix< R extends Num, C extends Num >.getNumRows ( )

Gets the number of rows in this matrix.

Returns
The number of rows, according to the internal storage.

◆ getStorage()

SimpleMatrix edu.wpi.first.math.Matrix< R extends Num, C extends Num >.getStorage ( )

Gets the underlying SimpleMatrix that this Matrix wraps.

NOTE:The use of this method is heavily discouraged as this removes any guarantee of type safety. This should only be called if the SimpleMatrix API is absolutely necessary due to the desired function not being accessible through the Matrix wrapper.

Returns
The underlying SimpleMatrix storage.

◆ inv()

final Matrix< R, C > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.inv ( )

Returns the inverse matrix of "this" matrix.

Returns
The inverse of "this" matrix.
Exceptions
org.ejml.data.SingularMatrixExceptionIf "this" matrix is non-invertable.

◆ isEqual()

boolean edu.wpi.first.math.Matrix< R extends Num, C extends Num >.isEqual ( Matrix<?, ?> other,
double tolerance )

Checks if another Matrix is equal to "this" within a specified tolerance.

This will check if each element is in tolerance of the corresponding element from the other Matrix.

tol ≥ |aij - bij|

Parameters
otherThe Matrix to check against this one.
toleranceThe tolerance to check equality with.
Returns
true if this matrix is equal to the one supplied.

◆ isIdentical()

boolean edu.wpi.first.math.Matrix< R extends Num, C extends Num >.isIdentical ( Matrix<?, ?> other,
double tolerance )

Checks if another Matrix is identical to "this" one within a specified tolerance.

This will check if each element is in tolerance of the corresponding element from the other Matrix or if the elements have the same symbolic meaning. For two elements to have the same symbolic meaning they both must be either Double.NaN, Double.POSITIVE_INFINITY, or Double.NEGATIVE_INFINITY.

NOTE:It is recommended to use Matrix#isEqual(Matrix, double) over this method when checking if two matrices are equal as Matrix#isEqual(Matrix, double) will return false if an element is uncountable. This method should only be used when uncountable elements need to be compared.

Parameters
otherThe Matrix to check against this one.
toleranceThe tolerance to check equality with.
Returns
true if this matrix is identical to the one supplied.

◆ lltDecompose()

Matrix< R, C > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.lltDecompose ( boolean lowerTriangular)

Decompose "this" matrix using Cholesky Decomposition. If the "this" matrix is zeros, it will return the zero matrix.

Parameters
lowerTriangularWhether we want to decompose to the lower triangular Cholesky matrix.
Returns
The decomposed matrix.
Exceptions
RuntimeExceptionif the matrix could not be decomposed(i.e. is not positive semidefinite).

◆ mat()

static< R extends Num, C extends Num > MatBuilder< R, C > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.mat ( Nat< R > rows,
Nat< C > cols )
static

Entrypoint to the MatBuilder class for creation of custom matrices with the given dimensions and contents.

Parameters
rowsThe number of rows of the desired matrix.
colsThe number of columns of the desired matrix.
<R>The number of rows of the desired matrix as a generic.
<C>The number of columns of the desired matrix as a generic.
Returns
A builder to construct the matrix.
Deprecated
Use MatBuilder#fill instead.

◆ max()

final double edu.wpi.first.math.Matrix< R extends Num, C extends Num >.max ( )

Returns the largest element of this matrix.

Returns
The largest element of this matrix.

◆ maxAbs()

final double edu.wpi.first.math.Matrix< R extends Num, C extends Num >.maxAbs ( )

Returns the absolute value of the element in this matrix with the largest absolute value.

Returns
The absolute value of the element with the largest absolute value.

◆ mean()

final double edu.wpi.first.math.Matrix< R extends Num, C extends Num >.mean ( )

Calculates the mean of the elements in this matrix.

Returns
The mean value of this matrix.

◆ minInternal()

final double edu.wpi.first.math.Matrix< R extends Num, C extends Num >.minInternal ( )

Returns the smallest element of this matrix.

Returns
The smallest element of this matrix.

◆ minus() [1/2]

final Matrix< R, C > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.minus ( double value)

Subtracts the given value from all the elements of this matrix.

Parameters
valueThe value to subtract.
Returns
The resultant matrix.

◆ minus() [2/2]

final Matrix< R, C > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.minus ( Matrix< R, C > value)

Subtracts the given matrix from this matrix.

Parameters
valueThe matrix to subtract.
Returns
The resultant matrix.

◆ normF()

final double edu.wpi.first.math.Matrix< R extends Num, C extends Num >.normF ( )

Computes the Frobenius normal of the matrix.

normF = Sqrt{ ∑i=1:mj=1:n { aij2} }

Returns
The matrix's Frobenius normal.

◆ normIndP1()

final double edu.wpi.first.math.Matrix< R extends Num, C extends Num >.normIndP1 ( )

Computes the induced p = 1 matrix norm.

||A||1= max(j=1 to n; sum(i=1 to m; |aij|))

Returns
The norm.

◆ plus() [1/2]

final Matrix< R, C > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.plus ( double value)

Adds the given value to all the elements of this matrix.

Parameters
valueThe value to add.
Returns
The resultant matrix.

◆ plus() [2/2]

final Matrix< R, C > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.plus ( Matrix< R, C > value)

Adds the given matrix to this matrix.

Parameters
valueThe matrix to add.
Returns
The resultant matrix.

◆ pow()

final Matrix< R, C > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.pow ( double exponent)

Computes the matrix power using Eigen's solver. This method only works for square matrices, and will otherwise throw an MatrixDimensionException.

Parameters
exponentThe exponent.
Returns
The exponential of A.

◆ rankUpdate()

void edu.wpi.first.math.Matrix< R extends Num, C extends Num >.rankUpdate ( Matrix< R, N1 > v,
double sigma,
boolean lowerTriangular )

Performs an inplace Cholesky rank update (or downdate).

If this matrix contains L where A = LLᵀ before the update, it will contain L where LLᵀ = A + σvvᵀ after the update.

Parameters
vVector to use for the update.
sigmaSigma to use for the update.
lowerTriangularWhether this matrix is lower triangular.

◆ set()

final void edu.wpi.first.math.Matrix< R extends Num, C extends Num >.set ( int row,
int col,
double value )

Sets the value at the given indices.

Parameters
rowThe row of the element.
colThe column of the element.
valueThe value to insert at the given location.

◆ setColumn()

final void edu.wpi.first.math.Matrix< R extends Num, C extends Num >.setColumn ( int column,
Matrix< R, N1 > val )

Sets a column to a given column vector.

Parameters
columnThe column to set.
valThe column vector to set the given row to.

◆ setRow()

final void edu.wpi.first.math.Matrix< R extends Num, C extends Num >.setRow ( int row,
Matrix< N1, C > val )

Sets a row to a given row vector.

Parameters
rowThe row to set.
valThe row vector to set the given row to.

◆ solve()

final< C2 extends Num > Matrix< C, C2 > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.solve ( Matrix< R, C2 > b)

Returns the solution x to the equation Ax = b, where A is "this" matrix.

The matrix equation could also be written as x = A-1b. Where the pseudo inverse is used if A is not square.

Note that this method does not support solving using a QR decomposition with full-pivoting, as only column-pivoting is supported. For full-pivoting, use solveFullPivHouseholderQr.

Parameters
<C2>Columns in b.
bThe right-hand side of the equation to solve.
Returns
The solution to the linear system.

◆ solveFullPivHouseholderQr()

final< R2 extends Num, C2 extends Num > Matrix< C, C2 > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.solveFullPivHouseholderQr ( Matrix< R2, C2 > other)

Solves the least-squares problem Ax=B using a QR decomposition with full pivoting, where this matrix is A.

Parameters
<R2>Number of rows in B.
<C2>Number of columns in B.
otherThe B matrix.
Returns
The solution matrix.

◆ times() [1/2]

Matrix< R, C > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.times ( double value)

Multiplies all the elements of this matrix by the given scalar.

Parameters
valueThe scalar value to multiply by.
Returns
A new matrix with all the elements multiplied by the given value.

◆ times() [2/2]

final< C2 extends Num > Matrix< R, C2 > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.times ( Matrix< C, C2 > other)

Multiplies this matrix with another that has C rows.

As matrix multiplication is only defined if the number of columns in the first matrix matches the number of rows in the second, this operation will fail to compile under any other circumstances.

Parameters
otherThe other matrix to multiply by.
<C2>The number of columns in the second matrix.
Returns
The result of the matrix multiplication between "this" and the given matrix.

◆ trace()

final double edu.wpi.first.math.Matrix< R extends Num, C extends Num >.trace ( )

Computes the trace of the matrix.

Returns
The trace of the matrix.

◆ transpose()

final Matrix< C, R > edu.wpi.first.math.Matrix< R extends Num, C extends Num >.transpose ( )

Calculates the transpose, Mᵀ of this matrix.

Returns
The transpose matrix.

Member Data Documentation

◆ m_storage

final SimpleMatrix edu.wpi.first.math.Matrix< R extends Num, C extends Num >.m_storage
protected

Storage for underlying EJML matrix.


The documentation for this class was generated from the following file: