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