void _KLTComputePyramid( _KLT_FloatImage img, _KLT_Pyramid pyramid, float sigma_fact) { _KLT_FloatImage currimg, tmpimg; int ncols = img->ncols, nrows = img->nrows; int subsampling = pyramid->subsampling; int subhalf = subsampling / 2; float sigma = subsampling * sigma_fact; /* empirically determined */ int oldncols; int i, x, y; if (subsampling != 2 && subsampling != 4 && subsampling != 8 && subsampling != 16 && subsampling != 32) KLTError("(_KLTComputePyramid) Pyramid's subsampling must " "be either 2, 4, 8, 16, or 32"); assert(pyramid->ncols[0] == img->ncols); assert(pyramid->nrows[0] == img->nrows); /* Copy original image to level 0 of pyramid */ memcpy(pyramid->img[0]->data, img->data, ncols*nrows*sizeof(float)); currimg = img; for (i = 1 ; i < pyramid->nLevels ; i++) { tmpimg = _KLTCreateFloatImage(ncols, nrows); _KLTComputeSmoothedImage(currimg, sigma, tmpimg); // int k; // for (k = 1280*100; k < 1280*100 + 100; k ++) { // //if(pyramid1->img[1]->data[k] > 0.0) { // //printf("%f\n", pyramid1->img[0]->data[k]); // //printf("%f\t", floatimg1->data[k]); // printf("%f\t", tmpimg->data[k]); // if ((k - 1280*100) % 5 == 0) { // printf("\n"); // } // //} // } // printf("\n\n"); // exit(1); /* Subsample */ oldncols = ncols; ncols /= subsampling; nrows /= subsampling; for (y = 0 ; y < nrows ; y++) for (x = 0 ; x < ncols ; x++) pyramid->img[i]->data[y*ncols+x] = tmpimg->data[(subsampling*y+subhalf)*oldncols + (subsampling*x+subhalf)]; /* Reassign current image */ currimg = pyramid->img[i]; _KLTFreeFloatImage(tmpimg); } }
static void _convolveSeparate( _KLT_FloatImage imgin, ConvolutionKernel horiz_kernel, ConvolutionKernel vert_kernel, _KLT_FloatImage imgout) { /* Create temporary image */ _KLT_FloatImage tmpimg; tmpimg = _KLTCreateFloatImage(imgin->ncols, imgin->nrows); /* Do convolution */ _convolveImageHoriz(imgin, horiz_kernel, tmpimg); _convolveImageVert(tmpimg, vert_kernel, imgout); /* Free memory */ _KLTFreeFloatImage(tmpimg); }
_KLT_Pyramid _KLTCreatePyramid( int ncols, int nrows, int subsampling, int nlevels) { _KLT_Pyramid pyramid; int nbytes = sizeof(_KLT_PyramidRec) + nlevels * sizeof(_KLT_FloatImage *) + nlevels * sizeof(int) + nlevels * sizeof(int); int i; if (subsampling != 2 && subsampling != 4 && subsampling != 8 && subsampling != 16 && subsampling != 32) KLTError("(_KLTCreatePyramid) Pyramid's subsampling must " "be either 2, 4, 8, 16, or 32"); /* Allocate memory for structure and set parameters */ pyramid = (_KLT_Pyramid) malloc(nbytes); if (pyramid == NULL) KLTError("(_KLTCreatePyramid) Out of memory"); /* Set parameters */ pyramid->subsampling = subsampling; pyramid->nLevels = nlevels; pyramid->img = (_KLT_FloatImage *) (pyramid + 1); pyramid->ncols = (int *) (pyramid->img + nlevels); pyramid->nrows = (int *) (pyramid->ncols + nlevels); /* Allocate memory for each level of pyramid and assign pointers */ for (i = 0 ; i < nlevels ; i++) { pyramid->img[i] = _KLTCreateFloatImage(ncols, nrows); pyramid->ncols[i] = ncols; pyramid->nrows[i] = nrows; ncols /= subsampling; nrows /= subsampling; } return pyramid; }
void _KLTSelectGoodFeatures( KLT_TrackingContext tc, KLT_PixelType *img, int ncols, int nrows, KLT_FeatureList featurelist, selectionMode mode) { // added timer for this function //Timer selectTimer; _KLT_FloatImage floatimg, gradx, grady; int window_hw, window_hh; int *pointlist; int npoints = 0; KLT_BOOL overwriteAllFeatures = (mode == SELECTING_ALL) ? TRUE : FALSE; KLT_BOOL floatimages_created = FALSE; // Initialize times //initTimer(&selectTimer, "select Feature Time"); // Start timer //startTimer(&selectTimer); /* Check window size (and correct if necessary) */ if (tc->window_width % 2 != 1) { tc->window_width = tc->window_width+1; KLTWarning("Tracking context's window width must be odd. " "Changing to %d.\n", tc->window_width); } if (tc->window_height % 2 != 1) { tc->window_height = tc->window_height+1; KLTWarning("Tracking context's window height must be odd. " "Changing to %d.\n", tc->window_height); } if (tc->window_width < 3) { tc->window_width = 3; KLTWarning("Tracking context's window width must be at least three. \n" "Changing to %d.\n", tc->window_width); } if (tc->window_height < 3) { tc->window_height = 3; KLTWarning("Tracking context's window height must be at least three. \n" "Changing to %d.\n", tc->window_height); } window_hw = tc->window_width/2; window_hh = tc->window_height/2; // After windows //stopTimer(&selectTimer); //printf("Time for checking the window size = %f\n", getTime(selectTimer)); // Restart for point list //restartTimer(&selectTimer); /* Create pointlist, which is a simplified version of a featurelist, */ /* for speed. Contains only integer locations and values. */ pointlist = (int *) malloc(ncols * nrows * 3 * sizeof(int)); // stop and print //stopTimer(&selectTimer); //printf("Time for initializing pointlist = %f\n", getTime(selectTimer)); // Restart for point list //restartTimer(&selectTimer); /* Create temporary images, etc. */ if (mode == REPLACING_SOME && tc->sequentialMode && tc->pyramid_last != NULL) { floatimg = ((_KLT_Pyramid) tc->pyramid_last)->img[0]; gradx = ((_KLT_Pyramid) tc->pyramid_last_gradx)->img[0]; grady = ((_KLT_Pyramid) tc->pyramid_last_grady)->img[0]; assert(gradx != NULL); assert(grady != NULL); } else { floatimages_created = TRUE; floatimg = _KLTCreateFloatImage(ncols, nrows); gradx = _KLTCreateFloatImage(ncols, nrows); grady = _KLTCreateFloatImage(ncols, nrows); if (tc->smoothBeforeSelecting) { _KLT_FloatImage tmpimg; tmpimg = _KLTCreateFloatImage(ncols, nrows); _KLTToFloatImage(img, ncols, nrows, tmpimg); _KLTComputeSmoothedImage(tmpimg, _KLTComputeSmoothSigma(tc), floatimg); _KLTFreeFloatImage(tmpimg); } else _KLTToFloatImage(img, ncols, nrows, floatimg); /* Compute gradient of image in x and y direction */ _KLTComputeGradients(floatimg, tc->grad_sigma, gradx, grady); } // stop and print //stopTimer(&selectTimer); //printf("Time for creating temporary mages = %f\n", getTime(selectTimer)); // Restart for point list //restartTimer(&selectTimer); /* Write internal images */ if (tc->writeInternalImages) { _KLTWriteFloatImageToPGM(floatimg, "kltimg_sgfrlf.pgm"); _KLTWriteFloatImageToPGM(gradx, "kltimg_sgfrlf_gx.pgm"); _KLTWriteFloatImageToPGM(grady, "kltimg_sgfrlf_gy.pgm"); } // stop and print //stopTimer(&selectTimer); //printf("Time for writing internal images = %f\n", getTime(selectTimer)); /* Compute trackability of each image pixel as the minimum of the two eigenvalues of the Z matrix */ {register int *ptr; /* register float gx, gy; register float gxx, gxy, gyy; register int xx, yy; float val; unsigned int limit = 1; int borderx = tc->borderx; // Must not touch cols int bordery = tc->bordery; // lost by convolution int x, y; int i; */ /* if (borderx < window_hw) borderx = window_hw; if (bordery < window_hh) bordery = window_hh; // Find largest value of an int for (i = 0 ; i < sizeof(int) ; i++) limit *= 256; limit = limit/2 - 1; */ /* For most of the pixels in the image, do ... */ ptr = pointlist; point_gradients(ptr, tc, &npoints, nrows, ncols, gradx, grady); /* for (y = bordery ; y < nrows - bordery ; y += tc->nSkippedPixels + 1) for (x = borderx ; x < ncols - borderx ; x += tc->nSkippedPixels + 1) { // Sum the gradients in the surrounding window gxx = 0; gxy = 0; gyy = 0; for (yy = y-window_hh ; yy <= y+window_hh ; yy++) for (xx = x-window_hw ; xx <= x+window_hw ; xx++) { gx = *(gradx->data + ncols*yy+xx); gy = *(grady->data + ncols*yy+xx); gxx += gx * gx; gxy += gx * gy; gyy += gy * gy; } // Store the trackability of the pixel as the minimum // of the two eigenvalues *ptr++ = x; *ptr++ = y; val = _minEigenvalue(gxx, gxy, gyy); if (val > limit) { KLTWarning("(_KLTSelectGoodFeatures) minimum eigenvalue %f is " "greater than the capacity of an int; setting " "to maximum value", val); val = (float) limit; } *ptr++ = (int) val; npoints++; } */ } // Restart for point list //restartTimer(&selectTimer); /* Sort the features */ _sortPointList(pointlist, npoints); // printing this loop //stopTimer(&selectTimer); //printf("Time for sorting the pointlist = %f\n", getTime(selectTimer)); // Restart for point list //restartTimer(&selectTimer); /* Check tc->mindist */ if (tc->mindist < 0) { KLTWarning("(_KLTSelectGoodFeatures) Tracking context field tc->mindist " "is negative (%d); setting to zero", tc->mindist); tc->mindist = 0; } // printing this loop //stopTimer(&selectTimer); //printf("Time for checking midlist = %f\n", getTime(selectTimer)); // Restart for point list //restartTimer(&selectTimer); /* Enforce minimum distance between features */ _enforceMinimumDistance( pointlist, npoints, featurelist, ncols, nrows, tc->mindist, tc->min_eigenvalue, overwriteAllFeatures); // printing this loop //stopTimer(&selectTimer); //printf("Time for enforcing minimum distance = %f\n", getTime(selectTimer)); // Restart for point list //restartTimer(&selectTimer); /* Free memory */ free(pointlist); if (floatimages_created) { _KLTFreeFloatImage(floatimg); _KLTFreeFloatImage(gradx); _KLTFreeFloatImage(grady); } // printing this loop //stopTimer(&selectTimer); //printf("Time for freeing stuff = %f\n", getTime(selectTimer)); }
static int _am_trackFeatureAffine( float x1, /* location of window in first image */ float y1, float *x2, /* starting location of search in second image */ float *y2, _KLT_FloatImage img1, _KLT_FloatImage gradx1, _KLT_FloatImage grady1, _KLT_FloatImage img2, _KLT_FloatImage gradx2, _KLT_FloatImage grady2, int width, /* size of window */ int height, float step_factor, /* 2.0 comes from equations, 1.0 seems to avoid overshooting */ int max_iterations, float small, /* determinant threshold for declaring KLT_SMALL_DET */ float th, /* displacement threshold for stopping */ float th_aff, float max_residue, /* residue threshold for declaring KLT_LARGE_RESIDUE */ int lighting_insensitive, /* whether to normalize for gain and bias */ int affine_map, /* whether to evaluates the consistency of features with affine mapping */ float mdd, /* difference between the displacements */ float *Axx, float *Ayx, float *Axy, float *Ayy) /* used affine mapping */ { _FloatWindow imgdiff, gradx, grady; float gxx, gxy, gyy, ex, ey, dx, dy; int iteration = 0; int status = 0; int hw = width/2; int hh = height/2; int nc1 = img1->ncols; int nr1 = img1->nrows; int nc2 = img2->ncols; int nr2 = img2->nrows; float **a; float **T; float one_plus_eps = 1.001f; /* To prevent rounding errors */ float old_x2 = *x2; float old_y2 = *y2; KLT_BOOL convergence = FALSE; #ifdef DEBUG_AFFINE_MAPPING char fname[80]; _KLT_FloatImage aff_diff_win = _KLTCreateFloatImage(width,height); printf("starting location x2=%f y2=%f\n", *x2, *y2); #endif /* Allocate memory for windows */ imgdiff = _allocateFloatWindow(width, height); gradx = _allocateFloatWindow(width, height); grady = _allocateFloatWindow(width, height); T = _am_matrix(6,6); a = _am_matrix(6,1); /* Iteratively update the window position */ do { if(!affine_map) { /* pure translation tracker */ /* If out of bounds, exit loop */ if ( x1-hw < 0.0f || nc1-( x1+hw) < one_plus_eps || *x2-hw < 0.0f || nc2-(*x2+hw) < one_plus_eps || y1-hh < 0.0f || nr1-( y1+hh) < one_plus_eps || *y2-hh < 0.0f || nr2-(*y2+hh) < one_plus_eps) { status = KLT_OOB; break; } /* Compute gradient and difference windows */ if (lighting_insensitive) { _computeIntensityDifferenceLightingInsensitive(img1, img2, x1, y1, *x2, *y2, width, height, imgdiff); _computeGradientSumLightingInsensitive(gradx1, grady1, gradx2, grady2, img1, img2, x1, y1, *x2, *y2, width, height, gradx, grady); } else { _computeIntensityDifference(img1, img2, x1, y1, *x2, *y2, width, height, imgdiff); _computeGradientSum(gradx1, grady1, gradx2, grady2, x1, y1, *x2, *y2, width, height, gradx, grady); } #ifdef DEBUG_AFFINE_MAPPING aff_diff_win->data = imgdiff; sprintf(fname, "./debug/kltimg_trans_diff_win%03d.%03d.pgm", glob_index, counter); printf("%s\n", fname); _KLTWriteAbsFloatImageToPGM(aff_diff_win, fname,256.0); printf("iter = %d translation tracker res: %f\n", iteration, _sumAbsFloatWindow(imgdiff, width, height)/(width*height)); #endif /* Use these windows to construct matrices */ _compute2by2GradientMatrix(gradx, grady, width, height, &gxx, &gxy, &gyy); _compute2by1ErrorVector(imgdiff, gradx, grady, width, height, step_factor, &ex, &ey); /* Using matrices, solve equation for new displacement */ status = _solveEquation(gxx, gxy, gyy, ex, ey, small, &dx, &dy); convergence = (fabs(dx) < th && fabs(dy) < th); *x2 += dx; *y2 += dy; }else{ /* affine tracker */ float ul_x = *Axx * (-hw) + *Axy * hh + *x2; /* upper left corner */ float ul_y = *Ayx * (-hw) + *Ayy * hh + *y2; float ll_x = *Axx * (-hw) + *Axy * (-hh) + *x2; /* lower left corner */ float ll_y = *Ayx * (-hw) + *Ayy * (-hh) + *y2; float ur_x = *Axx * hw + *Axy * hh + *x2; /* upper right corner */ float ur_y = *Ayx * hw + *Ayy * hh + *y2; float lr_x = *Axx * hw + *Axy * (-hh) + *x2; /* lower right corner */ float lr_y = *Ayx * hw + *Ayy * (-hh) + *y2; /* If out of bounds, exit loop */ if ( x1-hw < 0.0f || nc1-(x1+hw) < one_plus_eps || y1-hh < 0.0f || nr1-(y1+hh) < one_plus_eps || ul_x < 0.0f || nc2-(ul_x ) < one_plus_eps || ll_x < 0.0f || nc2-(ll_x ) < one_plus_eps || ur_x < 0.0f || nc2-(ur_x ) < one_plus_eps || lr_x < 0.0f || nc2-(lr_x ) < one_plus_eps || ul_y < 0.0f || nr2-(ul_y ) < one_plus_eps || ll_y < 0.0f || nr2-(ll_y ) < one_plus_eps || ur_y < 0.0f || nr2-(ur_y ) < one_plus_eps || lr_y < 0.0f || nr2-(lr_y ) < one_plus_eps) { status = KLT_OOB; break; } #ifdef DEBUG_AFFINE_MAPPING counter++; _am_computeAffineMappedImage(img1, x1, y1, 1.0, 0.0 , 0.0, 1.0, width, height, imgdiff); aff_diff_win->data = imgdiff; sprintf(fname, "./debug/kltimg_aff_diff_win%03d.%03d_1.pgm", glob_index, counter); printf("%s\n", fname); _KLTWriteAbsFloatImageToPGM(aff_diff_win, fname,256.0); _am_computeAffineMappedImage(img2, *x2, *y2, *Axx, *Ayx , *Axy, *Ayy, width, height, imgdiff); aff_diff_win->data = imgdiff; sprintf(fname, "./debug/kltimg_aff_diff_win%03d.%03d_2.pgm", glob_index, counter); printf("%s\n", fname); _KLTWriteAbsFloatImageToPGM(aff_diff_win, fname,256.0); #endif _am_computeIntensityDifferenceAffine(img1, img2, x1, y1, *x2, *y2, *Axx, *Ayx , *Axy, *Ayy, width, height, imgdiff); #ifdef DEBUG_AFFINE_MAPPING aff_diff_win->data = imgdiff; sprintf(fname, "./debug/kltimg_aff_diff_win%03d.%03d_3.pgm", glob_index,counter); printf("%s\n", fname); _KLTWriteAbsFloatImageToPGM(aff_diff_win, fname,256.0); printf("iter = %d affine tracker res: %f\n", iteration, _sumAbsFloatWindow(imgdiff, width, height)/(width*height)); #endif _am_getGradientWinAffine(gradx2, grady2, *x2, *y2, *Axx, *Ayx , *Axy, *Ayy, width, height, gradx, grady); switch(affine_map){ case 1: _am_compute4by1ErrorVector(imgdiff, gradx, grady, width, height, a); _am_compute4by4GradientMatrix(gradx, grady, width, height, T); status = _am_gauss_jordan_elimination(T,4,a,1); *Axx += a[0][0]; *Ayx += a[1][0]; *Ayy = *Axx; *Axy = -(*Ayx); dx = a[2][0]; dy = a[3][0]; break; case 2: _am_compute6by1ErrorVector(imgdiff, gradx, grady, width, height, a); _am_compute6by6GradientMatrix(gradx, grady, width, height, T); status = _am_gauss_jordan_elimination(T,6,a,1); *Axx += a[0][0]; *Ayx += a[1][0]; *Axy += a[2][0]; *Ayy += a[3][0]; dx = a[4][0]; dy = a[5][0]; break; } *x2 += dx; *y2 += dy; /* old upper left corner - new upper left corner */ ul_x -= *Axx * (-hw) + *Axy * hh + *x2; ul_y -= *Ayx * (-hw) + *Ayy * hh + *y2; /* old lower left corner - new lower left corner */ ll_x -= *Axx * (-hw) + *Axy * (-hh) + *x2; ll_y -= *Ayx * (-hw) + *Ayy * (-hh) + *y2; /* old upper right corner - new upper right corner */ ur_x -= *Axx * hw + *Axy * hh + *x2; ur_y -= *Ayx * hw + *Ayy * hh + *y2; /* old lower right corner - new lower right corner */ lr_x -= *Axx * hw + *Axy * (-hh) + *x2; lr_y -= *Ayx * hw + *Ayy * (-hh) + *y2; #ifdef DEBUG_AFFINE_MAPPING printf ("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", iteration, ul_x, ul_y, ll_x, ll_y, ur_x, ur_y, lr_x, lr_y); #endif convergence = (fabs(dx) < th && fabs(dy) < th && fabs(ul_x) < th_aff && fabs(ul_y) < th_aff && fabs(ll_x) < th_aff && fabs(ll_y) < th_aff && fabs(ur_x) < th_aff && fabs(ur_y) < th_aff && fabs(lr_x) < th_aff && fabs(lr_y) < th_aff); } if (status == KLT_SMALL_DET) break; iteration++; #ifdef DEBUG_AFFINE_MAPPING printf ("iter = %d, x1=%f, y1=%f, x2=%f, y2=%f, Axx=%f, Ayx=%f , Axy=%f, Ayy=%f \n",iteration, x1, y1, *x2, *y2, *Axx, *Ayx , *Axy, *Ayy); #endif } while ( !convergence && iteration < max_iterations); /*} while ( (fabs(dx)>=th || fabs(dy)>=th || (affine_map && iteration < 8) ) && iteration < max_iterations); */ _am_free_matrix(T); _am_free_matrix(a); /* Check whether window is out of bounds */ if (*x2-hw < 0.0f || nc2-(*x2+hw) < one_plus_eps || *y2-hh < 0.0f || nr2-(*y2+hh) < one_plus_eps) status = KLT_OOB; /* Check whether feature point has moved to much during iteration*/ if ( (*x2-old_x2) > mdd || (*y2-old_y2) > mdd ) status = KLT_OOB; /* Check whether residue is too large */ if (status == KLT_TRACKED) { if(!affine_map){ _computeIntensityDifference(img1, img2, x1, y1, *x2, *y2, width, height, imgdiff); }else{ _am_computeIntensityDifferenceAffine(img1, img2, x1, y1, *x2, *y2, *Axx, *Ayx , *Axy, *Ayy, width, height, imgdiff); } #ifdef DEBUG_AFFINE_MAPPING printf("iter = %d final_res = %f\n", iteration, _sumAbsFloatWindow(imgdiff, width, height)/(width*height)); #endif if (_sumAbsFloatWindow(imgdiff, width, height)/(width*height) > max_residue) status = KLT_LARGE_RESIDUE; } /* Free memory */ free(imgdiff); free(gradx); free(grady); #ifdef DEBUG_AFFINE_MAPPING printf("iter = %d status=%d\n", iteration, status); _KLTFreeFloatImage( aff_diff_win ); #endif /* Return appropriate value */ return status; }
void KLTTrackFeatures( KLT_TrackingContext tc, KLT_PixelType *img1, KLT_PixelType *img2, int ncols, int nrows, KLT_FeatureList featurelist) { _KLT_FloatImage tmpimg, floatimg1, floatimg2; _KLT_Pyramid pyramid1, pyramid1_gradx, pyramid1_grady, pyramid2, pyramid2_gradx, pyramid2_grady; float subsampling = (float) tc->subsampling; float xloc, yloc, xlocout, ylocout; int val; int indx, r; KLT_BOOL floatimg1_created = FALSE; int i; if (KLT_verbose >= 1) { fprintf(stderr, "(KLT) Tracking %d features in a %d by %d image... ", KLTCountRemainingFeatures(featurelist), ncols, nrows); fflush(stderr); } /* Check window size (and correct if necessary) */ if (tc->window_width % 2 != 1) { tc->window_width = tc->window_width+1; KLTWarning("Tracking context's window width must be odd. " "Changing to %d.\n", tc->window_width); } if (tc->window_height % 2 != 1) { tc->window_height = tc->window_height+1; KLTWarning("Tracking context's window height must be odd. " "Changing to %d.\n", tc->window_height); } if (tc->window_width < 3) { tc->window_width = 3; KLTWarning("Tracking context's window width must be at least three. \n" "Changing to %d.\n", tc->window_width); } if (tc->window_height < 3) { tc->window_height = 3; KLTWarning("Tracking context's window height must be at least three. \n" "Changing to %d.\n", tc->window_height); } /* Create temporary image */ tmpimg = _KLTCreateFloatImage(ncols, nrows); /* Process first image by converting to float, smoothing, computing */ /* pyramid, and computing gradient pyramids */ if (tc->sequentialMode && tc->pyramid_last != NULL) { pyramid1 = (_KLT_Pyramid) tc->pyramid_last; pyramid1_gradx = (_KLT_Pyramid) tc->pyramid_last_gradx; pyramid1_grady = (_KLT_Pyramid) tc->pyramid_last_grady; if (pyramid1->ncols[0] != ncols || pyramid1->nrows[0] != nrows) KLTError("(KLTTrackFeatures) Size of incoming image (%d by %d) " "is different from size of previous image (%d by %d)\n", ncols, nrows, pyramid1->ncols[0], pyramid1->nrows[0]); assert(pyramid1_gradx != NULL); assert(pyramid1_grady != NULL); } else { floatimg1_created = TRUE; floatimg1 = _KLTCreateFloatImage(ncols, nrows); _KLTToFloatImage(img1, ncols, nrows, tmpimg); _KLTComputeSmoothedImage(tmpimg, _KLTComputeSmoothSigma(tc), floatimg1); pyramid1 = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels); _KLTComputePyramid(floatimg1, pyramid1, tc->pyramid_sigma_fact); pyramid1_gradx = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels); pyramid1_grady = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels); for (i = 0 ; i < tc->nPyramidLevels ; i++) _KLTComputeGradients(pyramid1->img[i], tc->grad_sigma, pyramid1_gradx->img[i], pyramid1_grady->img[i]); } /* Do the same thing with second image */ floatimg2 = _KLTCreateFloatImage(ncols, nrows); _KLTToFloatImage(img2, ncols, nrows, tmpimg); _KLTComputeSmoothedImage(tmpimg, _KLTComputeSmoothSigma(tc), floatimg2); pyramid2 = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels); _KLTComputePyramid(floatimg2, pyramid2, tc->pyramid_sigma_fact); pyramid2_gradx = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels); pyramid2_grady = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels); for (i = 0 ; i < tc->nPyramidLevels ; i++) _KLTComputeGradients(pyramid2->img[i], tc->grad_sigma, pyramid2_gradx->img[i], pyramid2_grady->img[i]); /* Write internal images */ if (tc->writeInternalImages) { char fname[80]; for (i = 0 ; i < tc->nPyramidLevels ; i++) { sprintf(fname, "kltimg_tf_i%d.pgm", i); _KLTWriteFloatImageToPGM(pyramid1->img[i], fname); sprintf(fname, "kltimg_tf_i%d_gx.pgm", i); _KLTWriteFloatImageToPGM(pyramid1_gradx->img[i], fname); sprintf(fname, "kltimg_tf_i%d_gy.pgm", i); _KLTWriteFloatImageToPGM(pyramid1_grady->img[i], fname); sprintf(fname, "kltimg_tf_j%d.pgm", i); _KLTWriteFloatImageToPGM(pyramid2->img[i], fname); sprintf(fname, "kltimg_tf_j%d_gx.pgm", i); _KLTWriteFloatImageToPGM(pyramid2_gradx->img[i], fname); sprintf(fname, "kltimg_tf_j%d_gy.pgm", i); _KLTWriteFloatImageToPGM(pyramid2_grady->img[i], fname); } } /* For each feature, do ... */ for (indx = 0 ; indx < featurelist->nFeatures ; indx++) { /* Only track features that are not lost */ if (featurelist->feature[indx]->val >= 0) { xloc = featurelist->feature[indx]->x; yloc = featurelist->feature[indx]->y; /* Transform location to coarsest resolution */ for (r = tc->nPyramidLevels - 1 ; r >= 0 ; r--) { xloc /= subsampling; yloc /= subsampling; } xlocout = xloc; ylocout = yloc; /* Beginning with coarsest resolution, do ... */ for (r = tc->nPyramidLevels - 1 ; r >= 0 ; r--) { /* Track feature at current resolution */ xloc *= subsampling; yloc *= subsampling; xlocout *= subsampling; ylocout *= subsampling; val = _trackFeature(xloc, yloc, &xlocout, &ylocout, pyramid1->img[r], pyramid1_gradx->img[r], pyramid1_grady->img[r], pyramid2->img[r], pyramid2_gradx->img[r], pyramid2_grady->img[r], tc->window_width, tc->window_height, tc->step_factor, tc->max_iterations, tc->min_determinant, tc->min_displacement, tc->max_residue, tc->lighting_insensitive); if (val==KLT_SMALL_DET || val==KLT_OOB) break; } /* Record feature */ if (val == KLT_OOB) { featurelist->feature[indx]->x = -1.0; featurelist->feature[indx]->y = -1.0; featurelist->feature[indx]->val = KLT_OOB; if( featurelist->feature[indx]->aff_img ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img); if( featurelist->feature[indx]->aff_img_gradx ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_gradx); if( featurelist->feature[indx]->aff_img_grady ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_grady); featurelist->feature[indx]->aff_img = NULL; featurelist->feature[indx]->aff_img_gradx = NULL; featurelist->feature[indx]->aff_img_grady = NULL; } else if (_outOfBounds(xlocout, ylocout, ncols, nrows, tc->borderx, tc->bordery)) { featurelist->feature[indx]->x = -1.0; featurelist->feature[indx]->y = -1.0; featurelist->feature[indx]->val = KLT_OOB; if( featurelist->feature[indx]->aff_img ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img); if( featurelist->feature[indx]->aff_img_gradx ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_gradx); if( featurelist->feature[indx]->aff_img_grady ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_grady); featurelist->feature[indx]->aff_img = NULL; featurelist->feature[indx]->aff_img_gradx = NULL; featurelist->feature[indx]->aff_img_grady = NULL; } else if (val == KLT_SMALL_DET) { featurelist->feature[indx]->x = -1.0; featurelist->feature[indx]->y = -1.0; featurelist->feature[indx]->val = KLT_SMALL_DET; if( featurelist->feature[indx]->aff_img ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img); if( featurelist->feature[indx]->aff_img_gradx ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_gradx); if( featurelist->feature[indx]->aff_img_grady ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_grady); featurelist->feature[indx]->aff_img = NULL; featurelist->feature[indx]->aff_img_gradx = NULL; featurelist->feature[indx]->aff_img_grady = NULL; } else if (val == KLT_LARGE_RESIDUE) { featurelist->feature[indx]->x = -1.0; featurelist->feature[indx]->y = -1.0; featurelist->feature[indx]->val = KLT_LARGE_RESIDUE; if( featurelist->feature[indx]->aff_img ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img); if( featurelist->feature[indx]->aff_img_gradx ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_gradx); if( featurelist->feature[indx]->aff_img_grady ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_grady); featurelist->feature[indx]->aff_img = NULL; featurelist->feature[indx]->aff_img_gradx = NULL; featurelist->feature[indx]->aff_img_grady = NULL; } else if (val == KLT_MAX_ITERATIONS) { featurelist->feature[indx]->x = -1.0; featurelist->feature[indx]->y = -1.0; featurelist->feature[indx]->val = KLT_MAX_ITERATIONS; if( featurelist->feature[indx]->aff_img ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img); if( featurelist->feature[indx]->aff_img_gradx ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_gradx); if( featurelist->feature[indx]->aff_img_grady ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_grady); featurelist->feature[indx]->aff_img = NULL; featurelist->feature[indx]->aff_img_gradx = NULL; featurelist->feature[indx]->aff_img_grady = NULL; } else { featurelist->feature[indx]->x = xlocout; featurelist->feature[indx]->y = ylocout; featurelist->feature[indx]->val = KLT_TRACKED; if (tc->affineConsistencyCheck >= 0 && val == KLT_TRACKED) { /*for affine mapping*/ int border = 2; /* add border for interpolation */ #ifdef DEBUG_AFFINE_MAPPING glob_index = indx; #endif if(!featurelist->feature[indx]->aff_img){ /* save image and gradient for each feature at finest resolution after first successful track */ featurelist->feature[indx]->aff_img = _KLTCreateFloatImage((tc->affine_window_width+border), (tc->affine_window_height+border)); featurelist->feature[indx]->aff_img_gradx = _KLTCreateFloatImage((tc->affine_window_width+border), (tc->affine_window_height+border)); featurelist->feature[indx]->aff_img_grady = _KLTCreateFloatImage((tc->affine_window_width+border), (tc->affine_window_height+border)); _am_getSubFloatImage(pyramid1->img[0],xloc,yloc,featurelist->feature[indx]->aff_img); _am_getSubFloatImage(pyramid1_gradx->img[0],xloc,yloc,featurelist->feature[indx]->aff_img_gradx); _am_getSubFloatImage(pyramid1_grady->img[0],xloc,yloc,featurelist->feature[indx]->aff_img_grady); featurelist->feature[indx]->aff_x = xloc - (int) xloc + (tc->affine_window_width+border)/2; featurelist->feature[indx]->aff_y = yloc - (int) yloc + (tc->affine_window_height+border)/2;; }else{ /* affine tracking */ val = _am_trackFeatureAffine(featurelist->feature[indx]->aff_x, featurelist->feature[indx]->aff_y, &xlocout, &ylocout, featurelist->feature[indx]->aff_img, featurelist->feature[indx]->aff_img_gradx, featurelist->feature[indx]->aff_img_grady, pyramid2->img[0], pyramid2_gradx->img[0], pyramid2_grady->img[0], tc->affine_window_width, tc->affine_window_height, tc->step_factor, tc->affine_max_iterations, tc->min_determinant, tc->min_displacement, tc->affine_min_displacement, tc->affine_max_residue, tc->lighting_insensitive, tc->affineConsistencyCheck, tc->affine_max_displacement_differ, &featurelist->feature[indx]->aff_Axx, &featurelist->feature[indx]->aff_Ayx, &featurelist->feature[indx]->aff_Axy, &featurelist->feature[indx]->aff_Ayy ); featurelist->feature[indx]->val = val; if(val != KLT_TRACKED){ featurelist->feature[indx]->x = -1.0; featurelist->feature[indx]->y = -1.0; featurelist->feature[indx]->aff_x = -1.0; featurelist->feature[indx]->aff_y = -1.0; /* free image and gradient for lost feature */ _KLTFreeFloatImage(featurelist->feature[indx]->aff_img); _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_gradx); _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_grady); featurelist->feature[indx]->aff_img = NULL; featurelist->feature[indx]->aff_img_gradx = NULL; featurelist->feature[indx]->aff_img_grady = NULL; }else{ /*featurelist->feature[indx]->x = xlocout;*/ /*featurelist->feature[indx]->y = ylocout;*/ } } } } } } if (tc->sequentialMode) { tc->pyramid_last = pyramid2; tc->pyramid_last_gradx = pyramid2_gradx; tc->pyramid_last_grady = pyramid2_grady; } else { _KLTFreePyramid(pyramid2); _KLTFreePyramid(pyramid2_gradx); _KLTFreePyramid(pyramid2_grady); } /* Free memory */ _KLTFreeFloatImage(tmpimg); if (floatimg1_created) _KLTFreeFloatImage(floatimg1); _KLTFreeFloatImage(floatimg2); _KLTFreePyramid(pyramid1); _KLTFreePyramid(pyramid1_gradx); _KLTFreePyramid(pyramid1_grady); if (KLT_verbose >= 1) { fprintf(stderr, "\n\t%d features successfully tracked.\n", KLTCountRemainingFeatures(featurelist)); if (tc->writeInternalImages) fprintf(stderr, "\tWrote images to 'kltimg_tf*.pgm'.\n"); fflush(stderr); } }