package photogrammetry.util;
import java.io.File;
import java.io.IOException;
import Jama.Matrix;
import photogrammetry.util.model.HasCoordinates2d;
import photogrammetry.util.model.Point2d;
/**
* Holds intrinsic camera parameters and performs undistortion.
*
* @author johannes
*/
public class Camera {
private final Matrix intrinsics;
private final Matrix distCoeffs;
private final Matrix invIntrinsics;
private final double cx, cy, fx, fy;
private final double k1, k2, p1, p2;
/**
* Create a new camera.
*
* @param intr
* the file where the camera's intrinsic matrix is stored
* @param dist
* the file where the camera's distortion coefficients are stored. If null, (0,0,0,0)
* will be assumed.
* @throws IOException
* if one of the files couldn't be read
*/
public Camera(File intr, File dist) throws IOException {
intrinsics = new Matrix(MatrixUtils.loadMatrix(intr), 3).transpose();
distCoeffs = new Matrix(dist == null ? new double[] { 0, 0, 0, 0 }
: MatrixUtils.loadMatrix(dist), 1);
invIntrinsics = intrinsics.inverse();
cx = intrinsics.get(0, 2);
cy = intrinsics.get(1, 2);
fx = intrinsics.get(0, 0);
fy = intrinsics.get(1, 1);
k1 = distCoeffs.get(0, 0);
k2 = distCoeffs.get(0, 1);
p1 = distCoeffs.get(0, 2);
p2 = distCoeffs.get(0, 3);
}
/**
* Create a new camera with a given camera matrix and no distortion.
*
* @param intr
* the camera matrix
*/
public Camera(Matrix intr) {
intrinsics = intr;
distCoeffs = new Matrix(new double[] { 0, 0, 0, 0 }, 1);
invIntrinsics = intr.inverse();
cx = intrinsics.get(0, 2);
cy = intrinsics.get(1, 2);
fx = intrinsics.get(0, 0);
fy = intrinsics.get(1, 1);
k1 = k2 = p1 = p2 = 0;
}
/**
* Performs undistortion on a point.
*
* @param p
* the point in image coordinates (observed point coordinates)
* @return the ideal point coordinates
*/
public synchronized Point2d undistort(HasCoordinates2d p) {
double x = (p.getX() - cx) / fx;
double y = (p.getY() - cy) / fy;
final double r2 = x * x + y * y;
final double r4 = r2 * r2;
final double a = 1 + k1 * r2 + k2 * r4;
double xp = x * a + 2 * p1 * x * y + p2 * (r2 + 2 * x * x);
double yp = y * a + 2 * p2 * x * y + p1 * (r2 + 2 * y * y);
return new Point2d(fx * xp + cx, fy * yp + cy);
}
/**
* Return the inverse of the intrinsic matrix.
*
* @return the inverse of the camera matrix
*/
public Matrix getInvIntrinsics() {
return invIntrinsics;
}
/**
* Return the intrinsic matrix.
*
* @return the camera matrix
*/
public Matrix getIntrinsics() {
return intrinsics;
}
/**
* Check whether a point is behind the camera. See
* http://www.comp.nus.edu.sg/~cs4243/lecture/camera.pdf, slide 17.
*
* @param pt
* the point to check
* @param t
* the position of the world frame's origin in the camera frame
* @param r
* the rotation matrix describing the world frame in the camera's frame
* @return true iff the point lies behind the camera.
*/
public boolean isPointBehindCamera(Matrix pt, Matrix t, Matrix r) {
return r.times(pt).get(2, 0) + t.get(2, 0) <= 0;
}
}