Camera.java

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;
	}
	
}