boofcv.alg.geo.f.ParamFundamentalEpipolar 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.f;

import boofcv.alg.geo.MultiViewOps;
import georegression.struct.point.Point3D_F64;
import org.ddogleg.fitting.modelset.ModelCodec;
import org.ejml.UtilEjml;
import org.ejml.data.DMatrixRMaj;
import org.ejml.simple.SimpleMatrix;

/**
 * 
 * Parameterizes F by specifying the first two columns and the third being a linear combination of
 * the first two. By setting one of the elements in f1 or f2 to be one, it can achieve the minimum
 * possible parameter size of 7. Care is taken to avoid the degenerate case when f1 and f2
 * are linearly dependent.
 * 
 * 
 * F=[f1 , f2 , αf1 + β f2]
 * 
 *
 * 
 * Page 286 in: R. Hartley, and A. Zisserman, "Multiple View Geometry in Computer Vision", 2nd Ed, Cambridge 2003 
 * 
 *
 * @author Peter Abeles
 */
public class ParamFundamentalEpipolar implements ModelCodec {

	// order of columns
	int col0, col1, col2;
	// map from index in parameter to index in F
	// last index is the one index
	int indexes[] = new int[6];

	@Override
	public int getParamLength() {
		return 7;
	}

	/**
	 * Examines the matrix structure to determine how to parameterize F.
	 */
	@Override
	public void encode( DMatrixRMaj F, double[] param ) {
		// see which columns are to be used
		selectColumns(F);

		// set the largest element in the first two columns and normalize
		// using that value
		double v[] = new double[]{F.get(0, col0), F.get(1, col0), F.get(2, col0),
				F.get(0, col1), F.get(1, col1), F.get(2, col1)};
		double divisor = selectDivisor(v, param);

		// solve for alpha and beta and put into param
		SimpleMatrix A = new SimpleMatrix(3, 2);
		SimpleMatrix y = new SimpleMatrix(3, 1);
		for (int i = 0; i < 3; i++) {
			A.set(i, 0, v[i]);
			A.set(i, 1, v[i + 3]);
			y.set(i, 0, F.get(i, col2)/divisor);
		}

		SimpleMatrix x = A.solve(y);

		param[5] = x.get(0);
		param[6] = x.get(1);
	}

	/**
	 * The divisor is the element in the first two columns that has the largest absolute value.
	 * Finds this element, sets col0 to be the row which contains it, and specifies which elements
	 * in that column are to be used.
	 *
	 * @return Value of the divisor
	 */
	private double selectDivisor( double v[], double param[] ) {

		double maxValue = 0;
		int maxIndex = 0;
		for (int i = 0; i < v.length; i++) {
			if (Math.abs(v[i]) > maxValue) {
				maxValue = Math.abs(v[i]);
				maxIndex = i;
			}
		}

		double divisor = v[maxIndex];

		int index = 0;
		for (int i = 0; i < v.length; i++) {
			v[i] /= divisor;

			if (i != maxIndex) {
				// save first 5 parameters
				param[index] = v[i];
				// save indexes in the matrix
				int col = i < 3 ? col0 : col1;
				indexes[index++] = 3*(i%3) + col;
			}
		}

		// index of 1
		int col = maxIndex >= 3 ? col1 : col0;
		indexes[5] = 3*(maxIndex%3) + col;

		return divisor;
	}

	private void selectColumns( DMatrixRMaj F ) {
		Point3D_F64 e1 = new Point3D_F64();
		Point3D_F64 e2 = new Point3D_F64();

		MultiViewOps.extractEpipoles(F, e1, e2);

		// if the right epipole lies at infinity (z=0) then don't use the first two columns
		if (Math.abs(e2.z) <= UtilEjml.EPS) {
			col0 = 1; col1 = 2; col2 = 0;
		} else {
			col0 = 0; col1 = 1; col2 = 2;
		}
	}

	@Override
	public void decode( double[] input, DMatrixRMaj F ) {
		F.data[indexes[0]] = input[0];
		F.data[indexes[1]] = input[1];
		F.data[indexes[2]] = input[2];
		F.data[indexes[3]] = input[3];
		F.data[indexes[4]] = input[4];
		F.data[indexes[5]] = 1;

		double alpha = input[5];
		double beta = input[6];

		F.data[col2] = alpha*F.data[col0] + beta*F.data[col1];
		F.data[col2 + 3] = alpha*F.data[col0 + 3] + beta*F.data[col1 + 3];
		F.data[col2 + 6] = alpha*F.data[col0 + 6] + beta*F.data[col1 + 6];
	}
}