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; 031 032import gnu.trove.list.array.TIntArrayList; 033 034import java.io.FileWriter; 035import java.io.IOException; 036import java.io.PrintWriter; 037import java.util.ArrayList; 038import java.util.List; 039 040import org.apache.commons.math.geometry.Vector3D; 041import org.openimaj.image.FImage; 042import org.openimaj.math.geometry.shape.Triangle; 043import org.openimaj.math.geometry.triangulation.DelaunayTriangulator; 044import org.openimaj.math.matrix.PseudoInverse; 045import org.openimaj.math.matrix.ThinSingularValueDecomposition; 046import org.openimaj.video.FImageFileBackedVideo; 047import org.openimaj.video.Video; 048import org.openimaj.video.tracking.klt.Feature; 049import org.openimaj.video.tracking.klt.FeatureList; 050import org.openimaj.video.tracking.klt.FeatureTable; 051import org.openimaj.video.tracking.klt.KLTTracker; 052import org.openimaj.video.tracking.klt.TrackingContext; 053 054import Jama.EigenvalueDecomposition; 055import Jama.Matrix; 056 057public class SFMOrtho { 058 public Matrix R; 059 public Matrix S; 060 public FImage texture; 061 public int[][] triangleDefs; 062 private List<Feature> pts; 063 064 public SFMOrtho(Video<FImage> video, int nFeatures) { 065 texture = video.getCurrentFrame().clone(); 066 067 FeatureTable features = trackFeatures(video, nFeatures, false); 068 features = filterNonTracked(features); 069 070 pts = features.features.get(0); 071 final List<Triangle> tris = DelaunayTriangulator.triangulate(pts); 072 triangleDefs = new int[tris.size()][3]; 073 074 for (int i = 0; i < tris.size(); i++) { 075 final Triangle t = tris.get(i); 076 077 for (int j = 0; j < 3; j++) { 078 triangleDefs[i][j] = pts.indexOf(t.vertices[j]); 079 } 080 } 081 082 final Matrix w = buildMeasurementMatrix(features); 083 factorise(w); 084 applyMetricConstraint(); 085 alignWithFrame(0); 086 } 087 088 public List<Vector3D> getVertices() { 089 final List<Vector3D> vertices = new ArrayList<Vector3D>(S.getColumnDimension()); 090 091 for (int i = 0; i < S.getColumnDimension(); i++) { 092 vertices.add(new Vector3D(S.get(0, i), S.get(1, i), S.get(2, i))); 093 } 094 095 return vertices; 096 } 097 098 public String toObjString() { 099 final StringBuffer sb = new StringBuffer(); 100 101 final List<Vector3D> vertices = getVertices(); 102 for (final Vector3D v : vertices) { 103 sb.append("v " + v.getX() + " " + v.getY() + " " + v.getZ() + "\n"); 104 } 105 106 for (final int[] td : triangleDefs) { 107 sb.append("f " + (td[0] + 1) + " " + (td[1] + 1) + " " + (td[2] + 1) + "\n"); 108 } 109 110 return sb.toString(); 111 } 112 113 private void applyMetricConstraint() { 114 final Matrix Q = calculateOrthometricConstraint(R); 115 116 R = R.times(Q); 117 S = Q.inverse().times(S); 118 } 119 120 private void alignWithFrame(int frame) { 121 Vector3D i1 = new Vector3D(R.get(frame, 0), R.get(frame, 1), R.get(frame, 2)); 122 i1 = i1.scalarMultiply(1 / i1.getNorm()); 123 124 final int f = R.getRowDimension() / 2; 125 Vector3D j1 = new Vector3D(R.get(frame + f, 0), R.get(frame + f, 1), R.get(frame + f, 2)); 126 j1 = j1.scalarMultiply(1 / j1.getNorm()); 127 128 final Vector3D k1 = Vector3D.crossProduct(i1, j1); 129 k1.scalarMultiply(1 / k1.getNorm()); 130 131 final Matrix R0 = new Matrix(new double[][] { 132 { i1.getX(), j1.getX(), k1.getX() }, 133 { i1.getY(), j1.getY(), k1.getY() }, 134 { i1.getZ(), j1.getZ(), k1.getZ() }, 135 }); 136 137 R = R.times(R0); 138 S = R0.inverse().times(S); 139 } 140 141 private void factorise(Matrix w) { 142 final ThinSingularValueDecomposition svd = new ThinSingularValueDecomposition(w, 3); 143 144 final Matrix s_sqrt = svd.getSmatrixSqrt(); 145 this.R = svd.U.times(s_sqrt); 146 this.S = s_sqrt.times(svd.Vt); 147 } 148 149 FeatureTable trackFeatures(Video<FImage> video, int nFeatures, boolean replace) { 150 final TrackingContext tc = new TrackingContext(); 151 final FeatureList fl = new FeatureList(nFeatures); 152 final FeatureTable ft = new FeatureTable(nFeatures); 153 final KLTTracker tracker = new KLTTracker(tc, fl); 154 155 tc.setSequentialMode(true); 156 tc.setWriteInternalImages(false); 157 tc.setAffineConsistencyCheck(-1); 158 159 FImage prev = video.getCurrentFrame(); 160 tracker.selectGoodFeatures(prev); 161 ft.storeFeatureList(fl, 0); 162 163 while (video.hasNextFrame()) { 164 final FImage next = video.getNextFrame(); 165 tracker.trackFeatures(prev, next); 166 167 if (replace) 168 tracker.replaceLostFeatures(next); 169 170 prev = next; 171 172 ft.storeFeatureList(fl, video.getCurrentFrameIndex()); 173 } 174 175 return ft; 176 } 177 178 FeatureTable filterNonTracked(FeatureTable ft) { 179 final int nFrames = ft.features.size(); 180 final TIntArrayList tracksToRemove = new TIntArrayList(); 181 182 for (int i = 0; i < ft.nFeatures; i++) { 183 int sum = 0; 184 185 for (int f = 1; f < nFrames; f++) { 186 sum += ft.features.get(f).get(i).val; 187 } 188 189 if (sum != 0) { 190 tracksToRemove.add(i); 191 } 192 } 193 194 final FeatureTable filtered = new FeatureTable(ft.nFeatures - tracksToRemove.size()); 195 for (int f = 0; f < nFrames; f++) { 196 final FeatureList fl = new FeatureList(filtered.nFeatures); 197 198 for (int i = 0, j = 0; i < ft.nFeatures; i++) { 199 if (!tracksToRemove.contains(i)) 200 fl.features[j++] = ft.features.get(f).get(i); 201 } 202 filtered.storeFeatureList(fl, f); 203 } 204 205 return filtered; 206 } 207 208 Matrix buildMeasurementMatrix(FeatureTable ft) { 209 final int p = ft.nFeatures; // number of features tracked 210 final int f = ft.features.size(); // number of frames 211 212 final Matrix W = new Matrix(2 * f, p); 213 final double[][] Wdata = W.getArray(); 214 215 for (int r = 0; r < f; r++) { 216 for (int c = 0; c < p; c++) { 217 Wdata[r][c] = ft.features.get(r).get(c).x; 218 Wdata[r + f][c] = ft.features.get(r).get(c).y; 219 } 220 } 221 222 PrintWriter pw; 223 try { 224 pw = new PrintWriter(new FileWriter("/Users/jsh2/Desktop/measurement.txt")); 225 W.print(pw, 5, 5); 226 pw.close(); 227 } catch (final IOException e) { 228 } 229 230 for (int r = 0; r < 2 * f; r++) { 231 double mean = 0; 232 233 for (int c = 0; c < p; c++) { 234 mean += Wdata[r][c]; 235 } 236 237 mean /= p; 238 239 for (int c = 0; c < p; c++) { 240 Wdata[r][c] -= mean; 241 } 242 } 243 244 return W; 245 } 246 247 double[] gT(double[] a, double[] b) { 248 return new double[] { 249 a[0] * b[0], 250 a[0] * b[1] + a[1] * b[0], 251 a[0] * b[2] + a[2] * b[0], 252 a[1] * b[1], 253 a[1] * b[2] + a[2] * b[1], 254 a[2] * b[2] }; 255 } 256 257 Matrix calculateOrthometricConstraint(Matrix R) { 258 final int f = R.getRowDimension() / 2; 259 260 final double[][] ihT = R.getMatrix(0, f - 1, 0, 2).getArray(); 261 final double[][] jhT = R.getMatrix(f, (2 * f) - 1, 0, 2).getArray(); 262 263 final Matrix G = new Matrix(3 * f, 6); 264 final Matrix c = new Matrix(3 * f, 1); 265 for (int i = 0; i < f; i++) { 266 G.getArray()[i] = gT(ihT[i], ihT[i]); 267 G.getArray()[i + f] = gT(jhT[i], jhT[i]); 268 G.getArray()[i + 2 * f] = gT(ihT[i], jhT[i]); 269 270 c.set(i, 0, 1); 271 c.set(i + f, 0, 1); 272 } 273 274 final Matrix I = PseudoInverse.pseudoInverse(G).times(c); 275 276 final Matrix L = new Matrix(new double[][] { 277 { I.get(0, 0), I.get(1, 0), I.get(2, 0) }, 278 { I.get(1, 0), I.get(3, 0), I.get(4, 0) }, 279 { I.get(2, 0), I.get(4, 0), I.get(5, 0) } 280 }); 281 282 // enforcing positive definiteness 283 // see 284 // http://www-cse.ucsd.edu/classes/sp04/cse252b/notes/lec16/lec16.pdf 285 final Matrix Lsym = L.plus(L.transpose()).times(0.5); 286 287 final EigenvalueDecomposition eigs = Lsym.eig(); 288 289 final Matrix Dsqrt = eigs.getD(); 290 final double[][] Darr = Dsqrt.getArray(); 291 for (int r = 0; r < Darr.length; r++) { 292 if (Darr[r][r] < 0) 293 Darr[r][r] = 0.0000001; 294 295 Darr[r][r] = Math.sqrt(Darr[r][r]); 296 } 297 298 return eigs.getV().times(Dsqrt); 299 } 300 301 public static void main(String[] args) throws IOException { 302 final FImageFileBackedVideo video = new FImageFileBackedVideo( 303 "/Users/jsh2/Downloads/assignment4_part2_data/frame%08d.jpg", 1, 102); 304 305 final SFMOrtho o = new SFMOrtho(video, 200); 306 307 // double[][] data = o.S.getArray(); 308 // System.out.print("ListPointPlot3D[{"); 309 // for (int i=0; i<data[0].length; i++) { 310 // System.out.print("{"+data[0][i]+","+data[1][i]+","+data[2][i]+"}"); 311 // if (i<data[0].length-1) System.out.print(","); 312 // } 313 // System.out.println("}, BoxRatios->{1,1,1}]"); 314 315 System.out.println(o.toObjString()); 316 } 317}