/**
* Converts {@link DMatrixRMaj} into {@link DMatrix6}
*
* @param input Input matrix.
* @param output Output matrix. If null a new matrix will be declared.
* @return Converted matrix.
*/
public static DMatrix6 convert(DMatrixRMaj input, DMatrix6 output)
{
if (output == null)
{
output = new DMatrix6();
}
if (input.getNumRows() != 1 && input.getNumCols() != 1)
{
throw new ArgumentException("One row or column must have a length of 1 for it to be a vector");
}
int length = Math.Max(input.getNumRows(), input.getNumCols());
if (length != 6)
{
throw new ArgumentException("Length of input vector is not 6. It is " + length);
}
output.a1 = input.data[0];
output.a2 = input.data[1];
output.a3 = input.data[2];
output.a4 = input.data[3];
output.a5 = input.data[4];
output.a6 = input.data[5];
return(output);
}
/**
* Converts {@link DMatrixRMaj} into {@link DMatrix4x4}
*
* @param input Input matrix.
* @param output Output matrix. If null a new matrix will be declared.
* @return Converted matrix.
*/
public static DMatrix4x4 convert(DMatrixRMaj input, DMatrix4x4 output)
{
if (output == null)
{
output = new DMatrix4x4();
}
if (input.getNumRows() != output.getNumRows())
{
throw new ArgumentException("Number of rows do not match");
}
if (input.getNumCols() != output.getNumCols())
{
throw new ArgumentException("Number of columns do not match");
}
output.a11 = input.data[0];
output.a12 = input.data[1];
output.a13 = input.data[2];
output.a14 = input.data[3];
output.a21 = input.data[4];
output.a22 = input.data[5];
output.a23 = input.data[6];
output.a24 = input.data[7];
output.a31 = input.data[8];
output.a32 = input.data[9];
output.a33 = input.data[10];
output.a34 = input.data[11];
output.a41 = input.data[12];
output.a42 = input.data[13];
output.a43 = input.data[14];
output.a44 = input.data[15];
return(output);
}
/**
* Performs sanity checks on the input data and reshapes internal matrices. By reshaping
* a matrix it will only declare new memory when needed.
*/
public void configure(DMatrixRMaj initParam, DMatrixRMaj X, DMatrixRMaj Y)
{
if (Y.getNumRows() != X.getNumRows())
{
throw new ArgumentException("Different vector lengths");
}
else if (Y.getNumCols() != 1 || X.getNumCols() != 1)
{
throw new ArgumentException("Inputs must be a column vector");
}
int numParam = initParam.getNumElements();
int numPoints = Y.getNumRows();
if (param.getNumElements() != initParam.getNumElements())
{
// reshaping a matrix means that new memory is only declared when needed
this.param.reshape(numParam, 1, false);
this.d.reshape(numParam, 1, false);
this.H.reshape(numParam, numParam, false);
this.negDelta.reshape(numParam, 1, false);
this.tempParam.reshape(numParam, 1, false);
this.A.reshape(numParam, numParam, false);
}
param.set(initParam);
// reshaping a matrix means that new memory is only declared when needed
temp0.reshape(numPoints, 1, false);
temp1.reshape(numPoints, 1, false);
tempDH.reshape(numPoints, 1, false);
jacobian.reshape(numParam, numPoints, false);
}
/**
* Converts {@link DMatrixRMaj} into {@link DMatrix6x6}
*
* @param input Input matrix.
* @param output Output matrix. If null a new matrix will be declared.
* @return Converted matrix.
*/
public static DMatrix6x6 convert(DMatrixRMaj input, DMatrix6x6 output)
{
if (output == null)
{
output = new DMatrix6x6();
}
if (input.getNumRows() != output.getNumRows())
{
throw new ArgumentException("Number of rows do not match");
}
if (input.getNumCols() != output.getNumCols())
{
throw new ArgumentException("Number of columns do not match");
}
output.a11 = input.data[0];
output.a12 = input.data[1];
output.a13 = input.data[2];
output.a14 = input.data[3];
output.a15 = input.data[4];
output.a16 = input.data[5];
output.a21 = input.data[6];
output.a22 = input.data[7];
output.a23 = input.data[8];
output.a24 = input.data[9];
output.a25 = input.data[10];
output.a26 = input.data[11];
output.a31 = input.data[12];
output.a32 = input.data[13];
output.a33 = input.data[14];
output.a34 = input.data[15];
output.a35 = input.data[16];
output.a36 = input.data[17];
output.a41 = input.data[18];
output.a42 = input.data[19];
output.a43 = input.data[20];
output.a44 = input.data[21];
output.a45 = input.data[22];
output.a46 = input.data[23];
output.a51 = input.data[24];
output.a52 = input.data[25];
output.a53 = input.data[26];
output.a54 = input.data[27];
output.a55 = input.data[28];
output.a56 = input.data[29];
output.a61 = input.data[30];
output.a62 = input.data[31];
output.a63 = input.data[32];
output.a64 = input.data[33];
output.a65 = input.data[34];
output.a66 = input.data[35];
return(output);
}
/**
* Adds a new sample of the raw data to internal data structure for later processing. All the samples
* must be added before computeBasis is called.
*
* @param sampleData Sample from original raw data.
*/
public void addSample(double[] sampleData)
{
if (A.getNumCols() != sampleData.Length)
{
throw new ArgumentException("Unexpected sample size");
}
if (sampleIndex >= A.getNumRows())
{
throw new ArgumentException("Too many samples");
}
for (int i = 0; i < sampleData.Length; i++)
{
A.set(sampleIndex, i, sampleData[i]);
}
sampleIndex++;
}
/**
* Converts {@link DMatrixRMaj} into {@link DMatrix2x2}
*
* @param input Input matrix.
* @param output Output matrix. If null a new matrix will be declared.
* @return Converted matrix.
*/
public static DMatrix2x2 convert(DMatrixRMaj input, DMatrix2x2 output)
{
if (output == null)
{
output = new DMatrix2x2();
}
if (input.getNumRows() != output.getNumRows())
{
throw new ArgumentException("Number of rows do not match");
}
if (input.getNumCols() != output.getNumCols())
{
throw new ArgumentException("Number of columns do not match");
}
output.a11 = input.data[0];
output.a12 = input.data[1];
output.a21 = input.data[2];
output.a22 = input.data[3];
return(output);
}
