001/** 002 * This source code file is part of a direct port of Stan Birchfield's implementation 003 * of a Kanade-Lucas-Tomasi feature tracker. The original implementation can be found 004 * here: http://www.ces.clemson.edu/~stb/klt/ 005 * 006 * As per the original code, the source code is in the public domain, available 007 * for both commercial and non-commercial use. 008 */ 009package org.openimaj.video.tracking.klt; 010 011import java.io.File; 012import java.io.IOException; 013import java.util.Arrays; 014import java.util.BitSet; 015import java.util.Comparator; 016 017import org.openimaj.image.FImage; 018import org.openimaj.image.ImageUtilities; 019import org.openimaj.image.processing.convolution.FGaussianConvolve; 020import org.openimaj.math.geometry.point.Point2d; 021import org.openimaj.math.geometry.point.Point2dImpl; 022 023/** 024 * The KLT tracker 025 * 026 * @author Stan Birchfield 027 * @author Jonathon Hare (jsh2@ecs.soton.ac.uk) 028 */ 029public class KLTTracker { 030 protected int KLT_verbose = 0; 031 032 /** 033 * The feature was tracked 034 */ 035 public static final int KLT_TRACKED = 0; 036 /** 037 * The feature was not found 038 */ 039 public static final int KLT_NOT_FOUND = -1; 040 /** 041 * The determinant was too small 042 */ 043 public static final int KLT_SMALL_DET = -2; 044 /** 045 * The maximum number of iterations was exceeded 046 */ 047 public static final int KLT_MAX_ITERATIONS = -3; 048 /** 049 * The feature was out of bouns 050 */ 051 public static final int KLT_OOB = -4; 052 /** 053 * The residue was too large 054 */ 055 public static final int KLT_LARGE_RESIDUE = -5; 056 057 /** 058 * Modes of operation for selecting features. 059 */ 060 public enum SelectionMode { 061 /** 062 * selecting all features 063 */ 064 SELECTING_ALL, 065 /** 066 * Replacing some features 067 */ 068 REPLACING_SOME 069 } 070 071 TrackingContext tc; 072 FeatureList featurelist; 073 boolean isNorm = true; // true if input images are in [0..1] range, false if 074 // [0..255] 075 076 /** 077 * Construct with the given target number of features. 078 * 079 * @param nfeatures 080 */ 081 public KLTTracker(int nfeatures) { 082 this.tc = new TrackingContext(); 083 this.featurelist = new FeatureList(nfeatures); 084 } 085 086 /** 087 * Construct with the given context and feature list 088 * 089 * @param tc 090 * @param featurelist 091 */ 092 public KLTTracker(TrackingContext tc, FeatureList featurelist) { 093 this.tc = tc; 094 this.featurelist = featurelist; 095 } 096 097 /*********************************************************************/ 098 private void _fillFeaturemap(int x, int y, BitSet featuremap, int mindist, int ncols, int nrows) 099 { 100 int ix, iy; 101 102 for (iy = y - mindist; iy <= y + mindist; iy++) 103 for (ix = x - mindist; ix <= x + mindist; ix++) 104 if (ix >= 0 && ix < ncols && iy >= 0 && iy < nrows) 105 featuremap.set(iy * ncols + ix); 106 } 107 108 /********************************************************************* 109 * _enforceMinimumDistance 110 * 111 * Removes features that are within close proximity to better features. 112 * 113 * INPUTS featurelist: A list of features. The nFeatures property is used. 114 * 115 * OUTPUTS featurelist: Is overwritten. Nearby "redundant" features are 116 * removed. Writes -1's into the remaining elements. 117 * 118 * RETURNS The number of remaining features. 119 */ 120 private void _enforceMinimumDistance( 121 int[][] pointlist, /* featurepoints */ 122 int ncols, int nrows, /* size of images */ 123 int mindist, /* min. dist b/w features */ 124 int min_eigenvalue, /* min. eigenvalue */ 125 boolean overwriteAllFeatures) 126 { 127 final int npoints = pointlist.length; 128 129 int indx; /* Index into features */ 130 int x, y, val; /* 131 * Location and trackability of pixel under 132 * consideration 133 */ 134 BitSet featuremap; /* Boolean array recording proximity of features */ 135 int ptr; 136 137 /* Cannot add features with an eigenvalue less than one */ 138 if (min_eigenvalue < 1) 139 min_eigenvalue = 1; 140 141 /* Allocate memory for feature map and clear it */ 142 featuremap = new BitSet(ncols * nrows); 143 144 /* Necessary because code below works with (mindist-1) */ 145 mindist--; 146 147 /* 148 * If we are keeping all old good features, then add them to the 149 * featuremap 150 */ 151 if (!overwriteAllFeatures) 152 for (indx = 0; indx < featurelist.features.length; indx++) 153 if (featurelist.features[indx].val >= 0) { 154 x = (int) featurelist.features[indx].x; 155 y = (int) featurelist.features[indx].y; 156 _fillFeaturemap(x, y, featuremap, mindist, ncols, nrows); 157 } 158 159 /* For each feature point, in descending order of importance, do ... */ 160 ptr = 0; 161 indx = 0; 162 while (true) { 163 /* 164 * If we can't add all the points, then fill in the rest of the 165 * featurelist with -1's 166 */ 167 if (ptr >= npoints) { 168 while (indx < featurelist.features.length) { 169 if (overwriteAllFeatures || 170 featurelist.features[indx].val < 0) 171 { 172 featurelist.features[indx].x = -1; 173 featurelist.features[indx].y = -1; 174 featurelist.features[indx].val = KLT_NOT_FOUND; 175 // featurelist.features[indx].aff_img = null; 176 // featurelist.features[indx].aff_img_gradx = null; 177 // featurelist.features[indx].aff_img_grady = null; 178 // featurelist.features[indx].aff_x = -1.0f; 179 // featurelist.features[indx].aff_y = -1.0f; 180 // featurelist.features[indx].aff_Axx = 1.0f; 181 // featurelist.features[indx].aff_Ayx = 0.0f; 182 // featurelist.features[indx].aff_Axy = 0.0f; 183 // featurelist.features[indx].aff_Ayy = 1.0f; 184 } 185 indx++; 186 } 187 break; 188 } 189 190 x = pointlist[ptr][0]; 191 y = pointlist[ptr][1]; 192 val = pointlist[ptr][2]; 193 ptr++; 194 195 /* Ensure that feature is in-bounds */ 196 assert (x >= 0); 197 assert (x < ncols); 198 assert (y >= 0); 199 assert (y < nrows); 200 201 while (!overwriteAllFeatures && 202 indx < featurelist.features.length && 203 featurelist.features[indx].val >= 0) 204 indx++; 205 206 if (indx >= featurelist.features.length) 207 break; 208 209 /* 210 * If no neighbor has been selected, and if the minimum eigenvalue 211 * is large enough, then add feature to the current list 212 */ 213 if (!featuremap.get(y * ncols + x) && val >= min_eigenvalue) { 214 featurelist.features[indx].x = x; 215 featurelist.features[indx].y = y; 216 featurelist.features[indx].val = val; 217 // featurelist.features[indx].aff_img = null; 218 // featurelist.features[indx].aff_img_gradx = null; 219 // featurelist.features[indx].aff_img_grady = null; 220 // featurelist.features[indx].aff_x = -1.0f; 221 // featurelist.features[indx].aff_y = -1.0f; 222 // featurelist.features[indx].aff_Axx = 1.0f; 223 // featurelist.features[indx].aff_Ayx = 0.0f; 224 // featurelist.features[indx].aff_Axy = 0.0f; 225 // featurelist.features[indx].aff_Ayy = 1.0f; 226 indx++; 227 228 /* 229 * Fill in surrounding region of feature map, but make sure that 230 * pixels are in-bounds 231 */ 232 _fillFeaturemap(x, y, featuremap, mindist, ncols, nrows); 233 } 234 } 235 } 236 237 /********************************************************************* 238 * _sortPointList 239 */ 240 private void _sortPointList(int[][] pointlist) 241 { 242 Arrays.sort(pointlist, new Comparator<int[]>() { 243 @Override 244 public int compare(int[] o1, int[] o2) { 245 final int v1 = o1[2]; 246 final int v2 = o2[2]; 247 248 if (v1 > v2) 249 return (-1); 250 else if (v1 < v2) 251 return (1); 252 else 253 return (0); 254 } 255 }); 256 } 257 258 /********************************************************************* 259 * _minEigenvalue 260 * 261 * Given the three distinct elements of the symmetric 2x2 matrix [gxx gxy] 262 * [gxy gyy], Returns the minimum eigenvalue of the matrix. 263 */ 264 private float _minEigenvalue(float gxx, float gxy, float gyy) 265 { 266 return (float) ((gxx + gyy - Math.sqrt((gxx - gyy) * (gxx - gyy) + 4 * gxy * gxy)) / 2.0f); 267 } 268 269 /** 270 * @throws IOException 271 *******************************************************************/ 272 private void _selectGoodFeatures( 273 FImage img, 274 SelectionMode mode) 275 { 276 final int nrows = img.height, ncols = img.width; 277 FImage floatimg, gradx, grady; 278 int window_hw, window_hh; 279 int[][] pointlist; 280 281 final boolean overwriteAllFeatures = (mode == SelectionMode.SELECTING_ALL) ? true : false; 282 // boolean floatimages_created = false; 283 284 /* Check window size (and correct if necessary) */ 285 if (tc.window_width % 2 != 1) { 286 tc.window_width = tc.window_width + 1; 287 System.err.format("Tracking context's window width must be odd. Changing to %d.\n", tc.window_width); 288 } 289 if (tc.window_height % 2 != 1) { 290 tc.window_height = tc.window_height + 1; 291 System.err.format("Tracking context's window height must be odd. Changing to %d.\n", tc.window_height); 292 } 293 if (tc.window_width < 3) { 294 tc.window_width = 3; 295 System.err.format("Tracking context's window width must be at least three. \nChanging to %d.\n", 296 tc.window_width); 297 } 298 if (tc.window_height < 3) { 299 tc.window_height = 3; 300 System.err.format("Tracking context's window height must be at least three. \nChanging to %d.\n", 301 tc.window_height); 302 } 303 window_hw = tc.window_width / 2; 304 window_hh = tc.window_height / 2; 305 306 /* Create pointlist, which is a simplified version of a featurelist, */ 307 /* for speed. Contains only integer locations and values. */ 308 pointlist = new int[ncols * nrows][3]; 309 310 /* Create temporary images, etc. */ 311 final PyramidSet ppSet = tc.previousPyramidSet(); 312 if (mode == SelectionMode.REPLACING_SOME && 313 tc.sequentialMode && ppSet != null) 314 { 315 floatimg = ppSet.imgPyr.img[0]; 316 gradx = ppSet.gradx.img[0]; 317 grady = ppSet.grady.img[0]; 318 assert (gradx != null); 319 assert (grady != null); 320 } else { 321 // floatimages_created = true; 322 floatimg = new FImage(ncols, nrows); 323 gradx = new FImage(ncols, nrows); 324 grady = new FImage(ncols, nrows); 325 if (tc.smoothBeforeSelecting) { 326 floatimg = img.process(new FGaussianConvolve(tc.computeSmoothSigma())); 327 } else { 328 floatimg = img.clone(); 329 } 330 331 /* Compute gradient of image in x and y direction */ 332 tc.computeGradients(floatimg, tc.grad_sigma, gradx, grady); 333 } 334 335 /* Write internal images */ 336 if (tc.writeInternalImages) { 337 try { 338 ImageUtilities.write(floatimg, "png", new File("kltimg_sgfrlf.png")); 339 ImageUtilities.write(gradx, "png", new File("kltimg_sgfrlf_gx.png")); 340 ImageUtilities.write(grady, "png", new File("kltimg_sgfrlf_gy.png")); 341 } catch (final IOException e) { 342 343 } 344 } 345 346 /* 347 * Compute trackability of each image pixel as the minimum of the two 348 * eigenvalues of the Z matrix 349 */ 350 { 351 float gx, gy; 352 float gxx, gxy, gyy; 353 int xx, yy; 354 int ptr; 355 float val; 356 int limit = 1; 357 int borderx = tc.borderx; /* Must not touch cols */ 358 int bordery = tc.bordery; /* lost by convolution */ 359 int x, y; 360 361 if (borderx < window_hw) 362 borderx = window_hw; 363 if (bordery < window_hh) 364 bordery = window_hh; 365 366 /* Find largest value of an int */ 367 limit = Integer.MAX_VALUE / 2 - 1; 368 369 /* For most of the pixels in the image, do ... */ 370 ptr = 0; 371 for (y = bordery; y < nrows - bordery; y += tc.nSkippedPixels + 1) 372 for (x = borderx; x < ncols - borderx; x += tc.nSkippedPixels + 1) { 373 if (tc.getTargetArea() != null) { 374 final Point2d point = new Point2dImpl(x, y); 375 if (!tc.getTargetArea().isInside(point)) 376 continue; 377 } 378 /* Sum the gradients in the surrounding window */ 379 gxx = 0; 380 gxy = 0; 381 gyy = 0; 382 for (yy = y - window_hh; yy <= y + window_hh; yy++) 383 for (xx = x - window_hw; xx <= x + window_hw; xx++) { 384 gx = gradx.pixels[yy][xx]; 385 gy = grady.pixels[yy][xx]; 386 gxx += gx * gx; 387 gxy += gx * gy; 388 gyy += gy * gy; 389 } 390 391 /* 392 * Store the trackability of the pixel as the minimum of the 393 * two eigenvalues 394 */ 395 pointlist[ptr][0] = x; 396 pointlist[ptr][1] = y; 397 val = _minEigenvalue(gxx, gxy, gyy); 398 if (val > limit) { 399 System.err 400 .format("(_KLTSelectGoodFeatures) minimum eigenvalue %f is greater than the capacity of an int; setting to maximum value", 401 val); 402 val = limit; 403 } 404 pointlist[ptr][2] = (int) val; 405 ++ptr; 406 } 407 } 408 409 /* Sort the features */ 410 _sortPointList(pointlist); 411 412 /* Check tc.mindist */ 413 if (tc.mindist < 0) { 414 System.err.format( 415 "(_KLTSelectGoodFeatures) Tracking context field tc.mindist is negative (%d); setting to zero", 416 tc.mindist); 417 tc.mindist = 0; 418 } 419 420 /* Enforce minimum distance between features */ 421 _enforceMinimumDistance( 422 pointlist, 423 ncols, nrows, 424 tc.mindist, 425 tc.min_eigenvalue, 426 overwriteAllFeatures); 427 } 428 429 /********************************************************************* 430 * KLTSelectGoodFeatures 431 * 432 * Main routine, visible to the outside. Finds the good features in an 433 * image. 434 * 435 * INPUTS tc: Contains parameters used in computation (size of image, size 436 * of window, min distance b/w features, sigma to compute image gradients, # 437 * of features desired). 438 * 439 * @param img 440 * Pointer to the data of an image (probably unsigned chars). 441 * 442 * OUTPUTS features: List of features. The member nFeatures is 443 * computed. 444 */ 445 public void selectGoodFeatures(FImage img) { 446 if (isNorm) 447 img = img.multiply(255f); 448 449 if (KLT_verbose >= 1) { 450 System.err.format("(KLT) Selecting the %d best features from a %d by %d image... ", 451 featurelist.features.length, img.width, img.height); 452 } 453 454 _selectGoodFeatures(img, SelectionMode.SELECTING_ALL); 455 456 if (KLT_verbose >= 1) { 457 System.err.format("\n\t%d features found.\n", featurelist.countRemainingFeatures()); 458 if (tc.writeInternalImages) 459 System.err.format("\tWrote images to 'kltimg_sgfrlf*.pgm'.\n"); 460 } 461 } 462 463 /********************************************************************* 464 * KLTReplaceLostFeatures 465 * 466 * Main routine, visible to the outside. Replaces the lost features in an 467 * image. 468 * 469 * INPUTS tc: Contains parameters used in computation (size of image, size 470 * of window, min distance b/w features, sigma to compute image gradients, # 471 * of features desired). 472 * 473 * @param img 474 * Pointer to the data of an image (probably unsigned chars). 475 * 476 * OUTPUTS features: List of features. The member nFeatures is 477 * computed. 478 */ 479 public void replaceLostFeatures(FImage img) 480 { 481 if (isNorm) 482 img = img.multiply(255f); 483 484 final int nLostFeatures = featurelist.features.length - featurelist.countRemainingFeatures(); 485 486 if (KLT_verbose >= 1) { 487 System.err.format("(KLT) Attempting to replace %d features in a %d by %d image... ", nLostFeatures, 488 img.width, img.height); 489 } 490 491 /* If there are any lost features, replace them */ 492 if (nLostFeatures > 0) 493 _selectGoodFeatures(img, SelectionMode.REPLACING_SOME); 494 495 if (KLT_verbose >= 1) { 496 System.err.format("\n\t%d features replaced.\n", 497 nLostFeatures - featurelist.features.length + featurelist.countRemainingFeatures()); 498 if (tc.writeInternalImages) 499 System.err.format("\tWrote images to 'kltimg_sgfrlf*.pgm'.\n"); 500 } 501 } 502 503 /********************************************************************* 504 * KLTSetVerbosity 505 * 506 * @param verbosity 507 */ 508 public void setVerbosity(int verbosity) 509 { 510 KLT_verbose = verbosity; 511 } 512 513 /********************************************************************* 514 * _interpolate 515 * 516 * Given a point (x,y) in an image, computes the bilinear interpolated 517 * gray-level value of the point in the image. 518 */ 519 private float _interpolate(float x, float y, FImage img) 520 { 521 final int xt = (int) x; /* coordinates of top-left corner */ 522 final int yt = (int) y; 523 final float ax = x - xt; 524 final float ay = y - yt; 525 526 final float x0y0 = img.pixels[yt][xt]; 527 final float x1y0 = img.pixels[yt][xt + 1]; 528 final float x0y1 = img.pixels[yt + 1][xt]; 529 final float x1y1 = img.pixels[yt + 1][xt + 1]; 530 531 return ((1 - ax) * (1 - ay) * x0y0 + 532 ax * (1 - ay) * x1y0 + 533 (1 - ax) * ay * x0y1 + ax * ay * x1y1); 534 } 535 536 /********************************************************************* 537 * _computeIntensityDifference 538 * 539 * Given two images and the window center in both images, aligns the images 540 * wrt the window and computes the difference between the two overlaid 541 * images. 542 */ 543 private void _computeIntensityDifference( 544 FImage img1, /* images */ 545 FImage img2, 546 float x1, float y1, /* center of window in 1st img */ 547 float x2, float y2, /* center of window in 2nd img */ 548 int width, int height, /* size of window */ 549 float[] imgdiff) /* output */ 550 { 551 final int hw = width / 2, hh = height / 2; 552 float g1, g2; 553 int i, j; 554 555 /* Compute values */ 556 int idx = 0; 557 for (j = -hh; j <= hh; j++) 558 for (i = -hw; i <= hw; i++) { 559 g1 = _interpolate(x1 + i, y1 + j, img1); 560 g2 = _interpolate(x2 + i, y2 + j, img2); 561 imgdiff[idx++] = g1 - g2; 562 } 563 } 564 565 /********************************************************************* 566 * _computeGradientSum 567 * 568 * Given two gradients and the window center in both images, aligns the 569 * gradients wrt the window and computes the sum of the two overlaid 570 * gradients. 571 */ 572 private void _computeGradientSum( 573 FImage gradx1, /* gradient images */ 574 FImage grady1, 575 FImage gradx2, 576 FImage grady2, 577 float x1, float y1, /* center of window in 1st img */ 578 float x2, float y2, /* center of window in 2nd img */ 579 int width, int height, /* size of window */ 580 float[] gradx, /* output */ 581 float[] grady) /* " */ 582 { 583 final int hw = width / 2, hh = height / 2; 584 float g1, g2; 585 int i, j; 586 587 int gx = 0, gy = 0; 588 /* Compute values */ 589 for (j = -hh; j <= hh; j++) 590 for (i = -hw; i <= hw; i++) { 591 g1 = _interpolate(x1 + i, y1 + j, gradx1); 592 g2 = _interpolate(x2 + i, y2 + j, gradx2); 593 // *gradx++ = g1 + g2; 594 gradx[gx++] = g1 + g2; 595 g1 = _interpolate(x1 + i, y1 + j, grady1); 596 g2 = _interpolate(x2 + i, y2 + j, grady2); 597 // *grady++ = g1 + g2; 598 grady[gy++] = g1 + g2; 599 } 600 } 601 602 /********************************************************************* 603 * _computeIntensityDifferenceLightingInsensitive 604 * 605 * Given two images and the window center in both images, aligns the images 606 * wrt the window and computes the difference between the two overlaid 607 * images; normalizes for overall gain and bias. 608 */ 609 private void _computeIntensityDifferenceLightingInsensitive( 610 FImage img1, /* images */ 611 FImage img2, 612 float x1, float y1, /* center of window in 1st img */ 613 float x2, float y2, /* center of window in 2nd img */ 614 int width, int height, /* size of window */ 615 float[] imgdiff) /* output */ 616 { 617 final int hw = width / 2, hh = height / 2; 618 float g1, g2, sum1_squared = 0, sum2_squared = 0; 619 int i, j; 620 621 float sum1 = 0, sum2 = 0; 622 float mean1, mean2, alpha, belta; 623 /* Compute values */ 624 for (j = -hh; j <= hh; j++) 625 for (i = -hw; i <= hw; i++) { 626 g1 = _interpolate(x1 + i, y1 + j, img1); 627 g2 = _interpolate(x2 + i, y2 + j, img2); 628 sum1 += g1; 629 sum2 += g2; 630 sum1_squared += g1 * g1; 631 sum2_squared += g2 * g2; 632 } 633 mean1 = sum1_squared / (width * height); 634 mean2 = sum2_squared / (width * height); 635 alpha = (float) Math.sqrt(mean1 / mean2); 636 mean1 = sum1 / (width * height); 637 mean2 = sum2 / (width * height); 638 belta = mean1 - alpha * mean2; 639 640 int id = 0; 641 for (j = -hh; j <= hh; j++) 642 for (i = -hw; i <= hw; i++) { 643 g1 = _interpolate(x1 + i, y1 + j, img1); 644 g2 = _interpolate(x2 + i, y2 + j, img2); 645 // *imgdiff++ = g1- g2*alpha-belta; 646 imgdiff[id++] = g1 - g2 * alpha - belta; 647 } 648 } 649 650 /********************************************************************* 651 * _computeGradientSumLightingInsensitive 652 * 653 * Given two gradients and the window center in both images, aligns the 654 * gradients wrt the window and computes the sum of the two overlaid 655 * gradients; normalizes for overall gain and bias. 656 */ 657 private void _computeGradientSumLightingInsensitive( 658 FImage gradx1, /* gradient images */ 659 FImage grady1, 660 FImage gradx2, 661 FImage grady2, 662 FImage img1, /* images */ 663 FImage img2, 664 665 float x1, float y1, /* center of window in 1st img */ 666 float x2, float y2, /* center of window in 2nd img */ 667 int width, int height, /* size of window */ 668 float[] gradx, /* output */ 669 float[] grady) /* " */ 670 { 671 final int hw = width / 2, hh = height / 2; 672 float g1, g2, sum1_squared = 0, sum2_squared = 0; 673 int i, j; 674 675 // float sum1 = 0, sum2 = 0; 676 float mean1, mean2, alpha; 677 for (j = -hh; j <= hh; j++) 678 for (i = -hw; i <= hw; i++) { 679 g1 = _interpolate(x1 + i, y1 + j, img1); 680 g2 = _interpolate(x2 + i, y2 + j, img2); 681 sum1_squared += g1; 682 sum2_squared += g2; 683 } 684 mean1 = sum1_squared / (width * height); 685 mean2 = sum2_squared / (width * height); 686 alpha = (float) Math.sqrt(mean1 / mean2); 687 688 int gx = 0, gy = 0; 689 /* Compute values */ 690 for (j = -hh; j <= hh; j++) 691 for (i = -hw; i <= hw; i++) { 692 g1 = _interpolate(x1 + i, y1 + j, gradx1); 693 g2 = _interpolate(x2 + i, y2 + j, gradx2); 694 // *gradx++ = g1 + g2*alpha; 695 gradx[gx++] = g1 + g2 * alpha; 696 g1 = _interpolate(x1 + i, y1 + j, grady1); 697 g2 = _interpolate(x2 + i, y2 + j, grady2); 698 // *grady++ = g1+ g2*alpha; 699 grady[gy++] = g1 + g2 * alpha; 700 } 701 } 702 703 /********************************************************************* 704 * _compute2by2GradientMatrix 705 * 706 */ 707 private float[] _compute2by2GradientMatrix( 708 float[] gradx, 709 float[] grady, 710 int width, /* size of window */ 711 int height 712 ) 713 714 { 715 float gx, gy; 716 int i; 717 718 float gxx; /* return values */ 719 float gxy; 720 float gyy; 721 722 /* Compute values */ 723 gxx = 0.0f; 724 gxy = 0.0f; 725 gyy = 0.0f; 726 for (i = 0; i < width * height; i++) { 727 // gx = *gradx++; 728 gx = gradx[i]; 729 // gy = *grady++; 730 gy = grady[i]; 731 gxx += gx * gx; 732 gxy += gx * gy; 733 gyy += gy * gy; 734 } 735 736 return new float[] { gxx, gxy, gyy }; 737 } 738 739 /********************************************************************* 740 * _compute2by1ErrorVector 741 * 742 */ 743 744 private float[] _compute2by1ErrorVector( 745 float[] imgdiff, 746 float[] gradx, 747 float[] grady, 748 int width, /* size of window */ 749 int height, 750 float step_factor /* 751 * 2.0 comes from equations, 1.0 seems to avoid 752 * overshooting 753 */ 754 ) 755 { 756 float diff; 757 int i; 758 759 float ex, ey; 760 761 /* Compute values */ 762 ex = 0; 763 ey = 0; 764 for (i = 0; i < width * height; i++) { 765 diff = imgdiff[i]; 766 ex += diff * gradx[i]; 767 ey += diff * grady[i]; 768 } 769 ex *= step_factor; 770 ey *= step_factor; 771 772 return new float[] { ex, ey }; 773 } 774 775 /********************************************************************* 776 * _solveEquation 777 * 778 * Solves the 2x2 matrix equation [gxx gxy] [dx] = [ex] [gxy gyy] [dy] = 779 * [ey] for dx and dy. 780 * 781 * Returns KLT_TRACKED on success and KLT_SMALL_DET on failure 782 */ 783 private int _solveEquation( 784 float gxx, float gxy, float gyy, 785 float ex, float ey, 786 float small, 787 float[] output) 788 { 789 final float det = gxx * gyy - gxy * gxy; 790 791 if (det < small) 792 return KLT_SMALL_DET; 793 794 output[0] = (gyy * ex - gxy * ey) / det; // dx 795 output[1] = (gxx * ey - gxy * ex) / det; // dy 796 return KLT_TRACKED; 797 } 798 799 /********************************************************************* 800 * _sumAbsFloatWindow 801 */ 802 private float _sumAbsFloatWindow( 803 float[] fw, 804 int width, 805 int height) 806 { 807 float sum = 0.0f; 808 809 for (int i = 0; i < height * width; i++) 810 sum += Math.abs(fw[i]); 811 812 return sum; 813 } 814 815 /********************************************************************* 816 * _trackFeature 817 * 818 * Tracks a feature point from one image to the next. 819 * 820 * RETURNS KLT_SMALL_DET if feature is lost, KLT_MAX_ITERATIONS if tracking 821 * stopped because iterations timed out, KLT_TRACKED otherwise. 822 */ 823 private int _trackFeature( 824 float x1, /* location of window in first image */ 825 float y1, 826 float[] xy2, /* starting location of search in second image */ 827 FImage img1, 828 FImage gradx1, 829 FImage grady1, 830 FImage img2, 831 FImage gradx2, 832 FImage grady2, 833 int width, /* size of window */ 834 int height, 835 float step_factor, /* 836 * 2.0 comes from equations, 1.0 seems to avoid 837 * overshooting 838 */ 839 int max_iterations, 840 float small, /* determinant threshold for declaring KLT_SMALL_DET */ 841 float th, /* displacement threshold for stopping */ 842 float max_residue, /* 843 * residue threshold for declaring 844 * KLT_LARGE_RESIDUE 845 */ 846 boolean lighting_insensitive) /* 847 * whether to normalize for gain and 848 * bias 849 */ 850 { 851 float[] imgdiff, gradx, grady; 852 float gxx, gxy, gyy, ex, ey, dx, dy; 853 int iteration = 0; 854 int status; 855 final int hw = width / 2; 856 final int hh = height / 2; 857 final int nc = img1.width; 858 final int nr = img1.height; 859 final float one_plus_eps = 1.001f; /* To prevent rounding errors */ 860 861 /* Allocate memory for windows */ 862 imgdiff = new float[height * width]; 863 gradx = new float[height * width]; 864 grady = new float[height * width]; 865 866 /* Iteratively update the window position */ 867 do { 868 869 /* If out of bounds, exit loop */ 870 if (x1 - hw < 0.0f || nc - (x1 + hw) < one_plus_eps || 871 xy2[0] - hw < 0.0f || nc - (xy2[0] + hw) < one_plus_eps || 872 y1 - hh < 0.0f || nr - (y1 + hh) < one_plus_eps || 873 xy2[1] - hh < 0.0f || nr - (xy2[1] + hh) < one_plus_eps) 874 { 875 status = KLT_OOB; 876 break; 877 } 878 879 /* Compute gradient and difference windows */ 880 if (lighting_insensitive) { 881 _computeIntensityDifferenceLightingInsensitive(img1, img2, x1, y1, xy2[0], xy2[1], width, height, imgdiff); 882 _computeGradientSumLightingInsensitive(gradx1, grady1, gradx2, grady2, img1, img2, x1, y1, xy2[0], 883 xy2[1], width, height, gradx, grady); 884 } else { 885 _computeIntensityDifference(img1, img2, x1, y1, xy2[0], xy2[1], width, height, imgdiff); 886 _computeGradientSum(gradx1, grady1, gradx2, grady2, x1, y1, xy2[0], xy2[1], width, height, gradx, grady); 887 } 888 889 /* Use these windows to construct matrices */ 890 float[] tmp = _compute2by2GradientMatrix(gradx, grady, width, height); 891 gxx = tmp[0]; 892 gxy = tmp[1]; 893 gyy = tmp[2]; 894 895 tmp = _compute2by1ErrorVector(imgdiff, gradx, grady, width, height, step_factor); 896 ex = tmp[0]; 897 ey = tmp[1]; 898 899 /* Using matrices, solve equation for new displacement */ 900 tmp = new float[2]; 901 status = _solveEquation(gxx, gxy, gyy, ex, ey, small, tmp); 902 dx = tmp[0]; 903 dy = tmp[1]; 904 if (status == KLT_SMALL_DET) 905 break; 906 907 xy2[0] += dx; 908 xy2[1] += dy; 909 iteration++; 910 911 } while ((Math.abs(dx) >= th || Math.abs(dy) >= th) && iteration < max_iterations); 912 913 /* Check whether window is out of bounds */ 914 if (xy2[0] - hw < 0.0f || nc - (xy2[0] + hw) < one_plus_eps || 915 xy2[1] - hh < 0.0f || nr - (xy2[1] + hh) < one_plus_eps) 916 status = KLT_OOB; 917 918 /* Check whether residue is too large */ 919 if (status == KLT_TRACKED) { 920 if (lighting_insensitive) 921 _computeIntensityDifferenceLightingInsensitive(img1, img2, x1, y1, xy2[0], xy2[1], width, height, imgdiff); 922 else 923 _computeIntensityDifference(img1, img2, x1, y1, xy2[0], xy2[1], width, height, imgdiff); 924 if (_sumAbsFloatWindow(imgdiff, width, height) / (width * height) > max_residue) 925 status = KLT_LARGE_RESIDUE; 926 } 927 928 /* Return appropriate value */ 929 if (status == KLT_SMALL_DET) 930 return KLT_SMALL_DET; 931 else if (status == KLT_OOB) 932 return KLT_OOB; 933 else if (status == KLT_LARGE_RESIDUE) 934 return KLT_LARGE_RESIDUE; 935 else if (iteration >= max_iterations) 936 return KLT_MAX_ITERATIONS; 937 else 938 return KLT_TRACKED; 939 } 940 941 /*********************************************************************/ 942 943 private boolean _outOfBounds( 944 float x, 945 float y, 946 int ncols, 947 int nrows, 948 int borderx, 949 int bordery) 950 { 951 return (x < borderx || x > ncols - 1 - borderx || 952 y < bordery || y > nrows - 1 - bordery); 953 } 954 955 // /********************************************************************** 956 // * CONSISTENCY CHECK OF FEATURES BY AFFINE MAPPING (BEGIN) 957 // * 958 // * Created by: Thorsten Thormaehlen (University of Hannover) June 2004 959 // * thormae@tnt.uni-hannover.de 960 // * 961 // * Permission is granted to any individual or institution to use, copy, 962 // modify, 963 // * and distribute this part of the software, provided that this complete 964 // authorship 965 // * and permission notice is maintained, intact, in all copies. 966 // * 967 // * This software is provided "as is" without express or implied warranty. 968 // * 969 // * 970 // * The following static functions are helpers for the affine mapping. 971 // * They all start with "_am". 972 // * There are also small changes in other files for the 973 // * affine mapping these are all marked by "for affine mapping" 974 // * 975 // * Thanks to Kevin Koeser (koeser@mip.informatik.uni-kiel.de) for fixing a 976 // bug 977 // */ 978 // 979 // #define SWAP_ME(X,Y) {temp=(X);(X)=(Y);(Y)=temp;} 980 // 981 // static float **_am_matrix(long nr, long nc) 982 // { 983 // float **m; 984 // int a; 985 // m = (float **) malloc((size_t)(nr*sizeof(float*))); 986 // m[0] = (float *) malloc((size_t)((nr*nc)*sizeof(float))); 987 // for(a = 1; a < nr; a++) m[a] = m[a-1]+nc; 988 // return m; 989 // } 990 // 991 // static void _am_free_matrix(float **m) 992 // { 993 // free(m[0]); 994 // free(m); 995 // } 996 // 997 // 998 // static int _am_gauss_jordan_elimination(float **a, int n, float **b, int 999 // m) 1000 // { 1001 // /* re-implemented from Numerical Recipes in C */ 1002 // int *indxc,*indxr,*ipiv; 1003 // int i,j,k,l,ll; 1004 // float big,dum,pivinv,temp; 1005 // int col = 0; 1006 // int row = 0; 1007 // 1008 // indxc=(int *)malloc((size_t) (n*sizeof(int))); 1009 // indxr=(int *)malloc((size_t) (n*sizeof(int))); 1010 // ipiv=(int *)malloc((size_t) (n*sizeof(int))); 1011 // for (j=0;j<n;j++) ipiv[j]=0; 1012 // for (i=0;i<n;i++) { 1013 // big=0.0; 1014 // for (j=0;j<n;j++) 1015 // if (ipiv[j] != 1) 1016 // for (k=0;k<n;k++) { 1017 // if (ipiv[k] == 0) { 1018 // if (fabs(a[j][k]) >= big) { 1019 // big= (float) fabs(a[j][k]); 1020 // row=j; 1021 // col=k; 1022 // } 1023 // } else if (ipiv[k] > 1) return KLT_SMALL_DET; 1024 // } 1025 // ++(ipiv[col]); 1026 // if (row != col) { 1027 // for (l=0;l<n;l++) SWAP_ME(a[row][l],a[col][l]) 1028 // for (l=0;l<m;l++) SWAP_ME(b[row][l],b[col][l]) 1029 // } 1030 // indxr[i]=row; 1031 // indxc[i]=col; 1032 // if (a[col][col] == 0.0) return KLT_SMALL_DET; 1033 // pivinv=1.0f/a[col][col]; 1034 // a[col][col]=1.0; 1035 // for (l=0;l<n;l++) a[col][l] *= pivinv; 1036 // for (l=0;l<m;l++) b[col][l] *= pivinv; 1037 // for (ll=0;ll<n;ll++) 1038 // if (ll != col) { 1039 // dum=a[ll][col]; 1040 // a[ll][col]=0.0; 1041 // for (l=0;l<n;l++) a[ll][l] -= a[col][l]*dum; 1042 // for (l=0;l<m;l++) b[ll][l] -= b[col][l]*dum; 1043 // } 1044 // } 1045 // for (l=n-1;l>=0;l--) { 1046 // if (indxr[l] != indxc[l]) 1047 // for (k=0;k<n;k++) 1048 // SWAP_ME(a[k][indxr[l]],a[k][indxc[l]]); 1049 // } 1050 // free(ipiv); 1051 // free(indxr); 1052 // free(indxc); 1053 // 1054 // return KLT_TRACKED; 1055 // } 1056 // 1057 // /********************************************************************* 1058 // * _am_getGradientWinAffine 1059 // * 1060 // * aligns the gradients with the affine transformed window 1061 // */ 1062 // 1063 // static void _am_getGradientWinAffine( 1064 // FImage in_gradx, 1065 // FImage in_grady, 1066 // float x, float y, /* center of window*/ 1067 // float Axx, float Ayx , float Axy, float Ayy, /* affine mapping */ 1068 // int width, int height, /* size of window */ 1069 // _FloatWindow out_gradx, /* output */ 1070 // _FloatWindow out_grady) /* output */ 1071 // { 1072 // int hw = width/2, hh = height/2; 1073 // int i, j; 1074 // float mi, mj; 1075 // 1076 // /* Compute values */ 1077 // for (j = -hh ; j <= hh ; j++) 1078 // for (i = -hw ; i <= hw ; i++) { 1079 // mi = Axx * i + Axy * j; 1080 // mj = Ayx * i + Ayy * j; 1081 // *out_gradx++ = _interpolate(x+mi, y+mj, in_gradx); 1082 // *out_grady++ = _interpolate(x+mi, y+mj, in_grady); 1083 // } 1084 // 1085 // } 1086 // 1087 // /********************************************************************* 1088 // * _computeAffineMappedImage 1089 // * used only for DEBUG output 1090 // * 1091 // */ 1092 // 1093 // static void _am_computeAffineMappedImage( 1094 // FImage img, /* images */ 1095 // float x, float y, /* center of window */ 1096 // float Axx, float Ayx , float Axy, float Ayy, /* affine mapping */ 1097 // int width, int height, /* size of window */ 1098 // _FloatWindow imgdiff) /* output */ 1099 // { 1100 // int hw = width/2, hh = height/2; 1101 // int i, j; 1102 // float mi, mj; 1103 // 1104 // /* Compute values */ 1105 // for (j = -hh ; j <= hh ; j++) 1106 // for (i = -hw ; i <= hw ; i++) { 1107 // mi = Axx * i + Axy * j; 1108 // mj = Ayx * i + Ayy * j; 1109 // *imgdiff++ = _interpolate(x+mi, y+mj, img); 1110 // } 1111 // } 1112 // 1113 // 1114 // /********************************************************************* 1115 // * _getSubFloatImage 1116 // */ 1117 // 1118 // static void _am_getSubFloatImage( 1119 // FImage img, /* image */ 1120 // float x, float y, /* center of window */ 1121 // FImage window) /* output */ 1122 // { 1123 // int hw = window.ncols/2, hh = window.nrows/2; 1124 // int x0 = (int) x; 1125 // int y0 = (int) y; 1126 // float * windata = window.data; 1127 // int offset; 1128 // int i, j; 1129 // 1130 // assert(x0 - hw >= 0); 1131 // assert(y0 - hh >= 0); 1132 // assert(x0 + hw <= img.ncols); 1133 // assert(y0 + hh <= img.nrows); 1134 // 1135 // /* copy values */ 1136 // for (j = -hh ; j <= hh ; j++) 1137 // for (i = -hw ; i <= hw ; i++) { 1138 // offset = (j+y0)*img.ncols + (i+x0); 1139 // *windata++ = *(img.data+offset); 1140 // } 1141 // } 1142 // 1143 // /********************************************************************* 1144 // * _am_computeIntensityDifferenceAffine 1145 // * 1146 // * Given two images and the window center in both images, 1147 // * aligns the images with the window and computes the difference 1148 // * between the two overlaid images using the affine mapping. 1149 // * A = [ Axx Axy] 1150 // * [ Ayx Ayy] 1151 // */ 1152 // 1153 // static void _am_computeIntensityDifferenceAffine( 1154 // FImage img1, /* images */ 1155 // FImage img2, 1156 // float x1, float y1, /* center of window in 1st img */ 1157 // float x2, float y2, /* center of window in 2nd img */ 1158 // float Axx, float Ayx , float Axy, float Ayy, /* affine mapping */ 1159 // int width, int height, /* size of window */ 1160 // _FloatWindow imgdiff) /* output */ 1161 // { 1162 // int hw = width/2, hh = height/2; 1163 // float g1, g2; 1164 // int i, j; 1165 // float mi, mj; 1166 // 1167 // /* Compute values */ 1168 // for (j = -hh ; j <= hh ; j++) 1169 // for (i = -hw ; i <= hw ; i++) { 1170 // g1 = _interpolate(x1+i, y1+j, img1); 1171 // mi = Axx * i + Axy * j; 1172 // mj = Ayx * i + Ayy * j; 1173 // g2 = _interpolate(x2+mi, y2+mj, img2); 1174 // *imgdiff++ = g1 - g2; 1175 // } 1176 // } 1177 // 1178 // /********************************************************************* 1179 // * _am_compute6by6GradientMatrix 1180 // * 1181 // */ 1182 // 1183 // static void _am_compute6by6GradientMatrix( 1184 // _FloatWindow gradx, 1185 // _FloatWindow grady, 1186 // int width, /* size of window */ 1187 // int height, 1188 // float **T) /* return values */ 1189 // { 1190 // int hw = width/2, hh = height/2; 1191 // int i, j; 1192 // float gx, gy, gxx, gxy, gyy, x, y, xx, xy, yy; 1193 // 1194 // 1195 // /* Set values to zero */ 1196 // for (j = 0 ; j < 6 ; j++) { 1197 // for (i = j ; i < 6 ; i++) { 1198 // T[j][i] = 0.0; 1199 // } 1200 // } 1201 // 1202 // for (j = -hh ; j <= hh ; j++) { 1203 // for (i = -hw ; i <= hw ; i++) { 1204 // gx = *gradx++; 1205 // gy = *grady++; 1206 // gxx = gx * gx; 1207 // gxy = gx * gy; 1208 // gyy = gy * gy; 1209 // x = (float) i; 1210 // y = (float) j; 1211 // xx = x * x; 1212 // xy = x * y; 1213 // yy = y * y; 1214 // 1215 // T[0][0] += xx * gxx; 1216 // T[0][1] += xx * gxy; 1217 // T[0][2] += xy * gxx; 1218 // T[0][3] += xy * gxy; 1219 // T[0][4] += x * gxx; 1220 // T[0][5] += x * gxy; 1221 // 1222 // T[1][1] += xx * gyy; 1223 // T[1][2] += xy * gxy; 1224 // T[1][3] += xy * gyy; 1225 // T[1][4] += x * gxy; 1226 // T[1][5] += x * gyy; 1227 // 1228 // T[2][2] += yy * gxx; 1229 // T[2][3] += yy * gxy; 1230 // T[2][4] += y * gxx; 1231 // T[2][5] += y * gxy; 1232 // 1233 // T[3][3] += yy * gyy; 1234 // T[3][4] += y * gxy; 1235 // T[3][5] += y * gyy; 1236 // 1237 // T[4][4] += gxx; 1238 // T[4][5] += gxy; 1239 // 1240 // T[5][5] += gyy; 1241 // } 1242 // } 1243 // 1244 // for (j = 0 ; j < 5 ; j++) { 1245 // for (i = j+1 ; i < 6 ; i++) { 1246 // T[i][j] = T[j][i]; 1247 // } 1248 // } 1249 // 1250 // } 1251 // 1252 // 1253 // 1254 // /********************************************************************* 1255 // * _am_compute6by1ErrorVector 1256 // * 1257 // */ 1258 // 1259 // static void _am_compute6by1ErrorVector( 1260 // _FloatWindow imgdiff, 1261 // _FloatWindow gradx, 1262 // _FloatWindow grady, 1263 // int width, /* size of window */ 1264 // int height, 1265 // float **e) /* return values */ 1266 // { 1267 // int hw = width/2, hh = height/2; 1268 // int i, j; 1269 // float diff, diffgradx, diffgrady; 1270 // 1271 // /* Set values to zero */ 1272 // for(i = 0; i < 6; i++) e[i][0] = 0.0; 1273 // 1274 // /* Compute values */ 1275 // for (j = -hh ; j <= hh ; j++) { 1276 // for (i = -hw ; i <= hw ; i++) { 1277 // diff = *imgdiff++; 1278 // diffgradx = diff * (*gradx++); 1279 // diffgrady = diff * (*grady++); 1280 // e[0][0] += diffgradx * i; 1281 // e[1][0] += diffgrady * i; 1282 // e[2][0] += diffgradx * j; 1283 // e[3][0] += diffgrady * j; 1284 // e[4][0] += diffgradx; 1285 // e[5][0] += diffgrady; 1286 // } 1287 // } 1288 // 1289 // for(i = 0; i < 6; i++) e[i][0] *= 0.5; 1290 // 1291 // } 1292 // 1293 // 1294 // /********************************************************************* 1295 // * _am_compute4by4GradientMatrix 1296 // * 1297 // */ 1298 // 1299 // static void _am_compute4by4GradientMatrix( 1300 // _FloatWindow gradx, 1301 // _FloatWindow grady, 1302 // int width, /* size of window */ 1303 // int height, 1304 // float **T) /* return values */ 1305 // { 1306 // int hw = width/2, hh = height/2; 1307 // int i, j; 1308 // float gx, gy, x, y; 1309 // 1310 // 1311 // /* Set values to zero */ 1312 // for (j = 0 ; j < 4 ; j++) { 1313 // for (i = 0 ; i < 4 ; i++) { 1314 // T[j][i] = 0.0; 1315 // } 1316 // } 1317 // 1318 // for (j = -hh ; j <= hh ; j++) { 1319 // for (i = -hw ; i <= hw ; i++) { 1320 // gx = *gradx++; 1321 // gy = *grady++; 1322 // x = (float) i; 1323 // y = (float) j; 1324 // T[0][0] += (x*gx+y*gy) * (x*gx+y*gy); 1325 // T[0][1] += (x*gx+y*gy)*(x*gy-y*gx); 1326 // T[0][2] += (x*gx+y*gy)*gx; 1327 // T[0][3] += (x*gx+y*gy)*gy; 1328 // 1329 // T[1][1] += (x*gy-y*gx) * (x*gy-y*gx); 1330 // T[1][2] += (x*gy-y*gx)*gx; 1331 // T[1][3] += (x*gy-y*gx)*gy; 1332 // 1333 // T[2][2] += gx*gx; 1334 // T[2][3] += gx*gy; 1335 // 1336 // T[3][3] += gy*gy; 1337 // } 1338 // } 1339 // 1340 // for (j = 0 ; j < 3 ; j++) { 1341 // for (i = j+1 ; i < 4 ; i++) { 1342 // T[i][j] = T[j][i]; 1343 // } 1344 // } 1345 // 1346 // } 1347 // 1348 // /********************************************************************* 1349 // * _am_compute4by1ErrorVector 1350 // * 1351 // */ 1352 // 1353 // static void _am_compute4by1ErrorVector( 1354 // _FloatWindow imgdiff, 1355 // _FloatWindow gradx, 1356 // _FloatWindow grady, 1357 // int width, /* size of window */ 1358 // int height, 1359 // float **e) /* return values */ 1360 // { 1361 // int hw = width/2, hh = height/2; 1362 // int i, j; 1363 // float diff, diffgradx, diffgrady; 1364 // 1365 // /* Set values to zero */ 1366 // for(i = 0; i < 4; i++) e[i][0] = 0.0; 1367 // 1368 // /* Compute values */ 1369 // for (j = -hh ; j <= hh ; j++) { 1370 // for (i = -hw ; i <= hw ; i++) { 1371 // diff = *imgdiff++; 1372 // diffgradx = diff * (*gradx++); 1373 // diffgrady = diff * (*grady++); 1374 // e[0][0] += diffgradx * i + diffgrady * j; 1375 // e[1][0] += diffgrady * i - diffgradx * j; 1376 // e[2][0] += diffgradx; 1377 // e[3][0] += diffgrady; 1378 // } 1379 // } 1380 // 1381 // for(i = 0; i < 4; i++) e[i][0] *= 0.5; 1382 // 1383 // } 1384 // 1385 // 1386 // 1387 // /********************************************************************* 1388 // * _am_trackFeatureAffine 1389 // * 1390 // * Tracks a feature point from the image of first occurrence to the actual 1391 // image. 1392 // * 1393 // * RETURNS 1394 // * KLT_SMALL_DET or KLT_LARGE_RESIDUE or KLT_OOB if feature is lost, 1395 // * KLT_TRACKED otherwise. 1396 // */ 1397 // 1398 // /* if you enalbe the DEBUG_AFFINE_MAPPING make sure you have created a 1399 // directory "./debug" */ 1400 // /* #define DEBUG_AFFINE_MAPPING */ 1401 // 1402 // #ifdef DEBUG_AFFINE_MAPPING 1403 // static int counter = 0; 1404 // static int glob_index = 0; 1405 // #endif 1406 // 1407 // static int _am_trackFeatureAffine( 1408 // float x1, /* location of window in first image */ 1409 // float y1, 1410 // float *x2, /* starting location of search in second image */ 1411 // float *y2, 1412 // FImage img1, 1413 // FImage gradx1, 1414 // FImage grady1, 1415 // FImage img2, 1416 // FImage gradx2, 1417 // FImage grady2, 1418 // int width, /* size of window */ 1419 // int height, 1420 // float step_factor, /* 2.0 comes from equations, 1.0 seems to avoid 1421 // overshooting */ 1422 // int max_iterations, 1423 // float small, /* determinant threshold for declaring KLT_SMALL_DET */ 1424 // float th, /* displacement threshold for stopping */ 1425 // float th_aff, 1426 // float max_residue, /* residue threshold for declaring KLT_LARGE_RESIDUE 1427 // */ 1428 // int lighting_insensitive, /* whether to normalize for gain and bias */ 1429 // int affine_map, /* whether to evaluates the consistency of features with 1430 // affine mapping */ 1431 // float mdd, /* difference between the displacements */ 1432 // float *Axx, float *Ayx, 1433 // float *Axy, float *Ayy) /* used affine mapping */ 1434 // { 1435 // 1436 // 1437 // _FloatWindow imgdiff, gradx, grady; 1438 // float gxx, gxy, gyy, ex, ey, dx, dy; 1439 // int iteration = 0; 1440 // int status = 0; 1441 // int hw = width/2; 1442 // int hh = height/2; 1443 // int nc1 = img1.ncols; 1444 // int nr1 = img1.nrows; 1445 // int nc2 = img2.ncols; 1446 // int nr2 = img2.nrows; 1447 // float **a; 1448 // float **T; 1449 // float one_plus_eps = 1.001f; /* To prevent rounding errors */ 1450 // float old_x2 = *x2; 1451 // float old_y2 = *y2; 1452 // boolean convergence = false; 1453 // 1454 // #ifdef DEBUG_AFFINE_MAPPING 1455 // char fname[80]; 1456 // FImage aff_diff_win = _KLTCreateFloatImage(width,height); 1457 // printf("starting location x2=%f y2=%f\n", *x2, *y2); 1458 // #endif 1459 // 1460 // /* Allocate memory for windows */ 1461 // imgdiff = new float[height * width]; 1462 // gradx = new float[height * width]; 1463 // grady = new float[height * width]; 1464 // T = _am_matrix(6,6); 1465 // a = _am_matrix(6,1); 1466 // 1467 // /* Iteratively update the window position */ 1468 // do { 1469 // if(!affine_map) { 1470 // /* pure translation tracker */ 1471 // 1472 // /* If out of bounds, exit loop */ 1473 // if ( x1-hw < 0.0f || nc1-( x1+hw) < one_plus_eps || 1474 // *x2-hw < 0.0f || nc2-(*x2+hw) < one_plus_eps || 1475 // y1-hh < 0.0f || nr1-( y1+hh) < one_plus_eps || 1476 // *y2-hh < 0.0f || nr2-(*y2+hh) < one_plus_eps) { 1477 // status = KLT_OOB; 1478 // break; 1479 // } 1480 // 1481 // /* Compute gradient and difference windows */ 1482 // if (lighting_insensitive) { 1483 // _computeIntensityDifferenceLightingInsensitive(img1, img2, x1, y1, *x2, 1484 // *y2, 1485 // width, height, imgdiff); 1486 // _computeGradientSumLightingInsensitive(gradx1, grady1, gradx2, grady2, 1487 // img1, img2, x1, y1, *x2, *y2, width, height, gradx, grady); 1488 // } else { 1489 // _computeIntensityDifference(img1, img2, x1, y1, *x2, *y2, 1490 // width, height, imgdiff); 1491 // _computeGradientSum(gradx1, grady1, gradx2, grady2, 1492 // x1, y1, *x2, *y2, width, height, gradx, grady); 1493 // } 1494 // 1495 // #ifdef DEBUG_AFFINE_MAPPING 1496 // aff_diff_win.data = imgdiff; 1497 // fname = String.format("./debug/kltimg_trans_diff_win%03d.%03d.pgm", 1498 // glob_index, counter); 1499 // printf("%s\n", fname); 1500 // _KLTWriteAbsFloatImageToPGM(aff_diff_win, fname,256.0); 1501 // printf("iter = %d translation tracker res: %f\n", iteration, 1502 // _sumAbsFloatWindow(imgdiff, width, height)/(width*height)); 1503 // #endif 1504 // 1505 // /* Use these windows to construct matrices */ 1506 // _compute2by2GradientMatrix(gradx, grady, width, height, 1507 // &gxx, &gxy, &gyy); 1508 // _compute2by1ErrorVector(imgdiff, gradx, grady, width, height, 1509 // step_factor, 1510 // &ex, &ey); 1511 // 1512 // /* Using matrices, solve equation for new displacement */ 1513 // status = _solveEquation(gxx, gxy, gyy, ex, ey, small, &dx, &dy); 1514 // 1515 // convergence = (fabs(dx) < th && fabs(dy) < th); 1516 // 1517 // *x2 += dx; 1518 // *y2 += dy; 1519 // 1520 // }else{ 1521 // /* affine tracker */ 1522 // 1523 // float ul_x = *Axx * (-hw) + *Axy * hh + *x2; /* upper left corner */ 1524 // float ul_y = *Ayx * (-hw) + *Ayy * hh + *y2; 1525 // float ll_x = *Axx * (-hw) + *Axy * (-hh) + *x2; /* lower left corner */ 1526 // float ll_y = *Ayx * (-hw) + *Ayy * (-hh) + *y2; 1527 // float ur_x = *Axx * hw + *Axy * hh + *x2; /* upper right corner */ 1528 // float ur_y = *Ayx * hw + *Ayy * hh + *y2; 1529 // float lr_x = *Axx * hw + *Axy * (-hh) + *x2; /* lower right corner */ 1530 // float lr_y = *Ayx * hw + *Ayy * (-hh) + *y2; 1531 // 1532 // /* If out of bounds, exit loop */ 1533 // if ( x1-hw < 0.0f || nc1-(x1+hw) < one_plus_eps || 1534 // y1-hh < 0.0f || nr1-(y1+hh) < one_plus_eps || 1535 // ul_x < 0.0f || nc2-(ul_x ) < one_plus_eps || 1536 // ll_x < 0.0f || nc2-(ll_x ) < one_plus_eps || 1537 // ur_x < 0.0f || nc2-(ur_x ) < one_plus_eps || 1538 // lr_x < 0.0f || nc2-(lr_x ) < one_plus_eps || 1539 // ul_y < 0.0f || nr2-(ul_y ) < one_plus_eps || 1540 // ll_y < 0.0f || nr2-(ll_y ) < one_plus_eps || 1541 // ur_y < 0.0f || nr2-(ur_y ) < one_plus_eps || 1542 // lr_y < 0.0f || nr2-(lr_y ) < one_plus_eps) { 1543 // status = KLT_OOB; 1544 // break; 1545 // } 1546 // 1547 // #ifdef DEBUG_AFFINE_MAPPING 1548 // counter++; 1549 // _am_computeAffineMappedImage(img1, x1, y1, 1.0, 0.0 , 0.0, 1.0, width, 1550 // height, imgdiff); 1551 // aff_diff_win.data = imgdiff; 1552 // fname = String.format("./debug/kltimg_aff_diff_win%03d.%03d_1.pgm", 1553 // glob_index, counter); 1554 // printf("%s\n", fname); 1555 // _KLTWriteAbsFloatImageToPGM(aff_diff_win, fname,256.0); 1556 // 1557 // _am_computeAffineMappedImage(img2, *x2, *y2, *Axx, *Ayx , *Axy, *Ayy, 1558 // width, height, imgdiff); 1559 // aff_diff_win.data = imgdiff; 1560 // fname = String.format("./debug/kltimg_aff_diff_win%03d.%03d_2.pgm", 1561 // glob_index, counter); 1562 // printf("%s\n", fname); 1563 // _KLTWriteAbsFloatImageToPGM(aff_diff_win, fname,256.0); 1564 // #endif 1565 // 1566 // _am_computeIntensityDifferenceAffine(img1, img2, x1, y1, *x2, *y2, *Axx, 1567 // *Ayx , *Axy, *Ayy, 1568 // width, height, imgdiff); 1569 // #ifdef DEBUG_AFFINE_MAPPING 1570 // aff_diff_win.data = imgdiff; 1571 // fname = String.format("./debug/kltimg_aff_diff_win%03d.%03d_3.pgm", 1572 // glob_index,counter); 1573 // printf("%s\n", fname); 1574 // _KLTWriteAbsFloatImageToPGM(aff_diff_win, fname,256.0); 1575 // 1576 // printf("iter = %d affine tracker res: %f\n", iteration, 1577 // _sumAbsFloatWindow(imgdiff, width, height)/(width*height)); 1578 // #endif 1579 // 1580 // _am_getGradientWinAffine(gradx2, grady2, *x2, *y2, *Axx, *Ayx , *Axy, 1581 // *Ayy, 1582 // width, height, gradx, grady); 1583 // 1584 // switch(affine_map){ 1585 // case 1: 1586 // _am_compute4by1ErrorVector(imgdiff, gradx, grady, width, height, a); 1587 // _am_compute4by4GradientMatrix(gradx, grady, width, height, T); 1588 // 1589 // status = _am_gauss_jordan_elimination(T,4,a,1); 1590 // 1591 // *Axx += a[0][0]; 1592 // *Ayx += a[1][0]; 1593 // *Ayy = *Axx; 1594 // *Axy = -(*Ayx); 1595 // 1596 // dx = a[2][0]; 1597 // dy = a[3][0]; 1598 // 1599 // break; 1600 // case 2: 1601 // _am_compute6by1ErrorVector(imgdiff, gradx, grady, width, height, a); 1602 // _am_compute6by6GradientMatrix(gradx, grady, width, height, T); 1603 // 1604 // status = _am_gauss_jordan_elimination(T,6,a,1); 1605 // 1606 // *Axx += a[0][0]; 1607 // *Ayx += a[1][0]; 1608 // *Axy += a[2][0]; 1609 // *Ayy += a[3][0]; 1610 // 1611 // dx = a[4][0]; 1612 // dy = a[5][0]; 1613 // 1614 // break; 1615 // } 1616 // 1617 // *x2 += dx; 1618 // *y2 += dy; 1619 // 1620 // /* old upper left corner - new upper left corner */ 1621 // ul_x -= *Axx * (-hw) + *Axy * hh + *x2; 1622 // ul_y -= *Ayx * (-hw) + *Ayy * hh + *y2; 1623 // /* old lower left corner - new lower left corner */ 1624 // ll_x -= *Axx * (-hw) + *Axy * (-hh) + *x2; 1625 // ll_y -= *Ayx * (-hw) + *Ayy * (-hh) + *y2; 1626 // /* old upper right corner - new upper right corner */ 1627 // ur_x -= *Axx * hw + *Axy * hh + *x2; 1628 // ur_y -= *Ayx * hw + *Ayy * hh + *y2; 1629 // /* old lower right corner - new lower right corner */ 1630 // lr_x -= *Axx * hw + *Axy * (-hh) + *x2; 1631 // lr_y -= *Ayx * hw + *Ayy * (-hh) + *y2; 1632 // 1633 // #ifdef DEBUG_AFFINE_MAPPING 1634 // printf 1635 // ("iter = %d, ul_x=%f ul_y=%f ll_x=%f ll_y=%f ur_x=%f ur_y=%f lr_x=%f lr_y=%f \n", 1636 // iteration, ul_x, ul_y, ll_x, ll_y, ur_x, ur_y, lr_x, lr_y); 1637 // #endif 1638 // 1639 // convergence = (fabs(dx) < th && fabs(dy) < th && 1640 // fabs(ul_x) < th_aff && fabs(ul_y) < th_aff && 1641 // fabs(ll_x) < th_aff && fabs(ll_y) < th_aff && 1642 // fabs(ur_x) < th_aff && fabs(ur_y) < th_aff && 1643 // fabs(lr_x) < th_aff && fabs(lr_y) < th_aff); 1644 // } 1645 // 1646 // if (status == KLT_SMALL_DET) break; 1647 // iteration++; 1648 // #ifdef DEBUG_AFFINE_MAPPING 1649 // printf 1650 // ("iter = %d, x1=%f, y1=%f, x2=%f, y2=%f, Axx=%f, Ayx=%f , Axy=%f, Ayy=%f \n",iteration, 1651 // x1, y1, *x2, *y2, *Axx, *Ayx , *Axy, *Ayy); 1652 // #endif 1653 // } while ( !convergence && iteration < max_iterations); 1654 // /*} while ( (fabs(dx)>=th || fabs(dy)>=th || (affine_map && iteration < 1655 // 8) ) && iteration < max_iterations); */ 1656 // _am_free_matrix(T); 1657 // _am_free_matrix(a); 1658 // 1659 // /* Check whether window is out of bounds */ 1660 // if (*x2-hw < 0.0f || nc2-(*x2+hw) < one_plus_eps || 1661 // *y2-hh < 0.0f || nr2-(*y2+hh) < one_plus_eps) 1662 // status = KLT_OOB; 1663 // 1664 // /* Check whether feature point has moved to much during iteration*/ 1665 // if ( (*x2-old_x2) > mdd || (*y2-old_y2) > mdd ) 1666 // status = KLT_OOB; 1667 // 1668 // /* Check whether residue is too large */ 1669 // if (status == KLT_TRACKED) { 1670 // if(!affine_map){ 1671 // _computeIntensityDifference(img1, img2, x1, y1, *x2, *y2, 1672 // width, height, imgdiff); 1673 // }else{ 1674 // _am_computeIntensityDifferenceAffine(img1, img2, x1, y1, *x2, *y2, *Axx, 1675 // *Ayx , *Axy, *Ayy, 1676 // width, height, imgdiff); 1677 // } 1678 // #ifdef DEBUG_AFFINE_MAPPING 1679 // printf("iter = %d final_res = %f\n", iteration, 1680 // _sumAbsFloatWindow(imgdiff, width, height)/(width*height)); 1681 // #endif 1682 // if (_sumAbsFloatWindow(imgdiff, width, height)/(width*height) > 1683 // max_residue) 1684 // status = KLT_LARGE_RESIDUE; 1685 // } 1686 // 1687 // /* Free memory */ 1688 // free(imgdiff); free(gradx); free(grady); 1689 // 1690 // #ifdef DEBUG_AFFINE_MAPPING 1691 // printf("iter = %d status=%d\n", iteration, status); 1692 // _KLTFreeFloatImage( aff_diff_win ); 1693 // #endif 1694 // 1695 // /* Return appropriate value */ 1696 // return status; 1697 // } 1698 // 1699 // /* 1700 // * CONSISTENCY CHECK OF FEATURES BY AFFINE MAPPING (END) 1701 // **********************************************************************/ 1702 // 1703 // 1704 // 1705 /********************************************************************* 1706 * KLTTrackFeatures 1707 * 1708 * Tracks feature points from one image to the next. 1709 * 1710 * @param img1 1711 * @param img2 1712 */ 1713 public void trackFeatures(FImage img1, FImage img2) 1714 { 1715 if (isNorm) { 1716 img1 = img1.multiply(255f); 1717 img2 = img2.multiply(255f); 1718 } 1719 PyramidSet pyr1, pyr2; 1720 int i; 1721 final int nrows = img1.height, ncols = img1.width; 1722 1723 if (KLT_verbose >= 1) { 1724 System.out.println(String.format("(KLT) Tracking %d features in a %d by %d image... ", 1725 featurelist.countRemainingFeatures(), ncols, nrows)); 1726 } 1727 1728 /* Check window size (and correct if necessary) */ 1729 if (tc.window_width % 2 != 1) { 1730 tc.window_width = tc.window_width + 1; 1731 System.out.println(String.format("Tracking context's window width must be odd. Changing to %d.\n", 1732 tc.window_width)); 1733 } 1734 if (tc.window_height % 2 != 1) { 1735 tc.window_height = tc.window_height + 1; 1736 System.out.println(String.format("Tracking context's window height must be odd. Changing to %d.\n", 1737 tc.window_height)); 1738 } 1739 if (tc.window_width < 3) { 1740 tc.window_width = 3; 1741 System.out.println(String.format( 1742 "Tracking context's window width must be at least three. \nChanging to %d.\n", tc.window_width)); 1743 } 1744 if (tc.window_height < 3) { 1745 tc.window_height = 3; 1746 System.out.println(String.format( 1747 "Tracking context's window height must be at least three. \nChanging to %d.\n", tc.window_height)); 1748 } 1749 1750 /* Process first image by converting to float, smoothing, computing */ 1751 /* pyramid, and computing gradient pyramids */ 1752 final PyramidSet ppSet = tc.previousPyramidSet(); 1753 if (tc.sequentialMode && ppSet != null) { 1754 Pyramid pyramid1, pyramid1_gradx, pyramid1_grady; 1755 pyramid1 = ppSet.imgPyr; 1756 pyramid1_gradx = ppSet.gradx; 1757 pyramid1_grady = ppSet.grady; 1758 if (pyramid1.ncols[0] != ncols || pyramid1.nrows[0] != nrows) 1759 throw new RuntimeException( 1760 String.format("(KLTTrackFeatures) Size of incoming image (%d by %d) is different from size " + 1761 "of previous image (%d by %d)\n", ncols, nrows, pyramid1.ncols[0], pyramid1.nrows[0])); 1762 assert (pyramid1_gradx != null); 1763 assert (pyramid1_grady != null); 1764 pyr1 = ppSet; 1765 } else { 1766 pyr1 = new PyramidSet(img1, tc); 1767 1768 } 1769 1770 /* Do the same thing with second image */ 1771 pyr2 = new PyramidSet(img2, tc); 1772 1773 /* Write internal images */ 1774 if (tc.writeInternalImages) { 1775 String fname; 1776 for (i = 0; i < tc.nPyramidLevels; i++) { 1777 try { 1778 fname = String.format("kltimg_tf_i%d.png", i); 1779 ImageUtilities.write(pyr1.imgPyr.img[i], "png", new File(fname)); 1780 fname = String.format("kltimg_tf_i%d_gx.png", i); 1781 ImageUtilities.write(pyr1.gradx.img[i], "png", new File(fname)); 1782 fname = String.format("kltimg_tf_i%d_gy.png", i); 1783 ImageUtilities.write(pyr1.grady.img[i], "png", new File(fname)); 1784 fname = String.format("kltimg_tf_j%d.png", i); 1785 ImageUtilities.write(pyr2.imgPyr.img[i], "png", new File(fname)); 1786 fname = String.format("kltimg_tf_j%d_gx.png", i); 1787 ImageUtilities.write(pyr2.gradx.img[i], "png", new File(fname)); 1788 fname = String.format("kltimg_tf_j%d_gy.png", i); 1789 ImageUtilities.write(pyr2.grady.img[i], "png", new File(fname)); 1790 } catch (final IOException e) { 1791 1792 } 1793 } 1794 } 1795 1796 trackFeatures(img1, img2, pyr1, pyr2); 1797 1798 if (tc.sequentialMode) { 1799 tc.setPreviousPyramid(pyr2); 1800 } 1801 1802 if (KLT_verbose >= 1) { 1803 System.err.println(String.format("\n\t%d features successfully tracked.\n", 1804 featurelist.countRemainingFeatures())); 1805 if (tc.writeInternalImages) 1806 System.err.println("\tWrote images to 'kltimg_tf*.pgm'.\n"); 1807 } 1808 } 1809 1810 /** 1811 * KLTTrackFeatures 1812 * 1813 * Tracks feature points from one image to the next. Also takes in image 1814 * pyramids and gradient pyramids for both images. 1815 * 1816 * @param img1 1817 * @param img2 1818 * @param pyr1 1819 * @param pyr2 1820 */ 1821 public void trackFeatures(FImage img1, FImage img2, PyramidSet pyr1, PyramidSet pyr2) { 1822 float xloc, yloc, xlocout, ylocout; 1823 int val = -1; 1824 int indx, r; 1825 final float subsampling = tc.subsampling; 1826 final int nrows = img1.height, ncols = img1.width; 1827 /* For each feature, do ... */ 1828 for (indx = 0; indx < featurelist.features.length; indx++) { 1829 1830 /* Only track features that are not lost */ 1831 if (featurelist.features[indx].val >= 0) { 1832 1833 xloc = featurelist.features[indx].x; 1834 yloc = featurelist.features[indx].y; 1835 1836 /* Transform location to coarsest resolution */ 1837 for (r = tc.nPyramidLevels - 1; r >= 0; r--) { 1838 xloc /= subsampling; 1839 yloc /= subsampling; 1840 } 1841 xlocout = xloc; 1842 ylocout = yloc; 1843 1844 /* Beginning with coarsest resolution, do ... */ 1845 for (r = tc.nPyramidLevels - 1; r >= 0; r--) { 1846 1847 /* Track feature at current resolution */ 1848 xloc *= subsampling; 1849 yloc *= subsampling; 1850 xlocout *= subsampling; 1851 ylocout *= subsampling; 1852 1853 final float[] xylocout = new float[2]; 1854 xylocout[0] = xlocout; 1855 xylocout[1] = ylocout; 1856 1857 val = _trackFeature(xloc, yloc, 1858 xylocout, 1859 pyr1.imgPyr.img[r], 1860 pyr1.gradx.img[r], pyr1.grady.img[r], 1861 pyr2.imgPyr.img[r], 1862 pyr2.gradx.img[r], pyr2.grady.img[r], 1863 tc.window_width, tc.window_height, 1864 tc.step_factor, 1865 tc.max_iterations, 1866 tc.min_determinant, 1867 tc.min_displacement, 1868 tc.max_residue, 1869 tc.lighting_insensitive); 1870 1871 xlocout = xylocout[0]; 1872 ylocout = xylocout[1]; 1873 1874 if (val == KLT_SMALL_DET || val == KLT_OOB) 1875 break; 1876 } 1877 1878 /* Record feature */ 1879 if (val == KLT_OOB) { 1880 featurelist.features[indx].x = -1.0f; 1881 featurelist.features[indx].y = -1.0f; 1882 featurelist.features[indx].val = KLT_OOB; 1883 1884 // featurelist.features[indx].aff_img = null; 1885 // featurelist.features[indx].aff_img_gradx = null; 1886 // featurelist.features[indx].aff_img_grady = null; 1887 1888 } else if (_outOfBounds(xlocout, ylocout, ncols, nrows, tc.borderx, tc.bordery)) { 1889 featurelist.features[indx].x = -1.0f; 1890 featurelist.features[indx].y = -1.0f; 1891 featurelist.features[indx].val = KLT_OOB; 1892 1893 // featurelist.features[indx].aff_img = null; 1894 // featurelist.features[indx].aff_img_gradx = null; 1895 // featurelist.features[indx].aff_img_grady = null; 1896 } else if (val == KLT_SMALL_DET) { 1897 featurelist.features[indx].x = -1.0f; 1898 featurelist.features[indx].y = -1.0f; 1899 featurelist.features[indx].val = KLT_SMALL_DET; 1900 1901 // featurelist.features[indx].aff_img = null; 1902 // featurelist.features[indx].aff_img_gradx = null; 1903 // featurelist.features[indx].aff_img_grady = null; 1904 } else if (val == KLT_LARGE_RESIDUE) { 1905 featurelist.features[indx].x = -1.0f; 1906 featurelist.features[indx].y = -1.0f; 1907 featurelist.features[indx].val = KLT_LARGE_RESIDUE; 1908 1909 // featurelist.features[indx].aff_img = null; 1910 // featurelist.features[indx].aff_img_gradx = null; 1911 // featurelist.features[indx].aff_img_grady = null; 1912 } else if (val == KLT_MAX_ITERATIONS) { 1913 featurelist.features[indx].x = -1.0f; 1914 featurelist.features[indx].y = -1.0f; 1915 featurelist.features[indx].val = KLT_MAX_ITERATIONS; 1916 1917 // featurelist.features[indx].aff_img = null; 1918 // featurelist.features[indx].aff_img_gradx = null; 1919 // featurelist.features[indx].aff_img_grady = null; 1920 } else { 1921 featurelist.features[indx].x = xlocout; 1922 featurelist.features[indx].y = ylocout; 1923 featurelist.features[indx].val = KLT_TRACKED; 1924 if (tc.affineConsistencyCheck >= 0 && val == KLT_TRACKED) { /* 1925 * for 1926 * affine 1927 * mapping 1928 */ 1929 throw new UnsupportedOperationException("Affine mapping not yet implemented"); 1930 // int border = 2; /* add border for interpolation */ 1931 // 1932 // if(featurelist.features[indx].aff_img == null){ 1933 // /* save image and gradient for each feature at finest 1934 // resolution after first successful track */ 1935 // featurelist.features[indx].aff_img = new 1936 // FImage((tc.affine_window_height+border), 1937 // (tc.affine_window_width+border)); 1938 // featurelist.features[indx].aff_img_gradx = new 1939 // FImage((tc.affine_window_height+border), 1940 // (tc.affine_window_width+border)); 1941 // featurelist.features[indx].aff_img_grady = new 1942 // FImage((tc.affine_window_height+border), 1943 // (tc.affine_window_width+border)); 1944 // _am_getSubFloatImage(pyramid1.img[0],xloc,yloc,featurelist.features[indx].aff_img); 1945 // _am_getSubFloatImage(pyramid1_gradx.img[0],xloc,yloc,featurelist.features[indx].aff_img_gradx); 1946 // _am_getSubFloatImage(pyramid1_grady.img[0],xloc,yloc,featurelist.features[indx].aff_img_grady); 1947 // featurelist.features[indx].aff_x = xloc - (int) xloc 1948 // + (tc.affine_window_width+border)/2; 1949 // featurelist.features[indx].aff_y = yloc - (int) yloc 1950 // + (tc.affine_window_height+border)/2;; 1951 // }else{ 1952 // /* affine tracking */ 1953 // val = 1954 // _am_trackFeatureAffine(featurelist.features[indx].aff_x, 1955 // featurelist.features[indx].aff_y, 1956 // &xlocout, &ylocout, 1957 // featurelist.features[indx].aff_img, 1958 // featurelist.features[indx].aff_img_gradx, 1959 // featurelist.features[indx].aff_img_grady, 1960 // pyramid2.img[0], 1961 // pyramid2_gradx.img[0], pyramid2_grady.img[0], 1962 // tc.affine_window_width, tc.affine_window_height, 1963 // tc.step_factor, 1964 // tc.affine_max_iterations, 1965 // tc.min_determinant, 1966 // tc.min_displacement, 1967 // tc.affine_min_displacement, 1968 // tc.affine_max_residue, 1969 // tc.lighting_insensitive, 1970 // tc.affineConsistencyCheck, 1971 // tc.affine_max_displacement_differ, 1972 // &featurelist.features[indx].aff_Axx, 1973 // &featurelist.features[indx].aff_Ayx, 1974 // &featurelist.features[indx].aff_Axy, 1975 // &featurelist.features[indx].aff_Ayy 1976 // ); 1977 // featurelist.features[indx].val = val; 1978 // if(val != KLT_TRACKED){ 1979 // featurelist.features[indx].x = -1.0f; 1980 // featurelist.features[indx].y = -1.0f; 1981 // featurelist.features[indx].aff_x = -1.0f; 1982 // featurelist.features[indx].aff_y = -1.0f; 1983 // 1984 // featurelist.features[indx].aff_img = null; 1985 // featurelist.features[indx].aff_img_gradx = null; 1986 // featurelist.features[indx].aff_img_grady = null; 1987 // }else{ 1988 // /*featurelist.features[indx].x = xlocout;*/ 1989 // /*featurelist.features[indx].y = ylocout;*/ 1990 // } 1991 // } 1992 } 1993 1994 } 1995 } 1996 } 1997 } 1998 1999 /** 2000 * @return the tracking context 2001 */ 2002 public TrackingContext getTrackingContext() { 2003 return tc; 2004 } 2005 2006 /** 2007 * Set the tracking context 2008 * 2009 * @param tc 2010 */ 2011 public void setTrackingContext(TrackingContext tc) { 2012 this.tc = tc; 2013 } 2014 2015 /** 2016 * @return the feature list 2017 */ 2018 public FeatureList getFeatureList() { 2019 return featurelist; 2020 } 2021 2022 /** 2023 * Set the tracking context 2024 * 2025 * @param featurelist 2026 */ 2027 public void setFeatureList(FeatureList featurelist) { 2028 this.featurelist = featurelist; 2029 } 2030 2031 /** 2032 * @return true if input images are normalised in [0,1]; false if in [0, 2033 * 255] 2034 */ 2035 public boolean isNorm() { 2036 return isNorm; 2037 } 2038 2039 /** 2040 * Set whether input images are in [0,1] (true) or [0,255] (false). 2041 * 2042 * @param isNorm 2043 */ 2044 public void setNorm(boolean isNorm) { 2045 this.isNorm = isNorm; 2046 } 2047 2048}