boofcv.alg.geo.DecomposeHomography 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
Show all versions of boofcv-geo Show documentation
BoofCV is an open source Java library for real-time computer vision and robotics applications.
/*
* 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;
import georegression.geometry.GeometryMath_F64;
import georegression.struct.point.Vector3D_F64;
import georegression.struct.se.Se3_F64;
import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;
import org.ejml.dense.row.SingularOps_DDRM;
import org.ejml.dense.row.decomposition.svd.SafeSvd_DDRM;
import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
import org.ejml.interfaces.decomposition.SingularValueDecomposition;
import java.util.ArrayList;
import java.util.List;
/**
*
* Decomposes a homography matrix to extract its internal geometric structure. There are four possible solutions,
* with two that are physically possible. The physically possible solution can be found by imposing a positive
* depth constraint. See {@link PositiveDepthConstraintCheck} for details on how to do that.
*
*
*
* A homography matrix is defined as H = (R + (1/d)*T*NT), where R is a 3x3 rotation matrix,
* d is the distance of the plane, N is the plane's normal, T is the translation vector. The decomposition
* works by computing the SVD of HTH and the following the procedure outlines in [1].
*
*
*
* The input homography is assumed to be from view 'a' to view 'b'. Then the resulting transform (R,T) is the
* transform from view 'a' to view 'b'. The values of d and N are all from the view 'a' perspective. Since there's a
* scale ambiguity the value of d is assumed to be 1 and T is scaled appropriately.
*
*
*
* "An Invitation to 3-D Vision" 1st edition page 137.
*
*
* @author Peter Abeles
*/
public class DecomposeHomography {
private final SingularValueDecomposition svd;
// storage for the four possible solutions
// Camera motion part of the solution
final List solutionsSE = new ArrayList<>();
// normal of the plane
final List solutionsN = new ArrayList<>();
// used for internal house keeping during decomposition
final Vector3D_F64 u1 = new Vector3D_F64();
final Vector3D_F64 u2 = new Vector3D_F64();
final Vector3D_F64 v2 = new Vector3D_F64();
final Vector3D_F64 tempV = new Vector3D_F64();
final DMatrixRMaj W1 = new DMatrixRMaj(3, 3);
final DMatrixRMaj W2 = new DMatrixRMaj(3, 3);
final DMatrixRMaj U1 = new DMatrixRMaj(3, 3);
final DMatrixRMaj U2 = new DMatrixRMaj(3, 3);
final DMatrixRMaj Hv2 = new DMatrixRMaj(3, 3);
final DMatrixRMaj tempM = new DMatrixRMaj(3, 3);
// workspace for H
final DMatrixRMaj H = new DMatrixRMaj(3, 3);
public DecomposeHomography() {
for (int i = 0; i < 4; i++) {
solutionsN.add(new Vector3D_F64());
solutionsSE.add(new Se3_F64());
}
// insure that the inputs are not modified
svd = new SafeSvd_DDRM(DecompositionFactory_DDRM.svd(3, 3, false, true, false));
}
/**
* Decomposed the provided homography matrix into its R,T/d,N components. Four
* solutions will be produced and can be accessed with {@link #getSolutionsN()} and
* {@link #getSolutionsSE()}.
*
* @param homography Homography matrix. Not modified.
*/
public void decompose( DMatrixRMaj homography ) {
if (!svd.decompose(homography))
throw new RuntimeException("SVD failed somehow");
DMatrixRMaj V = svd.getV(null, false);
DMatrixRMaj S = svd.getW(null);
SingularOps_DDRM.descendingOrder(null, false, S, V, false);
double s0 = S.get(0, 0);
double s1 = S.get(1, 1); // This is the scale. If normalize it will be one
double s2 = S.get(2, 2);
// force s1 to be 1
s0 /= s1;
s2 /= s1;
CommonOps_DDRM.scale(1.0/s1, homography, this.H);
// square s0 and s1 for other parts of the calculations
s0 *= s0;
s2 *= s2;
v2.setTo(V.get(0, 1), V.get(1, 1), V.get(2, 1));
double a = Math.sqrt(1 - s2);
double b = Math.sqrt(s0 - 1);
double div = Math.sqrt(s0 - s2);
for (int i = 0; i < 3; i++) {
double e1 = (a*V.get(i, 0) + b*V.get(i, 2))/div;
double e2 = (a*V.get(i, 0) - b*V.get(i, 2))/div;
u1.setIdx(i, e1);
u2.setIdx(i, e2);
}
setU(U1, v2, u1);
setU(U2, v2, u2);
setW(W1, H, v2, u1);
setW(W2, H, v2, u2);
// create the four solutions
createSolution(W1, U1, u1, H, solutionsSE.get(0), solutionsN.get(0));
createSolution(W2, U2, u2, H, solutionsSE.get(1), solutionsN.get(1));
createMirrorSolution(0, 2);
createMirrorSolution(1, 3);
}
/**
*
* Returns the camera motion part of the solution. Note that se.T=(1/d)T
*
*
* WARNING: Data is reused each time decompose is called.
*
*
* @return Set of rigid body camera motions
*/
public List getSolutionsSE() {
return solutionsSE;
}
/**
*
* Returns the normal of the plane part of the solution
*
*
* WARNING: Data is reused each time decompose is called.
*
*
* @return Set of plane normals
*/
public List getSolutionsN() {
return solutionsN;
}
/**
* U=[a,b,hat(a)*b]
*/
private void setU( DMatrixRMaj U, Vector3D_F64 a, Vector3D_F64 b ) {
setColumn(U, 0, a);
setColumn(U, 1, b);
GeometryMath_F64.cross(a, b, tempV);
setColumn(U, 2, tempV);
}
/**
* W=[H*a,H*b,hat(H*a)*H*b]
*/
private void setW( DMatrixRMaj W, DMatrixRMaj H, Vector3D_F64 a, Vector3D_F64 b ) {
GeometryMath_F64.mult(H, b, tempV);
setColumn(W, 1, tempV);
GeometryMath_F64.mult(H, a, tempV);
setColumn(W, 0, tempV);
GeometryMath_F64.crossMatrix(tempV, Hv2);
CommonOps_DDRM.mult(Hv2, H, tempM);
GeometryMath_F64.mult(tempM, b, tempV);
setColumn(W, 2, tempV);
}
private void setColumn( DMatrixRMaj U, int column, Vector3D_F64 a ) {
U.set(0, column, a.x);
U.set(1, column, a.y);
U.set(2, column, a.z);
}
/**
* R = W*U^T
* N = v2 cross u
* (1/d)*T = (H-R)*N
*/
private void createSolution( DMatrixRMaj W, DMatrixRMaj U, Vector3D_F64 u,
DMatrixRMaj H,
Se3_F64 se, Vector3D_F64 N ) {
CommonOps_DDRM.multTransB(W, U, se.getR());
GeometryMath_F64.cross(v2, u, N);
CommonOps_DDRM.subtract(H, se.getR(), tempM);
GeometryMath_F64.mult(tempM, N, se.getT());
}
private void createMirrorSolution( int origIndex, int index ) {
Se3_F64 origSE = solutionsSE.get(origIndex);
Vector3D_F64 origN = solutionsN.get(origIndex);
Se3_F64 se = solutionsSE.get(index);
Vector3D_F64 N = solutionsN.get(index);
se.getR().setTo(origSE.getR());
N.x = -origN.x;
N.y = -origN.y;
N.z = -origN.z;
Vector3D_F64 origT = origSE.getT();
Vector3D_F64 T = se.getT();
T.x = -origT.x;
T.y = -origT.y;
T.z = -origT.z;
}
}