Merge pull request #12 from mb354/dyson-dev

Merge in to develop branch:

* Execute loss recovery when it doesn't track.
* Removed some asserts that were causing trouble.
* Take into account the orientation of the robot when creating a patch for tracking the target
* Corrected m_pos when lost near the limits of the window due to offsets. 
Otherwise it get in a state where ncc2 returns 0 and lossRecovery never recovers.
* Speed up the loss recovery it computes cross correlation only on pixels divisible by 2.
* Tidy up to README

README.md

@@ -18,14 +18,18 @@ __CMake__ (2.8.10.1)
 
 __Qt__ (4.8.1)
 
-	sudo apt-get install libqt4-dev
+	sudo apt-get install libqt4-dev qt4-dev-tools
+
+__Unicap__ (0.9.12)
 
-__OpenCV__ (2.4.6)
+	sudo apt-get install libunicap2-dev
+
+__Compiling OpenCV on Linux__ (2.4.6)
 
 	git clone https://github.com/Itseez/opencv.git 
 	git checkout 2.4.6
 	mkdir build && cd build
-	cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/path/to/opencv/install
+	cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/path/to/opencv/install ..
 	cmake --build . --target install
 
 __Unicap__ (0.9.12)
@@ -39,6 +43,16 @@ __Unicap__ (0.9.12)
 * [Xvid](http://www.xvid.org/) (1.3.2)
 
 * [OpenCV](http://sourceforge.net/projects/opencvlibrary/files/opencv-win/) (2.4.6)
+
+__OpenCV__ (2.4.6)
+
+	cd C:\
+	git clone https://github.com/Itseez/opencv.git 
+	git checkout 2.4.6
+	cd opencv
+	mkdir build && cd build
+	cmake -G"MinGW Makefiles" -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX="C:\opencv\install" ..
+	cmake --build . --target install
 
 __MinGW Only__
 
@@ -60,20 +74,20 @@ __Installer__
 
 ## Compiling
 
-From root directory:
 
 __Linux__
+From root directory:
 
 	$ mkdir build && cd build
 	$ cmake [options] ../
-	    e.g. cmake -DOpenCV_ROOT_DIR=/path/to/opencv/install -DCMAKE_BUILD_TYPE=Debug -DGTS_TESTS=ON -DCMAKE_INSTALL_PREFIX=/home/username/gts/ ../
+	    e.g. cmake -DOpenCV_ROOT_DIR=/path/to/opencv/install -DCMAKE_BUILD_TYPE=Debug -DGTS_TESTS=ON ../
 	$ cmake --build . --target install
 
 __Windows (MinGW)__
 
 	mkdir build
 	cd build
-	cmake.exe -G"MinGW Makefiles" -DOpenCV_ROOT_DIR=C:\PATH-TO-OPENCV-INSTALL ..   
+	cmake.exe -G"MinGW Makefiles" -DOpenCV_ROOT_DIR="C:\opencv\install" -DCMAKE_INSTALL_PREFIX="C:\GTS" ..   
 	cmake --build . --target install
 
 Alternatively, call [scripts/minGW_app.bat](scripts/minGW_app.bat) from root directory.

app/src/tools/core/tabs/PostProcessWidget.cpp

@@ -64,6 +64,7 @@ PostProcessWidget::PostProcessWidget( QWidget* const parent ) :
     Tool( parent, CreateSchema() ),
     m_ui( new Ui::PostProcessWidget )
 {
+    m_resultsModel=NULL;
     m_ui->setupUi( this );
 
     QObject::connect( m_ui->m_loadDataBtn,

app/src/tracking/GtsView.cpp

@@ -519,6 +519,12 @@ void GtsView::StepTracker( bool forward, CoverageSystem* coverage )
         if ( m_tracker->IsActive() )
         {
             tracking = m_tracker->Track( videoTimeStampInMillisecs );
+	    // If active but cannot track then go to lossRecovery
+	    if(!tracking)
+	    {
+		  m_tracker->DoInactiveProcessing( videoTimeStampInMillisecs );
+                  m_tracker->LossRecovery();
+	    }
         }
         else
         {

app/src/tracking/KltTracker.cpp

@@ -304,16 +304,26 @@ bool KltTracker::Track( double timestampInMillisecs, bool flipCorrect, bool init
         }
 
         // If we found a good track and the 2nd stage was a success then store the result
-        const float error = GetError();
+       // const float error = GetError();
 
-	assert( error >= -1.0 );
-        assert( error <= 1.0 );
-
-        m_history.emplace_back( TrackEntry( GetPosition(), GetHeading(), GetError(), timestampInMillisecs, warpGradient ) );
+        //assert( error >= -1.0 );
+        //assert( error <= 1.0 );warp gradient magnitude at the tracked position to store in TrackEntry for later use.
+        //const CvMat* w
+	m_history.emplace_back( TrackEntry( GetPosition(), GetHeading(), GetError(), timestampInMillisecs, warpGradient ) );
 
         return true;
     }
-
+    /*if ( !IsLost() )
+    {
+      LOG_WARN("Lost.");
+      SetJustLost(); // tracker has transitioned into lost state
+    }
+    else
+    {
+       LOG_WARN("Still lost.");
+       SetLost(); // ttacker is still lost
+    }
+    */
     return false;
 }
 
@@ -669,6 +679,34 @@ void KltTracker::PredictTargetAppearance( float angleInRadians, float offsetAngl
     cvSmooth( m_appearanceImg, m_appearanceImg, CV_GAUSSIAN, smoothing );
 }
 
+
+/**
+ Predicts the appearance of the target using computed heading, using
+ an extra parameter for specifying the position
+
+ **/
+void KltTracker::PredictTargetAppearance2( float angleInRadians, float offsetAngleDegrees, float x, float y )
+{
+    if ( !m_targetImg )
+        return;
+
+    float angle = (float)((180 + MathsConstants::R2D * angleInRadians) + offsetAngleDegrees);
+    int smoothing = 5;
+    // Set the background 'color' that will be used for pixels in the
+    // appearance model outside the radius of the target.
+    cvSet( m_appearanceImg, cvScalar( m_targetBackGroundGreyLevel ) );
+
+    float R[6];
+    CvMat rot = cvMat( 2, 3, CV_32F, R );
+    CvPoint2D32f centre = cvPoint2D32f( m_targetImg->width / 2.f, m_targetImg->height / 2.f );
+    cv2DRotationMatrix( centre, angle, 1, &rot );
+    R[2] += x - m_targetImg->width / 2.f;
+    R[5] += y - m_targetImg->height / 2.f;
+    cvWarpAffine( m_targetImg, m_appearanceImg, &rot, CV_INTER_LINEAR );
+
+    cvSmooth( m_appearanceImg, m_appearanceImg, CV_GAUSSIAN, smoothing );
+}
+
 /**
  Motion detection.
 
@@ -713,23 +751,22 @@ void KltTracker::LossRecovery()
 
     //cvConvertScale( m_avgFloat, m_avg, 1.0, 0.0 );
     cvAbsDiff(m_currImg, m_prevImg, m_diff);
-
     cvThreshold(m_diff, m_diff, 15, 255, CV_THRESH_BINARY);
 
     cvSetZero(m_filtered);
     OpenCvUtility::MotionFilter(m_diff, m_filtered, 24, 24);
 
     cvConvertScale(m_filtered, m_avg, 1, 0.0);
-
+    // Search for target in the mask region using cross-correlation
     cvThreshold(m_avg, m_avg, 200, 255, CV_THRESH_BINARY);
+    // Look for target in search zone.
     TargetSearch( m_avg );
 
     IplImage* colImg = cvCreateImage( cvSize( m_currImg->width, m_currImg->height ), IPL_DEPTH_8U, 3 );
     cvCvtColor( m_currImg, colImg, CV_GRAY2RGB );
 
     cvSetImageCOI( colImg, 1 );
     cvCopy( m_avg, colImg );
-
     cvReleaseImage(&colImg);
 }
 
@@ -752,30 +789,39 @@ void KltTracker::TargetSearch( const IplImage* mask )
 
     float maxVal = -1.f;
     CvPoint2D32f maxPos;
-
-    for ( int j = r; j < h - r; ++j )
+
+    //Correct m_pos when lost near the limits of the window due to offsets.
+    //Otherwise it get in a state where ncc2 returns 0 and lossRecovery never recovers
+    float x_lost = std::min( std::max(float(ws/2), m_pos.x) , float(m_appearanceImg->width-ws/2));
+    float y_lost = std::min( std::max(float(ws/2), m_pos.y) , float(m_appearanceImg->height-ws/2));
+
+    PredictTargetAppearance2(0,0,x_lost, y_lost);
+    for ( int j = r; j < h - r; j = j+2 )
     {
-        for ( int i = r; i < w - r; ++i )
+        for ( int i = r; i < w - r; i = i + 2 )
         {
             char* pMask = mask->imageData;
             float* pNcc = reinterpret_cast< float* > ( ncc->imageData );
 
             pMask += j * iStep + i;
             pNcc += j * fStep + i;
-
-            if ( *pMask )
-            {
-                //ComputeHeading( cvPoint2D32f(i,j) );
-                float val = CrossCorrelation::Ncc2d/*Radial*/( m_appearanceImg, m_currImg, m_pos.x, m_pos.y, i, j, ws, ws );
-
-                *pNcc = val;
-                if ( val > maxVal )
-                {
-                    maxVal = val;
-                    maxPos.x = i;
-                    maxPos.y = j;
-                }
-            }
+	    if ( *pMask )
+	    { 
+	      //Compute the heading of the robot for a given candidate position 
+	      // and creates a patch taking into account the orientation
+	      float newAngle = ComputeHeading(cvPoint2D32f( (float)i, (float)j ));
+	      PredictTargetAppearance2(newAngle,0,x_lost, y_lost);
+	      //Compare candidate with target appearance
+	      float val = CrossCorrelation::Ncc2dRadial( m_appearanceImg, m_currImg, x_lost, y_lost, i, j, ws, ws );
+	      *pNcc = val;
+	      if ( val > maxVal )
+	      {
+		maxVal = val;
+		maxPos.x = i;
+		maxPos.y = j;
+		m_angle = newAngle;
+	       }
+	    }
         }
     }
 

app/src/tracking/KltTracker.h

@@ -177,6 +177,7 @@ class KltTracker: public RobotTracker
     void SwapPyramids();
 
     void PredictTargetAppearance( float angleInRadians, float offsetAngleDegrees );
+    void PredictTargetAppearance2( float angleInRadians, float offsetAngleDegrees, float x, float y );
     bool TrackStage2( CvPoint2D32f initialPosition, bool flipCorrect, bool init );
 
     void InitialiseRecoverySystem();

app/src/tracking/TrackThread.cpp

@@ -76,11 +76,9 @@ void TrackThread::Execute()
     while ( !ShouldStop() )
     {
         const GtsScene::TrackStatus status = m_scene.StepTrackers( Forwards(), Seeking() );
-
-        bool trackingLost = ( status.numTrackersActive ==
-                              status.numTrackersLost );
-
-        emit position( status.videoPosition );
+
+        bool trackingLost = ( status.numTrackersActive == status.numTrackersLost && status.numTrackersActive==0  );
+       emit position( status.videoPosition );
 
         if ( ShouldPause() || (ShouldTrack() && trackingLost) )
         {