I’m working with a magnetometer and want to port a calibration program to processing. The problem is it relies on Tuples and was originally written in C#. It looked a lot like Java so I stuck it in a sketch to see what was different. The first thing was Tuple. I solved that by finding a library that allows you to use Tuples. The syntax was different than the original code but close. I kept fiddling with it until the errors went away I thought. I hard coded some example variable values instead of making a Swing gui for now and then I ran it. I get some java.lang.reflect.InvocationTargetException. It seems to be when it tests value of p in the LUPDecompostion Tuple function it fails. I can’t tell if it is because how I translated or if there is some other error.
What did I do wrong?
Here’s where the original MagMaster code is found:
https://github.com/YuriMat/MagMasterSource/archive/master.zip
Here’s where the javatuple library is found:
https://www.javatuples.org/download.html
import org.javatuples.Pair;
import org.javatuples.Quartet;
import org.javatuples.Triplet;
public static double[][] A;
public static int[] B;
// MyPanel controlPanel;
public String textBoxXPlus_X_0,textBoxXPlus_X_180,textBoxXPlus_Y_0,textBoxXPlus_Y_180,textBoxXPlus_Z_0,textBoxXPlus_Z_180;
public String textBoxYPlus_X_0,textBoxYPlus_X_180,textBoxYPlus_Y_0,textBoxYPlus_Y_180,textBoxYPlus_Z_0,textBoxYPlus_Z_180;
public String textBoxZPlus_X_0,textBoxZPlus_X_180,textBoxZPlus_Y_0,textBoxZPlus_Y_180,textBoxZPlus_Z_0,textBoxZPlus_Z_180;
public String textBoxXMinus_X_0,textBoxXMinus_X_180,textBoxXMinus_Y_0,textBoxXMinus_Y_180,textBoxXMinus_Z_0,textBoxXMinus_Z_180;
public String textBoxYMinus_X_0,textBoxYMinus_X_180,textBoxYMinus_Y_0,textBoxYMinus_Y_180,textBoxYMinus_Z_0,textBoxYMinus_Z_180;
public String textBoxZMinus_X_0,textBoxZMinus_X_180,textBoxZMinus_Y_0,textBoxZMinus_Y_180,textBoxZMinus_Z_0,textBoxZMinus_Z_180;
void setup() {
textBoxXPlus_X_0 = "-836";
textBoxXPlus_X_180 = "-866";
textBoxXPlus_Y_0 = "-191";
textBoxXPlus_Y_180 = "404";
textBoxXPlus_Z_0 = "15";
textBoxXPlus_Z_180 = "-258";
textBoxYPlus_X_0 = "-523";
textBoxYPlus_X_180 = "205";
textBoxYPlus_Y_0 = "-576";
textBoxYPlus_Y_180 = "-716";
textBoxYPlus_Z_0 = "-267";
textBoxYPlus_Z_180 = "16";
textBoxZPlus_X_0 = "-481";
textBoxZPlus_X_180 = "166";
textBoxZPlus_Y_0 = "208";
textBoxZPlus_Y_180 = "-101";
textBoxZPlus_Z_0 = "-743";
textBoxZPlus_Z_180 = "-821";
textBoxXMinus_X_0 = "504";
textBoxXMinus_X_180 = "558";
textBoxXMinus_Y_0 = "-243";
textBoxXMinus_Y_180 = "350";
textBoxXMinus_Z_0 = "-321";
textBoxXMinus_Z_180 = "-16";
textBoxYMinus_X_0 = "-461";
textBoxYMinus_X_180 = "169";
textBoxYMinus_Y_0 = "747";
textBoxYMinus_Y_180 = "765";
textBoxYMinus_Z_0 = "7";
textBoxYMinus_Z_180 = "-334";
textBoxZMinus_X_0 = "-538";
textBoxZMinus_X_180 = "163";
textBoxZMinus_Y_0 = "-98";
textBoxZMinus_Y_180 = "364";
textBoxZMinus_Z_0 = "492";
textBoxZMinus_Z_180 = "503";
}
void draw(){
calculate_transformation_matrix();
}
public void calculate_transformation_matrix()
{
//Axis X--------------------------------------------------------------------------------------------------
double[] Xplus_center = new double[3];
//Centers of the circles
Xplus_center[0] = (Double.parseDouble(textBoxXPlus_X_0) + Double.parseDouble(textBoxXPlus_X_180)) / 2;
Xplus_center[1] = (Double.parseDouble(textBoxXPlus_Y_0) + Double.parseDouble(textBoxXPlus_Y_180)) / 2;
Xplus_center[2] = (Double.parseDouble(textBoxXPlus_Z_0) + Double.parseDouble(textBoxXPlus_Z_180)) / 2;
//Centers of the circles
double[] Xminus_center = new double[3];
Xminus_center[0] = (Double.parseDouble(textBoxXMinus_X_0) + Double.parseDouble(textBoxXMinus_X_180)) / 2;
Xminus_center[1] = (Double.parseDouble(textBoxXMinus_Y_0) + Double.parseDouble(textBoxXMinus_Y_180)) / 2;
Xminus_center[2] = (Double.parseDouble(textBoxXMinus_Z_0) + Double.parseDouble(textBoxXMinus_Z_180)) / 2;
//Vector from the center of minus circle to the center of plus circle
double[] Xvector = new double[3];
Xvector[0] = Xplus_center[0] - Xminus_center[0];
Xvector[1] = Xplus_center[1] - Xminus_center[1];
Xvector[2] = Xplus_center[2] - Xminus_center[2];
//Axis Y--------------------------------------------------------------------------------------------------
double[] Yplus_center = new double[3];
//Centers of the circles
Yplus_center[0] = (Double.parseDouble(textBoxYPlus_X_0) + Double.parseDouble(textBoxYPlus_X_180)) / 2;
Yplus_center[1] = (Double.parseDouble(textBoxYPlus_Y_0) + Double.parseDouble(textBoxYPlus_Y_180)) / 2;
Yplus_center[2] = (Double.parseDouble(textBoxYPlus_Z_0) + Double.parseDouble(textBoxYPlus_Z_180)) / 2;
//Centers of the circles
double[] Yminus_center = new double[3];
Yminus_center[0] = (Double.parseDouble(textBoxYMinus_X_0) + Double.parseDouble(textBoxYMinus_X_180)) / 2;
Yminus_center[1] = (Double.parseDouble(textBoxYMinus_Y_0) + Double.parseDouble(textBoxYMinus_Y_180)) / 2;
Yminus_center[2] = (Double.parseDouble(textBoxYMinus_Z_0) + Double.parseDouble(textBoxYMinus_Z_180)) / 2;
//Vector from the center of minus circle to the center of plus circle
double[] Yvector = new double[3];
Yvector[0] = Yplus_center[0] - Yminus_center[0];
Yvector[1] = Yplus_center[1] - Yminus_center[1];
Yvector[2] = Yplus_center[2] - Yminus_center[2];
//Axis Z--------------------------------------------------------------------------------------------------
double[] Zplus_center = new double[3];
//Centers of the circles
Zplus_center[0] = (Double.parseDouble(textBoxZPlus_X_0) + Double.parseDouble(textBoxZPlus_X_180)) / 2;
Zplus_center[1] = (Double.parseDouble(textBoxZPlus_Y_0) + Double.parseDouble(textBoxZPlus_Y_180)) / 2;
Zplus_center[2] = (Double.parseDouble(textBoxZPlus_Z_0) + Double.parseDouble(textBoxZPlus_Z_180)) / 2;
//Centers of the circles
double[] Zminus_center = new double[3];
Zminus_center[0] = (Double.parseDouble(textBoxZMinus_X_0) + Double.parseDouble(textBoxZMinus_X_180)) / 2;
Zminus_center[1] = (Double.parseDouble(textBoxZMinus_Y_0) + Double.parseDouble(textBoxZMinus_Y_180)) / 2;
Zminus_center[2] = (Double.parseDouble(textBoxZMinus_Z_0) + Double.parseDouble(textBoxZMinus_Z_180)) / 2;
//Vector from the center of minus circle to the center of plus circle
double[] Zvector = new double[3];
Zvector[0] = Zplus_center[0] - Zminus_center[0];
Zvector[1] = Zplus_center[1] - Zminus_center[1];
Zvector[2] = Zplus_center[2] - Zminus_center[2];
// Rotation matrix--------------------------------------------------------------------------------------
// rotation_matrix[a][b], a - number of the rows, b - number of the columbs
double[][] rotation_matrix = new double[3][];
rotation_matrix[0] = new double[3];
rotation_matrix[1] = new double[3];
rotation_matrix[2] = new double[3];
//Deviding by main value, for example for X axis - deviding by X coordinate, for Y axis by Y coordinate, for Z axis by Z cordinate
rotation_matrix[0][0] = Xvector[0] / Xvector[0]; rotation_matrix[0][1] = Yvector[0] / Yvector[1]; rotation_matrix[0][2] = Zvector[0] / Zvector[2];
rotation_matrix[1][0] = Xvector[1] / Xvector[0]; rotation_matrix[1][1] = Yvector[1] / Yvector[1]; rotation_matrix[1][2] = Zvector[1] / Zvector[2];
rotation_matrix[2][0] = Xvector[2] / Xvector[0]; rotation_matrix[2][1] = Yvector[2] / Yvector[1]; rotation_matrix[2][2] = Zvector[2] / Zvector[2];
//Matrix inversion
rotation_matrix = InvertMatrix(rotation_matrix);
//Determinating of the corrected by ratation matrix centers of the circles
Xplus_center = MatrixVectorMultiply(rotation_matrix, Xplus_center);
Xminus_center = MatrixVectorMultiply(rotation_matrix, Xminus_center);
Yplus_center = MatrixVectorMultiply(rotation_matrix, Yplus_center);
Yminus_center = MatrixVectorMultiply(rotation_matrix, Yminus_center);
Zplus_center = MatrixVectorMultiply(rotation_matrix, Zplus_center);
Zminus_center = MatrixVectorMultiply(rotation_matrix, Zminus_center);
//Determinating of the elipsoid center---------------------------------------------------------------------------
double[] center = new double[3];
center[0] = (Xplus_center[0] + Xminus_center[0] + Yplus_center[0] + Yminus_center[0] + Zplus_center[0] + Zminus_center[0]) / 6;
center[1] = (Xplus_center[1] + Xminus_center[1] + Yplus_center[1] + Yminus_center[1] + Zplus_center[1] + Zminus_center[1]) / 6;
center[2] = (Xplus_center[2] + Xminus_center[2] + Yplus_center[2] + Yminus_center[2] + Zplus_center[2] + Zminus_center[2]) / 6;
//Determinating of the radius of the future sphere-----------------------------------------------------------------------
double x_length = Math.abs(Xplus_center[0] - Xminus_center[0])/2;
double y_length = Math.abs(Yplus_center[1] - Yminus_center[1])/2;
double z_length = Math.abs(Zplus_center[2] - Zminus_center[2])/2;
double[] Xplus_0 = new double[3];
Xplus_0[0] = Double.parseDouble(textBoxXPlus_X_0);
Xplus_0[1] = Double.parseDouble(textBoxXPlus_Y_0);
Xplus_0[2] = Double.parseDouble(textBoxXPlus_Z_0);
Xplus_0 = MatrixVectorMultiply(rotation_matrix, Xplus_0);
double[] Yplus_0 = new double[3];
Yplus_0[0] = Double.parseDouble(textBoxYPlus_X_0);
Yplus_0[1] = Double.parseDouble(textBoxYPlus_Y_0);
Yplus_0[2] = Double.parseDouble(textBoxYPlus_Z_0);
Yplus_0 = MatrixVectorMultiply(rotation_matrix, Yplus_0);
double[] Zplus_0 = new double[3];
Zplus_0[0] = Double.parseDouble(textBoxZPlus_X_0);
Zplus_0[1] = Double.parseDouble(textBoxZPlus_Y_0);
Zplus_0[2] = Double.parseDouble(textBoxZPlus_Z_0);
Zplus_0 = MatrixVectorMultiply(rotation_matrix, Zplus_0);
double x_abs = Math.sqrt(x_length * x_length + Xplus_0[1] * Xplus_0[1] + Xplus_0[2] * Xplus_0[2]);
double y_abs = Math.sqrt(Yplus_0[0] * Yplus_0[0] + y_length * y_length + Yplus_0[2] * Yplus_0[2]);
double z_abs = Math.sqrt(Zplus_0[0] * Zplus_0[0] + Zplus_0[1] * Zplus_0[1] + z_length * z_length);
//sphere radius
double sphere_radius = (x_abs + y_abs + z_abs) / 3;
//Scales for the each axis------------------------------------------------
//Diameter of the sphere
double diameter = sphere_radius * 2;
double kx = Math.abs(diameter / (Xplus_center[0] - Xminus_center[0]));
double ky = Math.abs(diameter / (Yplus_center[1] - Yminus_center[1]));
double kz = Math.abs(diameter / (Zplus_center[2] - Zminus_center[2]));
//Multiplying elements of matrix by scales
rotation_matrix[0][0] = rotation_matrix[0][0] * kx; rotation_matrix[0][1] = rotation_matrix[0][1] * ky; rotation_matrix[0][2] = rotation_matrix[0][2] * kz;
rotation_matrix[1][0] = rotation_matrix[1][0] * kx; rotation_matrix[1][1] = rotation_matrix[1][1] * ky; rotation_matrix[1][2] = rotation_matrix[1][2] * kz;
rotation_matrix[2][0] = rotation_matrix[2][0] * kx; rotation_matrix[2][1] = rotation_matrix[2][1] * ky; rotation_matrix[2][2] = rotation_matrix[2][2] * kz;
//Bias
double[] bias = new double[3];
bias[0] = center[0];
bias[1] = center[1];
bias[2] = center[2];
//Indication
//Transformation matrix
/* textBox_matrixX_x.Text = rotation_matrix[0][0].ToString("0.ddd"); textBox_matrixY_x.Text = rotation_matrix[0][1].ToString("0.###"); textBox_matrixZ_x.Text = rotation_matrix[0][2].ToString("0.###");
textBox_matrixX_y.Text = rotation_matrix[1][0].ToString("0.###"); textBox_matrixY_y.Text = rotation_matrix[1][1].ToString("0.###"); textBox_matrixZ_y.Text = rotation_matrix[1][2].ToString("0.###");
textBox_matrixX_z.Text = rotation_matrix[2][0].ToString("0.###"); textBox_matrixY_z.Text = rotation_matrix[2][1].ToString("0.###"); textBox_matrixZ_z.Text = rotation_matrix[2][2].ToString("0.###");
//Bias
textBox_biasX.Text = bias[0].ToString("0.###");
textBox_biasY.Text = bias[1].ToString("0.###");
textBox_biasZ.Text = bias[2].ToString("0.###"); */
}
public static double[] MatrixVectorMultiply(double[][] matrixA, double[] vectorB)
{
int aRows = matrixA.length; int aCols = matrixA[0].length;
int bRows = vectorB.length;
// if (aCols != bRows)
// throw new Exception("Non-conformable matrices in MatrixProduct");
double[] result = new double[aRows];
for (int i = 0; i < aRows; ++i) // each row of A
for (int k = 0; k < aCols; ++k)
result[i] += matrixA[i][k] * vectorB[k];
return result;
}
public static double[][] InvertMatrix(double[][] A)
{
int n = A.length;
//e will represent each column in the identity matrix
double[] e;
//x will hold the inverse matrix to be returned
double[][] x = new double[n][];
for (int i = 0; i < n; i++)
{
x[i] = new double[A[i].length];
}
/*
* solve will contain the vector solution for the LUP decomposition as we solve
* for each vector of x. We will combine the solutions into the double[][] array x.
* */
double[] solve;
//Get the LU matrix and P matrix (as an array)
// Tuple<double[][], int[]> results = LUPDecomposition(A);
Pair<double[][], int[]>results = Pair.with(LUPDecomposition.getValue0(),LUPDecomposition.getValue1());
// double[][] LU = results.Item1;
double[][] LU = results.getValue0();
// int[] P = results.Item2;
int[] P = results.getValue1();
/*
* Solve AX = e for each column ei of the identity matrix using LUP decomposition
* */
for (int i = 0; i < n; i++)
{
e = new double[A[i].length];
e[i] = 1;
solve = LUPSolve(LU, P, e);
for (int j = 0; j < solve.length; j++)
{
x[j][i] = solve[j];
}
}
return x;
}
public static double[] LUPSolve(double[][] LU, int[] pi, double[] b)
{
int n = LU.length - 1;
double[] x = new double[n + 1];
double[] y = new double[n + 1];
double suml = 0;
double sumu = 0;
double lij = 0;
/*
* Solve for y using formward substitution
* */
for (int i = 0; i <= n; i++)
{
suml = 0;
for (int j = 0; j <= i - 1; j++)
{
/*
* Since we've taken L and U as a singular matrix as an input
* the value for L at index i and j will be 1 when i equals j, not LU[i][j], since
* the diagonal values are all 1 for L.
* */
if (i == j)
{
lij = 1;
}
else
{
lij = LU[i][j];
}
suml = suml + (lij * y[j]);
}
y[i] = b[pi[i]] - suml;
}
//Solve for x by using back substitution
for (int i = n; i >= 0; i--)
{
sumu = 0;
for (int j = i + 1; j <= n; j++)
{
sumu = sumu + (LU[i][j] * x[j]);
}
x[i] = (y[i] - sumu) / LU[i][i];
}
return x;
}
///////////// public static Tuple<double[][], int[]> LUPDecomposition(double[][] A)
public static Pair<double[][], int[]>LUPDecomposition = Pair.with(A, B);
{
int n = A.length - 1;
/*
* pi represents the permutation matrix. We implement it as an array
* whose value indicates which column the 1 would appear. We use it to avoid
* dividing by zero or small numbers.
* */
int[] pi = new int[n + 1];
double p = 0;
int kp = 0;
int pik = 0;
int pikp = 0;
double aki = 0;
double akpi = 0;
//Initialize the permutation matrix, will be the identity matrix
for (int j = 0; j <= n; j++)
{
pi[j] = j;
}
for (int k = 0; k <= n; k++)
{
/*
* In finding the permutation matrix p that avoids dividing by zero
* we take a slightly different approach. For numerical stability
* We find the element with the largest
* absolute value of those in the current first column (column k). If all elements in
* the current first column are zero then the matrix is singluar and throw an
* error.
* */
p = 0;
for (int i = k; i <= n; i++)
{
if (Math.abs(A[i][k]) > p)
{
p = Math.abs(A[i][k]);
kp = i;
}
}
if (p == 0)
{
// throw new Exception("singular matrix");
}
/*
* These lines update the pivot array (which represents the pivot matrix)
* by exchanging pi[k] and pi[kp].
* */
pik = pi[k];
pikp = pi[kp];
pi[k] = pikp;
pi[kp] = pik;
/*
* Exchange rows k and kpi as determined by the pivot
* */
for (int i = 0; i <= n; i++)
{
aki = A[k][i];
akpi = A[kp][i];
A[k][i] = akpi;
A[kp][i] = aki;
}
/*
* Compute the Schur complement
* */
for (int i = k + 1; i <= n; i++)
{
A[i][k] = A[i][k] / A[k][k];
for (int j = k + 1; j <= n; j++)
{
A[i][j] = A[i][j] - (A[i][k] * A[k][j]);
}
}
}
// return
Pair<double[][],int[]> ReturnTuple = Pair.with(A,pi);
println(ReturnTuple);
}