From 781df934e4b004878c14b68ee1d84ca9498927a0 Mon Sep 17 00:00:00 2001
From: Ji Zhang <zhangji@cmu.edu>
Date: Mon, 10 Mar 2014 21:47:44 -0400
Subject: [PATCH] Recommit

---
 src/featureTracking.cpp | 17 +++++------------
 1 file changed, 5 insertions(+), 12 deletions(-)

diff --git a/src/featureTracking.cpp b/src/featureTracking.cpp
index 2dacdc3..a77012a 100644
--- a/src/featureTracking.cpp
+++ b/src/featureTracking.cpp
@@ -17,7 +17,6 @@ CvSize imgSize = cvSize(imageWidth, imageHeight);
 
 IplImage *imageCur = cvCreateImage(imgSize, IPL_DEPTH_8U, 1);
 IplImage *imageLast = cvCreateImage(imgSize, IPL_DEPTH_8U, 1);
-IplImage *harrisLast = cvCreateImage(imgSize, IPL_DEPTH_32F, 1);
 
 int showCount = 0;
 const int showSkipNum = 2;
@@ -25,6 +24,7 @@ const int showDSRate = 2;
 CvSize showSize = cvSize(imageWidth / showDSRate, imageHeight / showDSRate);
 
 IplImage *imageShow = cvCreateImage(showSize, IPL_DEPTH_8U, 1);
+IplImage *harrisLast = cvCreateImage(showSize, IPL_DEPTH_32F, 1);
 
 CvMat kMat = cvMat(3, 3, CV_64FC1, kImage);
 CvMat dMat = cvMat(4, 1, CV_64FC1, dImage);
@@ -83,7 +83,8 @@ void imageDataHandler(const sensor_msgs::Image::ConstPtr& imageData)
   //cvEqualizeHist(imageCur, imageCur);
   cvReleaseImage(&t);
 
-  cvCornerHarris(imageLast, harrisLast, 7);
+  cvResize(imageLast, imageShow);
+  cvCornerHarris(imageShow, harrisLast, 3);
 
   CvPoint2D32f *featuresTemp = featuresLast;
   featuresLast = featuresCur;
@@ -119,14 +120,8 @@ void imageDataHandler(const sensor_msgs::Image::ConstPtr& imageData)
           featuresLast[totalFeatureNum + k].x += subregionLeft;
           featuresLast[totalFeatureNum + k].y += subregionTop;
 
-          int xInd = (int)featuresLast[totalFeatureNum + k].x;
-          if (featuresLast[totalFeatureNum + k].x - xInd > 0.5) {
-            xInd++;
-          }
-          int yInd = (int)featuresLast[totalFeatureNum + k].y;
-          if (featuresLast[totalFeatureNum + k].y - yInd > 0.5) {
-            yInd++;
-          }
+          int xInd = (featuresLast[totalFeatureNum + k].x + 0.5) / showDSRate;
+          int yInd = (featuresLast[totalFeatureNum + k].y + 0.5) / showDSRate;
 
           if (((float*)(harrisLast->imageData + harrisLast->widthStep * yInd))[xInd] > 1e-7) {
             featuresLast[totalFeatureNum + numFound].x = featuresLast[totalFeatureNum + k].x;
@@ -208,8 +203,6 @@ void imageDataHandler(const sensor_msgs::Image::ConstPtr& imageData)
 
   showCount = (showCount + 1) % (showSkipNum + 1);
   if (showCount == showSkipNum) {
-    cvResize(imageLast, imageShow);
-
     Mat imageShowMat(imageShow);
     bridge.image = imageShowMat;
     bridge.encoding = "mono8";