/**
* Extracts the epipoles from the trifocal tensor. Extracted epipoles will have a norm of 1 as an
* artifact of using SVD.
*
* @param tensor Input: Trifocal tensor. Not Modified
* @param e2 Output: Epipole in image 2. Homogeneous coordinates. Modified
* @param e3 Output: Epipole in image 3. Homogeneous coordinates. Modified
*/
public void process(TrifocalTensor tensor, Point3D_F64 e2, Point3D_F64 e3) {
svd.decompose(tensor.T1);
SingularOps.nullVector(svd, true, v1);
SingularOps.nullVector(svd, false, u1);
svd.decompose(tensor.T2);
SingularOps.nullVector(svd, true, v2);
SingularOps.nullVector(svd, false, u2);
svd.decompose(tensor.T3);
SingularOps.nullVector(svd, true, v3);
SingularOps.nullVector(svd, false, u3);
for (int i = 0; i < 3; i++) {
U.set(i, 0, u1.get(i));
U.set(i, 1, u2.get(i));
U.set(i, 2, u3.get(i));
V.set(i, 0, v1.get(i));
V.set(i, 1, v2.get(i));
V.set(i, 2, v3.get(i));
}
svd.decompose(U);
SingularOps.nullVector(svd, false, tempE);
e2.set(tempE.get(0), tempE.get(1), tempE.get(2));
svd.decompose(V);
SingularOps.nullVector(svd, false, tempE);
e3.set(tempE.get(0), tempE.get(1), tempE.get(2));
}
@DeployableTestMethod(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testConstrainedSimple() {
int objectiveSize = 3;
int solutionSize = 3;
int constraintSize = 1;
DenseMatrix64F a = new DenseMatrix64F(objectiveSize, solutionSize);
CommonOps.setIdentity(a);
DenseMatrix64F b = new DenseMatrix64F(objectiveSize, 1);
b.zero();
DenseMatrix64F c = new DenseMatrix64F(constraintSize, solutionSize);
CommonOps.setIdentity(c);
DenseMatrix64F d = new DenseMatrix64F(constraintSize, 1);
d.set(0, 0, -1.0);
QuadraticProgram quadraticProgram = new QuadraticProgram(a, b, c, d);
DenseMatrix64F initialGuess = new DenseMatrix64F(solutionSize, 1);
initialGuess.set(0, 0, -10.0);
initialGuess.set(1, 0, -10.0);
initialGuess.set(2, 0, -10.0);
ActiveSearchSolutionInfo solutionInfo = solve(quadraticProgram, initialGuess);
assertTrue(solutionInfo.isConverged());
DenseMatrix64F expectedResult = new DenseMatrix64F(solutionSize, 1);
expectedResult.set(0, 0, d.get(0, 0));
expectedResult.set(1, 0, 0.0);
expectedResult.set(2, 0, 0.0);
assertTrue(MatrixFeatures.isEquals(expectedResult, solutionInfo.getSolution(), 1e-12));
}
private void compute(ValkyrieJointInterface[] jnt_data) {
double q1 = jnt_data[1].getPosition();
double q2 = jnt_data[0].getPosition();
double cq1 = Math.cos(q1);
double sq1 = Math.sin(q1);
double cq2 = Math.cos(q2);
double sq2 = Math.sin(q2);
double c2q1 = Math.cos(2.0 * q1);
double c2q2 = Math.cos(2.0 * q2);
double s2q1 = Math.sin(2.0 * q1);
double a1 = computeALeft();
double b1 = computeBLeft(cq1, sq1, cq2, sq2);
double c1 = computeCLeft(cq1, sq1, cq2, sq2);
double a2 = computeARight();
double b2 = computeBRight(cq1, sq1, cq2, sq2);
double c2 = computeCRight(cq1, sq1, cq2, sq2);
double Db1Dq1 = computeDb1Dq1(cq1, sq1, sq2);
double Db1Dq2 = computeDb1Dq2(cq1, cq2, sq2);
double Dc1Dq1 = computeDc1Dq1(cq1, sq1, sq2, c2q1, c2q2, s2q1);
double Dc1Dq2 = computeDc1Dq2(cq1, sq1, cq2, sq2, c2q1, s2q1);
double Db2Dq1 = computeDb2Dq1(cq1, sq1, sq2);
double Db2Dq2 = computeDb1Dq2(cq1, cq2, sq2);
double Dc2Dq1 = computeDc2Dq1(cq1, sq1, sq2, c2q1, c2q2, s2q1);
double Dc2Dq2 = computeDc2Dq2(cq1, sq1, cq2, sq2, c2q1);
double x1 = computePushrodPosition(a1, b1, c1);
double x2 = computePushrodPosition(a2, b2, c2);
pushrodPositions.set(0, 0, x1);
pushrodPositions.set(1, 0, x2);
double velocityMap00 = computeVelocityMapCoefficient(Db1Dq1, a1, b1, Dc1Dq1, c1);
double velocityMap01 = computeVelocityMapCoefficient(Db1Dq2, a1, b1, Dc1Dq2, c1);
double velocityMap10 = computeVelocityMapCoefficient(Db2Dq1, a2, b2, Dc2Dq1, c2);
double velocityMap11 = computeVelocityMapCoefficient(Db2Dq2, a2, b2, Dc2Dq2, c2);
jacobian.set(0, 0, velocityMap00);
jacobian.set(0, 1, velocityMap01);
jacobian.set(1, 0, velocityMap10);
jacobian.set(1, 1, velocityMap11);
double pushrodForceToJointTorqueMap00 =
computePushrodForceToJointTorqueMap00(cq1, sq1, sq2, x1);
double pushrodForceToJointTorqueMap01 =
computePushrodForceToJointTorqueMap01(cq1, sq1, sq2, x2);
double pushrodForceToJointTorqueMap10 =
computePushrodForceToJointTorqueMap10(cq1, sq1, cq2, sq2, x1);
double pushrodForceToJointTorqueMap11 =
computePushrodForceToJointTorqueMap11(cq1, sq1, cq2, sq2, x2);
jacobianTranspose.set(0, 0, pushrodForceToJointTorqueMap00);
jacobianTranspose.set(0, 1, pushrodForceToJointTorqueMap01);
jacobianTranspose.set(1, 0, pushrodForceToJointTorqueMap10);
jacobianTranspose.set(1, 1, pushrodForceToJointTorqueMap11);
}
/**
* Sets the values in the specified matrix to a rotation matrix about the z-axis.
*
* @param ang the angle it rotates a point by in radians.
* @param r A 3 by 3 matrix. Is modified.
*/
public static void setRotZ(float ang, DenseMatrix64F r) {
float c = (float) Math.cos(ang);
float s = (float) Math.sin(ang);
r.set(0, 0, c);
r.set(0, 1, -s);
r.set(1, 0, s);
r.set(1, 1, c);
r.set(2, 2, 1);
}
/**
* Sets the values in the specified matrix to a rotation matrix about the x-axis.
*
* @param ang the angle it rotates a point by in radians.
* @param R (Output) Storage for rotation matrix. Modified.
*/
public static void setRotX(float ang, DenseMatrix64F R) {
float c = (float) Math.cos(ang);
float s = (float) Math.sin(ang);
R.set(0, 0, 1);
R.set(1, 1, c);
R.set(1, 2, -s);
R.set(2, 1, s);
R.set(2, 2, c);
}
/**
* 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 IllegalArgumentException("Unexpected sample size");
if (sampleIndex >= A.getNumRows()) throw new IllegalArgumentException("Too many samples");
for (int i = 0; i < sampleData.length; i++) {
A.set(sampleIndex, i, sampleData[i]);
}
sampleIndex++;
}
public static DenseMatrix64F numDiff2d(MatrixChains chains, double[] angles, double epsilon) {
int l = angles.length;
DenseMatrix64F togo = new DenseMatrix64F(l, l);
for (int i = 0; i < l; ++i) {
for (int j = 0; j < l; ++j) {
togo.set(i, j, numDiff(chains, angles, i, j, epsilon));
}
}
return togo;
}
private void initPower(DenseMatrix64F A) {
if (A.numRows != A.numCols) throw new IllegalArgumentException("A must be a square matrix.");
if (seed != null) {
q0.set(seed);
} else {
for (int i = 0; i < A.numRows; i++) {
q0.data[i] = 1;
}
}
}
@Override
public void actuatorToJointEffort(LinearActuator[] act_data, ValkyrieJointInterface[] jnt_data) {
compute(jnt_data);
pushrodForces.set(0, 0, act_data[0].getEffort());
pushrodForces.set(1, 0, act_data[1].getEffort());
// CommonOps.multTransA(jacobian, pushrodForces, jointTorques);
CommonOps.mult(jacobianTranspose, pushrodForces, jointTorques);
jnt_data[1].setEffort(jointTorques.get(0, 0));
jnt_data[0].setEffort(jointTorques.get(1, 0));
}
public static long fillManual(DenseMatrix64F mat, int numTrials) {
long prev = System.currentTimeMillis();
for (int i = 0; i < numTrials; i++) {
final int size = mat.getNumElements();
for (int j = 0; j < size; j++) {
mat.set(j, 2);
}
}
long curr = System.currentTimeMillis();
return curr - prev;
}
/**
* Compute rectification transforms for the stereo pair given a fundamental matrix and its
* observations.
*
* @param F Fundamental matrix
* @param observations Observations used to compute F
* @param width Width of first image.
* @param height Height of first image.
*/
public void process(DenseMatrix64F F, List<AssociatedPair> observations, int width, int height) {
int centerX = width / 2;
int centerY = height / 2;
MultiViewOps.extractEpipoles(F, epipole1, epipole2);
checkEpipoleInside(width, height);
// compute the transform H which will send epipole2 to infinity
SimpleMatrix R = rotateEpipole(epipole2, centerX, centerY);
SimpleMatrix T = translateToOrigin(centerX, centerY);
SimpleMatrix G = computeG(epipole2, centerX, centerY);
SimpleMatrix H = G.mult(R).mult(T);
// Find the two matching transforms
SimpleMatrix Hzero = computeHZero(F, epipole2, H);
SimpleMatrix Ha = computeAffineH(observations, H.getMatrix(), Hzero.getMatrix());
rect1.set(Ha.mult(Hzero).getMatrix());
rect2.set(H.getMatrix());
}
@Override
public void actuatorToJointVelocity(
LinearActuator[] act_data, ValkyrieJointInterface[] jnt_data) {
compute(jnt_data);
pushrodVelocities.set(0, 0, act_data[0].getVelocity());
pushrodVelocities.set(1, 0, act_data[1].getVelocity());
DenseMatrix64F jacobianInverse = new DenseMatrix64F(2, 2);
CommonOps.invert(jacobian, jacobianInverse);
CommonOps.mult(jacobianInverse, pushrodVelocities, jointVelocites);
jnt_data[1].setVelocity(-jointVelocites.get(0, 0));
jnt_data[0].setVelocity(-jointVelocites.get(1, 0));
}
// ValkyrieJointInterface[] jnt_data = [ pitch, roll ]
@Override
public void jointToActuatorEffort(LinearActuator[] act_data, ValkyrieJointInterface[] jnt_data) {
compute(jnt_data);
jointTorques.set(0, 0, jnt_data[1].getDesiredEffort());
jointTorques.set(1, 0, jnt_data[0].getDesiredEffort());
DenseMatrix64F jacobianTransposeInverse = new DenseMatrix64F(2, 2);
CommonOps.invert(jacobianTranspose, jacobianTransposeInverse);
CommonOps.mult(jacobianTransposeInverse, jointTorques, pushrodForces);
act_data[0].setEffortCommand(pushrodForces.get(0, 0));
act_data[1].setEffortCommand(pushrodForces.get(1, 0));
}
/**
* Loads the word-vector pairs stored in the file whose path is
* <code>filename</code> The file can be a plain text file or a .gz archive.
* <p>
* The expected format is: </br>
* number_of_vectors space_dimensionality</br>
* word_i [TAB] 1.0 [TAB] 0 [TAB] vector values comma
* separated </code> </br>
* </br>
* Example: </br>
* </br>
* <code> 3 5</br>
* dog::n [TAB] 1.0 [TAB] 0 [TAB] 2.1,4.1,1.4,2.3,0.9</br>
* cat::n [TAB] 1.0 [TAB] 0 [TAB] 3.2,4.3,1.2,2.2,0.8</br>
* mouse::n [TAB] 1.0 [TAB] 0 [TAB] 2.4,4.4,2.4,1.3,0.92</br>
*
*
* @param filename
* the path of the file containing the word-vector pairs
* @throws IOException
*/
private void populate(String filename) throws IOException {
BufferedReader br = null;
GZIPInputStream gzis = null;
if (filename.endsWith(".gz")) {
gzis = new GZIPInputStream(new FileInputStream(filename));
InputStreamReader reader = new InputStreamReader(gzis, "UTF8");
br = new BufferedReader(reader);
} else {
br = new BufferedReader(new InputStreamReader(new FileInputStream(filename), "UTF8"));
}
String line;
ArrayList<String> split;
String label;
String[] vSplit;
Pattern iPattern = Pattern.compile(",");
float[] v = null;
while ((line = br.readLine()) != null) {
if (!line.contains("\t")) continue;
float norm2 = 0;
split = mySplit(line);
label = split.get(0);
vSplit = iPattern.split(split.get(3), 0);
if (v == null) v = new float[vSplit.length];
for (int i = 0; i < v.length; i++) {
v[i] = Float.parseFloat(vSplit[i]);
norm2 += v[i] * v[i];
}
float norm = (float) Math.sqrt(norm2);
for (int i = 0; i < v.length; i++) {
v[i] /= norm;
}
DenseMatrix64F featureVector = new DenseMatrix64F(1, v.length);
for (int i = 0; i < v.length; i++) {
featureVector.set(0, i, (double) v[i]);
}
DenseVector denseFeatureVector = new DenseVector(featureVector);
addWordVector(label, denseFeatureVector);
}
if (filename.endsWith(".gz")) {
gzis.close();
}
br.close();
}
// Produce 2nd derivative of objective function at specified angle setting
public static DenseMatrix64F get2d(MatrixChains chain, double[] angles) {
if (angles == null) // yingdan
return null;
int dim = angles.length;
DenseMatrix64F togo = new DenseMatrix64F(dim, dim);
for (int i = 0; i < dim; ++i) {
for (int j = 0; j < dim; ++j) {
double v = df2by(chain, angles, i, j);
if (chain.regularise > 0 && i == j) {
v += 2 * chain.regularise;
}
togo.set(i, j, v);
}
}
return togo;
}
public static OptimisationReport NiuDunLaFuSunStep(
MatrixChains chain, double[] angles, boolean printStep) {
// see: http://code.google.com/p/efficient-java-matrix-library/wiki/SolvingLinearSystems
int l = angles.length;
DenseMatrix64F J = get2d(chain, angles);
DenseMatrix64F grad = get1d(chain, angles);
DenseMatrix64F propose = vector(l);
boolean solved = CommonOps.solve(J, grad, propose);
for (int i = 0; i < l; ++i) {
if (Double.isNaN(propose.data[i])) {
solved = false;
}
}
OptimisationReport togo = new OptimisationReport();
try {
if (!solved) {
throw new RuntimeException("Failed to solve update equation");
}
CommonOps.scale(-1, propose);
if (printStep) {
EigenDecomposition<DenseMatrix64F> decomp = DecompositionFactory.eigSymm(l, false);
boolean posed = decomp.decompose(J);
if (!posed) {
throw new RuntimeException("Failed to decompose matrix");
}
double[] eigs = eigs(decomp);
System.out.println("Computed eigenvalues " + printVec(eigs));
togo.eigenvalues = eigs;
}
tryProposal(chain, propose, grad, angles, printStep, togo);
} catch (Exception e) {
System.out.println(
"Failed to find suitable proposal from Newton step - trying gradient step");
propose.set(grad);
CommonOps.scale(-1, propose);
tryProposal(chain, propose, grad, angles, printStep, togo);
}
return togo;
}
/**
* Computes a basis (the principle components) from the most dominant eigenvectors.
*
* @param numComponents Number of vectors it will use to describe the data. Typically much smaller
* than the number of elements in the input vector.
*/
public void computeBasis(int numComponents) {
if (numComponents > A.getNumCols())
throw new IllegalArgumentException("More components requested that the data's length.");
if (sampleIndex != A.getNumRows())
throw new IllegalArgumentException("Not all the data has been added");
if (numComponents > sampleIndex)
throw new IllegalArgumentException(
"More data needed to compute the desired number of components");
this.numComponents = numComponents;
// compute the mean of all the samples
for (int i = 0; i < A.getNumRows(); i++) {
for (int j = 0; j < mean.length; j++) {
mean[j] += A.get(i, j);
}
}
for (int j = 0; j < mean.length; j++) {
mean[j] /= A.getNumRows();
}
// subtract the mean from the original data
for (int i = 0; i < A.getNumRows(); i++) {
for (int j = 0; j < mean.length; j++) {
A.set(i, j, A.get(i, j) - mean[j]);
}
}
// Compute SVD and save time by not computing U
SingularValueDecomposition<DenseMatrix64F> svd =
DecompositionFactory.svd(A.numRows, A.numCols, false, true, false);
if (!svd.decompose(A)) throw new RuntimeException("SVD failed");
V_t = svd.getV(null, true);
DenseMatrix64F W = svd.getW(null);
// Singular values are in an arbitrary order initially
SingularOps.descendingOrder(null, false, W, V_t, true);
// strip off unneeded components and find the basis
V_t.reshape(numComponents, mean.length, true);
}
@Test
public void _solveVectorInternal() {
int width = 10;
DenseMatrix64F LU = RandomMatrices.createRandom(width, width, rand);
double a[] = new double[] {1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
double b[] = new double[] {1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
for (int i = 0; i < width; i++) LU.set(i, i, 1);
TriangularSolver.solveL(LU.data, a, width);
TriangularSolver.solveU(LU.data, a, width);
DebugDecompose alg = new DebugDecompose(width);
for (int i = 0; i < width; i++) alg.getIndx()[i] = i;
alg.setLU(LU);
alg._solveVectorInternal(b);
for (int i = 0; i < width; i++) {
assertEquals(a[i], b[i], 1e-6);
}
}
/**
* Converts a unit quaternion into a rotation matrix.
*
* <p>Equations is taken from: Paul J. Besl and Neil D. McKay, "A Method for Registration of 3-D
* Shapes" IEEE Transactions on Pattern Analysis and Machine Intelligence, Vol 14, No. 2, Feb.
* 1992
*
* @param quat Unit quaternion.
* @param R Storage for rotation matrix. If null a new matrix is created. Modified.
* @return Rotation matrix
*/
public static DenseMatrix64F quaternionToMatrix(Quaternion_F32 quat, DenseMatrix64F R) {
R = checkDeclare3x3(R);
final float q0 = quat.w;
final float q1 = quat.x;
final float q2 = quat.y;
final float q3 = quat.z;
R.set(0, 0, q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3);
R.set(0, 1, 2.0f * (q1 * q2 - q0 * q3));
R.set(0, 2, 2.0f * (q1 * q3 + q0 * q2));
R.set(1, 0, 2.0f * (q1 * q2 + q0 * q3));
R.set(1, 1, q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3);
R.set(1, 2, 2.0f * (q2 * q3 - q0 * q1));
R.set(2, 0, 2.0f * (q1 * q3 - q0 * q2));
R.set(2, 1, 2.0f * (q2 * q3 + q0 * q1));
R.set(2, 2, q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3);
return R;
}
public void setDesiredPosition(DenseMatrix64F q, int[] indices) {
if (indices.length != 7)
throw new RuntimeException("Unexpected number of indices: " + Arrays.toString(indices));
desiredConfiguration.reshape(7, 1);
for (int i = 0; i < indices.length; i++) desiredConfiguration.set(i, 0, q.get(indices[i], 0));
}
/**
* Complete the information held in this using other. Does not overwrite the data already set in
* this.
*/
public void completeWith(RootJointDesiredConfigurationData other) {
if (!hasDesiredConfiguration()) desiredConfiguration.set(other.desiredConfiguration);
if (!hasDesiredVelocity()) desiredVelocity.set(other.desiredVelocity);
if (!hasDesiredAcceleration()) desiredAcceleration.set(other.desiredAcceleration);
}
@Override
public TDoubleArrayList[] values(
Coordinate pos,
List<? extends Coordinate> nodes,
TDoubleArrayList[] distSquares,
TDoubleArrayList infRads,
TDoubleArrayList[] results) {
if (null == distSquares) {
distSquares = this.distSquaresCache;
euclideanDistSqFun.setPosition(pos);
euclideanDistSqFun.sqValues(nodes, distSquares);
}
int ndsNum = nodes.size();
weightFunction.values(distSquares, infRads, nodesWeightsCache);
results = ShapeFunctionUtils2D.init4Output(results, diffOrder, ndsNum);
int diffDim = CommonUtils.len2DBase(diffOrder);
int baseDim = basesFunction.getDim();
Coordinate zero = new Coordinate(0, 0, 0);
Coordinate radCoord = new Coordinate(0, 0, 0);
for (int i = 0; i < diffDim; i++) {
As[i].zero();
ArrayList<TDoubleArrayList> tB = Bs.get(i);
for (int j = 0; j < baseDim; j++) {
TDoubleArrayList v = tB.get(j);
v.resetQuick();
v.ensureCapacity(ndsNum);
v.fill(0, ndsNum, 0);
}
}
basesFunction.setDiffOrder(0);
for (int diffDimIdx = 0; diffDimIdx < diffDim; diffDimIdx++) {
TDoubleArrayList weights_d = nodesWeightsCache[diffDimIdx];
DenseMatrix64F A_d = As[diffDimIdx];
ArrayList<TDoubleArrayList> B_d = Bs.get(diffDimIdx);
double[] tp = ps_arr[0];
int ndIdx = 0;
for (Coordinate nd : nodes) {
if (areBasesRelative) {
GeometryMath.minus(nd, pos, radCoord);
basesFunction.values(radCoord, ps_arr);
} else {
basesFunction.values(nd, ps_arr);
}
double weight_d = weights_d.getQuick(ndIdx);
for (int i = 0; i < baseDim; i++) {
double p_i = tp[i];
for (int j = 0; j < baseDim; j++) {
double p_ij = p_i * tp[j];
A_d.add(i, j, weight_d * p_ij);
}
B_d.get(i).set(ndIdx, weight_d * p_i);
}
ndIdx++;
}
}
basesFunction.setDiffOrder(diffOrder);
if (areBasesRelative) {
basesFunction.values(zero, ps_arr);
} else {
basesFunction.values(pos, ps_arr);
}
A_bak.set(A);
luSolver.setA(A_bak);
tv.set(p);
luSolver.solve(tv, gamma);
// CommonOps.solve(A, p, gamma);
ShapeFunctionUtils2D.multAddTo(gamma, B, results[0]);
if (diffOrder < 1) {
return results;
}
tv.zero();
CommonOps.mult(-1, A_x, gamma, tv);
CommonOps.add(p_x, tv, tv);
luSolver.solve(tv, gamma_x);
// CommonOps.solve(A, tv, gamma_x);
tv.zero();
CommonOps.mult(-1, A_y, gamma, tv);
CommonOps.add(p_y, tv, tv);
luSolver.solve(tv, gamma_y);
// CommonOps.solve(A, tv, gamma_y);
ShapeFunctionUtils2D.multAddTo(gamma_x, B, results[1]);
ShapeFunctionUtils2D.multAddTo(gamma, B_x, results[1]);
ShapeFunctionUtils2D.multAddTo(gamma_y, B, results[2]);
ShapeFunctionUtils2D.multAddTo(gamma, B_y, results[2]);
return results;
}
public void setDesiredAcceleration(DenseMatrix64F qdd, int[] indices) {
if (indices.length != 6)
throw new RuntimeException("Unexpected number of indices: " + Arrays.toString(indices));
desiredAcceleration.reshape(6, 1);
for (int i = 0; i < indices.length; i++) desiredAcceleration.set(i, 0, qdd.get(indices[i], 0));
}
public void add(Double value) {
if (currPtr < dimensions) {
values.set(currPtr, 0, value);
currPtr++;
}
}
public void set(TrifocalTensor a) {
T1.set(a.T1);
T2.set(a.T2);
T3.set(a.T3);
}
private void estimateCPM() {
logger.info("Start EJML Estimation");
numIterations = 0;
boolean estimationDone = false;
DenseMatrix64F eL_hat = null;
DenseMatrix64F eP_hat = null;
DenseMatrix64F rhsL = null;
DenseMatrix64F rhsP = null;
// normalize master coordinates for stability -- only master!
TDoubleArrayList yMasterNorm = new TDoubleArrayList();
TDoubleArrayList xMasterNorm = new TDoubleArrayList();
for (int i = 0; i < yMaster.size(); i++) {
yMasterNorm.add(PolyUtils.normalize2(yMaster.getQuick(i), normWin.linelo, normWin.linehi));
xMasterNorm.add(PolyUtils.normalize2(xMaster.getQuick(i), normWin.pixlo, normWin.pixhi));
}
// helper variables
int winL;
int winP;
int maxWSum_idx = 0;
while (!estimationDone) {
String codeBlockMessage = "LS ESTIMATION PROCEDURE";
StopWatch stopWatch = new StopWatch();
StopWatch clock = new StopWatch();
clock.start();
stopWatch.setTag(codeBlockMessage);
stopWatch.start();
logger.info("Start iteration: {}" + numIterations);
/** Remove identified outlier from previous estimation */
if (numIterations != 0) {
logger.info(
"Removing observation {}, idxList {}, from observation vector."
+ index.getQuick(maxWSum_idx)
+ maxWSum_idx);
index.removeAt(maxWSum_idx);
yMasterNorm.removeAt(maxWSum_idx);
xMasterNorm.removeAt(maxWSum_idx);
yOffset.removeAt(maxWSum_idx);
xOffset.removeAt(maxWSum_idx);
// only for outlier removal
yMaster.removeAt(maxWSum_idx);
xMaster.removeAt(maxWSum_idx);
ySlave.removeAt(maxWSum_idx);
xSlave.removeAt(maxWSum_idx);
coherence.removeAt(maxWSum_idx);
// also take care of slave pins
slaveGCPList.remove(maxWSum_idx);
// if (demRefinement) {
// ySlaveGeometry.removeAt(maxWSum_idx);
// xSlaveGeometry.removeAt(maxWSum_idx);
// }
}
/** Check redundancy */
numObservations = index.size(); // Number of points > threshold
if (numObservations < numUnknowns) {
logger.severe(
"coregpm: Number of windows > threshold is smaller than parameters solved for.");
throw new ArithmeticException(
"coregpm: Number of windows > threshold is smaller than parameters solved for.");
}
// work with normalized values
DenseMatrix64F A =
new DenseMatrix64F(
SystemOfEquations.constructDesignMatrix_loop(
yMasterNorm.toArray(), xMasterNorm.toArray(), cpmDegree));
logger.info("TIME FOR SETUP of SYSTEM : {}" + stopWatch.lap("setup"));
RowD1Matrix64F Qy_1; // vector
double meanValue;
switch (cpmWeight) {
case "linear":
logger.info("Using sqrt(coherence) as weights");
Qy_1 = DenseMatrix64F.wrap(numObservations, 1, coherence.toArray());
// Normalize weights to avoid influence on estimated var.factor
logger.info("Normalizing covariance matrix for LS estimation");
meanValue = CommonOps.elementSum(Qy_1) / numObservations;
CommonOps.divide(meanValue, Qy_1); // normalize vector
break;
case "quadratic":
logger.info("Using coherence as weights.");
Qy_1 = DenseMatrix64F.wrap(numObservations, 1, coherence.toArray());
CommonOps.elementMult(Qy_1, Qy_1);
// Normalize weights to avoid influence on estimated var.factor
meanValue = CommonOps.elementSum(Qy_1) / numObservations;
logger.info("Normalizing covariance matrix for LS estimation.");
CommonOps.divide(meanValue, Qy_1); // normalize vector
break;
case "bamler":
// TODO: see Bamler papers IGARSS 2000 and 2004
logger.warning("Bamler weighting method NOT IMPLEMENTED, falling back to None.");
Qy_1 = onesEJML(numObservations);
break;
case "none":
logger.info("No weighting.");
Qy_1 = onesEJML(numObservations);
break;
default:
Qy_1 = onesEJML(numObservations);
break;
}
logger.info("TIME FOR SETUP of VC diag matrix: {}" + stopWatch.lap("diag VC matrix"));
/** tempMatrix_1 matrices */
final DenseMatrix64F yL_matrix = DenseMatrix64F.wrap(numObservations, 1, yOffset.toArray());
final DenseMatrix64F yP_matrix = DenseMatrix64F.wrap(numObservations, 1, xOffset.toArray());
logger.info("TIME FOR SETUP of TEMP MATRICES: {}" + stopWatch.lap("Temp matrices"));
/** normal matrix */
final DenseMatrix64F N =
new DenseMatrix64F(numUnknowns, numUnknowns); // = A_transpose.mmul(Qy_1_diag.mmul(A));
/*
// fork/join parallel implementation
RowD1Matrix64F result = A.copy();
DiagXMat dd = new DiagXMat(Qy_1, A, 0, A.numRows, result);
ForkJoinPool pool = new ForkJoinPool();
pool.invoke(dd);
CommonOps.multAddTransA(A, dd.result, N);
*/
CommonOps.multAddTransA(A, diagxmat(Qy_1, A), N);
DenseMatrix64F Qx_hat = N.copy();
logger.info("TIME FOR SETUP of NORMAL MATRIX: {}" + stopWatch.lap("Normal matrix"));
/** right hand sides */
// azimuth
rhsL = new DenseMatrix64F(numUnknowns, 1); // A_transpose.mmul(Qy_1_diag.mmul(yL_matrix));
CommonOps.multAddTransA(1d, A, diagxmat(Qy_1, yL_matrix), rhsL);
// range
rhsP = new DenseMatrix64F(numUnknowns, 1); // A_transpose.mmul(Qy_1_diag.mmul(yP_matrix));
CommonOps.multAddTransA(1d, A, diagxmat(Qy_1, yP_matrix), rhsP);
logger.info("TIME FOR SETUP of RightHand Side: {}" + stopWatch.lap("Right-hand-side"));
LinearSolver<DenseMatrix64F> solver = LinearSolverFactory.leastSquares(100, 100);
/** compute solution */
if (!solver.setA(Qx_hat)) {
throw new IllegalArgumentException("Singular Matrix");
}
solver.solve(rhsL, rhsL);
solver.solve(rhsP, rhsP);
logger.info("TIME FOR SOLVING of System: {}" + stopWatch.lap("Solving System"));
/** inverting of Qx_hat for stability check */
solver.invert(Qx_hat);
logger.info("TIME FOR INVERSION OF N: {}" + stopWatch.lap("Inversion of N"));
/** test inversion and check stability: max(abs([N*inv(N) - E)) ?= 0 */
DenseMatrix64F tempMatrix_1 = new DenseMatrix64F(N.numRows, N.numCols);
CommonOps.mult(N, Qx_hat, tempMatrix_1);
CommonOps.subEquals(
tempMatrix_1, CommonOps.identity(tempMatrix_1.numRows, tempMatrix_1.numCols));
double maxDeviation = CommonOps.elementMaxAbs(tempMatrix_1);
if (maxDeviation > .01) {
logger.severe(
"COREGPM: maximum deviation N*inv(N) from unity = {}. This is larger than 0.01"
+ maxDeviation);
throw new IllegalStateException("COREGPM: maximum deviation N*inv(N) from unity)");
} else if (maxDeviation > .001) {
logger.warning(
"COREGPM: maximum deviation N*inv(N) from unity = {}. This is between 0.01 and 0.001"
+ maxDeviation);
}
logger.info("TIME FOR STABILITY CHECK: {}" + stopWatch.lap("Stability Check"));
logger.info("Coeffs in Azimuth direction: {}" + rhsL.toString());
logger.info("Coeffs in Range direction: {}" + rhsP.toString());
logger.info("Max Deviation: {}" + maxDeviation);
logger.info("System Quality: {}" + solver.quality());
/** some other stuff if the scale is okay */
DenseMatrix64F Qe_hat = new DenseMatrix64F(numObservations, numObservations);
DenseMatrix64F tempMatrix_2 = new DenseMatrix64F(numObservations, numUnknowns);
CommonOps.mult(A, Qx_hat, tempMatrix_2);
CommonOps.multTransB(-1, tempMatrix_2, A, Qe_hat);
scaleInputDiag(Qe_hat, Qy_1);
// solution: Azimuth
DenseMatrix64F yL_hat = new DenseMatrix64F(numObservations, 1);
eL_hat = new DenseMatrix64F(numObservations, 1);
CommonOps.mult(A, rhsL, yL_hat);
CommonOps.sub(yL_matrix, yL_hat, eL_hat);
// solution: Range
DenseMatrix64F yP_hat = new DenseMatrix64F(numObservations, 1);
eP_hat = new DenseMatrix64F(numObservations, 1);
CommonOps.mult(A, rhsP, yP_hat);
CommonOps.sub(yP_matrix, yP_hat, eP_hat);
logger.info("TIME FOR DATA preparation for TESTING: {}" + stopWatch.lap("Testing Setup"));
/** overal model test (variance factor) */
double overAllModelTest_L = 0;
double overAllModelTest_P = 0;
for (int i = 0; i < numObservations; i++) {
overAllModelTest_L += FastMath.pow(eL_hat.get(i), 2) * Qy_1.get(i);
overAllModelTest_P += FastMath.pow(eP_hat.get(i), 2) * Qy_1.get(i);
}
overAllModelTest_L =
(overAllModelTest_L / FastMath.pow(SIGMA_L, 2)) / (numObservations - numUnknowns);
overAllModelTest_P =
(overAllModelTest_P / FastMath.pow(SIGMA_P, 2)) / (numObservations - numUnknowns);
logger.info("Overall Model Test Lines: {}" + overAllModelTest_L);
logger.info("Overall Model Test Pixels: {}" + overAllModelTest_P);
logger.info("TIME FOR OMT: {}" + stopWatch.lap("OMT"));
/** ---------------------- DATASNOPING ----------------------------------- * */
/** Assumed Qy diag */
/** initialize */
DenseMatrix64F wTest_L = new DenseMatrix64F(numObservations, 1);
DenseMatrix64F wTest_P = new DenseMatrix64F(numObservations, 1);
for (int i = 0; i < numObservations; i++) {
wTest_L.set(i, eL_hat.get(i) / (Math.sqrt(Qe_hat.get(i, i)) * SIGMA_L));
wTest_P.set(i, eP_hat.get(i) / (Math.sqrt(Qe_hat.get(i, i)) * SIGMA_P));
}
/** find maxima's */
// azimuth
winL = absArgmax(wTest_L);
double maxWinL = Math.abs(wTest_L.get(winL));
logger.info(
"maximum wtest statistic azimuth = {} for window number: {} "
+ maxWinL
+ index.getQuick(winL));
// range
winP = absArgmax(wTest_P);
double maxWinP = Math.abs(wTest_P.get(winP));
logger.info(
"maximum wtest statistic range = {} for window number: {} "
+ maxWinP
+ index.getQuick(winP));
/** use summed wTest in Azimuth and Range direction for outlier detection */
DenseMatrix64F wTestSum = new DenseMatrix64F(numObservations);
for (int i = 0; i < numObservations; i++) {
wTestSum.set(i, FastMath.pow(wTest_L.get(i), 2) + FastMath.pow(wTest_P.get(i), 2));
}
maxWSum_idx = absArgmax(wTest_P);
double maxWSum = wTest_P.get(winP);
logger.info(
"Detected outlier: summed sqr.wtest = {}; observation: {}"
+ maxWSum
+ index.getQuick(maxWSum_idx));
/** Test if we are estimationDone yet */
// check on number of observations
if (numObservations <= numUnknowns) {
logger.warning("NO redundancy! Exiting iterations.");
estimationDone = true; // cannot remove more than this
}
// check on test k_alpha
if (Math.max(maxWinL, maxWinP) <= criticalValue) {
// all tests accepted?
logger.info("All outlier tests accepted! (final solution computed)");
estimationDone = true;
}
if (numIterations >= maxIterations) {
logger.info("max. number of iterations reached (exiting loop).");
estimationDone = true; // we reached max. (or no max_iter specified)
}
/** Only warn if last iteration has been estimationDone */
if (estimationDone) {
if (overAllModelTest_L > 10) {
logger.warning(
"COREGPM: Overall Model Test, Lines = {} is larger than 10. (Suggest model or a priori sigma not correct.)"
+ overAllModelTest_L);
}
if (overAllModelTest_P > 10) {
logger.warning(
"COREGPM: Overall Model Test, Pixels = {} is larger than 10. (Suggest model or a priori sigma not correct.)"
+ overAllModelTest_P);
}
/** if a priori sigma is correct, max wtest should be something like 1.96 */
if (Math.max(maxWinL, maxWinP) > 200.0) {
logger.warning(
"Recommendation: remove window number: {} and re-run step COREGPM. max. wtest is: {}."
+ index.get(winL)
+ Math.max(maxWinL, maxWinP));
}
}
logger.info("TIME FOR wTestStatistics: {}" + stopWatch.lap("WTEST"));
logger.info("Total Estimation TIME: {}" + clock.getElapsedTime());
numIterations++; // update counter here!
} // only warn when iterating
yError = eL_hat.getData();
xError = eP_hat.getData();
yCoef = rhsL.getData();
xCoef = rhsP.getData();
}
public void set(RootJointDesiredConfigurationDataReadOnly other) {
desiredConfiguration.set(other.getDesiredConfiguration());
desiredVelocity.set(other.getDesiredVelocity());
desiredAcceleration.set(other.getDesiredAcceleration());
}
public void setDesiredConfiguration(DenseMatrix64F q) {
if (q.getNumRows() != 7 || q.getNumCols() != 1)
throw new RuntimeException("Unexpected size: " + q);
desiredConfiguration.set(q);
}
public void setDesiredVelocity(DenseMatrix64F qd) {
if (qd.getNumRows() != 6 || qd.getNumCols() != 1)
throw new RuntimeException("Unexpected size: " + qd);
desiredVelocity.set(qd);
}
public void setDesiredAcceleration(DenseMatrix64F qdd) {
if (qdd.getNumRows() != 6 || qdd.getNumCols() != 1)
throw new RuntimeException("Unexpected size: " + qdd);
desiredAcceleration.set(qdd);
}
