Java CommonOps.multTransB方法代码示例

import org.ejml.ops.CommonOps; //导入方法依赖的package包/类
@Override
public void encode(Se3_F64 input, double[] output) {

	// force the "rotation matrix" to be an exact rotation matrix
	// otherwise Rodrigues will have issues with the noise
	if( !svd.decompose(input.getR()) )
		throw new RuntimeException("SVD failed");

	DenseMatrix64F U = svd.getU(null,false);
	DenseMatrix64F V = svd.getV(null,false);

	CommonOps.multTransB(U, V, R);

	// extract Rodrigues coordinates
	RotationMatrixGenerator.matrixToRodrigues(R,rotation);

	output[0] = rotation.unitAxisRotation.x*rotation.theta;
	output[1] = rotation.unitAxisRotation.y*rotation.theta;
	output[2] = rotation.unitAxisRotation.z*rotation.theta;

	Vector3D_F64 T = input.getT();

	output[3] = T.x;
	output[4] = T.y;
	output[5] = T.z;
}
 

import org.ejml.ops.CommonOps; //导入方法依赖的package包/类
/**
 * There are four possible reconstructions from an essential matrix.  This function will compute different
 * permutations depending on optionA and optionB being true or false.
 */
private void extractTransform( DenseMatrix64F U , DenseMatrix64F V , DenseMatrix64F S ,
							   Se3_F64 se , boolean optionA , boolean optionB )
{
	DenseMatrix64F R = se.getR();
	Vector3D_F64 T = se.getT();

	// extract rotation
	if( optionA )
		CommonOps.mult(U,Rz,temp);
	else
		CommonOps.multTransB(U,Rz,temp);
	CommonOps.multTransB(temp,V,R);

	// extract screw symmetric translation matrix
	if( optionB )
		CommonOps.multTransB(U,Rz,temp);
	else
		CommonOps.mult(U,Rz,temp);
	CommonOps.mult(temp,S,temp2);
	CommonOps.multTransB(temp2,U,temp);

	T.x = temp.get(2,1);
	T.y = temp.get(0,2);
	T.z = temp.get(1,0);
}
 

import org.ejml.ops.CommonOps; //导入方法依赖的package包/类
/**
 * Projects the found estimate of E onto essential space.
 *
 * @return true if svd returned true.
 */
protected boolean projectOntoEssential( DenseMatrix64F E ) {
	if( !svdConstraints.decompose(E) ) {
		return false;
	}
	svdV = svdConstraints.getV(svdV,false);
	svdU = svdConstraints.getU(svdU,false);
	svdS = svdConstraints.getW(svdS);

	SingularOps.descendingOrder(svdU, false, svdS, svdV, false);

	// project it into essential space
	// the scale factor is arbitrary, but the first two singular values need
	// to be the same.  so just set them to one
	svdS.unsafe_set(0, 0, 1);
	svdS.unsafe_set(1, 1, 1);
	svdS.unsafe_set(2, 2, 0);

	// recompute F
	CommonOps.mult(svdU, svdS, temp0);
	CommonOps.multTransB(temp0,svdV, E);

	return true;
}
 

import org.ejml.ops.CommonOps; //导入方法依赖的package包/类
/**
 * Projects the found estimate of F onto Fundamental space.
 *
 * @return true if svd returned true.
 */
protected boolean projectOntoFundamentalSpace( DenseMatrix64F F ) {
	if( !svdConstraints.decompose(F) ) {
		return false;
	}
	svdV = svdConstraints.getV(svdV,false);
	svdU = svdConstraints.getU(svdU,false);
	svdS = svdConstraints.getW(svdS);

	SingularOps.descendingOrder(svdU, false, svdS, svdV, false);

	// the smallest singular value needs to be set to zero, unlike
	svdS.set(2, 2, 0);

	// recompute F
	CommonOps.mult(svdU, svdS, temp0);
	CommonOps.multTransB(temp0,svdV, F);

	return true;
}
 

import org.ejml.ops.CommonOps; //导入方法依赖的package包/类
private double getExponentTerm(double[] features) {
    DenseMatrix64F ft =new DenseMatrix64F(1,features.length);

    for(int i=0;i<features.length;i++){
        ft.set(0,i,features[i]-avg[i]);
    }

    DenseMatrix64F ft2=new DenseMatrix64F(1,features.length);
    CommonOps.mult(ft, inv, ft2);
    DenseMatrix64F ft3=new DenseMatrix64F(1,1);
    CommonOps.multTransB(1, ft2, ft, ft3);

    return FastMath.exp(-0.5 *ft3.get(0,0));
}
 

import org.ejml.ops.CommonOps; //导入方法依赖的package包/类
@Override
public int computeResiduals(AssociatedPair p, double[] residuals, int index) {

	GeometryMath_F64.mult(H, p.p1, temp);

	double top1 = error1(p.p1.x,p.p1.y,p.p2.x,p.p2.y);
	double top2 = error2(p.p1.x, p.p1.y, p.p2.x, p.p2.y);

	computeJacobian(p.p1,p.p2);
	// JJ = J*J'
	CommonOps.multTransB(J, J, JJ);

	// solve JJ'*x = -e
	e.data[0] = -top1;
	e.data[1] = -top2;

	if( solver.setA(JJ) ) {
		solver.solve(e,x);
		// -J'(J*J')^-1*e
		CommonOps.multTransA(J,x,error);
		residuals[index++] = error.data[0];
		residuals[index++] = error.data[1];
		residuals[index++] = error.data[2];
		residuals[index++] = error.data[3];
	} else {
		residuals[index++] = 0;
		residuals[index++] = 0;
		residuals[index++] = 0;
		residuals[index++] = 0;
	}

	return index;
}
 

import org.ejml.ops.CommonOps; //导入方法依赖的package包/类
/**
	 * Computes M'*M and finds the null space.  The 4 eigenvectors with the smallest eigenvalues are found
	 * and the null points extracted from them.
	 */
	protected void extractNullPoints( DenseMatrix64F M )
	{
		// compute MM and find its null space
		MM.reshape(M.numRows,M.numRows,false);
		CommonOps.multTransB(M, M, MM);

		if( !svd.decompose(MM) )
			throw new IllegalArgumentException("SVD failed?!?!");

		double []singularValues = svd.getSingularValues();
		DenseMatrix64F V = svd.getV(null,false);

		SingularOps.descendingOrder(null,false,singularValues,3,V,false);

		// extract null points from the null space
		for( int i = 0; i < numControl; i++ ) {
			int column = M.numRows-1-i;
//			nullPts[i].clear();
			for( int j = 0; j < numControl; j++ ) {
				Point3D_F64 p = nullPts[i].get(j);
//				Point3D_F64 p = new Point3D_F64();
				p.x = V.get(j*3+0,column);
				p.y = V.get(j*3+1,column);
				p.z = V.get(j*3+2,column);
//				nullPts[i].add(p);
			}
		}
	}
 

import org.ejml.ops.CommonOps; //导入方法依赖的package包/类
/**
 * <p>
 * Since the linear solution is probably not an exact rotation matrix, this code finds the best
 * approximation.
 * </p>
 *
 * See page 280 of [1]
 */
private void massageResults() {
	DenseMatrix64F R = motion.getR();
	Vector3D_F64 T = motion.getT();

	if( !svd.decompose(R))
		throw new RuntimeException("SVD Failed");

	CommonOps.multTransB(svd.getU(null,false),svd.getV(null,false),R);

	// determinant should be +1
	double det = CommonOps.det(R);

	if( det < 0 )
		CommonOps.scale(-1,R);

	// compute the determinant of the singular matrix
	double b = 1.0;
	double s[] = svd.getSingularValues();

	for( int i = 0; i < svd.numberOfSingularValues(); i++ ) {
		b *= s[i];
	}

	b = Math.signum(det)/Math.pow(b,1.0/3.0);

	GeometryMath_F64.scale(T,b);
}
 

import org.ejml.ops.CommonOps; //导入方法依赖的package包/类
/**
 * R = W*U^T
 * N = v2 cross u
 * (1/d)*T = (H-R)*N
 */
private void createSolution( DenseMatrix64F W , DenseMatrix64F U , Vector3D_F64 u ,
							 DenseMatrix64F H ,
							 Se3_F64 se , Vector3D_F64 N )
{
	CommonOps.multTransB(W,U,se.getR());
	GeometryMath_F64.cross(v2,u,N);

	CommonOps.sub(H, se.getR(),tempM);
	GeometryMath_F64.mult(tempM,N,se.getT());
}
 

import org.ejml.ops.CommonOps; //导入方法依赖的package包/类
ConditionalVarianceAndTransform2(final DenseMatrix64F variance,
                                        final int[] missingIndices, final int[] notMissingIndices) {

    assert (missingIndices.length + notMissingIndices.length == variance.getNumRows());
    assert (missingIndices.length + notMissingIndices.length == variance.getNumCols());

    this.missingIndices = missingIndices;
    this.notMissingIndices = notMissingIndices;

    if (DEBUG) {
        System.err.println("variance:\n" + variance);
    }

    DenseMatrix64F S22 = new DenseMatrix64F(notMissingIndices.length, notMissingIndices.length);
    gatherRowsAndColumns(variance, S22, notMissingIndices, notMissingIndices);

    if (DEBUG) {
        System.err.println("S22:\n" + S22);
    }

    DenseMatrix64F S22Inv = new DenseMatrix64F(notMissingIndices.length, notMissingIndices.length);
    CommonOps.invert(S22, S22Inv);

    if (DEBUG) {
        System.err.println("S22Inv:\n" + S22Inv);
    }

    DenseMatrix64F S12 = new DenseMatrix64F(missingIndices.length, notMissingIndices.length);
    gatherRowsAndColumns(variance, S12, missingIndices, notMissingIndices);

    if (DEBUG) {
        System.err.println("S12:\n" + S12);
    }

    DenseMatrix64F S12S22Inv = new DenseMatrix64F(missingIndices.length, notMissingIndices.length);
    CommonOps.mult(S12, S22Inv, S12S22Inv);

    if (DEBUG) {
        System.err.println("S12S22Inv:\n" + S12S22Inv);
    }

    DenseMatrix64F S12S22InvS21 = new DenseMatrix64F(missingIndices.length, missingIndices.length);
    CommonOps.multTransB(S12S22Inv, S12, S12S22InvS21);

    if (DEBUG) {
        System.err.println("S12S22InvS21:\n" + S12S22InvS21);
    }

    sBar = new DenseMatrix64F(missingIndices.length, missingIndices.length);
    gatherRowsAndColumns(variance, sBar, missingIndices, missingIndices);
    CommonOps.subtract(sBar, S12S22InvS21, sBar);


    if (DEBUG) {
        System.err.println("sBar:\n" + sBar);
    }


    this.affineTransform = S12S22Inv;
    this.tempStorage = new double[missingIndices.length];

    this.numMissing = missingIndices.length;
    this.numNotMissing = notMissingIndices.length;

}
 

import org.ejml.ops.CommonOps; //导入方法依赖的package包/类
@Override
public void encode(CalibratedPoseAndPoint model, double[] param) {
	int paramIndex = 0;

	// first decode the transformation
	for( int i = 0; i < numViews; i++ ) {
		// don't encode if it is already known
		if( knownView[i] )
			continue;

		Se3_F64 se = model.getWorldToCamera(i);

		// force the "rotation matrix" to be an exact rotation matrix
		// otherwise Rodrigues will have issues with the noise
		if( !svd.decompose(se.getR()) )
			throw new RuntimeException("SVD failed");

		DenseMatrix64F U = svd.getU(null,false);
		DenseMatrix64F V = svd.getV(null,false);

		CommonOps.multTransB(U,V,R);

		// extract Rodrigues coordinates
		RotationMatrixGenerator.matrixToRodrigues(R,rotation);

		param[paramIndex++] = rotation.unitAxisRotation.x*rotation.theta;
		param[paramIndex++] = rotation.unitAxisRotation.y*rotation.theta;
		param[paramIndex++] = rotation.unitAxisRotation.z*rotation.theta;

		Vector3D_F64 T = se.getT();

		param[paramIndex++] = T.x;
		param[paramIndex++] = T.y;
		param[paramIndex++] = T.z;
	}

	for( int i = 0; i < numPoints; i++ ) {
		Point3D_F64 p = model.getPoint(i);

		param[paramIndex++] = p.x;
		param[paramIndex++] = p.y;
		param[paramIndex++] = p.z;
	}
}