@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 CentroidalMomentumHandler(
RigidBody rootBody, ReferenceFrame centerOfMassFrame, YoVariableRegistry parentRegistry) {
this.jointsInOrder = ScrewTools.computeSupportAndSubtreeJoints(rootBody);
this.centroidalMomentumMatrix = new CentroidalMomentumMatrix(rootBody, centerOfMassFrame);
this.previousCentroidalMomentumMatrix =
new DenseMatrix64F(
centroidalMomentumMatrix.getMatrix().getNumRows(),
centroidalMomentumMatrix.getMatrix().getNumCols());
yoPreviousCentroidalMomentumMatrix =
new DoubleYoVariable[previousCentroidalMomentumMatrix.getNumRows()]
[previousCentroidalMomentumMatrix.getNumCols()];
MatrixYoVariableConversionTools.populateYoVariables(
yoPreviousCentroidalMomentumMatrix, "previousCMMatrix", registry);
int nDegreesOfFreedom = ScrewTools.computeDegreesOfFreedom(jointsInOrder);
this.v = new DenseMatrix64F(nDegreesOfFreedom, 1);
for (InverseDynamicsJoint joint : jointsInOrder) {
TIntArrayList listToPackIndices = new TIntArrayList();
ScrewTools.computeIndexForJoint(jointsInOrder, listToPackIndices, joint);
int[] indices = listToPackIndices.toArray();
columnsForJoints.put(joint, indices);
}
centroidalMomentumRate = new SpatialForceVector(centerOfMassFrame);
this.centerOfMassFrame = centerOfMassFrame;
parentRegistry.addChild(registry);
double robotMass = TotalMassCalculator.computeSubTreeMass(rootBody);
this.centroidalMomentumRateTermCalculator =
new CentroidalMomentumRateTermCalculator(rootBody, centerOfMassFrame, v, robotMass);
}
private void init(WeightFunction weightFunction, BasesFunction baseFunction) {
this.weightFunction = weightFunction;
this.basesFunction = baseFunction;
final int baseDim = baseFunction.getDim();
A = new DenseMatrix64F(baseDim, baseDim);
A_x = new DenseMatrix64F(baseDim, baseDim);
A_y = new DenseMatrix64F(baseDim, baseDim);
As = new DenseMatrix64F[] {A, A_x, A_y};
B = new ArrayList<>(baseDim);
B_x = new ArrayList<>(baseDim);
B_y = new ArrayList<>(baseDim);
Bs = Arrays.asList(B, B_x, B_y);
for (ArrayList<TDoubleArrayList> tB : Bs) {
for (int i = 0; i < baseDim; i++) {
tB.add(new TDoubleArrayList(ShapeFunctionUtils2D.MAX_NODES_SIZE_GUESS));
}
}
gamma = new DenseMatrix64F(baseDim, 1);
gamma_x = new DenseMatrix64F(baseDim, 1);
gamma_y = new DenseMatrix64F(baseDim, 1);
ps_arr = new double[3][baseDim];
p = DenseMatrix64F.wrap(ps_arr[0].length, 1, ps_arr[0]);
p_x = DenseMatrix64F.wrap(ps_arr[1].length, 1, ps_arr[1]);
p_y = DenseMatrix64F.wrap(ps_arr[2].length, 1, ps_arr[2]);
tv = new DenseMatrix64F(baseDim, 1);
luSolver = new LinearSolverLu(new LUDecompositionAlt());
A_bak = new DenseMatrix64F(baseDim, baseDim);
}
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);
}
/**
* Specifies function being optimized.
*
* @param function Computes residuals and Jacobian.
*/
public void setFunction(CoupledJacobian function) {
internalInitialize(function.getN(), function.getM());
this.function = function;
jacobianVals.reshape(M, N);
B.reshape(N, N);
Bdiag.reshape(N, 1);
}
/**
* 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;
}
/**
* 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);
}
/**
* Checks to see if the matrix is or is not modified as according to the modified flag.
*
* @param decomp
*/
public static void checkModifiedInput(DecompositionInterface decomp) {
DenseMatrix64F A = RandomMatrices.createSymmPosDef(4, new Random(0x434));
DenseMatrix64F A_orig = A.copy();
assertTrue(decomp.decompose(A));
boolean modified = !MatrixFeatures.isEquals(A, A_orig);
assertTrue(modified + " " + decomp.inputModified(), decomp.inputModified() == modified);
}
/**
* 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);
}
@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));
}
@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));
}
// 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));
}
/**
* Converts a vector from eigen space into sample space.
*
* @param eigenData Eigen space data.
* @return Sample space projection.
*/
public double[] eigenToSampleSpace(double[] eigenData) {
if (eigenData.length != numComponents)
throw new IllegalArgumentException("Unexpected sample length");
DenseMatrix64F s = new DenseMatrix64F(A.getNumCols(), 1);
DenseMatrix64F r = DenseMatrix64F.wrap(numComponents, 1, eigenData);
CommonOps.multTransA(V_t, r, s);
DenseMatrix64F mean = DenseMatrix64F.wrap(A.getNumCols(), 1, this.mean);
CommonOps.add(s, mean, s);
return s.data;
}
@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));
}
/**
* Converts a vector from sample space into eigen space.
*
* @param sampleData Sample space data.
* @return Eigen space projection.
*/
public double[] sampleToEigenSpace(double[] sampleData) {
if (sampleData.length != A.getNumCols())
throw new IllegalArgumentException("Unexpected sample length");
DenseMatrix64F mean = DenseMatrix64F.wrap(A.getNumCols(), 1, this.mean);
DenseMatrix64F s = new DenseMatrix64F(A.getNumCols(), 1, true, sampleData);
DenseMatrix64F r = new DenseMatrix64F(numComponents, 1);
CommonOps.sub(s, mean, s);
CommonOps.mult(V_t, s, r);
return r.data;
}
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;
}
/**
* Computes inverse coefficients
*
* @param border
* @param forward Forward coefficients.
* @param inverse Inverse used in the inner portion of the data stream.
* @return
*/
private static WlBorderCoef<WlCoef_F32> computeBorderCoefficients(
BorderIndex1D border, WlCoef_F32 forward, WlCoef_F32 inverse) {
int N = Math.max(forward.getScalingLength(), forward.getWaveletLength());
N += N % 2;
N *= 2;
border.setLength(N);
// Because the wavelet transform is a linear invertible system the inverse coefficients
// can be found by creating a matrix and inverting the matrix. Boundary conditions are then
// extracted from this inverted matrix.
DenseMatrix64F A = new DenseMatrix64F(N, N);
for (int i = 0; i < N; i += 2) {
for (int j = 0; j < forward.scaling.length; j++) {
int index = border.getIndex(j + i + forward.offsetScaling);
A.add(i, index, forward.scaling[j]);
}
for (int j = 0; j < forward.wavelet.length; j++) {
int index = border.getIndex(j + i + forward.offsetWavelet);
A.add(i + 1, index, forward.wavelet[j]);
}
}
LinearSolver<DenseMatrix64F> solver = LinearSolverFactory.linear(N);
if (!solver.setA(A) || solver.quality() < 1e-5) {
throw new IllegalArgumentException("Can't invert matrix");
}
DenseMatrix64F A_inv = new DenseMatrix64F(N, N);
solver.invert(A_inv);
int numBorder = UtilWavelet.borderForwardLower(inverse) / 2;
WlBorderCoefFixed<WlCoef_F32> ret = new WlBorderCoefFixed<>(numBorder, numBorder + 1);
ret.setInnerCoef(inverse);
// add the lower coefficients first
for (int i = 0; i < ret.getLowerLength(); i++) {
computeLowerCoef(inverse, A_inv, ret, i * 2);
}
// add upper coefficients
for (int i = 0; i < ret.getUpperLength(); i++) {
computeUpperCoef(inverse, N, A_inv, ret, i * 2);
}
return ret;
}
/**
* Extracts quaternions from the provided rotation matrix.
*
* @param R (Input) rotation matrix
* @param quat (Output) Optional storage for quaternion. If null a new class will be used.
* @return unit quaternion representation of the rotation matrix.
*/
public static Quaternion_F32 matrixToQuaternion(DenseMatrix64F R, Quaternion_F32 quat) {
if (quat == null) quat = new Quaternion_F32();
// algorithm from:
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
//
// Designed to minimize numerical error by not dividing by very small numbers
float m00 = (float) R.unsafe_get(0, 0);
float m01 = (float) R.unsafe_get(0, 1);
float m02 = (float) R.unsafe_get(0, 2);
float m10 = (float) R.unsafe_get(1, 0);
float m11 = (float) R.unsafe_get(1, 1);
float m12 = (float) R.unsafe_get(1, 2);
float m20 = (float) R.unsafe_get(2, 0);
float m21 = (float) R.unsafe_get(2, 1);
float m22 = (float) R.unsafe_get(2, 2);
float trace = m00 + m11 + m22;
if (trace > 0) {
float S = (float) Math.sqrt(trace + 1.0f) * 2; // S=4*qw
quat.w = 0.25f * S;
quat.x = (m21 - m12) / S;
quat.y = (m02 - m20) / S;
quat.z = (m10 - m01) / S;
} else if ((m00 > m11) & (m00 > m22)) {
float S = (float) Math.sqrt(1.0f + m00 - m11 - m22) * 2; // S=4*qx
quat.w = (m21 - m12) / S;
quat.x = 0.25f * S;
quat.y = (m01 + m10) / S;
quat.z = (m02 + m20) / S;
} else if (m11 > m22) {
float S = (float) Math.sqrt(1.0f + m11 - m00 - m22) * 2; // S=4*qy
quat.w = (m02 - m20) / S;
quat.x = (m01 + m10) / S;
quat.y = 0.25f * S;
quat.z = (m12 + m21) / S;
} else {
float S = (float) Math.sqrt(1.0f + m22 - m00 - m11) * 2; // S=4*qz
quat.w = (m10 - m01) / S;
quat.x = (m02 + m20) / S;
quat.y = (m12 + m21) / S;
quat.z = 0.25f * S;
}
return quat;
}
/**
* 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();
}
public DenseMatrix64F getCentroidalMomentumMatrixPart(InverseDynamicsJoint[] joints) {
int partDegreesOfFreedom = ScrewTools.computeDegreesOfFreedom(joints);
centroidalMomentumMatrixPart.reshape(Momentum.SIZE, partDegreesOfFreedom);
centroidalMomentumMatrixPart.zero();
int startColumn = 0;
for (InverseDynamicsJoint joint : joints) {
int[] columnsForJoint = columnsForJoints.get(joint);
MatrixTools.extractColumns(
centroidalMomentumRateTermCalculator.getCentroidalMomentumMatrix(),
columnsForJoint,
centroidalMomentumMatrixPart,
startColumn);
startColumn += columnsForJoint.length;
}
return centroidalMomentumMatrixPart;
}
public void setDesiredAcceleration(
FrameVector angularAcceleration, FrameVector linearAcceleration) {
desiredAcceleration.reshape(6, 1);
MatrixTools.insertTuple3dIntoEJMLVector(
angularAcceleration.getVector(), desiredAcceleration, 0);
MatrixTools.insertTuple3dIntoEJMLVector(linearAcceleration.getVector(), desiredAcceleration, 3);
}
// 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 MomentumSolver3(
SixDoFJoint rootJoint,
RigidBody elevator,
ReferenceFrame centerOfMassFrame,
TwistCalculator twistCalculator,
LinearSolver<DenseMatrix64F> jacobianSolver,
double controlDT,
YoVariableRegistry parentRegistry) {
this.rootJoint = rootJoint;
this.jointsInOrder = ScrewTools.computeSupportAndSubtreeJoints(rootJoint.getSuccessor());
this.motionConstraintHandler =
new MotionConstraintHandler("solver3", jointsInOrder, twistCalculator, null, registry);
this.centroidalMomentumMatrix = new CentroidalMomentumMatrix(elevator, centerOfMassFrame);
this.previousCentroidalMomentumMatrix =
new DenseMatrix64F(
centroidalMomentumMatrix.getMatrix().getNumRows(),
centroidalMomentumMatrix.getMatrix().getNumCols());
this.centroidalMomentumMatrixDerivative =
new DenseMatrix64F(
centroidalMomentumMatrix.getMatrix().getNumRows(),
centroidalMomentumMatrix.getMatrix().getNumCols());
yoPreviousCentroidalMomentumMatrix =
new DoubleYoVariable[previousCentroidalMomentumMatrix.getNumRows()]
[previousCentroidalMomentumMatrix.getNumCols()];
MatrixYoVariableConversionTools.populateYoVariables(
yoPreviousCentroidalMomentumMatrix, "previousCMMatrix", registry);
this.controlDT = controlDT;
nDegreesOfFreedom = ScrewTools.computeDegreesOfFreedom(jointsInOrder);
this.b = new DenseMatrix64F(SpatialMotionVector.SIZE, 1);
this.v = new DenseMatrix64F(nDegreesOfFreedom, 1);
// nullspaceMotionConstraintEnforcer = new
// EqualityConstraintEnforcer(LinearSolverFactory.pseudoInverse(true));
equalityConstraintEnforcer =
new EqualityConstraintEnforcer(LinearSolverFactory.pseudoInverse(true));
solver = LinearSolverFactory.pseudoInverse(true);
parentRegistry.addChild(registry);
reset();
}
@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 the Jacobian. */
public void compute() {
int column = 0;
for (int i = 0; i < unitTwistList.size(); i++) {
Twist twist = unitTwistList.get(i);
tempTwist.set(twist);
tempTwist.changeFrame(jacobianFrame);
tempTwist.getMatrix(tempMatrix, 0);
CommonOps.extract(
tempMatrix,
0,
tempMatrix.getNumRows(),
0,
tempMatrix.getNumCols(),
jacobian,
0,
column++);
}
}
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);
}
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;
}
/**
* Converts this matrix formated trifocal into a 27 element vector:<br>
* m.data[ i*9 + j*3 + k ] = T_i(j,k)
*
* @param m Output: Trifocal tensor encoded in a vector
*/
public void convertTo(DenseMatrix64F m) {
if (m.getNumElements() != 27)
throw new IllegalArgumentException("Input matrix/vector must have 27 elements");
for (int i = 0; i < 9; i++) {
m.data[i] = T1.data[i];
m.data[i + 9] = T2.data[i];
m.data[i + 18] = T3.data[i];
}
}
public static long fillArrays(DenseMatrix64F mat, int numTrials) {
long prev = System.currentTimeMillis();
for (int i = 0; i < numTrials; i++) {
Arrays.fill(mat.data, 0, mat.getNumElements(), 2);
}
long curr = System.currentTimeMillis();
return curr - prev;
}
