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.math.geometry.transforms;
031
032import java.util.List;
033
034import org.openimaj.math.geometry.point.Point2d;
035import org.openimaj.math.model.EstimatableModel;
036import org.openimaj.math.model.fit.residuals.AbstractResidualCalculator;
037import org.openimaj.math.model.fit.residuals.ResidualCalculator;
038import org.openimaj.util.pair.IndependentPair;
039import org.openimaj.util.pair.Pair;
040
041import Jama.Matrix;
042
043/**
044 * Implementation of a Fundamental matrix model that estimates the epipolar
045 * geometry.
046 *
047 * @author Jonathon Hare (jsh2@ecs.soton.ac.uk)
048 *
049 */
050public class FundamentalModel implements EstimatableModel<Point2d, Point2d> {
051        /**
052         * Computes the algebraic residual of the point pairs applied to the F
053         * matrix
054         *
055         * @author Jonathon Hare (jsh2@ecs.soton.ac.uk)
056         *
057         */
058        public static class Fundamental8PtResidual extends AbstractResidualCalculator<Point2d, Point2d, FundamentalModel> {
059                @Override
060                public double computeResidual(IndependentPair<Point2d, Point2d> data) {
061                        final Matrix F = model.fundamental;
062
063                        final Point2d p1 = data.firstObject();
064                        final Point2d p2 = data.secondObject();
065
066                        final float x1_1 = p1.getX(); // u
067                        final float x1_2 = p1.getY(); // v
068                        final float x2_1 = p2.getX(); // u'
069                        final float x2_2 = p2.getY(); // v'
070
071                        double res = 0;
072                        res += F.get(0, 0) * x2_1 * x1_1; // u' * u
073                        res += F.get(0, 1) * x2_1 * x1_2; // u' * v
074                        res += F.get(0, 2) * x2_1; // u'
075                        res += F.get(1, 0) * x2_2 * x1_1; // v' * u
076                        res += F.get(1, 1) * x2_2 * x1_2; // v' * v
077                        res += F.get(1, 2) * x2_2; // v'
078                        res += F.get(2, 0) * x1_1; // u
079                        res += F.get(2, 1) * x1_2; // v
080                        res += F.get(2, 2); // 1
081
082                        return res * res;
083                }
084        }
085
086        /**
087         * Geometric residual that sums the distance of points from the closest
088         * epipolar line.
089         *
090         * @author Jonathon Hare (jsh2@ecs.soton.ac.uk)
091         */
092        public static class EpipolarResidual extends AbstractResidualCalculator<Point2d, Point2d, FundamentalModel> {
093                @Override
094                public double computeResidual(IndependentPair<Point2d, Point2d> data) {
095                        final Matrix p1Mat = new Matrix(3, 1);
096                        final Matrix p2Mat = new Matrix(3, 1);
097
098                        // x
099                        p1Mat.set(0, 0, data.firstObject().getX());
100                        p1Mat.set(1, 0, data.firstObject().getY());
101                        p1Mat.set(2, 0, 1);
102
103                        // x'
104                        p2Mat.set(0, 0, data.secondObject().getX());
105                        p2Mat.set(1, 0, data.secondObject().getY());
106                        p2Mat.set(2, 0, 1);
107
108                        final Matrix l1 = model.fundamental.times(p1Mat);
109                        final double n1 = Math.sqrt(l1.get(0, 0) * l1.get(0, 0) + l1.get(1, 0) * l1.get(1, 0));
110                        final double d1 = Math.abs((l1.get(0, 0) * p2Mat.get(0, 0) + l1.get(1, 0) * p2Mat.get(1, 0) + l1.get(2, 0)
111                                        * p2Mat.get(2, 0))
112                                        / n1);
113
114                        final Matrix l2 = model.fundamental.transpose().times(p2Mat);
115                        final double n2 = Math.sqrt(l2.get(0, 0) * l2.get(0, 0) + l2.get(1, 0) * l2.get(1, 0));
116                        final double d2 = Math.abs((l2.get(0, 0) * p1Mat.get(0, 0) + l2.get(1, 0) * p1Mat.get(1, 0) + l2.get(2, 0)
117                                        * p1Mat.get(2, 0))
118                                        / n2);
119
120                        return d1 + d2;
121                }
122        }
123
124        /**
125         * {@link ResidualCalculator} based on Sampson's geometric error.
126         *
127         * @author Jonathon Hare (jsh2@ecs.soton.ac.uk)
128         *
129         */
130        public static class SampsonGeometricResidual extends AbstractResidualCalculator<Point2d, Point2d, FundamentalModel> {
131                @Override
132                public double computeResidual(IndependentPair<Point2d, Point2d> data) {
133                        final Matrix p1 = new Matrix(3, 1);
134                        final Matrix p2 = new Matrix(3, 1);
135
136                        // x
137                        p1.set(0, 0, data.firstObject().getX());
138                        p1.set(1, 0, data.firstObject().getY());
139                        p1.set(2, 0, 1);
140
141                        // x'
142                        p2.set(0, 0, data.secondObject().getX());
143                        p2.set(1, 0, data.secondObject().getY());
144                        p2.set(2, 0, 1);
145
146                        final double p2tFp1 = p2.transpose().times(model.fundamental).times(p1).get(0, 0);
147                        final Matrix Fp1 = model.fundamental.times(p1);
148                        final Matrix Ftp2 = model.fundamental.transpose().times(p2);
149
150                        final double dist = (p2tFp1 * p2tFp1)
151                                        / (Fp1.get(0, 0) * Fp1.get(0, 0) + Fp1.get(1, 0) * Fp1.get(1, 0) + Ftp2.get(0, 0) * Ftp2.get(0, 0) + Ftp2
152                                                        .get(1, 0)
153                                                        * Ftp2.get(1, 0));
154
155                        return Math.abs(dist);
156                }
157        }
158
159        protected boolean normalise;
160        protected Matrix fundamental;
161
162        /**
163         * Create a {@link FundamentalModel}, automatically normalising data when
164         * estimating the model
165         */
166        public FundamentalModel()
167        {
168                this(true);
169        }
170
171        /**
172         * Create a {@link FundamentalModel} with optional automatic normalisation.
173         *
174         * @param norm
175         *            true if the data should be automatically normalised before
176         *            running the 8-point algorithm
177         */
178        public FundamentalModel(boolean norm)
179        {
180                this.normalise = norm;
181        }
182
183        @Override
184        public boolean estimate(List<? extends IndependentPair<Point2d, Point2d>> data) {
185                if (normalise) {
186                        final Pair<Matrix> norms = TransformUtilities.getNormalisations(data);
187                        final List<? extends IndependentPair<Point2d, Point2d>> normData = TransformUtilities.normalise(data, norms);
188
189                        final Matrix normFundamental = TransformUtilities.fundamentalMatrix8Pt(normData);
190                        this.fundamental = norms.secondObject().transpose().times(normFundamental).times(norms.firstObject());
191                } else {
192                        this.fundamental = TransformUtilities.fundamentalMatrix8Pt(data);
193                }
194                return true;
195        }
196
197        /**
198         * De-normalise a fundamental estimate. Use when {@link #estimate(List)} was
199         * called with pre-normalised data.
200         *
201         * @param norms
202         *            the normalisation transforms
203         */
204        public void denormaliseFundamental(Pair<Matrix> norms) {
205                this.fundamental = norms.secondObject().transpose().times(fundamental).times(norms.firstObject());
206        }
207
208        @Override
209        public Point2d predict(Point2d data) {
210                throw new UnsupportedOperationException();
211        }
212
213        @Override
214        public int numItemsToEstimate() {
215                return 8;
216        }
217
218        /**
219         * Clone the model
220         *
221         * @return a cloned copy
222         */
223        @Override
224        public FundamentalModel clone() {
225                final FundamentalModel model = new FundamentalModel(this.normalise);
226                if (model.fundamental != null)
227                        model.fundamental = fundamental.copy();
228                return model;
229        }
230
231        /**
232         * Set the Fundamental matrix
233         *
234         * @param optimised
235         */
236        public void setF(Matrix optimised) {
237                this.fundamental = optimised;
238        }
239
240        /**
241         * Get the fundamental matrix
242         *
243         * @return the F matrix
244         */
245        public Matrix getF() {
246                return fundamental;
247        }
248}