1 /**
2 * Copyright (c) 2011, The University of Southampton and the individual contributors.
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without modification,
6 * are permitted provided that the following conditions are met:
7 *
8 * * Redistributions of source code must retain the above copyright notice,
9 * this list of conditions and the following disclaimer.
10 *
11 * * Redistributions in binary form must reproduce the above copyright notice,
12 * this list of conditions and the following disclaimer in the documentation
13 * and/or other materials provided with the distribution.
14 *
15 * * Neither the name of the University of Southampton nor the names of its
16 * contributors may be used to endorse or promote products derived from this
17 * software without specific prior written permission.
18 *
19 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
20 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
21 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
23 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
24 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
25 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
26 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
27 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
28 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29 */
30 package org.openimaj.image.camera;
31
32 import org.openimaj.image.FImage;
33 import org.openimaj.image.Image;
34 import org.openimaj.image.processing.transform.RemapProcessor;
35 import org.openimaj.image.processor.SinglebandImageProcessor;
36 import org.openimaj.math.geometry.point.Point2d;
37 import org.openimaj.math.geometry.transforms.TransformUtilities;
38
39 import Jama.Matrix;
40
41 /**
42 * The intrinsic parameters of a camera (focal length in x and y; skew;
43 * principal point in x and y; 3-term radial distorion; 2-term tangential
44 * distortion).
45 *
46 * @author Jonathon Hare (jsh2@ecs.soton.ac.uk)
47 */
48 public class CameraIntrinsics {
49 /**
50 * The camera calibration matrix
51 */
52 public Matrix calibrationMatrix;
53
54 /**
55 * First radial distortion term
56 */
57 public double k1;
58
59 /**
60 * Second radial distortion term
61 */
62 public double k2;
63
64 /**
65 * Third radial distortion term
66 */
67 public double k3;
68
69 /**
70 * First tangential distortion term
71 */
72 public double p1;
73
74 /**
75 * Second tangential distortion term
76 */
77 public double p2;
78
79 /**
80 * The image width of the camera in pixels
81 */
82 public int width;
83
84 /**
85 * The image height of the camera in pixels
86 */
87 public int height;
88
89 /**
90 * Construct with the given matrix.
91 *
92 * @param m
93 * the matrix
94 * @param width
95 * the image width of the camera in pixels
96 * @param height
97 * the image height of the camera in pixels
98 */
99 public CameraIntrinsics(Matrix m, int width, int height) {
100 this.calibrationMatrix = m;
101 this.width = width;
102 this.height = height;
103 }
104
105 /**
106 * Get the focal length in the x direction (in pixels)
107 *
108 * @return fx
109 */
110 public double getFocalLengthX() {
111 return calibrationMatrix.get(0, 0);
112 }
113
114 /**
115 * Get the focal length in the y direction (in pixels)
116 *
117 * @return fy
118 */
119 public double getFocalLengthY() {
120 return calibrationMatrix.get(1, 1);
121 }
122
123 /**
124 * Get the x-ordinate of the principal point (in pixels).
125 *
126 * @return cx
127 */
128 public double getPrincipalPointX() {
129 return calibrationMatrix.get(0, 2);
130 }
131
132 /**
133 * Get the y-ordinate of the principal point (in pixels).
134 *
135 * @return cy
136 */
137 public double getPrincipalPointY() {
138 return calibrationMatrix.get(1, 2);
139 }
140
141 /**
142 * Set the focal length in the x direction (in pixels)
143 *
144 * @param fy
145 * the focal length in the x direction
146 */
147 public void setFocalLengthX(double fy) {
148 calibrationMatrix.set(0, 0, fy);
149 }
150
151 /**
152 * Set the focal length in the y direction (in pixels)
153 *
154 * @param fy
155 * the focal length in the y direction
156 */
157 public void setFocalLengthY(double fy) {
158 calibrationMatrix.set(1, 1, fy);
159 }
160
161 /**
162 * Set the x-ordinate of the principal point (in pixels).
163 *
164 * @param cx
165 * the y-ordinate of the principal point
166 */
167 public void setPrincipalPointX(double cx) {
168 calibrationMatrix.set(0, 2, cx);
169 }
170
171 /**
172 * Set the y-ordinate of the principal point (in pixels).
173 *
174 * @param cy
175 * the y-ordinate of the principal point
176 */
177 public void setPrincipalPointY(double cy) {
178 calibrationMatrix.set(1, 2, cy);
179 }
180
181 /**
182 * Set the skew factor.
183 *
184 * @param skew
185 * the skew.
186 */
187 public void setSkewFactor(double skew) {
188 calibrationMatrix.set(0, 1, skew);
189 }
190
191 /**
192 * Get the skew factor.
193 *
194 * @return skew
195 */
196 public double getSkewFactor() {
197 return calibrationMatrix.get(0, 1);
198 }
199
200 /**
201 * Apply the radial and tangential distortion of this camera to the given
202 * projected point (presumably computed by projecting a world point through
203 * the homography defined by the extrinsic parameters of a camera). The
204 * point is modified in-place.
205 *
206 * @param p
207 * the projected point
208 * @return the input point (with the distortion added)
209 */
210 public Point2d applyDistortion(Point2d p) {
211 final double dx = (p.getX() - getPrincipalPointX()) / getFocalLengthX();
212 final double dy = (p.getY() - getPrincipalPointY()) / getFocalLengthY();
213 final double r2 = dx * dx + dy * dy;
214 final double r4 = r2 * r2;
215 final double r6 = r2 * r2 * r2;
216
217 // radial component
218 double tx = dx * (k1 * r2 + k2 * r4 + k3 * r6);
219 double ty = dy * (k1 * r2 + k2 * r4 + k3 * r6);
220
221 // tangential component
222 tx += 2 * p1 * dx * dy + p2 * (r2 + 2 * dx * dx);
223 ty += p1 * (r2 + 2 * dy * dy) + 2 * p2 * dx * dy;
224
225 p.translate((float) tx, (float) ty);
226 return p;
227 }
228
229 /**
230 * Compute a scaled set of intrinsic parameters based on the given image
231 * size.
232 *
233 * @param newWidth
234 * the target image size
235 * @param newHeight
236 * the target image width
237 * @return the new scaled intrinsics
238 */
239 public CameraIntrinsics getScaledIntrinsics(int newWidth, int newHeight) {
240 final double sx = (double) newWidth / (double) width;
241 final double sy = (double) newHeight / (double) height;
242
243 final Matrix m = TransformUtilities.scaleMatrix(sx, sy).times(this.calibrationMatrix);
244
245 final CameraIntrinsics newCam = new CameraIntrinsics(m, newWidth, newHeight);
246 newCam.k1 = k1;
247 newCam.k2 = k2;
248 newCam.k3 = k3;
249 newCam.p1 = p1;
250 newCam.p2 = p2;
251
252 return newCam;
253 }
254
255 @Override
256 public String toString() {
257 return String
258 .format("fx: %2.2f; fy: %2.2f; sk: %2.6f; u0: %2.2f; v0: %2.2f; k1: %2.6f; k2: %2.6f; k3: %2.6f; p1: %2.6f; p2: %2.6f",
259 getFocalLengthX(), getFocalLengthY(), getSkewFactor(), getPrincipalPointX(),
260 getPrincipalPointY(), k1, k2, k3, p1, p2);
261 }
262
263 /**
264 * Build a {@link RemapProcessor} capable of correcting the radial and
265 * tangential distortion of this camera.
266 * <p>
267 * <b>Note:</b> Skew is currently assumed to be zero
268 *
269 * @return the processor for undistorting the image
270 */
271 public RemapProcessor buildUndistortionProcessor() {
272 return buildUndistortionProcessor(width, height);
273 }
274
275 /**
276 * Build a {@link RemapProcessor} capable of correcting the radial and
277 * tangential distortion of this camera.
278 * <p>
279 * <b>Note:</b> Skew is currently assumed to be zero
280 *
281 * @param width
282 * the target width of images produced by the processor
283 * @param height
284 * the target height of images produced by the processor
285 * @return the processor for undistorting the image
286 */
287 public RemapProcessor buildUndistortionProcessor(int width, int height) {
288 final FImage[] map = buildUndistortionMap(width, height);
289 return new RemapProcessor(map[0], map[1]);
290 }
291
292 /**
293 * Build a {@link RemapProcessor} capable of correcting the radial and
294 * tangential distortion of this camera. The distortion map is computed such
295 * that the warped image will appear as if it were viewed through the
296 * undistorted <code>target</code> camera, rather than this one.
297 * <p>
298 * <b>Note:</b> Skew is currently assumed to be zero
299 *
300 * @param width
301 * the target width of images produced by the processor
302 * @param height
303 * the target height of images produced by the processor
304 * @param target
305 * the target camera
306 * @return the processor for undistorting the image
307 */
308 public RemapProcessor buildUndistortionProcessor(int width, int height, CameraIntrinsics target) {
309 final FImage[] map = buildUndistortionMap(width, height, target);
310 return new RemapProcessor(map[0], map[1]);
311 }
312
313 /**
314 * Build a {@link RemapProcessor} capable of correcting the radial and
315 * tangential distortion of this camera. The distortion map is computed such
316 * that the warped image will appear as if it were viewed through the
317 * undistorted <code>target</code> camera rather than this one, and the
318 * 3x3matrix <code>R</code> controls the rectification (i.e. the relative
319 * change in position of the camera in world space).
320 * <p>
321 * <b>Note:</b> Skew is currently assumed to be zero
322 *
323 * @param width
324 * the target width of images produced by the processor
325 * @param height
326 * the target height of images produced by the processor
327 * @param target
328 * the target camera
329 * @param R
330 * the rectification matrix
331 * @return the processor for undistorting the image
332 */
333 public RemapProcessor buildRectifiedUndistortionProcessor(int width, int height, CameraIntrinsics target, Matrix R) {
334 final FImage[] map = buildRectifiedUndistortionMap(width, height, target, R);
335 return new RemapProcessor(map[0], map[1]);
336 }
337
338 /**
339 * Build the distortion map, which for every un-distorted point in the given
340 * size image contains the x and y ordinates of the corresponding distorted
341 * point. The resultant map can be used with a {@link RemapProcessor} to
342 * undistort imaaes.
343 * <p>
344 * <b>Note:</b> Skew is currently assumed to be zero
345 *
346 * @param width
347 * the desired width; typically the same as the image width used
348 * to calibrate the camera
349 * @param height
350 * the desired height; typically the same as the image height
351 * used to calibrate the camera
352 * @return the distortion map
353 */
354 public FImage[] buildUndistortionMap(final int width, final int height) {
355 final FImage xords = new FImage(width, height);
356 final FImage yords = new FImage(width, height);
357
358 final double px = this.getPrincipalPointX();
359 final double py = this.getPrincipalPointY();
360 final double fx = this.getFocalLengthX();
361 final double fy = this.getFocalLengthY();
362
363 for (int v = 0; v < height; v++) {
364 final double y = (v - py) / fy;
365
366 for (int u = 0; u < width; u++) {
367 final double x = (u - px) / fx;
368
369 final double r2 = x * x + y * y;
370 final double r4 = r2 * r2;
371 final double r6 = r2 * r2 * r2;
372
373 // radial component
374 double tx = x * (k1 * r2 + k2 * r4 + k3 * r6);
375 double ty = y * (k1 * r2 + k2 * r4 + k3 * r6);
376
377 // tangential component
378 tx += 2 * p1 * x * y + p2 * (r2 + 2 * x * x);
379 ty += p1 * (r2 + 2 * y * y) + 2 * p2 * x * y;
380
381 xords.pixels[v][u] = (float) ((x + tx) * fx + px);
382 yords.pixels[v][u] = (float) ((y + ty) * fy + py);
383 }
384 }
385
386 return new FImage[] { xords, yords };
387 }
388
389 /**
390 * Build the distortion map, which for every un-distorted point in the given
391 * size image contains the x and y ordinates of the corresponding distorted
392 * point. The resultant map can be used with a {@link RemapProcessor} to
393 * undistort imaaes. The distortion map is computed such that the warped
394 * image will appear as if it were viewed through the undistorted
395 * <code>target</code> camera, rather than this one.
396 * <p>
397 * <b>Note:</b> Skew is currently assumed to be zero
398 *
399 * @param width
400 * the desired width; typically the same as the image width used
401 * to calibrate the camera
402 * @param height
403 * the desired height; typically the same as the image height
404 * used to calibrate the camera
405 * @param target
406 * the target camera intrinsics
407 * @return the distortion map
408 */
409 public FImage[] buildUndistortionMap(final int width, final int height, CameraIntrinsics target) {
410 final FImage xords = new FImage(width, height);
411 final FImage yords = new FImage(width, height);
412
413 final double pxp = target.getPrincipalPointX();
414 final double pyp = target.getPrincipalPointY();
415 final double fxp = target.getFocalLengthX();
416 final double fyp = target.getFocalLengthY();
417
418 final double px = this.getPrincipalPointX();
419 final double py = this.getPrincipalPointY();
420 final double fx = this.getFocalLengthX();
421 final double fy = this.getFocalLengthY();
422
423 for (int v = 0; v < height; v++) {
424 final double y = (v - pyp) / fyp;
425
426 for (int u = 0; u < width; u++) {
427 final double x = (u - pxp) / fxp;
428
429 final double r2 = x * x + y * y;
430 final double r4 = r2 * r2;
431 final double r6 = r2 * r2 * r2;
432
433 // radial component
434 double tx = x * (k1 * r2 + k2 * r4 + k3 * r6);
435 double ty = y * (k1 * r2 + k2 * r4 + k3 * r6);
436
437 // tangential component
438 tx += 2 * p1 * x * y + p2 * (r2 + 2 * x * x);
439 ty += p1 * (r2 + 2 * y * y) + 2 * p2 * x * y;
440
441 xords.pixels[v][u] = (float) ((x + tx) * fx + px);
442 yords.pixels[v][u] = (float) ((y + ty) * fy + py);
443 }
444 }
445
446 return new FImage[] { xords, yords };
447 }
448
449 /**
450 * Build the rectified distortion map, which for every un-distorted point in
451 * the given size image contains the x and y ordinates of the corresponding
452 * rectifed distorted point. The resultant map can be used with a
453 * {@link RemapProcessor} to undistort imaaes. The distortion map is
454 * computed such that the warped image will appear as if it were viewed
455 * through the undistorted <code>target</code> camera rather than this one,
456 * and the 3x3matrix <code>R</code> controls the rectification (i.e. the
457 * relative change in position of the camera in world space).
458 * <p>
459 * <b>Note:</b> Skew is currently assumed to be zero
460 *
461 * @param width
462 * the desired width; typically the same as the image width used
463 * to calibrate the camera
464 * @param height
465 * the desired height; typically the same as the image height
466 * used to calibrate the camera
467 * @param target
468 * the target camera intrinsics
469 * @param R
470 * the rectification matrix
471 * @return the distortion map
472 */
473 public FImage[] buildRectifiedUndistortionMap(final int width, final int height, CameraIntrinsics target, Matrix R) {
474 final FImage xords = new FImage(width, height);
475 final FImage yords = new FImage(width, height);
476
477 final double pxp = target.getPrincipalPointX();
478 final double pyp = target.getPrincipalPointY();
479 final double fxp = target.getFocalLengthX();
480 final double fyp = target.getFocalLengthY();
481
482 final double px = this.getPrincipalPointX();
483 final double py = this.getPrincipalPointY();
484 final double fx = this.getFocalLengthX();
485 final double fy = this.getFocalLengthY();
486
487 final Matrix Rinv = R.inverse();
488 final Matrix tmp = new Matrix(3, 1);
489 tmp.set(2, 0, 1);
490
491 for (int v = 0; v < height; v++) {
492 double y = (v - pyp) / fyp;
493
494 for (int u = 0; u < width; u++) {
495 double x = (u - pxp) / fxp;
496
497 tmp.set(0, 0, x);
498 tmp.set(1, 0, y);
499 final Matrix pro = Rinv.times(tmp);
500 x = pro.get(0, 0) / pro.get(2, 0);
501 y = pro.get(1, 0) / pro.get(2, 0);
502
503 final double r2 = x * x + y * y;
504 final double r4 = r2 * r2;
505 final double r6 = r2 * r2 * r2;
506
507 // radial component
508 double tx = x * (k1 * r2 + k2 * r4 + k3 * r6);
509 double ty = y * (k1 * r2 + k2 * r4 + k3 * r6);
510
511 // tangential component
512 tx += 2 * p1 * x * y + p2 * (r2 + 2 * x * x);
513 ty += p1 * (r2 + 2 * y * y) + 2 * p2 * x * y;
514
515 xords.pixels[v][u] = (float) ((x + tx) * fx + px);
516 yords.pixels[v][u] = (float) ((y + ty) * fy + py);
517 }
518 }
519
520 return new FImage[] { xords, yords };
521 }
522
523 /**
524 * Undistort the given image by removing the radial and tangential
525 * distortions of this camera. It is assumed that the input image has the
526 * same dimensions as images produced by this {@link CameraIntrinsics}.
527 * <p>
528 * This method is inefficient if you need to process many images as the
529 * distortion map is computed each time. For more efficient operation, use
530 * {@link #buildUndistortionProcessor()} to make a reusable
531 * {@link RemapProcessor} capable of efficiently undistorting multiple
532 * images.
533 *
534 * @param image
535 * the image to undistort
536 * @return the undistorted image
537 */
538 public <I extends Image<?, I> & SinglebandImageProcessor.Processable<Float, FImage, I>> I undistort(I image) {
539 final RemapProcessor proc = buildUndistortionProcessor(image.getWidth(), image.getHeight());
540 return image.process(proc);
541 }
542 }