001package org.openimaj.demos.sandbox.tldcpp.tracker; 002 003import org.openimaj.image.FImage; 004import org.openimaj.image.analysis.algorithm.TemplateMatcher; 005import org.openimaj.math.geometry.line.Line2d; 006import org.openimaj.math.geometry.shape.Rectangle; 007import org.openimaj.video.tracking.klt.Feature; 008import org.openimaj.video.tracking.klt.FeatureList; 009import org.openimaj.video.tracking.klt.KLTTracker; 010import org.openimaj.video.tracking.klt.PyramidSet; 011import org.openimaj.video.tracking.klt.TrackingContext; 012 013/** 014 * The MedianFlowTracker is backed by a {@link KLTTracker} which has some special 015 * checks to make sure tracked points are actually good, and once it knows an overall 016 * median of motion is calculated and reflected in the update of a bounding box. 017 * 018 * The ForwardBackward procedure checks whether a given point is tracked well. 019 * Points in a uniform grid are tracked within a bounding box from frame A -> B 020 * The points that are tracked correctly are then tracked from B -> A. 021 * 022 * The Normalised Cross correlation is measured between points in A and B which survive this A -> B -> A transfer 023 * The euclidian distance is measured between those points which started at A and the same points as tracked to A via B. 024 * 025 * The median cross correlation and euclidian distance is used as a threshold to select points which were tracked well 026 * between A and B. 027 * 028 * The relative motion of these points from A to B is used to calcualte a movement and scale shift of the bounding box in A. 029 * 030 * @author Sina Samangooei (ss@ecs.soton.ac.uk) 031 * 032 */ 033public class MedianFlowTracker { 034 /** 035 * The current tracker bounding box. 036 */ 037 public Rectangle trackerBB; 038 private TrackingContext context; 039 private KLTTracker klttracker; 040 /** 041 * Features tracked to B from A 042 */ 043 public FeatureList featuresTrackedToBfromA; 044 /** 045 * Features tracked back to A form B 046 */ 047 public FeatureList featuresTrackedToAviaB; 048 049 /** 050 * null bounding box and init the {@link KLTTracker} 051 */ 052 public MedianFlowTracker() { 053 trackerBB = null; 054 this.context = new TrackingContext(); 055 klttracker = new KLTTracker(context, null); 056 this.context.setSequentialMode(true); 057 } 058 059 /** 060 * null the bounding box 061 */ 062 public void cleanPreviousData() { 063 trackerBB = null; 064 } 065 066 /** 067 * track points from the previous image within the bounding box to the current image (from A to B) 068 * @param prevMat - Image A 069 * @param currMat - Image B 070 * @param prevBB - Bounding box in A 071 */ 072 public void track(FImage prevMat, FImage currMat, Rectangle prevBB) { 073 if (prevBB != null) { 074 if (prevBB.width <= 0 || prevBB.height <= 0) { 075 return; 076 } 077 float bb_tracker[] = { prevBB.x, prevBB.y, prevBB.width + prevBB.x - 1, prevBB.height + prevBB.y - 1 }; 078 079 FImage prevImg = prevMat; 080 FImage currImg = currMat; 081 082 boolean success = fbtrack(prevImg, currImg, bb_tracker, bb_tracker); 083 084 // Extract subimage 085 float x, y, w, h; 086 x = (float) Math.floor(bb_tracker[0] + 0.5); 087 y = (float) Math.floor(bb_tracker[1] + 0.5); 088 w = (float) Math.floor(bb_tracker[2] - bb_tracker[0] + 1 + 0.5); 089 h = (float) Math.floor(bb_tracker[3] - bb_tracker[1] + 1 + 0.5); 090 091 // TODO: Introduce a check for a minimum size 092 if (!success 093 || x < 0 || y < 0 || w <= 0 || h <= 0 094 || x + w > currMat.width || y + h > currMat.height 095 || x != x || y != y || w != w || h != h) { 096 } else { 097 098 trackerBB = new Rectangle(x, y, w, h); 099 } 100 } 101 } 102 103 private boolean fbtrack(FImage imgI, FImage imgJ, float[] bb, float[] bbnew) { 104 int numM = 10; 105 int numN = 10; 106 int nPoints = numM * numN; 107 int sizePointsArray = nPoints; 108 109 boolean[] status = new boolean[nPoints]; 110 111 // float [] pt = new float[sizePointsArray]; 112 // float [] ptTracked = new float[sizePointsArray]; 113 114 FeatureList pt = new FeatureList(sizePointsArray); 115 116 int i; 117 int nRealPoints; 118 float medFb; 119 float medNcc; 120 int nAfterFbUsage; 121 getFilledBBPoints(bb, numM, numN, 5, pt); 122 // getFilledBBPoints(bb, numM, numN, 5, &ptTracked); 123 124 // initImgs(); 125 FeatureList ptTracked = trackLK(imgI, imgJ, pt, status); // the points 126 // tracked 127 // to the 128 // next 129 // image, 130 // with the 131 // ncc 132 // (from start -> next) and 133 // fb 134 // (from next -> start) 135 // initImgs(); 136 // char* status = *statusP; 137 int nlkPoints = 0; 138 for (i = 0; i < nPoints; i++) { 139 nlkPoints += status[i] ? 1 : 0; 140 } 141 // startPoints = (CvPoint2D32f*) malloc(nlkPoints * 142 // sizeof(CvPoint2D32f)); 143 // targetPoints = (CvPoint2D32f*) malloc(nlkPoints * 144 // sizeof(CvPoint2D32f)); 145 // fbLkCleaned = (float*) malloc(nlkPoints * sizeof(float)); 146 // nccLkCleaned = (float*) malloc(nlkPoints * sizeof(float)); 147 FeatureList cleanedTracked = new FeatureList(nlkPoints); 148 FeatureList cleanedStart = new FeatureList(nlkPoints); 149 nRealPoints = 0; 150 float[] fbLkCleaned = new float[nlkPoints]; 151 float[] nccLkCleaned = new float[nlkPoints]; 152 for (i = 0; i < nPoints; i++) { 153 // TODO:handle Missing Points 154 // or status[i]==0 155 if (ptTracked.features[i] == null || ptTracked.features[i].val < 0 || !status[i]) { 156 } else { 157 cleanedStart.features[nRealPoints] = pt.features[i].clone(); 158 cleanedTracked.features[nRealPoints] = ptTracked.features[i].clone(); 159 fbLkCleaned[nRealPoints] = ((FBNCCFeature) cleanedTracked.features[nRealPoints]).fbDistance; 160 nccLkCleaned[nRealPoints] = ((FBNCCFeature) cleanedTracked.features[nRealPoints]).ncc; 161 nRealPoints++; 162 } 163 } 164 if(nlkPoints == 0) return false; 165 // assert nRealPoints==nlkPoints 166 medFb = FastMedian.getMedian(fbLkCleaned, nlkPoints); 167 medNcc = FastMedian.getMedian(nccLkCleaned, nlkPoints); 168 /* 169 * printf("medianfb: %f\nmedianncc: %f\n", medFb, medNcc); 170 * printf("Number of points after lk: %d\n", nlkPoints); 171 */ 172 nAfterFbUsage = 0; 173 for (i = 0; i < nlkPoints; i++) { 174 if ((fbLkCleaned[i] <= medFb) & (nccLkCleaned[i] >= medNcc)) { 175 cleanedStart.features[nAfterFbUsage] = cleanedStart.features[i]; 176 cleanedTracked.features[nAfterFbUsage] = cleanedTracked.features[i]; 177 nAfterFbUsage++; 178 } 179 } 180 /* printf("Number of points after fb correction: %d\n", nAfterFbUsage); */ 181 // showIplImage(IMGS[1]); 182 // show "OpticalFlow" fb filtered. 183 // drawLinesCvPoint2D32f(imgI, startPoints, nRealPoints, targetPoints, 184 // nRealPoints); 185 // showIplImage(imgI); 186 predictbb(bb, cleanedStart, cleanedTracked, nAfterFbUsage, bbnew); 187 this.featuresTrackedToBfromA = new FeatureList(nAfterFbUsage); 188 System.arraycopy(cleanedStart.features, 0, this.featuresTrackedToBfromA.features, 0, nAfterFbUsage); 189 this.featuresTrackedToAviaB = new FeatureList(nAfterFbUsage); 190 System.arraycopy(cleanedTracked.features, 0, this.featuresTrackedToAviaB.features, 0, nAfterFbUsage); 191 /* 192 * printf("bbnew: %f,%f,%f,%f\n", bbnew[0], bbnew[1], bbnew[2], 193 * bbnew[3]); printf("relative scale: %f \n", scaleshift[0]); 194 */ 195 196 if (medFb > 10) 197 return false; 198 else 199 return true; 200 201 } 202 203 private boolean predictbb(float[] bb0, FeatureList pt0, FeatureList pt1, int nPts, float []bb1) { 204 float[] ofx = new float[nPts]; 205 float[] ofy = new float[nPts]; 206 int i; 207 int j; 208 int d = 0; 209 float dx,dy; 210 int lenPdist; 211 float[] dist0; 212 float[] dist1; 213 float s0,s1; 214 for (i = 0; i < nPts; i++) 215 { 216 ofx[i] = pt1.features[i].x - pt0.features[i].x; 217 ofy[i] = pt1.features[i].y - pt0.features[i].y; 218 } 219 dx = FastMedian.getMedianUnmanaged(ofx, nPts); 220 dy = FastMedian.getMedianUnmanaged(ofy, nPts); 221 ofx = null; 222 ofy = null; 223 //m(m-1)/2 224 lenPdist = nPts * (nPts - 1) / 2; 225 dist0 = new float[lenPdist]; 226 dist1 = new float[lenPdist]; 227 for (i = 0; i < nPts; i++) 228 { 229 for (j = i + 1; j < nPts; j++, d++) 230 { 231 dist0[d] = (float) Math.sqrt( 232 Math.pow(pt0.features[i].x - pt0.features[j].x, 2) + 233 Math.pow(pt0.features[i].y - pt0.features[j].y, 2) 234 ); 235 dist1[d] = (float) Math.sqrt(Math.pow(pt1.features[i].x - pt1.features[j].x, 2) + Math.pow(pt1.features[i].y - pt1.features[j].y, 2)); 236 dist0[d] = dist1[d] / dist0[d]; 237 } 238 } 239 // The scale change is the median of all changes of distance. 240 // same as s = median(d2./d1) with above 241 float shift = 0f; 242 if(lenPdist == 0){ 243 244 } 245 else{ 246 shift = FastMedian.getMedianUnmanaged(dist0, lenPdist); 247 } 248// float shift = 1; 249 dist0 = null; 250 dist1 = null; 251 s0 = 0.5f * (shift - 1) * getBbWidth(bb0); 252 s1 = 0.5f * (shift - 1) * getBbHeight(bb0); 253 254 // Apply transformations (translation& scale) to old Bounding Box 255 bb1[0] = bb0[0] - s0 + dx; 256 bb1[1] = bb0[1] - s1 + dy; 257 bb1[2] = bb0[2] + s0 + dx; 258 bb1[3] = bb0[3] + s1 + dy; 259 260 //return absolute scale change 261 // shift[0] = s0; 262 // shift[1] = s1; 263 return true; 264 } 265 266 private float getBbHeight(float[] bb) { 267 return Math.abs(bb[3] - bb[1] + 1); 268 } 269 270 private float getBbWidth(float[] bb) { 271 return Math.abs(bb[2] - bb[0] + 1); 272 } 273 274 private FeatureList trackLK(FImage imgI, FImage imgJ, FeatureList pt,boolean[] status) { 275 276 // TODO: watch NaN cases 277 // double nan = std::numeric_limits<double>::quiet_NaN(); 278 // double inf = std::numeric_limits<double>::infinity(); 279 280 // tracking 281 int winsize_ncc; 282 int i; 283 winsize_ncc = 10; 284 // Get the current pyramid (use it for efficient tracking back) 285 PyramidSet pyrI = this.context.getPreviousPyramid(); 286 if(pyrI.isNull()){ 287 pyrI = new PyramidSet(imgI, context); 288 this.context.setPreviousPyramid(pyrI); 289 } 290 291 // Set the starting points (the grid) 292 this.klttracker.setFeatureList(pt); 293 // Track the grid to the next image 294 this.klttracker.trackFeatures(imgI, imgJ); 295 // Store the tracked points (these will be the output also) 296 FeatureList ptTracked = this.klttracker.getFeatureList().clone(); 297 // Hold on to the pyramid, we must set this at the end 298 PyramidSet pyrJ = this.context.getPreviousPyramid(); 299 // Track these points back to the first image 300 this.klttracker.trackFeatures(imgJ, imgI, pyrJ, pyrI); 301 // Hold the tracked back points 302 FeatureList trackedBack = this.klttracker.getFeatureList(); 303 // fix the KLTTracker context for the next round 304 this.context.setPreviousPyramid(pyrJ); 305 // Figure out which points failed 306 for (i = 0; i < ptTracked.features.length; i++) { 307 if (trackedBack.features[i].val >= 0 308 && ptTracked.features[i].val >= 0) { 309 status[i] = true; 310 } else { 311 status[i] = false; 312 } 313 } 314 // set the ncc in the ptTracked points 315 normCrossCorrelation(imgI, imgJ, pt, ptTracked, status, winsize_ncc); 316 // set the fbDistance in the ptTracked points 317 euclideanDistance(pt, trackedBack, ptTracked, status); // compare the 318 // first two and 319 // store the 320 // results in 321 // the last one 322 323 return ptTracked; 324 } 325 326 private void euclideanDistance(FeatureList pt, FeatureList trackedBack, FeatureList ptTracked, boolean[] status) { 327 for (int i = 0; i < status.length; i++) { 328 boolean tracked = status[i]; 329 FBNCCFeature feat = (FBNCCFeature) pt.features[i]; 330 FBNCCFeature trackedBackFeat = (FBNCCFeature) trackedBack.features[i]; 331 FBNCCFeature storageFeat = (FBNCCFeature) ptTracked.features[i]; 332 if (tracked) { 333 storageFeat.fbDistance = (float) Line2d.distance(feat, trackedBackFeat); 334 } else { 335 storageFeat.fbDistance = Float.NaN; 336 } 337 } 338 } 339 340 private void normCrossCorrelation(FImage imgI, FImage imgJ, FeatureList pt, 341 FeatureList ptTracked, boolean[] status, int winsize_ncc) { 342 for (int i = 0; i < status.length; i++) { 343 boolean tracked = status[i]; 344 FBNCCFeature feat = (FBNCCFeature) pt.features[i]; 345 FBNCCFeature featTracked = (FBNCCFeature) ptTracked.features[i]; 346 if (tracked) { 347 feat.ncc = TemplateMatcher.Mode.NORM_SUM_SQUARED_DIFFERENCE 348 .computeMatchScore(imgI.pixels, (int) feat.x, 349 (int) feat.y, imgJ.pixels, (int) featTracked.x, 350 (int) featTracked.y, winsize_ncc, winsize_ncc); 351 } else { 352 feat.ncc = Float.NaN; 353 } 354 } 355 } 356 357 /** 358 * Creates numM x numN points grid on BBox. Points ordered in 1 dimensional 359 * array (x1, y1, x2, y2). 360 * 361 * @param bb 362 * Bounding box represented through 2 points(x1,y1,x2,y2) 363 * @param numM 364 * Number of points in height direction. 365 * @param numN 366 * Number of points in width direction. 367 * @param margin 368 * margin (in pixel) 369 * @param pt2 370 * Contains the calculated points in the form (x1, y1, x2, y2). 371 * Size of the array must be numM * numN * 2. 372 */ 373 boolean getFilledBBPoints(float[] bb, int numM, int numN, int margin,FeatureList pt2) { 374 FeatureList pt = pt2; 375 int i; 376 int j; 377 float[] pbb_local; 378 /** 379 * gap between points in width direction 380 */ 381 float divN = 0; 382 /** 383 * gap between points in height direction 384 */ 385 float divM = 0; 386 float[] bb_local = new float[4]; 387 float spaceN; 388 float spaceM; 389 Feature cen; 390 /* add margin */ 391 bb_local[0] = bb[0] + margin; 392 bb_local[1] = bb[1] + margin; 393 bb_local[2] = bb[2] - margin; 394 bb_local[3] = bb[3] - margin; 395 pbb_local = bb_local; 396 /* printf("PointArraySize should be: %d\n", numM * numN * pointDim); */ 397 /* handle cases numX = 1 */// If there is 1 point pick one in the middle 398 if (numN == 1 && numM == 1) { 399 Feature center = calculateBBCenter(pbb_local); 400 center.val = KLTTracker.KLT_TRACKED; 401 pt2.features[0] = center; 402 return true; 403 } else if (numN == 1 && numM > 1) { 404 divM = numM - 1; 405 divN = 2; 406 /* maybe save center coordinate into bb[1] instead of loop again */ 407 /* calculate step width */ 408 spaceM = (bb_local[3] - bb_local[1]) / divM; 409 cen = calculateBBCenter(pbb_local); 410 /* calculate points and save them to the array */ 411 for (i = 0; i < numN; i++) { 412 for (j = 0; j < numM; j++) { 413 pt.features[i * numM + j] = new FBNCCFeature(); 414 Feature f = pt.features[i * numM + j]; 415 f.x = cen.x; 416 f.y = bb_local[1] + j * spaceM; 417 f.val = KLTTracker.KLT_TRACKED; 418 } 419 } 420 return true; 421 } else if (numN > 1 && numM == 1) { 422 divM = 2; 423 divN = numN - 1; 424 // maybe save center coordinate into bb[1] instead of loop again 425 // calculate step width 426 spaceN = (bb_local[2] - bb_local[0]) / divN; 427 cen = calculateBBCenter(pbb_local); 428 // calculate points and save them to the array 429 for (i = 0; i < numN; i++) { 430 for (j = 0; j < numM; j++) { 431 pt.features[i * numM + j] = new FBNCCFeature(); 432 Feature f = pt.features[i * numM + j]; 433 f.x = bb_local[0] + i * spaceN; 434 f.y = cen.y; 435 f.val = KLTTracker.KLT_TRACKED; 436 } 437 } 438 return true; 439 } else if (numN > 1 && numM > 1) { 440 divM = numM - 1; 441 divN = numN - 1; 442 } 443 // calculate step width 444 spaceN = (bb_local[2] - bb_local[0]) / divN; 445 spaceM = (bb_local[3] - bb_local[1]) / divM; 446 // calculate points and save them to the array 447 for (i = 0; i < numN; i++) { 448 for (j = 0; j < numM; j++) { 449 pt.features[i * numM + j] = new FBNCCFeature(); 450 Feature f = pt.features[i * numM + j]; 451 f.x = bb_local[0] + i * spaceN; 452 f.y = bb_local[1] + j * spaceM; 453 f.val = KLTTracker.KLT_TRACKED; 454 } 455 } 456 return true; 457 } 458 459 /** 460 * Calculates center of a Rectangle/Boundingbox. 461 * 462 * @param bb 463 * defined with 2 points x,y,x1,y1 464 */ 465 Feature calculateBBCenter(float[] bb) { 466 float[] bbnow = bb; 467 Feature centernow = null; 468 if (bbnow == null) { 469 return null; 470 } 471 centernow = new FBNCCFeature(); 472 centernow.x = 0.5f * (bbnow[0] + bbnow[2]); 473 centernow.y = 0.5f * (bbnow[1] + bbnow[3]); 474 return centernow; 475 } 476}