boofcv.alg.geo.pose.Relinearlize Maven / Gradle / Ivy

Go to download
Show more of this group Show more artifacts with this name
Show all versions of boofcv-geo Show documentation
BoofCV is an open source Java library for real-time computer vision and robotics applications.
There is a newer version: 1.1.7
/*
 * Copyright (c) 2011-2017, Peter Abeles. All Rights Reserved.
 *
 * This file is part of BoofCV (http://boofcv.org).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package boofcv.alg.geo.pose;

import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;
import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
import org.ejml.dense.row.factory.LinearSolverFactory_DDRM;
import org.ejml.interfaces.decomposition.SingularValueDecomposition;
import org.ejml.interfaces.linsol.LinearSolverDense;

/**
 * 
 * Used in case 4 of EPnP.  See [1] for details.  This technique appears to not be all that accurate
 * in practice, but better than nothing.  Or maybe there is a bug in the implementation below
 * since even with perfect data it appears to generate large errors.  One possible source
 * of implementation error is that the entire null space is not being used.  More of
 * the null space (particularly in the planar case) could be used if more relinearization
 * was run multiple times.
 * 
 *
 * 
 * [1] Aviad Kipnis and Adi Shamir, "Cryptanalysis of HPE Public Key Cryptosystem" 1999
 * 
 *
 * @author Peter Abeles
 */
public class Relinearlize {

	// number of control points.  4 for general 3 for planar
	int numControl;

	// how much of the null space is sued
	int numNull;

	// contains the null space
	DMatrixRMaj V;
	// contains one possible solution
	DMatrixRMaj x0 = new DMatrixRMaj(1,1);
	// lookup table for indices
	int table[] = new int[10*10];

	SingularValueDecomposition svd = DecompositionFactory_DDRM.svd(3, 3, false, true, false);

	// used inside of solveConstraintMatrix
	DMatrixRMaj AA = new DMatrixRMaj(1,1);
	DMatrixRMaj yy = new DMatrixRMaj(1,1);
	DMatrixRMaj xx = new DMatrixRMaj(1,1);

	// used to compute one possible solution
	LinearSolverDense pseudo = LinearSolverFactory_DDRM.pseudoInverse(true);

	// stores constraints
	double XiiXjk[] = new double[10];
	double XikXji[] = new double[10];

	/**
	 * Specified the number of control points.
	 *
	 * @param numControl 3 = planar, 4 = general
	 */
	public void setNumberControl( int numControl ) {
		this.numControl = numControl;

		if( numControl == 4 ) {
			x0.reshape(10,1,false);
			AA.reshape(10,9,false);
			yy.reshape(10,1,false);
			xx.reshape(9,1,false);
			numNull = 3;
		} else {
			x0.reshape(6,1,false);
			AA.reshape(4,2,false);
			yy.reshape(4,1,false);
			xx.reshape(2,1,false);
			numNull = 1;
		}

		int index = 0;
		for( int i = 0; i < numControl; i++ ) {
			for( int j = i; j < numControl; j++ ) {
				table[i*numControl+j] = table[j*numControl+i] = index++;
			}
		}
	}

	/**
	 * Estimates betas using relinearization.
	 *
	 * @param L_full Linear constraint matrix
	 * @param y distances between world control points
	 * @param betas Estimated betas.  Output.
	 */
	public void process( DMatrixRMaj L_full , DMatrixRMaj y , double betas[] ) {

		svd.decompose(L_full);

		// extract null space
		V = svd.getV(null,true);

		// compute one possible solution
		pseudo.setA(L_full);
		pseudo.solve(y,x0);

		// add additional constraints to reduce the number of possible solutions
		DMatrixRMaj alphas = solveConstraintMatrix();

		// compute the final solution
		for( int i = 0; i < x0.numRows; i++ ) {
			for( int j = 0; j < numNull; j++ ) {
				x0.data[i] += alphas.data[j]*valueNull(j,i);
			}
		}

		if( numControl == 4 ) {
			betas[0] = Math.sqrt(Math.abs(x0.data[0]));
			betas[1] = Math.sqrt(Math.abs(x0.data[4]))*Math.signum(x0.data[1]);
			betas[2] = Math.sqrt(Math.abs(x0.data[7]))*Math.signum(x0.data[2]);
			betas[3] = Math.sqrt(Math.abs(x0.data[9]))*Math.signum(x0.data[3]);
		} else {
			betas[0] = Math.sqrt(Math.abs(x0.data[0]));
			betas[1] = Math.sqrt(Math.abs(x0.data[3]))*Math.signum(x0.data[1]);
			betas[2] = Math.sqrt(Math.abs(x0.data[5]))*Math.signum(x0.data[2]);
		}
	}

	/**
	 * Apply additional constraints to reduce the number of possible solutions
	 *
	 * x(k) = x_{ij} = bi*bj = x0(k) + a1*V0(k) + a2*V1(k) + a3*V2(k)
	 *
	 * constraint:
	 * x_{ii}*x_{jk} = x_{ik}*x_{ji}
	 *
	 */
	protected DMatrixRMaj solveConstraintMatrix() {

		int rowAA = 0;
		for( int i = 0; i < numControl; i++ ) {
			for( int j = i+1; j < numControl; j++ ) {
				for( int k = j; k < numControl; k++ , rowAA++ ) {
					// x_{ii}*x_{jk} = x_{ik}*x_{ji}
					extractXaXb(getIndex(i, i), getIndex(j, k), XiiXjk);
					extractXaXb(getIndex(i, k), getIndex(j, i), XikXji);

					for( int l = 1; l <= AA.numCols; l++ ) {
						AA.set(rowAA,l-1,XikXji[l]-XiiXjk[l]);
					}
					yy.set(rowAA,XiiXjk[0]-XikXji[0]);
				}
			}
		}
//		AA.print();
		CommonOps_DDRM.solve(AA, yy, xx);

		return xx;
	}

	public double valueNull( int which , int index ) {
		return V.get(V.numCols-numControl+which,index);
	}

	private int getIndex( int i , int j ) {
		return table[i*numControl+j];
	}

	private void extractXaXb(int indexA, int indexB, double quadratic[]) {

		double x0a = x0.get(indexA);
		double v0a = valueNull(0, indexA);

		double x0b = x0.get(indexB);
		double v0b = valueNull(0, indexB);

		if( numControl == 4 ) {
			double v1a = valueNull(1, indexA);
			double v2a = valueNull(2, indexA);
			
			double v1b = valueNull(1, indexB);
			double v2b = valueNull(2, indexB);
			multiplyQuadratic4(x0a,v0a,v1a,v2a,x0b,v0b,v1b,v2b,quadratic);
		} else {
			multiplyQuadratic2(x0a,v0a,x0b,v0b,quadratic);
		}
	}


	private void multiplyQuadratic4( double x0 , double x1 , double x2 , double x3 ,
									 double y0 , double y1 , double y2 , double y3 ,
									 double quadratic[] )
	{
		quadratic[0] = x0*y0;
		quadratic[1] = x0*y1 + y0*x1;
		quadratic[2] = x0*y2 + y0*x2;
		quadratic[3] = x0*y3 + y0*x3;
		quadratic[4] = x1*y1;
		quadratic[5] = x1*y2 + y1*x2;
		quadratic[6] = x1*y3 + y1*x3;
		quadratic[7] = x2*y2;
		quadratic[8] = x2*y3 + y2*x3;
		quadratic[9] = x3*y3;
	}

	private void multiplyQuadratic2( double x0 , double x1 ,
									 double y0 , double y1 ,
									 double quadratic[] )
	{
		quadratic[0] = x0*y0;
		quadratic[1] = x0*y1 + y0*x1;
		quadratic[2] = x1*y1;
	}
}