51 #include "visp/vpConfig.h"
53 #ifdef VISP_HAVE_OPENCV
57 #include "vpKltOpencv.h"
59 void vpKltOpencv::clean()
61 if (image) cvReleaseImage(&image);
62 if (prev_image) cvReleaseImage(&prev_image);
63 if (pyramid) cvReleaseImage(&pyramid);
64 if (prev_pyramid) cvReleaseImage(&prev_pyramid);
73 countPrevFeatures = 0;
76 globalcountFeatures = 0;
79 void vpKltOpencv::cleanAll()
82 if (features) cvFree(&features);
83 if (prev_features) cvFree(&prev_features);
84 if (status) cvFree(&status);
85 if (lostDuringTrack) cvFree(&lostDuringTrack);
86 if (featuresid) cvFree(&featuresid);
87 if (prev_featuresid) cvFree(&prev_featuresid);
96 void vpKltOpencv::reset()
108 countPrevFeatures = 0;
109 globalcountFeatures = 0;
116 harris_free_parameter = 0.04;
134 OnMeasureFeature = 0;
137 features = (CvPoint2D32f*)cvAlloc((
unsigned int)maxFeatures*
sizeof(features[0]));
138 prev_features = (CvPoint2D32f*)cvAlloc((
unsigned int)maxFeatures*
sizeof(prev_features[0]));
139 status = (
char*)cvAlloc((
size_t)maxFeatures);
140 lostDuringTrack = (
bool*)cvAlloc((
size_t)maxFeatures);
141 featuresid = (
long*)cvAlloc((
unsigned int)maxFeatures*
sizeof(long));
142 prev_featuresid = (
long*)cvAlloc((
unsigned int)maxFeatures*
sizeof(long));
151 initialized = copy.initialized;
152 maxFeatures = copy.maxFeatures;
153 countFeatures = copy.countFeatures;
154 countPrevFeatures = copy.countPrevFeatures;
155 globalcountFeatures = copy.globalcountFeatures;
157 win_size = copy.win_size;
158 quality = copy.quality;
159 min_distance = copy.min_distance;
160 harris_free_parameter = copy.harris_free_parameter;
161 block_size = copy.block_size;
162 use_harris = copy.use_harris;
163 pyramid_level = copy.pyramid_level;
166 OnInitialize = copy.OnInitialize;
167 OnFeatureLost = copy.OnFeatureLost;
168 OnNewFeature = copy.OnNewFeature;
169 OnMeasureFeature = copy.OnMeasureFeature;
170 IsFeatureValid = copy.IsFeatureValid;
185 countPrevFeatures = 0;
188 globalcountFeatures = 0;
193 image = cvCreateImage(cvGetSize(copy.image), 8, 1);
195 cvCopy(copy.image, image, 0);
200 prev_image = cvCreateImage(cvGetSize(copy.prev_image), IPL_DEPTH_8U, 1);
202 cvCopy(copy.prev_image, prev_image, 0);
207 pyramid = cvCreateImage(cvGetSize(copy.pyramid), IPL_DEPTH_8U, 1);
209 cvCopy(copy.pyramid, pyramid, 0);
212 if (copy.prev_pyramid)
214 prev_pyramid = cvCreateImage(cvGetSize(copy.prev_pyramid), IPL_DEPTH_8U, 1);
216 cvCopy(copy.prev_pyramid, prev_pyramid, 0);
222 (CvPoint2D32f*)cvAlloc((
unsigned int)copy.maxFeatures*
sizeof(CvPoint2D32f));
223 for (
int i = 0; i < copy.maxFeatures; i++)
224 features[i] = copy.features[i];
227 if (copy.prev_features) {
229 (CvPoint2D32f*)cvAlloc((
unsigned int)copy.maxFeatures*
sizeof(CvPoint2D32f));
230 for (
int i = 0; i < copy.maxFeatures; i++)
231 prev_features[i] = copy.prev_features[i];
234 if (copy.featuresid) {
235 featuresid = (
long*)cvAlloc((
unsigned int)copy.maxFeatures*
sizeof(long));
236 for (
int i = 0; i < copy.maxFeatures; i++)
237 featuresid[i] = copy.featuresid[i];
240 if (copy.prev_featuresid) {
241 prev_featuresid = (
long*)cvAlloc((
unsigned int)copy.maxFeatures*
sizeof(long));
242 for (
int i = 0; i < copy.maxFeatures; i++)
243 prev_featuresid[i] = copy.prev_featuresid[i];
247 status = (
char*)cvAlloc((
unsigned int)copy.maxFeatures*
sizeof(char));
248 for (
int i = 0; i < copy.maxFeatures; i++)
249 status[i] = copy.status[i];
252 if (copy.lostDuringTrack) {
253 lostDuringTrack = (
bool*)cvAlloc((
unsigned int)copy.maxFeatures*
sizeof(bool));
254 for (
int i = 0; i < copy.maxFeatures; i++)
255 lostDuringTrack[i] = copy.lostDuringTrack[i];
272 initialized = 0; maxFeatures=input;
274 if (features) cvFree(&features);
275 if (prev_features) cvFree(&prev_features);
276 if (status) cvFree(&status);
277 if (lostDuringTrack) cvFree(&lostDuringTrack);
278 if (featuresid) cvFree(&featuresid);
279 if (prev_featuresid) cvFree(&prev_featuresid);
282 features = (CvPoint2D32f*)cvAlloc((
unsigned int)maxFeatures*
sizeof(CvPoint2D32f));
283 prev_features = (CvPoint2D32f*)cvAlloc((
unsigned int)maxFeatures*
sizeof(CvPoint2D32f));
284 status = (
char*)cvAlloc((
unsigned int)maxFeatures*
sizeof(char));
285 lostDuringTrack = (
bool*)cvAlloc((
unsigned int)maxFeatures*
sizeof(bool));
286 featuresid = (
long*)cvAlloc((
unsigned int)maxFeatures*
sizeof(long));
287 prev_featuresid = (
long*)cvAlloc((
unsigned int)maxFeatures*
sizeof(long));
296 if (I->depth != IPL_DEPTH_8U || I->nChannels != 1) {
301 if (masque->depth != IPL_DEPTH_8U || I->nChannels != 1) {
308 CvSize Sizeim, SizeI;
309 SizeI = cvGetSize(I);
312 Sizeim = cvGetSize(image);
313 if(SizeI.width != Sizeim.width || SizeI.height != Sizeim.height) b_imOK =
false;
315 if(image == NULL || prev_image == NULL || pyramid==NULL || prev_pyramid ==NULL || !b_imOK){
317 image = cvCreateImage(cvGetSize(I), 8, 1);image->origin = I->origin;
318 prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
319 pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
320 prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
324 countPrevFeatures = 0;
327 globalcountFeatures = 0;
336 countFeatures = maxFeatures;
337 countPrevFeatures = 0;
338 IplImage* eig = cvCreateImage(cvGetSize(image), 32, 1);
339 IplImage* temp = cvCreateImage(cvGetSize(image), 32, 1);
340 cvGoodFeaturesToTrack(image, eig, temp, features,
341 &countFeatures, quality, min_distance,
342 masque, block_size, use_harris, harris_free_parameter);
343 cvFindCornerSubPix(image, features, countFeatures, cvSize(win_size, win_size),
344 cvSize(-1,-1),cvTermCriteria(CV_TERMCRIT_ITER|
345 CV_TERMCRIT_EPS,20,0.03));
346 cvReleaseImage(&eig);
347 cvReleaseImage(&temp);
353 for (
int boucle=0; boucle<countFeatures;boucle++) {
354 featuresid[boucle] = globalcountFeatures;
355 globalcountFeatures++;
358 OnNewFeature(_tid, boucle, featuresid[boucle], features[boucle].x,
370 "KLT Not initialized")) ;
375 "Image Not initialized")) ;
378 if (I->depth != IPL_DEPTH_8U || I->nChannels != 1) {
380 "Bad Image format")) ;
384 countPrevFeatures = countFeatures;
385 for (
int boucle=0; boucle<countFeatures;boucle++) {
386 prev_featuresid[boucle] = featuresid[boucle];
389 CvPoint2D32f *swap_features = 0;
391 CV_SWAP(prev_image, image, swap_temp);
392 CV_SWAP(prev_pyramid, pyramid, swap_temp);
393 CV_SWAP(prev_features, features, swap_features);
397 if (countFeatures <= 0)
return;
399 cvCalcOpticalFlowPyrLK( prev_image, image, prev_pyramid, pyramid,
400 prev_features, features, countFeatures,
401 cvSize(win_size, win_size), pyramid_level,
402 status, 0, cvTermCriteria(CV_TERMCRIT_ITER
403 |CV_TERMCRIT_EPS,20,0.03),
405 flags |= CV_LKFLOW_PYR_A_READY;
408 for (i = k = 0; i < countFeatures ; i++) {
410 lostDuringTrack[i] = 1;
412 OnFeatureLost(_tid, i, featuresid[i], features[i].x,
417 if (IsFeatureValid) {
418 if (!IsFeatureValid(_tid, features[i].x, features[i].y)) {
419 lostDuringTrack[i] = 1;
421 OnFeatureLost(_tid, i, featuresid[i], features[i].x, features[i].y);
425 features[k] = features[i];
426 featuresid[k] = featuresid[i];
428 if (OnMeasureFeature) OnMeasureFeature(_tid, k, featuresid[k], features[k].x, features[k].y);
430 lostDuringTrack[i] = 0;
439 if ((features == 0) || (I.
bitmap==0) || (!initialized))
458 if (index >= countFeatures)
464 x = features[index].x;
465 y = features[index].y;
466 id = featuresid[index];
479 if (index >= countPrevFeatures)
485 x = prev_features[index].x;
486 y = prev_features[index].y;
487 id = prev_featuresid[index];
497 const float &x,
const float &y)
499 if (maxFeatures == countFeatures)
508 features[countFeatures] = f;
509 featuresid[countFeatures] = id;
515 if (index >= countFeatures)
523 for (
int i=index ; i < countFeatures; i ++) {
524 features[i] = features[i+1];
525 featuresid[i] = featuresid[i+1];
544 const int &nbFeatures,
vpColor color,
unsigned int thickness)
547 for (
int i = 0 ; i < nbFeatures ; i++)
572 const long *featuresid_list,
const int &nbFeatures,
573 vpColor color,
unsigned int thickness)
576 for (
int i = 0 ; i < nbFeatures ; i++)
583 sprintf(
id,
"%ld", featuresid_list[i]);
void track(const IplImage *I)
Type * bitmap
points toward the bitmap
Class to define colors available for display functionnalities.
void addFeature(const int &id, const float &x, const float &y)
static int round(const double x)
void getFeature(int index, int &id, float &x, float &y) const
void set_u(const double u)
virtual void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)=0
void set_v(const double v)
void getPrevFeature(int index, int &id, float &x, float &y) const
void initTracking(const IplImage *I, const IplImage *mask=NULL)
virtual void displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color=vpColor::green)=0
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented with OpenCV.
void suppressFeature(int index)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void setMaxFeatures(const int input)
void display(const vpImage< unsigned char > &I, vpColor color=vpColor::red)