ACT-CV - Machine Vision for Cognitive Modeling
OpticalFlow.cpp
Go to the documentation of this file.
1 // -*- mode: c++; indent-tabs-mode: nil; c-basic-offset: 4; coding: iso-8859-1; -*-
2 
3 /*
4 ACT-CV - Machine Vision for Cognitive Modeling
5 Copyright (c) 2008 Marc Halbruegge
6 
7 This program is free software; you can redistribute it and/or modify
8 it under the terms of the GNU General Public License as published by
9 the Free Software Foundation; either version 2 of the License, or
10 (at your option) any later version.
11 
12 This program is distributed in the hope that it will be useful,
13 but WITHOUT ANY WARRANTY; without even the implied warranty of
14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 GNU General Public License for more details.
16 
17 You should have received a copy of the GNU General Public License
18 along with this program; if not, write to the Free Software
19 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
20 */
21 
22 
28 #ifdef HAVE_CONFIG_H
29 #include <config.h>
30 #endif
31 
32 #include <mhexcptn.h>
33 #include <vector>
34 #define _USE_MATH_DEFINES
35 #include <cmath>
36 #include <boost/math/special_functions/fpclassify.hpp> // isnan
37 
38 #include <statistiken.h>
39 #include <linien/linien.h>
40 #include "OpticalFlow.h"
41 
42 using namespace std;
43 
44 OpticalFlow::OpticalFlow()
45  : prevFeatures(0),
46  curFeatures(0),
47  featStatus(0),
48  numCalls(0),
49  mask(0) {
50  focusOfExpansion.setNeuAnteil(.25);
51 
52  // initial mean, initial sd, process-noise, measurement-noise
53  kalmanFOE.InitRandom(160,100,.25,5);
54 }
55 
56 OpticalFlow::~OpticalFlow() {
57  MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
58  FreeStatusData();
59  MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
60  eigenBuf=nullptr;
61  MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
62  tempBuf=nullptr;
63  MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
64  imgBuf.Add(nullptr);
65  imgBuf.Add(nullptr);
66  MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
67  pyrBuf.Add(nullptr);
68  pyrBuf.Add(nullptr);
69  MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
70  cvReleaseMat(&mask);
71  MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
72 }
73 
74 void OpticalFlow::FreeStatusData() {
75  if (prevFeatures) {delete[] prevFeatures; prevFeatures=0;}
76  if (curFeatures) {delete[] curFeatures; curFeatures=0;}
77  if (featStatus) {delete[] featStatus; featStatus=0;}
78 }
79 
80 
81 #define PIX_ABSTAND 5
82 #define OFFSET 50
83 #define TRMEAN_MARGIN 3
84 #define IMG_MARGIN 30
85 
92 void OpticalFlow::SelectFeatures() {
93  MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
94  FreeStatusData();
95 
96  MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
97 #if 0
98  // old stuff
99  numFeatures=(imgSize.height-IMG_MARGIN-focus.y-OFFSET)/PIX_ABSTAND;
100  //cerr << numFeatures << endl;
101 
102  prevFeatures = new CvPoint2D32f[numFeatures];
103  curFeatures = new CvPoint2D32f[numFeatures];
104 
105  for (int i=0;i<numFeatures;i++) {
106  prevFeatures[i].x = focus.x;
107  prevFeatures[i].y = focus.y + i*PIX_ABSTAND + OFFSET;
108  }
109 
110  featStatus = new char[numFeatures];
111 #else
112 
113  MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
114  numFeatures=100;
115  prevFeatures = new CvPoint2D32f[numFeatures];
116  curFeatures = new CvPoint2D32f[numFeatures];
117  featStatus = new char[numFeatures];
118 
119 #define XGRID 3
120 #define YGRID 3
121  for (int x=0;x<XGRID;x++) {
122  for (int y=0;y<YGRID;y++) {
123  MaskUseGridCell(x,y,XGRID,YGRID);
124  numFeatures=20;
125  cvGoodFeaturesToTrack(imgBuf.GetPrev()->GetImage(),
126  eigenBuf->GetImage(),
127  tempBuf->GetImage(),
128  prevFeatures,
129  &numFeatures,
130  .005, //quality (min eigen value)
131  3, //euclidian distance between corners
132  mask,
133  5, // averaging block size
134  1, // use Harris
135  0.005 // free parameter in Harris
136  );
137  for (int i=0;i<numFeatures;i++) {
138  particles.push_back(new Particle(prevFeatures[i]));
139  }
140  }
141  }
142  random_shuffle(particles.begin(),particles.end());
143  if (particles.size()>250) {
144  particles.resize(200);
145  }
146 
147  FreeStatusData();
148  numFeatures=static_cast<int>(particles.size());
149  prevFeatures = new CvPoint2D32f[numFeatures];
150  curFeatures = new CvPoint2D32f[numFeatures];
151  featStatus = new char[numFeatures];
152  for (int i=0;i<numFeatures;i++) {
153  prevFeatures[i].x=static_cast<float>(particles[i]->pos[0]);
154  prevFeatures[i].y=static_cast<float>(particles[i]->pos[1]);
155  }
156 #endif
157 }
158 
168 #define VERSCHIEBUNG 90.0
169 
173 double OpticalFlow::GetWinkel() {
174 #if 1
175  return kalmanState[0] - focus.x;
176 #elif 1
177  return focusOfExpansion.get()[0] - focus.x;
178 #else
179 
180  double res=0;
181 
182  TrimmedMean avg;
183 
184  for(int i = 0; i < numFeatures; i++) {
185  if (featStatus[i] == 0) continue;
186 
187  Linie flowLine(prevFeatures[i], curFeatures[i]);
188 
189  if (flowLine.GetLen()<2) continue;
190 
191  Linie focusLinePrev(focus, prevFeatures[i]);
192  Linie focusLineCur(focus, curFeatures[i]);
193 
194  double sollWinkel = 0.5*(focusLinePrev.GetWinkel()+focusLineCur.GetWinkel());
195  double istWinkel = flowLine.GetWinkel();
196 
197  double diff=(sollWinkel-istWinkel)*180.0/M_PI;
198 
199  // Umbruch in einen unwichtigen Bereich legen
200  diff += VERSCHIEBUNG;
201 
202  while (diff>180.0) diff-=360.0;
203  while (diff<-180.0) diff+=360.0;
204 
205  if (false && rand()%10==3) cout << diff << ": "
206  << focusLinePrev.GetWinkel()*180.0/M_PI << " -> "
207  << flowLine.GetWinkel()*180.0/M_PI << endl;
208 
209  avg.Add(diff);
210  }
211  avg.Compute();
212  try {
213  res=avg.GetMean()-VERSCHIEBUNG;
214 #if 0
215  cout << avg.GetMean() << " "
216  << "(s=" << avg.GetStDev() << ") "
217  << "N=" << avg.GetNumCases() << endl;
218 #endif
219  } catch(...) {
220 #if 0
221  cout << endl;
222 #endif
223  }
224  return res;
225 #endif
226 }
227 
228 
235 void OpticalFlow::PrintFlowField(IplImage *img) {
236 #if 0
237  for(unsigned int i = 0; i < particles.size(); i++) {
238 
239  Linie flowLine(particles[i]->pos,
240  particles[i]->pos+particles[i]->vec);
241 
242  PrintArrow(flowLine, img, CV_RGB(255,0,0));
243  }
244 
245 #elif 0
246  for(int i = 0; i < numFeatures; i++) {
247  if (featStatus[i] == 0) continue;
248 
249  Linie flowLine(prevFeatures[i], curFeatures[i]);
250  Linie focusLinePrev(focus, prevFeatures[i]);
251  Linie focusLineCur(focus, curFeatures[i]);
252 
253  double sollWinkel = 0.5*(focusLinePrev.GetWinkel()+focusLineCur.GetWinkel());
254  double istWinkel = flowLine.GetWinkel();
255 
256  double diff=sollWinkel-istWinkel;
257 
258 
259  //PrintLine(focusLinePrev, img, CV_RGB(0,0,255));
260  PrintArrow(flowLine, img, CV_RGB(255,0,0));
261  }
262 #else
263  vector<Linie> linien;
264 
265  Mean<double> particleAge;
266  for(unsigned int i = 0; i < particles.size(); i++) {
267  particleAge.add(particles[i]->numFound);
268 
269  Linie flowLine(particles[i]->pos,
270  particles[i]->pos+particles[i]->vec);
271 
272  if (flowLine.GetLen()>1) {
273  PrintArrow(flowLine, img, CV_RGB(255,0,0));
274  linien.push_back(flowLine);
275  }
276  }
277 #ifdef DEBUG
278  try {
279  cout << "age: " << particleAge.get_mean() << endl;
280  } catch (...) {
281  }
282 #endif
283 
284  Mean<Pos<2,double> > foe; // focus of expansion
285 
286  TrimmedMean foeX;
287  TrimmedMean foeY;
288 
289  for (unsigned int i=0;i<linien.size()*10;i++) {
290  try {
291  int i1=static_cast<int>(rand()%linien.size());
292  int i2=static_cast<int>(rand()%linien.size());
293 
294  if (GetWinkelDiff(linien[i1].GetWinkel(),linien[i2].GetWinkel()) < M_PI/8) {
295  continue;
296  }
297 
298  // TODO Heuristik �berpr�fen
299  if (Linie(linien[i1].GetLinks(),linien[i1].GetOben(),
300  linien[i2].GetLinks(),linien[i2].GetOben()).GetLen()<100) {
301  continue;
302  }
303 
304  Pos<2,double> p = linien[i1].SchnittPunkt(linien[i2]);
305  foe.add(p);
306  foeX.Add(p[0]);
307  foeY.Add(p[1]);
308  cvCircle(img, cvPoint((int)p[0],(int)p[1]), 1, CV_RGB(0,0,255), CV_FILLED);
309  } catch (...) {
310  }
311  }
312 
313 
314  try {
315  foeX.Compute(.25);
316  foeY.Compute(.25);
317  Pos<2,double> avgFoe(foeX.GetMean(),foeY.GetMean());
318 
319  // exponentially smoothed version
320  if (numCalls>1) {
321  focusOfExpansion.add(avgFoe);
322  } else {
323  focusOfExpansion.set(avgFoe);
324  }
325 
326  // kalman version
327  kalmanState[0]=static_cast<float>(avgFoe[0]);
328  kalmanState[1]=static_cast<float>(avgFoe[1]);
329  kalmanFOE.AddMeasure(kalmanState);
330  kalmanFOE.Predict(kalmanState);
331 
332  cvCircle(img, cvPoint((int)kalmanState[0],(int)kalmanState[1]),
333  4, CV_RGB(0,255,0), CV_FILLED);
334 
335 #if 0
336  cvCircle(img, cvPoint((int)focusOfExpansion.get()[0],
337  (int)focusOfExpansion.get()[1]), 4, CV_RGB(255,255,0), CV_FILLED);
338 #endif
339  } catch (...) {
340  }
341 #endif
342 
343 }
344 
345 #define FOCUS_CROSS_LWIDTH 3
346 void OpticalFlow::PrintFocus(IplImage *img) {
347  cvLine(img,
348  focus+cvPoint(-FOCUS_CROSS_LWIDTH*2,0),
349  focus+cvPoint(FOCUS_CROSS_LWIDTH*2,0),
350  CV_RGB(0,255,0),
351  FOCUS_CROSS_LWIDTH,
352  CV_AA, 0 );
353  cvLine(img,
354  focus+cvPoint(0,-FOCUS_CROSS_LWIDTH*2),
355  focus+cvPoint(0,FOCUS_CROSS_LWIDTH*2),
356  CV_RGB(0,255,0),
357  FOCUS_CROSS_LWIDTH,
358  CV_AA, 0 );
359 }
360 
361 
362 
365 void OpticalFlow::Calc() {
366  MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
367  SelectFeatures();
368  MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
369  cvCalcOpticalFlowPyrLK(imgBuf.GetPrev()->GetImage(), imgBuf.GetCur()->GetImage(), // images
370  pyrBuf.GetPrev()->GetImage(), pyrBuf.GetCur()->GetImage(), // pyramid buffers
371  prevFeatures, curFeatures, numFeatures, // feature points
372  cvSize(5,5), 4, featStatus, nullptr,
373  cvTermCriteria(CV_TERMCRIT_ITER, 10, 1.0),
374  (numCalls>0)?CV_LKFLOW_PYR_A_READY:0);
375 
376  pyrBuf.IncIndex();
377 
378  vector<GCPointer<Particle> > newParticles;
379  for (unsigned int i=0;i<particles.size();i++) {
380  if (featStatus[i]) {
381  if (particles[i]->Found(curFeatures[i])) {
382  if (particles[i]->ToTheOutside(focus)) {
383  newParticles.push_back(particles[i]);
384  }
385  } else {
386  //cout << "."; cout.flush();
387  }
388  }
389  }
390  //cout << endl;
391  particles=newParticles;
392 
393  numCalls++;
394  MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
395 }
396 
401 void OpticalFlow::PrintInfo() {
402  MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
403  vector<double> wVec;
404  for (int i=0;i<numFeatures;i++) {
405  if (featStatus[i]) {
406  //cout << prevFeatures[i] << " -> " << curFeatures[i] << " ";
407 
408  double w = atan2(curFeatures[i].x-focus.x,curFeatures[i].y-focus.y);
409  //cout << "(" << w*180.0/M_PI << ")" << endl;
410  if (!boost::math::isnan(w)) {
411  wVec.push_back(w*180.0/M_PI);
412  }
413 #if 0
414  cout << numCalls << " "
415  << i << " "
416  << prevFeatures[i].x << " "
417  << prevFeatures[i].y << " "
418  << curFeatures[i].x << " "
419  << curFeatures[i].y << " "
420  << w*180.0/M_PI << endl;
421 #endif
422  }
423  }
424  MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
425  if (wVec.size()>TRMEAN_MARGIN*2) {
426  MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
427  MHDBGMSG_I(__FILE__,__LINE__,"wVec.size",static_cast<int>(wVec.size()));
428 #if 0
429  for (vector<double>::iterator i=wVec.begin();i!=wVec.end();i++) {
430  cerr << *i << " ";
431  }
432  cerr << endl;
433 #endif
434  sort(wVec.begin(),wVec.end());
435 #if 0
436  for (vector<double>::iterator i=wVec.begin();i!=wVec.end();i++) {
437  cerr << *i << " ";
438  }
439  cerr << endl;
440 #endif
441  Mean<double> winkel;
442  // trimmed mean
443  MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
444  for (int i=TRMEAN_MARGIN;i<static_cast<int>(wVec.size())-TRMEAN_MARGIN;i++) {
445  winkel.add(wVec[i]);
446  }
447  MHDBGMSG_I(__FILE__,__LINE__,"winkel.get_num_cases",winkel.get_num_cases());
448 #if 1
449  try {
450  cout << numCalls << ": "
451  << winkel.get_mean() << " "
452  << "(N=" << winkel.get_num_cases() << ")"
453  << endl;
454  } catch (...) {
455  }
456 #endif
457  }
458  MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
459 }
460 
463 void OpticalFlow::SetFocus(const CvPoint &focus) {
464  this->focus = focus;
465 
466  FreeStatusData();
467 
468  cvZero(mask);
469 
470 #if 0
471  for (int y=1;imgSize.height-y*IMG_MARGIN>focus.y;y++) {
472  int x=y/2+1;
473  cvRectangle(mask,
474  cvPoint(IMG_MARGIN*x,
475  imgSize.height-IMG_MARGIN*(y+1)),
476  cvPoint(imgSize.width-IMG_MARGIN*x,
477  imgSize.height-IMG_MARGIN*y),
478  cvScalar(0xff),
479  CV_FILLED);
480  }
481 
482 #elif 0
483  CvPoint trapez[4];
484  trapez[0]=cvPoint(imgSize.width/4,focus.y);
485  trapez[1]=cvPoint(IMG_MARGIN,imgSize.height-IMG_MARGIN);
486  trapez[2]=cvPoint(imgSize.width-IMG_MARGIN,imgSize.height-IMG_MARGIN);
487  trapez[3]=cvPoint(3*imgSize.width/4,focus.y);
488 
489  // doesn't work, don't know why...
490  cvFillConvexPoly(mask,
491  trapez, 4,
492  cvScalar(0xff),
493  CV_FILLED);
494 #else
495  MaskUseFullArea();
496 #endif
497 }
498 
499 int OpticalFlow::GetXMin() {
500  return IMG_MARGIN;
501 }
502 int OpticalFlow::GetXMax() {
503  return imgSize.width-IMG_MARGIN;
504 }
505 
506 int OpticalFlow::GetYMin() {
507  // return IMG_MARGIN;
508  // return focus.y-IMG_MARGIN
509  return focus.y;
510 }
511 int OpticalFlow::GetYMax() {
512  return imgSize.height-IMG_MARGIN;
513  //return imgSize.height-200; // CB-Files
514 }
515 
516 void OpticalFlow::MaskUseFullArea() {
517  cvZero(mask);
518  cvRectangle(mask,
519  cvPoint(GetXMin(),GetYMin()),
520  cvPoint(GetXMax(),GetYMax()),
521  cvScalar(0xff),
522  CV_FILLED);
523 }
524 
525 void OpticalFlow::MaskUseGridCell(int x, int y, int lenX, int lenY) {
526  cvZero(mask);
527  float width = static_cast<float>(GetXMax() - GetXMin());
528  float height = static_cast<float>(GetYMax() - GetYMin());
529 
530  int cellSizeX = (int)floor(width/lenX + .5);
531  int cellSizeY = (int)floor(height/lenY + .5);
532 
533  cvRectangle(mask,
534  cvPoint(GetXMin() + x*cellSizeX, GetYMin() + y*cellSizeY),
535  cvPoint(GetXMin() + (x+1)*cellSizeX, GetYMin() + (y+1)*cellSizeY),
536  cvScalar(0xff),
537  CV_FILLED);
538 }
539 
540 
543 void OpticalFlow::SetSize(const CvSize &size) {
544  imgSize=size;
545 
546  // (image_width+8)*image_height/3 bytes
547  //CvSize pyrSize = cvSize(imgSize.width+8,imgSize.height/3);
548  CvSize pyrSize = cvSize(imgSize.width+16,imgSize.height/2);
549  pyrBuf.Add(new ImageHolder(cvCreateImage(pyrSize, IPL_DEPTH_8U, 1)));
550  pyrBuf.Add(new ImageHolder(cvCreateImage(pyrSize, IPL_DEPTH_8U, 1)));
551 
552  eigenBuf = new ImageHolder(cvCreateImage(imgSize, IPL_DEPTH_32F, 1));
553  tempBuf = new ImageHolder(cvCreateImage(imgSize, IPL_DEPTH_32F, 1));
554 
555  // mask is initialized 0
556  cvReleaseMat(&mask);
557  // 0xff / 0x00 in der Maske
558  mask = cvCreateMat(imgSize.height, imgSize.width, CV_8UC1);
559 
560  cvZero(mask);
561  cvRectangle(mask,
562  cvPoint(IMG_MARGIN,IMG_MARGIN),
563  cvPoint(imgSize.width-IMG_MARGIN,imgSize.height-IMG_MARGIN),
564  cvScalar(0xff),
565  CV_FILLED);
566 }
567 
568 
570 bool OpticalFlow::Particle::Found(const CvPoint2D32f &p) {
571  Pos<2,double> newPos(p.x,p.y);
572  Pos<2,double> newVec=newPos-pos;
573 
574  if (numFound) {
575  vec*=.75; // Einfluss des alten verringern
576  } else {
577  vec[0]=vec[1]=0;
578  }
579 
580  if (numFound>0 && fabs(GetWinkelDiff(atan2(vec[1],vec[0]),
581  atan2(newVec[1],newVec[0])))>M_PI/16) {
582 #if 0
583  // reinit
584  vec=newVec;
585  pos=newPos;
586  numFound=1;
587  return true;
588 #else
589  // remove me from the list of features
590  return false;
591 #endif
592  }
593  numFound++;
594  vec+=newVec;
595  pos=newPos;
596 
597  return true;
598 }
599 

ACT-CV - Machine Vision for Cognitive Modeling
© 2015 Marc Halbruegge (actcvlibrary@googlemail.com)