Robust/src/Benchmarks/Scheduling/Tracking/TrackDemo.java

   1 public class TrackDemo {
   2     flag toaddBP;
   3     flag topreresize;
   4     flag toresize;
   5     flag tomergeIDX;
   6     flag tocalcF;
   7     flag tostartL;
   8     flag toaddBP2;
   9     flag toresize2;
  10     flag tocalcT;
  11     flag finish;
  12
  13     /* input data and the counter to record the input to be processed next */
  14     int[][] m_inputs;
  15     int m_count;
  16
  17     /* current processing image related */
  18     float[] m_image;  // Icur/Jpyr1
  19     int m_rows;
  20     int m_cols;
  21     float[] m_image_resized;  // Jpyr2
  22     int m_rows_r;
  23     int m_cols_r;
  24
  25     /* BP related */
  26     int m_num_bp;
  27     /* BPL related */
  28     int m_num_bpl;
  29
  30     /* feature related */
  31     float[] m_features;
  32     int m_rows_f;
  33     int m_cols_f;
  34
  35     /*  */
  36     float[][] m_3f;
  37     int m_rows_3f;
  38     int m_cols_3f;
  39     int m_counter_3f;
  40     int m_num_p;
  41
  42     /* benchmark constants */
  43     public int WINSZ;
  44     public int N_FEA;
  45     int SUPPRESION_RADIUS;
  46     int LK_ITER;
  47     int m_counter;
  48     float accuracy;
  49
  50     /* constructor */
  51     public TrackDemo(int nump) {
  52       this.m_inputs = new int[2][];
  53
  54       int rows = 10 * 60; // * 2;
  55       int cols = 12 * 5;
  56       int offset = 0;
  57       this.m_inputs[0] = this.makeImage(rows, cols, offset);
  58       offset = 100;
  59       this.m_inputs[1] = this.makeImage(rows, cols, offset);
  60       this.m_count = 0;
  61
  62       this.m_num_bp = 0;
  63       this.m_num_bpl = 0;
  64
  65       this.WINSZ = 8;
  66       this.N_FEA = 16; //00;
  67       this.SUPPRESION_RADIUS = 10;
  68       this.LK_ITER = 20;
  69       this.accuracy = (float)0.03;
  70       this.m_counter = 2;
  71 //    #ifdef test
  72       /*this.WINSZ = 2;
  73       this.N_FEA = 10;
  74       this.LK_ITER = 1;
  75       this.m_counter = 2;
  76       this.accuracy = (float)0.1;
  77       /*
  78       //#ifdef sim_fast
  79           this.WINSZ = 4;
  80           this.N_FEA = 10;
  81           this.LK_ITER = 2;
  82           this.counter = 2;
  83
  84       //#ifdef sim
  85           this.WINSZ = 4;
  86           this.N_FEA = 20;
  87           this.LK_ITER = 4;
  88           this.counter = 2;
  89       */
  90       this.m_3f = new float[3][this.N_FEA];
  91       this.m_rows_3f = this.N_FEA;
  92       this.m_cols_3f = 1; //this.N_FEA;
  93       this.m_counter_3f = 3;
  94       this.m_num_p = nump;
  95
  96       this.m_rows = 0;
  97       this.m_cols = 0;
  98       this.m_image = null;
  99       this.m_image_resized = null;
 100       this.m_rows_r = 0;
 101       this.m_cols_r = 0;
 102
 103       this.m_features = null;
 104       this.m_rows_f = 0;
 105       this.m_cols_f = 0;
 106     }
 107
 108     public int getNumP() {
 109       return this.m_num_p;
 110     }
 111
 112     public boolean isFinish() {
 113       return (this.m_count == this.m_counter);
 114     }
 115
 116     public int getRows() {
 117       return this.m_rows;
 118     }
 119
 120     public int getCols() {
 121       return this.m_cols;
 122     }
 123
 124     public float[] getImage() {
 125       return this.m_image;
 126     }
 127
 128     public int getRowsR() {
 129       return this.m_rows_r;
 130     }
 131
 132     public int getColsR() {
 133       return this.m_cols_r;
 134     }
 135
 136     public float[] getImageR() {
 137       return this.m_image_resized;
 138     }
 139
 140     public int[] getInput(boolean isadvance) {
 141       int[] input = this.m_inputs[this.m_count];
 142       if(isadvance) {
 143         this.m_count++;
 144       }
 145
 146       return input;
 147     }
 148
 149     public void setBPNum(int num) {
 150       this.m_num_bp = num;
 151     }
 152
 153     public void setBPLNum(int num) {
 154       this.m_num_bpl = num;
 155     }
 156
 157     public int[] makeImage(int rows,
 158                            int cols,
 159                            int offset) {
 160       int k, i, j;
 161       int[] out;
 162
 163       out = new int[rows * cols + 4];
 164       out[0] = rows;
 165       out[1] = cols;
 166       out[2] = rows * cols;
 167       out[3] = 2;
 168
 169       k = offset;
 170       for(i=0; i<rows; i++) {
 171         for(j=0; j<cols; j++) {
 172           out[i*cols+j+4] = ((k++)*rows)%255;
 173         }
 174       }
 175
 176       return out;
 177     }
 178
 179     public boolean addBP(BlurPiece bp) {
 180       int startRow = bp.getRowsRS();
 181       int endRow = bp.getRowsRE();
 182       int i, j, k, cols;
 183       float[] image, input;
 184
 185       if(this.m_image == null) {
 186         this.m_rows = bp.getRows();
 187         this.m_cols = bp.getCols();
 188         this.m_image = new float[this.m_rows * this.m_cols];
 189       }
 190       image = this.m_image;
 191       cols = this.m_cols;
 192
 193       input = bp.getResult();
 194       k = 0;
 195       for(i = startRow; i < endRow; i++) {
 196         for(j = 0; j < cols; j++) {
 197           image[i * cols + j] = input[k * cols + j];
 198         }
 199         k++;
 200       }
 201
 202       this.m_num_bp--;
 203       return (0 == this.m_num_bp);
 204     }
 205
 206     public boolean addBPL(BlurPieceL bpl) {
 207       int startRow = bpl.getRowsRS();
 208       int endRow = bpl.getRowsRE();
 209       int i, j, k, cols;
 210       float[] image, input;
 211
 212       if(this.m_image == null) {
 213         this.m_rows = bpl.getRows();
 214         this.m_cols = bpl.getCols();
 215         this.m_image = new float[this.m_rows * this.m_cols];
 216       }
 217       image = this.m_image;
 218       cols = this.m_cols;
 219
 220       input = bpl.getResult();
 221       k = 0;
 222       for(i = startRow; i < endRow; i++) {
 223         for(j = 0; j < cols; j++) {
 224           image[i * cols + j] = input[k * cols + j];
 225         }
 226         k++;
 227       }
 228
 229       this.m_num_bpl--;
 230       return (0 == this.m_num_bpl);
 231     }
 232
 233     public void postBlur() {
 234       int rows, cols;
 235       float temp;
 236       int[] kernel;
 237       int rows_k, cols_k;
 238       int k, i, j;
 239       int kernelSize, startCol, endCol, halfKernel, startRow, endRow, kernelSum;
 240       float[] image;
 241
 242       rows = this.m_rows;
 243       cols = this.m_cols;
 244       image = this.m_image;
 245
 246       kernel = new int[5];
 247       rows_k = 1;
 248       cols_k = 5;
 249
 250       kernel[0] = 1;
 251       kernel[1] = 4;
 252       kernel[2] = 6;
 253       kernel[3] = 4;
 254       kernel[4] = 1;
 255
 256       kernelSize = 5;
 257       kernelSum = 16;
 258
 259       startCol = 2;       //((kernelSize)/2);
 260       endCol = cols - 2;  //round(cols - (kernelSize/2));
 261       halfKernel = 2;     //(kernelSize-1)/2;
 262
 263       startRow = 2;       //(kernelSize)/2;
 264       endRow = rows-2;  //(rows - (kernelSize)/2);
 265
 266       for(i=startRow; i<endRow; i++) {
 267         for(j=startCol; j<endCol; j++) {
 268           temp = 0.0f;
 269           for(k=-halfKernel; k<=halfKernel; k++)  {
 270             temp += (float)((image[(i+k) * cols + j]
 271                                    * (float)(kernel[k+halfKernel])));
 272           }
 273           image[i * cols + j] = (float)(temp/kernelSum);
 274         }
 275       }
 276     }
 277
 278     public void resize() {
 279       int m, k, i, j;
 280       int kernel[];
 281       int rows_k, cols_k;
 282       float tempVal;
 283       int kernelSize, startCol, endCol, halfKernel, startRow, endRow, kernelSum;
 284       int outputRows, outputCols;
 285       float temp[];
 286       int rows_t;
 287       int cols_t;
 288       float[] image, resized;
 289       int rows = this.m_rows;
 290       int cols = this.m_cols;
 291
 292       // level 1 is the base image.
 293
 294       outputRows = (int)(Math.floor((rows+1)/2));
 295       outputCols = (int)(Math.floor((cols+1)/2));
 296
 297       rows_t = rows;
 298       cols_t = outputCols;
 299       temp = new float[rows_t * cols_t];
 300
 301       this.m_rows_r = outputRows;
 302       this.m_cols_r = outputCols;
 303       resized = this.m_image_resized = new float[this.m_rows_r * this.m_cols_r];
 304       image = this.m_image;
 305
 306       rows_k = 1;
 307       cols_k = 5;
 308       kernel = new int[rows_k * cols_k];
 309
 310       kernel[0] = 1;
 311       kernel[1] = 4;
 312       kernel[2] = 6;
 313       kernel[3] = 4;
 314       kernel[4] = 1;
 315
 316       kernelSize = 5;
 317       kernelSum = 16;
 318
 319       startCol = 2;       //(kernelSize/2);
 320       endCol = cols - 2;  //(int)(cols - (kernelSize/2));
 321       halfKernel = 2;     //(kernelSize-1)/2;
 322
 323       startRow = 2;       //kernelSize/2;
 324       endRow = rows - 2;  //(rows - (kernelSize)/2);
 325
 326       for(i=startRow; i<endRow; i++) {
 327         m = 0;
 328         for(j=startCol; j<endCol; j+=2) {
 329           tempVal = 0;
 330           for(k=-halfKernel; k<=halfKernel; k++) {
 331             tempVal += (float)(image[i * cols + (j+k)]
 332                                      * (float)(kernel[k+halfKernel]));
 333           }
 334           temp[i * outputCols + m] = (float)(tempVal/kernelSum);
 335           m = m+1;
 336         }
 337       }
 338
 339       m = 0;
 340       for(i=startRow; i<endRow; i+=2) {
 341         for(j=0; j<outputCols; j++) {
 342           tempVal = 0;
 343           for(k=-halfKernel; k<=halfKernel; k++) {
 344             tempVal += (float)(temp[(i+k) * outputCols + j]*kernel[k+halfKernel]);
 345           }
 346           resized[m * outputCols + j] = (float)(tempVal/kernelSum);
 347         }
 348         m = m+1;
 349       }
 350     }
 351
 352     public boolean addIDX(IDX idx) {
 353       float[][] m3f = this.m_3f;
 354       int rows = idx.getRowsRS();
 355       int rowe = idx.getRowsRE();
 356       int threshold = this.N_FEA;
 357       int[] ind = idx.getInd();
 358       float[] image = idx.getImage();
 359       int r = idx.getR();
 360       int nfea = this.N_FEA;
 361       int length = this.m_rows * this.m_cols;
 362       int[] h_ind = new int[this.N_FEA];
 363       boolean[] f_ind = new boolean[this.N_FEA];
 364       for(int i = 0; i < this.N_FEA; i++) {
 365         f_ind[i] = false;
 366       }
 367
 368       int j = 0;
 369       int localindex = 0;
 370       int rindex = 0;
 371       for(int i = rows; i < rowe; i++) {
 372         rindex = length - ind[j];
 373         if(rindex < nfea) {
 374           localindex = j + rows;
 375           if(!f_ind[rindex]) {
 376             // empty
 377             m3f[2][rindex] = image[localindex];
 378             h_ind[rindex] = localindex;
 379             localindex++;
 380             m3f[0][rindex] = Math.ceilf((float)(localindex / (float)r));
 381             m3f[1][rindex] = (float)localindex
 382                             -(m3f[0][rindex]-1)*(float)r*(float)1.0;
 383             f_ind[rindex] = true;
 384           } else {
 385             // previously held by some others with the same value
 386             int k = rindex; // the place to insert
 387             int k1 = rindex; // the first one which is not set
 388             for(; k1 < nfea; k1++) {
 389               if(h_ind[k1] > localindex) {
 390                 k = k1;
 391               }
 392               if(!f_ind[k1]) {
 393                 break;
 394               }
 395             }
 396             if(k == nfea) {
 397               //System.printI(77777777);
 398               return false;
 399             } else if(k == rindex) {
 400               k = k1;
 401             }
 402             if(f_ind[k] && (m3f[2][k] != image[localindex])) {
 403               //System.printI(88888888);
 404               return false;
 405             }
 406             // move all things after k behind
 407             int p  = k1;
 408             for(; p > k; p--) {
 409               m3f[2][p] = m3f[2][p-1];
 410               h_ind[p] = h_ind[p-1];
 411               m3f[0][p] = m3f[0][p-1];
 412               m3f[1][p] = m3f[1][p-1];
 413               f_ind[p] = true;
 414             }
 415             // insert
 416             m3f[2][p] = image[localindex];
 417             h_ind[p] = localindex;
 418             localindex++;
 419             m3f[0][p] = Math.ceilf((float)(localindex / (float)r));
 420             m3f[1][p] = (float)localindex
 421                             -(m3f[0][p]-1)*(float)r*(float)1.0;
 422             f_ind[p] = true;
 423           }
 424         }
 425         j++;
 426       }
 427
 428       this.m_num_p--;
 429
 430       return (0 == this.m_num_p);
 431     }
 432
 433     public void calcFeatures() {
 434       float[] f1, f2, f3;
 435       int rows_f1, cols_f1, rows_f2, cols_f2, rows_f3, cols_f3;
 436       float[] interestPnts;
 437       int[] rows_ip, cols_ip;
 438       int rows_ipt, cols_ipt;
 439       rows_ip = new int[1];
 440       cols_ip = new int[1];
 441
 442       f1 = this.m_3f[0];
 443       f2 = this.m_3f[1];
 444       f3 = this.m_3f[2];
 445       rows_f1 = this.m_rows_3f;
 446       rows_f2 = this.m_rows_3f;
 447       rows_f3 = this.m_rows_3f;
 448       cols_f1 = this.m_cols_3f;
 449       cols_f2 = this.m_cols_3f;
 450       cols_f3 = this.m_cols_3f;
 451
 452       interestPnts = this.getANMs(f1,
 453                                   rows_f1,
 454                                   cols_f1,
 455                                   f2,
 456                                   rows_f2,
 457                                   cols_f2,
 458                                   f3,
 459                                   rows_f3,
 460                                   cols_f3,
 461                                   rows_ip,
 462                                   cols_ip);
 463       rows_ipt = rows_ip[0];
 464       cols_ipt = cols_ip[0];
 465       rows_ip = cols_ip = null;
 466
 467       // fTranspose(interestPnts)
 468       float[] trans;
 469       int i, j, k, rows_trans, cols_trans;
 470
 471       rows_trans = cols_ipt;
 472       cols_trans = rows_ipt;
 473       trans = new float[rows_trans * cols_trans];
 474
 475       k = 0;
 476       for(i=0; i<cols_ipt; i++) {
 477           for(j=0; j<rows_ipt; j++) {
 478             trans[k++] = interestPnts[j * cols_ipt + i];
 479           }
 480       }
 481
 482       // fDeepCopyRange(interestPnt, 0, 2, 0, cols_ip[0])
 483       int rows, cols;
 484       int numberRows = 2;
 485       int startRow = 0;
 486       int numberCols = cols_trans;
 487       int startCol = 0;
 488
 489       rows = numberRows + startRow;
 490       cols = numberCols + startCol;
 491
 492       rows_ipt = numberRows;
 493       cols_ipt = numberCols;
 494       interestPnts = new float[rows_ipt * cols_ipt];
 495
 496       k = 0;
 497       for(i=startRow; i<rows; i++) {
 498           for(j=startCol; j<cols; j++) {
 499             interestPnts[k++] = trans[i*cols_trans+j];
 500           }
 501       }
 502
 503       float[] features;
 504       this.m_rows_f = 2;
 505       this.m_cols_f = cols_ipt;
 506       features = this.m_features = new float[this.m_rows_f * this.m_cols_f];
 507       for(i=0; i<2; i++) {
 508           for(j=0; j<cols_ipt; j++) {
 509               features[i * cols_ipt + j] = interestPnts[i * cols_ipt + j];
 510           }
 511       }
 512     }
 513
 514     public float[] horzcat(float[] f1,
 515                            int rows_f1,
 516                            int cols_f1,
 517                            float[] f2,
 518                            int rows_f2,
 519                            int cols_f2,
 520                            float[] f3,
 521                            int rows_f3,
 522                            int cols_f3) {
 523       float[] out;
 524       int rows=0, cols=0, i, j, k, c_1, c_2, r_3, c_3;
 525
 526       c_1 = cols_f1;
 527       cols += c_1;
 528
 529       c_2 = cols_f2;
 530       cols += c_2;
 531
 532       r_3 = rows_f3;
 533       c_3 = cols_f3;
 534       cols += c_3;
 535
 536       rows = r_3;
 537
 538       out = new float[rows * cols];
 539
 540       for(i=0; i<rows; i++) {
 541           k = 0;
 542           for(j=0; j<c_1; j++) {
 543               out[i*cols+k] = f1[i*c_1+j];
 544               k++;
 545           }
 546           for(j=0; j<c_2; j++) {
 547               out[i*cols+k] = f2[i*c_2+j];
 548               k++;
 549           }
 550           for(j=0; j<c_3; j++) {
 551               out[i*cols+k] = f3[i*c_3+j];
 552               k++;
 553           }
 554       }
 555
 556       return out;
 557     }
 558
 559     public int[] fSortIndices(float[] input,
 560                               int rows,
 561                               int cols) {
 562       float[] in;
 563       int i, j, k;
 564       int[] ind;
 565
 566       // fDeepCopy
 567       in = new float[input.length];
 568       for(i=0; i<input.length; i++) {
 569         in[i] = input[i];
 570       }
 571
 572       ind = new int[rows * cols];
 573
 574       for(k=0; k<cols; k++) {
 575         for(i=0; i<rows; i++) {
 576           float localMax = in[i * cols + k];
 577           int localIndex = i;
 578           ind[i * cols + k] = i;
 579           for(j=0; j<rows; j++) {
 580             if(localMax < in[j*cols+k]) {
 581               ind[i * cols + k] = j;
 582               localMax = in[j * cols + k];
 583               localIndex = j;
 584             }
 585           }
 586           in[localIndex * cols + k] = 0;
 587         }
 588       }
 589
 590       return ind;
 591     }
 592
 593     public float[] ffVertcat(float[] matrix1,
 594                              int rows_m1,
 595                              int cols_m1,
 596                              float[] matrix2,
 597                              int rows_m2,
 598                              int cols_m2) {
 599       float[] outMatrix;
 600       int rows_o, cols_o, i, j, k;
 601
 602       rows_o = rows_m1 + rows_m2;
 603       cols_o = cols_m1;
 604       outMatrix = new float[rows_o * cols_o];
 605
 606       for( i=0; i<cols_m1; i++) {
 607         for (j=0; j<rows_m1; j++) {
 608           outMatrix[j * cols_m1 + i] = matrix1[j * cols_m1 + i];
 609         }
 610         for( k=0; k<rows_m2; k++) {
 611           outMatrix[(k+rows_m1) * cols_m1 + i] = matrix2[ k * cols_m2 + i];
 612         }
 613       }
 614
 615       return outMatrix;
 616
 617     }
 618
 619     public float[] getANMs(float[] f1,
 620                            int rows_f1,
 621                            int cols_f1,
 622                            float[] f2,
 623                            int rows_f2,
 624                            int cols_f2,
 625                            float[] f3,
 626                            int rows_f3,
 627                            int cols_f3,
 628                            int[] rows_ip,
 629                            int[] cols_ip) {
 630       float MAX_LIMIT = (float)100000000;
 631       float C_ROBUST = (float)1.0;
 632       float[] suppressR, points, srtdPnts, tempF, srtdV, interestPnts, temp;
 633       int rows_sr, cols_sr, rows_p, cols_p, rows_sp, cols_sp, rows_tf, cols_tf;
 634       int rows_sv, cols_sv, rows_tmp, cols_tmp;
 635       int[] srtdVIdx, supId;
 636       int rows_svi, cols_svi, rows_si, cols_si;
 637       float t, t1, r_sq;
 638       int n, i, j, k, validCount, cnt, end, iter, rows, cols;
 639       int supIdPtr = 0;
 640
 641       r_sq = (float)(this.SUPPRESION_RADIUS ^ 2);
 642       points = this.horzcat (f1,
 643                              rows_f1,
 644                              cols_f1,
 645                              f2,
 646                              rows_f2,
 647                              cols_f2,
 648                              f3,
 649                              rows_f3,
 650                              cols_f3);
 651       rows_p = rows_f3;
 652       cols_p = cols_f1 + cols_f2 + cols_f3;
 653       n = rows_f3;
 654
 655       /** sort() arg 2 is for descend = 1, arg3 = indices. Returns sorted values **/
 656
 657       srtdVIdx = this.fSortIndices (f3, rows_f3, cols_f3);
 658       rows_svi = rows_f3;
 659       cols_svi = cols_f3;
 660
 661       rows_sp = rows_svi;
 662       cols_sp = cols_p;
 663       srtdPnts = new float[rows_sp * cols_sp];
 664
 665       for (i = 0; i < rows_sp; i++) {
 666         for(j=0; j<cols_sp; j++) {
 667           srtdPnts[i*cols_sp+j] = points[srtdVIdx[i]*cols_sp+j];
 668         }
 669       }
 670
 671       rows_tmp = 1;
 672       cols_tmp = 3;
 673       temp = new float[rows_tmp * cols_tmp];
 674       rows_sr = n;
 675       cols_sr = 1;
 676       suppressR = new float[rows_sr * cols_sr];
 677       for(i = 0; i < rows_sr; i++) {
 678         for(j = 0; j < cols_sr; j++) {
 679           suppressR[i*cols_sr+j] = MAX_LIMIT;
 680         }
 681       }
 682
 683       validCount = 0;
 684       iter = 0;
 685       for (i = 0; i < rows_sr; i++) {
 686         if (suppressR[i] > r_sq) {
 687           validCount++;
 688         }
 689       }
 690
 691       k = 0;
 692       rows_si = validCount;
 693       cols_si = 1;
 694       supId = new int[rows_si * cols_si];
 695       for (i = 0; i < (rows_sr*cols_sr); i++) {
 696         if (suppressR[i] > r_sq) {
 697           supId[k++] = i;
 698         }
 699       }
 700
 701       while (validCount > 0) {
 702         float[] tempp, temps;
 703         int rows_tpp, cols_tpp, rows_tps, cols_tps;
 704         temp[0] = srtdPnts[supId[0] * cols_sp + 0];
 705         temp[1] = srtdPnts[supId[0] * cols_sp + 1];
 706         temp[2] = srtdPnts[supId[0] * cols_sp + 2];
 707
 708         if(iter == 0) {
 709           interestPnts = temp;
 710           rows_ip[0] = rows_tmp;
 711           cols_ip[0] = cols_tmp;
 712         } else {
 713           interestPnts = this.ffVertcat(interestPnts,
 714                                         rows_ip[0],
 715                                         cols_ip[0],
 716                                         temp,
 717                                         rows_tmp,
 718                                         cols_tmp);
 719           rows_ip[0] = rows_ip[0] + rows_tmp;
 720           cols_ip[0] = cols_ip[0];
 721         }
 722
 723         iter++;
 724
 725         // fDeepCopy
 726         rows_tpp = rows_sp;
 727         cols_tpp = cols_sp;
 728         tempp = new float[rows_tpp * cols_tpp];
 729         for(i=0; i<rows_tpp * cols_tpp; i++) {
 730           tempp[i] = srtdPnts[i];
 731         }
 732         // fDeepCopy
 733         rows_tps = rows_sr;
 734         cols_tps = cols_sr;
 735         temps = new float[rows_tps * cols_tps];
 736         for(i=0; i<rows_tps * cols_tps; i++) {
 737           temps[i] = suppressR[i];
 738         }
 739
 740         rows_sp = validCount - 1;
 741         cols_sp = 3;
 742         srtdPnts = new float[rows_sp * cols_sp];
 743         rows_sr = validCount - 1;
 744         cols_sr = 1;
 745         suppressR = new float[rows_sr * cols_sr];
 746
 747         k=0;
 748         for(i=0; i<(validCount-1); i++) {
 749           srtdPnts[i * cols_sp + 0] = tempp[supId[i+1] * cols_sp + 0];
 750           srtdPnts[i * cols_sp + 1] = tempp[supId[i+1] * cols_sp + 1];
 751           srtdPnts[i * cols_sp + 2] = tempp[supId[i+1] * cols_sp + 2];
 752           suppressR[i * cols_sr + 0] = temps[supId[i+1] * cols_sr + 0];
 753         }
 754
 755         int rows1 = rows_ip[0]-1;
 756         int cols1 = cols_ip[0];
 757         for (i = 0; i < rows_sp; i++) {
 758           t = (float)0;
 759           t1 = (float)0;
 760
 761           if ((C_ROBUST * interestPnts[rows1 * cols1 + 2])
 762               >= srtdPnts[i * cols_sp + 2]) {
 763             t = srtdPnts[i * cols_sp + 0] - interestPnts[rows1 * cols1 + 0];
 764             t1 = srtdPnts[i * cols_sp + 1] - interestPnts[rows1 * cols1 + 1];
 765             t = t * t + t1 * t1;
 766             t1 = (float)0;
 767           }
 768
 769           if ((C_ROBUST * interestPnts[rows1 * cols1 + 2])
 770               < srtdPnts[i * cols_sp + 2]) {
 771             t1 = (float)1 * (float)MAX_LIMIT;
 772           }
 773
 774           if (suppressR[i] > (t + t1)) {
 775             suppressR[i] = t + t1;
 776           }
 777         }
 778
 779         validCount=0;
 780         for (i = 0; i < rows_sr; i++) {
 781           if (suppressR[i] > r_sq) {
 782             validCount++;
 783           }
 784         }
 785
 786         k = 0;
 787         rows_si = validCount;
 788         cols_si = 1;
 789         supId = new int[rows_si * cols_si];
 790
 791         for (i = 0; i < rows_sr*cols_sr; i++) {
 792           if (suppressR[i] > r_sq) {
 793             supId[k++] = i;
 794           }
 795         }
 796       }
 797
 798       return interestPnts;
 799     }
 800
 801     public void startTrackingLoop() {
 802       this.m_image = null;
 803       this.m_image_resized = null;
 804     }
 805
 806     public float[] getInterpolatePatch(float[] src,
 807                                        int rows,
 808                                        int cols,
 809                                        float centerX,
 810                                        float centerY) {
 811       float[] dst;
 812       int rows_d, cols_d;
 813       float a, b, a11, a12, a21, a22;
 814       int i, j, srcIdxX, dstIdxX, srcIdy, dstIdy, dstIndex;
 815
 816       a = centerX - (float)(Math.floor(centerX));
 817       b = centerY - (float)(Math.floor(centerY));
 818
 819       a11 = (1-a)*(1-b);
 820       a12=a*(1-b);
 821       a21=(1-a)*b;
 822       a22 = a*b;
 823
 824       rows_d = 1;
 825       cols_d = 2*this.WINSZ*2*this.WINSZ;
 826       dst = new float[rows_d * cols_d];
 827
 828       for(i=-this.WINSZ; i<=(this.WINSZ-1); i++) {
 829         srcIdxX = (int)(Math.floor(centerX)) + i;
 830         dstIdxX = i+this.WINSZ;
 831
 832         for(j=-this.WINSZ; j<=(this.WINSZ-1); j++) {
 833           srcIdy = (int)(Math.floor(centerY)) + j;
 834           dstIdy = j+this.WINSZ;
 835           dstIndex = dstIdy * 2 * this.WINSZ + dstIdxX;
 836 //        printf("%f\t%f\t%d\t%d\n", centerX, centerY, srcIdxX, srcIdy);
 837           dst[dstIndex] = src[srcIdy * cols + srcIdxX]*a11
 838                         + src[(srcIdy+1)* cols + srcIdxX]*a12
 839                         + src[srcIdy * cols + (srcIdxX+1)]*a21
 840                         + src[(srcIdy+1) * cols+ (srcIdxX+1)]*a22;
 841         }
 842       }
 843
 844       return dst;
 845     }
 846
 847     public int[] calcPyrLKTrack(float[][] Ipyrs,
 848                                 int[] rows,
 849                                 int[] cols,
 850                                 float[] newPnt) {
 851       float[] ip1, ip2, idxp1, idxp2, idyp1, idyp2, jp1, jp2, fPnt;
 852       int k = 0;
 853
 854       ip1 = Ipyrs[k++];
 855       ip2 = Ipyrs[k++];
 856       idxp1 = Ipyrs[k++];
 857       idyp1 = Ipyrs[k++];
 858       idxp2 = Ipyrs[k++];
 859       idyp2 = Ipyrs[k++];
 860       jp1 = this.m_image;
 861       jp2 = this.m_image_resized;
 862       fPnt = this.m_features;
 863
 864       int idx, level, pLevel, i, winSizeSq;
 865       int[] valid, imgDims;
 866       int rows_v, cols_v, rows_id, cols_id;
 867       float[] rate, iPatch, jPatch, iDxPatch, iDyPatch;
 868       int rows_r, cols_r, rows_ip, cols_ip, rows_jp, cols_jp;
 869       int rows_idxp, cols_idxp, rows_idyp, cols_idyp;
 870       float x, y, dX, dY, c_xx, c_yy, c_xy, tr;
 871       int imgSize_1, /*max_iter, */imgSize_2;
 872       float mX, mY, dIt, eX, eY, c_det;
 873       int nFeatures = this.m_cols_f;
 874
 875       rows_id = 4;
 876       cols_id = 1;
 877       imgDims = new int[rows_id * cols_id];
 878
 879       imgDims[0] = rows[0];
 880       imgDims[1] = cols[0];
 881       imgDims[2] = rows[1];
 882       imgDims[3] = cols[1];
 883
 884       pLevel = 2;
 885       rows_r = 1;
 886       cols_r = 6;
 887       rate = new float[rows_r * cols_r];
 888
 889       rate[0] = (float)1;
 890       rate[1] = (float)0.5;
 891       rate[2] = (float)0.25;
 892       rate[3] = (float)0.125;
 893       rate[4] = (float)0.0625;
 894       rate[5] = (float)0.03125;
 895
 896       winSizeSq = 4*this.WINSZ*this.WINSZ;
 897       rows_ip = 1;
 898       cols_ip = winSizeSq;
 899       iPatch = new float[rows_ip * cols_ip];
 900       rows_jp = 1;
 901       cols_jp = winSizeSq;
 902       jPatch = new float[rows_jp * cols_jp];
 903       rows_idxp = 1;
 904       cols_idxp = winSizeSq;
 905       iDxPatch = new float[rows_idxp * cols_idxp];
 906       rows_idyp = 1;
 907       cols_idyp = winSizeSq;
 908       iDyPatch = new float[rows_idyp * cols_idyp];
 909
 910       rows_v = 1;
 911       cols_v = nFeatures;
 912       valid = new int[rows_v * cols_v];
 913       for(int valid_idx=0;valid_idx<valid.length;valid_idx++){
 914         valid[valid_idx]=1;
 915       }
 916
 917       for(i=0; i<nFeatures; i++) {
 918         dX = (float)0;
 919         dY = (float)0;
 920         x = fPnt[i*2+0] * rate[pLevel];
 921         y = fPnt[i*2+1] * rate[pLevel];
 922         c_det = (float)0;
 923
 924         for(level = pLevel-1; level>=0; level--) {
 925           x = x+x;
 926           y = y+y;
 927           dX = dX + dX;
 928           dY = dY + dY;
 929           imgSize_1 = imgDims[level*2];
 930           imgSize_2 = imgDims[level*2+1];
 931
 932           c_xx = (float)0;
 933           c_xy = (float)0;
 934           c_yy = (float)0;
 935
 936           if( (x-(float)this.WINSZ)<(float)0 || (y-(float)this.WINSZ)<(float)0
 937               || (y+(float)this.WINSZ)>=(float)imgSize_1
 938               || (x+(float)this.WINSZ)>=(float)imgSize_2 ) {
 939             valid[i] = 0;
 940             break;
 941           }
 942
 943           if(level ==0) {
 944             iPatch = getInterpolatePatch(ip1, rows[0], cols[0], x, y);
 945             iDxPatch = getInterpolatePatch(idxp1, rows[2], cols[2], x, y);
 946             iDyPatch = getInterpolatePatch(idyp1, rows[3], cols[3], x, y);
 947           }
 948           if(level ==1) {
 949             iPatch = getInterpolatePatch(ip2, rows[1], cols[1], x, y);
 950             iDxPatch = getInterpolatePatch(idxp2, rows[4], cols[4], x, y);
 951             iDyPatch = getInterpolatePatch(idyp2, rows[5], cols[5], x, y);
 952           }
 953           rows_ip = rows_idxp = rows_idyp = 1;
 954           cols_ip = cols_idxp = cols_idyp = 2*this.WINSZ*2*this.WINSZ;
 955
 956           for(idx=0; idx<this.WINSZ; idx++) {
 957             c_xx += iDxPatch[idx] * iDxPatch[idx];
 958             c_xy += iDxPatch[idx] * iDyPatch[idx];
 959             c_yy += iDyPatch[idx] * iDyPatch[idx];
 960           }
 961
 962           c_det = (c_xx * c_yy -c_xy * c_xy);
 963           tr = c_xx + c_yy;
 964
 965           if(c_det == (float)0) {
 966             break;
 967           }
 968
 969           if((float)(c_det/(tr+(float)0.00001)) < (float)this.accuracy) {
 970             valid[i] = 0;
 971             break;
 972           }
 973
 974           c_det = (float)(1/c_det);
 975           for(k=0; k<this.LK_ITER;/*max_iter; */k++) {
 976             if( (x+dX-(float)this.WINSZ)<(float)0 || (y+dY-(float)this.WINSZ)<(float)0
 977                 || (y+dY+(float)this.WINSZ)>=(float)imgSize_1
 978                 || (x+dX+(float)this.WINSZ)>=(float)imgSize_2) {
 979               valid[i] = 0;
 980               break;
 981             }
 982
 983 //          printf("x and dx = %d\t%d\t%f\t%f\t%f\t%f\n", i, level, x, dX, y, dY);
 984             if(level == 0) {
 985               jPatch = getInterpolatePatch(jp1,
 986                                            this.m_rows,
 987                                            this.m_cols,
 988                                            x+dX,
 989                                            y+dY);
 990             }
 991             if(level == 1) {
 992               jPatch = getInterpolatePatch(jp2,
 993                                            this.m_rows_r,
 994                                            this.m_cols_r,
 995                                            x+dX,
 996                                            y+dY);
 997             }
 998             rows_jp = 1;
 999             cols_jp = 2*this.WINSZ*2*this.WINSZ;
1000
1001             eX = 0;
1002             eY = 0;
1003             for(idx=0; idx<winSizeSq; idx++) {
1004               dIt = iPatch[idx] - jPatch[idx];
1005               eX += dIt * iDxPatch[idx];
1006               eY += dIt * iDyPatch[idx];
1007             }
1008
1009             mX = c_det * (eX * c_yy - eY * c_xy);
1010             mY = c_det * (-eX * c_xy + eY * c_xx);
1011 //          printf("mx = %d\t%d\t%f\t%f\t%f\t%f\t%f\n", i, level, mX, mY, c_det, eX, eY);
1012             dX = dX + mX;
1013             dY = dY + mY;
1014
1015             if( (mX*mX+mY+mY) < this.accuracy) {
1016               break;
1017             }
1018           }
1019         }
1020
1021         newPnt[i] = fPnt[i*2] + dX;
1022         newPnt[1*nFeatures+i] = fPnt[i*2+1] + dY;
1023
1024       }
1025
1026       return valid;
1027     }
1028
1029     public void calcTrack(IXLM ixlm,
1030                           IYLM iylm,
1031                           IXLMR ixlmr,
1032                           IYLMR iylmr) {
1033       float[][] Ipyrs  = new float[6][];
1034       int[] rows  = new int[6];
1035       int[] cols = new int[6];
1036       float[] newpoints, features, np_temp;
1037       int rows_n, cols_n, rows_np, cols_np, i, j, k, m, n, numFind;
1038       int[] status;
1039       int rows_s, cols_s;
1040
1041       Ipyrs[0] = ixlm.getImage();
1042       rows[0] = ixlm.getRows();
1043       cols[0] = ixlm.getCols();
1044       Ipyrs[2] = ixlm.getResult();
1045       rows[2] = ixlm.getRowsR();
1046       cols[2] = ixlm.getColsR();
1047       Ipyrs[3] = iylm.getResult();
1048       rows[3] = iylm.getRowsR();
1049       cols[3] = iylm.getColsR();
1050
1051       Ipyrs[1] = ixlmr.getImage();
1052       rows[1] = ixlmr.getRows();
1053       cols[1] = ixlmr.getCols();
1054       Ipyrs[4] = ixlmr.getResult();
1055       rows[4] = ixlmr.getRowsR();
1056       cols[4] = ixlmr.getColsR();
1057       Ipyrs[5] = iylmr.getResult();
1058       rows[5] = iylmr.getRowsR();
1059       cols[5] = iylmr.getColsR();
1060
1061       features = this.m_features;
1062       rows_n = 2;
1063       cols_n = this.m_cols_f;
1064       newpoints = new float[rows_n * cols_n];
1065
1066       // status_ = calcPyrLKTrack(...)
1067       status = this.calcPyrLKTrack(Ipyrs, rows, cols, newpoints);
1068       rows_s = 1;
1069       cols_s = this.m_cols_f;
1070
1071       // fDeepCopy
1072       np_temp = new float[newpoints.length];
1073       rows_np = rows_n;
1074       cols_np = cols_n;
1075       for(i = 0; i < newpoints.length; i++) {
1076         np_temp[i] = newpoints[i];
1077       }
1078
1079       if(rows_s*cols_s > 0 ) {
1080         int[] findInd;
1081         int rows_f, cols_f;
1082         rows_f = rows_s * cols_s;
1083         cols_f = 1;
1084         findInd = new int[rows_f * cols_f];
1085
1086         k = 0;
1087         m = 0;
1088         numFind = 0;
1089         for(i=0; i<cols_s; i++) {
1090           for(j=0; j<rows_s; j++) {
1091             if(status[j*cols_s+i] != 0) {
1092               findInd[k] = m;
1093               numFind++;
1094             } else {
1095               findInd[k] = 0;
1096             }
1097
1098             m++;
1099             k++;
1100           }
1101         }
1102
1103         rows_n = rows_np;
1104         cols_n = numFind;
1105         newpoints = new float[rows_n * cols_n];
1106
1107         k = 0;
1108         n = 0;
1109         for(i=0; i<rows_np; i++) {
1110           for(j=0; j<cols_np; j++) {
1111             m = findInd[j];
1112             if(m > 0) {
1113               newpoints[k++] = np_temp[i*cols_np+m];
1114             }
1115           }
1116         }
1117       }
1118
1119       // features_ = fDeepCopy(newpoints_);
1120       this.m_rows_f = rows_n;
1121       this.m_cols_f = cols_n;
1122       features = this.m_features = new float[newpoints.length];
1123       for(k = 0; k < newpoints.length; k++) {
1124         features[k] = newpoints[k];
1125       }
1126     }
1127
1128     public void printImage() {
1129       //    result validation
1130       for(int i=0; i<this.m_rows; i++) {
1131           for(int j=0; j<this.m_cols; j++) {
1132               System.printI((int)(this.m_image[i * this.m_cols + j]*10));
1133           }
1134       }
1135     }
1136
1137     public void print3f() {
1138       //    result validation
1139       System.printI(11111111);
1140       for(int j=0; j<this.N_FEA; j++) {
1141         System.printI((int)(this.m_3f[0][j]*10));
1142       }
1143       System.printI(22222222);
1144       for(int j=0; j<this.N_FEA; j++) {
1145         System.printI((int)(this.m_3f[1][j]*10));
1146       }
1147       System.printI(33333333);
1148       for(int j=0; j<this.N_FEA; j++) {
1149         System.printI((int)(this.m_3f[2][j]*10));
1150       }
1151     }
1152
1153     public void printFeatures() {
1154       //    result validation
1155       for(int i=0; i<this.m_rows_f; i++) {
1156           for(int j=0; j<this.m_cols_f; j++) {
1157               System.printI((int)(this.m_features[i * this.m_cols_f + j]*10));
1158           }
1159       }
1160     }
1161 }