boofcv.alg.geo.f.EssentialNister5 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-2018, 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.f;

import boofcv.struct.geo.AssociatedPair;
import georegression.struct.point.Point2D_F64;
import org.ddogleg.solver.Polynomial;
import org.ddogleg.solver.PolynomialRoots;
import org.ddogleg.solver.impl.FindRealRootsSturm;
import org.ddogleg.solver.impl.WrapRealRootsSturm;
import org.ddogleg.struct.FastQueue;
import org.ejml.data.Complex_F64;
import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;
import org.ejml.dense.row.factory.LinearSolverFactory_DDRM;
import org.ejml.dense.row.linsol.qr.SolveNullSpaceQRP_DDRM;
import org.ejml.interfaces.SolveNullSpace;
import org.ejml.interfaces.linsol.LinearSolverDense;

import java.util.List;

/**
 * 
 * Finds the essential matrix given exactly 5 corresponding points.  The approach is described
 * in details in [1] and works by linearlizing the problem then solving for the roots in a polynomial.  It
 * is considered one of the fastest and most stable solutions for the 5-point problem.
 * 
 *
 * 
 * THIS IMPLEMENTATION DOES NOT CONTAIN ALL THE OPTIMIZATIONS OUTLIED IN [1].  A full implementation is
 * quite involved. Example: SVD instead of the proposed QR factorization.
 * 
 *
 * 
 * NOTE: This solution could be generalized for an arbitrary number of points.  However, it would complicate
 * the algorithm even more and isn't considered to be worth the effort.
 * 
 *
 * 
 * [1] David Nister "An Efficient Solution to the Five-Point Relative Pose Problem"
 * Pattern Analysis and Machine Intelligence, 2004
 * 
 *
 * @author Peter Abeles
 */
public class EssentialNister5 {

	// Linear system describing p'*E*q = 0
	private DMatrixRMaj Q = new DMatrixRMaj(5,9);
	// contains the span of A
	private DMatrixRMaj nullspace = new DMatrixRMaj(9,9);
	private SolveNullSpace solverNull = new SolveNullSpaceQRP_DDRM();

	// where all the ugly equations go
	private HelperNister5 helper = new HelperNister5();

	// the span containing E
	private double []X = new double[9];
	private double []Y = new double[9];
	private double []Z = new double[9];
	private double []W = new double[9];

	// unknowns for E = x*X + y*Y + z*Z + W
	private double x,y,z;

	// Solver for the linear system below
	LinearSolverDense solver = LinearSolverFactory_DDRM.linear(10);

	// Storage for linear systems
	private DMatrixRMaj A1 = new DMatrixRMaj(10,10);
	private DMatrixRMaj A2 = new DMatrixRMaj(10,10);
	private DMatrixRMaj C = new DMatrixRMaj(10,10);

	// Used for finding polynomial roots
	private FindRealRootsSturm sturm = new FindRealRootsSturm(11,-1,1e-10,20,20);
	private PolynomialRoots findRoots = new WrapRealRootsSturm(sturm);

	// private PolynomialRoots findRoots = new RootFinderCompanion();
	private Polynomial poly = new Polynomial(11);

	/**
	 * 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 points , FastQueue 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( Complex_F64 c : findRoots.getRoots() ) {
			if( !c.isReal() )
				continue;

			solveForXandY(c.real);

			DMatrixRMaj 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;
	}

	/**
	 * From the epipolar constraint p2^T*E*p1 = 0 construct a linear system
	 * and find its null space.
	 */
	private void computeSpan( List points ) {

		Q.reshape(points.size(), 9);
		int index = 0;

		for( int i = 0; i < points.size(); i++ ) {
			AssociatedPair p = points.get(i);

			Point2D_F64 a = p.p2;
			Point2D_F64 b = p.p1;

			// The points are assumed to be in homogeneous coordinates.  This means z = 1
			Q.data[index++] =  a.x*b.x;
			Q.data[index++] =  a.x*b.y;
			Q.data[index++] =  a.x;
			Q.data[index++] =  a.y*b.x;
			Q.data[index++] =  a.y*b.y;
			Q.data[index++] =  a.y;
			Q.data[index++] =      b.x;
			Q.data[index++] =      b.y;
			Q.data[index++] =  1;
		}

		if( !solverNull.process(Q,4,nullspace) )
			throw new RuntimeException("Nullspace solver should never fail, probably bad input");

		// extract the span of solutions for E from the null space
		for( int i = 0; i < 9; i++ ) {
			X[i] = nullspace.unsafe_get(i,0);
			Y[i] = nullspace.unsafe_get(i,1);
			Z[i] = nullspace.unsafe_get(i,2);
			W[i] = nullspace.unsafe_get(i,3);
		}
	}

	DMatrixRMaj tmpA = new DMatrixRMaj(3,2);
	DMatrixRMaj tmpY = new DMatrixRMaj(3,1);
	DMatrixRMaj tmpX = new DMatrixRMaj(2,1);
	/**
	 * Once z is known then x and y can be solved for using the B matrix
	 */
	private void solveForXandY( double z ) {
		this.z = z;

		// solve for x and y using the first two rows of B
		tmpA.data[0] = ((helper.K00*z + helper.K01)*z + helper.K02)*z + helper.K03;
		tmpA.data[1] = ((helper.K04*z + helper.K05)*z + helper.K06)*z + helper.K07;
		tmpY.data[0] = (((helper.K08*z + helper.K09)*z + helper.K10)*z + helper.K11)*z + helper.K12;

		tmpA.data[2] = ((helper.L00*z + helper.L01)*z + helper.L02)*z + helper.L03;
		tmpA.data[3] = ((helper.L04*z + helper.L05)*z + helper.L06)*z + helper.L07;
		tmpY.data[1] = (((helper.L08*z + helper.L09)*z + helper.L10)*z + helper.L11)*z + helper.L12;

		tmpA.data[4] = ((helper.M00*z + helper.M01)*z + helper.M02)*z + helper.M03;
		tmpA.data[5] = ((helper.M04*z + helper.M05)*z + helper.M06)*z + helper.M07;
		tmpY.data[2] = (((helper.M08*z + helper.M09)*z + helper.M10)*z + helper.M11)*z + helper.M12;

		CommonOps_DDRM.scale(-1,tmpY);
		CommonOps_DDRM.solve(tmpA,tmpY,tmpX);

		this.x = tmpX.get(0,0);
		this.y = tmpX.get(1,0);
	}
}