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.image.camera; 031 032import org.openimaj.image.FImage; 033import org.openimaj.image.Image; 034import org.openimaj.image.processing.transform.RemapProcessor; 035import org.openimaj.image.processor.SinglebandImageProcessor; 036import org.openimaj.math.geometry.point.Point2d; 037import org.openimaj.math.geometry.transforms.TransformUtilities; 038 039import Jama.Matrix; 040 041/** 042 * The intrinsic parameters of a camera (focal length in x and y; skew; 043 * principal point in x and y; 3-term radial distorion; 2-term tangential 044 * distortion). 045 * 046 * @author Jonathon Hare (jsh2@ecs.soton.ac.uk) 047 */ 048public class CameraIntrinsics { 049 /** 050 * The camera calibration matrix 051 */ 052 public Matrix calibrationMatrix; 053 054 /** 055 * First radial distortion term 056 */ 057 public double k1; 058 059 /** 060 * Second radial distortion term 061 */ 062 public double k2; 063 064 /** 065 * Third radial distortion term 066 */ 067 public double k3; 068 069 /** 070 * First tangential distortion term 071 */ 072 public double p1; 073 074 /** 075 * Second tangential distortion term 076 */ 077 public double p2; 078 079 /** 080 * The image width of the camera in pixels 081 */ 082 public int width; 083 084 /** 085 * The image height of the camera in pixels 086 */ 087 public int height; 088 089 /** 090 * Construct with the given matrix. 091 * 092 * @param m 093 * the matrix 094 * @param width 095 * the image width of the camera in pixels 096 * @param height 097 * the image height of the camera in pixels 098 */ 099 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}