HomographyDecomposer.java
/*
* Copyright (C) 2017 Alberto Irurueta Carro (alberto@irurueta.com)
*
* 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 com.irurueta.ar.epipolar.estimators;
import com.irurueta.algebra.AlgebraException;
import com.irurueta.algebra.Matrix;
import com.irurueta.algebra.SingularValueDecomposer;
import com.irurueta.geometry.EuclideanTransformation3D;
import com.irurueta.geometry.InvalidRotationMatrixException;
import com.irurueta.geometry.MatrixRotation3D;
import com.irurueta.geometry.PinholeCameraIntrinsicParameters;
import com.irurueta.geometry.Transformation2D;
import com.irurueta.geometry.estimators.LockedException;
import com.irurueta.geometry.estimators.NotReadyException;
import java.util.ArrayList;
import java.util.List;
/**
* Decomposes a 2D homography to extract its internal geometry 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 (reconstructed points must lie in front of the cameras).
* An homography matrix is defined as H = (R + (1/d)*T*N<sup>T</sup>), 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 H<sup>T</sup>H and then following the procedure defined in
* O. Faugeras, Motion and structure from motion in a piecewise planar
* environment.
*/
@SuppressWarnings("DuplicatedCode")
public class HomographyDecomposer {
/**
* Number of inhomogeneous coordinates in 3D.
*/
public static final int NUM_COORDS_3D = 3;
/**
* Threshold to determine that two singular values are equal.
*/
public static final double EQUAL_SINGULAR_VALUE_THRESHOLD = 1e-12;
/**
* 2D transformation relating two views (left view to right view).
*/
private Transformation2D homography;
/**
* Intrinsic parameters to be used on left view.
*/
private PinholeCameraIntrinsicParameters leftIntrinsics;
/**
* Intrinsic parameters to be used on right view.
*/
private PinholeCameraIntrinsicParameters rightIntrinsics;
/**
* Listener to handle events raised by this instance.
*/
private HomographyDecomposerListener listener;
/**
* Indicates whether decomposer is locked while computing decomposition.
*/
private boolean locked;
/**
* Constructor.
*/
public HomographyDecomposer() {
}
/**
* Constructor.
*
* @param homography 2D transformation relating two views (left view to
* right view).
* @param leftIntrinsics intrinsic parameters to be used on left view.
* @param rightIntrinsics intrinsic parameters to be used on right view.
*/
public HomographyDecomposer(final Transformation2D homography,
final PinholeCameraIntrinsicParameters leftIntrinsics,
final PinholeCameraIntrinsicParameters rightIntrinsics) {
this.homography = homography;
this.leftIntrinsics = leftIntrinsics;
this.rightIntrinsics = rightIntrinsics;
}
/**
* Constructor.
*
* @param homography 2D transformation relating two views (left view to
* right view).
* @param leftIntrinsics intrinsic parameters to be used on left view.
* @param rightIntrinsics intrinsic parameters to be used on right view.
* @param listener listener to attend events generated by this instance.
*/
public HomographyDecomposer(final Transformation2D homography,
final PinholeCameraIntrinsicParameters leftIntrinsics,
final PinholeCameraIntrinsicParameters rightIntrinsics,
final HomographyDecomposerListener listener) {
this(homography, leftIntrinsics, rightIntrinsics);
this.listener = listener;
}
/**
* Gets 2D transformation relating two views (left view to right view).
*
* @return 2D transformation relating two views.
*/
public Transformation2D getHomography() {
return homography;
}
/**
* Sets 2D transformation relating two views (left view to right view).
*
* @param homography 2D transformation relating two views.
* @throws LockedException if estimator is locked.
*/
public void setHomography(final Transformation2D homography) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
this.homography = homography;
}
/**
* Gets intrinsic parameters to be used on left view.
*
* @return intrinsic parameters to be used on left view.
*/
public PinholeCameraIntrinsicParameters getLeftIntrinsics() {
return leftIntrinsics;
}
/**
* Sets intrinsic parameters to be used on left view.
*
* @param leftIntrinsics intrinsic parameters to be used on left view.
* @throws LockedException if estimator is locked.
*/
public void setLeftIntrinsics(final PinholeCameraIntrinsicParameters leftIntrinsics) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
this.leftIntrinsics = leftIntrinsics;
}
/**
* Gets intrinsic parameters to be used on right view.
*
* @return intrinsic parameters to be used on right view.
*/
public PinholeCameraIntrinsicParameters getRightIntrinsics() {
return rightIntrinsics;
}
/**
* Sets intrinsic parameters to be used on right view.
*
* @param rightIntrinsics intrinsic parameters to be used on right view.
* @throws LockedException if estimator is locked.
*/
public void setRightIntrinsics(final PinholeCameraIntrinsicParameters rightIntrinsics) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
this.rightIntrinsics = rightIntrinsics;
}
/**
* Gets listener to handle events raised by this instance.
*
* @return listener to handle events raised by this instance.
*/
public HomographyDecomposerListener getListener() {
return listener;
}
/**
* Sets listener to handle events raised by this instance.
*
* @param listener listener to handle events raised by this instance.
*/
public void setListener(final HomographyDecomposerListener listener) {
this.listener = listener;
}
/**
* Indicates whether estimator is locked while computing decomposition.
*
* @return true if decomposer is locked, false otherwise.
*/
public boolean isLocked() {
return locked;
}
/**
* Indicates whether decomposer is ready to start the decomposition when all
* required data has been provided.
*
* @return true if decomposer is ready, false otherwise.
*/
public boolean isReady() {
return homography != null && leftIntrinsics != null && rightIntrinsics != null;
}
/**
* Decomposes homography into possible solutions containing possible 3D
* rotation, 3D translation and normal and distance of the plane relating
* two views via provided homography.
*
* @return possible solutions.
* @throws LockedException if decomposer is locked.
* @throws NotReadyException if decomposer is not ready.
* @throws HomographyDecomposerException if decomposition fails for some
* other reason (i.e. numerical instabilities).
*/
public List<HomographyDecomposition> decompose() throws LockedException, NotReadyException,
HomographyDecomposerException {
final var result = new ArrayList<HomographyDecomposition>();
decompose(result);
return result;
}
/**
* Decomposes homography into possible solutions containing possible 3D
* rotation, 3D translation and normal and distance of the plane relating
* two views via provided homography.
*
* @param result instance where possible solutions will be stored.
* @throws LockedException if decomposer is locked.
* @throws NotReadyException if decomposer is not ready.
* @throws HomographyDecomposerException if decomposition fails for some
* other reason (i.e. numerical instabilities).
*/
public void decompose(final List<HomographyDecomposition> result) throws LockedException, NotReadyException,
HomographyDecomposerException {
if (isLocked()) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
locked = true;
result.clear();
if (listener != null) {
listener.onDecomposeStart(this);
}
final var h = computeNormalizedCoordinatesHomographyMatrix();
// Homography matrix H can be expressed as:
// H = d*R + t*n^T
// where d is the distance to the plane used to relate both views, R
// is a rotation relating both views so that R*R^T = I and
// det(R) = 1, t is the translation relating both views and n is the
// normal of the plane relating both views.
// By using SVD decomposition, we get:
// H = U*A'*V^T
// where U and V are orthonormal matrices and A' is a diagonal matrix
// containing singular values in decreasing order d1 >= d2 >= d3
// Hence A' can also be expressed similarly to H as:
// A' = d'*R' + t'*n'^T, and the relation to d, R, t and n is as
// follows:
// H = U*(d'*R' + t'*n'^T)*V^T = d'*U*R'*V^T + U*t'*n'^T*V^T
// d = d'
// R = U*R'*V^T
// t = U*t'
// n^T = n'T*V^T --> n = V*n'
final var svdDecomposer = new SingularValueDecomposer(h);
svdDecomposer.decompose();
final var u = svdDecomposer.getU();
final var singularValues = svdDecomposer.getSingularValues();
final var v = svdDecomposer.getV();
final var n = new ArrayList<double[]>();
final var r = new ArrayList<Matrix>();
final var t = new ArrayList<double[]>();
final var d = new ArrayList<Double>();
final var numSolutions = decomposeAllFromSingularValues(singularValues, n, r, t, d);
final var transV = v.transposeAndReturnNew();
final var translationMatrix = new Matrix(NUM_COORDS_3D, 1);
final var planeNormalMatrix = new Matrix(NUM_COORDS_3D, 1);
for (var i = 0; i < numSolutions; i++) {
// undo U, V decomposition
// R = U*R'*V^T
final var denormalizedR = new Matrix(u);
denormalizedR.multiply(r.get(i));
denormalizedR.multiply(transV);
// t = U*t'
translationMatrix.fromArray(t.get(i), true);
final var denormalizedTranslationMatrix = u.multiplyAndReturnNew(translationMatrix);
// n = V*n'
planeNormalMatrix.fromArray(n.get(i));
final var denormalizedPlaneNormalMatrix = v.multiplyAndReturnNew(planeNormalMatrix);
final var denormalizedPlaneDistance = d.get(i);
// rotation
final var denormalizedRotation = new MatrixRotation3D(denormalizedR);
final var denormalizedTranslation = denormalizedTranslationMatrix.getBuffer();
final var denormalizedPlaneNormal = denormalizedPlaneNormalMatrix.getBuffer();
// set rotation and translation
final var transformation = new EuclideanTransformation3D(denormalizedRotation, denormalizedTranslation);
result.add(new HomographyDecomposition(transformation, denormalizedPlaneNormal,
denormalizedPlaneDistance));
}
} catch (final InvalidRotationMatrixException | AlgebraException e) {
throw new HomographyDecomposerException(e);
} finally {
if (listener != null) {
listener.onDecomposeEnd(this, result);
}
locked = false;
}
}
/**
* Decompose solutions from singular values.
*
* @param singularValues input singular values.
* @param n list containing possible plane normals.
* @param r list containing possible camera rotations.
* @param t list containing possible camera translations.
* @param d list of distances to plane.
* @return number of solutions.
* @throws HomographyDecomposerException if decomposition fails (i.e. numerical instabilities, etc).
*/
private int decomposeAllFromSingularValues(
final double[] singularValues,
final List<double[]> n,
final List<Matrix> r,
final List<double[]> t,
final List<Double> d)
throws HomographyDecomposerException {
n.clear();
r.clear();
t.clear();
d.clear();
if (areThreeDifferentSingularValues(singularValues)) {
// Three different singular values
final var n1 = new double[NUM_COORDS_3D];
final var n2 = new double[NUM_COORDS_3D];
final var n3 = new double[NUM_COORDS_3D];
final var n4 = new double[NUM_COORDS_3D];
Matrix r1 = null;
Matrix r2 = null;
Matrix r3 = null;
Matrix r4 = null;
try {
r1 = new Matrix(NUM_COORDS_3D, NUM_COORDS_3D);
r2 = new Matrix(NUM_COORDS_3D, NUM_COORDS_3D);
r3 = new Matrix(NUM_COORDS_3D, NUM_COORDS_3D);
r4 = new Matrix(NUM_COORDS_3D, NUM_COORDS_3D);
} catch (final AlgebraException ignore) {
// never thrown
}
final var t1 = new double[NUM_COORDS_3D];
final var t2 = new double[NUM_COORDS_3D];
final var t3 = new double[NUM_COORDS_3D];
final var t4 = new double[NUM_COORDS_3D];
final var planeDistance1 = decomposeFromSingularValues(singularValues, n1, r1, t1, true,
true);
final var planeDistance2 = decomposeFromSingularValues(singularValues, n2, r2, t2, false,
true);
final var planeDistance3 = decomposeFromSingularValues(singularValues, n3, r3, t3, true,
false);
final var planeDistance4 = decomposeFromSingularValues(singularValues, n4, r4, t4, false,
false);
n.add(n1);
n.add(n2);
n.add(n3);
n.add(n4);
r.add(r1);
r.add(r2);
r.add(r3);
r.add(r4);
t.add(t1);
t.add(t2);
t.add(t3);
t.add(t4);
d.add(planeDistance1);
d.add(planeDistance2);
d.add(planeDistance3);
d.add(planeDistance4);
return n.size();
} else if (areTwoEqualSingularValues(singularValues)) {
// Two different singular values
final var n1 = new double[NUM_COORDS_3D];
final var n2 = new double[NUM_COORDS_3D];
Matrix r1 = null;
Matrix r2 = null;
try {
r1 = new Matrix(NUM_COORDS_3D, NUM_COORDS_3D);
r2 = new Matrix(NUM_COORDS_3D, NUM_COORDS_3D);
} catch (final AlgebraException ignore) {
// never thrown
}
final var t1 = new double[NUM_COORDS_3D];
final var t2 = new double[NUM_COORDS_3D];
final var planeDistance1 = decomposeFromSingularValues(singularValues, n1, r1, t1, true,
true);
final var planeDistance2 = decomposeFromSingularValues(singularValues, n2, r2, t2, true,
false);
n.add(n1);
n.add(n2);
r.add(r1);
r.add(r2);
t.add(t1);
t.add(t2);
d.add(planeDistance1);
d.add(planeDistance2);
return n.size();
} else {
// Three equal singular values
throw new HomographyDecomposerException("undefined plane normal");
}
}
/**
* Determines whether there are three different singular values or not.
*
* @param singularValues singular values to be checked.
* @return true if there are three different singular values, false
* otherwise.
*/
private static boolean areThreeDifferentSingularValues(final double[] singularValues) {
final var d1 = singularValues[0];
final var d2 = singularValues[1];
final var d3 = singularValues[2];
return (Math.abs(d1 - d2) > EQUAL_SINGULAR_VALUE_THRESHOLD)
&& (Math.abs(d2 - d3) > EQUAL_SINGULAR_VALUE_THRESHOLD);
}
/**
* Determines whether there are two equal singular values or not.
*
* @param singularValues singular values to be checked.
* @return true if there are two equal singular values, false otherwise.
*/
private static boolean areTwoEqualSingularValues(final double[] singularValues) {
final var d1 = singularValues[0];
final var d2 = singularValues[1];
final var d3 = singularValues[2];
return ((Math.abs(d1 - d2) <= EQUAL_SINGULAR_VALUE_THRESHOLD)
&& (Math.abs(d2 - d3) > EQUAL_SINGULAR_VALUE_THRESHOLD))
|| ((Math.abs(d1 - d2) > EQUAL_SINGULAR_VALUE_THRESHOLD)
&& (Math.abs(d2 - d3) <= EQUAL_SINGULAR_VALUE_THRESHOLD));
}
/**
* Decomposes one possible solution using provided singular values and signs
*
* @param singularValues singular values to use for
* @param n array where plane normal will be store.
* @param r matrix where rotation will be stored.
* @param t array where translation will be stored.
* @param positive1 sign of 1st coordinate of plane normal.
* @param positive3 sign of 2nd coordinate of plane normal.
* @return plane distance.
* @throws HomographyDecomposerException if decomposition is undetermined
* when all three singular values are equal.
*/
private double decomposeFromSingularValues(
final double[] singularValues, final double[] n, final Matrix r, final double[] t, final boolean positive1,
final boolean positive3) throws HomographyDecomposerException {
final var d1 = singularValues[0];
final var d2 = singularValues[1];
final var d3 = singularValues[2];
if (areThreeDifferentSingularValues(singularValues)) {
// Three different singular values
if (d2 > 0.0) {
// Three different singular values d1 != d2 != d3 and d'= d2 > 0
return decomposeFromThreeDifferentSingularValuesPositive(d1, d2, d3, n, r, t, positive1, positive3);
} else {
// Three different singular values and d' = d2 < 0
return decomposeFromThreeDifferentSingularValuesNegative(d1, d2, d3, n, r, t, positive1, positive3);
}
}
if (areTwoEqualSingularValues(singularValues)) {
// Two different singular values
if (d2 > 0.0) {
// Two different singular values d1 = d2 != d3 or d1 != d2 = d3
// and d2 > 0
return decomposeFromTwoDifferentSingularValuesPositive(d1, d2, d3, n, r, t, positive3);
} else {
// Two different singular values d1 = d2 != d3 or d1 != d2 = d3
// and d2 < 0
return decomposeFromTwoDifferentSingularValuesNegative(d1, d2, d3, n, r, t, positive3);
}
} else {
// Three equal singular values
throw new HomographyDecomposerException("undefined plane normal");
}
}
/**
* Generates one homography decomposition using provided singular values and
* assuming that there are two equal singular values and that d2 is
* negative.
*
* @param d1 1st singular value.
* @param d2 2nd singular value.
* @param d3 3rd singular value.
* @param n plane normal.
* @param r rotation.
* @param t translation.
* @param positive3 true to assume positive x3, false otherwise.
* @return distance to plane.
*/
private double decomposeFromTwoDifferentSingularValuesNegative(
final double d1, final double d2, final double d3, final double[] n, final Matrix r, final double[] t,
final boolean positive3) {
// fill plane normal solution
n[0] = 0.0;
n[1] = 0.0;
n[2] = positive3 ? 1.0 : -1.0;
// compute rotation
// fill rotation matrix
r.setElementAt(0, 0, -1.0);
r.setElementAt(1, 0, 0.0);
r.setElementAt(2, 0, 0.0);
r.setElementAt(0, 1, 0.0);
r.setElementAt(1, 1, -1.0);
r.setElementAt(2, 1, 0.0);
r.setElementAt(0, 2, 0.0);
r.setElementAt(1, 2, 0.0);
r.setElementAt(2, 2, 1.0);
// compute translation
final var sum = d3 + d1;
t[0] = 0.0;
t[1] = 0.0;
t[2] = sum * n[2];
// plane distance
return d2;
}
/**
* Generates one homography decomposition using provided singular values and
* assuming that there are two equal singular values and that d2 is
* positive.
*
* @param d1 1st singular value.
* @param d2 2nd singular value.
* @param d3 3rd singular value.
* @param n plane normal.
* @param r rotation.
* @param t translation.
* @param positive3 true to assume positive x3, false otherwise.
* @return distance to plane.
*/
private double decomposeFromTwoDifferentSingularValuesPositive(
final double d1, final double d2, final double d3, final double[] n, final Matrix r, final double[] t,
final boolean positive3) {
// fill plane normal solution
n[0] = 0.0;
n[1] = 0.0;
n[2] = positive3 ? 1.0 : -1.0;
// compute rotation
//fill rotation matrix
r.setElementAt(0, 0, 1.0);
r.setElementAt(1, 0, 0.0);
r.setElementAt(2, 0, 0.0);
r.setElementAt(0, 1, 0.0);
r.setElementAt(1, 1, 1.0);
r.setElementAt(2, 1, 0.0);
r.setElementAt(0, 2, 0.0);
r.setElementAt(1, 2, 0.0);
r.setElementAt(2, 2, 1.0);
// compute translation
final var diff = d1 - d3;
t[0] = 0.0;
t[1] = 0.0;
t[2] = -diff * n[2];
// plane distance
return d2;
}
/**
* Generates one homography decomposition using provided singular values and
* assuming that there are three different singular values and that d2 is
* negative.
*
* @param d1 1st singular value.
* @param d2 2nd singular value.
* @param d3 3rd singular value.
* @param n plane normal.
* @param r rotation.
* @param t translation.
* @param positive1 true to assume positive x1, false otherwise.
* @param positive3 true to assume positive x3, false otherwise.
* @return distance to plane.
*/
private double decomposeFromThreeDifferentSingularValuesNegative(
final double d1, final double d2, final double d3, final double[] n, final Matrix r, final double[] t,
final boolean positive1, final boolean positive3) {
final var d1Sqr = d1 * d1;
final var d2Sqr = d2 * d2;
final var d3Sqr = d3 * d3;
// compute plane normal
final var denom = d1Sqr - d3Sqr;
var x1 = Math.sqrt((d1Sqr - d2Sqr) / denom);
if (!positive1) {
x1 = -x1;
}
final var x2 = 0.0;
var x3 = Math.sqrt((d2Sqr - d3Sqr) / denom);
if (!positive3) {
x3 = -x3;
}
// fill plane normal solution
n[0] = x1;
n[1] = x2;
n[2] = x3;
// compute rotation
final var x1Sqr = x1 * x1;
final var x3Sqr = x3 * x3;
final var sinTheta = (d1 + d3) * x1 * x3 / d2;
final var cosTheta = (d3 * x1Sqr - d1 * x3Sqr) / d2;
// fill rotation matrix
r.setElementAt(0, 0, cosTheta);
r.setElementAt(1, 0, 0.0);
r.setElementAt(2, 0, sinTheta);
r.setElementAt(0, 1, 0.0);
r.setElementAt(1, 1, 1.0);
r.setElementAt(2, 1, 0.0);
r.setElementAt(0, 2, -sinTheta);
r.setElementAt(1, 2, 0.0);
r.setElementAt(2, 2, cosTheta);
// compute translation
final var sum = d1 + d3;
t[0] = sum * x1;
t[1] = 0.0;
t[2] = sum * x3;
// plane distance
return d2;
}
/**
* Generates one homography decomposition using provided singular values and
* assuming that there are three different singular values and that d2 is
* positive.
*
* @param d1 1st singular value.
* @param d2 2nd singular value.
* @param d3 3rd singular value.
* @param n plane normal.
* @param r rotation.
* @param t translation.
* @param positive1 true to assume positive x1, false otherwise.
* @param positive3 true to assume positive x3, false otherwise.
* @return distance to plane.
*/
private double decomposeFromThreeDifferentSingularValuesPositive(
final double d1, final double d2, final double d3, final double[] n, final Matrix r, final double[] t,
final boolean positive1, final boolean positive3) {
final var d1Sqr = d1 * d1;
final var d2Sqr = d2 * d2;
final var d3Sqr = d3 * d3;
// compute plane normal
final var denom = d1Sqr - d3Sqr;
var x1 = Math.sqrt((d1Sqr - d2Sqr) / denom);
if (!positive1) {
x1 = -x1;
}
final var x2 = 0.0;
var x3 = Math.sqrt((d2Sqr - d3Sqr) / denom);
if (!positive3) {
x3 = -x3;
}
// fill plane normal solution
n[0] = x1;
n[1] = x2;
n[2] = x3;
// compute rotation
final var x1Sqr = x1 * x1;
final var x3Sqr = x3 * x3;
final var sinTheta = (d1 - d3) * x1 * x3 / d2;
final var cosTheta = (d1 * x3Sqr + d3 * x1Sqr) / d2;
// fill rotation matrix
r.setElementAt(0, 0, cosTheta);
r.setElementAt(1, 0, 0.0);
r.setElementAt(2, 0, sinTheta);
r.setElementAt(0, 1, 0.0);
r.setElementAt(1, 1, 1.0);
r.setElementAt(2, 1, 0.0);
r.setElementAt(0, 2, -sinTheta);
r.setElementAt(1, 2, 0.0);
r.setElementAt(2, 2, cosTheta);
// compute translation
final var diff = d1 - d3;
t[0] = diff * x1;
t[1] = 0.0;
t[2] = -diff * x3;
// plane distance
return d2;
}
/**
* Computes homography matrix in terms of normalized point coordinates by
* taking into account intrinsic camera parameters on left and right views.
*
* @return normalized homography matrix
* @throws AlgebraException if there are numerical instabilities.
*/
private Matrix computeNormalizedCoordinatesHomographyMatrix() throws AlgebraException {
// we know that point p1 in the left view is related to point p2
// in the right view by homography G so that:
// p2 = G*p1
// where:
// p1 = K1*m1
// p2 = K2*m2
// where K1 and K2 are intrinsic parameters on left and right views
// and m1, m2 are normalized point coordinates on left and right
// views.
// Hence:
// K2*m2 = G*K1*m1 --> m2 = K2^-1*G*K1*m1
// and so we obtain:
// H = K2^-1*G*K1, which is an homography in normalized coordinates
final var k1 = leftIntrinsics.getInternalMatrix();
final var invK2 = rightIntrinsics.getInverseInternalMatrix();
final var g = homography.asMatrix();
// compute H = K2^-1*G*K1
g.multiply(k1);
invK2.multiply(g);
return invK2;
}
}