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}