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 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) 2021, 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.alg.geo.f.EpipolarMinimizeGeometricError;
import boofcv.struct.geo.AssociatedPair;
import georegression.geometry.GeometryMath_F64;
import georegression.struct.point.Point2D_F64;
import georegression.struct.point.Point3D_F64;
import org.ddogleg.struct.DogArray;
import org.ejml.LinearSolverSafe;
import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.factory.LinearSolverFactory_DDRM;
import org.ejml.interfaces.linsol.LinearSolverDense;
import org.jetbrains.annotations.Nullable;

/**
 * 
 * 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] Page 332, R. Hartley, and A. Zisserman, "Multiple View Geometry in Computer Vision", 2nd Ed, Cambridge 2003
 * 
 *
 * @author Peter Abeles
 */
public class HomographyInducedStereo3Pts {

	// Input fundamental matrix
	private final DMatrixRMaj F21 = new DMatrixRMaj(3, 3);

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

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

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

	private final DMatrixRMaj temp0 = new DMatrixRMaj(3, 1);
	private final DMatrixRMaj temp1 = new DMatrixRMaj(3, 1);

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

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

	private final LinearSolverDense solver;

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

	// Adjusts points to minimize geometric error
	EpipolarMinimizeGeometricError adjusterEpipolar = new EpipolarMinimizeGeometricError();
	private final DogArray adjustedPairs = new DogArray<>(AssociatedPair::new);

	public HomographyInducedStereo3Pts() {
		// ensure that the inputs are not modified
		solver = new LinearSolverSafe<>(LinearSolverFactory_DDRM.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( DMatrixRMaj F, @Nullable Point3D_F64 e2 ) {
		if (e2 != null)
			this.e2.setTo(e2);
		else {
			MultiViewOps.extractEpipoles(F, new Point3D_F64(), this.e2);
		}
		GeometryMath_F64.multCrossA(this.e2, F, A);
		this.F21.setTo(F);
	}

	/**
	 * 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 ) {
		// Computed corrected points that minimize epipolar error
		adjustedPairs.resize(3);
		adjustEpipolar(p1, adjustedPairs.get(0));
		adjustEpipolar(p2, adjustedPairs.get(1));
		adjustEpipolar(p3, adjustedPairs.get(2));

		// The algorithm in the book doesn't appear to be terribly stable.
		// One possible way to improve it is to normalize the inputs so that they have a magnitude around one
		// This is a bit of a pain and nothing is using the code right now. I'm being lazy
		// but at least I'm documenting my laziness
//		LowLevelMultiViewOps.computeNormalization(adjustedPairs.toList(), N1, N2);

		// Fill rows of M with observations from image 1
		fillM(adjustedPairs.get(0).p1, adjustedPairs.get(1).p1, adjustedPairs.get(2).p1);

		// Compute 'b' vector
		b.x = computeB(adjustedPairs.get(0).p2);
		b.y = computeB(adjustedPairs.get(1).p2);
		b.z = computeB(adjustedPairs.get(2).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;
	}

	private void adjustEpipolar( AssociatedPair a, AssociatedPair adjusted ) {
		adjusterEpipolar.process(F21, a.p1.x, a.p1.y, a.p2.x, a.p2.y, adjusted.p1, adjusted.p2);
	}

	/**
	 * 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 DMatrixRMaj getHomography() {
		return H;
	}
}