001/** 002 * FaceTracker Licence 003 * ------------------- 004 * (Academic, non-commercial, not-for-profit licence) 005 * 006 * Copyright (c) 2010 Jason Mora Saragih 007 * All rights reserved. 008 * 009 * Redistribution and use in source and binary forms, with or without 010 * modification, are permitted provided that the following conditions are met: 011 * 012 * * The software is provided under the terms of this licence stricly for 013 * academic, non-commercial, not-for-profit purposes. 014 * * Redistributions of source code must retain the above copyright notice, 015 * this list of conditions (licence) and the following disclaimer. 016 * * Redistributions in binary form must reproduce the above copyright 017 * notice, this list of conditions (licence) and the following disclaimer 018 * in the documentation and/or other materials provided with the 019 * distribution. 020 * * The name of the author may not be used to endorse or promote products 021 * derived from this software without specific prior written permission. 022 * * As this software depends on other libraries, the user must adhere to and 023 * keep in place any licencing terms of those libraries. 024 * * Any publications arising from the use of this software, including but 025 * not limited to academic journal and conference publications, technical 026 * reports and manuals, must cite the following work: 027 * 028 * J. M. Saragih, S. Lucey, and J. F. Cohn. Face Alignment through Subspace 029 * Constrained Mean-Shifts. International Journal of Computer Vision 030 * (ICCV), September, 2009. 031 * 032 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR "AS IS" AND ANY EXPRESS OR IMPLIED 033 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 034 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO 035 * EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 036 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 037 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 038 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY 039 * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 040 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 041 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 042 */ 043package com.jsaragih; 044 045import java.io.BufferedReader; 046import java.io.BufferedWriter; 047import java.io.FileNotFoundException; 048import java.io.FileReader; 049import java.io.FileWriter; 050import java.io.IOException; 051import java.util.Scanner; 052 053import org.openimaj.math.matrix.MatrixUtils; 054 055import com.jsaragih.CLM.SimTData; 056 057import Jama.Matrix; 058import Jama.SingularValueDecomposition; 059 060/** 061 * 3D Point Distribution Model 062 * 063 * @author Jason Mora Saragih 064 * @author Jonathon Hare (jsh2@ecs.soton.ac.uk) 065 */ 066public class PDM { 067 static { Tracker.init(); } 068 069 /** basis of variation */ 070 public Matrix _V; 071 072 /** vector of eigenvalues (row vector) */ 073 public Matrix _E; 074 075 /** mean 3D shape vector [x1,..,xn,y1,...yn] */ 076 public Matrix _M; 077 078 private Matrix S_, R_, P_, Px_, Py_, Pz_, R1_, R2_, R3_; 079 080 /** 081 * Returns a copy of this PDM. 082 * 083 * @return A copy of this PDM. 084 */ 085 public PDM copy() { 086 PDM p = new PDM(); 087 p._V = _V.copy(); 088 p._E = _E.copy(); 089 p._M = _M.copy(); 090 p.S_ = S_.copy(); 091 p.R_ = R_.copy(); 092 p.P_ = P_.copy(); 093 p.Px_ = Px_.copy(); 094 p.Py_ = Py_.copy(); 095 p.Pz_ = Pz_.copy(); 096 p.R1_ = R1_.copy(); 097 p.R2_ = R2_.copy(); 098 p.R3_ = R3_.copy(); 099 return p; 100 } 101 102 void addOrthRow(Matrix R) { 103 assert ((R.getRowDimension() == 3) && (R.getColumnDimension() == 3)); 104 105 R.set(2, 0, R.get(0, 1) * R.get(1, 2) - R.get(0, 2) * R.get(1, 1)); 106 R.set(2, 1, R.get(0, 2) * R.get(1, 0) - R.get(0, 0) * R.get(1, 2)); 107 R.set(2, 2, R.get(0, 0) * R.get(1, 1) - R.get(0, 1) * R.get(1, 0)); 108 } 109 110 void metricUpgrade(Matrix R) { 111 assert ((R.getRowDimension() == 3) && (R.getColumnDimension() == 3)); 112 SingularValueDecomposition svd = R.svd(); 113 114 Matrix X = svd.getU().times(svd.getV().transpose()); 115 Matrix W = Matrix.identity(3, 3); 116 W.set(2, 2, X.det()); 117 118 R.setMatrix(0, 3 - 1, 0, 3 - 1, 119 svd.getU().times(W).times(svd.getV().transpose())); 120 } 121 122 Matrix euler2Rot(final double pitch, final double yaw, final double roll) { 123 return euler2Rot(pitch, yaw, roll, true); 124 } 125 126 Matrix euler2Rot(final double pitch, final double yaw, final double roll, 127 boolean full) { 128 Matrix R; 129 if (full) { 130 R = new Matrix(3, 3); 131 } else { 132 R = new Matrix(2, 3); 133 } 134 135 double sina = Math.sin(pitch), sinb = Math.sin(yaw), sinc = Math 136 .sin(roll); 137 double cosa = Math.cos(pitch), cosb = Math.cos(yaw), cosc = Math 138 .cos(roll); 139 R.set(0, 0, cosb * cosc); 140 R.set(0, 1, -cosb * sinc); 141 R.set(0, 2, sinb); 142 R.set(1, 0, cosa * sinc + sina * sinb * cosc); 143 R.set(1, 1, cosa * cosc - sina * sinb * sinc); 144 R.set(1, 2, -sina * cosb); 145 146 if (full) 147 addOrthRow(R); 148 149 return R; 150 } 151 152 Matrix euler2Rot(Matrix p) { 153 return euler2Rot(p, true); 154 } 155 156 Matrix euler2Rot(Matrix p, boolean full) { 157 assert ((p.getRowDimension() == 6) && (p.getColumnDimension() == 1)); 158 return euler2Rot(p.get(1, 0), p.get(2, 0), p.get(3, 0), full); 159 } 160 161 double[] rot2Euler(Matrix R) { 162 assert ((R.getRowDimension() == 3) && (R.getColumnDimension() == 3)); 163 double[] q = new double[4]; 164 q[0] = Math.sqrt(1 + R.get(0, 0) + R.get(1, 1) + R.get(2, 2)) / 2; 165 q[1] = (R.get(2, 1) - R.get(1, 2)) / (4 * q[0]); 166 q[2] = (R.get(0, 2) - R.get(2, 0)) / (4 * q[0]); 167 q[3] = (R.get(1, 0) - R.get(0, 1)) / (4 * q[0]); 168 double yaw = Math.asin(2 * (q[0] * q[2] + q[1] * q[3])); 169 double pitch = Math.atan2(2 * (q[0] * q[1] - q[2] * q[3]), q[0] * q[0] 170 - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); 171 double roll = Math.atan2(2 * (q[0] * q[3] - q[1] * q[2]), q[0] * q[0] 172 + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); 173 return new double[] { pitch, roll, yaw }; 174 } 175 176 void rot2Euler(Matrix R, Matrix p) { 177 assert ((p.getRowDimension() == 6) && (p.getColumnDimension() == 1)); 178 double[] pry = rot2Euler(R); 179 180 p.set(1, 0, pry[0]); 181 p.set(2, 0, pry[2]); 182 p.set(3, 0, pry[1]); 183 } 184 185 class AlignmentParams { 186 double scale; 187 double pitch; 188 double yaw; 189 double roll; 190 double x; 191 double y; 192 } 193 194 void align3Dto2DShapes(AlignmentParams ap, Matrix s2D, Matrix s3D) { 195 assert ((s2D.getColumnDimension() == 1) 196 && (s3D.getRowDimension() == 3 * (s2D.getRowDimension() / 2)) && (s3D 197 .getColumnDimension() == 1)); 198 199 final int n = s2D.getRowDimension() / 2; 200 double[] t2 = new double[2]; 201 double[] t3 = new double[3]; 202 203 Matrix X = MatrixUtils.reshape(s2D, 2).transpose(); 204 Matrix S = MatrixUtils.reshape(s3D, 3).transpose(); 205 206 for (int i = 0; i < 2; i++) { 207 t2[i] = MatrixUtils.sumColumn(X, i) / n; 208 MatrixUtils.incrColumn(X, i, -t2[i]); 209 } 210 211 for (int i = 0; i < 3; i++) { 212 t3[i] = MatrixUtils.sumColumn(S, i) / n; 213 MatrixUtils.incrColumn(S, i, -t3[i]); 214 } 215 216 Matrix M = ((S.transpose().times(S)).inverse()).times(S.transpose()) 217 .times(X); 218 219 Matrix MtM = M.transpose().times(M); 220 221 SingularValueDecomposition svd = MtM.svd(); 222 Matrix svals = svd.getS(); 223 svals.set(0, 0, 1.0 / Math.sqrt(svals.get(0, 0))); 224 svals.set(1, 1, 1.0 / Math.sqrt(svals.get(1, 1))); 225 226 Matrix T = new Matrix(3, 3); 227 T.setMatrix( 228 0, 229 2 - 1, 230 0, 231 3 - 1, 232 svd.getU().times(svals).times(svd.getV().transpose()) 233 .times(M.transpose())); 234 235 ap.scale = 0; 236 for (int r = 0; r < 2; r++) 237 for (int c = 0; c < 3; c++) 238 ap.scale += T.get(r, c) * M.get(c, r); 239 ap.scale *= 0.5; 240 241 addOrthRow(T); 242 243 double[] pyr = rot2Euler(T); 244 ap.pitch = pyr[0]; 245 ap.roll = pyr[1]; 246 ap.yaw = pyr[2]; 247 248 T = T.times(ap.scale); 249 250 ap.x = t2[0] 251 - (T.get(0, 0) * t3[0] + T.get(0, 1) * t3[1] + T.get(0, 2) 252 * t3[2]); 253 ap.y = t2[1] 254 - (T.get(1, 0) * t3[0] + T.get(1, 1) * t3[1] + T.get(1, 2) 255 * t3[2]); 256 } 257 258 void clamp(Matrix p, double c) { 259 assert ((p.getRowDimension() == _E.getColumnDimension()) && (p 260 .getColumnDimension() == 1)); 261 262 for (int i = 0; i < p.getRowDimension(); i++) { 263 double v = c * Math.sqrt(_E.get(0, i)); 264 double p1 = p.get(i, 0); 265 266 if (Math.abs(p1) > v) { 267 if (p1 > 0.0) { 268 p1 = v; 269 } else { 270 p1 = -v; 271 } 272 } 273 } 274 } 275 276 Matrix calcShape3D(Matrix plocal) { 277 assert ((plocal.getRowDimension() == _E.getColumnDimension()) && (plocal 278 .getColumnDimension() == 1)); 279 280 Matrix s = _M.plus(_V.times(plocal)); 281 282 return s; 283 } 284 285 /** 286 * Calculate Shape 2D 287 * 288 * @param s 289 * @param plocal 290 * @param pglobl 291 */ 292 public void calcShape2D(Matrix s, Matrix plocal, Matrix pglobl) { 293 assert ((plocal.getRowDimension() == _E.getColumnDimension()) && (plocal 294 .getColumnDimension() == 1)); 295 assert ((pglobl.getRowDimension() == 6) && (pglobl.getColumnDimension() == 1)); 296 297 int n = _M.getRowDimension() / 3; 298 double a = pglobl.get(0, 0); 299 double x = pglobl.get(4, 0); 300 double y = pglobl.get(5, 0); 301 302 R_ = euler2Rot(pglobl); 303 304 S_ = _M.plus(_V.times(plocal)); 305 306 for (int i = 0; i < n; i++) { 307 s.set(i, 308 0, 309 a 310 * (R_.get(0, 0) * S_.get(i, 0) + R_.get(0, 1) 311 * S_.get(i + n, 0) + R_.get(0, 2) 312 * S_.get(i + n * 2, 0)) + x); 313 s.set(i + n, 314 0, 315 a 316 * (R_.get(1, 0) * S_.get(i, 0) + R_.get(1, 1) 317 * S_.get(i + n, 0) + R_.get(1, 2) 318 * S_.get(i + n * 2, 0)) + y); 319 } 320 } 321 322 /** 323 * Calculate the PDM parameters 324 * 325 * @param s 326 * @param plocal 327 * @param pglobl 328 */ 329 public void calcParams(Matrix s, Matrix plocal, Matrix pglobl) { 330 assert ((s.getRowDimension() == 2 * (_M.getRowDimension() / 3)) && (s 331 .getColumnDimension() == 1)); 332 333 int n = _M.getRowDimension() / 3; 334 335 Matrix R = new Matrix(3, 3); 336 Matrix t = new Matrix(3, 1); 337 Matrix p = new Matrix(_V.getColumnDimension(), 1); 338 339 MatrixUtils.zero(plocal); 340 341 AlignmentParams ap = new AlignmentParams(); 342 343 for (int iter = 0; iter < 100; iter++) { 344 S_ = calcShape3D(plocal); 345 346 align3Dto2DShapes(ap, s, S_); 347 348 R = euler2Rot(ap.pitch, ap.yaw, ap.roll); 349 350 Matrix r = new Matrix(new double[][] { R.getArray()[2] }); 351 352 Matrix S = MatrixUtils.reshape(S_, 3).transpose(); 353 354 Matrix z = (S.times(r.transpose())).times(ap.scale); 355 double si = 1.0 / ap.scale; 356 357 double Tx = -si * (R.get(0, 0) * ap.x + R.get(1, 0) * ap.y); 358 double Ty = -si * (R.get(0, 1) * ap.x + R.get(1, 1) * ap.y); 359 double Tz = -si * (R.get(0, 2) * ap.x + R.get(1, 2) * ap.y); 360 361 for (int j = 0; j < n; j++) { 362 t.set(0, 0, s.get(j, 0)); 363 t.set(1, 0, s.get(j + n, 0)); 364 t.set(2, 0, z.get(j, 0)); 365 366 S_.set(j, 0, si * dotCol(t, R, 0) + Tx); 367 S_.set(j + n, 0, si * dotCol(t, R, 1) + Ty); 368 S_.set(j + n * 2, 0, si * dotCol(t, R, 2) + Tz); 369 } 370 371 plocal.setMatrix(0, p.getRowDimension() - 1, 0, 1 - 1, _V 372 .transpose().times(S_.minus(_M))); 373 374 if (iter > 0) { 375 double norm = 0; 376 for (int i = 0; i < plocal.getRowDimension(); i++) { 377 double diff = plocal.get(i, 0) - p.get(i, 0); 378 norm += Math.abs(diff * diff); 379 } 380 norm = Math.sqrt(norm); 381 382 if (norm < 1.0e-5) 383 break; 384 } 385 386 p.setMatrix(0, p.getRowDimension() - 1, 0, 1 - 1, plocal); 387 } 388 389 pglobl.set(0, 0, ap.scale); 390 pglobl.set(1, 0, ap.pitch); 391 pglobl.set(2, 0, ap.yaw); 392 pglobl.set(3, 0, ap.roll); 393 pglobl.set(4, 0, ap.x); 394 pglobl.set(5, 0, ap.y); 395 396 return; 397 } 398 399 private double dotCol(Matrix colvec, Matrix m, int col) { 400 final int rows = colvec.getRowDimension(); 401 402 final double[][] colvec_arr = colvec.getArray(); 403 final double[][] m_arr = m.getArray(); 404 405 double dp = 0; 406 for (int i = 0; i < rows; i++) 407 dp += colvec_arr[i][0] * m_arr[i][col]; 408 409 return dp; 410 } 411 412 /** 413 * Initialise the identify face parameters 414 * @param plocal 415 * @param pglobl 416 */ 417 public void identity(Matrix plocal, Matrix pglobl) { 418 MatrixUtils.zero(plocal); 419 420 MatrixUtils.zero(pglobl); 421 pglobl.set(0, 0, 1); 422 } 423 424 void calcRigidJacob(Matrix plocal, Matrix pglobl, Matrix Jacob) { 425 final int n = _M.getRowDimension() / 3; 426 final int m = _V.getColumnDimension(); 427 428 assert ((plocal.getRowDimension() == m) 429 && (plocal.getColumnDimension() == 1) 430 && (pglobl.getRowDimension() == 6) 431 && (pglobl.getColumnDimension() == 1) 432 && (Jacob.getRowDimension() == 2 * n) && (Jacob 433 .getColumnDimension() == 6)); 434 435 Matrix Rx = new Matrix(new double[][] { { 0, 0, 0 }, { 0, 0, -1 }, 436 { 0, 1, 0 } }); 437 Matrix Ry = new Matrix(new double[][] { { 0, 0, 1 }, { 0, 0, 0 }, 438 { -1, 0, 0 } }); 439 Matrix Rz = new Matrix(new double[][] { { 0, -1, 0 }, { 1, 0, 0 }, 440 { 0, 0, 0 } }); 441 442 double s = pglobl.get(0, 0); 443 444 S_ = calcShape3D(plocal); 445 446 R_ = euler2Rot(pglobl); 447 448 P_ = R_.getMatrix(0, 2 - 1, 0, 3 - 1).times(s); 449 Px_ = P_.times(Rx); 450 Py_ = P_.times(Ry); 451 Pz_ = P_.times(Rz); 452 453 final double[][] px = Px_.getArray(); 454 final double[][] py = Py_.getArray(); 455 final double[][] pz = Pz_.getArray(); 456 final double[][] r = R_.getArray(); 457 final double[][] J = Jacob.getArray(); 458 459 for (int i = 0; i < n; i++) { 460 double X = S_.get(i, 0); 461 double Y = S_.get(i + n, 0); 462 double Z = S_.get(i + n * 2, 0); 463 J[i][0] = r[0][0] * X + r[0][1] * Y + r[0][2] * Z; 464 J[i + n][0] = r[1][0] * X + r[1][1] * Y + r[1][2] * Z; 465 J[i][1] = px[0][0] * X + px[0][1] * Y + px[0][2] * Z; 466 J[i + n][1] = px[1][0] * X + px[1][1] * Y + px[1][2] * Z; 467 J[i][2] = py[0][0] * X + py[0][1] * Y + py[0][2] * Z; 468 J[i + n][2] = py[1][0] * X + py[1][1] * Y + py[1][2] * Z; 469 J[i][3] = pz[0][0] * X + pz[0][1] * Y + pz[0][2] * Z; 470 J[i + n][3] = pz[1][0] * X + pz[1][1] * Y + pz[1][2] * Z; 471 J[i][4] = 1.0; 472 J[i + n][4] = 0.0; 473 J[i][5] = 0.0; 474 J[i + n][5] = 1.0; 475 } 476 } 477 478 void calcJacob(Matrix plocal, Matrix pglobl, Matrix Jacob) { 479 final int n = _M.getRowDimension() / 3; 480 final int m = _V.getColumnDimension(); 481 482 assert ((plocal.getRowDimension() == m) 483 && (plocal.getColumnDimension() == 1) 484 && (pglobl.getRowDimension() == 6) 485 && (pglobl.getColumnDimension() == 1) 486 && (Jacob.getRowDimension() == 2 * n) && (Jacob 487 .getColumnDimension() == 6 + m)); 488 double s = pglobl.get(0, 0); 489 490 Matrix Rx = new Matrix(new double[][] { { 0, 0, 0 }, { 0, 0, -1 }, 491 { 0, 1, 0 } }); 492 Matrix Ry = new Matrix(new double[][] { { 0, 0, 1 }, { 0, 0, 0 }, 493 { -1, 0, 0 } }); 494 Matrix Rz = new Matrix(new double[][] { { 0, -1, 0 }, { 1, 0, 0 }, 495 { 0, 0, 0 } }); 496 497 S_ = calcShape3D(plocal); 498 499 R_ = euler2Rot(pglobl); 500 501 P_ = R_.getMatrix(0, 2 - 1, 0, 3 - 1).times(s); 502 Px_ = P_.times(Rx); 503 Py_ = P_.times(Ry); 504 Pz_ = P_.times(Rz); 505 506 final double[][] px = Px_.getArray(); 507 final double[][] py = Py_.getArray(); 508 final double[][] pz = Pz_.getArray(); 509 final double[][] p = P_.getArray(); 510 final double[][] r = R_.getArray(); 511 512 final double[][] V = _V.getArray(); 513 514 final double[][] J = Jacob.getArray(); 515 516 for (int i = 0; i < n; i++) { 517 double X = S_.get(i, 0); 518 double Y = S_.get(i + n, 0); 519 double Z = S_.get(i + n * 2, 0); 520 521 J[i][0] = r[0][0] * X + r[0][1] * Y + r[0][2] * Z; 522 J[i + n][0] = r[1][0] * X + r[1][1] * Y + r[1][2] * Z; 523 J[i][1] = px[0][0] * X + px[0][1] * Y + px[0][2] * Z; 524 J[i + n][1] = px[1][0] * X + px[1][1] * Y + px[1][2] * Z; 525 J[i][2] = py[0][0] * X + py[0][1] * Y + py[0][2] * Z; 526 J[i + n][2] = py[1][0] * X + py[1][1] * Y + py[1][2] * Z; 527 J[i][3] = pz[0][0] * X + pz[0][1] * Y + pz[0][2] * Z; 528 J[i + n][3] = pz[1][0] * X + pz[1][1] * Y + pz[1][2] * Z; 529 J[i][4] = 1.0; 530 J[i + n][4] = 0.0; 531 J[i][5] = 0.0; 532 J[i + n][5] = 1.0; 533 534 for (int j = 0; j < m; j++) { 535 J[i][6 + j] = p[0][0] * V[i][j] + p[0][1] * V[i + n][j] 536 + p[0][2] * V[i + 2 * n][j]; 537 J[i + n][6 + j] = p[1][0] * V[i][j] + p[1][1] * V[i + n][j] 538 + p[1][2] * V[i + 2 * n][j]; 539 } 540 } 541 } 542 543 void calcReferenceUpdate(Matrix dp, Matrix plocal, Matrix pglobl) { 544 assert ((dp.getRowDimension() == 6 + _V.getColumnDimension()) && (dp 545 .getColumnDimension() == 1)); 546 547 plocal.setMatrix(0, plocal.getRowDimension() - 1, 0, plocal 548 .getColumnDimension() - 1, plocal.plus(dp.getMatrix(6, 549 6 + _V.getColumnDimension() - 1, 0, 1 - 1))); 550 551 pglobl.set(0, 0, pglobl.get(0, 0) + dp.get(0, 0)); 552 pglobl.set(4, 0, pglobl.get(4, 0) + dp.get(4, 0)); 553 pglobl.set(5, 0, pglobl.get(5, 0) + dp.get(5, 0)); 554 555 R1_ = euler2Rot(pglobl); 556 557 R2_ = Matrix.identity(3, 3); 558 R2_.set(2, 1, dp.get(1, 0)); 559 R2_.set(1, 2, -R2_.get(2, 1)); 560 561 R2_.set(0, 2, dp.get(2, 0)); 562 R2_.set(2, 0, -R2_.get(0, 2)); 563 564 R2_.set(1, 0, dp.get(3, 0)); 565 R2_.set(0, 1, -R2_.get(1, 0)); 566 567 metricUpgrade(R2_); 568 R3_ = R1_.times(R2_); 569 rot2Euler(R3_, pglobl); 570 } 571 572 void applySimT(SimTData data, Matrix pglobl) { 573 assert ((pglobl.getRowDimension() == 6) && (pglobl.getColumnDimension() == 1)); 574 575 double angle = Math.atan2(data.b, data.a); 576 double scale = data.a / Math.cos(angle); 577 double ca = Math.cos(angle); 578 double sa = Math.sin(angle); 579 double xc = pglobl.get(4, 0); 580 double yc = pglobl.get(5, 0); 581 582 MatrixUtils.zero(R1_); 583 R1_.set(2, 2, 1.0); 584 R1_.set(0, 0, ca); 585 R1_.set(0, 1, -sa); 586 R1_.set(1, 0, sa); 587 R1_.set(1, 1, ca); 588 589 R2_ = euler2Rot(pglobl); 590 R3_ = R1_.times(R2_); 591 592 pglobl.set(0, 0, pglobl.get(0, 0) * scale); 593 rot2Euler(R3_, pglobl); 594 595 pglobl.set(4, 0, data.a * xc - data.b * yc + data.tx); 596 pglobl.set(5, 0, data.b * xc + data.a * yc + data.ty); 597 } 598 599 static PDM read(Scanner s, boolean readType) { 600 if (readType) { 601 int type = s.nextInt(); 602 assert (type == IO.Types.PDM.ordinal()); 603 } 604 605 PDM pdm = new PDM(); 606 607 pdm._V = IO.readMat(s); 608 pdm._E = IO.readMat(s); 609 pdm._M = IO.readMat(s); 610 611 pdm.S_ = new Matrix(pdm._M.getRowDimension(), 1); 612 pdm.R_ = new Matrix(3, 3); 613 pdm.P_ = new Matrix(2, 3); 614 pdm.Px_ = new Matrix(2, 3); 615 pdm.Py_ = new Matrix(2, 3); 616 pdm.Pz_ = new Matrix(2, 3); 617 pdm.R1_ = new Matrix(3, 3); 618 pdm.R2_ = new Matrix(3, 3); 619 pdm.R3_ = new Matrix(3, 3); 620 621 return pdm; 622 } 623 624 void write(BufferedWriter s) throws IOException { 625 s.write(IO.Types.PDM.ordinal() + " "); 626 627 IO.writeMat(s, _V); 628 IO.writeMat(s, _E); 629 IO.writeMat(s, _M); 630 } 631 632 static PDM load(final String fname) throws FileNotFoundException { 633 BufferedReader br = null; 634 try { 635 br = new BufferedReader(new FileReader(fname)); 636 Scanner sc = new Scanner(br); 637 return read(sc, true); 638 } finally { 639 try { 640 br.close(); 641 } catch (IOException e) { 642 } 643 } 644 } 645 646 void save(final String fname) throws IOException { 647 BufferedWriter bw = null; 648 try { 649 bw = new BufferedWriter(new FileWriter(fname)); 650 651 write(bw); 652 } finally { 653 try { 654 if (bw != null) 655 bw.close(); 656 } catch (IOException e) { 657 } 658 } 659 } 660 661 /** 662 * @return the number of points in the face model 663 */ 664 public final int nPoints() { 665 return _M.getRowDimension() / 3; 666 } 667 668 int nModes() { 669 return _V.getColumnDimension(); 670 } 671 672 double var(int i) { 673 assert (i < _E.getColumnDimension()); 674 675 return _E.get(0, i); 676 } 677}