tracking.cpp

   1 #include <stdio.h>
   2 #include <iostream>
   3
   4 using namespace std;
   5
   6 #include "tracking.h"
   7 #include "stickletrack.h"
   8
   9 #define SEPCOST_AREA      0
  10 #define SEPCOST_CIRCUM    1
  11 #define SEPCOST_SHORTAXIS 2
  12 #define SEPCOST_LONGAXIS  3
  13 #define SEPCOST_ANGLE     4
  14 #define SEPCOST_DIST      5
  15
  16 struct fishblobb {
  17   double x;
  18   double y;
  19   bool identified;
  20   int contourid;
  21   vector<Point> *points;
  22   double longaxis;
  23   double shortaxis;
  24   double angle;
  25   int area;
  26   int circum;
  27   Point2f head, tail;
  28 };
  29
  30 static int enablecontours=0;
  31 static Mat mContours;
  32
  33 struct tagcost {
  34   int tagid;
  35   double tagcost;
  36 };
  37
  38 struct blobbcost {
  39   int blobbid;
  40   vector<tagcost> tagcosts;
  41 };
  42
  43 bool sort_tags (tagcost i, tagcost j) {
  44   return i.tagcost < j.tagcost;
  45 }
  46
  47 bool sort_blobbs (blobbcost i, blobbcost j) {
  48   return i.tagcosts[0].tagcost < j.tagcosts[0].tagcost;
  49 }
  50
  51 Mat& tracking_getFrame() {
  52   return mContours;
  53 }
  54
  55 int tracking_showFrame() {
  56   if ( ! isWindowClosed("stickletrack_tracking_screen") ) {
  57     imshow("stickletrack_tracking_screen", mContours);
  58     return 0;
  59   }
  60   else
  61     return 1;
  62 }
  63
  64 bool tracking_isEnabled () {
  65   return enablecontours;
  66 }
  67
  68 void tracking_init( void (*mouseTracking)(int, int, int, int, void*) ) {
  69   namedWindow("stickletrack_tracking_prefs", CV_WINDOW_KEEPRATIO);
  70   namedWindow("stickletrack_tracking_screen", CV_WINDOW_KEEPRATIO);
  71
  72   createTrackbar("Enable", "stickletrack_tracking_prefs", &enablecontours, 1, NULL, 0);
  73   createTrackbar("Manyfish", "stickletrack_tracking_prefs", &Prefs.manyfish, 10, NULL, 0);
  74   createTrackbar("min area", "stickletrack_tracking_prefs", &Prefs.contours_minarea, 100, &trackbarCallbackUpdateNormPrefs, 0);
  75   createTrackbar("max area", "stickletrack_tracking_prefs", &Prefs.contours_maxarea, 100, &trackbarCallbackUpdateNormPrefs, 0);
  76   createTrackbar("min circum", "stickletrack_tracking_prefs", &Prefs.contours_mincircum, 100, &trackbarCallbackUpdateNormPrefs, 0);
  77   createTrackbar("max circum", "stickletrack_tracking_prefs", &Prefs.contours_maxcircum, 100, &trackbarCallbackUpdateNormPrefs, 0);
  78   createTrackbar("min shortaxis", "stickletrack_tracking_prefs", &Prefs.contours_minshortaxis, 100, &trackbarCallbackUpdateNormPrefs, 0);
  79   createTrackbar("max shortaxis", "stickletrack_tracking_prefs", &Prefs.contours_maxshortaxis, 100, &trackbarCallbackUpdateNormPrefs, 0);
  80   createTrackbar("min longaxis", "stickletrack_tracking_prefs", &Prefs.contours_minlongaxis, 100, &trackbarCallbackUpdateNormPrefs, 0);
  81   createTrackbar("max longaxis", "stickletrack_tracking_prefs", &Prefs.contours_maxlongaxis, 100, &trackbarCallbackUpdateNormPrefs, 0);
  82   createTrackbar("max speed", "stickletrack_tracking_prefs", &Prefs.contours_maxspeed, 100, &trackbarCallbackUpdateNormPrefs, 0);
  83   createTrackbar("max rotation speed", "stickletrack_tracking_prefs", &Prefs.contours_maxrot, 100, &trackbarCallbackUpdateNormPrefs, 0);
  84
  85   cvSetMouseCallback("stickletrack_tracking_screen", mouseTracking, 0);
  86
  87   mContours = Mat::zeros(Props.height, Props.width, CV_8UC3);
  88 }
  89
  90 double kitvalue (double min, double max) {
  91   return min + pow( cos(frametime*10), 2)*(max-min);
  92 }
  93
  94 void findFishBlobbs(vector<fishblobb>& fishblobbs, vector< vector<Point> >& contours, vector<Vec4i>& hierarchy) {
  95   for (int icontour=0; icontour >= 0; icontour = hierarchy[icontour][0]) {
  96     fishblobb newblobb;
  97
  98     if ( contours[icontour].size() < 5 )
  99       continue;
 100
 101     newblobb.circum = contours[icontour].size();
 102     if ( newblobb.circum < normalPrefs.contours_mincircum || newblobb.circum > normalPrefs.contours_maxcircum )
 103       continue;
 104
 105     newblobb.area = contourArea(contours[icontour]);
 106     if ( newblobb.area < normalPrefs.contours_minarea || newblobb.area > normalPrefs.contours_maxarea )
 107       continue;
 108
 109     RotatedRect minEllipse = fitEllipse(contours[icontour]);
 110     newblobb.longaxis = minEllipse.size.height;
 111     newblobb.shortaxis = minEllipse.size.width;
 112     if ( newblobb.shortaxis < normalPrefs.contours_minshortaxis || newblobb.shortaxis > normalPrefs.contours_maxshortaxis
 113          || newblobb.longaxis < normalPrefs.contours_minlongaxis || newblobb.longaxis > normalPrefs.contours_maxlongaxis )
 114       continue;
 115
 116     newblobb.x = minEllipse.center.x;
 117     newblobb.y = minEllipse.center.y;
 118
 119     RotatedRect minRect;
 120     Mat mFish = Mat::zeros(Props.height, Props.width, CV_8U);
 121     Moments fishMom;
 122     Point2f massCenter;
 123     Point2f recHead, recTail;
 124
 125     newblobb.identified = false;
 126     newblobb.points = &contours[icontour];
 127
 128     newblobb.contourid = icontour;
 129
 130     /* find head/tail sensitive angle
 131        maybe the method could be improved */
 132     drawContours( mFish, contours, icontour, Scalar(255, 255, 255), CV_FILLED );
 133     fishMom = moments(mFish);
 134
 135     massCenter = Point2f( fishMom.m10/fishMom.m00 , fishMom.m01/fishMom.m00 );
 136     minRect = minAreaRect(contours[ icontour ]);
 137
 138     recHead = Point2f( minRect.center.x + newblobb.longaxis*cos(minEllipse.angle * M_PI / 180 + M_PI/2),
 139                        minRect.center.y + newblobb.longaxis*sin(minEllipse.angle * M_PI / 180 + M_PI/2) );
 140     recTail = Point2f( minRect.center.x + newblobb.longaxis*cos(minEllipse.angle * M_PI / 180 - M_PI/2),
 141                        minRect.center.y + newblobb.longaxis*sin(minEllipse.angle * M_PI / 180 - M_PI/2) );
 142
 143     if ( pow( recHead.x - massCenter.x, 2 ) + pow( recHead.y - massCenter.y, 2 ) > pow( recTail.x - massCenter.x, 2 ) + pow( recTail.y - massCenter.y, 2 ) ) {
 144       newblobb.angle = minEllipse.angle + 180;
 145
 146       if (newblobb.angle > 360)
 147         newblobb.angle = newblobb.angle - 360;
 148     }
 149     else {
 150       newblobb.angle = minEllipse.angle;
 151     }
 152
 153     newblobb.tail = Point2f( minEllipse.center.x + newblobb.longaxis*cos(newblobb.angle * M_PI / 180 - M_PI/2),
 154                              minEllipse.center.y + newblobb.longaxis*sin(newblobb.angle * M_PI / 180 - M_PI/2) );
 155     newblobb.head = Point2f( minEllipse.center.x + newblobb.longaxis*cos(newblobb.angle * M_PI / 180 + M_PI/2),
 156                              minEllipse.center.y + newblobb.longaxis*sin(newblobb.angle * M_PI / 180 + M_PI/2) );
 157
 158     fishblobbs.push_back(newblobb);
 159   }
 160 }
 161
 162 void tagCare(vector<tag>& tags) {
 163   while ( tags.size() > Prefs.manyfish )
 164     tags.pop_back();
 165
 166   while ( tags.size() < Prefs.manyfish ) {
 167     tag tmp;
 168     tmp.x = 0;
 169     tmp.y = 0;
 170     tmp.propZ = 0;
 171     tmp.lastseen = frametime;
 172
 173     switch ( tags.size() ) {
 174     case 0:
 175       tmp.color = Scalar( 0, 255, 255 );
 176       break;
 177     case 1:
 178       tmp.color = Scalar( 255, 0, 255 );
 179       break;
 180     case 2:
 181       tmp.color = Scalar( 255, 255, 0 );
 182       break;
 183     case 3:
 184       tmp.color = Scalar( 255, 0, 0 );
 185       break;
 186     case 4:
 187       tmp.color = Scalar( 0, 255, 0 );
 188       break;
 189     case 5:
 190       tmp.color = Scalar( 0, 0, 255 );
 191       break;
 192     default:
 193       tmp.color = Scalar( rand()%255, rand()%255, rand()%255 );
 194     }
 195
 196     tmp.angle = 0;
 197     tmp.area = 0;
 198     tmp.circum = 0;
 199     tmp.shortaxis = 0;
 200     tmp.longaxis = 0;
 201     tmp.virgin = true;
 202     tags.push_back(tmp);
 203   }
 204 }
 205
 206 void matchBlobbsNTags(vector<tag>& tags, vector<fishblobb>& fishblobbs) {
 207   vector<blobbcost> blobbcosts;
 208
 209   for (int iblobb = 0; iblobb < fishblobbs.size(); iblobb++) {
 210     double sepcost[tags.size()][6];
 211     double maxcost[6];
 212     blobbcost blobbCost;
 213
 214     blobbCost.blobbid = iblobb;
 215
 216     for (int imax = 0; imax < 6; imax++)
 217       maxcost[imax] = 0;
 218
 219     for (int itag = 0; itag < tags.size(); itag++) {
 220       double anglediff = fabs (fishblobbs[iblobb].angle - tags[itag].angle);
 221
 222       if ( anglediff > 180 )
 223         anglediff = 360 - anglediff;
 224
 225       sepcost[itag][SEPCOST_ANGLE] = anglediff;
 226
 227       sepcost[itag][SEPCOST_AREA] = fabs ( fishblobbs[iblobb].area - tags[itag].area );
 228       sepcost[itag][SEPCOST_CIRCUM] = fabs ( fishblobbs[iblobb].circum - tags[itag].circum );
 229       sepcost[itag][SEPCOST_SHORTAXIS] = fabs ( fishblobbs[iblobb].shortaxis - tags[itag].shortaxis );
 230       sepcost[itag][SEPCOST_LONGAXIS] = fabs ( fishblobbs[iblobb].longaxis - tags[itag].longaxis );
 231       sepcost[itag][SEPCOST_DIST] = sqrt( pow(tags[itag].x - fishblobbs[iblobb].x, 2) + pow(tags[itag].y - fishblobbs[iblobb].y, 2) );
 232
 233       for (int imax = 0; imax < 6; imax++)
 234         if ( maxcost[imax] < sepcost[itag][imax] )
 235           maxcost[imax] = sepcost[itag][imax];
 236     }
 237
 238     for (int itag = 0; itag < tags.size(); itag++) {
 239       tagcost newTagCost;
 240
 241       newTagCost.tagid = itag;
 242       newTagCost.tagcost = 0;
 243
 244       if ( sepcost[itag][SEPCOST_DIST] > ( frametime - tags[itag].lastseen )*normalPrefs.contours_maxspeed
 245            || sepcost[itag][SEPCOST_ANGLE] > ( frametime - tags[itag].lastseen )*normalPrefs.contours_maxrot )
 246         continue;
 247
 248       for (int isep = 0; isep < 6; isep++)
 249         newTagCost.tagcost += sepcost[itag][isep] / maxcost[isep];
 250
 251       blobbCost.tagcosts.push_back( newTagCost );
 252     }
 253
 254     if ( blobbCost.tagcosts.size() > 0 ) {
 255       sort( blobbCost.tagcosts.begin(), blobbCost.tagcosts.end(), sort_tags );
 256       blobbcosts.push_back( blobbCost );
 257     }
 258   }
 259
 260   for (int itag = 0; itag < tags.size() && blobbcosts.size() > 0; itag++) {
 261     sort( blobbcosts.begin(), blobbcosts.end(), sort_blobbs );
 262
 263     int tagnow = blobbcosts[0].tagcosts[0].tagid;
 264
 265     tags[tagnow].virgin = false;
 266     tags[tagnow].x = fishblobbs[ blobbcosts[0].blobbid ].x;
 267     tags[tagnow].y = fishblobbs[ blobbcosts[0].blobbid ].y;
 268     tags[tagnow].shortaxis = fishblobbs[ blobbcosts[0].blobbid ].shortaxis;
 269     tags[tagnow].longaxis =  fishblobbs[ blobbcosts[0].blobbid ].longaxis;
 270     tags[tagnow].angle = fishblobbs[ blobbcosts[0].blobbid ].angle;
 271     tags[tagnow].lastseen = frametime;
 272     tags[tagnow].area = fishblobbs[ blobbcosts[0].blobbid ].area;
 273     tags[tagnow].circum = fishblobbs[ blobbcosts[0].blobbid ].circum;
 274     tags[tagnow].head = fishblobbs[ blobbcosts[0].blobbid ].head;
 275     tags[tagnow].tail = fishblobbs[ blobbcosts[0].blobbid ].tail;
 276
 277     blobbcosts.erase( blobbcosts.begin() );
 278
 279     for (int iblobb = 0; iblobb < blobbcosts.size(); iblobb++) {
 280
 281       for (int itag = 0; itag < blobbcosts[iblobb].tagcosts.size(); itag++)
 282         if ( blobbcosts[iblobb].tagcosts[itag].tagid == tagnow ) {
 283           blobbcosts[iblobb].tagcosts.erase( blobbcosts[iblobb].tagcosts.begin() + itag );
 284           break;
 285         }
 286
 287       if ( blobbcosts[iblobb].tagcosts.size() == 0 ) {
 288         blobbcosts.erase( blobbcosts.begin() + iblobb );
 289         iblobb--;
 290       }
 291     }
 292   }
 293 }
 294
 295 void drawTags (const vector<tag>& tags) {
 296   RotatedRect rRect;
 297   Point2f vertices[4];
 298
 299   for (int itag = 0; itag < tags.size(); itag++) {
 300     double recside = sqrt( kitvalue( normalPrefs.contours_minarea, normalPrefs.contours_maxarea) );
 301     rRect = RotatedRect( Point(tags[itag].x, tags[itag].y), Size2f(recside, recside), tags[itag].angle );
 302     rRect.points(vertices);
 303     for (int i = 0; i < 4; i++)
 304       line(mContours, vertices[i], vertices[(i+1)%4], Scalar(255,255,255), Props.diagonal/1000.0);
 305
 306     ellipse(mContours, Point(tags[itag].x, tags[itag].y),
 307             Size( kitvalue( normalPrefs.contours_minshortaxis, normalPrefs.contours_maxshortaxis ),
 308                   kitvalue( normalPrefs.contours_minlongaxis, normalPrefs.contours_maxlongaxis ) ),
 309             tags[itag].angle, 0, 360, Scalar(255,255,255), Props.diagonal/1000.0, 8);
 310
 311     circle(mContours, Point(tags[itag].x, tags[itag].y), kitvalue( normalPrefs.contours_mincircum, normalPrefs.contours_maxcircum )/(2*M_PI), Scalar(255,255,255),
 312            Props.diagonal/1000.0, 8);
 313
 314     if ( ! tags[itag].virgin ) {
 315       float searchradius = ( frametime - tags[itag].lastseen )*normalPrefs.contours_maxspeed;
 316       float searchangle = ( frametime - tags[itag].lastseen )*normalPrefs.contours_maxrot;
 317       ellipse(mContours, Point(tags[itag].x, tags[itag].y), Size(searchradius, searchradius), tags[itag].angle, 90-searchangle, 90+searchangle,
 318               tags[itag].color, Props.diagonal/1000.0, 8);
 319
 320       rRect = RotatedRect( Point(tags[itag].x, tags[itag].y), Size2f(sqrt(tags[itag].area), sqrt(tags[itag].area)), tags[itag].angle );
 321       rRect.points(vertices);
 322       for (int i = 0; i < 4; i++)
 323         line(mContours, vertices[i], vertices[(i+1)%4], tags[itag].color, 2);
 324
 325       ellipse(mContours, Point(tags[itag].x, tags[itag].y),
 326               Size( tags[itag].shortaxis, tags[itag].longaxis ), tags[itag].angle, 0, 360, tags[itag].color, 2*Props.diagonal/1000.0, 8);
 327
 328       circle(mContours, Point(tags[itag].x, tags[itag].y), tags[itag].circum / (2*M_PI), tags[itag].color, 2*Props.diagonal/1000.0, 8);
 329
 330       circle(mContours, tags[itag].head, 5*Props.diagonal/1000.0, tags[itag].color, -1, 8);
 331     }
 332   }
 333 }
 334
 335 void tracking_locateTags (vector<tag>& tags, Mat combinedmask_contour) {
 336   mContours = Mat::zeros(Props.height, Props.width, CV_8UC3);
 337
 338   if (enablecontours) {
 339     vector< vector<Point> > contours;
 340     vector<Vec4i> hierarchy;
 341     vector <fishblobb> fishblobbs;
 342
 343     findContours( combinedmask_contour, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE );
 344
 345     if ( contours.size() > 0 ) {
 346
 347       for(int idx = 0 ; idx >= 0; idx = hierarchy[idx][0] )
 348         drawContours(mContours, contours, idx, Scalar(255, 255, 255), CV_FILLED, 8, hierarchy);
 349
 350       findFishBlobbs(fishblobbs, contours, hierarchy);
 351
 352       for ( int iblobb = 0; iblobb < fishblobbs.size(); iblobb++) {
 353         Scalar color( rand()%255, rand()%255, rand()%255 );
 354         drawContours( mContours, contours, fishblobbs[iblobb].contourid, color, CV_FILLED, 8, hierarchy );
 355       }
 356
 357       tagCare(tags);
 358       matchBlobbsNTags(tags, fishblobbs);
 359
 360     }
 361
 362     drawTags(tags);
 363   }
 364 }