boofcv.alg.geo.h.HomographyInducedStereo3Pts Maven / Gradle / Ivy

Go to download
Show more of this group Show more artifacts with this name
Show all versions of geo Show documentation
BoofCV is an open source Java library for real-time computer vision and robotics applications.
There is a newer version: 0.26
/*
 * Copyright (c) 2011-2014, 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.h;

import boofcv.alg.geo.MultiViewOps;
import boofcv.struct.geo.AssociatedPair;
import georegression.geometry.GeometryMath_F64;
import georegression.struct.point.Point2D_F64;
import georegression.struct.point.Point3D_F64;
import org.ejml.alg.dense.linsol.LinearSolverSafe;
import org.ejml.data.DenseMatrix64F;
import org.ejml.factory.LinearSolverFactory;
import org.ejml.interfaces.linsol.LinearSolver;

/**
 * 
 * Computes the homography induced by a plane from 3 point correspondences.  Works with both calibrated and
 * uncalibrated cameras.  The Fundamental/Essential matrix must be known.  The found homography will be from view 1
 * to view 2.  The passed in Fundamental matrix must have the following properties for each set of
 * point correspondences: x2*F*x1 = 0, where x1 and x2 are views of the point in image 1 and image 2 respectively.
 * For more information see [1].
 * 
 *
 * 
 * [1] R. Hartley, and A. Zisserman, "Multiple View Geometry in Computer Vision", 2nd Ed, Cambridge 2003
 * 
 *
 * @author Peter Abeles
 */
public class HomographyInducedStereo3Pts {

	// Epipole in camera 2
	private Point3D_F64 e2 = new Point3D_F64();

	// The found homography from view 1 to view 2
	private DenseMatrix64F H = new DenseMatrix64F(3,3);

	// A = cross(e2)*F
	private DenseMatrix64F A = new DenseMatrix64F(3,3);
	// Rows filled with x from image 1
	private DenseMatrix64F M = new DenseMatrix64F(3,3);

	private DenseMatrix64F temp0 = new DenseMatrix64F(3,1);
	private DenseMatrix64F temp1 = new DenseMatrix64F(3,1);

	private Point3D_F64 A_inv_b = new Point3D_F64();
	private Point3D_F64 Ax = new Point3D_F64();
	private Point3D_F64 b = new Point3D_F64();

	private Point3D_F64 t0 = new Point3D_F64();
	private Point3D_F64 t1 = new Point3D_F64();

	private LinearSolver solver;

	// pick a reasonable scale and sign
	private AdjustHomographyMatrix adjust = new AdjustHomographyMatrix();

	public HomographyInducedStereo3Pts()
	{
		// ensure that the inputs are not modified
		solver = new LinearSolverSafe(LinearSolverFactory.linear(3));
	}

   /**
	 * Specify the fundamental matrix and the camera 2 epipole.
	 *
	 * @param F Fundamental matrix.
	 * @param e2 Epipole for camera 2.  If null it will be computed internally.
	 */
	public void setFundamental( DenseMatrix64F F , Point3D_F64 e2 ) {
		if( e2 != null )
			this.e2.set(e2);
		else {
			MultiViewOps.extractEpipoles(F,new Point3D_F64(),this.e2);
		}
		GeometryMath_F64.multCrossA(this.e2,F,A);
	}

	/**
	 * Estimates the homography from view 1 to view 2 induced by a plane from 3 point associations.
	 * Each pair must pass the epipolar constraint.  This can fail if the points are colinear.
	 *
	 * @param p1 Associated point observation
	 * @param p2 Associated point observation
	 * @param p3 Associated point observation
	 * @return True if successful or false if it failed
	  */
	public boolean process(AssociatedPair p1, AssociatedPair p2, AssociatedPair p3) {

		// Fill rows of M with observations from image 1
		fillM(p1.p1,p2.p1,p3.p1);

		// Compute 'b' vector
		b.x = computeB(p1.p2);
		b.y = computeB(p2.p2);
		b.z = computeB(p3.p2);

		// A_inv_b = inv(A)*b
		if( !solver.setA(M) )
			return false;

		GeometryMath_F64.toMatrix(b,temp0);
		solver.solve(temp0,temp1);
		GeometryMath_F64.toTuple3D(temp1, A_inv_b);

		GeometryMath_F64.addOuterProd(A, -1, e2, A_inv_b, H);

		// pick a good scale and sign for H
		adjust.adjust(H, p1);

		return true;
	}

	/**
	 * Fill rows of M with observations from image 1
	 */
	private void fillM( Point2D_F64 x1 , Point2D_F64 x2 , Point2D_F64 x3 ) {
		M.data[0] = x1.x; M.data[1] = x1.y; M.data[2] = 1;
		M.data[3] = x2.x; M.data[4] = x2.y; M.data[5] = 1;
		M.data[6] = x3.x; M.data[7] = x3.y; M.data[8] = 1;
	}

	/**
	 * b = [(x cross (A*x))^T ( x cross e2 )] / || x cross e2 ||^2
	 */
	private double computeB( Point2D_F64 x ) {
		GeometryMath_F64.mult(A,x,Ax);

		GeometryMath_F64.cross(x,Ax,t0);
		GeometryMath_F64.cross(x,e2,t1);

		double top = GeometryMath_F64.dot(t0,t1);
		double bottom = t1.normSq();

		return top/bottom;
	}

	/**
	 * The found homography from view 1 to view 2
	 * @return homography
	 */
	public DenseMatrix64F getHomography() {
		return H;
	}
}