/**
* Computes inverse coefficients
*
* @param border
* @param forward Forward coefficients.
* @param inverse Inverse used in the inner portion of the data stream.
* @return
*/
private static WlBorderCoef<WlCoef_F32> computeBorderCoefficients(
BorderIndex1D border, WlCoef_F32 forward, WlCoef_F32 inverse) {
int N = Math.max(forward.getScalingLength(), forward.getWaveletLength());
N += N % 2;
N *= 2;
border.setLength(N);
// Because the wavelet transform is a linear invertible system the inverse coefficients
// can be found by creating a matrix and inverting the matrix. Boundary conditions are then
// extracted from this inverted matrix.
DenseMatrix64F A = new DenseMatrix64F(N, N);
for (int i = 0; i < N; i += 2) {
for (int j = 0; j < forward.scaling.length; j++) {
int index = border.getIndex(j + i + forward.offsetScaling);
A.add(i, index, forward.scaling[j]);
}
for (int j = 0; j < forward.wavelet.length; j++) {
int index = border.getIndex(j + i + forward.offsetWavelet);
A.add(i + 1, index, forward.wavelet[j]);
}
}
LinearSolver<DenseMatrix64F> solver = LinearSolverFactory.linear(N);
if (!solver.setA(A) || solver.quality() < 1e-5) {
throw new IllegalArgumentException("Can't invert matrix");
}
DenseMatrix64F A_inv = new DenseMatrix64F(N, N);
solver.invert(A_inv);
int numBorder = UtilWavelet.borderForwardLower(inverse) / 2;
WlBorderCoefFixed<WlCoef_F32> ret = new WlBorderCoefFixed<>(numBorder, numBorder + 1);
ret.setInnerCoef(inverse);
// add the lower coefficients first
for (int i = 0; i < ret.getLowerLength(); i++) {
computeLowerCoef(inverse, A_inv, ret, i * 2);
}
// add upper coefficients
for (int i = 0; i < ret.getUpperLength(); i++) {
computeUpperCoef(inverse, N, A_inv, ret, i * 2);
}
return ret;
}
/**
* Computes the most dominant eigen vector of A using an inverted shifted matrix. The inverted
* shifted matrix is defined as <b>B = (A - αI)<sup>-1</sup></b> and can converge faster if
* α is chosen wisely.
*
* @param A An invertible square matrix matrix.
* @param alpha Shifting factor.
* @return If it converged or not.
*/
public boolean computeShiftInvert(DenseMatrix64F A, double alpha) {
initPower(A);
LinearSolver solver = LinearSolverFactory.linear(A.numCols);
SpecializedOps.addIdentity(A, B, -alpha);
solver.setA(B);
boolean converged = false;
for (int i = 0; i < maxIterations && !converged; i++) {
solver.solve(q0, q1);
double s = NormOps.normPInf(q1);
CommonOps.divide(q1, s, q2);
converged = checkConverged(A);
}
return converged;
}
/**
* Computes the Sampson distance residual for a set of observations given a homography matrix. For
* use in least-squares non-linear optimization algorithms. The full 9 elements of the 3x3 matrix
* are used to parameterize. This has an extra redundant parameter, but is much simpler and should
* not affect the final result.
*
* <p>R. Hartley, and A. Zisserman, "Multiple View Geometry in Computer Vision", 2nd Ed, Cambridge
* 2003
*
* @author Peter Abeles
*/
public class HomographyResidualSampson
implements ModelObservationResidualN<DenseMatrix64F, AssociatedPair> {
DenseMatrix64F H;
Point2D_F64 temp = new Point2D_F64();
DenseMatrix64F J = new DenseMatrix64F(2, 4);
DenseMatrix64F JJ = new DenseMatrix64F(2, 2);
DenseMatrix64F e = new DenseMatrix64F(2, 1);
DenseMatrix64F x = new DenseMatrix64F(2, 1);
DenseMatrix64F error = new DenseMatrix64F(4, 1);
LinearSolver<DenseMatrix64F> solver = LinearSolverFactory.linear(2);
@Override
public void setModel(DenseMatrix64F F) {
this.H = F;
}
@Override
public int computeResiduals(AssociatedPair p, double[] residuals, int index) {
GeometryMath_F64.mult(H, p.p1, temp);
double top1 = error1(p.p1.x, p.p1.y, p.p2.x, p.p2.y);
double top2 = error2(p.p1.x, p.p1.y, p.p2.x, p.p2.y);
computeJacobian(p.p1, p.p2);
// JJ = J*J'
CommonOps.multTransB(J, J, JJ);
// solve JJ'*x = -e
e.data[0] = -top1;
e.data[1] = -top2;
if (solver.setA(JJ)) {
solver.solve(e, x);
// -J'(J*J')^-1*e
CommonOps.multTransA(J, x, error);
residuals[index++] = error.data[0];
residuals[index++] = error.data[1];
residuals[index++] = error.data[2];
residuals[index++] = error.data[3];
} else {
residuals[index++] = 0;
residuals[index++] = 0;
residuals[index++] = 0;
residuals[index++] = 0;
}
return index;
}
/** x2 = H*x1 */
public double error1(double x1, double y1, double x2, double y2) {
double ret;
ret = -(x1 * H.get(1, 0) + y1 * H.get(1, 1) + H.get(1, 2));
ret += y2 * (x1 * H.get(2, 0) + y1 * H.get(2, 1) + H.get(2, 2));
return ret;
}
public double error2(double x1, double y1, double x2, double y2) {
double ret;
ret = (x1 * H.get(0, 0) + y1 * H.get(0, 1) + H.get(0, 2));
ret -= x2 * (x1 * H.get(2, 0) + y1 * H.get(2, 1) + H.get(2, 2));
return ret;
}
public void computeJacobian(Point2D_F64 x1, Point2D_F64 x2) {
J.data[0] = -H.get(1, 0) + x2.y * H.get(2, 0);
J.data[1] = -H.get(1, 1) + x2.y * H.get(2, 1);
J.data[2] = 0;
J.data[3] = x1.x * H.get(2, 0) + x1.y * H.get(2, 1) + H.get(2, 2);
J.data[4] = H.get(0, 0) - x2.x * H.get(2, 0);
J.data[5] = H.get(0, 1) - x2.x * H.get(2, 1);
J.data[6] = -J.data[3];
J.data[7] = 0;
}
@Override
public int getN() {
return 4;
}
}
