001/******************************************************************************* 002 * Permission to use and distribute this software is granted under the BSD 2-Clause 003 * "Simplified" License (see http://opensource.org/licenses/BSD-2-Clause). 004 * Copyright (c) 2016-2023 Wilhelm Burger. All rights reserved. 005 * Visit https://imagingbook.com for additional details. 006 ******************************************************************************/ 007package imagingbook.calibration.zhang; 008 009import imagingbook.calibration.zhang.util.MathUtil; 010import imagingbook.common.geometry.basic.Pnt2d; 011import org.apache.commons.math3.analysis.MultivariateMatrixFunction; 012import org.apache.commons.math3.analysis.MultivariateVectorFunction; 013import org.apache.commons.math3.fitting.leastsquares.LeastSquaresFactory; 014import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer.Optimum; 015import org.apache.commons.math3.fitting.leastsquares.LeastSquaresProblem; 016import org.apache.commons.math3.fitting.leastsquares.LevenbergMarquardtOptimizer; 017import org.apache.commons.math3.linear.MatrixUtils; 018import org.apache.commons.math3.linear.RealMatrix; 019import org.apache.commons.math3.linear.RealVector; 020 021/** 022 * This class defines methods for estimating the homography (projective) transformation between pairs of 2D point sets. 023 * 024 * @author WB 025 */ 026public class HomographyEstimator { 027 028 public static int MaxLmEvaluations = 1000; 029 public static int MaxLmIterations = 1000; 030 031 private final boolean normalizePointCoordinates; 032 private final boolean doNonlinearRefinement; 033 034 // ------------------------------------------------------------ 035 036 public HomographyEstimator() { 037 this(true, true); 038 } 039 040 public HomographyEstimator(boolean normalizePointCoordinates, boolean doNonlinearRefinement) { 041 this.normalizePointCoordinates = normalizePointCoordinates; 042 this.doNonlinearRefinement = doNonlinearRefinement; 043 } 044 045 // ------------------------------------------------------------ 046 047 /** 048 * Estimates the homographies between a fixed set of 2D model points and multiple observations (image point sets). 049 * The correspondence between the points is assumed to be known. 050 * 051 * @param modelPts a sequence of 2D points on the model (calibration target) 052 * @param obsPoints a sequence 2D image point sets (one set per view). 053 * @return the sequence of estimated homographies (3 x 3 matrices), one for each view 054 */ 055 public RealMatrix[] estimateHomographies(Pnt2d[] modelPts, Pnt2d[][] obsPoints) { 056 final int M = obsPoints.length; 057 RealMatrix[] homographies = new RealMatrix[M]; 058 for (int i = 0; i < M; i++) { 059 RealMatrix Hinit = estimateHomography(modelPts, obsPoints[i]); 060 RealMatrix H = doNonlinearRefinement ? 061 refineHomography(Hinit, modelPts, obsPoints[i]) : Hinit; 062 homographies[i] = H; 063 } 064 return homographies; 065 } 066 067 /** 068 * Estimates the homography (projective) transformation from two given 2D point sets. The correspondence between the 069 * points is assumed to be known. 070 * 071 * @param ptsA the 1st sequence of 2D points 072 * @param ptsB the 2nd sequence of 2D points 073 * @return the estimated homography (3 x 3 matrix) 074 */ 075 public RealMatrix estimateHomography(Pnt2d[] ptsA, Pnt2d[] ptsB) { 076 int n = ptsA.length; 077 078 RealMatrix Na = (normalizePointCoordinates) ? getNormalisationMatrix(ptsA) : MatrixUtils.createRealIdentityMatrix(3); 079 RealMatrix Nb = (normalizePointCoordinates) ? getNormalisationMatrix(ptsB) : MatrixUtils.createRealIdentityMatrix(3); 080 RealMatrix M = MatrixUtils.createRealMatrix(n * 2, 9); 081 082 for (int j = 0, r = 0; j < ptsA.length; j++) { 083 final double[] pA = transform(MathUtil.toArray(ptsA[j]), Na); 084 final double[] pB = transform(MathUtil.toArray(ptsB[j]), Nb); 085 final double xA = pA[0]; 086 final double yA = pA[1]; 087 final double xB = pB[0]; 088 final double yB = pB[1]; 089 M.setRow(r + 0, new double[]{xA, yA, 1, 0, 0, 0, -(xA * xB), -(yA * xB), -(xB)}); 090 M.setRow(r + 1, new double[]{0, 0, 0, xA, yA, 1, -(xA * yB), -(yA * yB), -(yB)}); 091 r = r + 2; 092 } 093 094 // find h, such that M . h = 0: 095 double[] h = MathUtil.solveHomogeneousSystem(M).toArray(); 096 097 // assemble homography matrix H from h: 098 RealMatrix H = MatrixUtils.createRealMatrix(new double[][] 099 {{h[0], h[1], h[2]}, 100 {h[3], h[4], h[5]}, 101 {h[6], h[7], h[8]}}); 102 103 // de-normalize the homography 104 H = MatrixUtils.inverse(Nb).multiply(H).multiply(Na); 105 106 // rescale M such that H[2][2] = 1 (unless H[2][2] close to 0) 107 if (Math.abs(H.getEntry(2, 2)) > 10e-8) { 108 H = H.scalarMultiply(1.0 / H.getEntry(2, 2)); 109 } 110 111 return doNonlinearRefinement ? refineHomography(H, ptsA, ptsB) : H; 112 } 113 114 115 /** 116 * Refines the initial homography by non-linear (Levenberg-Marquart) optimization. 117 * 118 * @param Hinit the initial (estimated) homography matrix 119 * @param pntsA the 1st sequence of 2D points 120 * @param pntsB the 2nd sequence of 2D points 121 * @return the refined homography matrix 122 */ 123 public RealMatrix refineHomography(RealMatrix Hinit, Pnt2d[] pntsA, Pnt2d[] pntsB) { 124 final int M = pntsA.length; 125 double[] observed = new double[2 * M]; 126 for (int i = 0; i < M; i++) { 127 observed[i * 2 + 0] = pntsB[i].getX(); 128 observed[i * 2 + 1] = pntsB[i].getY(); 129 } 130 MultivariateVectorFunction value = getValueFunction(pntsA); 131 MultivariateMatrixFunction jacobian = getJacobianFunction(pntsA); 132 133 LeastSquaresProblem problem = LeastSquaresFactory.create( 134 LeastSquaresFactory.model(value, jacobian), 135 MatrixUtils.createRealVector(observed), 136 MathUtil.getRowPackedVector(Hinit), 137 null, // ConvergenceChecker 138 MaxLmEvaluations, 139 MaxLmIterations); 140 141 LevenbergMarquardtOptimizer lm = new LevenbergMarquardtOptimizer(); 142 Optimum result = lm.optimize(problem); 143 144 RealVector optimum = result.getPoint(); 145 RealMatrix Hopt = MathUtil.fromRowPackedVector(optimum, 3, 3); 146 int iterations = result.getIterations(); 147 if (iterations >= MaxLmIterations) { 148 throw new RuntimeException("refineHomography(): max. number of iterations exceeded"); 149 } 150 // System.out.println("LM optimizer iterations " + iterations); 151 152 return Hopt.scalarMultiply(1.0 / Hopt.getEntry(2, 2)); 153 } 154 155 156 private MultivariateVectorFunction getValueFunction(final Pnt2d[] X) { 157 // System.out.println("MultivariateVectorFunction getValueFunction"); 158 return new MultivariateVectorFunction() { 159 public double[] value(double[] h) { 160 161 final double[] Y = new double[X.length * 2]; 162 for (int j = 0; j < X.length; j++) { 163 final double x = X[j].getX(); 164 final double y = X[j].getY(); 165 final double w = h[6] * x + h[7] * y + h[8]; 166 Y[j * 2 + 0] = (h[0] * x + h[1] * y + h[2]) / w; 167 Y[j * 2 + 1] = (h[3] * x + h[4] * y + h[5]) / w; 168 } 169 return Y; 170 } 171 }; 172 } 173 174 protected MultivariateMatrixFunction getJacobianFunction(final Pnt2d[] X) { 175 return new MultivariateMatrixFunction() { 176 public double[][] value(double[] h) { 177 final double[][] J = new double[2 * X.length][]; 178 for (int i = 0; i < X.length; i++) { 179 final double x = X[i].getX(); 180 final double y = X[i].getY(); 181 182 final double w = h[6] * x + h[7] * y + h[8]; 183 final double w2 = w * w; 184 185 final double sx = h[0] * x + h[1] * y + h[2]; 186 J[2 * i + 0] = new double[]{x / w, y / w, 1 / w, 0, 0, 0, -sx * x / w2, -sx * y / w2, -sx / w2}; 187 188 final double sy = h[3] * x + h[4] * y + h[5]; 189 J[2 * i + 1] = new double[]{0, 0, 0, x / w, y / w, 1 / w, -sy * x / w2, -sy * y / w2, -sy / w2}; 190 } 191 return J; 192 } 193 }; 194 } 195 196 static double[] transform(double[] p, RealMatrix M3x3) { 197 if (p.length != 2) { 198 throw new IllegalArgumentException("transform(): vector p must be of length 2 but is " + p.length); 199 } 200 double[] pA = MathUtil.toHomogeneous(p); 201 double[] pAt = M3x3.operate(pA); 202 return MathUtil.toCartesian(pAt); // need to de-homogenize, since pAt[2] == 1? 203 } 204 205 private RealMatrix getNormalisationMatrix(Pnt2d[] pnts) { 206 final int N = pnts.length; 207 double[] x = new double[N]; 208 double[] y = new double[N]; 209 210 for (int i = 0; i < N; i++) { 211 x[i] = pnts[i].getX(); 212 y[i] = pnts[i].getY(); 213 } 214 215 // calculate the means in x/y 216 double meanx = MathUtil.mean(x); 217 double meany = MathUtil.mean(y); 218 219 // calculate the variances in x/y 220 double varx = MathUtil.variance(x); 221 double vary = MathUtil.variance(y); 222 223 double sx = Math.sqrt(2 / varx); 224 double sy = Math.sqrt(2 / vary); 225 226 RealMatrix matrixA = MatrixUtils.createRealMatrix(new double[][]{ 227 {sx, 0, -sx * meanx}, 228 {0, sy, -sy * meany}, 229 {0, 0, 1}}); 230 231 return matrixA; 232 } 233 234}