转载请注明出处：本文转自zhch_pan的博客http://www.cnblogs.com/zhchp-blog/
本博客为本人之前做项目时做的源码阅读工作，po到网上希望帮助其他人更好的理解V-LOAM的工程实现，有些地方代码做了修改，可能和原工程有出入，但对于该工程的整体流程理解没有妨碍。
源码下载链接：https://github.com/Jinqiang/demo_lidar
节点名称：featureTracking
订阅topic：<sensor_msgs::Image>（"/camera/image_raw"）
发布topic：1、<sensor_msgs::PointCloud2> ("/image_points_last"）
2、<sensor_msgs::Image>("/image/show")
1 #include <math.h> 2 #include <stdio.h> 3 #include <stdlib.h> 4 #include <ros/ros.h> 5 6 #include "cameraParameters.h" 7 #include "pointDefinition.h" 8 9 using namespace std; 10 using namespace cv; 11 12 bool systemInited = false; 13 double timeCur, timeLast; 14 15 const int imagePixelNum = imageHeight * imageWidth; 16 CvSize imgSize = cvSize(imageWidth, imageHeight); 17 18 IplImage *imageCur = cvCreateImage(imgSize, IPL_DEPTH_8U, 1); 19 IplImage *imageLast = cvCreateImage(imgSize, IPL_DEPTH_8U, 1); 20 21 int showCount = 0; 22 const int showSkipNum = 2; 23 const int showDSRate = 2; 24 CvSize showSize = cvSize(imageWidth / showDSRate, imageHeight / showDSRate); 25 26 IplImage *imageShow = cvCreateImage(showSize, IPL_DEPTH_8U, 1); 27 IplImage *harrisLast = cvCreateImage(showSize, IPL_DEPTH_32F, 1); 28 29 CvMat kMat = cvMat(3, 3, CV_64FC1, kImage); 30 CvMat dMat = cvMat(4, 1, CV_64FC1, dImage); 31 32 IplImage *mapx, *mapy; 33 34 const int maxFeatureNumPerSubregion = 2; 35 const int xSubregionNum = 12; 36 const int ySubregionNum = 8; 37 const int totalSubregionNum = xSubregionNum * ySubregionNum; 38 const int MAXFEATURENUM = maxFeatureNumPerSubregion * totalSubregionNum; 39 40 const int xBoundary = 20; 41 const int yBoundary = 20; 42 const double subregionWidth = (double)(imageWidth - 2 * xBoundary) / (double)xSubregionNum; 43 const double subregionHeight = (double)(imageHeight - 2 * yBoundary) / (double)ySubregionNum; 44 45 const double maxTrackDis = 100; 46 const int winSize = 15; 47 48 IplImage *imageEig, *imageTmp, *pyrCur, *pyrLast; 49 50 CvPoint2D32f *featuresCur = new CvPoint2D32f[2 * MAXFEATURENUM]; 51 CvPoint2D32f *featuresLast = new CvPoint2D32f[2 * MAXFEATURENUM]; 52 char featuresFound[2 * MAXFEATURENUM]; 53 float featuresError[2 * MAXFEATURENUM]; 54 55 int featuresIndFromStart = 0; 56 int featuresInd[2 * MAXFEATURENUM] = {0}; 57 58 int totalFeatureNum = 0; 59 //maxFeatureNumPerSubregion=2 60 int subregionFeatureNum[2 * totalSubregionNum] = {0}; 61 62 pcl::PointCloud<ImagePoint>::Ptr imagePointsCur(new pcl::PointCloud<ImagePoint>()); 63 pcl::PointCloud<ImagePoint>::Ptr imagePointsLast(new pcl::PointCloud<ImagePoint>()); 64 65 ros::Publisher *imagePointsLastPubPointer; 66 ros::Publisher *imageShowPubPointer; 67 cv_bridge::CvImage bridge; 68 69 void imageDataHandler(const sensor_msgs::Image::ConstPtr& imageData) 70 { 71 timeLast = timeCur; 72 timeCur = imageData->header.stamp.toSec() - 0.1163; 73 74 IplImage *imageTemp = imageLast; 75 imageLast = imageCur; 76 imageCur = imageTemp; 77 78 for (int i = 0; i < imagePixelNum; i++) { 79 imageCur->imageData[i] = (char)imageData->data[i]; 80 } 81 82 IplImage *t = cvCloneImage(imageCur); 83 //去除图像畸变 84 cvRemap(t, imageCur, mapx, mapy); 85 //cvEqualizeHist(imageCur, imageCur); 86 cvReleaseImage(&t); 87 88 //缩小一点可能角点检测速度比较快 89 cvResize(imageLast, imageShow); 90 cvCornerHarris(imageShow, harrisLast, 3); 91 92 CvPoint2D32f *featuresTemp = featuresLast; 93 featuresLast = featuresCur; 94 featuresCur = featuresTemp; 95 96 pcl::PointCloud<ImagePoint>::Ptr imagePointsTemp = imagePointsLast; 97 imagePointsLast = imagePointsCur; 98 imagePointsCur = imagePointsTemp; 99 imagePointsCur->clear(); 100 101 if (!systemInited) { 102 systemInited = true; 103 return; 104 } 105 106 int recordFeatureNum = totalFeatureNum; 107 for (int i = 0; i < ySubregionNum; i++) { 108 for (int j = 0; j < xSubregionNum; j++) { 109 //ind指向当前的subregion编号 110 int ind = xSubregionNum * i + j; 111 int numToFind = maxFeatureNumPerSubregion - subregionFeatureNum[ind]; 112 113 if (numToFind > 0) { 114 int subregionLeft = xBoundary + (int)(subregionWidth * j); 115 int subregionTop = yBoundary + (int)(subregionHeight * i); 116 //将当前的subregion框选出来 117 CvRect subregion = cvRect(subregionLeft, subregionTop, (int)subregionWidth, (int)subregionHeight); 118 cvSetImageROI(imageLast, subregion); 119 //在框选出来的subregion中寻找好的特征点 120 cvGoodFeaturesToTrack(imageLast, imageEig, imageTmp, featuresLast + totalFeatureNum, 121 &numToFind, 0.1, 5.0, NULL, 3, 1, 0.04); 122 123 int numFound = 0; 124 for(int k = 0; k < numToFind; k++) { 125 featuresLast[totalFeatureNum + k].x += subregionLeft; 126 featuresLast[totalFeatureNum + k].y += subregionTop; 127 128 int xInd = (featuresLast[totalFeatureNum + k].x + 0.5) / showDSRate; 129 int yInd = (featuresLast[totalFeatureNum + k].y + 0.5) / showDSRate; 130 //查看检测的角点中是否有匹配到的合适的特征点 131 if (((float*)(harrisLast->imageData + harrisLast->widthStep * yInd))[xInd] > 1e-7) { 132 featuresLast[totalFeatureNum + numFound].x = featuresLast[totalFeatureNum + k].x; 133 featuresLast[totalFeatureNum + numFound].y = featuresLast[totalFeatureNum + k].y; 134 featuresInd[totalFeatureNum + numFound] = featuresIndFromStart; 135 136 numFound++; 137 featuresIndFromStart++; 138 } 139 } 140 totalFeatureNum += numFound; 141 subregionFeatureNum[ind] += numFound; 142 143 cvResetImageROI(imageLast); 144 } 145 } 146 } 147 148 cvCalcOpticalFlowPyrLK(imageLast, imageCur, pyrLast, pyrCur, 149 featuresLast, featuresCur, totalFeatureNum, cvSize(winSize, winSize), 150 3, featuresFound, featuresError, 151 cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 30, 0.01), 0); 152 153 for (int i = 0; i < totalSubregionNum; i++) { 154 subregionFeatureNum[i] = 0; 155 } 156 157 ImagePoint point; 158 int featureCount = 0; 159 double meanShiftX = 0, meanShiftY = 0; 160 for (int i = 0; i < totalFeatureNum; i++) { 161 double trackDis = sqrt((featuresLast[i].x - featuresCur[i].x) 162 * (featuresLast[i].x - featuresCur[i].x) 163 + (featuresLast[i].y - featuresCur[i].y) 164 * (featuresLast[i].y - featuresCur[i].y)); 165 166 if (!(trackDis > maxTrackDis || featuresCur[i].x < xBoundary || 167 featuresCur[i].x > imageWidth - xBoundary || featuresCur[i].y < yBoundary || 168 featuresCur[i].y > imageHeight - yBoundary)) { 169 //计算当前特征点是哪个subregion中检测到的，ind是subregion的编号 170 int xInd = (int)((featuresLast[i].x - xBoundary) / subregionWidth); 171 int yInd = (int)((featuresLast[i].y - yBoundary) / subregionHeight); 172 int ind = xSubregionNum * yInd + xInd; 173 174 if (subregionFeatureNum[ind] < maxFeatureNumPerSubregion) { 175 //根据筛选准则将光流法匹配到的特征点进行筛选,这里featureCount是从0开始的， 176 //所以featuresCur[]和featuresLast[]只保存了邻近图像的特征点，很久之前的没有保存 177 featuresCur[featureCount].x = featuresCur[i].x; 178 featuresCur[featureCount].y = featuresCur[i].y; 179 featuresLast[featureCount].x = featuresLast[i].x; 180 featuresLast[featureCount].y = featuresLast[i].y; 181 //有些特征点被筛掉，所以这里featureCount不一定和i相等 182 featuresInd[featureCount] = featuresInd[i]; 183 /* 这一步将图像坐标系下的特征点[u,v]，变换到了相机坐标系下，即[u,v]->[X/Z,Y/Z,1],参考《14讲》式5.5 184 * 不过要注意这里加了个负号。相机坐标系默认是z轴向前，x轴向右，y轴向下，图像坐标系默认在图像的左上角， 185 * featuresCur[featureCount].x - kImage[2]先将图像坐标系从左上角还原到图像中心，然后加个负号， 186 * 即将默认相机坐标系的x轴负方向作为正方向，y轴同理。所以此时相机坐标系z轴向前，x轴向左，y轴向上 187 */ 188 point.u = -(featuresCur[featureCount].x - kImage[2]) / kImage[0]; 189 point.v = -(featuresCur[featureCount].y - kImage[5]) / kImage[4]; 190 point.ind = featuresInd[featureCount]; 191 imagePointsCur->push_back(point); 192 193 if (i >= recordFeatureNum) { 194 point.u = -(featuresLast[featureCount].x - kImage[2]) / kImage[0]; 195 point.v = -(featuresLast[featureCount].y - kImage[5]) / kImage[4]; 196 imagePointsLast->push_back(point); 197 } 198 199 meanShiftX += fabs((featuresCur[featureCount].x - featuresLast[featureCount].x) / kImage[0]); 200 meanShiftY += fabs((featuresCur[featureCount].y - featuresLast[featureCount].y) / kImage[4]); 201 202 featureCount++; 203 //subregionFeatureNum是根据当前帧与上一帧的特征点匹配数目来计数的 204 subregionFeatureNum[ind]++; 205 } 206 } 207 } 208 totalFeatureNum = featureCount; 209 meanShiftX /= totalFeatureNum; 210 meanShiftY /= totalFeatureNum; 211 212 sensor_msgs::PointCloud2 imagePointsLast2; 213 pcl::toROSMsg(*imagePointsLast, imagePointsLast2); 214 imagePointsLast2.header.stamp = ros::Time().fromSec(timeLast); 215 imagePointsLastPubPointer->publish(imagePointsLast2); 216 217 //隔两张图像才输出一副图像，如0,1不要，2输出，3,4不要，5输出 218 showCount = (showCount + 1) % (showSkipNum + 1); 219 if (showCount == showSkipNum) { 220 Mat imageShowMat(imageShow); 221 bridge.image = imageShowMat; 222 bridge.encoding = "mono8"; 223 sensor_msgs::Image::Ptr imageShowPointer = bridge.toImageMsg(); 224 imageShowPubPointer->publish(imageShowPointer); 225 } 226 } 227 228 int main(int argc, char** argv) 229 { 230 ros::init(argc, argv, "featureTracking"); 231 ros::NodeHandle nh; 232 233 mapx = cvCreateImage(imgSize, IPL_DEPTH_32F, 1); 234 mapy = cvCreateImage(imgSize, IPL_DEPTH_32F, 1); 235 cvInitUndistortMap(&kMat, &dMat, mapx, mapy); 236 237 CvSize subregionSize = cvSize((int)subregionWidth, (int)subregionHeight); 238 imageEig = cvCreateImage(subregionSize, IPL_DEPTH_32F, 1); 239 imageTmp = cvCreateImage(subregionSize, IPL_DEPTH_32F, 1); 240 241 CvSize pyrSize = cvSize(imageWidth + 8, imageHeight / 3); 242 pyrCur = cvCreateImage(pyrSize, IPL_DEPTH_32F, 1); 243 pyrLast = cvCreateImage(pyrSize, IPL_DEPTH_32F, 1); 244 245 ros::Subscriber imageDataSub = nh.subscribe<sensor_msgs::Image>("/camera/image_raw", 1, imageDataHandler); 246 247 ros::Publisher imagePointsLastPub = nh.advertise<sensor_msgs::PointCloud2> ("/image_points_last", 5); 248 imagePointsLastPubPointer = &imagePointsLastPub; 249 250 ros::Publisher imageShowPub = nh.advertise<sensor_msgs::Image>("/image/show", 1); 251 imageShowPubPointer = &imageShowPub; 252 253 ros::spin(); 254 255 return 0; 256 }