public void setEstimate( double[] est ) { this.estimate = new DenseMatrix64F( ndims, 1 ); estimate.setData( est ); }
public void setTarget( double[] tgt ) { this.target = new DenseMatrix64F( ndims, 1 ); target.setData( tgt ); }
target.setData(targetData);
public JointConfigurationData(Random random) { int size = Math.abs(random.nextInt(100)); jointConfigurations = new DenseMatrix64F[size]; for (int i = 0; i < jointConfigurations.length; i++) { double[] data = new double[random.nextInt(1000)]; for (int j = 0; j < data.length; j++) { data[j] = random.nextDouble(); } jointConfigurations[i] = new DenseMatrix64F(); jointConfigurations[i].setData(data); } }
/** * Constructor with transformation parameters. aMatrix and bVector are * allowed to be null * * @param srcPts source points * @param aMatrix A matrix * @param bVector b vector * @param dMatrixData D matrix */ public ThinPlateR2LogRSplineKernelTransform( final double[][] srcPts, final double[][] aMatrix, final double[] bVector, final double[] dMatrixData ) { ndims = srcPts.length; if ( srcPts != null && srcPts.length > 0 ) nLandmarks = srcPts[ 0 ].length; else nLandmarks = 0; this.sourceLandmarks = srcPts; this.aMatrix = aMatrix; this.bVector = bVector; dMatrix = new DenseMatrix64F( ndims, nLandmarks ); dMatrix.setData( dMatrixData ); }
public void setEstimateXfm( double[] est ) { this.estimateXfm = new DenseMatrix64F( ndims, 1 ); estimateXfm.setData( est ); updateError(); }
public void setSpecialSecondOrderConeInequality(double[][] coneInequalityMatrixB, double[] coneInequalityVectorU, ArrayList<ConvexMultivariateRealFunction> otherInequalities) { int numberOfRows = coneInequalityMatrixB.length; int numberOfColumns = coneInequalityMatrixB[0].length; if (numberOfRows != numberOfColumns) { throw new RuntimeException("coneInequalityMatrixB must be square!"); } if (coneInequalityVectorU.length != numberOfRows) { throw new RuntimeException("coneInequalityVectorU must be have correct length!"); } DenseMatrix64F coneInequalityDenseMatrixB = new DenseMatrix64F(coneInequalityMatrixB); DenseMatrix64F coneInequalityVectorUAsDenseMatrix = new DenseMatrix64F(numberOfRows, 1); coneInequalityVectorUAsDenseMatrix.setData(coneInequalityVectorU); setSpecialSecondOrderConeInequality(coneInequalityDenseMatrixB, coneInequalityVectorUAsDenseMatrix, otherInequalities); }
public void setSpecialSecondOrderConeInequality(double[][] coneInequalityMatrixB, double[] coneInequalityVectorU, ArrayList<ConvexMultivariateRealFunction> otherInequalities) { int numberOfRows = coneInequalityMatrixB.length; int numberOfColumns = coneInequalityMatrixB[0].length; if (numberOfRows != numberOfColumns) { throw new RuntimeException("coneInequalityMatrixB must be square!"); } if (coneInequalityVectorU.length != numberOfRows) { throw new RuntimeException("coneInequalityVectorU must be have correct length!"); } DenseMatrix64F coneInequalityDenseMatrixB = new DenseMatrix64F(coneInequalityMatrixB); DenseMatrix64F coneInequalityVectorUAsDenseMatrix = new DenseMatrix64F(numberOfRows, 1); coneInequalityVectorUAsDenseMatrix.setData(coneInequalityVectorU); setSpecialSecondOrderConeInequality(coneInequalityDenseMatrixB, coneInequalityVectorUAsDenseMatrix, otherInequalities); }
double gridResolution = 0.01; DenseMatrix64F matrix = new DenseMatrix64F(11, 11); matrix.setData(new double[]
double gridResolution = 0.01; DenseMatrix64F matrix = new DenseMatrix64F(11, 11); matrix.setData(new double[]
HeightMapWithPoints map = getHeightMap(-2,-2,2,2,gridResolution); DenseMatrix64F matrix = new DenseMatrix64F(7, 7); matrix.setData(new double[] { 0, 0, 0, 0, 3, 0, 0, 0, 0, 8, 8, 0, 0, 0,
HeightMapWithPoints map = new DoubleHashHeightMap(gridResolution); DenseMatrix64F matrix = new DenseMatrix64F(7, 7); matrix.setData(new double[] { 0, 0, 0, 0, 3, 0, 0, 0, 0, 8, 8, 0, 0, 0,
private void solveSystem(int numRows, int numCols) { LinearSolver<DenseMatrix64F> qrSolver = LinearSolverFactory.qr(numRows, numCols); QRDecomposition<DenseMatrix64F> decomposition = qrSolver.getDecomposition(); qrSolver.setA(X); y.setData(response); qrSolver.solve(this.y, this.b); DenseMatrix64F R = decomposition.getR(null, true); LinearSolver<DenseMatrix64F> linearSolver = LinearSolverFactory.linear(numCols); linearSolver.setA(R); DenseMatrix64F Rinverse = new DenseMatrix64F(numCols, numCols); linearSolver.invert(Rinverse); // stores solver's solution inside of Rinverse. CommonOps.multOuter(Rinverse, this.XtXInv); }
double n = Double.NaN; DenseMatrix64F matrix = new DenseMatrix64F(7, 7); matrix.setData(new double[]{0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, n, 0, 1, 0,
public void compute() { jacobian.compute(); CommonOps.extract(jacobian.getJacobianMatrix(), 0, 3, 0, 3, jacobianAngularPart64F, 0, 0); if (Math.abs(CommonOps.det(jacobianAngularPart64F)) < 1e-5) return; CommonOps.invert(jacobianAngularPart64F, inverseAngularJacobian64F); chestAngularVelocity.setToZero(chestIMU.getMeasurementFrame()); chestIMU.getAngularVelocityMeasurement(chestAngularVelocity.getVector()); chestAngularVelocity.changeFrame(jacobian.getJacobianFrame()); pelvisAngularVelocity.setToZero(pelvisIMU.getMeasurementFrame()); pelvisIMU.getAngularVelocityMeasurement(pelvisAngularVelocity.getVector()); pelvisAngularVelocity.changeFrame(jacobian.getJacobianFrame()); chestAngularVelocity.sub(pelvisAngularVelocity); omega.setData(chestAngularVelocity.toArray()); CommonOps.mult(inverseAngularJacobian64F, omega, qd_estimated); for (int i = 0; i < joints.length; i++) { OneDoFJoint joint = joints[i]; double qd_sensorMap = sensorMap.getJointVelocityProcessedOutput(joint); double qd_IMU = qd_estimated.get(i, 0); double qd_fused = (1.0 - alpha.getDoubleValue()) * qd_sensorMap + alpha.getDoubleValue() * qd_IMU; jointVelocitiesFromIMUOnly.get(joint).set(qd_IMU); jointVelocities.get(joint).update(qd_fused); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void basicBestFitPlaneCalculatorTest() throws InsufficientDataException { double gridResolution = 1; HeightMapWithPoints map = new DoubleHashHeightMap(gridResolution); FramePoint2D footCenterPoint = new FramePoint2D(ReferenceFrame.getWorldFrame(), 0.0, 0.0); DenseMatrix64F matrix = new DenseMatrix64F(7, 7); matrix.setData(new double[]{0, 0, 0, 0, 3, 0, 0, 0, 0, 8, 8, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 8, 1, 1, 1, 0, 0, 0, 0, 2, 2, 2, 0, 0, 0, 0, 0, 0, 8, 1, 0, 0, 0, 0, 0, 0, 0, 0} ); DataGridTools.fillMapWithMatrixCentered(map, matrix, gridResolution); HeightMapBestFitPlaneCalculator calculator = new HeightMapBestFitPlaneCalculator(); Plane3D plane = calculator.calculatePlane(map, footCenterPoint, 2.0, 2.0); System.out.println("BestFitPlaneCalculatorTest: calculator.getPointList() = " + calculator.getPointList()); Point3D point = plane.getPointCopy(); Vector3D normal = plane.getNormalCopy(); assertEquals(1.0, point.getZ(), eps); assertEquals(footCenterPoint.getX(), point.getX(), eps); assertEquals(footCenterPoint.getY(), point.getY(), eps); assertEquals(-Math.sqrt(2) / 2.0, normal.getX(), eps); assertEquals(0.0, normal.getY(), eps); assertEquals(Math.sqrt(2) / 2.0, normal.getZ(), eps); }
solutionMatrix.setData(solution); double objectiveCost = solver.getObjectiveCost(solutionMatrix); assertEquals(248.0, objectiveCost, 1e-7);
assertEquals(5.0, solution[0], 1e-7); DenseMatrix64F solutionMatrix = new DenseMatrix64F(costQuadraticMatrix.length, 1); solutionMatrix.setData(solution); double objectiveCost = solver.getObjectiveCost(solutionMatrix); assertEquals(0.0, objectiveCost, 1e-7); assertEquals(3.0, solution[1], 1e-7); solutionMatrix = new DenseMatrix64F(costQuadraticMatrix.length, 1); solutionMatrix.setData(solution); objectiveCost = solver.getObjectiveCost(solutionMatrix); assertEquals(0.0, objectiveCost, 1e-7); assertEquals(-1.0, lagrangeMultipliers[0], 1e-7); // Lagrange multiplier is -1.0; solutionMatrix = new DenseMatrix64F(costQuadraticMatrix.length, 1); solutionMatrix.setData(solution); objectiveCost = solver.getObjectiveCost(solutionMatrix); assertEquals(0.5, objectiveCost, 1e-7); assertEquals(0.0, lagrangeMultipliers[1], 1e-7); // Lagrange multiplier solutionMatrix = new DenseMatrix64F(costQuadraticMatrix.length, 1); solutionMatrix.setData(solution); objectiveCost = solver.getObjectiveCost(solutionMatrix); assertEquals(2.0, objectiveCost, 1e-7);