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