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.ArrayList; 033import java.util.List; 034 035import org.openimaj.citation.annotation.Reference; 036import org.openimaj.citation.annotation.ReferenceType; 037import org.openimaj.math.geometry.point.Coordinate; 038import org.openimaj.math.geometry.point.Point2d; 039import org.openimaj.math.geometry.point.Point2dImpl; 040import org.openimaj.math.geometry.shape.Rectangle; 041import org.openimaj.math.matrix.MatrixUtils; 042import org.openimaj.util.pair.IndependentPair; 043import org.openimaj.util.pair.Pair; 044 045import Jama.Matrix; 046import Jama.SingularValueDecomposition; 047 048/** 049 * A collection of static methods for creating transform matrices. 050 * 051 * @author Sina Samangooei (ss@ecs.soton.ac.uk) 052 * @author Jonathon Hare (jsh2@ecs.soton.ac.uk) 053 * 054 */ 055public class TransformUtilities { 056 057 private TransformUtilities() { 058 } 059 060 /** 061 * Construct a rotation about 0, 0. 062 * 063 * @param rot 064 * The amount of rotation in radians. 065 * @return The rotation matrix. 066 */ 067 public static Matrix rotationMatrix(double rot) { 068 final Matrix matrix = Matrix.constructWithCopy(new double[][] { 069 { Math.cos(rot), -Math.sin(rot), 0 }, 070 { Math.sin(rot), Math.cos(rot), 0 }, 071 { 0, 0, 1 }, 072 }); 073 return matrix; 074 } 075 076 /** 077 * Given two points, get a transform matrix that takes points from point a 078 * to point b 079 * 080 * @param from 081 * from this point 082 * @param to 083 * to this point 084 * @return transform matrix 085 */ 086 public static Matrix translateToPointMatrix(Point2d from, Point2d to) { 087 final Matrix matrix = Matrix.constructWithCopy(new double[][] { 088 { 1, 0, to.minus(from).getX() }, 089 { 0, 1, to.minus(from).getY() }, 090 { 0, 0, 1 }, 091 }); 092 return matrix; 093 } 094 095 /** 096 * Construct a translation. 097 * 098 * @param x 099 * The amount to translate in the x-direction. 100 * @param y 101 * The amount to translate in the y-direction. 102 * @return The translation matrix. 103 */ 104 public static Matrix translateMatrix(double x, double y) { 105 final Matrix matrix = Matrix.constructWithCopy(new double[][] { 106 { 1, 0, x }, 107 { 0, 1, y }, 108 { 0, 0, 1 }, 109 }); 110 return matrix; 111 } 112 113 /** 114 * Construct a rotation about the centre of the rectangle defined by width 115 * and height (i.e. width/2, height/2). 116 * 117 * @param rot 118 * The amount of rotation in radians. 119 * @param width 120 * The width of the rectangle. 121 * @param height 122 * The height of the rectangle. 123 * @return The rotation matrix. 124 */ 125 public static Matrix centeredRotationMatrix(double rot, int width, int height) { 126 final int halfWidth = Math.round(width / 2); 127 final int halfHeight = Math.round(height / 2); 128 129 return rotationMatrixAboutPoint(rot, halfWidth, halfHeight); 130 } 131 132 /** 133 * Create a scaling centered around a point. 134 * 135 * @param sx 136 * x-scale 137 * @param sy 138 * y-scale 139 * @param tx 140 * x-position 141 * @param ty 142 * y-position 143 * @return The scaling transform. 144 */ 145 public static Matrix scaleMatrixAboutPoint(double sx, double sy, int tx, int ty) { 146 return Matrix.identity(3, 3). 147 times(translateMatrix(tx, ty)). 148 times(scaleMatrix(sx, sy)). 149 times(translateMatrix(-tx, -ty)); 150 } 151 152 /** 153 * Create a scaling centered around a point. 154 * 155 * @param sx 156 * x-scale 157 * @param sy 158 * y-scale 159 * @param point 160 * The point 161 * @return The scaling transform. 162 */ 163 public static Matrix scaleMatrixAboutPoint(double sx, double sy, Point2d point) { 164 return Matrix.identity(3, 3). 165 times(translateMatrix(point.getX(), point.getY())). 166 times(scaleMatrix(sx, sy)). 167 times(translateMatrix(-point.getX(), -point.getY())); 168 } 169 170 /** 171 * Construct a rotation about the centre of the rectangle defined by width 172 * and height (i.e. width/2, height/2). 173 * 174 * @param rot 175 * The amount of rotation in radians. 176 * @param tx 177 * the x translation 178 * @param ty 179 * the y translation 180 * @return The rotation matrix. 181 */ 182 public static Matrix rotationMatrixAboutPoint(double rot, float tx, float ty) { 183 return Matrix.identity(3, 3). 184 times(translateMatrix(tx, ty)). 185 times(rotationMatrix(rot)). 186 times(translateMatrix(-tx, -ty)); 187 } 188 189 /** 190 * Find the affine transform between pairs of matching points in 191 * n-dimensional space. The transform is the "best" possible in the 192 * least-squares sense. 193 * 194 * @param q 195 * first set of points 196 * @param p 197 * second set of points 198 * @return least-squares estimated affine transform matrix 199 */ 200 @Reference( 201 author = "Sp\"ath, Helmuth", 202 title = "Fitting affine and orthogonal transformations between two sets of points.", 203 type = ReferenceType.Article, 204 year = "2004", 205 journal = "Mathematical Communications", 206 publisher = "Croatian Mathematical Society, Division Osijek, Osijek; Faculty of Electrical Engineering, University of Osijek, Osijek", 207 pages = { "27", "34" }, 208 volume = "9", 209 number = "1" 210 ) 211 public static Matrix 212 affineMatrixND(double[][] q, double[][] p) 213 { 214 final int dim = q[0].length; 215 216 final double[][] c = new double[dim + 1][dim]; 217 for (int j = 0; j < dim; j++) { 218 for (int k = 0; k < dim + 1; k++) { 219 for (int i = 0; i < q.length; i++) { 220 double qtk = 1; 221 if (k < 3) 222 qtk = q[i][k]; 223 c[k][j] += qtk * p[i][j]; 224 } 225 } 226 } 227 228 final double[][] Q = new double[dim + 1][dim + 1]; 229 for (final double[] qt : q) { 230 for (int i = 0; i < dim + 1; i++) { 231 for (int j = 0; j < dim + 1; j++) { 232 double qti = 1; 233 if (i < 3) 234 qti = qt[i]; 235 double qtj = 1; 236 if (j < 3) 237 qtj = qt[j]; 238 Q[i][j] += qti * qtj; 239 } 240 } 241 } 242 243 final Matrix Qm = new Matrix(Q); 244 final Matrix cm = new Matrix(c); 245 final Matrix a = Qm.solve(cm); 246 247 final Matrix t = Matrix.identity(dim + 1, dim + 1); 248 t.setMatrix(0, dim - 1, 0, dim, a.transpose()); 249 250 return t; 251 } 252 253 /** 254 * Find the affine transform between pairs of matching points in 255 * n-dimensional space. The transform is the "best" possible in the 256 * least-squares sense. 257 * 258 * @param data 259 * pairs of matching n-dimensional {@link Coordinate}s 260 * @return least-squares estimated affine transform matrix 261 */ 262 @Reference( 263 author = { "Sp\"ath, Helmuth" }, 264 title = "Fitting affine and orthogonal transformations between two sets of points.", 265 type = ReferenceType.Article, 266 year = "2004", 267 journal = "Mathematical Communications", 268 publisher = "Croatian Mathematical Society, Division Osijek, Osijek; Faculty of Electrical Engineering, University of Osijek, Osijek", 269 pages = { "27", "34" }, 270 volume = "9", 271 number = "1" 272 ) 273 public static Matrix 274 affineMatrixND(List<? extends IndependentPair<? extends Coordinate, ? extends Coordinate>> data) 275 { 276 final int dim = data.get(0).firstObject().getDimensions(); 277 final int nItems = data.size(); 278 279 final double[][] c = new double[dim + 1][dim]; 280 for (int j = 0; j < dim; j++) { 281 for (int k = 0; k < dim + 1; k++) { 282 for (int i = 0; i < nItems; i++) { 283 double qtk = 1; 284 if (k < 3) 285 qtk = data.get(i).firstObject().getOrdinate(k).doubleValue(); 286 c[k][j] += qtk * data.get(i).secondObject().getOrdinate(j).doubleValue(); 287 } 288 } 289 } 290 291 final double[][] Q = new double[dim + 1][dim + 1]; 292 for (int k = 0; k < nItems; k++) { 293 final double[] qt = new double[dim + 1]; 294 for (int i = 0; i < dim + 1; i++) { 295 qt[i] = data.get(k).firstObject().getOrdinate(i).doubleValue(); 296 } 297 qt[dim] = 1; 298 299 for (int i = 0; i < dim + 1; i++) { 300 for (int j = 0; j < dim + 1; j++) { 301 final double qti = qt[i]; 302 final double qtj = qt[j]; 303 Q[i][j] += qti * qtj; 304 } 305 } 306 } 307 308 final Matrix Qm = new Matrix(Q); 309 final Matrix cm = new Matrix(c); 310 final Matrix a = Qm.solve(cm); 311 312 final Matrix t = Matrix.identity(dim + 1, dim + 1); 313 t.setMatrix(0, dim - 1, 0, dim, a.transpose()); 314 315 return t; 316 } 317 318 /** 319 * Compute the least-squares rigid alignment between two sets of matching 320 * points in N-dimensional space. Allows scaling and translation but nothing 321 * else. 322 * 323 * @param q 324 * first set of points 325 * @param p 326 * second set of points 327 * @return rigid transformation matrix. 328 */ 329 @Reference( 330 type = ReferenceType.Article, 331 author = { "Berthold K. P. Horn", "H.M. Hilden", "Shariar Negahdaripour" }, 332 title = "Closed-Form Solution of Absolute Orientation using Orthonormal Matrices", 333 year = "1988", 334 journal = "JOURNAL OF THE OPTICAL SOCIETY AMERICA", 335 pages = { "1127", "1135" }, 336 number = "7", 337 volume = "5" 338 ) 339 public static Matrix rigidMatrix(double[][] q, double[][] p) { 340 final int dim = q[0].length; 341 final int nitems = q.length; 342 343 final double[] qmean = new double[dim]; 344 final double[] pmean = new double[dim]; 345 for (int j = 0; j < nitems; j++) { 346 for (int i = 0; i < dim; i++) { 347 qmean[i] += q[j][i]; 348 pmean[i] += p[j][i]; 349 } 350 } 351 for (int i = 0; i < dim; i++) { 352 qmean[i] /= nitems; 353 pmean[i] /= nitems; 354 } 355 356 final double[][] M = new double[dim][dim]; 357 358 for (int k = 0; k < nitems; k++) { 359 for (int j = 0; j < dim; j++) { 360 for (int i = 0; i < dim; i++) { 361 M[j][i] += (p[k][j] - pmean[j]) * (q[k][i] - qmean[i]); 362 } 363 } 364 } 365 366 final Matrix Mm = new Matrix(M); 367 final Matrix Qm = Mm.transpose().times(Mm); 368 final Matrix QmInvSqrt = MatrixUtils.invSqrtSym(Qm); 369 final Matrix R = Mm.times(QmInvSqrt); 370 371 final Matrix pm = new Matrix(new double[][] { pmean }).transpose(); 372 final Matrix qm = new Matrix(new double[][] { qmean }).transpose(); 373 final Matrix T = pm.minus(R.times(qm)); 374 375 final Matrix tf = Matrix.identity(dim + 1, dim + 1); 376 tf.setMatrix(0, dim - 1, 0, dim - 1, R); 377 tf.setMatrix(0, dim - 1, dim, dim, T); 378 379 return tf; 380 } 381 382 /** 383 * Compute the least-squares rigid alignment between two sets of matching 384 * points in N-dimensional space. Allows scaling and translation but nothing 385 * else. 386 * 387 * @param data 388 * set of points matching points 389 * @return rigid transformation matrix. 390 */ 391 @Reference( 392 type = ReferenceType.Article, 393 author = { "Berthold K. P. Horn", "H.M. Hilden", "Shariar Negahdaripour" }, 394 title = "Closed-Form Solution of Absolute Orientation using Orthonormal Matrices", 395 year = "1988", 396 journal = "JOURNAL OF THE OPTICAL SOCIETY AMERICA", 397 pages = { "1127", "1135" }, 398 number = "7", 399 volume = "5" 400 ) 401 public static Matrix rigidMatrix( 402 List<? extends IndependentPair<? extends Coordinate, ? extends Coordinate>> data) 403 { 404 final int dim = data.get(0).firstObject().getDimensions(); 405 final int nitems = data.size(); 406 407 final double[] qmean = new double[dim]; 408 final double[] pmean = new double[dim]; 409 for (int j = 0; j < nitems; j++) { 410 for (int i = 0; i < dim; i++) { 411 qmean[i] += data.get(j).firstObject().getOrdinate(i).doubleValue(); 412 pmean[i] += data.get(j).secondObject().getOrdinate(i).doubleValue(); 413 } 414 } 415 for (int i = 0; i < dim; i++) { 416 qmean[i] /= nitems; 417 pmean[i] /= nitems; 418 } 419 420 final double[][] M = new double[dim][dim]; 421 422 for (int k = 0; k < nitems; k++) { 423 for (int j = 0; j < dim; j++) { 424 for (int i = 0; i < dim; i++) { 425 M[j][i] += (data.get(k).secondObject().getOrdinate(j).doubleValue() - pmean[j]) 426 * (data.get(k).firstObject().getOrdinate(i).doubleValue() - qmean[i]); 427 } 428 } 429 } 430 431 final Matrix Mm = new Matrix(M); 432 final Matrix Qm = Mm.transpose().times(Mm); 433 final Matrix QmInvSqrt = MatrixUtils.invSqrtSym(Qm); 434 final Matrix R = Mm.times(QmInvSqrt); 435 436 final Matrix pm = new Matrix(new double[][] { pmean }).transpose(); 437 final Matrix qm = new Matrix(new double[][] { qmean }).transpose(); 438 final Matrix T = pm.minus(R.times(qm)); 439 440 final Matrix tf = Matrix.identity(dim + 1, dim + 1); 441 tf.setMatrix(0, dim - 1, 0, dim - 1, R); 442 tf.setMatrix(0, dim - 1, dim, dim, T); 443 444 return tf; 445 } 446 447 /** 448 * Construct an affine transform using a least-squares fit of the provided 449 * point pairs. There must be at least 3 point pairs for this to work. 450 * 451 * @param data 452 * Data to calculate affine matrix from. 453 * @return an affine transform matrix. 454 */ 455 public static Matrix affineMatrix(List<? extends IndependentPair<Point2d, Point2d>> data) { 456 final Matrix transform = new Matrix(3, 3); 457 458 transform.set(2, 0, 0); 459 transform.set(2, 1, 0); 460 transform.set(2, 2, 1); 461 462 // Solve Ax=0 463 final Matrix A = new Matrix(data.size() * 2, 7); 464 465 for (int i = 0, j = 0; i < data.size(); i++, j += 2) { 466 final float x1 = data.get(i).firstObject().getX(); 467 final float y1 = data.get(i).firstObject().getY(); 468 final float x2 = data.get(i).secondObject().getX(); 469 final float y2 = data.get(i).secondObject().getY(); 470 471 A.set(j, 0, x1); 472 A.set(j, 1, y1); 473 A.set(j, 2, 1); 474 A.set(j, 3, 0); 475 A.set(j, 4, 0); 476 A.set(j, 5, 0); 477 A.set(j, 6, -x2); 478 479 A.set(j + 1, 0, 0); 480 A.set(j + 1, 1, 0); 481 A.set(j + 1, 2, 0); 482 A.set(j + 1, 3, x1); 483 A.set(j + 1, 4, y1); 484 A.set(j + 1, 5, 1); 485 A.set(j + 1, 6, -y2); 486 } 487 488 final double[] W = MatrixUtils.solveHomogeneousSystem(A); 489 490 // build matrix 491 transform.set(0, 0, W[0] / W[6]); 492 transform.set(0, 1, W[1] / W[6]); 493 transform.set(0, 2, W[2] / W[6]); 494 495 transform.set(1, 0, W[3] / W[6]); 496 transform.set(1, 1, W[4] / W[6]); 497 transform.set(1, 2, W[5] / W[6]); 498 499 return transform; 500 } 501 502 /** 503 * Construct a homogeneous scaling transform with the given amounts of 504 * scaling. 505 * 506 * @param d 507 * Scaling in the x-direction. 508 * @param e 509 * Scaling in the y-direction. 510 * @return a scaling matrix. 511 */ 512 public static Matrix scaleMatrix(double d, double e) { 513 final Matrix scaleMatrix = new Matrix(new double[][] { 514 { d, 0, 0 }, 515 { 0, e, 0 }, 516 { 0, 0, 1 }, 517 }); 518 return scaleMatrix; 519 } 520 521 /** 522 * Generates the data for normalisation of the points such that each matched 523 * point is centered about the origin and also scaled be be within 524 * Math.sqrt(2) of the origin. This corrects for some errors which occured 525 * when distances between matched points were extremely large. 526 * 527 * @param data 528 * @return the normalisation data 529 */ 530 public static Pair<Matrix> getNormalisations( 531 List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) 532 { 533 final Point2dImpl firstMean = new Point2dImpl(0, 0), secondMean = new Point2dImpl(0, 0); 534 for (final IndependentPair<? extends Point2d, ? extends Point2d> pair : data) { 535 firstMean.x += pair.firstObject().getX(); 536 firstMean.y += pair.firstObject().getY(); 537 secondMean.x += pair.secondObject().getX(); 538 secondMean.y += pair.secondObject().getY(); 539 } 540 541 firstMean.x /= data.size(); 542 firstMean.y /= data.size(); 543 secondMean.x /= data.size(); 544 secondMean.y /= data.size(); 545 546 // Calculate the std 547 final Point2dImpl firstStd = new Point2dImpl(0, 0), secondStd = new Point2dImpl(0, 0); 548 549 for (final IndependentPair<? extends Point2d, ? extends Point2d> pair : data) { 550 firstStd.x += Math.pow(firstMean.x - pair.firstObject().getX(), 2); 551 firstStd.y += Math.pow(firstMean.y - pair.firstObject().getY(), 2); 552 secondStd.x += Math.pow(secondMean.x - pair.secondObject().getX(), 2); 553 secondStd.y += Math.pow(secondMean.y - pair.secondObject().getY(), 2); 554 } 555 556 firstStd.x = firstStd.x < 0.00001 ? 1.0f : firstStd.x; 557 firstStd.y = firstStd.y < 0.00001 ? 1.0f : firstStd.y; 558 559 secondStd.x = secondStd.x < 0.00001 ? 1.0f : secondStd.x; 560 secondStd.y = secondStd.y < 0.00001 ? 1.0f : secondStd.y; 561 562 firstStd.x = (float) (Math.sqrt(2) / Math.sqrt(firstStd.x / (data.size() - 1))); 563 firstStd.y = (float) (Math.sqrt(2) / Math.sqrt(firstStd.y / (data.size() - 1))); 564 secondStd.x = (float) (Math.sqrt(2) / Math.sqrt(secondStd.x / (data.size() - 1))); 565 secondStd.y = (float) (Math.sqrt(2) / Math.sqrt(secondStd.y / (data.size() - 1))); 566 567 final Matrix firstMatrix = new Matrix(new double[][] { 568 { firstStd.x, 0, -firstMean.x * firstStd.x }, 569 { 0, firstStd.y, -firstMean.y * firstStd.y }, 570 { 0, 0, 1 }, 571 }); 572 573 final Matrix secondMatrix = new Matrix(new double[][] { 574 { secondStd.x, 0, -secondMean.x * secondStd.x }, 575 { 0, secondStd.y, -secondMean.y * secondStd.y }, 576 { 0, 0, 1 }, 577 }); 578 579 return new Pair<Matrix>(firstMatrix, secondMatrix); 580 } 581 582 /** 583 * Normalise the data, returning a normalised copy. 584 * 585 * @param data 586 * @param normalisations 587 * @return the normalised data 588 */ 589 public static List<? extends IndependentPair<Point2d, Point2d>> normalise( 590 List<? extends IndependentPair<Point2d, Point2d>> data, Pair<Matrix> normalisations) 591 { 592 final List<Pair<Point2d>> normData = new ArrayList<Pair<Point2d>>(); 593 594 for (int i = 0; i < data.size(); i++) { 595 final Point2d p1 = data.get(i).firstObject().transform(normalisations.firstObject()); 596 final Point2d p2 = data.get(i).secondObject().transform(normalisations.secondObject()); 597 598 normData.add(new Pair<Point2d>(p1, p2)); 599 } 600 601 return normData; 602 } 603 604 /** 605 * Normalise the data, returning a normalised copy. 606 * 607 * @param data 608 * the data 609 * @param normalisations 610 * the normalisation matrices 611 * @return the normalised data 612 */ 613 public static IndependentPair<Point2d, Point2d> normalise(IndependentPair<Point2d, Point2d> data, 614 Pair<Matrix> normalisations) 615 { 616 final Point2d p1 = data.firstObject().transform(normalisations.firstObject()); 617 final Point2d p2 = data.secondObject().transform(normalisations.secondObject()); 618 619 return new Pair<Point2d>(p1, p2); 620 } 621 622 /** 623 * The normalised 8-point algorithm for estimating the Fundamental matrix 624 * 625 * @param data 626 * @return the estimated Fundamental matrix 627 */ 628 public static Matrix fundamentalMatrix8PtNorm(List<? extends IndependentPair<Point2d, Point2d>> data) { 629 Matrix A; 630 631 final Pair<Matrix> normalisations = getNormalisations(data); 632 A = new Matrix(data.size(), 9); 633 for (int i = 0; i < data.size(); i++) { 634 final Point2d p1 = data.get(i).firstObject().transform(normalisations.firstObject()); 635 final Point2d p2 = data.get(i).secondObject().transform(normalisations.secondObject()); 636 637 final float x1_1 = p1.getX(); // u 638 final float x1_2 = p1.getY(); // v 639 final float x2_1 = p2.getX(); // u' 640 final float x2_2 = p2.getY(); // v' 641 642 A.set(i, 0, x2_1 * x1_1); // u' * u 643 A.set(i, 1, x2_1 * x1_2); // u' * v 644 A.set(i, 2, x2_1); // u' 645 A.set(i, 3, x2_2 * x1_1); // v' * u 646 A.set(i, 4, x2_2 * x1_2); // v' * v 647 A.set(i, 5, x2_2); // v' 648 A.set(i, 6, x1_1); // u 649 A.set(i, 7, x1_2); // v 650 A.set(i, 8, 1); // 1 651 } 652 653 final double[] W = MatrixUtils.solveHomogeneousSystem(A); 654 Matrix fundamental = new Matrix(3, 3); 655 fundamental.set(0, 0, W[0]); 656 fundamental.set(0, 1, W[1]); 657 fundamental.set(0, 2, W[2]); 658 fundamental.set(1, 0, W[3]); 659 fundamental.set(1, 1, W[4]); 660 fundamental.set(1, 2, W[5]); 661 fundamental.set(2, 0, W[6]); 662 fundamental.set(2, 1, W[7]); 663 fundamental.set(2, 2, W[8]); 664 665 fundamental = MatrixUtils.reduceRank(fundamental, 2); 666 fundamental = normalisations.secondObject().transpose().times(fundamental).times(normalisations.firstObject()); 667 return fundamental; 668 } 669 670 /** 671 * The un-normalised 8-point algorithm for estimation of the Fundamental 672 * matrix. Only use with pre-normalised data! 673 * 674 * @param data 675 * @return the estimated Fundamental matrix 676 */ 677 public static Matrix fundamentalMatrix8Pt(List<? extends IndependentPair<Point2d, Point2d>> data) { 678 Matrix A; 679 680 A = new Matrix(data.size(), 9); 681 for (int i = 0; i < data.size(); i++) { 682 final Point2d p1 = data.get(i).firstObject(); 683 final Point2d p2 = data.get(i).secondObject(); 684 685 final float x1_1 = p1.getX(); // u 686 final float x1_2 = p1.getY(); // v 687 final float x2_1 = p2.getX(); // u' 688 final float x2_2 = p2.getY(); // v' 689 690 A.set(i, 0, x2_1 * x1_1); // u' * u 691 A.set(i, 1, x2_1 * x1_2); // u' * v 692 A.set(i, 2, x2_1); // u' 693 A.set(i, 3, x2_2 * x1_1); // v' * u 694 A.set(i, 4, x2_2 * x1_2); // v' * v 695 A.set(i, 5, x2_2); // v' 696 A.set(i, 6, x1_1); // u 697 A.set(i, 7, x1_2); // v 698 A.set(i, 8, 1); // 1 699 } 700 701 final double[] W = MatrixUtils.solveHomogeneousSystem(A); 702 Matrix fundamental = new Matrix(3, 3); 703 fundamental.set(0, 0, W[0]); 704 fundamental.set(0, 1, W[1]); 705 fundamental.set(0, 2, W[2]); 706 fundamental.set(1, 0, W[3]); 707 fundamental.set(1, 1, W[4]); 708 fundamental.set(1, 2, W[5]); 709 fundamental.set(2, 0, W[6]); 710 fundamental.set(2, 1, W[7]); 711 fundamental.set(2, 2, W[8]); 712 713 fundamental = MatrixUtils.reduceRank(fundamental, 2); 714 return fundamental; 715 } 716 717 /** 718 * Compute the least-squares estimate (the normalised Direct Linear 719 * Transform approach) of the homography between a set of matching data 720 * points. The data is automatically normalised to prevent numerical 721 * problems. The returned homography is the one that can be applied to the 722 * first set of points to generate the second set. 723 * 724 * @param data 725 * the matching points 726 * @return the estimated homography 727 */ 728 public static Matrix homographyMatrixNorm(List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) 729 { 730 final Pair<Matrix> normalisations = getNormalisations(data); 731 732 final Matrix A = new Matrix(data.size() * 2, 9); 733 734 for (int i = 0, j = 0; i < data.size(); i++, j += 2) { 735 final Point2d p1 = data.get(i).firstObject().transform(normalisations.firstObject()); 736 final Point2d p2 = data.get(i).secondObject().transform(normalisations.secondObject()); 737 final float x1 = p1.getX(); 738 final float y1 = p1.getY(); 739 final float x2 = p2.getX(); 740 final float y2 = p2.getY(); 741 742 A.set(j, 0, x1); // x 743 A.set(j, 1, y1); // y 744 A.set(j, 2, 1); // 1 745 A.set(j, 3, 0); // 0 746 A.set(j, 4, 0); // 0 747 A.set(j, 5, 0); // 0 748 A.set(j, 6, -(x2 * x1)); // -x'*x 749 A.set(j, 7, -(x2 * y1)); // -x'*y 750 A.set(j, 8, -(x2)); // -x' 751 752 A.set(j + 1, 0, 0); // 0 753 A.set(j + 1, 1, 0); // 0 754 A.set(j + 1, 2, 0); // 0 755 A.set(j + 1, 3, x1); // x 756 A.set(j + 1, 4, y1); // y 757 A.set(j + 1, 5, 1); // 1 758 A.set(j + 1, 6, -(y2 * x1)); // -y'*x 759 A.set(j + 1, 7, -(y2 * y1)); // -y'*y 760 A.set(j + 1, 8, -(y2)); // -y' 761 } 762 763 final double[] W = MatrixUtils.solveHomogeneousSystem(A); 764 765 Matrix homography = new Matrix(3, 3); 766 homography.set(0, 0, W[0]); 767 homography.set(0, 1, W[1]); 768 homography.set(0, 2, W[2]); 769 homography.set(1, 0, W[3]); 770 homography.set(1, 1, W[4]); 771 homography.set(1, 2, W[5]); 772 homography.set(2, 0, W[6]); 773 homography.set(2, 1, W[7]); 774 homography.set(2, 2, W[8]); 775 776 homography = normalisations.secondObject().inverse().times(homography).times(normalisations.firstObject()); 777 778 // it makes sense to rescale the matrix here by 1 / tf[2][2], 779 // unless tf[2][2] == 0 780 if (Math.abs(homography.get(2, 2)) > 0.000001) { 781 MatrixUtils.times(homography, 1.0 / homography.get(2, 2)); 782 } 783 784 return homography; 785 } 786 787 /** 788 * Compute the least-squares estimate (the normalised Direct Linear 789 * Transform approach) of the homography between a set of matching data 790 * points. This method is potentially numerically unstable if the data has 791 * not been pre-normalised (using {@link #normalise(List, Pair)}). For 792 * un-normalised data, use {@link #homographyMatrixNorm(List)} instead. 793 * 794 * @param data 795 * the matching points 796 * @return the estimated homography 797 */ 798 public static Matrix homographyMatrix(List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) { 799 final Matrix A = new Matrix(data.size() * 2, 9); 800 801 for (int i = 0, j = 0; i < data.size(); i++, j += 2) { 802 final Point2d p1 = data.get(i).firstObject(); 803 final Point2d p2 = data.get(i).secondObject(); 804 final float x1 = p1.getX(); 805 final float y1 = p1.getY(); 806 final float x2 = p2.getX(); 807 final float y2 = p2.getY(); 808 809 A.set(j, 0, x1); // x 810 A.set(j, 1, y1); // y 811 A.set(j, 2, 1); // 1 812 A.set(j, 3, 0); // 0 813 A.set(j, 4, 0); // 0 814 A.set(j, 5, 0); // 0 815 A.set(j, 6, -(x2 * x1)); // -x'*x 816 A.set(j, 7, -(x2 * y1)); // -x'*y 817 A.set(j, 8, -(x2)); // -x' 818 819 A.set(j + 1, 0, 0); // 0 820 A.set(j + 1, 1, 0); // 0 821 A.set(j + 1, 2, 0); // 0 822 A.set(j + 1, 3, x1); // x 823 A.set(j + 1, 4, y1); // y 824 A.set(j + 1, 5, 1); // 1 825 A.set(j + 1, 6, -(y2 * x1)); // -y'*x 826 A.set(j + 1, 7, -(y2 * y1)); // -y'*y 827 A.set(j + 1, 8, -(y2)); // -y' 828 } 829 830 final double[] W = MatrixUtils.solveHomogeneousSystem(A); 831 832 final Matrix homography = new Matrix(3, 3); 833 homography.set(0, 0, W[0]); 834 homography.set(0, 1, W[1]); 835 homography.set(0, 2, W[2]); 836 homography.set(1, 0, W[3]); 837 homography.set(1, 1, W[4]); 838 homography.set(1, 2, W[5]); 839 homography.set(2, 0, W[6]); 840 homography.set(2, 1, W[7]); 841 homography.set(2, 2, W[8]); 842 843 // it makes sense to rescale the matrix here by 1 / tf[2][2], 844 // unless tf[2][2] == 0 845 if (Math.abs(homography.get(2, 2)) > 0.000001) { 846 MatrixUtils.times(homography, 1.0 / homography.get(2, 2)); 847 } 848 849 return homography; 850 } 851 852 /** 853 * Given a point x and y, calculate the 2x2 affine transform component of 854 * the 3x3 homography provided such that: 855 * 856 * H = AH_p H = { {h11,h12,h13}, {h21,h22,h23}, {h31,h32,h33} } H_p = { 857 * {1,0,0}, {0,1,0}, {h31,h32,1} } A = { {a11,a12,a13}, {a21,a22,a23}, 858 * {0,0,1} } 859 * 860 * so 861 * 862 * @param homography 863 * @return the estimated Homography 864 */ 865 public static Matrix homographyToAffine(Matrix homography) { 866 final double h11 = homography.get(0, 0); 867 final double h12 = homography.get(0, 1); 868 final double h13 = homography.get(0, 2); 869 final double h21 = homography.get(1, 0); 870 final double h22 = homography.get(1, 1); 871 final double h23 = homography.get(1, 2); 872 final double h31 = homography.get(2, 0); 873 final double h32 = homography.get(2, 1); 874 final double h33 = homography.get(2, 2); 875 876 final Matrix affine = new Matrix(3, 3); 877 affine.set(0, 0, h11 - (h13 * h31) / h33); 878 affine.set(0, 1, h12 - (h13 * h32) / h33); 879 affine.set(0, 2, h13 / h33); 880 affine.set(1, 0, h21 - (h23 * h31) / h33); 881 affine.set(1, 1, h22 - (h23 * h32) / h33); 882 affine.set(1, 2, h23 / h33); 883 affine.set(2, 0, 0); 884 affine.set(2, 1, 0); 885 affine.set(2, 2, 1); 886 887 return affine; 888 } 889 890 /** 891 * Estimate the closest (in the least-squares sense) affine transform for a 892 * homography. 893 * 894 * @param homography 895 * the homography 896 * @param x 897 * @param y 898 * 899 * @return estimated affine transform. 900 */ 901 public static Matrix homographyToAffine(Matrix homography, double x, double y) { 902 final double h11 = homography.get(0, 0); 903 final double h12 = homography.get(0, 1); 904 final double h13 = homography.get(0, 2); 905 final double h21 = homography.get(1, 0); 906 final double h22 = homography.get(1, 1); 907 final double h23 = homography.get(1, 2); 908 final double h31 = homography.get(2, 0); 909 final double h32 = homography.get(2, 1); 910 final double h33 = homography.get(2, 2); 911 912 final Matrix affine = new Matrix(3, 3); 913 final double fxdx = h11 / (h31 * x + h32 * y + h33) - (h11 * x + h12 * y + h13) * h31 914 / Math.pow((h31 * x + h32 * y + h33), 2); 915 final double fxdy = h12 / (h31 * x + h32 * y + h33) - (h11 * x + h12 * y + h13) * h32 916 / Math.pow((h31 * x + h32 * y + h33), 2); 917 918 final double fydx = h21 / (h31 * x + h32 * y + h33) - (h21 * x + h22 * y + h23) * h31 919 / Math.pow((h31 * x + h32 * y + h33), 2); 920 final double fydy = h22 / (h31 * x + h32 * y + h33) - (h21 * x + h22 * y + h23) * h32 921 / Math.pow((h31 * x + h32 * y + h33), 2); 922 affine.set(0, 0, fxdx); 923 affine.set(0, 1, fxdy); 924 affine.set(0, 2, 0); 925 affine.set(1, 0, fydx); 926 affine.set(1, 1, fydy); 927 affine.set(1, 2, 0); 928 affine.set(2, 0, 0); 929 affine.set(2, 1, 0); 930 affine.set(2, 2, 1); 931 932 return affine; 933 } 934 935 /** 936 * Create a transform to transform from one rectangle to another. 937 * 938 * @param from 939 * first rectangle 940 * @param to 941 * second rectangle 942 * @return the transform 943 */ 944 public static Matrix makeTransform(Rectangle from, Rectangle to) { 945 final Point2d trans = to.getTopLeft().minus(from.getTopLeft()); 946 final double scaleW = to.getWidth() / from.getWidth(); 947 final double scaleH = to.getHeight() / from.getHeight(); 948 949 return TransformUtilities.translateMatrix(trans.getX(), trans.getY()).times( 950 TransformUtilities.scaleMatrix(scaleW, scaleH)); 951 } 952 953 /** 954 * Given an approximate rotation matrix Q (that doesn't necessarily conform 955 * properly to a rotation matrix), return the best estimate of a rotation 956 * matrix R such that the Frobenius norm is minimised: min||r-Q||^2_F. 957 * <p> 958 * Fundamentally, this works by performing an SVD of the matrix, setting all 959 * the singular values to 1 and then reconstructing the input. 960 * 961 * @param approx 962 * the initial guess 963 * @return the rotation matrix 964 */ 965 public static Matrix approximateRotationMatrix(Matrix approx) { 966 final SingularValueDecomposition svd = approx.svd(); 967 968 return svd.getU().times(svd.getV().transpose()); 969 } 970 971 /** 972 * Convert a 3D rotation matrix to a Rodrigues rotation vector, which is 973 * oriented along the rotation axis, and has magnitude equal to the rotation 974 * angle. 975 * 976 * @param R 977 * the rotation matrix 978 * @return the Rodrigues rotation vector 979 */ 980 public static double[] rodrigues(Matrix R) { 981 final double w_norm = Math.acos((R.trace() - 1) / 2); 982 if (w_norm < 10e-10) { 983 return new double[3]; 984 } else { 985 final double norm = w_norm / (2 * Math.sin(w_norm)); 986 return new double[] { 987 norm * (R.get(2, 1) - R.get(1, 2)), 988 norm * (R.get(0, 2) - R.get(2, 0)), 989 norm * (R.get(1, 0) - R.get(0, 1)) 990 }; 991 } 992 } 993 994 /** 995 * Convert a Rodrigues rotation vector to a rotation matrix. 996 * 997 * @param r 998 * the Rodrigues rotation vector 999 * @return the rotation matrix 1000 */ 1001 public static Matrix rodrigues(double[] r) { 1002 final Matrix wx = new Matrix(new double[][] { 1003 { 0, -r[2], r[1] }, 1004 { r[2], 0, -r[0] }, 1005 { -r[1], r[0], 0 } 1006 }); 1007 1008 final double norm = Math.sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2]); 1009 1010 if (norm < 1e-10) { 1011 return Matrix.identity(3, 3); 1012 } else { 1013 final Matrix t1 = wx.times(Math.sin(norm) / norm); 1014 final Matrix t2 = wx.times(wx).times((1 - Math.cos(norm)) / (norm * norm)); 1015 1016 return Matrix.identity(3, 3).plus(t1).plus(t2); 1017 } 1018 } 1019}