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.math.geometry.point.Point2d;
033import org.openimaj.math.geometry.point.Point2dImpl;
034import org.openimaj.math.geometry.point.Point3d;
035import org.openimaj.math.matrix.MatrixUtils;
036
037import Jama.Matrix;
038
039/**
040 * A model of the extrinsic parameters of a pinhole (projective) camera
041 * (translation in 3d space, and 3d rotation matrix), coupled with the camera's
042 * intrinsic parameters.
043 * 
044 * @author Jonathon Hare (jsh2@ecs.soton.ac.uk)
045 */
046public class Camera {
047        /**
048         * The intrinsic parameters of this camera
049         */
050        public CameraIntrinsics intrinsicParameters;
051
052        /**
053         * The rotation of this camera in world coordinates
054         */
055        public Matrix rotation;
056
057        /**
058         * The position of this camera in world coordinates
059         */
060        public Point3d translation;
061
062        /**
063         * Compute the homography of this camera to the z=0 plane based on it's
064         * parameters: H = KA, where A = [r1 r2 t] and r1 and r2 are the first and
065         * second columns of the rotation matrix, and K is the calibration matrix.
066         * 
067         * @return the camera's homography
068         */
069        public Matrix computeHomography() {
070                final Matrix A = rotation.copy();
071                A.set(0, 2, translation.getX());
072                A.set(1, 2, translation.getY());
073                A.set(2, 2, translation.getZ());
074
075                final Matrix H = intrinsicParameters.calibrationMatrix.times(A);
076                MatrixUtils.times(H, 1.0 / H.get(2, 2));
077
078                return H;
079        }
080
081        /**
082         * Project a 2d point (technically a 3d point on the z=0 world plane)
083         * 
084         * @param pt
085         *            the point to project in world coordinates
086         * @return the image coordinates
087         */
088        public Point2d project(Point2d pt) {
089                final Matrix H = this.computeHomography();
090
091                final Point2d p = pt.transform(H);
092
093                return intrinsicParameters.applyDistortion(p);
094        }
095
096        /**
097         * Project a 3d point onto the image plane
098         * 
099         * @param pt
100         *            the point to project in world coordinates
101         * @return the image coordinates
102         */
103        public Point2d project(Point3d pt) {
104                final Matrix ptm = new Matrix(new double[][] { { pt.getX() }, { pt.getY() }, { pt.getZ() }, { 1 } });
105                final double[][] rv = rotation.getArray();
106                final Matrix Rt = new Matrix(new double[][] {
107                                { rv[0][0], rv[0][1], rv[0][2], translation.getX() },
108                                { rv[1][0], rv[1][1], rv[1][2], translation.getY() },
109                                { rv[2][0], rv[2][1], rv[2][2], translation.getZ() },
110                });
111                final Matrix ARt = intrinsicParameters.calibrationMatrix.times(Rt);
112
113                final Matrix pr = ARt.times(ptm);
114
115                final Point2dImpl p = new Point2dImpl(pr.get(0, 0) / pr.get(2, 0), pr.get(1, 0) / pr.get(2, 0));
116
117                return intrinsicParameters.applyDistortion(p);
118        }
119}