View Javadoc

1   /**
2    * FaceTracker Licence
3    * -------------------
4    * (Academic, non-commercial, not-for-profit licence)
5    *
6    * Copyright (c) 2010 Jason Mora Saragih
7    * All rights reserved.
8    *
9    * Redistribution and use in source and binary forms, with or without
10   * modification, are permitted provided that the following conditions are met:
11   *
12   *     * The software is provided under the terms of this licence stricly for
13   *       academic, non-commercial, not-for-profit purposes.
14   *     * Redistributions of source code must retain the above copyright notice,
15   *       this list of conditions (licence) and the following disclaimer.
16   *     * Redistributions in binary form must reproduce the above copyright
17   *       notice, this list of conditions (licence) and the following disclaimer
18   *       in the documentation and/or other materials provided with the
19   *       distribution.
20   *     * The name of the author may not be used to endorse or promote products
21   *       derived from this software without specific prior written permission.
22   *     * As this software depends on other libraries, the user must adhere to and
23   *       keep in place any licencing terms of those libraries.
24   *     * Any publications arising from the use of this software, including but
25   *       not limited to academic journal and conference publications, technical
26   *       reports and manuals, must cite the following work:
27   *
28   *       J. M. Saragih, S. Lucey, and J. F. Cohn. Face Alignment through Subspace
29   *       Constrained Mean-Shifts. International Journal of Computer Vision
30   *       (ICCV), September, 2009.
31   *
32   * THIS SOFTWARE IS PROVIDED BY THE AUTHOR "AS IS" AND ANY EXPRESS OR IMPLIED
33   * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
34   * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO
35   * EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
36   * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
37   * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
38   * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
39   * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
40   * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
41   * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
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   * Constrained Local Model
63   * 
64   * @author Jason Mora Saragih
65   * @author Jonathon Hare (jsh2@ecs.soton.ac.uk)
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  		// data for similarity xform
87  		double a;
88  		double b;
89  		double tx;
90  		double ty;
91  	}
92  
93  	/** 3D Shape model */
94  	public PDM _pdm;
95  
96  	/** local parameters */
97  	public Matrix _plocal;
98  
99  	/** global parameters */
100 	public Matrix _pglobl;
101 
102 	/** Reference shape */
103 	public Matrix _refs;
104 
105 	/** Centers/view (Euler) */
106 	public Matrix[] _cent;
107 
108 	/** Visibility for each view */
109 	public Matrix[] _visi;
110 
111 	/** Patches/point/view */
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 	 * Construct CLM
189 	 * 
190 	 * @param s
191 	 * @param r
192 	 * @param c
193 	 * @param v
194 	 * @param p
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 	 * Read a CLM
292 	 * @param s
293 	 * @param readType
294 	 * @return the CLM
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 	 * Makes a copy of this CLM.
344 	 * 
345 	 * @return A copy of this CLM.
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 	 * @return View index
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 	 * Fit the model to the image
420 	 * @param im
421 	 * @param wSize
422 	 * @param nIter
423 	 * @param clamp
424 	 * @param fTol
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 				// gah, we need to get a subimage backed by the original;
458 				// luckily its from the origin
459 				FImage wimg = subImage(wmem_[i], w, h);
460 
461 				FImage wimg_o = wimg; // why? is this supposed to clone?
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 	 * Construct a view on an FImage from the origin to a new height/width
489 	 * (which must be the same or smaller than in the input image)
490 	 * 
491 	 * @param fImage
492 	 * @param i
493 	 * @param j
494 	 * @return
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 		// FIXME: move this somewhere appropriate
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 			// FIXME - in the original this creates "views" rather than
537 			// copies
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 		// FIXME do we need to deal with rigid setting underlying _u correctly?
632 		// this attempts do do so, but might not be the best way!
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 }