...

diff --git a/.gitignore b/.gitignore
new file mode 100644 (file)
index 0000000..cf0d71b
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,5 @@
+CMakeCache.txt
+CMakeFiles
+Makefile
+cmake_install.cmake
+stickletrack

diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644 (file)
index 0000000..7fd670a
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 2.6)
+project( stickletrack )
+
+SET(CMAKE_BUILD_TYPE Release)
+
+# Uncomment if you want to do profiling with gprof
+# SET(CMAKE_CXX_FLAGS "-pg")
+
+find_package( OpenCV REQUIRED )
+add_executable( stickletrack stickletrack.cpp masking.cpp tracking.cpp )
+target_link_libraries( stickletrack ${OpenCV_LIBS} ) 

diff --git a/masking.cpp b/masking.cpp
new file mode 100644 (file)
index 0000000..d76a8b4
--- /dev/null
+++ b/masking.cpp
@@ -0,0 +1,110 @@
+#include "masking.h"
+#include "stickletrack.h"
+
+static Mat background, foregroundmask, colormask, mScissors, scissorsmask;
+static double Z = 1;
+
+void mouseScissors(int evt, int x, int y, int flags, void* param){
+  if (evt==CV_EVENT_LBUTTONDOWN) {
+    Prefs.scissorpoints->push_back( Point(x, y) );
+  }
+
+  else if (evt==CV_EVENT_RBUTTONDOWN) {
+    int nearestid = -1;
+    double nearestdist;
+
+    for ( int ipoint = 0; ipoint < Prefs.scissorpoints->size(); ipoint++ ) {
+      double dist = pow( x - (*Prefs.scissorpoints)[ipoint].x, 2 ) + pow( y - (*Prefs.scissorpoints)[ipoint].y, 2 );
+
+      if ( dist < nearestdist || nearestid == -1 ) {
+       nearestdist = dist;
+       nearestid = ipoint;
+      }
+    }
+    if ( nearestid != -1 )
+      Prefs.scissorpoints->erase( Prefs.scissorpoints->begin() + nearestid );
+  }
+}
+
+void masking_init() {
+  background = Mat::zeros(Props.height, Props.width, CV_32FC3);
+  
+  namedWindow("background", CV_WINDOW_KEEPRATIO);
+  namedWindow("foregroundmask", CV_WINDOW_KEEPRATIO);
+  namedWindow("colormask", CV_WINDOW_KEEPRATIO);
+  namedWindow("scissorsmask", CV_WINDOW_KEEPRATIO);
+
+  createTrackbar("Decay", "background", &Prefs.halfdecay, 100, &trackbarCallbackUpdateNormPrefs, 0);
+  createTrackbar("Threshold", "foregroundmask", &Prefs.forethreshold, 255, &trackbarCallbackUpdateNormPrefs, 0);
+
+  createTrackbar("min H", "colormask", &Prefs.mincolor[0], 255, NULL, 0);
+  createTrackbar("min S", "colormask", &Prefs.mincolor[1], 255, NULL, 0);
+  createTrackbar("min V", "colormask", &Prefs.mincolor[2], 255, NULL, 0);
+  createTrackbar("max H", "colormask", &Prefs.maxcolor[0], 255, NULL, 0);
+  createTrackbar("max S", "colormask", &Prefs.maxcolor[1], 255, NULL, 0);
+  createTrackbar("max V", "colormask", &Prefs.maxcolor[2], 255, NULL, 0);
+
+  cvSetMouseCallback("scissorsmask", mouseScissors, 0);
+}
+
+void computeBackground (const Mat& origframe, Mat& background, double& Z, const double& decayfac) {
+  background = ( exp(-decayfac) * background * Z + origframe ) / ( exp(-decayfac) * Z + 1 );
+  Z = exp(-decayfac) * Z + 1;
+
+  imshow("background", background/255.0);
+}
+
+void computeForegroundMask (const Mat& origframe, const Mat& background, Mat& foregroundMask) {
+  absdiff(origframe, background, foregroundMask);
+  cvtColor(foregroundMask, foregroundMask, CV_RGB2GRAY);
+  threshold(foregroundMask, foregroundMask, Prefs.forethreshold, 255, CV_THRESH_BINARY);
+  foregroundMask.convertTo(foregroundMask, CV_8UC3);
+
+  imshow("foregroundmask", foregroundmask);
+}
+
+void computeColorMask (const Mat& origframe, Mat& colormask) {
+  origframe.convertTo(colormask, CV_8UC3);
+  cvtColor(colormask, colormask, CV_RGB2HSV);
+  inRange(colormask, Scalar(Prefs.mincolor[0],Prefs.mincolor[1],Prefs.mincolor[2]), Scalar(Prefs.maxcolor[0], Prefs.maxcolor[1], Prefs.maxcolor[2]), colormask);
+  colormask.convertTo(colormask, CV_8UC3);
+
+  imshow("colormask", colormask);
+}
+
+void computeScissors (const Mat& origframe, Mat& mScissors) {
+  origframe.convertTo(mScissors, CV_8UC3);
+  for (int ipoint = 0; ipoint < Prefs.scissorpoints->size(); ipoint++) {
+    circle(mScissors, (*Prefs.scissorpoints)[ipoint], 5, Scalar(0,0,255), -1, 8);
+    line(mScissors, (*Prefs.scissorpoints)[ipoint], (*Prefs.scissorpoints)[ (ipoint+1)%Prefs.scissorpoints->size() ], Scalar(0,0,255), 1, 8);
+  }
+}
+
+void computeScissorsMask (Mat& scissorsmask) {
+  if ( Prefs.scissorpoints->size() >= 3) {
+    const Point* elementPoints[1] = { &((*Prefs.scissorpoints)[0]) };
+    int numberOfPoints = (int)Prefs.scissorpoints->size();
+    scissorsmask = Mat::zeros(Props.height, Props.width, CV_8U);
+    fillPoly (scissorsmask, elementPoints, &numberOfPoints, 1, Scalar (255, 255, 255), 8);
+  }
+  else
+    scissorsmask = Mat::ones(Props.height, Props.width, CV_8U);
+
+  imshow("scissorsmask", mScissors);
+}
+
+void masking_getCombinedMask(const Mat& origframe, Mat& combinedmask) {
+  double decayfac = (log(2) / normalPrefs.halfdecay) / Props.fps;
+
+  computeBackground(origframe, background, Z, decayfac);
+  computeForegroundMask(origframe, background, foregroundmask );
+
+  computeColorMask(origframe, colormask);
+
+
+  computeScissors(origframe, mScissors);
+  computeScissorsMask(scissorsmask);
+
+  bitwise_and(foregroundmask, colormask, combinedmask);
+  bitwise_and(combinedmask, scissorsmask, combinedmask);
+}

diff --git a/masking.h b/masking.h
new file mode 100644 (file)
index 0000000..d0025fe
--- /dev/null
+++ b/masking.h
@@ -0,0 +1,8 @@
+#include "opencv2/highgui/highgui.hpp"
+#include "opencv2/imgproc/imgproc.hpp"
+
+using namespace cv;
+using namespace std;
+
+void masking_getCombinedMask(const Mat& origframe, Mat& combinedmask);
+void masking_init();

diff --git a/stickletrack.cpp b/stickletrack.cpp
new file mode 100755 (executable)
index 0000000..7462af7
--- /dev/null
+++ b/stickletrack.cpp
@@ -0,0 +1,511 @@
+#include "opencv2/highgui/highgui.hpp"
+#include "opencv2/imgproc/imgproc.hpp"
+#include <iostream>
+#include <algorithm>
+#include <functional>
+#include <sys/stat.h>
+#include <fstream>
+#include <sstream>
+#include <iterator>
+#include <ctime>
+#include <iomanip>
+
+#include "masking.h"
+#include "stickletrack.h"
+#include "tracking.h"
+
+#define BACKSECONDS 30
+
+double frametime = 0;
+int framenum = 0;
+
+prefs Prefs;
+props Props;
+
+normalprefs normalPrefs;
+
+static VideoCapture capture;
+
+static bool stopvideo=false;
+static bool leftbuttondown=false;
+static Point2f startClickPos;
+static int ilatetagintime=-1, iearlytagintime=-1;
+static int startClickFrame;
+static bool stoppedbefore;
+static bool dooneframe=false;
+static vector<tag> tags;
+static int tagsselected=0;
+static int nearestTags[2];
+static vector<tag> **tagintime;
+static int tagintimeidx=0;
+static int moresleep=0;
+static int gotoframe=0;
+static int maxoutputfps;
+static double rescalingfactor;
+static ofstream tanjaLogFile;
+
+void genBaseDir() { 
+#if defined(_WIN32)
+  Props.basedir = getenv("HOMEDRIVE");
+  Props.basedir += getenv("HOMEPATH");
+#else
+  Props.basedir = getenv("HOME");
+#endif
+  
+  Props.basedir += "/.stickletrack";
+
+  cout << "Using " << Props.basedir << " for data output." << endl;
+}
+
+void beforeExit() {
+  cout << "Exitting ..." << endl;
+
+#if defined(_WIN32)
+  _mkdir(Props.basedir.c_str());
+  _mkdir((Props.basedir+"/prefs").c_str());
+#else 
+  mkdir( Props.basedir.c_str(), 0777 );
+  mkdir( (Props.basedir+"/prefs").c_str(), 0777 );
+#endif
+
+  cout << "Saving preferences ..." << endl;
+  int scissorpointsize = Prefs.scissorpoints->size();
+  ofstream prefFile( (Props.basedir+"/prefs/"+Props.videohash+".prf").c_str(), ios::binary );
+  prefFile.write((char*)&Prefs, sizeof(Prefs));
+  prefFile.write((char*)&scissorpointsize, sizeof(int));
+  
+  for (int ipoint = 0; ipoint < scissorpointsize; ipoint++)
+    prefFile.write( (char*)&(*Prefs.scissorpoints)[ipoint], sizeof(Point) );
+
+  prefFile.close();
+}
+
+void computeNormalizedPrefs() {
+  normalPrefs.contours_minshortaxis = Prefs.contours_minshortaxis * Props.diagonal / 1000.0;
+  normalPrefs.contours_maxshortaxis = Prefs.contours_maxshortaxis * Props.diagonal / 1000.0;
+  normalPrefs.contours_minlongaxis = Prefs.contours_minlongaxis * Props.diagonal / 500.0;
+  normalPrefs.contours_maxlongaxis = Prefs.contours_maxlongaxis * Props.diagonal / 500.0;
+
+  normalPrefs.contours_minarea = pow(Prefs.contours_minarea,2) * Props.width * Props.height / 1000000.0;
+  normalPrefs.contours_maxarea = pow(Prefs.contours_maxarea,2) * Props.width * Props.height / 1000000.0;
+  
+  normalPrefs.contours_mincircum = Prefs.contours_mincircum * Props.diagonal / 500.0;
+  normalPrefs.contours_maxcircum = Prefs.contours_maxcircum * Props.diagonal / 500.0;
+  
+  normalPrefs.contours_maxspeed = Prefs.contours_maxspeed * Props.diagonal / 100.0;
+
+  normalPrefs.contours_maxrot = 10 * Prefs.contours_maxrot;
+
+  normalPrefs.halfdecay = 10.0 * Prefs.halfdecay / Props.fps;
+}
+
+void trackbarCallbackUpdateNormPrefs (int trackpos, void *userdata) {
+  computeNormalizedPrefs();
+}
+
+void drawTimes(Mat& mContours) {
+  stringstream alles;
+
+  alles.precision(2);
+
+  alles << framenum << " : " << fixed << (framenum)/(double)Props.fps << "s : +" << (double)moresleep/Props.fps << "ms";
+
+  string text = alles.str();
+  int fontFace = FONT_HERSHEY_SCRIPT_SIMPLEX;
+
+  double fontScale = Props.diagonal/1000.0;
+  int thickness = 3;
+
+  int baseline=0;
+  Size textSize = getTextSize(text, fontFace,
+                             fontScale, thickness, &baseline);
+
+  Point textOrg(10, 10 + textSize.height);
+
+  rectangle(mContours, Point(0, 0),
+           Point(Props.width, 20+textSize.height),
+           Scalar(0,0,255), -1);
+
+  putText(mContours, text, textOrg, fontFace, fontScale,
+         Scalar::all(255), thickness, 8);
+}
+
+void readCreatePrefs() {
+  ifstream prefFile( (Props.basedir+"/prefs/"+Props.videohash+".prf").c_str(), ios::binary );
+  if ( prefFile.is_open() ) {
+    int scissorpointsize;
+
+    prefFile.read((char*)&Prefs, sizeof(Prefs));
+    prefFile.read((char*)&scissorpointsize, sizeof(int));
+
+    Prefs.scissorpoints = new vector<Point>;
+    for (int ipoint = 0; ipoint < scissorpointsize; ipoint++) {
+      Point tmpPoint;
+      prefFile.read((char*)&tmpPoint, sizeof(Point));
+      Prefs.scissorpoints->push_back(tmpPoint);
+    }
+
+    prefFile.close();
+
+    cout << "Loaded saved preferences." << endl;
+  }
+  else {
+    Prefs.scissorpoints = new vector<Point>;
+  }
+}
+
+void loadDefaultPrefs () {
+  Prefs.halfdecay = 100;
+  Prefs.forethreshold = 0;
+  Prefs.mincolor[0] = 0; Prefs.mincolor[1] = 0; Prefs.mincolor[2] = 0;
+  Prefs.maxcolor[0] = 255; Prefs.maxcolor[1] = 255; Prefs.maxcolor[2] = 255;
+  Prefs.manyfish = 3;
+  Prefs.contours_minshortaxis = 0;
+  Prefs.contours_maxshortaxis = 100;
+  Prefs.contours_minlongaxis = 0;
+  Prefs.contours_maxlongaxis = 100;
+  Prefs.contours_minarea = 0;
+  Prefs.contours_maxarea = 100;
+  Prefs.contours_mincircum = 0;
+  Prefs.contours_maxcircum = 100;
+  Prefs.contours_maxspeed = 100;
+  Prefs.contours_maxrot = 100;
+}
+
+unsigned long frameHash(const Mat& frame) {
+  unsigned long hash = 5381;
+  int c;
+  unsigned char *str = (unsigned char*)frame.data;
+
+  while (c = *str++) {
+    hash = ((hash << 5) + hash) + c;
+  }
+
+  return hash;
+}
+
+int timeWarp(int x, int y) {
+  int horiMove = startClickPos.x - x;
+  int wannago = startClickFrame + 10*horiMove*Props.fps/Props.width;
+
+  int maxgo = min(ilatetagintime, Props.totframes);
+  int mingo = max(1, ilatetagintime - BACKSECONDS*Props.fps + 2);
+
+  if (wannago > maxgo)
+    return maxgo;
+  else if (wannago < mingo)
+    return mingo;
+  else
+    return wannago;
+}
+
+int rotatingTime( int whereyouwant ) {
+  if ( whereyouwant < 0 )
+    return BACKSECONDS*Props.fps + whereyouwant;
+  else
+    return whereyouwant;
+}
+
+void mouseTracking(int evt, int x, int y, int flags, void* param){
+  if ( stopvideo && evt == CV_EVENT_LBUTTONDBLCLK ) {
+    tagsselected++;
+
+    double mindist = -1;
+    for (int itag = 0; itag < tags.size(); itag++) {
+      double dist = pow(tags[itag].x - x, 2) + pow(tags[itag].y - y, 2);
+      if ( dist < mindist || mindist == -1 ) {
+       nearestTags[tagsselected-1] = itag;
+       mindist = dist;
+      }
+    }
+
+    if (tagsselected == 2) {
+      if ( nearestTags[0] < tags.size() && nearestTags[0] >= 0
+          && nearestTags[1] < tags.size() && nearestTags[1] >= 0
+          && nearestTags[0] != nearestTags[1] ) {
+
+       tag tmp;
+
+       tmp = tags[nearestTags[0]];
+       tags[nearestTags[0]] = tags[nearestTags[1]];
+       tags[nearestTags[1]] = tmp;
+       tags[nearestTags[1]].color = tags[nearestTags[0]].color;
+       tags[nearestTags[0]].color = tmp.color;
+
+       for (int itagintime = framenum-1; itagintime <= ilatetagintime; itagintime++) {
+         tmp = (*tagintime[ rotatingTime(tagintimeidx - (ilatetagintime-itagintime) - 1) ])[nearestTags[0]];
+
+         (*tagintime[ rotatingTime(tagintimeidx - (ilatetagintime-itagintime) - 1) ])[nearestTags[0]] = 
+           (*tagintime[ rotatingTime(tagintimeidx - (ilatetagintime-itagintime) - 1) ])[nearestTags[1]];
+
+         (*tagintime[ rotatingTime(tagintimeidx - (ilatetagintime-itagintime) - 1) ])[nearestTags[1]] = tmp;
+
+         (*tagintime[ rotatingTime(tagintimeidx - (ilatetagintime-itagintime) - 1) ])[nearestTags[1]].color = 
+           (*tagintime[ rotatingTime(tagintimeidx - (ilatetagintime-itagintime) - 1) ])[nearestTags[0]].color;
+
+         (*tagintime[ rotatingTime(tagintimeidx - (ilatetagintime-itagintime) - 1) ])[nearestTags[0]].color = tmp.color;
+       } 
+      }
+      tagsselected = 0;
+
+      gotoframe = framenum;
+      dooneframe = true;
+    }
+  }
+
+  if ( evt == CV_EVENT_RBUTTONUP ) {
+    stopvideo = (stopvideo+1)%2;
+  }
+
+  if (evt==CV_EVENT_LBUTTONDOWN) {
+    leftbuttondown = true;
+    startClickPos = Point2f(x,y);
+    startClickFrame = framenum;
+    
+    if (stopvideo)
+      stoppedbefore = true;
+    else
+      stoppedbefore = false;
+    
+    stopvideo = true;
+  }
+
+  if (evt==CV_EVENT_LBUTTONUP) {
+    leftbuttondown = false;
+    
+    int vertiMove = startClickPos.y - y;
+
+    if ( ! stoppedbefore )
+      stopvideo = false;
+ }
+
+  if ( leftbuttondown && startClickPos.x - x != 0  && ! (startClickPos.x > Props.width*5.0/6.0 && x > Props.width*5.0/6.0) ) {
+    gotoframe = timeWarp(x, y);
+    dooneframe = true;
+  }
+
+  if ( leftbuttondown && startClickPos.y - y != 0) {
+    int vertiMove = startClickPos.y - y;
+
+    if ( startClickPos.x > Props.width*5.0/6.0 && x > Props.width*5.0/6.0 ) {
+      moresleep -= vertiMove*10000/Props.height;
+      if ( moresleep < 0 )
+       moresleep = 0;
+      
+      drawTimes(tracking_getFrame());
+    }    
+  }
+}
+
+void writeTanjaLog (int writeframenum, vector<tag> *tagstowrite) {
+  stringstream outline;
+
+  if ( writeframenum%(Props.fps/maxoutputfps) == 0 ) {
+    if ( tagstowrite->size() >= Prefs.manyfish ) {
+      outline << (double)writeframenum/Props.fps;
+      for ( int itag = 0; itag < Prefs.manyfish; itag++) {
+       outline << "," << (*tagstowrite)[itag].x << "," << (*tagstowrite)[itag].y << "," << (double)writeframenum/Props.fps-(*tagstowrite)[itag].lastseen;
+      }
+      outline << endl;
+      tanjaLogFile.write( outline.str().c_str(), outline.str().size() );
+      tanjaLogFile.flush();
+    }
+  }
+}
+
+void openTanjaLog () {
+  stringstream tanjalogfilename;
+
+#if defined(_WIN32)
+  _mkdir(Props.basedir.c_str());
+  _mkdir((Props.basedir+"/logs").c_str());
+#else 
+  mkdir( Props.basedir.c_str(), 0777 );
+  mkdir( (Props.basedir+"/logs").c_str(), 0777 );
+#endif
+
+#if defined(_WIN32)
+  _mkdir(Props.basedir.c_str());
+  _mkdir((Props.basedir+"/logs/"+Props.videohash).c_str());
+#else 
+  mkdir( Props.basedir.c_str(), 0777 );
+  mkdir( (Props.basedir+"/logs/"+Props.videohash).c_str(), 0777 );
+#endif
+
+  tanjalogfilename << Props.basedir << "/logs/"  << Props.videohash << "/" << time(0) << ".dat";
+  tanjaLogFile.open( tanjalogfilename.str().c_str() );
+
+  if ( ! tanjaLogFile.is_open() ) {
+    cerr << "Could not open tanjalogfile " << tanjalogfilename.str() << "!" << endl;
+    exit(1);
+  }
+
+  tanjaLogFile.write("# STICKLETRACK - TANJALOG\n", 26);
+  tanjaLogFile.write("# format: <time in s>,<tag_1 xpos>,<tag_1 ypos>,<tag_1 losttime>,...,<tag_n xpos>,<tag_n ypos>,<tag_n losttime>\n", 112);
+
+  tanjaLogFile.flush();
+}
+
+int process(VideoCapture& capture) {
+  int n = 0;
+  char filename[200];
+  bool pleaseExit = false;
+
+  namedWindow("original", CV_WINDOW_KEEPRATIO);
+    
+  Mat frame, origframe, combinedmask;
+
+  Props.fps = capture.get(CV_CAP_PROP_FPS);
+  Props.width = capture.get(CV_CAP_PROP_FRAME_WIDTH)*rescalingfactor;
+  Props.height = capture.get(CV_CAP_PROP_FRAME_HEIGHT)*rescalingfactor;
+  Props.totframes = capture.get(CV_CAP_PROP_FRAME_COUNT);
+  Props.diagonal = sqrt( pow(Props.width, 2) + pow(Props.height, 2) );
+
+  Mat frameintime[BACKSECONDS*Props.fps];
+
+  tagintime = new vector<tag>*[BACKSECONDS*Props.fps];
+
+  for (int iback = 0; iback < BACKSECONDS*Props.fps; iback++)
+    tagintime[iback] = new vector<tag>;
+
+  capture >> frame;
+
+  if (frame.empty())
+    exit(1);
+
+  stringstream tmphash;
+  tmphash << frameHash(frame);
+  Props.videohash = tmphash.str();
+
+  loadDefaultPrefs();
+  readCreatePrefs();
+
+  computeNormalizedPrefs();
+
+  openTanjaLog();
+
+  masking_init();
+  tracking_init();
+
+  cvSetMouseCallback("contours_picture", mouseTracking, 0);
+
+  capture.set(CV_CAP_PROP_POS_FRAMES, 0);
+
+  for (; !pleaseExit;) {
+    if ( !stopvideo || dooneframe ) {
+      framenum = gotoframe;
+
+      dooneframe = false;
+
+      frametime = (framenum-1.0) / Props.fps;
+
+      if ( framenum <= ilatetagintime ) {
+       origframe = frameintime[ rotatingTime(tagintimeidx - (ilatetagintime-framenum) - 2) ];
+       masking_getCombinedMask(origframe, combinedmask);
+
+       tags.clear();
+       for (int itag = 0; itag < tagintime[ rotatingTime( tagintimeidx - 2 - (ilatetagintime-framenum) ) ]->size(); itag++)
+         tags.push_back( (*tagintime[ rotatingTime(tagintimeidx - (ilatetagintime-framenum) - 2) ])[itag] );
+       
+       tracking_locateTags (tags, combinedmask);
+      }
+      else {
+       capture >> frame;
+
+       if (frame.empty())
+         break;
+
+       frame.convertTo(origframe, CV_32FC3);
+       resize(origframe, origframe, Size(0,0), rescalingfactor, rescalingfactor);
+
+       masking_getCombinedMask(origframe, combinedmask);
+      
+       tracking_locateTags (tags, combinedmask);
+
+       tagintime[tagintimeidx]->clear();
+       for (int itag = 0; itag < tags.size(); itag++)
+         tagintime[tagintimeidx]->push_back(tags[itag]);
+
+       frameintime[tagintimeidx] = origframe;
+       
+       ilatetagintime = framenum;
+       tagintimeidx = (tagintimeidx+1)%(BACKSECONDS*Props.fps);
+
+       if ( ilatetagintime >= BACKSECONDS*Props.fps-1 )
+         writeTanjaLog( framenum - (BACKSECONDS*Props.fps-1), tagintime[ tagintimeidx ] );
+      }
+
+      gotoframe = framenum + 1;
+ 
+      imshow("original", origframe/255.0);
+    }
+    
+    drawTimes(tracking_getFrame());
+
+    if ( tagsselected == 1 ) {
+      circle( tracking_getFrame(), Point2f(tags[nearestTags[0]].x, tags[nearestTags[0]].y), Props.diagonal / 100.0, Scalar(0,0,255), -1, 8 );
+    }
+   
+    tracking_showFrame();
+
+    char key;
+
+    if (!stopvideo)
+      key = (char)waitKey(5 + (double)moresleep/Props.fps);
+    else
+      key = (char)waitKey(5);
+
+    switch (key) {
+    case 'q':
+      pleaseExit = true;
+      break;
+    default:
+      break;
+    }
+
+  }
+  
+  if ( ilatetagintime >= BACKSECONDS*Props.fps-1 ) {
+    int timeidx = (tagintimeidx+1)%(BACKSECONDS*Props.fps);
+    for ( int itime = 0; itime < BACKSECONDS*Props.fps-1; itime++ ) {
+      writeTanjaLog( framenum+1+itime - (BACKSECONDS*Props.fps-1), tagintime[ timeidx ] );
+      timeidx = (timeidx+1)%(BACKSECONDS*Props.fps);
+    }
+  }
+  else {
+    for ( int itime = 0; itime < ilatetagintime+1; itime++ )
+      writeTanjaLog( itime, tagintime[ itime ] );
+  }
+
+  tanjaLogFile.close();
+
+  beforeExit();
+
+  cout << "Bye bye." << endl;
+
+  return 0;
+}
+
+int main(int ac, char** av) {
+  if ( ac != 4 ) {
+    cout << "Usage: stickletrack <maxoutputfps> <rescalingfactor> <videofilename>" << endl;
+    exit(0);
+  }
+
+  maxoutputfps = atoi( av[1] );
+  rescalingfactor = atof( av[2] );
+
+  capture.open( av[3] ); 
+
+  genBaseDir();
+  
+   if (!capture.isOpened()) {
+    cerr << "Failed to open a video device or video file!\n" << endl;
+    return 1;
+  }
+
+   cout << "Exit with q if you want to save your data!!!" << endl;
+  
+  return process(capture);
+}
+

diff --git a/stickletrack.h b/stickletrack.h
new file mode 100644 (file)
index 0000000..51715f0
--- /dev/null
+++ b/stickletrack.h
@@ -0,0 +1,70 @@
+#include <string>
+#include <vector>
+#include "opencv2/highgui/highgui.hpp"
+#include "opencv2/imgproc/imgproc.hpp"
+
+using namespace cv;
+using namespace std;
+
+struct props {
+  int width;
+  int height;
+  int fps;
+  string videohash;
+  string basedir;
+  float diagonal;
+  int totframes;
+};
+
+struct prefs {
+  vector<Point> *scissorpoints;
+
+  int halfdecay;
+  int forethreshold;
+  int mincolor[3];
+  int maxcolor[3];
+  int manyfish;
+
+  int contours_minshortaxis;
+  int contours_maxshortaxis;
+  int contours_minlongaxis;
+  int contours_maxlongaxis;
+
+  int contours_minarea;
+  int contours_maxarea;
+  
+  int contours_mincircum;
+  int contours_maxcircum;
+
+  int contours_maxspeed;
+
+  int contours_maxrot;
+};
+
+struct normalprefs {
+  double contours_minshortaxis;
+  double contours_maxshortaxis;
+  double contours_minlongaxis;
+  double contours_maxlongaxis;
+
+  double contours_minarea;
+  double contours_maxarea;
+  
+  double contours_mincircum;
+  double contours_maxcircum;
+  
+  double contours_maxspeed;
+
+  double contours_maxrot;
+
+  double halfdecay;
+};
+
+extern prefs Prefs;
+extern props Props;
+extern double frametime;
+extern int framenum;
+extern int wannabeframenum;
+extern normalprefs normalPrefs;
+
+void trackbarCallbackUpdateNormPrefs (int trackpos, void *userdata);

diff --git a/tracking.cpp b/tracking.cpp
new file mode 100644 (file)
index 0000000..4e401f8
--- /dev/null
+++ b/tracking.cpp
@@ -0,0 +1,357 @@
+#include <stdio.h>
+#include <iostream>
+
+using namespace std;
+
+#include "tracking.h"
+#include "stickletrack.h"
+
+#define SEPCOST_AREA      0
+#define SEPCOST_CIRCUM    1
+#define SEPCOST_SHORTAXIS 2
+#define SEPCOST_LONGAXIS  3
+#define SEPCOST_ANGLE     4
+#define SEPCOST_DIST      5
+
+struct fishblobb {
+  double x;
+  double y;
+  bool identified;
+  int contourid;
+  vector<Point> *points;
+  double longaxis;
+  double shortaxis;
+  double angle;
+  int area;
+  int circum;
+  Point2f head, tail;
+};
+
+static int enablecontours=0;
+static Mat mContours;
+
+struct tagcost {
+  int tagid;
+  double tagcost;
+};
+
+struct blobbcost {
+  int blobbid;
+  vector<tagcost> tagcosts;
+};
+
+bool sort_tags (tagcost i, tagcost j) {
+  return i.tagcost < j.tagcost;
+}
+
+bool sort_blobbs (blobbcost i, blobbcost j) {
+  return i.tagcosts[0].tagcost < j.tagcosts[0].tagcost;
+}
+
+Mat& tracking_getFrame() {
+  return mContours;
+}
+
+void tracking_showFrame() {
+  imshow("contours_picture", mContours);
+}
+
+bool tracking_isEnabled () {
+  return enablecontours;
+}
+
+void tracking_init() {
+  namedWindow("contours", CV_WINDOW_KEEPRATIO);
+  namedWindow("contours_picture", CV_WINDOW_KEEPRATIO);
+
+  createTrackbar("Enable", "contours", &enablecontours, 1, NULL, 0);
+  createTrackbar("Manyfish", "contours", &Prefs.manyfish, 10, NULL, 0);
+  createTrackbar("min area", "contours", &Prefs.contours_minarea, 100, &trackbarCallbackUpdateNormPrefs, 0);
+  createTrackbar("max area", "contours", &Prefs.contours_maxarea, 100, &trackbarCallbackUpdateNormPrefs, 0);
+  createTrackbar("min circum", "contours", &Prefs.contours_mincircum, 100, &trackbarCallbackUpdateNormPrefs, 0);
+  createTrackbar("max circum", "contours", &Prefs.contours_maxcircum, 100, &trackbarCallbackUpdateNormPrefs, 0);
+  createTrackbar("min shortaxis", "contours", &Prefs.contours_minshortaxis, 100, &trackbarCallbackUpdateNormPrefs, 0);
+  createTrackbar("max shortaxis", "contours", &Prefs.contours_maxshortaxis, 100, &trackbarCallbackUpdateNormPrefs, 0);
+  createTrackbar("min longaxis", "contours", &Prefs.contours_minlongaxis, 100, &trackbarCallbackUpdateNormPrefs, 0);
+  createTrackbar("max longaxis", "contours", &Prefs.contours_maxlongaxis, 100, &trackbarCallbackUpdateNormPrefs, 0);
+  createTrackbar("max speed", "contours", &Prefs.contours_maxspeed, 100, &trackbarCallbackUpdateNormPrefs, 0);
+  createTrackbar("max rotation speed", "contours", &Prefs.contours_maxrot, 100, &trackbarCallbackUpdateNormPrefs, 0);
+
+  mContours = Mat::zeros(Props.height, Props.width, CV_8UC3);
+}
+
+double kitvalue (double min, double max) {
+  return min + pow( cos(frametime*10), 2)*(max-min);
+}
+
+void findFishBlobbs(vector<fishblobb>& fishblobbs, vector< vector<Point> >& contours, vector<Vec4i>& hierarchy) {
+  for (int icontour=0; icontour >= 0; icontour = hierarchy[icontour][0]) {
+    fishblobb newblobb;
+
+    if ( contours[icontour].size() < 5 )
+      continue;
+
+    newblobb.circum = contours[icontour].size();
+    if ( newblobb.circum < normalPrefs.contours_mincircum || newblobb.circum > normalPrefs.contours_maxcircum )
+      continue;
+
+    newblobb.area = contourArea(contours[icontour]);
+    if ( newblobb.area < normalPrefs.contours_minarea || newblobb.area > normalPrefs.contours_maxarea )
+      continue;
+
+    RotatedRect minEllipse = fitEllipse(contours[icontour]);
+    newblobb.longaxis = minEllipse.size.height;
+    newblobb.shortaxis = minEllipse.size.width;
+    if ( newblobb.shortaxis < normalPrefs.contours_minshortaxis || newblobb.shortaxis > normalPrefs.contours_maxshortaxis
+        || newblobb.longaxis < normalPrefs.contours_minlongaxis || newblobb.longaxis > normalPrefs.contours_maxlongaxis )
+      continue;
+     
+    newblobb.x = minEllipse.center.x;
+    newblobb.y = minEllipse.center.y;
+     
+    RotatedRect minRect;
+    Mat mFish = Mat::zeros(Props.height, Props.width, CV_8U);
+    Moments fishMom;
+    Point2f massCenter;
+    Point2f recHead, recTail;
+       
+    newblobb.identified = false;
+    newblobb.points = &contours[icontour];
+
+    newblobb.contourid = icontour;
+
+    /* find head/tail sensitive angle
+       maybe the method could be improved */
+    drawContours( mFish, contours, icontour, Scalar(255, 255, 255), CV_FILLED );
+    fishMom = moments(mFish);
+      
+    massCenter = Point2f( fishMom.m10/fishMom.m00 , fishMom.m01/fishMom.m00 );
+    minRect = minAreaRect(contours[ icontour ]);
+      
+    recHead = Point2f( minRect.center.x + newblobb.longaxis*cos(minEllipse.angle * M_PI / 180 + M_PI/2), 
+                      minRect.center.y + newblobb.longaxis*sin(minEllipse.angle * M_PI / 180 + M_PI/2) );
+    recTail = Point2f( minRect.center.x + newblobb.longaxis*cos(minEllipse.angle * M_PI / 180 - M_PI/2), 
+                      minRect.center.y + newblobb.longaxis*sin(minEllipse.angle * M_PI / 180 - M_PI/2) );
+      
+    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 ) ) {
+      newblobb.angle = minEllipse.angle + 180;
+       
+      if (newblobb.angle > 360)
+       newblobb.angle = newblobb.angle - 360;
+    }
+    else {
+      newblobb.angle = minEllipse.angle;
+    }
+
+    newblobb.tail = Point2f( minEllipse.center.x + newblobb.longaxis*cos(newblobb.angle * M_PI / 180 - M_PI/2),
+                            minEllipse.center.y + newblobb.longaxis*sin(newblobb.angle * M_PI / 180 - M_PI/2) );
+    newblobb.head = Point2f( minEllipse.center.x + newblobb.longaxis*cos(newblobb.angle * M_PI / 180 + M_PI/2), 
+                            minEllipse.center.y + newblobb.longaxis*sin(newblobb.angle * M_PI / 180 + M_PI/2) );
+
+    fishblobbs.push_back(newblobb);
+  }
+}
+
+void tagCare(vector<tag>& tags) {
+  while ( tags.size() > Prefs.manyfish )
+    tags.pop_back();
+
+  while ( tags.size() < Prefs.manyfish ) {
+    tag tmp;
+    tmp.x = 0;
+    tmp.y = 0;
+    tmp.propZ = 0;
+    tmp.lastseen = frametime;
+
+    switch ( tags.size() ) {
+    case 0:
+      tmp.color = Scalar( 0, 255, 255 );
+      break;
+    case 1:
+      tmp.color = Scalar( 255, 0, 255 );
+      break;
+    case 2:
+      tmp.color = Scalar( 255, 255, 0 );
+      break;
+    case 3:
+      tmp.color = Scalar( 255, 0, 0 );
+      break;
+    case 4:
+      tmp.color = Scalar( 0, 255, 0 );
+      break;
+    case 5:
+      tmp.color = Scalar( 0, 0, 255 );
+      break;
+    default:
+      tmp.color = Scalar( rand()%255, rand()%255, rand()%255 );
+    }
+
+    tmp.angle = 0;
+    tmp.area = 0;
+    tmp.circum = 0;
+    tmp.shortaxis = 0;
+    tmp.longaxis = 0;
+    tmp.virgin = true;
+    tags.push_back(tmp);
+  }
+}
+
+void matchBlobbsNTags(vector<tag>& tags, vector<fishblobb>& fishblobbs) {
+  vector<blobbcost> blobbcosts;
+
+  for (int iblobb = 0; iblobb < fishblobbs.size(); iblobb++) {
+    double sepcost[tags.size()][6];
+    double maxcost[6];
+    blobbcost blobbCost;
+
+    blobbCost.blobbid = iblobb;
+
+    for (int imax = 0; imax < 6; imax++)
+      maxcost[imax] = 0;
+
+    for (int itag = 0; itag < tags.size(); itag++) {
+      double anglediff = fabs (fishblobbs[iblobb].angle - tags[itag].angle);
+           
+      if ( anglediff > 180 )
+       anglediff = 360 - anglediff;
+
+      sepcost[itag][SEPCOST_ANGLE] = anglediff;
+
+      sepcost[itag][SEPCOST_AREA] = fabs ( fishblobbs[iblobb].area - tags[itag].area );
+      sepcost[itag][SEPCOST_CIRCUM] = fabs ( fishblobbs[iblobb].circum - tags[itag].circum );
+      sepcost[itag][SEPCOST_SHORTAXIS] = fabs ( fishblobbs[iblobb].shortaxis - tags[itag].shortaxis );
+      sepcost[itag][SEPCOST_LONGAXIS] = fabs ( fishblobbs[iblobb].longaxis - tags[itag].longaxis );
+      sepcost[itag][SEPCOST_DIST] = sqrt( pow(tags[itag].x - fishblobbs[iblobb].x, 2) + pow(tags[itag].y - fishblobbs[iblobb].y, 2) );
+
+      for (int imax = 0; imax < 6; imax++)
+       if ( maxcost[imax] < sepcost[itag][imax] )
+         maxcost[imax] = sepcost[itag][imax];
+    }
+
+    for (int itag = 0; itag < tags.size(); itag++) {
+      tagcost newTagCost;
+           
+      newTagCost.tagid = itag;
+      newTagCost.tagcost = 0;
+
+      if ( sepcost[itag][SEPCOST_DIST] > ( frametime - tags[itag].lastseen )*normalPrefs.contours_maxspeed 
+          || sepcost[itag][SEPCOST_ANGLE] > ( frametime - tags[itag].lastseen )*normalPrefs.contours_maxrot ) 
+       continue;
+
+      for (int isep = 0; isep < 6; isep++)
+       newTagCost.tagcost += sepcost[itag][isep] / maxcost[isep];
+
+      blobbCost.tagcosts.push_back( newTagCost );                                              
+    }
+
+    if ( blobbCost.tagcosts.size() > 0 ) { 
+      sort( blobbCost.tagcosts.begin(), blobbCost.tagcosts.end(), sort_tags );
+      blobbcosts.push_back( blobbCost );
+    }
+  }
+       
+  for (int itag = 0; itag < tags.size() && blobbcosts.size() > 0; itag++) {
+    sort( blobbcosts.begin(), blobbcosts.end(), sort_blobbs );
+
+    int tagnow = blobbcosts[0].tagcosts[0].tagid;
+
+    tags[tagnow].virgin = false;
+    tags[tagnow].x = fishblobbs[ blobbcosts[0].blobbid ].x;
+    tags[tagnow].y = fishblobbs[ blobbcosts[0].blobbid ].y;
+    tags[tagnow].shortaxis = fishblobbs[ blobbcosts[0].blobbid ].shortaxis;
+    tags[tagnow].longaxis =  fishblobbs[ blobbcosts[0].blobbid ].longaxis;
+    tags[tagnow].angle = fishblobbs[ blobbcosts[0].blobbid ].angle;
+    tags[tagnow].lastseen = frametime;
+    tags[tagnow].area = fishblobbs[ blobbcosts[0].blobbid ].area;
+    tags[tagnow].circum = fishblobbs[ blobbcosts[0].blobbid ].circum;
+    tags[tagnow].head = fishblobbs[ blobbcosts[0].blobbid ].head;
+    tags[tagnow].tail = fishblobbs[ blobbcosts[0].blobbid ].tail;
+
+    blobbcosts.erase( blobbcosts.begin() );
+
+    for (int iblobb = 0; iblobb < blobbcosts.size(); iblobb++) {
+           
+      for (int itag = 0; itag < blobbcosts[iblobb].tagcosts.size(); itag++)
+       if ( blobbcosts[iblobb].tagcosts[itag].tagid == tagnow ) {
+         blobbcosts[iblobb].tagcosts.erase( blobbcosts[iblobb].tagcosts.begin() + itag );
+         break;
+       }
+          
+      if ( blobbcosts[iblobb].tagcosts.size() == 0 ) {
+       blobbcosts.erase( blobbcosts.begin() + iblobb );
+       iblobb--;
+      }
+    }
+  }
+}
+
+void drawTags (const vector<tag>& tags) {
+  RotatedRect rRect;
+  Point2f vertices[4];
+
+  for (int itag = 0; itag < tags.size(); itag++) {
+    double recside = sqrt( kitvalue( normalPrefs.contours_minarea, normalPrefs.contours_maxarea) );
+    rRect = RotatedRect( Point(tags[itag].x, tags[itag].y), Size2f(recside, recside), tags[itag].angle );
+    rRect.points(vertices);
+    for (int i = 0; i < 4; i++)
+      line(mContours, vertices[i], vertices[(i+1)%4], Scalar(255,255,255), Props.diagonal/1000.0);
+
+    ellipse(mContours, Point(tags[itag].x, tags[itag].y), 
+           Size( kitvalue( normalPrefs.contours_minshortaxis, normalPrefs.contours_maxshortaxis ), 
+                 kitvalue( normalPrefs.contours_minlongaxis, normalPrefs.contours_maxlongaxis ) ), 
+           tags[itag].angle, 0, 360, Scalar(255,255,255), Props.diagonal/1000.0, 8);
+
+    circle(mContours, Point(tags[itag].x, tags[itag].y), kitvalue( normalPrefs.contours_mincircum, normalPrefs.contours_maxcircum )/(2*M_PI), Scalar(255,255,255), 
+          Props.diagonal/1000.0, 8);
+      
+    if ( ! tags[itag].virgin ) {
+      float searchradius = ( frametime - tags[itag].lastseen )*normalPrefs.contours_maxspeed;
+      float searchangle = ( frametime - tags[itag].lastseen )*normalPrefs.contours_maxrot;
+      ellipse(mContours, Point(tags[itag].x, tags[itag].y), Size(searchradius, searchradius), tags[itag].angle, 90-searchangle, 90+searchangle, 
+             tags[itag].color, Props.diagonal/1000.0, 8);
+      
+      rRect = RotatedRect( Point(tags[itag].x, tags[itag].y), Size2f(sqrt(tags[itag].area), sqrt(tags[itag].area)), tags[itag].angle );
+      rRect.points(vertices);
+      for (int i = 0; i < 4; i++)
+       line(mContours, vertices[i], vertices[(i+1)%4], tags[itag].color, 2);
+      
+      ellipse(mContours, Point(tags[itag].x, tags[itag].y), 
+             Size( tags[itag].shortaxis, tags[itag].longaxis ), tags[itag].angle, 0, 360, tags[itag].color, 2*Props.diagonal/1000.0, 8);
+
+      circle(mContours, Point(tags[itag].x, tags[itag].y), tags[itag].circum / (2*M_PI), tags[itag].color, 2*Props.diagonal/1000.0, 8);
+      
+      circle(mContours, tags[itag].head, 5*Props.diagonal/1000.0, tags[itag].color, -1, 8);
+    }
+  }
+}
+
+void tracking_locateTags (vector<tag>& tags, Mat combinedmask_contour) {
+  mContours = Mat::zeros(Props.height, Props.width, CV_8UC3);
+
+  if (enablecontours) {
+    vector< vector<Point> > contours;
+    vector<Vec4i> hierarchy;
+    vector <fishblobb> fishblobbs;
+
+    findContours( combinedmask_contour, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE );
+
+    if ( contours.size() > 0 ) {
+
+      for(int idx = 0 ; idx >= 0; idx = hierarchy[idx][0] )
+       drawContours(mContours, contours, idx, Scalar(255, 255, 255), CV_FILLED, 8, hierarchy);
+
+      findFishBlobbs(fishblobbs, contours, hierarchy);
+
+      for ( int iblobb = 0; iblobb < fishblobbs.size(); iblobb++) {
+       Scalar color( rand()%255, rand()%255, rand()%255 );
+       drawContours( mContours, contours, fishblobbs[iblobb].contourid, color, CV_FILLED, 8, hierarchy );
+      }
+
+      tagCare(tags);
+      matchBlobbsNTags(tags, fishblobbs);
+
+    }
+
+    drawTags(tags);
+  }
+}

diff --git a/tracking.h b/tracking.h
new file mode 100644 (file)
index 0000000..c7cda7b
--- /dev/null
+++ b/tracking.h
@@ -0,0 +1,26 @@
+#include "opencv2/highgui/highgui.hpp"
+#include "opencv2/imgproc/imgproc.hpp"
+
+using namespace std;
+using namespace cv;
+
+struct tag {
+  double x;
+  double y;
+  double propZ;
+  double lastseen;
+  Scalar color;
+  double angle;
+  double longaxis;
+  double shortaxis;
+  double area;
+  double circum;
+  bool virgin;
+  Point2f head, tail;
+};
+
+void tracking_init();
+void tracking_locateTags(vector<tag>& tags, Mat combinedmask_contour);
+Mat& tracking_getFrame();
+void tracking_showFrame();
+bool tracking_isEnabled();