001/** 002 * Copyright (c) 2011, The University of Southampton and the individual contributors. 003 * All rights reserved. 004 * 005 * Redistribution and use in source and binary forms, with or without modification, 006 * are permitted provided that the following conditions are met: 007 * 008 * * Redistributions of source code must retain the above copyright notice, 009 * this list of conditions and the following disclaimer. 010 * 011 * * Redistributions in binary form must reproduce the above copyright notice, 012 * this list of conditions and the following disclaimer in the documentation 013 * and/or other materials provided with the distribution. 014 * 015 * * Neither the name of the University of Southampton nor the names of its 016 * contributors may be used to endorse or promote products derived from this 017 * software without specific prior written permission. 018 * 019 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 020 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 021 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 022 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 023 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 024 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 025 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 026 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 027 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 028 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 029 */ 030package org.openimaj.math.geometry.transforms; 031 032import java.util.List; 033 034import org.apache.commons.math3.analysis.MultivariateMatrixFunction; 035import org.apache.commons.math3.analysis.MultivariateVectorFunction; 036import org.apache.commons.math3.fitting.leastsquares.LeastSquaresFactory; 037import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer.Optimum; 038import org.apache.commons.math3.fitting.leastsquares.LevenbergMarquardtOptimizer; 039import org.apache.commons.math3.fitting.leastsquares.MultivariateJacobianFunction; 040import org.apache.commons.math3.linear.ArrayRealVector; 041import org.apache.commons.math3.linear.RealVector; 042import org.openimaj.citation.annotation.Reference; 043import org.openimaj.citation.annotation.ReferenceType; 044import org.openimaj.math.geometry.point.Point2d; 045import org.openimaj.math.matrix.MatrixUtils; 046import org.openimaj.util.pair.IndependentPair; 047 048import Jama.Matrix; 049 050/** 051 * Refinement of homographies estimates using non-linear optimisation 052 * (Levenberg-Marquardt) under different geometric distance/error assumptions. 053 * 054 * @author Jonathon Hare (jsh2@ecs.soton.ac.uk) 055 */ 056@Reference( 057 type = ReferenceType.Book, 058 author = { "Hartley, R.~I.", "Zisserman, A." }, 059 title = "Multiple View Geometry in Computer Vision", 060 year = "2004", 061 edition = "Second", 062 publisher = "Cambridge University Press, ISBN: 0521540518") 063public enum HomographyRefinement { 064 /** 065 * Don't perform any refinement and just return the initial input matrix 066 */ 067 NONE { 068 @Override 069 protected MultivariateVectorFunction getValueFunction( 070 List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) 071 { 072 return null; 073 } 074 075 @Override 076 public double computeError(Matrix h, List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) { 077 return 0; // points are ideal! 078 } 079 080 @Override 081 protected MultivariateMatrixFunction getJacobianFunction( 082 List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) 083 { 084 return null; 085 } 086 087 @Override 088 public Matrix refine(Matrix initial, List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) { 089 return initial; // initial is the result 090 } 091 }, 092 /** 093 * The points in the first image are projected by the homography matrix to 094 * produce new estimates of the second image points from which the residuals 095 * are computed and minimised by the optimiser. The assumption is that there 096 * is only noise in the second image, and that the first image is noise 097 * free. 098 * <p> 099 * Value and analytic Jacobian implementations auto-generated from Matlab 100 * using the following: 101 * 102 * <pre> 103 * <code> 104 * syms h0 h1 h2 h3 h4 h5 h6 h7 h8 real 105 * syms X1 Y1 real 106 * Mi = [X1 Y1 1]'; 107 * H1 = [h0 h1 h2]; 108 * H2 = [h3 h4 h5]; 109 * H3 = [h6 h7 h8]; 110 * mihat = (1 / (H3*Mi)) * [H1*Mi; H2*Mi]; 111 * J = jacobian(mihat, [h0,h1,h2,h3,h4,h5,h6,h7,h8]); 112 * ccode(mihat, 'file', 'singleImageTransfer_value.c') 113 * ccode(J, 'file', 'singleImageTransfer_jacobian.c') 114 * </code> 115 * </pre> 116 */ 117 SINGLE_IMAGE_TRANSFER { 118 @Override 119 protected MultivariateVectorFunction getValueFunction( 120 final List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) 121 { 122 return new MultivariateVectorFunction() { 123 @Override 124 public double[] value(double[] h) throws IllegalArgumentException { 125 final double[] result = new double[data.size() * 2]; 126 127 for (int i = 0; i < data.size(); i++) { 128 final float X1 = data.get(i).firstObject().getX(); 129 final float Y1 = data.get(i).firstObject().getY(); 130 131 final double t2 = X1 * h[6]; 132 final double t3 = Y1 * h[7]; 133 final double t4 = h[8] + t2 + t3; 134 final double t5 = 1.0 / t4; 135 result[i * 2 + 0] = t5 * (h[2] + X1 * h[0] + Y1 * h[1]); 136 result[i * 2 + 1] = t5 * (h[5] + X1 * h[3] + Y1 * h[4]); 137 138 } 139 return result; 140 } 141 }; 142 } 143 144 @Override 145 protected MultivariateMatrixFunction getJacobianFunction( 146 final List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) 147 { 148 return new MultivariateMatrixFunction() { 149 // See Multi View Geometry in Computer Vision, eq 4.21, p129 150 @Override 151 public double[][] value(double[] h) 152 { 153 final double[][] result = new double[2 * data.size()][9]; 154 155 for (int i = 0; i < data.size(); i++) { 156 final float X1 = data.get(i).firstObject().getX(); 157 final float Y1 = data.get(i).firstObject().getY(); 158 159 final double t2 = X1 * h[6]; 160 final double t3 = Y1 * h[7]; 161 final double t4 = h[8] + t2 + t3; 162 final double t5 = 1.0 / t4; 163 final double t6 = X1 * h[0]; 164 final double t7 = Y1 * h[1]; 165 final double t8 = h[2] + t6 + t7; 166 final double t9 = 1.0 / (t4 * t4); 167 final double t10 = X1 * t5; 168 final double t11 = Y1 * t5; 169 final double t12 = X1 * h[3]; 170 final double t13 = Y1 * h[4]; 171 final double t14 = h[5] + t12 + t13; 172 result[i * 2 + 0][0] = t10; 173 result[i * 2 + 0][1] = t11; 174 result[i * 2 + 0][2] = t5; 175 result[i * 2 + 0][6] = -X1 * t8 * t9; 176 result[i * 2 + 0][7] = -Y1 * t8 * t9; 177 result[i * 2 + 0][8] = -t8 * t9; 178 result[i * 2 + 1][3] = t10; 179 result[i * 2 + 1][4] = t11; 180 result[i * 2 + 1][5] = t5; 181 result[i * 2 + 1][6] = -X1 * t9 * t14; 182 result[i * 2 + 1][7] = -Y1 * t9 * t14; 183 result[i * 2 + 1][8] = -t9 * t14; 184 } 185 186 return result; 187 } 188 }; 189 } 190 191 @Override 192 public double computeError(Matrix h, List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) { 193 double error = 0; 194 for (int i = 0; i < data.size(); i++) { 195 final Point2d p1 = data.get(i).firstObject().transform(h); 196 final Point2d p2 = data.get(i).secondObject(); 197 198 final float dx = p1.getX() - p2.getX(); 199 final float dy = p1.getY() - p2.getY(); 200 error += dx * dx + dy * dy; 201 } 202 return error; 203 } 204 205 @Override 206 public Matrix refine(Matrix initial, List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) { 207 final LevenbergMarquardtOptimizer lm = new LevenbergMarquardtOptimizer(); 208 final RealVector start = new ArrayRealVector(initial.getRowPackedCopy()); 209 final RealVector observed = toRealVector(data, false); 210 final int maxEvaluations = 1000; 211 final int maxIterations = 1000; 212 213 final MultivariateVectorFunction value = getValueFunction(data); 214 final MultivariateMatrixFunction jacobian = getJacobianFunction(data); 215 final MultivariateJacobianFunction model = LeastSquaresFactory.model(value, jacobian); 216 217 final Optimum result = lm.optimize(LeastSquaresFactory.create(model, 218 observed, start, null, maxEvaluations, maxIterations)); 219 220 final Matrix improved = MatrixUtils.fromRowPacked(result.getPoint().toArray(), 3); 221 MatrixUtils.times(improved, 1.0 / improved.get(2, 2)); 222 223 return improved; 224 } 225 }, 226 /** 227 * The points in the second image are projected by the inverse homography 228 * matrix to produce new estimates of the first image points from which the 229 * residuals are computed and minimised by the optimiser. Technically, the 230 * optimiser optimises the inverse of the initial homography and then 231 * inverts the result. The assumption is that there is only noise in the 232 * first image. 233 */ 234 SINGLE_IMAGE_TRANSFER_INVERSE { 235 236 @Override 237 protected MultivariateVectorFunction getValueFunction( 238 List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) 239 { 240 final List<IndependentPair<? extends Point2d, ? extends Point2d>> dataInv = IndependentPair.swapList(data); 241 return SINGLE_IMAGE_TRANSFER.getValueFunction(dataInv); 242 } 243 244 @Override 245 public double computeError(Matrix h, List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) { 246 final Matrix hInv = h.inverse(); 247 final List<IndependentPair<? extends Point2d, ? extends Point2d>> dataInv = IndependentPair.swapList(data); 248 return SINGLE_IMAGE_TRANSFER.computeError(hInv, dataInv); 249 } 250 251 @Override 252 protected MultivariateMatrixFunction getJacobianFunction( 253 List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) 254 { 255 final List<IndependentPair<? extends Point2d, ? extends Point2d>> dataInv = IndependentPair.swapList(data); 256 return SINGLE_IMAGE_TRANSFER.getJacobianFunction(dataInv); 257 } 258 259 @Override 260 public Matrix refine(Matrix initial, List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) { 261 final Matrix hInv = initial.inverse(); 262 final List<IndependentPair<? extends Point2d, ? extends Point2d>> dataInv = IndependentPair.swapList(data); 263 264 return SINGLE_IMAGE_TRANSFER.refine(hInv, dataInv).inverse(); 265 } 266 }, 267 /** 268 * The points in the first image are projected by the homography matrix to 269 * produce new estimates of the second image points and the second image 270 * point projected by the inverse homography to produce estimates of the 271 * first. Residuals are computed from both point sets and minimised by the 272 * optimiser. 273 * <p> 274 * Value and analytic Jacobian implementations auto-generated from Matlab 275 * using the following: 276 * 277 * <pre> 278 * <code> 279 * syms h0 h1 h2 h3 h4 h5 h6 h7 h8 real 280 * syms X1 Y1 X2 Y2 real 281 * M1 = [X1 Y1 1]'; 282 * M2 = [X2 Y2 1]'; 283 * H = [h0 h1 h2; h3 h4 h5; h6 h7 h8]; 284 * Hi = inv(H); 285 * H1 = H(1,:); 286 * H2 = H(2,:); 287 * H3 = H(3,:); 288 * H1i = Hi(1,:); 289 * H2i = Hi(2,:); 290 * H3i = Hi(3,:); 291 * mihat = [(1 / (H3i*M2)) * [H1i*M2; H2i*M2]; (1 / (H3*M1)) * [H1*M1; H2*M1]]; 292 * J = jacobian(mihat, [h0,h1,h2,h3,h4,h5,h6,h7,h8]); 293 * ccode(mihat, 'file', 'symImageTransfer_value.c') 294 * ccode(J, 'file', 'symImageTransfer_jacobian.c') 295 * 296 * </code> 297 * </pre> 298 */ 299 SYMMETRIC_TRANSFER { 300 @Override 301 protected MultivariateVectorFunction getValueFunction( 302 final List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) 303 { 304 return new MultivariateVectorFunction() { 305 @Override 306 public double[] value(double[] h) throws IllegalArgumentException { 307 final double[] result = new double[data.size() * 4]; 308 309 for (int i = 0; i < data.size(); i++) { 310 final float X1 = data.get(i).firstObject().getX(); 311 final float Y1 = data.get(i).firstObject().getY(); 312 final float X2 = data.get(i).secondObject().getX(); 313 final float Y2 = data.get(i).secondObject().getY(); 314 315 final double t2 = h[0] * h[4] * h[8]; 316 final double t3 = h[1] * h[5] * h[6]; 317 final double t4 = h[2] * h[3] * h[7]; 318 final double t7 = h[0] * h[5] * h[7]; 319 final double t8 = h[1] * h[3] * h[8]; 320 final double t9 = h[2] * h[4] * h[6]; 321 final double t5 = t2 + t3 + t4 - t7 - t8 - t9; 322 final double t6 = 1.0 / t5; 323 final double t10 = h[0] * h[4]; 324 final double t11 = t10 - h[1] * h[3]; 325 final double t12 = t6 * t11; 326 final double t13 = h[3] * h[7]; 327 final double t14 = t13 - h[4] * h[6]; 328 final double t15 = X2 * t6 * t14; 329 final double t16 = h[0] * h[7]; 330 final double t17 = t16 - h[1] * h[6]; 331 final double t18 = t12 + t15 - Y2 * t6 * t17; 332 final double t19 = 1.0 / t18; 333 final double t20 = X1 * h[6]; 334 final double t21 = Y1 * h[7]; 335 final double t22 = h[8] + t20 + t21; 336 final double t23 = 1.0 / t22; 337 result[4 * i + 0] = t19 338 * (t6 * (h[1] * h[5] - h[2] * h[4]) + X2 * t6 * (h[4] * h[8] - h[5] * h[7]) - Y2 * t6 339 * (h[1] * h[8] - h[2] * h[7])); 340 result[4 * i + 1] = -t19 341 * (t6 * (h[0] * h[5] - h[2] * h[3]) + X2 * t6 * (h[3] * h[8] - h[5] * h[6]) - Y2 * t6 342 * (h[0] * h[8] - h[2] * h[6])); 343 result[4 * i + 2] = t23 * (h[2] + X1 * h[0] + Y1 * h[1]); 344 result[4 * i + 3] = t23 * (h[5] + X1 * h[3] + Y1 * h[4]); 345 } 346 return result; 347 } 348 }; 349 } 350 351 @Override 352 public double computeError(Matrix h, List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) { 353 final Matrix hInv = h.inverse(); 354 double error = 0; 355 for (int i = 0; i < data.size(); i++) { 356 final Point2d p1 = data.get(i).firstObject(); 357 final Point2d p1t = p1.transform(h); 358 final Point2d p2 = data.get(i).secondObject(); 359 final Point2d p2t = p2.transform(hInv); 360 361 final float dx1 = p1t.getX() - p2.getX(); 362 final float dy1 = p1t.getY() - p2.getY(); 363 final float dx2 = p1.getX() - p2t.getX(); 364 final float dy2 = p1.getY() - p2t.getY(); 365 error += dx1 * dx1 + dy1 * dy1 + dx2 * dx2 + dy2 * dy2; 366 } 367 return error; 368 } 369 370 @Override 371 protected MultivariateMatrixFunction getJacobianFunction( 372 final List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) 373 { 374 return new MultivariateMatrixFunction() { 375 // See Multi View Geometry in Computer Vision, p147 376 @Override 377 public double[][] value(double[] h) 378 { 379 final double[][] result = new double[4 * data.size()][9]; 380 381 for (int i = 0; i < data.size(); i++) { 382 final float X1 = data.get(i).firstObject().getX(); 383 final float Y1 = data.get(i).firstObject().getY(); 384 final float X2 = data.get(i).secondObject().getX(); 385 final float Y2 = data.get(i).secondObject().getY(); 386 387 final double t3 = h[4] * h[8]; 388 final double t4 = h[5] * h[7]; 389 final double t2 = t3 - t4; 390 final double t5 = h[0] * h[4] * h[8]; 391 final double t6 = h[1] * h[5] * h[6]; 392 final double t7 = h[2] * h[3] * h[7]; 393 final double t10 = h[0] * h[5] * h[7]; 394 final double t11 = h[1] * h[3] * h[8]; 395 final double t12 = h[2] * h[4] * h[6]; 396 final double t8 = t5 + t6 + t7 - t10 - t11 - t12; 397 final double t9 = 1.0 / (t8 * t8); 398 final double t13 = 1.0 / t8; 399 final double t14 = h[0] * h[4]; 400 final double t27 = h[1] * h[3]; 401 final double t15 = t14 - t27; 402 final double t16 = t13 * t15; 403 final double t17 = h[3] * h[7]; 404 final double t28 = h[4] * h[6]; 405 final double t18 = t17 - t28; 406 final double t19 = X2 * t13 * t18; 407 final double t20 = h[0] * h[7]; 408 final double t29 = h[1] * h[6]; 409 final double t21 = t20 - t29; 410 final double t30 = Y2 * t13 * t21; 411 final double t22 = t16 + t19 - t30; 412 final double t23 = h[1] * h[5]; 413 final double t32 = h[2] * h[4]; 414 final double t24 = t23 - t32; 415 final double t25 = h[1] * h[8]; 416 final double t35 = h[2] * h[7]; 417 final double t26 = t25 - t35; 418 final double t31 = 1.0 / t22; 419 final double t33 = h[3] * h[8]; 420 final double t36 = h[5] * h[6]; 421 final double t34 = t33 - t36; 422 final double t37 = 1.0 / (t22 * t22); 423 final double t38 = t13 * t24; 424 final double t39 = X2 * t2 * t13; 425 final double t43 = Y2 * t13 * t26; 426 final double t40 = t38 + t39 - t43; 427 final double t41 = Y2 * h[7] * t13; 428 final double t42 = X2 * t2 * t9 * t18; 429 final double t44 = h[0] * h[8]; 430 final double t46 = h[2] * h[6]; 431 final double t45 = t44 - t46; 432 final double t47 = X2 * h[7] * t13; 433 final double t48 = h[0] * h[5]; 434 final double t50 = h[2] * h[3]; 435 final double t49 = t48 - t50; 436 final double t51 = t9 * t15 * t24; 437 final double t52 = X2 * h[4] * t13; 438 final double t53 = h[5] * t13; 439 final double t54 = X2 * t2 * t9 * t34; 440 final double t55 = h[4] * t13; 441 final double t56 = t2 * t9 * t15; 442 final double t57 = t13 * t49; 443 final double t58 = X2 * t13 * t34; 444 final double t69 = Y2 * t13 * t45; 445 final double t59 = t57 + t58 - t69; 446 final double t60 = t9 * t15 * t34; 447 final double t61 = Y2 * h[6] * t13; 448 final double t62 = X2 * t9 * t18 * t34; 449 final double t64 = h[3] * t13; 450 final double t63 = t60 + t61 + t62 - t64 - Y2 * t9 * t21 * t34; 451 final double t65 = t18 * t18; 452 final double t66 = X2 * t9 * t65; 453 final double t67 = t9 * t15 * t18; 454 final double t68 = t66 + t67 - Y2 * t9 * t18 * t21; 455 final double t70 = h[2] * t13; 456 final double t71 = h[1] * t13; 457 final double t72 = t9 * t15 * t26; 458 final double t73 = X2 * t9 * t18 * t26; 459 final double t74 = t9 * t15 * t45; 460 final double t75 = X2 * h[6] * t13; 461 final double t76 = X2 * t9 * t18 * t45; 462 final double t78 = h[0] * t13; 463 final double t79 = Y2 * t9 * t21 * t45; 464 final double t77 = t74 + t75 + t76 - t78 - t79; 465 final double t80 = t21 * t21; 466 final double t81 = t9 * t15 * t21; 467 final double t82 = X2 * t9 * t18 * t21; 468 final double t83 = t81 + t82 - Y2 * t9 * t80; 469 final double t84 = t9 * t24 * t49; 470 final double t85 = Y2 * h[2] * t13; 471 final double t86 = Y2 * h[1] * t13; 472 final double t87 = X2 * t9 * t18 * t24; 473 final double t88 = t9 * t15 * t49; 474 final double t89 = X2 * h[3] * t13; 475 final double t90 = X2 * t9 * t18 * t49; 476 final double t92 = Y2 * h[0] * t13; 477 final double t91 = t88 + t89 + t90 - t92 - Y2 * t9 * t21 * t49; 478 final double t93 = t15 * t15; 479 final double t94 = t9 * t93; 480 final double t95 = X2 * t9 * t15 * t18; 481 final double t96 = t94 + t95 - Y2 * t9 * t15 * t21; 482 final double t97 = X1 * h[6]; 483 final double t98 = Y1 * h[7]; 484 final double t99 = h[8] + t97 + t98; 485 final double t100 = 1.0 / t99; 486 final double t101 = X1 * h[0]; 487 final double t102 = Y1 * h[1]; 488 final double t103 = h[2] + t101 + t102; 489 final double t104 = 1.0 / (t99 * t99); 490 final double t105 = X1 * t100; 491 final double t106 = Y1 * t100; 492 final double t107 = X1 * h[3]; 493 final double t108 = Y1 * h[4]; 494 final double t109 = h[5] + t107 + t108; 495 result[4 * i + 0][0] = -t31 * (t2 * t9 * t24 + X2 * (t2 * t2) * t9 - Y2 * t2 * t9 * t26) + t37 496 * t40 497 * (t41 + t42 + t56 - h[4] * t13 - Y2 * t2 * t9 * t21); 498 result[4 * i + 0][1] = t31 * (t53 + t54 - Y2 * h[8] * t13 + t9 * t24 * t34 - Y2 * t9 * t26 * t34) 499 - t37 * t40 500 * t63; 501 result[4 * i + 0][2] = -t31 * (-t41 + t42 + t55 + t9 * t18 * t24 - Y2 * t9 * t18 * t26) + t37 502 * t40 * t68; 503 result[4 * i + 0][3] = t31 * (t9 * t24 * t26 - Y2 * t9 * (t26 * t26) + X2 * t2 * t9 * t26) - t37 504 * t40 505 * (t47 + t72 + t73 - h[1] * t13 - Y2 * t9 * t21 * t26); 506 result[4 * i + 0][4] = -t31 507 * (t70 - X2 * h[8] * t13 + t9 * t24 * t45 + X2 * t2 * t9 * t45 - Y2 * t9 * t26 * t45) 508 + t37 * t40 * t77; 509 result[4 * i + 0][5] = t31 510 * (-t47 + t71 + t9 * t21 * t24 + X2 * t2 * t9 * t21 - Y2 * t9 * t21 * t26) - t37 511 * t40 * t83; 512 result[4 * i + 0][6] = -t31 * (t9 * (t24 * t24) + X2 * t2 * t9 * t24 - Y2 * t9 * t24 * t26) + t37 513 * t40 514 * (t51 + t52 + t87 - Y2 * h[1] * t13 - Y2 * t9 * t21 * t24); 515 result[4 * i + 0][7] = t31 516 * (t84 + t85 - X2 * h[5] * t13 + X2 * t2 * t9 * t49 - Y2 * t9 * t26 * t49) - t37 517 * t40 * t91; 518 result[4 * i + 0][8] = -t31 * (t51 - t52 + t86 + X2 * t2 * t9 * t15 - Y2 * t9 * t15 * t26) + t37 519 * t40 * t96; 520 result[4 * i + 1][0] = t31 * (-t53 + t54 + Y2 * h[8] * t13 + t2 * t9 * t49 - Y2 * t2 * t9 * t45) 521 - t37 * t59 522 * (t41 + t42 - t55 + t56 - Y2 * t2 * t9 * t21); 523 result[4 * i + 1][1] = -t31 * (t9 * t34 * t49 + X2 * t9 * (t34 * t34) - Y2 * t9 * t34 * t45) 524 + t37 * t59 525 * t63; 526 result[4 * i + 1][2] = t31 * (-t61 + t62 + t64 + t9 * t18 * t49 - Y2 * t9 * t18 * t45) - t37 527 * t59 * t68; 528 result[4 * i + 1][3] = -t31 529 * (-t70 + X2 * h[8] * t13 + t9 * t26 * t49 + X2 * t9 * t26 * t34 - Y2 * t9 * t26 * t45) 530 + t37 * t59 * (t47 - t71 + t72 + t73 - Y2 * t9 * t21 * t26); 531 result[4 * i + 1][4] = t31 * (t9 * t45 * t49 - Y2 * t9 * (t45 * t45) + X2 * t9 * t34 * t45) - t37 532 * t59 * t77; 533 result[4 * i + 1][5] = -t31 * (-t75 + t78 - t79 + t9 * t21 * t49 + X2 * t9 * t21 * t34) + t37 534 * t59 * t83; 535 result[4 * i + 1][6] = t31 536 * (t84 - t85 + X2 * h[5] * t13 + X2 * t9 * t24 * t34 - Y2 * t9 * t24 * t45) - t37 537 * t59 * (t51 + t52 - t86 + t87 - Y2 * t9 * t21 * t24); 538 result[4 * i + 1][7] = -t31 * (t9 * (t49 * t49) + X2 * t9 * t34 * t49 - Y2 * t9 * t45 * t49) 539 + t37 * t59 540 * t91; 541 result[4 * i + 1][8] = t31 * (t88 - t89 + t92 + X2 * t9 * t15 * t34 - Y2 * t9 * t15 * t45) - t37 542 * t59 * t96; 543 result[4 * i + 2][0] = t105; 544 result[4 * i + 2][1] = t106; 545 result[4 * i + 2][2] = t100; 546 result[4 * i + 2][6] = -X1 * t103 * t104; 547 result[4 * i + 2][7] = -Y1 * t103 * t104; 548 result[4 * i + 2][8] = -t103 * t104; 549 result[4 * i + 3][3] = t105; 550 result[4 * i + 3][4] = t106; 551 result[4 * i + 3][5] = t100; 552 result[4 * i + 3][6] = -X1 * t104 * t109; 553 result[4 * i + 3][7] = -Y1 * t104 * t109; 554 result[4 * i + 3][8] = -t104 * t109; 555 556 } 557 558 return result; 559 } 560 }; 561 } 562 563 @Override 564 public Matrix refine(Matrix initial, List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) { 565 final LevenbergMarquardtOptimizer lm = new LevenbergMarquardtOptimizer(); 566 final RealVector start = new ArrayRealVector(initial.getRowPackedCopy()); 567 final RealVector observed = toRealVector(data); 568 final int maxEvaluations = 1000; 569 final int maxIterations = 1000; 570 571 final MultivariateVectorFunction value = getValueFunction(data); 572 final MultivariateMatrixFunction jacobian = getJacobianFunction(data); 573 final MultivariateJacobianFunction model = LeastSquaresFactory.model(value, jacobian); 574 575 final Optimum result = lm.optimize(LeastSquaresFactory.create(model, 576 observed, start, null, maxEvaluations, maxIterations)); 577 578 final Matrix improved = MatrixUtils.fromRowPacked(result.getPoint().toArray(), 3); 579 MatrixUtils.times(improved, 1.0 / improved.get(2, 2)); 580 581 return improved; 582 } 583 }; 584 585 protected abstract MultivariateVectorFunction getValueFunction( 586 final List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data); 587 588 protected abstract MultivariateMatrixFunction getJacobianFunction( 589 final List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data); 590 591 /** 592 * Compute the error value being optimised between the two point sets. 593 * Actual computation depends on the specific {@link HomographyRefinement} 594 * method in use. 595 * 596 * @param h 597 * the homography 598 * @param data 599 * the data point-pairs 600 * @return the error value 601 */ 602 public abstract double computeError(Matrix h, 603 List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data); 604 605 private static RealVector toRealVector(List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data, 606 boolean first) 607 { 608 final double[] vec = new double[data.size() * 2]; 609 610 if (first) { 611 for (int i = 0; i < data.size(); i++) { 612 vec[i * 2 + 0] = data.get(i).firstObject().getX(); 613 vec[i * 2 + 1] = data.get(i).firstObject().getY(); 614 } 615 } else { 616 for (int i = 0; i < data.size(); i++) { 617 vec[i * 2 + 0] = data.get(i).secondObject().getX(); 618 vec[i * 2 + 1] = data.get(i).secondObject().getY(); 619 } 620 } 621 622 return new ArrayRealVector(vec); 623 } 624 625 private static RealVector toRealVector(List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) 626 { 627 final double[] vec = new double[data.size() * 4]; 628 629 for (int i = 0; i < data.size(); i++) { 630 vec[i * 4 + 0] = data.get(i).firstObject().getX(); 631 vec[i * 4 + 1] = data.get(i).firstObject().getY(); 632 vec[i * 4 + 2] = data.get(i).secondObject().getX(); 633 vec[i * 4 + 3] = data.get(i).secondObject().getY(); 634 } 635 636 return new ArrayRealVector(vec); 637 } 638 639 /** 640 * Refine an initial guess at the homography that takes the first points in 641 * data to the second using non-linear Levenberg Marquardt optimisation. The 642 * initial guess would normally be computed using the direct linear 643 * transform ({@link TransformUtilities#homographyMatrixNorm(List)}). 644 * 645 * @param initial 646 * the initial estimate (probably from the DLT technique) 647 * @param data 648 * the pairs of data points 649 * @return the optimised estimate 650 */ 651 public abstract Matrix refine(Matrix initial, 652 List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data); 653}