boofcv.alg.geo.impl.ProjectiveToIdentity 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.impl;

import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;
import org.ejml.dense.row.SingularOps_DDRM;
import org.ejml.dense.row.SpecializedOps_DDRM;
import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
import org.ejml.interfaces.decomposition.SingularValueDecomposition_F64;

/**
 * Uses SVD to compute the projective transform which will turn a matrix matrix into identity, e.g.
 * P*H = [I|0], where P is a 3x4 camera matrix and H is a 4x4 homography transform. H has the structure
 * [P'|u] where P' (4x3) is the pseudo inverse of P and u is the nullspace of P (4x1)
 *
 * @author Peter Abeles
 */
public class ProjectiveToIdentity {
	SingularValueDecomposition_F64 svd = DecompositionFactory_DDRM.svd(true,true,false);
	DMatrixRMaj Ut = new DMatrixRMaj(3,3);
	DMatrixRMaj Wt = new DMatrixRMaj(4,3);
	DMatrixRMaj V = new DMatrixRMaj(4,4);

	DMatrixRMaj tmp = new DMatrixRMaj(4,4);
	DMatrixRMaj ns = new DMatrixRMaj(4,1);

	// storage for pseudo inverse of P
	DMatrixRMaj PA = new DMatrixRMaj(4,3);

	/**
	 * Compute projective transform that converts P into identity
	 * @param P (Input) 3x4 camera matrix
	 * @return true if no errors
	 */
	public boolean process( DMatrixRMaj P ) {
		if( !svd.decompose(P) )
			return false;

		svd.getU(Ut,true);
		svd.getV(V,false);
		double sv[] = svd.getSingularValues();

		SingularOps_DDRM.descendingOrder(Ut,true,sv,3,V,false);

		// compute W+, which is transposed and non-negative inverted
		for (int i = 0; i < 3; i++) {
			Wt.unsafe_set(i,i, 1.0/sv[i]);
		}

		// get the pseudo inverse
		// A+ = V*(W+)*U'
		CommonOps_DDRM.mult(V,Wt,tmp);
		CommonOps_DDRM.mult(tmp, Ut,PA);

		// Vector U, which is P*U = 0
		SpecializedOps_DDRM.subvector(V,0,3,V.numRows,false,0,ns);

		return true;
	}

	/**
	 * Retrieve projective transform H
	 */
	public void computeH( DMatrixRMaj H ) {
		H.reshape(4,4);

		CommonOps_DDRM.insert(PA,H,0,0);

		for (int i = 0; i < 4; i++) {
			H.unsafe_set(i,3,ns.data[i]);
		}

	}

	public DMatrixRMaj getPseudoInvP() {
		return PA;
	}

	public DMatrixRMaj getU() {
		return ns;
	}
}