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.demos.sandbox.c3d;
031
032import java.awt.event.KeyEvent;
033import java.awt.event.KeyListener;
034import java.util.ArrayList;
035import java.util.List;
036
037import javax.swing.SwingUtilities;
038
039import org.openimaj.image.FImage;
040import org.openimaj.image.MBFImage;
041import org.openimaj.image.camera.CameraIntrinsics;
042import org.openimaj.image.colour.Transforms;
043import org.openimaj.math.geometry.point.Point2d;
044import org.openimaj.math.geometry.transforms.FundamentalRefinement;
045import org.openimaj.math.geometry.transforms.estimation.RobustFundamentalEstimator;
046import org.openimaj.util.pair.Pair;
047import org.openimaj.video.VideoDisplay;
048import org.openimaj.video.VideoDisplayListener;
049import org.openimaj.video.capture.VideoCapture;
050import org.openimaj.video.tracking.klt.Feature;
051import org.openimaj.video.tracking.klt.FeatureList;
052import org.openimaj.video.tracking.klt.KLTTracker;
053import org.openimaj.video.tracking.klt.TrackingContext;
054
055import Jama.Matrix;
056
057public class Main implements VideoDisplayListener<MBFImage>, KeyListener {
058        private VideoCapture capture;
059        private VideoDisplay<MBFImage> videoFrame;
060        private KLTTracker tracker;
061        private FeatureList fl1;
062        private FeatureList fl2;
063        private FImage oldFrame;
064
065        boolean firstFrame = true;
066        private int nFeatures = 150;
067        private CameraIntrinsics cam;
068
069        public Main() throws Exception {
070                capture = new VideoCapture(640, 480);
071                videoFrame = VideoDisplay.createVideoDisplay(capture);
072                videoFrame.addVideoListener(this);
073                SwingUtilities.getRoot(videoFrame.getScreen()).addKeyListener(this);
074                // videoFrame.getScreen().setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
075
076                final TrackingContext tc = new TrackingContext();
077                fl1 = new FeatureList(nFeatures);
078                tracker = new KLTTracker(tc, fl1);
079
080                tc.setSequentialMode(true);
081                tc.setWriteInternalImages(false);
082                tc.setAffineConsistencyCheck(-1);
083
084                final Matrix m = new Matrix(3, 3);
085                this.cam = new CameraIntrinsics(m, 640, 480);
086                cam.setFocalLengthX(640);
087                cam.setFocalLengthX(480);
088                cam.setPrincipalPointX(640 / 2);
089                cam.setPrincipalPointY(320 / 2);
090        }
091
092        public boolean needsReset() {
093                return this.firstFrame;
094        }
095
096        @Override
097        public void afterUpdate(VideoDisplay<MBFImage> arg0) {
098                // TODO Auto-generated method stub
099
100        }
101
102        @Override
103        public void beforeUpdate(MBFImage image) {
104                final FImage greyFrame = Transforms.calculateIntensityNTSC(image);
105                if (needsReset()) {
106                        tracker.selectGoodFeatures(greyFrame);
107                }
108                else {
109                        fl2 = fl1.clone();
110                        tracker.trackFeatures(oldFrame, greyFrame);
111                        tracker.replaceLostFeatures(greyFrame);
112
113                        final List<Pair<Point2d>> corres = new ArrayList<Pair<Point2d>>();
114                        for (int i = 0; i < fl1.features.length; i++) {
115                                final Feature p = fl2.features[i];
116                                final Feature c = fl1.features[i];
117
118                                if (c.val == 0) {
119                                        corres.add(new Pair<Point2d>(p, c));
120                                }
121                        }
122
123                        try {
124                                final RobustFundamentalEstimator rfe = new RobustFundamentalEstimator(0.35, FundamentalRefinement.SAMPSON);
125                                rfe.fitData(corres);
126
127                                rfe.getModel().getF().print(5, 5);
128                        } catch (final Exception e) {
129
130                        }
131                }
132                fl1.drawFeatures(image);
133
134                this.oldFrame = greyFrame;
135                this.firstFrame = false;
136        }
137
138        static Matrix computeEssentialMatrix(CameraIntrinsics ci, Matrix F) {
139                return ci.calibrationMatrix.transpose().times(F).times(ci.calibrationMatrix);
140        }
141
142        @Override
143        public void keyTyped(KeyEvent e) {
144
145        }
146
147        @Override
148        public void keyPressed(KeyEvent e) {
149                if (e.getKeyChar() == 'r') {
150                        this.firstFrame = true;
151                }
152        }
153
154        @Override
155        public void keyReleased(KeyEvent e) {
156                // TODO Auto-generated method stub
157
158        }
159
160        public static void main(String args[]) throws Exception {
161                new Main();
162        }
163}