featureTracking.cpp
Go to the documentation of this file.
00001 #include <math.h>
00002 #include <stdio.h>
00003 #include <stdlib.h>
00004 #include <ros/ros.h>
00005 
00006 #include "cameraParameters.h"
00007 #include "pointDefinition.h"
00008 
00009 using namespace std;
00010 using namespace cv;
00011 
00012 bool systemInited = false;
00013 double timeCur, timeLast;
00014 
00015 const int imagePixelNum = imageHeight * imageWidth;
00016 CvSize imgSize = cvSize(imageWidth, imageHeight);
00017 
00018 IplImage *imageCur = cvCreateImage(imgSize, IPL_DEPTH_8U, 1);
00019 IplImage *imageLast = cvCreateImage(imgSize, IPL_DEPTH_8U, 1);
00020 
00021 int showCount = 0;
00022 const int showSkipNum = 2;
00023 const int showDSRate = 2;
00024 CvSize showSize = cvSize(imageWidth / showDSRate, imageHeight / showDSRate);
00025 
00026 IplImage *imageShow = cvCreateImage(showSize, IPL_DEPTH_8U, 1);
00027 IplImage *harrisLast = cvCreateImage(showSize, IPL_DEPTH_32F, 1);
00028 
00029 CvMat kMat = cvMat(3, 3, CV_64FC1, kImage);
00030 
00031 const int maxFeatureNumPerSubregion = 2;
00032 const int xSubregionNum = 12;
00033 const int ySubregionNum = 8;
00034 const int totalSubregionNum = xSubregionNum * ySubregionNum;
00035 const int MAXFEATURENUM = maxFeatureNumPerSubregion * totalSubregionNum;
00036 
00037 const int xBoundary = 20;
00038 const int yBoundary = 20;
00039 const double subregionWidth = (double)(imageWidth - 2 * xBoundary) / (double)xSubregionNum;
00040 const double subregionHeight = (double)(imageHeight - 2 * yBoundary) / (double)ySubregionNum;
00041 
00042 const double maxTrackDis = 100;
00043 const int winSize = 15;
00044 
00045 IplImage *imageEig, *imageTmp, *pyrCur, *pyrLast;
00046 
00047 CvPoint2D32f *featuresCur = new CvPoint2D32f[2 * MAXFEATURENUM];
00048 CvPoint2D32f *featuresLast = new CvPoint2D32f[2 * MAXFEATURENUM];
00049 char featuresFound[2 * MAXFEATURENUM];
00050 float featuresError[2 * MAXFEATURENUM];
00051 
00052 int featuresIndFromStart = 0;
00053 int featuresInd[2 * MAXFEATURENUM] = {0};
00054 
00055 int totalFeatureNum = 0;
00056 int subregionFeatureNum[2 * totalSubregionNum] = {0};
00057 
00058 pcl::PointCloud<ImagePoint>::Ptr imagePointsCur(new pcl::PointCloud<ImagePoint>());
00059 pcl::PointCloud<ImagePoint>::Ptr imagePointsLast(new pcl::PointCloud<ImagePoint>());
00060 
00061 ros::Publisher *imagePointsLastPubPointer;
00062 ros::Publisher *imageShowPubPointer;
00063 cv_bridge::CvImage bridge;
00064 
00065 void imageDataHandler(const sensor_msgs::Image::ConstPtr& imageData) 
00066 {
00067   timeLast = timeCur;
00068   timeCur = imageData->header.stamp.toSec();
00069 
00070   IplImage *imageTemp = imageLast;
00071   imageLast = imageCur;
00072   imageCur = imageTemp;
00073 
00074   for (int i = 0; i < imagePixelNum; i++) {
00075     imageCur->imageData[i] = (char)imageData->data[i];
00076   }
00077 
00078   //cvEqualizeHist(imageCur, imageCur);
00079 
00080   cvResize(imageLast, imageShow);
00081   cvCornerHarris(imageShow, harrisLast, 3);
00082 
00083   CvPoint2D32f *featuresTemp = featuresLast;
00084   featuresLast = featuresCur;
00085   featuresCur = featuresTemp;
00086 
00087   pcl::PointCloud<ImagePoint>::Ptr imagePointsTemp = imagePointsLast;
00088   imagePointsLast = imagePointsCur;
00089   imagePointsCur = imagePointsTemp;
00090   imagePointsCur->clear();
00091 
00092   if (!systemInited) {
00093     systemInited = true;
00094     return;
00095   }
00096 
00097   int recordFeatureNum = totalFeatureNum;
00098   for (int i = 0; i < ySubregionNum; i++) {
00099     for (int j = 0; j < xSubregionNum; j++) {
00100       int ind = xSubregionNum * i + j;
00101       int numToFind = maxFeatureNumPerSubregion - subregionFeatureNum[ind];
00102 
00103       if (numToFind > 0) {
00104         int subregionLeft = xBoundary + (int)(subregionWidth * j);
00105         int subregionTop = yBoundary + (int)(subregionHeight * i);
00106         CvRect subregion = cvRect(subregionLeft, subregionTop, (int)subregionWidth, (int)subregionHeight);
00107         cvSetImageROI(imageLast, subregion);
00108 
00109         cvGoodFeaturesToTrack(imageLast, imageEig, imageTmp, featuresLast + totalFeatureNum,
00110                               &numToFind, 0.1, 5.0, NULL, 3, 1, 0.04);
00111 
00112         int numFound = 0;
00113         for(int k = 0; k < numToFind; k++) {
00114           featuresLast[totalFeatureNum + k].x += subregionLeft;
00115           featuresLast[totalFeatureNum + k].y += subregionTop;
00116 
00117           int xInd = (featuresLast[totalFeatureNum + k].x + 0.5) / showDSRate;
00118           int yInd = (featuresLast[totalFeatureNum + k].y + 0.5) / showDSRate;
00119 
00120           if (((float*)(harrisLast->imageData + harrisLast->widthStep * yInd))[xInd] > 1e-6) {
00121             featuresLast[totalFeatureNum + numFound].x = featuresLast[totalFeatureNum + k].x;
00122             featuresLast[totalFeatureNum + numFound].y = featuresLast[totalFeatureNum + k].y;
00123             featuresInd[totalFeatureNum + numFound] = featuresIndFromStart;
00124 
00125             numFound++;
00126             featuresIndFromStart++;
00127           }
00128         }
00129         totalFeatureNum += numFound;
00130         subregionFeatureNum[ind] += numFound;
00131 
00132         cvResetImageROI(imageLast);
00133       }
00134     }
00135   }
00136 
00137   cvCalcOpticalFlowPyrLK(imageLast, imageCur, pyrLast, pyrCur,
00138                          featuresLast, featuresCur, totalFeatureNum, cvSize(winSize, winSize), 
00139                          3, featuresFound, featuresError, 
00140                          cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 30, 0.01), 0);
00141 
00142   for (int i = 0; i < totalSubregionNum; i++) {
00143     subregionFeatureNum[i] = 0;
00144   }
00145 
00146   ImagePoint point;
00147   int featureCount = 0;
00148   double meanShiftX = 0, meanShiftY = 0;
00149   for (int i = 0; i < totalFeatureNum; i++) {
00150     double trackDis = sqrt((featuresLast[i].x - featuresCur[i].x) 
00151                     * (featuresLast[i].x - featuresCur[i].x)
00152                     + (featuresLast[i].y - featuresCur[i].y) 
00153                     * (featuresLast[i].y - featuresCur[i].y));
00154 
00155     if (!(trackDis > maxTrackDis || featuresCur[i].x < xBoundary || 
00156       featuresCur[i].x > imageWidth - xBoundary || featuresCur[i].y < yBoundary || 
00157       featuresCur[i].y > imageHeight - yBoundary)) {
00158 
00159       int xInd = (int)((featuresLast[i].x - xBoundary) / subregionWidth);
00160       int yInd = (int)((featuresLast[i].y - yBoundary) / subregionHeight);
00161       int ind = xSubregionNum * yInd + xInd;
00162 
00163       if (subregionFeatureNum[ind] < maxFeatureNumPerSubregion) {
00164         featuresCur[featureCount].x = featuresCur[i].x;
00165         featuresCur[featureCount].y = featuresCur[i].y;
00166         featuresLast[featureCount].x = featuresLast[i].x;
00167         featuresLast[featureCount].y = featuresLast[i].y;
00168         featuresInd[featureCount] = featuresInd[i];
00169 
00170         point.u = -(featuresCur[featureCount].x - kImage[2]) / kImage[0];
00171         point.v = -(featuresCur[featureCount].y - kImage[5]) / kImage[4];
00172         point.ind = featuresInd[featureCount];
00173         imagePointsCur->push_back(point);
00174 
00175         if (i >= recordFeatureNum) {
00176           point.u = -(featuresLast[featureCount].x - kImage[2]) / kImage[0];
00177           point.v = -(featuresLast[featureCount].y - kImage[5]) / kImage[4];
00178           imagePointsLast->push_back(point);
00179         }
00180 
00181         meanShiftX += fabs((featuresCur[featureCount].x - featuresLast[featureCount].x) / kImage[0]);
00182         meanShiftY += fabs((featuresCur[featureCount].y - featuresLast[featureCount].y) / kImage[4]);
00183 
00184         featureCount++;
00185         subregionFeatureNum[ind]++;
00186       }
00187     }
00188   }
00189   totalFeatureNum = featureCount;
00190   meanShiftX /= totalFeatureNum;
00191   meanShiftY /= totalFeatureNum;
00192 
00193   sensor_msgs::PointCloud2 imagePointsLast2;
00194   pcl::toROSMsg(*imagePointsLast, imagePointsLast2);
00195   imagePointsLast2.header.stamp = ros::Time().fromSec(timeLast);
00196   imagePointsLastPubPointer->publish(imagePointsLast2);
00197 
00198   showCount = (showCount + 1) % (showSkipNum + 1);
00199   if (showCount == showSkipNum) {
00200     Mat imageShowMat(imageShow);
00201     bridge.image = imageShowMat;
00202     bridge.encoding = "mono8";
00203     sensor_msgs::Image::Ptr imageShowPointer = bridge.toImageMsg();
00204     imageShowPubPointer->publish(imageShowPointer);
00205   }
00206 }
00207 
00208 int main(int argc, char** argv)
00209 {
00210   ros::init(argc, argv, "featureTracking");
00211   ros::NodeHandle nh;
00212 
00213   CvSize subregionSize = cvSize((int)subregionWidth, (int)subregionHeight);
00214   imageEig = cvCreateImage(subregionSize, IPL_DEPTH_32F, 1);
00215   imageTmp = cvCreateImage(subregionSize, IPL_DEPTH_32F, 1);
00216 
00217   CvSize pyrSize = cvSize(imageWidth + 8, imageHeight / 3);
00218   pyrCur = cvCreateImage(pyrSize, IPL_DEPTH_32F, 1);
00219   pyrLast = cvCreateImage(pyrSize, IPL_DEPTH_32F, 1);
00220 
00221   ros::Subscriber imageDataSub = nh.subscribe<sensor_msgs::Image>
00222                                  ("/camera/rgb/image_rect", 1, imageDataHandler);
00223 
00224   ros::Publisher imagePointsLastPub = nh.advertise<sensor_msgs::PointCloud2> ("/image_points_last", 5);
00225   imagePointsLastPubPointer = &imagePointsLastPub;
00226 
00227   ros::Publisher imageShowPub = nh.advertise<sensor_msgs::Image>("/image/show", 1);
00228   imageShowPubPointer = &imageShowPub;
00229 
00230   ros::spin();
00231 
00232   return 0;
00233 }


demo_rgbd
Author(s): Ji Zhang
autogenerated on Sun Oct 5 2014 23:25:11