TrackerFeatures.cpp
Go to the documentation of this file.
00001 /*
00002  * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
00003  *
00004  * Copyright 2007-2012 VTT Technical Research Centre of Finland
00005  *
00006  * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
00007  *          <http://www.vtt.fi/multimedia/alvar.html>
00008  *
00009  * ALVAR is free software; you can redistribute it and/or modify it under the
00010  * terms of the GNU Lesser General Public License as published by the Free
00011  * Software Foundation; either version 2.1 of the License, or (at your option)
00012  * any later version.
00013  *
00014  * This library is distributed in the hope that it will be useful, but WITHOUT
00015  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00016  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
00017  * for more details.
00018  *
00019  * You should have received a copy of the GNU Lesser General Public License
00020  * along with ALVAR; if not, see
00021  * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
00022  */
00023 
00024 #include "TrackerFeatures.h"
00025 #include <cv.h>
00026 
00027 using namespace std;
00028 
00029 namespace alvar {
00030 using namespace std;
00031 
00032 TrackerFeatures::TrackerFeatures(int _max_features, int _min_features, double _quality_level, double _min_distance, int _pyr_levels, int _win_size) :
00033         x_res(0), y_res(0), frame_count(0), quality_level(0), min_distance(0), min_features(0), max_features(0), status(0),
00034         img_eig(0), img_tmp(0), gray(0), prev_gray(0), pyramid(0), prev_pyramid(0), mask(0), next_id(0), win_size(0), pyr_levels(0),
00035         prev_features(0), features(0), prev_feature_count(0), feature_count(0), prev_ids(0), ids(0)
00036 {
00037         next_id=1; // When this should be reset?
00038         pyr_levels = _pyr_levels;
00039         win_size   = _win_size;
00040         ChangeSettings(_max_features, _min_features, _quality_level, _min_distance);
00041 }
00042 
00043 TrackerFeatures::~TrackerFeatures() {
00044         if (status) delete [] status;
00045         if (prev_features) delete [] prev_features;
00046         if (features) delete [] features;
00047         if (prev_ids) delete [] prev_ids;
00048         if (ids) delete [] ids;
00049         if (img_eig) cvReleaseImage(&img_eig);
00050         if (img_tmp) cvReleaseImage(&img_tmp);
00051         if (gray) cvReleaseImage(&gray);
00052         if (prev_gray) cvReleaseImage(&prev_gray);
00053         if (pyramid) cvReleaseImage(&pyramid);
00054         if (prev_pyramid) cvReleaseImage(&prev_pyramid);
00055         if (mask) cvReleaseImage(&mask);
00056 }
00057 
00058 void TrackerFeatures::ChangeSettings(int _max_features, int _min_features, double _quality_level, double _min_distance) {
00059 
00060         if(_max_features==max_features && _min_features==min_features && _quality_level==quality_level && _min_distance==min_distance) return;
00061 
00062         int common_features = min(feature_count, _max_features);
00063         max_features = _max_features;
00064         min_features = _min_features;
00065         quality_level = _quality_level;
00066         min_distance = _min_distance;
00067         if (status) delete [] status; status = NULL;
00068         if (prev_ids) delete [] prev_ids; prev_ids = NULL;
00069         if (prev_features) delete [] prev_features; prev_features = NULL;
00070         if (ids) {
00071                 int *ids_new = new int[max_features];
00072                 assert(common_features < max_features);
00073                 memcpy(ids_new, ids, sizeof(int)*common_features);
00074                 delete [] ids;
00075                 ids = ids_new;
00076         } else {
00077                 ids = new int[max_features];
00078         }
00079         if (features) {
00080                 CvPoint2D32f *features_new = new CvPoint2D32f[max_features];
00081                 memcpy(features_new, features, sizeof(CvPoint2D32f)*common_features);
00082                 delete [] features;
00083                 features = features_new;
00084         } else {
00085                 features = new CvPoint2D32f[max_features];
00086         }
00087         status = new char[max_features];
00088         prev_ids = new int[max_features];
00089         prev_features = new CvPoint2D32f[max_features];
00090         prev_feature_count=0;
00091         feature_count=common_features;
00092 
00093         assert(ids);
00094         assert(features);
00095         assert(status);
00096         assert(prev_ids);
00097         assert(prev_features);
00098 }
00099 
00100 void TrackerFeatures::Reset() {
00101         feature_count=0;
00102         frame_count=0;
00103 }
00104 
00105 bool TrackerFeatures::DelFeature(int index) {
00106         if (index > feature_count) return false;
00107         feature_count--;
00108         for (int i=index; i<feature_count; i++) {
00109                 features[i] = features[i+1];
00110                 ids[i] = ids[i+1];
00111         }
00112         return true;
00113 }
00114 
00115 bool TrackerFeatures::DelFeatureId(int id) {
00116         for (int i=0; i<feature_count; i++) {
00117                 if (ids[i] == id) return DelFeature(i);
00118         }
00119         return false;
00120 }
00121 
00122 int TrackerFeatures::Purge() {
00123         int removed_count=0;
00124         float dist = 0.7f*float(min_distance);
00125         for (int i=1; i<feature_count; i++) {
00126                 for (int ii=0; ii<i; ii++) {
00127                         float dx = features[i].x - features[ii].x;
00128                         float dy = features[i].y - features[ii].y;
00129                         if (dx < 0) dx = -dx; if (dy < 0) dy = -dy;
00130                         if ((dx < dist) && (dy < dist)) {
00131                                 DelFeature(i); i--;
00132                                 removed_count++;
00133                                 break;
00134                         }
00135                 }
00136         }
00137         return removed_count;
00138 }
00139 
00140 double TrackerFeatures::TrackHid(IplImage *img, IplImage *new_features_mask, bool add_features) {
00141         if ((x_res != img->width) || (y_res != img->height)) {
00142                 if (img_eig) cvReleaseImage(&img_eig);
00143                 if (img_tmp) cvReleaseImage(&img_tmp);
00144                 if (gray) cvReleaseImage(&gray);
00145                 if (prev_gray) cvReleaseImage(&prev_gray);
00146                 if (pyramid) cvReleaseImage(&pyramid);
00147                 if (prev_pyramid) cvReleaseImage(&prev_pyramid);
00148                 if (mask) cvReleaseImage(&mask);
00149                 x_res = img->width; y_res = img->height;
00150                 img_eig = cvCreateImage(cvGetSize(img), IPL_DEPTH_32F, 1);
00151                 img_tmp = cvCreateImage(cvGetSize(img), IPL_DEPTH_32F, 1);
00152                 gray = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
00153                 prev_gray = cvCreateImage(cvGetSize(img),IPL_DEPTH_8U, 1);
00154                 pyramid = cvCreateImage(cvSize(img->width+8,img->height/3), IPL_DEPTH_8U, 1);
00155                 prev_pyramid = cvCreateImage(cvSize(img->width+8,img->height/3), IPL_DEPTH_8U, 1);
00156                 mask = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
00157                 frame_count=0;
00158                 if (min_distance == 0) {
00159                         min_distance = std::sqrt(double(img->width*img->height/max_features));
00160                         min_distance *= 0.8; //(double(min_features)/max_features);
00161                 }
00162         }
00163         // Swap 
00164         IplImage* tmp;
00165         CvPoint2D32f* tmp2;
00166         CV_SWAP(prev_gray, gray, tmp);
00167         CV_SWAP(prev_features, features, tmp2);
00168         prev_feature_count=feature_count;
00169         memcpy(prev_ids, ids, sizeof(int)*max_features);
00170         if (img->nChannels == 1) {
00171                 cvCopy(img, gray);
00172         } else {
00173                 cvCvtColor(img, gray, CV_RGB2GRAY);
00174         }
00175         // TODO: We used to add features here
00176         //if (prev_feature_count < 1) return -1;
00177         frame_count++;
00178         if (frame_count <= 1) {
00179                 memcpy(features, prev_features, sizeof(CvPoint2D32f)*prev_feature_count);
00180                 memcpy(ids, prev_ids, sizeof(int)*prev_feature_count);
00181                 feature_count = prev_feature_count;
00182         } else if (prev_feature_count > 0) {
00183                 // Track
00184                 cvCalcOpticalFlowPyrLK(prev_gray, gray, prev_pyramid, pyramid,
00185                         prev_features, features, prev_feature_count, cvSize(win_size, win_size), pyr_levels, status, 0,
00186                         cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03), 0);
00187                 feature_count=0;
00188                 for (int i=0; i<prev_feature_count; i++) {
00189                         if (!status[i]) continue;
00190                         features[feature_count] = features[i]; 
00191                         ids[feature_count] = prev_ids[i];
00192                         feature_count++;
00193                 }
00194         }
00195 
00196         if (add_features) AddFeatures(new_features_mask);
00197 
00198         return 1;
00199 }
00200 
00201 double TrackerFeatures::Reset(IplImage *img, IplImage *new_features_mask) {
00202         feature_count=0;
00203         frame_count=0;
00204         return TrackHid(img, new_features_mask);
00205 }
00206 
00207 double TrackerFeatures::Track(IplImage *img, bool add_features) {
00208         return TrackHid(img, NULL); //, add_features);
00209 }
00210 
00211 double TrackerFeatures::Track(IplImage* img, IplImage* mask)
00212 {
00213         return TrackHid(img, mask); //, true);
00214 }
00215 
00216 IplImage *TrackerFeatures::NewFeatureMask() {
00217         cvSet(mask, cvScalar(255));
00218         for (int i=0; i<feature_count; i++) {
00219                 cvRectangle(mask, 
00220                                         cvPoint(int(features[i].x-min_distance), int(features[i].y-min_distance)), 
00221                                         cvPoint(int(features[i].x+min_distance), int(features[i].y+min_distance)), 
00222                                         cvScalar(0), CV_FILLED);
00223         }
00224         return mask;
00225 }
00226 
00227 int TrackerFeatures::AddFeatures(IplImage *new_features_mask) {
00228         if (gray == NULL) return 0;
00229         if (feature_count < min_features) {
00230                 int new_feature_count=max_features-feature_count;
00231                 if (new_features_mask == NULL) {
00232                         cvSet(mask, cvScalar(255));
00233                         for (int i=0; i<feature_count; i++) {
00234                                 cvRectangle(mask, 
00235                                                         cvPoint(int(features[i].x-min_distance), int(features[i].y-min_distance)), 
00236                                                         cvPoint(int(features[i].x+min_distance), int(features[i].y+min_distance)), 
00237                                                         cvScalar(0), CV_FILLED);
00238                         }
00239                         // Find new features
00240                         cvGoodFeaturesToTrack(gray, img_eig, img_tmp,
00241                                                                 &(features[feature_count]), &new_feature_count,
00242                                                                 quality_level, min_distance, mask, 3, 1, 0.04);
00243 
00244                 } else {
00245                         cvGoodFeaturesToTrack(gray, img_eig, img_tmp,
00246                                                                 &(features[feature_count]), &new_feature_count,
00247                                                                 quality_level, min_distance, new_features_mask, 3, 1, 0.04);
00248 
00249                 }
00250                 if (new_feature_count >= 1) {
00251                         for (int i=feature_count; i<feature_count+new_feature_count; i++) {
00252                                 ids[i] = next_id;
00253                                 next_id=((next_id+1)%(0x7fff));
00254                         }
00255                         feature_count += new_feature_count;
00256                 }
00257         }
00258         return feature_count;
00259 }
00260 
00261 } // namespace alvar


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Thu Feb 16 2017 03:23:02