@Test
public void incorrectInput() {
init(30, false);
// compute true essential matrix
DenseMatrix64F E = MultiViewOps.createEssential(worldToCamera.getR(), worldToCamera.getT());
// create an alternative incorrect matrix
Vector3D_F64 T = worldToCamera.getT().copy();
T.x += 0.1;
DenseMatrix64F Emod = MultiViewOps.createEssential(worldToCamera.getR(), T);
ModelFitter<DenseMatrix64F, AssociatedPair> alg = createAlgorithm();
// compute and compare results
assertTrue(alg.fitModel(pairs, Emod, found));
// normalize to allow comparison
CommonOps.divide(E.get(2, 2), E);
CommonOps.divide(Emod.get(2, 2), Emod);
CommonOps.divide(found.get(2, 2), found);
double error0 = 0;
double error1 = 0;
// very crude error metric
for (int i = 0; i < 9; i++) {
error0 += Math.abs(Emod.data[i] - E.data[i]);
error1 += Math.abs(found.data[i] - E.data[i]);
}
// System.out.println("error "+error1+" other "+error0);
assertTrue(error1 < error0);
}
public static void jacobianPrint(
FunctionNtoM func,
FunctionNtoMxN jacobian,
double param[],
double tol,
double differenceScale) {
NumericalJacobianForward numerical = new NumericalJacobianForward(func, differenceScale);
DenseMatrix64F found = new DenseMatrix64F(func.getNumOfOutputsM(), func.getNumOfInputsN());
DenseMatrix64F expected = new DenseMatrix64F(func.getNumOfOutputsM(), func.getNumOfInputsN());
jacobian.process(param, found.data);
numerical.process(param, expected.data);
System.out.println("FOUND:");
found.print();
System.out.println("-----------------------------");
System.out.println("Numerical");
expected.print();
System.out.println("-----------------------------");
System.out.println("Large Differences");
for (int y = 0; y < found.numRows; y++) {
for (int x = 0; x < found.numCols; x++) {
double diff = Math.abs(found.get(y, x) - expected.get(y, x));
if (diff > tol) {
// double e = expected.get(y,x);
// double f = found.get(y,x);
System.out.print("1");
} else System.out.print("0");
}
System.out.println();
}
}
@Override
protected MultivariateGaussianDM createPerfectMeas(
KalmanFilterInterface f, DenseMatrix64F state) {
KalmanFilter filter = (KalmanFilter) f;
DenseMatrix64F H = filter.getProjector().getProjectionMatrix();
DenseMatrix64F X = state;
DenseMatrix64F z = new DenseMatrix64F(2, 1);
CommonOps.mult(H, X, z);
return createState(2.0, z.get(0, 0), z.get(1, 0));
}
@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 static void computeLowerCoef(
WlCoef_F32 inverse, DenseMatrix64F a_inv, WlBorderCoefFixed ret, int col) {
int lengthWavelet = inverse.wavelet.length + inverse.offsetWavelet + col;
int lengthScaling = inverse.scaling.length + inverse.offsetScaling + col;
lengthWavelet = Math.min(lengthWavelet, inverse.wavelet.length);
lengthScaling = Math.min(lengthScaling, inverse.scaling.length);
float[] coefScaling = new float[lengthScaling];
float[] coefWavelet = new float[lengthWavelet];
for (int j = 0; j < lengthScaling; j++) {
coefScaling[j] = (float) a_inv.get(j, col);
}
for (int j = 0; j < lengthWavelet; j++) {
coefWavelet[j] = (float) a_inv.get(j, col + 1);
}
ret.lowerCoef[col] = new WlCoef_F32(coefScaling, 0, coefWavelet, 0);
}
@Test
public void perfectInput() {
init(30, false);
// compute true essential matrix
DenseMatrix64F E = MultiViewOps.createEssential(worldToCamera.getR(), worldToCamera.getT());
ModelFitter<DenseMatrix64F, AssociatedPair> alg = createAlgorithm();
// give it the perfect matrix and see if it screwed it up
assertTrue(alg.fitModel(pairs, E, found));
// normalize so that they are the same
CommonOps.divide(E.get(2, 2), E);
CommonOps.divide(found.get(2, 2), found);
assertTrue(MatrixFeatures.isEquals(E, found, 1e-8));
}
/**
* 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);
}
/** Debug. */
public void debug() {
logger.info(
"numRows: " + numRows() + " numCols: " + numCols() + " type:" + getClass().getSimpleName());
for (int i = 0; i < numRows(); i++) {
String line = "";
for (int j = 0; j < numCols(); j++) {
line += String.format("%8f", mat.get(i, j)) + " ";
}
logger.info("idx: " + i + " values: " + line);
}
}
@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));
}
private static void computeUpperCoef(
WlCoef_F32 inverse, int n, DenseMatrix64F a_inv, WlBorderCoefFixed ret, int col) {
int indexEnd = n - col - 2;
int lengthWavelet = indexEnd + inverse.offsetWavelet + inverse.wavelet.length;
int lengthScaling = indexEnd + inverse.offsetScaling + inverse.scaling.length;
lengthWavelet =
lengthWavelet > n ? inverse.wavelet.length - (lengthWavelet - n) : inverse.wavelet.length;
lengthScaling =
lengthScaling > n ? inverse.scaling.length - (lengthScaling - n) : inverse.scaling.length;
float[] coefScaling = new float[lengthScaling];
float[] coefWavelet = new float[lengthWavelet];
for (int j = 0; j < lengthScaling; j++) {
coefScaling[j] = (float) a_inv.get(indexEnd + j + inverse.offsetScaling, n - 2 - col);
}
for (int j = 0; j < lengthWavelet; j++) {
coefWavelet[j] = (float) a_inv.get(indexEnd + j + inverse.offsetWavelet, n - 2 - col + 1);
}
ret.upperCoef[col / 2] =
new WlCoef_F32(coefScaling, inverse.offsetScaling, coefWavelet, inverse.offsetWavelet);
}
public void computeJacobian(Point2D_F64 x1, Point2D_F64 x2) {
J.data[0] = -H.get(1, 0) + x2.y * H.get(2, 0);
J.data[1] = -H.get(1, 1) + x2.y * H.get(2, 1);
J.data[2] = 0;
J.data[3] = x1.x * H.get(2, 0) + x1.y * H.get(2, 1) + H.get(2, 2);
J.data[4] = H.get(0, 0) - x2.x * H.get(2, 0);
J.data[5] = H.get(0, 1) - x2.x * H.get(2, 1);
J.data[6] = -J.data[3];
J.data[7] = 0;
}
public double error2(double x1, double y1, double x2, double y2) {
double ret;
ret = (x1 * H.get(0, 0) + y1 * H.get(0, 1) + H.get(0, 2));
ret -= x2 * (x1 * H.get(2, 0) + y1 * H.get(2, 1) + H.get(2, 2));
return ret;
}
/** x2 = H*x1 */
public double error1(double x1, double y1, double x2, double y2) {
double ret;
ret = -(x1 * H.get(1, 0) + y1 * H.get(1, 1) + H.get(1, 2));
ret += y2 * (x1 * H.get(2, 0) + y1 * H.get(2, 1) + H.get(2, 2));
return ret;
}
// 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));
}
@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));
}
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 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 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));
}
/**
* Returns one entry from the Jacobian matrix. Does not recompute the Jacobian.
*
* @param row the desired row of the Jacobian
* @param column the desired column of the Jacobian
* @return the entry at (row, column)
*/
public double getJacobianEntry(int row, int column) {
return jacobian.get(row, column);
}
/**
* 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));
}
/**
* Converts a rotation matrix into {@link georegression.struct.so.Rodrigues_F32}.
*
* @param R Rotation matrix.
* @param rodrigues Storage used for solution. If null a new instance is declared.
* @return The found axis and rotation angle.
*/
public static Rodrigues_F32 matrixToRodrigues(DenseMatrix64F R, Rodrigues_F32 rodrigues) {
if (rodrigues == null) {
rodrigues = new Rodrigues_F32();
}
// parts of this are from wikipedia
// http://en.wikipedia.org/wiki/Rotation_representation_%28mathematics%29#Rotation_matrix_.E2.86f.94_Euler_axis.2Fangle
float diagSum =
((float) (R.unsafe_get(0, 0) + R.unsafe_get(1, 1) + R.unsafe_get(2, 2)) - 1.0f) / 2.0f;
float absDiagSum = (float) Math.abs(diagSum);
if (absDiagSum <= 1.0f && 1.0f - absDiagSum > 10.0f * GrlConstants.F_EPS) {
// if numerically stable use a faster technique
rodrigues.theta = (float) Math.acos(diagSum);
float bottom = 2.0f * (float) Math.sin(rodrigues.theta);
// in cases where bottom is close to zero that means theta is also close to zero and the
// vector
// doesn't matter that much
rodrigues.unitAxisRotation.x = (float) (R.unsafe_get(2, 1) - R.unsafe_get(1, 2)) / bottom;
rodrigues.unitAxisRotation.y = (float) (R.unsafe_get(0, 2) - R.unsafe_get(2, 0)) / bottom;
rodrigues.unitAxisRotation.z = (float) (R.unsafe_get(1, 0) - R.unsafe_get(0, 1)) / bottom;
// in extreme underflow situations the result can be unnormalized
rodrigues.unitAxisRotation.normalize();
// In theory this might be more stable
// rotationAxis( R, rodrigues.unitAxisRotation);
} else {
// this handles the special case where the bottom is very very small or equal to zero
if (diagSum >= 1.0f) rodrigues.theta = 0;
else if (diagSum <= -1.0f) rodrigues.theta = (float) Math.PI;
else rodrigues.theta = (float) Math.acos(diagSum);
// compute the value of x,y,z up to a sign ambiguity
rodrigues.unitAxisRotation.x = (float) Math.sqrt((R.get(0, 0) + 1) / 2);
rodrigues.unitAxisRotation.y = (float) Math.sqrt((R.get(1, 1) + 1) / 2);
rodrigues.unitAxisRotation.z = (float) Math.sqrt((R.get(2, 2) + 1) / 2);
float x = rodrigues.unitAxisRotation.x;
float y = rodrigues.unitAxisRotation.y;
float z = rodrigues.unitAxisRotation.z;
if (Math.abs(R.get(1, 0) - 2 * x * y) > GrlConstants.F_EPS) {
x *= -1;
}
if (Math.abs(R.get(2, 0) - 2 * x * z) > GrlConstants.F_EPS) {
z *= -1;
}
if (Math.abs(R.get(2, 1) - 2 * z * y) > GrlConstants.F_EPS) {
y *= -1;
x *= -1;
}
rodrigues.unitAxisRotation.x = x;
rodrigues.unitAxisRotation.y = y;
rodrigues.unitAxisRotation.z = z;
}
return rodrigues;
}
/**
* Multiplies the two sub-matrices together. Checks to see if the same result is found when
* multiplied using the normal algorithm versus the submatrix one.
*/
private static void checkMult_submatrix(
Method func,
int operationType,
boolean transA,
boolean transB,
D1Submatrix64F A,
D1Submatrix64F B) {
if (A.col0 % BLOCK_LENGTH != 0 || A.row0 % BLOCK_LENGTH != 0)
throw new IllegalArgumentException("Submatrix A is not block aligned");
if (B.col0 % BLOCK_LENGTH != 0 || B.row0 % BLOCK_LENGTH != 0)
throw new IllegalArgumentException("Submatrix B is not block aligned");
BlockMatrix64F origA = BlockMatrixOps.createRandom(numRows, numCols, -1, 1, rand, BLOCK_LENGTH);
BlockMatrix64F origB = BlockMatrixOps.createRandom(numCols, numRows, -1, 1, rand, BLOCK_LENGTH);
A.original = origA;
B.original = origB;
int w = B.col1 - B.col0;
int h = A.row1 - A.row0;
// offset it to make the test harder
// randomize to see if its set or adding
BlockMatrix64F subC =
BlockMatrixOps.createRandom(BLOCK_LENGTH + h, BLOCK_LENGTH + w, -1, 1, rand, BLOCK_LENGTH);
D1Submatrix64F C =
new D1Submatrix64F(subC, BLOCK_LENGTH, subC.numRows, BLOCK_LENGTH, subC.numCols);
DenseMatrix64F rmC = multByExtract(operationType, A, B, C);
if (transA) {
origA = BlockMatrixOps.transpose(origA, null);
transposeSub(A);
A.original = origA;
}
if (transB) {
origB = BlockMatrixOps.transpose(origB, null);
transposeSub(B);
B.original = origB;
}
try {
func.invoke(null, BLOCK_LENGTH, A, B, C);
} catch (IllegalAccessException e) {
throw new RuntimeException(e);
} catch (InvocationTargetException e) {
throw new RuntimeException(e);
}
for (int i = C.row0; i < C.row1; i++) {
for (int j = C.col0; j < C.col1; j++) {
// System.out.println(i+" "+j);
double diff = Math.abs(subC.get(i, j) - rmC.get(i - C.row0, j - C.col0));
// System.out.println(subC.get(i,j)+" "+rmC.get(i-C.row0,j-C.col0));
if (diff >= 1e-12) {
subC.print();
rmC.print();
System.out.println(func.getName());
System.out.println("transA " + transA);
System.out.println("transB " + transB);
System.out.println("type " + operationType);
fail("Error too large");
}
}
}
}
