1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43 package com.jsaragih;
44
45 import java.io.BufferedReader;
46 import java.io.BufferedWriter;
47 import java.io.FileNotFoundException;
48 import java.io.FileReader;
49 import java.io.FileWriter;
50 import java.io.IOException;
51 import java.util.Arrays;
52 import java.util.Scanner;
53
54 import org.openimaj.citation.annotation.Reference;
55 import org.openimaj.citation.annotation.ReferenceType;
56 import org.openimaj.image.FImage;
57 import org.openimaj.math.matrix.MatrixUtils;
58
59 import Jama.Matrix;
60
61
62
63
64
65
66
67 @Reference(
68 type = ReferenceType.Inproceedings,
69 author = { "Jason M. Saragih", "Simon Lucey", "Jeffrey F. Cohn" },
70 title = "Face alignment through subspace constrained mean-shifts",
71 year = "2009",
72 booktitle = "IEEE 12th International Conference on Computer Vision, ICCV 2009, Kyoto, Japan, September 27 - October 4, 2009",
73 pages = { "1034", "1041" },
74 publisher = "IEEE",
75 customData = {
76 "doi", "http://dx.doi.org/10.1109/ICCV.2009.5459377",
77 "researchr", "http://researchr.org/publication/SaragihLC09",
78 "cites", "0",
79 "citedby", "0"
80 }
81 )
82 public class CLM {
83 static { Tracker.init(); }
84
85 class SimTData {
86
87 double a;
88 double b;
89 double tx;
90 double ty;
91 }
92
93
94 public PDM _pdm;
95
96
97 public Matrix _plocal;
98
99
100 public Matrix _pglobl;
101
102
103 public Matrix _refs;
104
105
106 public Matrix[] _cent;
107
108
109 public Matrix[] _visi;
110
111
112 public MPatch[][] _patch;
113
114 private Matrix cshape_, bshape_, oshape_, ms_, u_, g_, J_, H_;
115 private FImage[] prob_;
116 private FImage[] pmem_;
117 private FImage[] wmem_;
118
119 void calcSimT(Matrix src, Matrix dst, SimTData data) {
120 assert ((src.getRowDimension() == dst.getRowDimension())
121 && (src.getColumnDimension() == dst.getColumnDimension()) && (src
122 .getColumnDimension() == 1));
123
124 int n = src.getRowDimension() / 2;
125
126 Matrix H = new Matrix(4, 4);
127 Matrix g = new Matrix(4, 1);
128
129 final double[][] Hv = H.getArray();
130 final double[][] gv = g.getArray();
131
132 for (int i = 0; i < n; i++) {
133 double ptr1x = src.get(i, 0);
134 double ptr1y = src.get(i + n, 0);
135 double ptr2x = dst.get(i, 0);
136 double ptr2y = dst.get(i + n, 0);
137
138 Hv[0][0] += (ptr1x * ptr1x) + (ptr1y * ptr1y);
139 Hv[0][2] += ptr1x;
140 Hv[0][3] += ptr1y;
141
142 gv[0][0] += ptr1x * ptr2x + ptr1y * ptr2y;
143 gv[1][0] += ptr1x * ptr2y - ptr1y * ptr2x;
144 gv[2][0] += ptr2x;
145 gv[3][0] += ptr2y;
146 }
147
148 Hv[1][1] = Hv[0][0];
149 Hv[3][0] = Hv[0][3];
150 Hv[1][2] = Hv[2][1] = -Hv[3][0];
151 Hv[1][3] = Hv[3][1] = Hv[2][0] = Hv[0][2];
152 Hv[2][2] = Hv[3][3] = n;
153
154 Matrix p = H.solve(g);
155
156 data.a = p.get(0, 0);
157 data.b = p.get(1, 0);
158 data.tx = p.get(2, 0);
159 data.ty = p.get(3, 0);
160 }
161
162 void invSimT(SimTData in, SimTData out) {
163 Matrix M = new Matrix(
164 new double[][] { { in.a, -in.b }, { in.b, in.a } });
165 Matrix N = M.inverse();
166 out.a = N.get(0, 0);
167 out.b = N.get(1, 0);
168
169 out.tx = -1.0 * (N.get(0, 0) * in.tx + N.get(0, 1) * in.ty);
170 out.ty = -1.0 * (N.get(1, 0) * in.tx + N.get(1, 1) * in.ty);
171 }
172
173 void simT(Matrix s, SimTData data) {
174 assert (s.getColumnDimension() == 1);
175
176 int n = s.getRowDimension() / 2;
177
178 for (int i = 0; i < n; i++) {
179 double x = s.get(i, 0);
180 double y = s.get(i + n, 0);
181
182 s.set(i, 0, data.a * x - data.b * y + data.tx);
183 s.set(i + n, 0, data.b * x + data.a * y + data.ty);
184 }
185 }
186
187
188
189
190
191
192
193
194
195
196 public CLM(PDM s, Matrix r, Matrix[] c, Matrix[] v, MPatch[][] p) {
197 int n = p.length;
198
199 assert (((int) c.length == n) && ((int) v.length == n));
200 assert ((r.getRowDimension() == 2 * s.nPoints()) && (r
201 .getColumnDimension() == 1));
202
203 for (int i = 0; i < n; i++) {
204 assert ((int) p[i].length == s.nPoints());
205 assert ((c[i].getRowDimension() == 3) && (c[i].getColumnDimension() == 1));
206 assert ((v[i].getRowDimension() == s.nPoints()) && (v[i]
207 .getColumnDimension() == 1));
208 }
209
210 _pdm = s;
211 _refs = r.copy();
212 _cent = new Matrix[n];
213 _visi = new Matrix[n];
214 _patch = new MPatch[n][];
215
216 for (int i = 0; i < n; i++) {
217 _cent[i] = c[i].copy();
218 _visi[i] = v[i].copy();
219 _patch[i] = new MPatch[p[i].length];
220
221 for (int j = 0; j < p[i].length; j++)
222 _patch[i][j] = p[i][j];
223 }
224
225 _plocal = new Matrix(_pdm.nModes(), 1);
226 _pglobl = new Matrix(6, 1);
227 cshape_ = new Matrix(2 * _pdm.nPoints(), 1);
228 bshape_ = new Matrix(2 * _pdm.nPoints(), 1);
229 oshape_ = new Matrix(2 * _pdm.nPoints(), 1);
230 ms_ = new Matrix(2 * _pdm.nPoints(), 1);
231 u_ = new Matrix(6 + _pdm.nModes(), 1);
232 g_ = new Matrix(6 + _pdm.nModes(), 1);
233 J_ = new Matrix(2 * _pdm.nPoints(), 6 + _pdm.nModes());
234 H_ = new Matrix(6 + _pdm.nModes(), 6 + _pdm.nModes());
235
236 prob_ = new FImage[_pdm.nPoints()];
237 pmem_ = new FImage[_pdm.nPoints()];
238 wmem_ = new FImage[_pdm.nPoints()];
239 }
240
241 CLM() {
242 }
243
244 static CLM load(final String fname) throws FileNotFoundException {
245 BufferedReader br = null;
246 try {
247 br = new BufferedReader(new FileReader(fname));
248 Scanner sc = new Scanner(br);
249 return read(sc, true);
250 } finally {
251 try {
252 br.close();
253 } catch (IOException e) {
254 }
255 }
256 }
257
258 void save(final String fname) throws IOException {
259 BufferedWriter bw = null;
260 try {
261 bw = new BufferedWriter(new FileWriter(fname));
262
263 write(bw);
264 } finally {
265 try {
266 if (bw != null)
267 bw.close();
268 } catch (IOException e) {
269 }
270 }
271 }
272
273 void write(BufferedWriter s) throws IOException {
274 s.write(IO.Types.CLM.ordinal() + " " + _patch.length + " ");
275 _pdm.write(s);
276 IO.writeMat(s, _refs);
277
278 for (int i = 0; i < _cent.length; i++)
279 IO.writeMat(s, _cent[i]);
280
281 for (int i = 0; i < _visi.length; i++)
282 IO.writeMat(s, _visi[i]);
283
284 for (int i = 0; i < _patch.length; i++) {
285 for (int j = 0; j < _pdm.nPoints(); j++)
286 _patch[i][j].write(s);
287 }
288 }
289
290
291
292
293
294
295
296 public static CLM read(Scanner s, boolean readType) {
297 if (readType) {
298 int type = s.nextInt();
299 assert (type == IO.Types.CLM.ordinal());
300 }
301
302 CLM clm = new CLM();
303
304 int n = s.nextInt();
305 clm._pdm = PDM.read(s, true);
306 clm._cent = new Matrix[n];
307 clm._visi = new Matrix[n];
308 clm._patch = new MPatch[n][];
309 clm._refs = IO.readMat(s);
310
311 for (int i = 0; i < clm._cent.length; i++)
312 clm._cent[i] = IO.readMat(s);
313
314 for (int i = 0; i < clm._visi.length; i++)
315 clm._visi[i] = IO.readMat(s);
316
317 for (int i = 0; i < clm._patch.length; i++) {
318 clm._patch[i] = new MPatch[clm._pdm.nPoints()];
319
320 for (int j = 0; j < clm._pdm.nPoints(); j++) {
321 clm._patch[i][j] = MPatch.read(s, true);
322 }
323 }
324
325 clm._plocal = new Matrix(clm._pdm.nModes(), 1);
326 clm._pglobl = new Matrix(6, 1);
327 clm.cshape_ = new Matrix(2 * clm._pdm.nPoints(), 1);
328 clm.bshape_ = new Matrix(2 * clm._pdm.nPoints(), 1);
329 clm.oshape_ = new Matrix(2 * clm._pdm.nPoints(), 1);
330 clm.ms_ = new Matrix(2 * clm._pdm.nPoints(), 1);
331 clm.u_ = new Matrix(6 + clm._pdm.nModes(), 1);
332 clm.g_ = new Matrix(6 + clm._pdm.nModes(), 1);
333 clm.J_ = new Matrix(2 * clm._pdm.nPoints(), 6 + clm._pdm.nModes());
334 clm.H_ = new Matrix(6 + clm._pdm.nModes(), 6 + clm._pdm.nModes());
335 clm.prob_ = new FImage[clm._pdm.nPoints()];
336 clm.pmem_ = new FImage[clm._pdm.nPoints()];
337 clm.wmem_ = new FImage[clm._pdm.nPoints()];
338
339 return clm;
340 }
341
342
343
344
345
346
347 public CLM copy() {
348 CLM c = new CLM();
349 c._pdm = _pdm.copy();
350 c._cent = new Matrix[_cent.length];
351 c._visi = new Matrix[_visi.length];
352 c._patch = new MPatch[_patch.length][];
353
354 for (int i = 0; i < _cent.length; i++)
355 c._cent[i] = _cent[i].copy();
356
357 for (int i = 0; i < _visi.length; i++)
358 c._visi[i] = _visi[i].copy();
359
360 for (int i = 0; i < _patch.length; i++) {
361 c._patch[i] = new MPatch[_pdm.nPoints()];
362 for (int j = 0; j < _pdm.nPoints(); j++)
363 c._patch[i][j] = _patch[i][j].copy();
364 }
365
366 c._refs = _refs.copy();
367 c._plocal = _plocal.copy();
368 c._pglobl = _pglobl.copy();
369 c.cshape_ = cshape_.copy();
370 c.bshape_ = bshape_.copy();
371 c.oshape_ = oshape_.copy();
372 c.ms_ = ms_.copy();
373 c.u_ = u_.copy();
374 c.g_ = g_.copy();
375 c.J_ = J_.copy();
376 c.H_ = H_.copy();
377 c.prob_ = Arrays
378 .copyOf(prob_, prob_.length, (new FImage[0]).getClass());
379 c.pmem_ = Arrays
380 .copyOf(pmem_, pmem_.length, (new FImage[0]).getClass());
381 c.wmem_ = Arrays
382 .copyOf(wmem_, wmem_.length, (new FImage[0]).getClass());
383
384 return c;
385 }
386
387 final int nViews() {
388 return _patch.length;
389 }
390
391
392
393
394 public int getViewIdx() {
395 int idx = 0;
396
397 if (this.nViews() == 1) {
398 return 0;
399 } else {
400 int i;
401 double v1, v2, v3, d, dbest = -1.0;
402 for (i = 0; i < this.nViews(); i++) {
403 v1 = _pglobl.get(1, 0) - _cent[i].get(0, 0);
404 v2 = _pglobl.get(2, 0) - _cent[i].get(1, 0);
405 v3 = _pglobl.get(3, 0) - _cent[i].get(2, 0);
406
407 d = v1 * v1 + v2 * v2 + v3 * v3;
408
409 if (dbest < 0 || d < dbest) {
410 dbest = d;
411 idx = i;
412 }
413 }
414 return idx;
415 }
416 }
417
418
419
420
421
422
423
424
425
426 public void fit(FImage im, int[] wSize, int nIter, double clamp, double fTol) {
427 int i, idx, n = _pdm.nPoints();
428
429 SimTData d1 = new SimTData();
430 SimTData d2 = new SimTData();
431
432 for (int witer = 0; witer < wSize.length; witer++) {
433 _pdm.calcShape2D(cshape_, _plocal, _pglobl);
434
435 calcSimT(_refs, cshape_, d1);
436 invSimT(d1, d2);
437
438 idx = getViewIdx();
439
440 for (i = 0; i < n; i++) {
441 if (_visi[idx].getRowDimension() == n) {
442 if (_visi[idx].get(i, 0) == 0)
443 continue;
444 }
445
446 int w = wSize[witer] + _patch[idx][i]._w - 1;
447 int h = wSize[witer] + _patch[idx][i]._h - 1;
448
449 Matrix sim = new Matrix(new double[][] {
450 { d1.a, -d1.b, cshape_.get(i, 0) },
451 { d1.b, d1.a, cshape_.get(i + n, 0) } });
452
453 if (wmem_[i] == null || (w > wmem_[i].width)
454 || (h > wmem_[i].height))
455 wmem_[i] = new FImage(w, h);
456
457
458
459 FImage wimg = subImage(wmem_[i], w, h);
460
461 FImage wimg_o = wimg;
462 Matrix sim_o = sim;
463 FImage im_o = im;
464
465 cvGetQuadrangleSubPix(im_o, wimg_o, sim_o);
466
467 if (pmem_[i] == null || wSize[witer] > pmem_[i].height)
468 pmem_[i] = new FImage(wSize[witer], wSize[witer]);
469
470 prob_[i] = subImage(pmem_[i], wSize[witer], wSize[witer]);
471
472 _patch[idx][i].response(wimg, prob_[i]);
473 }
474
475 simT(cshape_, d2);
476 _pdm.applySimT(d2, _pglobl);
477 bshape_.setMatrix(0, cshape_.getRowDimension() - 1, 0,
478 cshape_.getColumnDimension() - 1, cshape_);
479
480 this.optimize(idx, wSize[witer], nIter, fTol, clamp, true);
481 this.optimize(idx, wSize[witer], nIter, fTol, clamp, false);
482
483 _pdm.applySimT(d1, _pglobl);
484 }
485 }
486
487
488
489
490
491
492
493
494
495
496 private FImage subImage(FImage fImage, int w, int h) {
497 FImage img = new FImage(fImage.pixels);
498 img.width = w;
499 img.height = h;
500 return img;
501 }
502
503 private void cvGetQuadrangleSubPix(FImage src, FImage dest, Matrix tx) {
504
505 final float[][] dpix = dest.pixels;
506
507 final double A11 = tx.get(0, 0);
508 final double A12 = tx.get(0, 1);
509 final double A21 = tx.get(1, 0);
510 final double A22 = tx.get(1, 1);
511 final double b1 = tx.get(0, 2);
512 final double b2 = tx.get(1, 2);
513
514 for (int y = 0; y < dest.width; y++) {
515 for (int x = 0; x < dest.height; x++) {
516 double xp = x - (dest.width - 1) * 0.5;
517 double yp = y - (dest.height - 1) * 0.5;
518
519 float xpp = (float) (A11 * xp + A12 * yp + b1);
520 float ypp = (float) (A21 * xp + A22 * yp + b2);
521
522 dpix[y][x] = src.getPixelInterpNative(xpp, ypp, 0);
523 }
524 }
525 }
526
527 void optimize(int idx, int wSize, int nIter, double fTol, double clamp,
528 boolean rigid) {
529 int m = _pdm.nModes();
530 int n = _pdm.nPoints();
531
532 double sigma = (wSize * wSize) / 36.0;
533
534 Matrix u, g, J, H;
535 if (rigid) {
536
537
538 u = u_.getMatrix(0, 6 - 1, 0, 1 - 1);
539 g = g_.getMatrix(0, 6 - 1, 0, 1 - 1);
540 J = J_.getMatrix(0, 2 * n - 1, 0, 6 - 1);
541 H = H_.getMatrix(0, 6 - 1, 0, 6 - 1);
542 } else {
543 u = u_;
544 g = g_;
545 J = J_;
546 H = H_;
547 }
548
549 for (int iter = 0; iter < nIter; iter++) {
550 _pdm.calcShape2D(cshape_, _plocal, _pglobl);
551
552 if (iter > 0) {
553 if (l2norm(cshape_, oshape_) < fTol)
554 break;
555 }
556
557 oshape_.setMatrix(0, oshape_.getRowDimension() - 1, 0,
558 oshape_.getColumnDimension() - 1, cshape_);
559
560 if (rigid) {
561 _pdm.calcRigidJacob(_plocal, _pglobl, J);
562 } else {
563 _pdm.calcJacob(_plocal, _pglobl, J);
564 }
565
566 for (int i = 0; i < n; i++) {
567 if (_visi[idx].getRowDimension() == n) {
568 if (_visi[idx].get(i, 0) == 0) {
569 MatrixUtils.setRow(J, i, 0);
570
571 MatrixUtils.setRow(J, i + n, 0);
572
573 ms_.set(i, 0, 0);
574 ms_.set(i + n, 0, 0);
575
576 continue;
577 }
578 }
579
580 double dx = cshape_.get(i, 0) - bshape_.get(i, 0) + (wSize - 1)
581 / 2;
582 double dy = cshape_.get(i + n, 0) - bshape_.get(i + n, 0)
583 + (wSize - 1) / 2;
584
585 double mx = 0.0, my = 0.0, sum = 0.0;
586 for (int ii = 0; ii < wSize; ii++) {
587 double vx = (dy - ii) * (dy - ii);
588
589 for (int jj = 0; jj < wSize; jj++) {
590 double vy = (dx - jj) * (dx - jj);
591
592 double v = prob_[i].pixels[ii][jj];
593 v *= Math.exp(-0.5 * (vx + vy) / sigma);
594 sum += v;
595 mx += v * jj;
596 my += v * ii;
597 }
598 }
599
600 ms_.set(i, 0, mx / sum - dx);
601 ms_.set(i + n, 0, my / sum - dy);
602 }
603
604 g = J.transpose().times(ms_);
605 H = J.transpose().times(J);
606
607 if (!rigid) {
608 for (int i = 0; i < m; i++) {
609 double var = 0.5 * sigma / _pdm._E.get(0, i);
610
611 H.getArray()[6 + i][6 + i] += var;
612 g.getArray()[6 + i][0] -= var * _plocal.get(i, 0);
613 }
614 }
615
616 MatrixUtils.fill(u_, 0);
617 u = H.solve(g);
618
619 if (rigid)
620 u_.setMatrix(0, 6 - 1, 0, 1 - 1, u);
621 else
622 u_.setMatrix(0, u.getRowDimension() - 1, 0,
623 u.getColumnDimension() - 1, u);
624
625 _pdm.calcReferenceUpdate(u_, _plocal, _pglobl);
626
627 if (!rigid)
628 _pdm.clamp(_plocal, clamp);
629 }
630
631
632
633 if (rigid) {
634 u_.setMatrix(0, 6 - 1, 0, 1 - 1, u);
635 g_.setMatrix(0, 6 - 1, 0, 1 - 1, g);
636 J_.setMatrix(0, 2 * n - 1, 0, 6 - 1, J);
637 H_.setMatrix(0, 6 - 1, 0, 6 - 1, H);
638 } else {
639 u_.setMatrix(0, u.getRowDimension() - 1, 0,
640 u.getColumnDimension() - 1, u);
641 g_.setMatrix(0, g.getRowDimension() - 1, 0,
642 g.getColumnDimension() - 1, g);
643 J_.setMatrix(0, J.getRowDimension() - 1, 0,
644 J.getColumnDimension() - 1, J);
645 H_.setMatrix(0, H.getRowDimension() - 1, 0,
646 H.getColumnDimension() - 1, H);
647 }
648 }
649
650 private double l2norm(Matrix m1, Matrix m2) {
651 final double[][] m1v = m1.getArray();
652 final double[][] m2v = m2.getArray();
653 final int rows = m1.getRowDimension();
654 final int cols = m1.getColumnDimension();
655
656 double sum = 0;
657 for (int r = 0; r < rows; r++) {
658 for (int c = 0; c < cols; c++) {
659 double diff = m1v[r][c] - m2v[r][c];
660
661 sum += diff * diff;
662 }
663 }
664
665 return Math.sqrt(sum);
666 }
667 }