@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; }