main/src/georegression/struct/se/SpecialEuclideanOps_F64.java
public static boolean bestFit( Se3_F64 a ) {
		SingularValueDecomposition_F64<DMatrixRMaj> svd = DecompositionFactory_DDRM.svd(true,true,true);

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

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

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

		if( det < 0 ) {
			CommonOps_DDRM.scale(-1, a.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(a.T,b);

		return true;
	}
public static boolean bestFit( Se3_F64 a ) {
		SingularValueDecomposition_F64<DMatrixRMaj> svd = DecompositionFactory_DDRM.svd(true,true,true);

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

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

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

		if( det < 0 ) {
			CommonOps_DDRM.scale(-1, a.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(a.T,b);

		return true;
	}
public static boolean bestFit ( Se3_F64 a ) { SingularValueDecomposition_F64 < DMatrixRMaj > svd = DecompositionFactory_DDRM . svd ( true , true , true ) ; if ( ! svd . decompose ( a . R ) ) throw new RuntimeException ( "SVD Failed" ) ; CommonOps_DDRM . multTransB ( svd . getU ( null , false ) , svd . getV ( null , false ) , a . R ) ; // determinant should be +1 double det = CommonOps_DDRM . det ( a . R ) ; if ( det < 0 ) { CommonOps_DDRM . scale ( - 1 , a . 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 ( a . T , b ) ; return true ; }
Finds the best fit projection of 'a' onto SE(3).  This is useful when a was estimated using a linear algorithm.

<p>See page 280 of "An Invitation to 3-D Vision, From Images to Geometric Models" 1st Ed. 2004. Springer.</p>
@param a Approximate SE(3). Modified.
@return true if successful
Finds the best fit projection of a onto SE ( 3 ) . This is useful when a was estimated using a linear algorithm .