featureTracking_ocl.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 "opencv2/ocl/ocl.hpp"
00008 #include "pointDefinition.h"
00009 
00010 using namespace std;
00011 using namespace cv;
00012 using namespace cv::ocl;
00013 
00014 bool systemInited = false;
00015 bool isOddFrame = true;
00016 
00017 double timeCur, timeLast;
00018 
00019 int detectionCount = 0;
00020 const int detectionSkipNum = 3;
00021 
00022 int showCount = 0;
00023 const int showSkipNum = 2;
00024 const int showDSRate = 2;
00025 Size showSize = Size(imageWidth / showDSRate, imageHeight / showDSRate);
00026 
00027 Mat image0, image1;
00028 oclMat oclImage0, oclImage1;
00029 Mat imageShow, harrisLast;
00030 oclMat oclImageShow, oclHarrisLast;
00031 
00032 Mat kMat = Mat(3, 3, CV_64FC1, kImage);
00033 Mat dMat = Mat(4, 1, CV_64FC1, dImage);
00034 
00035 Mat mapx, mapy;
00036 
00037 const int maxFeatureNumPerSubregion = 20;
00038 const int xSubregionNum = 6;
00039 const int ySubregionNum = 4;
00040 const int totalSubregionNum = xSubregionNum * ySubregionNum;
00041 const int MAXFEATURENUM = maxFeatureNumPerSubregion * totalSubregionNum;
00042 
00043 const int xBoundary = 20;
00044 const int yBoundary = 20;
00045 const double subregionWidth = (double)(imageWidth - 2 * xBoundary) / (double)xSubregionNum;
00046 const double subregionHeight = (double)(imageHeight - 2 * yBoundary) / (double)ySubregionNum;
00047 
00048 const double maxTrackDis = 100;
00049 const int winSize = 21;
00050 
00051 GoodFeaturesToTrackDetector_OCL oclFeatureDetector;
00052 PyrLKOpticalFlow oclOpticalFlow;
00053 
00054 vector<Point2f> *featuresCur = new vector<Point2f>();
00055 vector<Point2f> *featuresLast = new vector<Point2f>();
00056 vector<Point2f> featuresSub;
00057 vector<unsigned char> featuresStatus;
00058 
00059 int featuresIndFromStart = 0;
00060 vector<int> featuresInd;
00061 
00062 int totalFeatureNum = 0;
00063 int subregionFeatureNum[2 * totalSubregionNum] = {0};
00064 
00065 pcl::PointCloud<ImagePoint>::Ptr imagePointsCur(new pcl::PointCloud<ImagePoint>());
00066 pcl::PointCloud<ImagePoint>::Ptr imagePointsLast(new pcl::PointCloud<ImagePoint>());
00067 
00068 ros::Publisher *imagePointsLastPubPointer;
00069 ros::Publisher *imageShowPubPointer;
00070 cv_bridge::CvImage bridge;
00071 
00072 static void download(const oclMat& ocl_mat, vector<Point2f>& vec)
00073 {
00074   vec.resize(ocl_mat.cols);
00075   Mat mat(1, ocl_mat.cols, CV_32FC2, (void*)&vec[0]);
00076   ocl_mat.download(mat);
00077 }
00078 
00079 static void download(const oclMat& ocl_mat, vector<unsigned char>& vec)
00080 {
00081   vec.resize(ocl_mat.cols);
00082   Mat mat(1, ocl_mat.cols, CV_8UC1, (void*)&vec[0]);
00083   ocl_mat.download(mat);
00084 }
00085 
00086 void imageDataHandler(const sensor_msgs::Image::ConstPtr& imageData) 
00087 {
00088   timeLast = timeCur;
00089   timeCur = imageData->header.stamp.toSec() - 0.1163;
00090 
00091   cv_bridge::CvImageConstPtr imageDataCv = cv_bridge::toCvShare(imageData, "mono8");
00092 
00093   if (!systemInited) {
00094     remap(imageDataCv->image, image0, mapx, mapy, CV_INTER_LINEAR);
00095     oclImage0 = oclMat(image0);
00096     systemInited = true;
00097 
00098     return;
00099   }
00100 
00101   Mat imageLast, imageCur;
00102   oclMat oclImageLast, oclImageCur;
00103   if (isOddFrame) {
00104     remap(imageDataCv->image, image1, mapx, mapy, CV_INTER_LINEAR);
00105     oclImage1 = oclMat(image1);
00106 
00107     imageLast = image0;
00108     imageCur = image1;
00109 
00110     oclImageLast = oclImage0;
00111     oclImageCur = oclImage1;
00112   } else {
00113     remap(imageDataCv->image, image0, mapx, mapy, CV_INTER_LINEAR);
00114     oclImage0 = oclMat(image0);
00115 
00116     imageLast = image1;
00117     imageCur = image0;
00118 
00119     oclImageLast = oclImage1;
00120     oclImageCur = oclImage0;
00121   }
00122   isOddFrame = !isOddFrame;
00123 
00124   resize(oclImageLast, oclImageShow, showSize);
00125   oclImageShow.download(imageShow);
00126   cornerHarris(oclImageShow, oclHarrisLast, 2, 3, 0.04);
00127   oclHarrisLast.download(harrisLast);
00128 
00129   vector<Point2f> *featuresTemp = featuresLast;
00130   featuresLast = featuresCur;
00131   featuresCur = featuresTemp;
00132 
00133   pcl::PointCloud<ImagePoint>::Ptr imagePointsTemp = imagePointsLast;
00134   imagePointsLast = imagePointsCur;
00135   imagePointsCur = imagePointsTemp;
00136   imagePointsCur->clear();
00137 
00138   int recordFeatureNum = totalFeatureNum;
00139   detectionCount = (detectionCount + 1) % (detectionSkipNum + 1);
00140   if (detectionCount == detectionSkipNum) {
00141     oclMat oclFeaturesSub;
00142     for (int i = 0; i < ySubregionNum; i++) {
00143       for (int j = 0; j < xSubregionNum; j++) {
00144         int ind = xSubregionNum * i + j;
00145         int numToFind = maxFeatureNumPerSubregion - subregionFeatureNum[ind];
00146 
00147         if (numToFind > maxFeatureNumPerSubregion / 5.0) {
00148           int subregionLeft = xBoundary + (int)(subregionWidth * j);
00149           int subregionTop = yBoundary + (int)(subregionHeight * i);
00150           Rect subregion = Rect(subregionLeft, subregionTop, (int)subregionWidth, (int)subregionHeight);
00151 
00152           oclFeatureDetector.maxCorners = numToFind;
00153           oclFeatureDetector(oclImageLast(subregion), oclFeaturesSub);
00154           if (oclFeaturesSub.cols > 0) {
00155             oclFeatureDetector.downloadPoints(oclFeaturesSub, featuresSub);
00156             numToFind = featuresSub.size();
00157           } else {
00158             numToFind = 0;
00159           }
00160 
00161           int numFound = 0;
00162           for(int k = 0; k < numToFind; k++) {
00163             featuresSub[k].x += subregionLeft;
00164             featuresSub[k].y += subregionTop;
00165 
00166             int xInd = (featuresSub[k].x + 0.5) / showDSRate;
00167             int yInd = (featuresSub[k].y + 0.5) / showDSRate;
00168 
00169             if (harrisLast.at<float>(yInd, xInd) > 1e-7) {
00170               featuresLast->push_back(featuresSub[k]);
00171               featuresInd.push_back(featuresIndFromStart);
00172 
00173               numFound++;
00174               featuresIndFromStart++;
00175             }
00176           }
00177           totalFeatureNum += numFound;
00178           subregionFeatureNum[ind] += numFound;
00179         }
00180       }
00181     }
00182   }
00183 
00184   if (totalFeatureNum > 0) {
00185     Mat featuresLastMat(1, totalFeatureNum, CV_32FC2, (void*)&(*featuresLast)[0]);
00186     oclMat oclFeaturesLast(featuresLastMat);
00187     oclMat oclFeaturesCur, oclFeaturesStatus;
00188 
00189     oclOpticalFlow.sparse(oclImageLast, oclImageCur, oclFeaturesLast, oclFeaturesCur, oclFeaturesStatus);
00190     if (oclFeaturesCur.cols > 0 && oclFeaturesCur.cols == oclFeaturesStatus.cols) {
00191       download(oclFeaturesCur, *featuresCur);
00192       download(oclFeaturesStatus, featuresStatus);
00193       totalFeatureNum = featuresCur->size();
00194     } else {
00195       totalFeatureNum = 0;
00196     }
00197   }
00198 
00199   for (int i = 0; i < totalSubregionNum; i++) {
00200     subregionFeatureNum[i] = 0;
00201   }
00202 
00203   ImagePoint point;
00204   int featureCount = 0;
00205   for (int i = 0; i < totalFeatureNum; i++) {
00206     double trackDis = sqrt(((*featuresLast)[i].x - (*featuresCur)[i].x) 
00207                     * ((*featuresLast)[i].x - (*featuresCur)[i].x)
00208                     + ((*featuresLast)[i].y - (*featuresCur)[i].y) 
00209                     * ((*featuresLast)[i].y - (*featuresCur)[i].y));
00210 
00211     if (!(trackDis > maxTrackDis || (*featuresCur)[i].x < xBoundary || 
00212       (*featuresCur)[i].x > imageWidth - xBoundary || (*featuresCur)[i].y < yBoundary || 
00213       (*featuresCur)[i].y > imageHeight - yBoundary) && featuresStatus[i]) {
00214 
00215       int xInd = (int)(((*featuresLast)[i].x - xBoundary) / subregionWidth);
00216       int yInd = (int)(((*featuresLast)[i].y - yBoundary) / subregionHeight);
00217       int ind = xSubregionNum * yInd + xInd;
00218 
00219       if (subregionFeatureNum[ind] < maxFeatureNumPerSubregion) {
00220         (*featuresCur)[featureCount] = (*featuresCur)[i];
00221         (*featuresLast)[featureCount] = (*featuresLast)[i];
00222         featuresInd[featureCount] = featuresInd[i];
00223 
00224         point.u = -((*featuresCur)[featureCount].x - kImage[2]) / kImage[0];
00225         point.v = -((*featuresCur)[featureCount].y - kImage[5]) / kImage[4];
00226         point.ind = featuresInd[featureCount];
00227         imagePointsCur->push_back(point);
00228 
00229         if (i >= recordFeatureNum) {
00230           point.u = -((*featuresLast)[featureCount].x - kImage[2]) / kImage[0];
00231           point.v = -((*featuresLast)[featureCount].y - kImage[5]) / kImage[4];
00232           imagePointsLast->push_back(point);
00233         }
00234 
00235         featureCount++;
00236         subregionFeatureNum[ind]++;
00237       }
00238     }
00239   }
00240   totalFeatureNum = featureCount;
00241   featuresCur->resize(totalFeatureNum);
00242   featuresLast->resize(totalFeatureNum);
00243   featuresInd.resize(totalFeatureNum);
00244 
00245   sensor_msgs::PointCloud2 imagePointsLast2;
00246   pcl::toROSMsg(*imagePointsLast, imagePointsLast2);
00247   imagePointsLast2.header.stamp = ros::Time().fromSec(timeLast);
00248   imagePointsLastPubPointer->publish(imagePointsLast2);
00249 
00250   showCount = (showCount + 1) % (showSkipNum + 1);
00251   if (showCount == showSkipNum) {
00252     bridge.image = imageShow;
00253     bridge.encoding = "mono8";
00254     sensor_msgs::Image::Ptr imageShowPointer = bridge.toImageMsg();
00255     imageShowPubPointer->publish(imageShowPointer);
00256   }
00257 }
00258 
00259 int main(int argc, char* argv[])
00260 {
00261   ros::init(argc, argv, "featureTracking_ocl");
00262   ros::NodeHandle nh;
00263 
00264   Size imageSize = Size(imageWidth, imageHeight);
00265   mapx.create(imageSize, CV_32FC1);
00266   mapy.create(imageSize, CV_32FC1);
00267   initUndistortRectifyMap(kMat, dMat, Mat(), kMat, imageSize, CV_32FC1, mapx, mapy);
00268 
00269   oclOpticalFlow.winSize = Size(winSize, winSize);
00270 
00271   ros::Subscriber imageDataSub = nh.subscribe<sensor_msgs::Image>("/image/raw", 1, imageDataHandler);
00272 
00273   ros::Publisher imagePointsLastPub = nh.advertise<sensor_msgs::PointCloud2> ("/image_points_last", 5);
00274   imagePointsLastPubPointer = &imagePointsLastPub;
00275 
00276   ros::Publisher imageShowPub = nh.advertise<sensor_msgs::Image>("/image/show", 1);
00277   imageShowPubPointer = &imageShowPub;
00278 
00279   ros::spin();
00280 
00281   return 0;
00282 }


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