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


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