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


demo_lidar
Author(s): Ji Zhang
autogenerated on Sun Oct 5 2014 23:22:43