Java Complex64F类代码示例

import org.ejml.data.Complex64F; //导入依赖的package包/类
/**
 * <p>
 * Find the polynomial roots and for each root compute the Fundamental matrix.
 * Given the two matrices it will compute an alpha such that the determinant is zero.<br>
 *
 * det(&alpha*F1 + (1-&alpha;)*F2 ) = 0
 * </p>

 */
public void computeSolutions( FastQueue<DenseMatrix64F> solutions )
{
	if( !rootFinder.process(poly))
		return;

	List<Complex64F> zeros = rootFinder.getRoots();

	for( Complex64F c : zeros ) {
		if( !c.isReal() && Math.abs(c.imaginary) > 1e-10 )
			continue;

		DenseMatrix64F F = solutions.grow();

		double a = c.real;
		double b = 1-c.real;

		for( int i = 0; i < 9; i++ ) {
			F.data[i] = a*F1.data[i] + b*F2.data[i];
		}

		// det(F) = 0 is already enforced, but for essential matrices it needs to enforce
		// that the first two singular values are zero and the last one is zero
		if( !computeFundamental && !projectOntoEssential(F) ) {
				solutions.removeTail();
		}
	}
}
 

import org.ejml.data.Complex64F; //导入依赖的package包/类
/**
 * Computes the essential matrix from point correspondences.
 *
 * @param points Input: List of points correspondences in normalized image coordinates
 * @param solutions Output: Storage for the found solutions.   .
 * @return true for success or false if a fault has been detected
 */
public boolean process( List<AssociatedPair> points , FastQueue<DenseMatrix64F> solutions ) {
	if( points.size() != 5 )
		throw new IllegalArgumentException("Exactly 5 points are required, not "+points.size());
	solutions.reset();

	// Computes the 4-vector span which contains E.  See equations 7-9
	computeSpan(points);

	// Construct a linear system based on the 10 constraint equations. See equations 5,6, and 10 .
	helper.setNullSpace(X,Y,Z,W);
	helper.setupA1(A1);
	helper.setupA2(A2);

	// instead of Gauss-Jordan elimination LU decomposition is used to solve the system
	solver.setA(A1);
	solver.solve(A2, C);

	// construct the z-polynomial matrix.  Equations 11-14
	helper.setDeterminantVectors(C);
	helper.extractPolynomial(poly.getCoefficients());

	if( !findRoots.process(poly) )
		return false;

	for( Complex64F c : findRoots.getRoots() ) {
		if( !c.isReal() )
			continue;

		solveForXandY(c.real);

		DenseMatrix64F E = solutions.grow();

		for( int i = 0; i < 9; i++ ) {
			E.data[i] = x*X[i] + y*Y[i] + z*Z[i] + W[i];
		}
	}

	return true;
}
 

import org.ejml.data.Complex64F; //导入依赖的package包/类
public static double magsqr(Complex64F v){
	return (v.real*v.real + v.imaginary*v.imaginary);
}
 

import org.ejml.data.Complex64F; //导入依赖的package包/类
/**
 * @inheritDoc
 */
@Override
public boolean process( Point2D_F64 obs1 , Point2D_F64 obs2, Point2D_F64 obs3,
						double length23 , double length13 , double length12 ) {

	double cos12 = computeCosine(obs1,obs2);
	double cos13 = computeCosine(obs1,obs3);
	double cos23 = computeCosine(obs2,obs3);

	double a = length23, b = length13, c = length12;

	// divide out numbers before multiplying them.  less overflow/underflow that way
	double a2_div_b2 = (a/b)*(a/b);
	double c2_div_b2 = (c/b)*(c/b);
	double a2_m_c2_div_b2 = a2_div_b2 - c2_div_b2;
	double a2_p_c2_div_b2 = a2_div_b2 + c2_div_b2;

	poly.c[0] = -4*a2_div_b2*pow2(cos12) + pow2(a2_m_c2_div_b2 + 1);
	poly.c[1] = 4*(-a2_m_c2_div_b2*(1 + a2_m_c2_div_b2)*cos13 + 2*a2_div_b2*pow2(cos12)*cos13 - (1-a2_p_c2_div_b2)*cos23*cos12);
	poly.c[2] = 2*(pow2(a2_m_c2_div_b2) - 1 + 2*pow2(a2_m_c2_div_b2)*pow2(cos13) + 2*(1-c2_div_b2)*pow2(cos23) - 4*a2_p_c2_div_b2*cos12*cos13*cos23 + 2*(1-a2_div_b2)*pow2(cos12));
	poly.c[3] = 4*(a2_m_c2_div_b2*(1-a2_m_c2_div_b2)*cos13 - (1 - a2_p_c2_div_b2)*cos23*cos12 + 2*c2_div_b2*pow2(cos23)*cos13);
	poly.c[4] = -4*c2_div_b2*cos23*cos23 + pow2(a2_m_c2_div_b2 - 1);

	// solve for real roots
	solutions.reset();
	if( !rootFinder.process(poly) )
		return false;

	List<Complex64F> roots = rootFinder.getRoots();

	for( Complex64F r : roots ) {
		if( !r.isReal() ) {
			continue;
		}

		double v = r.real;
		double u = ((-1 + a2_div_b2 - c2_div_b2)*v*v - 2*(a2_div_b2 - c2_div_b2)*cos13*v + 1 + a2_div_b2 - c2_div_b2)/
				(2*(cos12 - v*cos23));

		// compute the distance of each point
		PointDistance3 s = solutions.grow();

		s.dist1 = Math.sqrt(a*a/(u*u + v*v - 2*u*v*cos23));
		s.dist2 = s.dist1*u;
		s.dist3 = s.dist1*v;
	}

	return solutions.size() != 0;
}
 

import org.ejml.data.Complex64F; //导入依赖的package包/类
/**
	 * @inheritDoc
	 */
	public boolean process( Point2D_F64 obs1 , Point2D_F64 obs2, Point2D_F64 obs3,
							double length23 , double length13 , double length12 ) {

		solutions.reset();

		cos12 = computeCosine(obs1,obs2); // cos(gama)
		cos13 = computeCosine(obs1,obs3); // cos(beta)
		cos23 = computeCosine(obs2,obs3); // cos(alpha)

		double a = length23, b = length13, c = length12;

		double a2_d_b2 = (a/b)*(a/b);
		double c2_d_b2 = (c/b)*(c/b);

		a2=a*a;  b2=b*b;  c2 = c*c;

//		poly.c[0] = a2*(a2*pow2(sin13) - b2*pow2(sin23));
//		poly.c[1] = b2*(b2-c2)*pow2(sin23) + a2*(a2 + 2*c2)*pow2(sin13) + 2*a2*b2*(-1 + cos23*cos13*cos12);
//		poly.c[2] = b2*(b2-a2)*pow2(sin12) + c2*(c2 + 2*a2)*pow2(sin13) + 2*b2*c2*(-1 + cos23*cos13*cos12);
//		poly.c[3] = c2*(c2*pow2(sin13) - b2*pow2(sin12) );

		// Auto generated code + hand simplification.  See P3PFinsterwalder.py  I prefer it over the equations found
		// in the paper (commented out above) since it does not require sin(theta).
		poly.c[0] = a2*(a2*(1 - pow2(cos13)) + b2*(pow2(cos23) - 1));
		poly.c[1] = 2*a2*b2*(cos12*cos13*cos23 - 1) + a2*(a2 + 2*c2)*(1 - pow2(cos13)) + b2*(b2 - c2)*( 1 - pow2(cos23));
		poly.c[2] = 2*c2*b2*(cos12*cos13*cos23 - 1) + c2*(c2 + 2*a2)*(1 - pow2(cos13)) + b2*(b2 - a2)*( 1 - pow2(cos12));
		poly.c[3] = c2*(b2*(pow2(cos12) - 1) + c2*( 1 - pow2(cos13)));

		if( poly.computeDegree() < 0 )
			return false;

		if( !rootFinder.process(poly) )
			return false;

		// search for real roots
		Complex64F root = null;
		for( Complex64F r : rootFinder.getRoots() ) {
			if( r.isReal() ) {
				root = r;
				break;
			}
		}

		if( root == null )
			return false;

		double lambda = root.real;

		double A = 1 + lambda;
		double B = -cos23;
		double C = 1 - a2_d_b2 - lambda*c2_d_b2;
		double D = -lambda*cos12;
		double E = (a2_d_b2 + lambda*c2_d_b2)*cos13;
		double F = -a2_d_b2 + lambda*(1-c2_d_b2);

		p = Math.sqrt(B*B - A*C);
		q = Math.signum(B*E - C*D)*Math.sqrt(E*E - C*F);

		computeU((-B+p)/C,(-E+q)/C);
		computeU((-B-p)/C,(-E-q)/C);

		return true;
	}