Java MatrixFeatures类代码示例

import org.ejml.ops.MatrixFeatures; //导入依赖的package包/类
/**
 * Check the trifocal tensor using its definition
 */
@Test
public void createTrifocal_CameraMatrix() {

	SimpleMatrix P1 = SimpleMatrix.wrap(
			PerspectiveOps.createCameraMatrix(worldToCam2.getR(), worldToCam2.getT(), null, null));
	SimpleMatrix P2 = SimpleMatrix.wrap(
			PerspectiveOps.createCameraMatrix(worldToCam3.getR(), worldToCam3.getT(), null, null));

	TrifocalTensor found = MultiViewOps.createTrifocal(worldToCam2,worldToCam3,null);

	for( int i = 0; i < 3; i++ ) {
		SimpleMatrix ai = P1.extractVector(false,i);
		SimpleMatrix b4 = P2.extractVector(false,3);
		SimpleMatrix a4 = P1.extractVector(false,3);
		SimpleMatrix bi = P2.extractVector(false,i);

		SimpleMatrix expected = ai.mult(b4.transpose()).minus(a4.mult(bi.transpose()));

		assertTrue(MatrixFeatures.isIdentical(expected.getMatrix(),found.getT(i),1e-8));
	}
}
 

import org.ejml.ops.MatrixFeatures; //导入依赖的package包/类
@Test
public void extractEpipoles_threeview() {
	Point3D_F64 found2 = new Point3D_F64();
	Point3D_F64 found3 = new Point3D_F64();

	TrifocalTensor input = tensor.copy();

	MultiViewOps.extractEpipoles(input,found2,found3);

	// make sure the input was not modified
	for( int i = 0; i < 3; i++ )
		assertTrue(MatrixFeatures.isIdentical(tensor.getT(i),input.getT(i),1e-8));

	Point3D_F64 space = new Point3D_F64();

	// check to see if it is the left-null space of their respective Fundamental matrices
	GeometryMath_F64.multTran(F2, found2, space);
	assertEquals(0,space.norm(),1e-8);

	GeometryMath_F64.multTran(F3, found3, space);
	assertEquals(0,space.norm(),1e-8);
}
 

import org.ejml.ops.MatrixFeatures; //导入依赖的package包/类
@Test
public void extractFundamental_threeview() {
	DenseMatrix64F found2 = new DenseMatrix64F(3,3);
	DenseMatrix64F found3 = new DenseMatrix64F(3,3);

	TrifocalTensor input = tensor.copy();
	MultiViewOps.extractFundamental(input, found2, found3);

	// make sure the input was not modified
	for( int i = 0; i < 3; i++ )
		assertTrue(MatrixFeatures.isIdentical(tensor.getT(i),input.getT(i),1e-8));

	CommonOps.scale(1.0/CommonOps.elementMaxAbs(found2),found2);
	CommonOps.scale(1.0/CommonOps.elementMaxAbs(found3),found3);

	Point3D_F64 X = new Point3D_F64(0.1,0.05,2);

	// remember the first view is assumed to have a projection matrix of [I|0]
	Point2D_F64 x1 = PerspectiveOps.renderPixel(new Se3_F64(), null, X);
	Point2D_F64 x2 = PerspectiveOps.renderPixel(worldToCam2, K, X);
	Point2D_F64 x3 = PerspectiveOps.renderPixel(worldToCam3, K, X);

	assertEquals(0, MultiViewOps.constraint(found2, x1, x2), 1e-8);
	assertEquals(0, MultiViewOps.constraint(found3, x1, x3), 1e-8);
}
 

import org.ejml.ops.MatrixFeatures; //导入依赖的package包/类
public static void checkHasOriginal( List<Se3_F64> solutionsSE , List<Vector3D_F64> solutionsN ,
									 DenseMatrix64F R, Vector3D_F64 T, double d , Vector3D_F64 N ) {

	int numMatches = 0;
	for( int i = 0; i < 4; i++ ) {
		Se3_F64 foundSE = solutionsSE.get(i);
		Vector3D_F64 foundN = solutionsN.get(i);

		if(!MatrixFeatures.isIdentical(foundSE.getR(), R, 1e-4)) break;

		if( Math.abs(T.x/d - foundSE.getT().x) > 1e-8 ) break;
		if( Math.abs(T.y/d - foundSE.getT().y) > 1e-8 ) break;
		if( Math.abs(T.z/d - foundSE.getT().z) > 1e-8 ) break;

		if( Math.abs(N.x - foundN.x) > 1e-8 ) break;
		if( Math.abs(N.y - foundN.y) > 1e-8 ) break;
		if( Math.abs(N.z - foundN.z) > 1e-8 ) break;

		numMatches++;
	}

	assertEquals(1,numMatches);
}
 

import org.ejml.ops.MatrixFeatures; //导入依赖的package包/类
@Test
public void decode_encode() {
	double param[] = new double[]{0.1,-0.3,4,1,2,3};

	PnPRodriguesCodec alg = new PnPRodriguesCodec();

	double found[] = new double[6];
	Se3_F64 storage = new Se3_F64();
	Se3_F64 storage2 = new Se3_F64();

	alg.decode(param, storage);
	alg.encode(storage,found);
	alg.decode(found,storage2);

	// multiple parameterization can represent the same model, so test using the model
	assertTrue(storage.T.isIdentical(storage2.T,1e-8));
	assertTrue(MatrixFeatures.isIdentical(storage.R,storage2.R,1e-8));
}
 

import org.ejml.ops.MatrixFeatures; //导入依赖的package包/类
@Test
public void testCase0() {
	Se3_F64 a = new Se3_F64();

	a.R = UtilEjml.parseMatrix(
					"1.000000e+00        -5.423439e-14        -3.165003e-13 \n" +
					"5.420664e-14         1.000000e+00         2.461642e-13 \n" +
					"3.162678e-13        -2.464418e-13         1.000000e+00",3);

	PnPRodriguesCodec alg = new PnPRodriguesCodec();

	double param[] = new double[6];
	alg.encode(a,param);

	Se3_F64 found = new Se3_F64();
	alg.decode(param,found);

	assertTrue(a.T.isIdentical(found.T,1e-8));
	assertTrue(MatrixFeatures.isIdentical(a.R,found.R,1e-8));
}
 

import org.ejml.ops.MatrixFeatures; //导入依赖的package包/类
@Test
public void backAndForth() {
	DenseMatrix64F R = RotationMatrixGenerator.eulerXYZ(1, 2, -0.5, null);
	Vector3D_F64 T = new Vector3D_F64(0.5,0.7,-0.3);

	DenseMatrix64F E = MultiViewOps.createEssential(R, T);
	
	double param[] = new double[7];
	
	ParamFundamentalEpipolar alg = new ParamFundamentalEpipolar();
	
	DenseMatrix64F found = new DenseMatrix64F(3,3);
	alg.encode(E, param);
	alg.decode(param, found);

	// normalize to take in account scale different when testing
	CommonOps.divide(E.get(2,2),E);
	CommonOps.divide(found.get(2, 2), found);
	
	assertTrue(MatrixFeatures.isEquals(E, found, 1e-8));
}
 

import org.ejml.ops.MatrixFeatures; //导入依赖的package包/类
@Test
public void perfectInput() {
	createScene(30,false);

	// use the linear algorithm to compute the homography
	HomographyLinear4 estimator = new HomographyLinear4(true);
	estimator.process(pairs,H);

	GeoModelRefine<DenseMatrix64F,AssociatedPair> alg = createAlgorithm();

	//give it the perfect matrix and see if it screwed it up
	assertTrue(alg.process(H, pairs, found));

	// normalize so that they are the same
	CommonOps.divide(H.get(2, 2), H);
	CommonOps.divide(found.get(2, 2), found);

	assertTrue(MatrixFeatures.isEquals(H, found, 1e-8));
}
 

import org.ejml.ops.MatrixFeatures; //导入依赖的package包/类
@Test
public void perfectInput() {
	init(30,false);

	// compute true essential matrix
	DenseMatrix64F E = MultiViewOps.createEssential(worldToCamera.getR(), worldToCamera.getT());

	GeoModelRefine<DenseMatrix64F,AssociatedPair> alg = createAlgorithm();

	//give it the perfect matrix and see if it screwed it up
	assertTrue(alg.process(E, pairs, 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));
}
 

import org.ejml.ops.MatrixFeatures; //导入依赖的package包/类
private void perfectData(int numExtra) {
	EstimateNofPnP pnp = FactoryMultiView.computePnP_N(EnumPNP.P3P_FINSTERWALDER, -1);
	DistanceModelMonoPixels<Se3_F64,Point2D3D> distanceMono = new PnPDistanceReprojectionSq();

	PnPStereoEstimator alg = new PnPStereoEstimator(pnp,distanceMono,numExtra);

	Se3_F64 expected = new Se3_F64();
	expected.getR().set(RotationMatrixGenerator.eulerArbitrary(0, 1, 2, 0.05, -0.03, 0.02));
	expected.getT().set(0.2,-0.1,0.01);

	generateScene(alg.getMinimumPoints(),expected,false);

	Se3_F64 found = new Se3_F64();

	alg.setLeftToRight(leftToRight);
	assertTrue(alg.process(pointPose, found));

	found.print();
	expected.print();

	assertTrue(MatrixFeatures.isIdentical(expected.getR(), found.getR(), 1e-8));
	assertTrue(found.getT().isIdentical(expected.getT(), 1e-8));
}
 

import org.ejml.ops.MatrixFeatures; //导入依赖的package包/类
/**
 * Perfect input.  Shouldn't change anything
 */
@Test
public void perfect() {
	generateScene(10,null,false);

	PnPStereoRefineRodrigues alg = new PnPStereoRefineRodrigues(1e-12,200);
	alg.setLeftToRight(leftToRight);


	Se3_F64 found = new Se3_F64();
	assertTrue(alg.process(worldToLeft.copy(), pointPose, found));
	assertTrue(alg.minimizer.getFunctionValue() < 1e-10);

	assertTrue(MatrixFeatures.isIdentical(worldToLeft.getR(), found.getR(), 1e-8));
	assertTrue(found.getT().isIdentical(worldToLeft.getT(), 1e-8));
}
 

import org.ejml.ops.MatrixFeatures; //导入依赖的package包/类
/**
 * Noisy input.  Should generate something close to the actual solution
 */
@Test
public void noisy() {
	generateScene(30,null,false);

	PnPStereoRefineRodrigues alg = new PnPStereoRefineRodrigues(1e-12,200);
	alg.setLeftToRight(leftToRight);


	Se3_F64 input = worldToLeft.copy();
	// noise up the initial guess
	DenseMatrix64F R = RotationMatrixGenerator.eulerXYZ(0.1, -0.04, -0.2, null);
	CommonOps.mult(R,input.getR().copy(),input.getR());
	input.T.x += 0.2;
	input.T.x -= 0.05;
	input.T.z += 0.03;

	Se3_F64 found = new Se3_F64();
	assertTrue(alg.process(input, pointPose, found));
	assertTrue(alg.minimizer.getFunctionValue()<1e-12);

	assertTrue(MatrixFeatures.isIdentical(worldToLeft.getR(), found.getR(), 1e-8));
	assertTrue(found.getT().isIdentical(worldToLeft.getT(), 1e-8));
}
 

import org.ejml.ops.MatrixFeatures; //导入依赖的package包/类
/**
 * Test against a simple known case
 */
@Test
public void knownCase() {
	DenseMatrix64F R = RotationMatrixGenerator.eulerXYZ(0.02, -0.05, 0.01, null);
	Vector3D_F64 T = new Vector3D_F64(100,50,-1000);
	DenseMatrix64F K = GenericCalibrationGrid.createStandardCalibration();
	DenseMatrix64F H = GenericCalibrationGrid.computeHomography(K,R,T);

	Zhang99DecomposeHomography alg = new Zhang99DecomposeHomography();
	alg.setCalibrationMatrix(K);
	Se3_F64 motion = alg.decompose(H);

	assertTrue(MatrixFeatures.isIdentical(R, motion.getR(), 1e-5));
	assertEquals(T.x,motion.getX(), 1e-5);
}
 

import org.ejml.ops.MatrixFeatures; //导入依赖的package包/类
/**
 * Check the trifocal tensor using its definition
 */
@Test
public void createTrifocal_SE() {

	TrifocalTensor found = MultiViewOps.createTrifocal(worldToCam2,worldToCam3,null);

	SimpleMatrix R2 = SimpleMatrix.wrap(worldToCam2.getR());
	SimpleMatrix R3 = SimpleMatrix.wrap(worldToCam3.getR());
	SimpleMatrix b4 = new SimpleMatrix(3,1);
	SimpleMatrix a4 = new SimpleMatrix(3,1);

	b4.set(0,worldToCam3.getX());
	b4.set(1,worldToCam3.getY());
	b4.set(2,worldToCam3.getZ());

	a4.set(0,worldToCam2.getX());
	a4.set(1,worldToCam2.getY());
	a4.set(2,worldToCam2.getZ());

	for( int i = 0; i < 3; i++ ) {
		SimpleMatrix ai = R2.extractVector(false, i);
		SimpleMatrix bi = R3.extractVector(false,i);

		SimpleMatrix expected = ai.mult(b4.transpose()).minus(a4.mult(bi.transpose()));

		assertTrue(MatrixFeatures.isIdentical(expected.getMatrix(),found.getT(i),1e-8));
	}
}
 

import org.ejml.ops.MatrixFeatures; //导入依赖的package包/类
@Test
public void extractCameraMatrices() {
	DenseMatrix64F P2 = new DenseMatrix64F(3,4);
	DenseMatrix64F P3 = new DenseMatrix64F(3,4);

	TrifocalTensor input = tensor.copy();
	MultiViewOps.extractCameraMatrices(input, P2, P3);

	// make sure the input was not modified
	for( int i = 0; i < 3; i++ )
		assertTrue(MatrixFeatures.isIdentical(tensor.getT(i),input.getT(i),1e-8));

	// Using found camera matrices render the point's location
	Point3D_F64 X = new Point3D_F64(0.1,0.05,2);

	Point2D_F64 x1 = new Point2D_F64(X.x/X.z,X.y/X.z);
	Point2D_F64 x2 = PerspectiveOps.renderPixel(P2, X);
	Point2D_F64 x3 = PerspectiveOps.renderPixel(P3, X);

	// validate correctness by testing a constraint on the points
	DenseMatrix64F A = new DenseMatrix64F(3,3);
	MultiViewOps.constraint(tensor, x1, x2, x3, A);

	for( int i = 0; i < 3; i++ ) {
		for( int j = 0; j < 3; j++ ) {
			assertEquals(0,A.get(i,j),1e-7);
		}
	}
}