V-LOAM源码解析(二)

转载请注明出处:本文转自zhch_pan的博客http://www.cnblogs.com/zhchp-blog/

 

本博客为本人之前做项目时做的源码阅读工作,po到网上希望帮助其他人更好的理解V-LOAM的工程实现,有些地方代码做了修改,可能和原工程有出入,但对于该工程的整体流程理解没有妨碍。

源码下载链接:https://github.com/Jinqiang/demo_lidar

 


 

节点:visualOdometry

功能:订阅"/image_points_last""/depth_cloud"消息,将图像特征与三维点云匹配,获得图像特征点深度值,然后采用非线性优化的方法迭代收敛得到两帧图像之间的变换矩阵,即融合了三维点云信息的视觉里程计。

  1 #include <math.h>
  2 #include <stdio.h>
  3 #include <stdlib.h>
  4 #include <ros/ros.h>
  5 
  6 #include <nav_msgs/Odometry.h>
  7 #include <sensor_msgs/Imu.h>
  8 
  9 #include <tf/transform_datatypes.h>
 10 #include <tf/transform_listener.h>
 11 #include <tf/transform_broadcaster.h>
 12 
 13 #include "cameraParameters.h"
 14 #include "pointDefinition.h"
 15 
 16 const double PI = 3.1415926;
 17 
 18 pcl::PointCloud<ImagePoint>::Ptr imagePointsCur(new pcl::PointCloud<ImagePoint>());
 19 pcl::PointCloud<ImagePoint>::Ptr imagePointsLast(new pcl::PointCloud<ImagePoint>());
 20 pcl::PointCloud<ImagePoint>::Ptr startPointsCur(new pcl::PointCloud<ImagePoint>());
 21 pcl::PointCloud<ImagePoint>::Ptr startPointsLast(new pcl::PointCloud<ImagePoint>());
 22 pcl::PointCloud<pcl::PointXYZHSV>::Ptr startTransCur(new pcl::PointCloud<pcl::PointXYZHSV>());
 23 pcl::PointCloud<pcl::PointXYZHSV>::Ptr startTransLast(new pcl::PointCloud<pcl::PointXYZHSV>());
 24 pcl::PointCloud<pcl::PointXYZHSV>::Ptr ipRelations(new pcl::PointCloud<pcl::PointXYZHSV>());
 25 pcl::PointCloud<pcl::PointXYZHSV>::Ptr ipRelations2(new pcl::PointCloud<pcl::PointXYZHSV>());
 26 pcl::PointCloud<pcl::PointXYZ>::Ptr imagePointsProj(new pcl::PointCloud<pcl::PointXYZ>());
 27 pcl::PointCloud<DepthPoint>::Ptr depthPointsCur(new pcl::PointCloud<DepthPoint>());
 28 pcl::PointCloud<DepthPoint>::Ptr depthPointsLast(new pcl::PointCloud<DepthPoint>());
 29 pcl::PointCloud<DepthPoint>::Ptr depthPointsSend(new pcl::PointCloud<DepthPoint>());
 30 
 31 std::vector<int> ipInd;
 32 std::vector<float> ipy2;
 33 
 34 std::vector<float>* ipDepthCur = new std::vector<float>();
 35 std::vector<float>* ipDepthLast = new std::vector<float>();
 36 
 37 double imagePointsCurTime;
 38 double imagePointsLastTime;
 39 
 40 int imagePointsCurNum = 0;
 41 int imagePointsLastNum = 0;
 42 
 43 int depthPointsCurNum = 0;
 44 int depthPointsLastNum = 0;
 45 
 46 pcl::PointCloud<pcl::PointXYZI>::Ptr depthCloud(new pcl::PointCloud<pcl::PointXYZI>());
 47 pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdTree(new pcl::KdTreeFLANN<pcl::PointXYZI>());
 48 
 49 double depthCloudTime;
 50 int depthCloudNum = 0;
 51 
 52 std::vector<int> pointSearchInd;
 53 std::vector<float> pointSearchSqrDis;
 54 
 55 double transformSum[6] = {0};
 56 double angleSum[3] = {0};
 57 
 58 int imuPointerFront = 0;
 59 int imuPointerLast = -1;
 60 const int imuQueLength = 200;
 61 bool imuInited = false;
 62 
 63 double imuRollCur = 0, imuPitchCur = 0, imuYawCur = 0;
 64 double imuRollLast = 0, imuPitchLast = 0, imuYawLast = 0;
 65 
 66 double imuYawInit = 0;
 67 double imuTime[imuQueLength] = {0};
 68 double imuRoll[imuQueLength] = {0};
 69 double imuPitch[imuQueLength] = {0};
 70 double imuYaw[imuQueLength] = {0};
 71 
 72 ros::Publisher *voDataPubPointer = NULL;
 73 tf::TransformBroadcaster *tfBroadcasterPointer = NULL;
 74 ros::Publisher *depthPointsPubPointer = NULL;
 75 ros::Publisher *imagePointsProjPubPointer = NULL;
 76 ros::Publisher *imageShowPubPointer;
 77 
 78 const int showDSRate = 2;
 79 
 80 void accumulateRotation(double cx, double cy, double cz, double lx, double ly, double lz, 
 81                         double &ox, double &oy, double &oz)
 82 {
 83     /*R_wl=[ccy 0 scy;0 1 0;-scy 0 ccy]*[1 0 0;0 ccx -scx;0 scx ccx]*[ccz -scz 0;scz ccz 0;0 0 1];(表示以world为参考坐标系)
 84      *R_cl=[clz -slz 0;slz clz 0;0 0 1]*[1 0 0;0 clx -slx;0 slx clx]*[cly 0 sly;0 1 0;-sly 0 cly];(表示以current为参考坐标系)
 85      *R_wc=R_wl*(R_cl).';
 86      *最后求出来(-sin(rx))=cos(cx)*cos(cz)*sin(lx) - cos(lx)*cos(ly)*sin(cx) - cos(cx)*cos(lx)*sin(cz)*sin(ly)
 87      *而程序中是(-sin(rx))= cos(lx)*cos(cx)*sin(ly)*sin(cz) - cos(cx)*cos(cz)*sin(lx) - cos(lx)*cos(ly)*sin(cx);(程序里的srx=(-sin(rx)))
 88      *可以发现两个公式之间差了lx,ly,lz的负号,所以accumulateRotation()函数传入的是transform[0]~[2]的负值
 89      *至于为什么-sinx等于上式,可以通过看R_wl,发现第二行第三列的元素为-sinx,因此两个旋转矩阵相乘后,对应位置上的元素就对应着新的pitch角的sin 值
 90     */
 91   double srx = cos(lx)*cos(cx)*sin(ly)*sin(cz) - cos(cx)*cos(cz)*sin(lx) - cos(lx)*cos(ly)*sin(cx);
 92   ox = -asin(srx);
 93 
 94   double srycrx = sin(lx)*(cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy)) + cos(lx)*sin(ly)*(cos(cy)*cos(cz) 
 95                 + sin(cx)*sin(cy)*sin(cz)) + cos(lx)*cos(ly)*cos(cx)*sin(cy);
 96   double crycrx = cos(lx)*cos(ly)*cos(cx)*cos(cy) - cos(lx)*sin(ly)*(cos(cz)*sin(cy) 
 97                 - cos(cy)*sin(cx)*sin(cz)) - sin(lx)*(sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx));
 98   oy = atan2(srycrx / cos(ox), crycrx / cos(ox));
 99 
100   double srzcrx = sin(cx)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) + cos(cx)*sin(cz)*(cos(ly)*cos(lz) 
101                 + sin(lx)*sin(ly)*sin(lz)) + cos(lx)*cos(cx)*cos(cz)*sin(lz);
102   double crzcrx = cos(lx)*cos(lz)*cos(cx)*cos(cz) - cos(cx)*sin(cz)*(cos(ly)*sin(lz) 
103                 - cos(lz)*sin(lx)*sin(ly)) - sin(cx)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx));
104   oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox));
105 }
106 
107 void diffRotation(double cx, double cy, double cz, double lx, double ly, double lz, 
108                   double &ox, double &oy, double &oz)
109 {
110   double srx = cos(cx)*cos(cy)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx)) 
111              - cos(cx)*sin(cy)*(cos(ly)*sin(lz) - cos(lz)*sin(lx)*sin(ly)) - cos(lx)*cos(lz)*sin(cx);
112   ox = -asin(srx);
113 
114   double srycrx = cos(cx)*sin(cy)*(cos(ly)*cos(lz) + sin(lx)*sin(ly)*sin(lz)) 
115                 - cos(cx)*cos(cy)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) - cos(lx)*sin(cx)*sin(lz);
116   double crycrx = sin(cx)*sin(lx) + cos(cx)*cos(cy)*cos(lx)*cos(ly) + cos(cx)*cos(lx)*sin(cy)*sin(ly);
117   oy = atan2(srycrx / cos(ox), crycrx / cos(ox));
118 
119   double srzcrx = cos(cx)*cos(lx)*cos(lz)*sin(cz) - (cos(cz)*sin(cy) 
120                 - cos(cy)*sin(cx)*sin(cz))*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx)) 
121                 - (cos(cy)*cos(cz) + sin(cx)*sin(cy)*sin(cz))*(cos(ly)*sin(lz) - cos(lz)*sin(lx)*sin(ly));
122   double crzcrx = (sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx))*(sin(ly)*sin(lz) 
123                 + cos(ly)*cos(lz)*sin(lx)) + (cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy))*(cos(ly)*sin(lz) 
124                 - cos(lz)*sin(lx)*sin(ly)) + cos(cx)*cos(cz)*cos(lx)*cos(lz);
125   oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox));
126 }
127 
128 void imagePointsHandler(const sensor_msgs::PointCloud2ConstPtr& imagePoints2)
129 {
130   imagePointsLastTime = imagePointsCurTime;
131   imagePointsCurTime = imagePoints2->header.stamp.toSec();
132 
133   imuRollLast = imuRollCur;
134   imuPitchLast = imuPitchCur;
135   imuYawLast = imuYawCur;
136   //transform用于记录帧与帧之间的转移矩阵,transformSum记录当前帧与初始帧的转移矩阵,
137   double transform[6] = {0};
138   if (imuPointerLast >= 0) {
139       //将该帧图像到来之前的所有IMU信息提取出来
140     while (imuPointerFront != imuPointerLast) {
141       if (imagePointsCurTime < imuTime[imuPointerFront]) {
142         break;
143       }
144       imuPointerFront = (imuPointerFront + 1) % imuQueLength;
145     }
146 
147     if (imagePointsCurTime > imuTime[imuPointerFront]) {
148       imuRollCur = imuRoll[imuPointerFront];
149       imuPitchCur = imuPitch[imuPointerFront];
150       imuYawCur = imuYaw[imuPointerFront];
151     } else {
152       int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
153       double ratioFront = (imagePointsCurTime - imuTime[imuPointerBack]) 
154                         / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
155       double ratioBack = (imuTime[imuPointerFront] - imagePointsCurTime) 
156                        / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
157 
158       //通过插值得到img时刻的roll,pitch,yaw值
159       imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;
160       imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;
161       if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > PI) {
162         imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * PI) * ratioBack;
163       } else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -PI) {
164         imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * PI) * ratioBack;
165       } else {
166         imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;
167       }
168     }
169 
170     if (imuInited) {
171       //transform[0] -= imuPitchCur - imuPitchLast;
172       //transform[1] -= imuYawCur - imuYawLast;
173       //transform[2] -= imuRollCur - imuRollLast;
174     }
175   }
176 
177   pcl::PointCloud<ImagePoint>::Ptr imagePointsTemp = imagePointsLast;
178   imagePointsLast = imagePointsCur;
179   imagePointsCur = imagePointsTemp;
180 
181   imagePointsCur->clear();
182   pcl::fromROSMsg(*imagePoints2, *imagePointsCur);
183 
184   imagePointsLastNum = imagePointsCurNum;
185   imagePointsCurNum = imagePointsCur->points.size();
186 
187   pcl::PointCloud<ImagePoint>::Ptr startPointsTemp = startPointsLast;
188   startPointsLast = startPointsCur;
189   startPointsCur = startPointsTemp;
190 
191   pcl::PointCloud<pcl::PointXYZHSV>::Ptr startTransTemp = startTransLast;
192   startTransLast = startTransCur;
193   startTransCur = startTransTemp;
194 
195   std::vector<float>* ipDepthTemp = ipDepthLast;
196   ipDepthLast = ipDepthCur;
197   ipDepthCur = ipDepthTemp;
198 
199   int j = 0;
200   pcl::PointXYZI ips;
201   pcl::PointXYZHSV ipr;
202   ipRelations->clear();
203   ipInd.clear();
204 
205   //这里是以imagePointsLast为基准进行查找,有些imagePointsCur中的点不会被查询到
206   for (int i = 0; i < imagePointsLastNum; i++) {
207     bool ipFound = false;
208     //查找是否有匹配到的特征点
209     for (; j < imagePointsCurNum; j++) {
210       if (imagePointsCur->points[j].ind == imagePointsLast->points[i].ind) {
211         ipFound = true;
212       }
213       if (imagePointsCur->points[j].ind >= imagePointsLast->points[i].ind) {
214         break;
215       }
216     }
217 
218     //如果发现匹配的特征点,尝试获取深度信息
219     if (ipFound) {
220       ipr.x = imagePointsLast->points[i].u;
221       ipr.y = imagePointsLast->points[i].v;
222       ipr.z = imagePointsCur->points[j].u;
223       ipr.h = imagePointsCur->points[j].v;
224 
225       ips.x = 10 * ipr.x;
226       ips.y = 10 * ipr.y;
227       ips.z = 10;
228       
229       if (depthCloudNum > 10) {
230         kdTree->nearestKSearch(ips, 3, pointSearchInd, pointSearchSqrDis);
231 
232         double minDepth, maxDepth;
233         if (pointSearchSqrDis[0] < 0.5 && pointSearchInd.size() == 3) {
234           pcl::PointXYZI depthPoint = depthCloud->points[pointSearchInd[0]];
235           double x1 = depthPoint.x * depthPoint.intensity / 10;
236           double y1 = depthPoint.y * depthPoint.intensity / 10;
237           double z1 = depthPoint.intensity;
238           minDepth = z1;
239           maxDepth = z1;
240 
241           depthPoint = depthCloud->points[pointSearchInd[1]];
242           double x2 = depthPoint.x * depthPoint.intensity / 10;
243           double y2 = depthPoint.y * depthPoint.intensity / 10;
244           double z2 = depthPoint.intensity;
245           minDepth = (z2 < minDepth)? z2 : minDepth;
246           maxDepth = (z2 > maxDepth)? z2 : maxDepth;
247 
248           depthPoint = depthCloud->points[pointSearchInd[2]];
249           double x3 = depthPoint.x * depthPoint.intensity / 10;
250           double y3 = depthPoint.y * depthPoint.intensity / 10;
251           double z3 = depthPoint.intensity;
252           minDepth = (z3 < minDepth)? z3 : minDepth;
253           maxDepth = (z3 > maxDepth)? z3 : maxDepth;
254           //目前只知道该特征点在相机坐标系下的归一化坐标[u,v](即[X/Z,Y/Z,1]),
255           //通过计算ipr.s获得对应于该特征点的深度值,即系数Z,则Z*u和Z*v就可获得该特征点在相机坐标系下实际的X,Y,Z坐标
256           double u = ipr.x;
257           double v = ipr.y;
258           ipr.s = (x1*y2*z3 - x1*y3*z2 - x2*y1*z3 + x2*y3*z1 + x3*y1*z2 - x3*y2*z1) 
259                 / (x1*y2 - x2*y1 - x1*y3 + x3*y1 + x2*y3 - x3*y2 + u*y1*z2 - u*y2*z1
260                 - v*x1*z2 + v*x2*z1 - u*y1*z3 + u*y3*z1 + v*x1*z3 - v*x3*z1 + u*y2*z3 
261                 - u*y3*z2 - v*x2*z3 + v*x3*z2);
262           ipr.v = 1;
263 
264           if (maxDepth - minDepth > 2) {
265             ipr.s = 0;
266             ipr.v = 0;
267           } else if (ipr.s - maxDepth > 0.2) {
268             ipr.s = maxDepth;
269           } else if (ipr.s - minDepth < -0.2) {
270             ipr.s = minDepth;
271           }
272         } else {
273           ipr.s = 0;
274           ipr.v = 0;
275         }
276       } else {
277         ipr.s = 0;
278         ipr.v = 0;
279       }
280       //如果无法从点云获取深度信息,三角测量?
281       if (fabs(ipr.v) < 0.5) {
282         double disX = transformSum[3] - startTransLast->points[i].h;
283         double disY = transformSum[4] - startTransLast->points[i].s;
284         double disZ = transformSum[5] - startTransLast->points[i].v;
285         //若移动距离大于1m
286         if (sqrt(disX * disX + disY * disY + disZ * disZ) > 1) {
287 
288           double u0 = startPointsLast->points[i].u;
289           double v0 = startPointsLast->points[i].v;
290           double u1 = ipr.x;
291           double v1 = ipr.y;
292 
293           double srx0 = sin(-startTransLast->points[i].x);
294           double crx0 = cos(-startTransLast->points[i].x);
295           double sry0 = sin(-startTransLast->points[i].y);
296           double cry0 = cos(-startTransLast->points[i].y);
297           double srz0 = sin(-startTransLast->points[i].z);
298           double crz0 = cos(-startTransLast->points[i].z);
299 
300           double srx1 = sin(-transformSum[0]);
301           double crx1 = cos(-transformSum[0]);
302           double sry1 = sin(-transformSum[1]);
303           double cry1 = cos(-transformSum[1]);
304           double srz1 = sin(-transformSum[2]);
305           double crz1 = cos(-transformSum[2]);
306 
307           double tx0 = -startTransLast->points[i].h;
308           double ty0 = -startTransLast->points[i].s;
309           double tz0 = -startTransLast->points[i].v;
310 
311           double tx1 = -transformSum[3];
312           double ty1 = -transformSum[4];
313           double tz1 = -transformSum[5];
314 
315           double x1 = crz0 * u0 + srz0 * v0;
316           double y1 = -srz0 * u0 + crz0 * v0;
317           double z1 = 1;
318 
319           double x2 = x1;
320           double y2 = crx0 * y1 + srx0 * z1;
321           double z2 = -srx0 * y1 + crx0 * z1;
322 
323           double x3 = cry0 * x2 - sry0 * z2;
324           double y3 = y2;
325           double z3 = sry0 * x2 + cry0 * z2;
326 
327           double x4 = cry1 * x3 + sry1 * z3;
328           double y4 = y3;
329           double z4 = -sry1 * x3 + cry1 * z3;
330 
331           double x5 = x4;
332           double y5 = crx1 * y4 - srx1 * z4;
333           double z5 = srx1 * y4 + crx1 * z4;
334 
335           double x6 = crz1 * x5 - srz1 * y5;
336           double y6 = srz1 * x5 + crz1 * y5;
337           double z6 = z5;
338 
339           u0 = x6 / z6;
340           v0 = y6 / z6;
341 
342           x1 = cry1 * (tx1 - tx0) + sry1 * (tz1 - tz0);
343           y1 = ty1 - ty0;
344           z1 = -sry1 * (tx1 - tx0) + cry1 * (tz1 - tz0);
345 
346           x2 = x1;
347           y2 = crx1 * y1 - srx1 * z1;
348           z2 = srx1 * y1 + crx1 * z1;
349 
350           double tx = crz1 * x2 - srz1 * y2;
351           double ty = srz1 * x2 + crz1 * y2;
352           double tz = z2;
353 
354           double delta = sqrt((v0 - v1) * (v0 - v1) + (u0 - u1) * (u0 - u1))
355                        * cos(atan2(tz * v1 - ty, tz * u1 - tx) - atan2(v0 - v1, u0 - u1));
356           double depth = sqrt((tz * u0 - tx) * (tz * u0 - tx) + (tz * v0 - ty) * (tz * v0 - ty)) / delta;
357 
358           if (depth > 0.5 && depth < 100) {
359             ipr.s = depth;
360             ipr.v = 2;
361           }
362         }
363 
364         //每一个匹配上的特征点对都会被打上一个ipr.v标签,ipr.v=0代表未从点云获得深度,ipr.v=1
365         //代表从点云获得深度,ipr.v=2代表此时可以通过三角测量获得该特征点的深度值。
366         /*
367          *ipDepthLast)[i]存储的是第i个特征点的三角测量值,若该特征点从未被三角测量过,
368          *ipDepthLast)[i]=-1;若该特征点在一段时间内能一直被观测到,且没有从点云中获得
369          *深度信息,则不断通过三角测量的融合收敛该特征点的深度值;若在这个过程中某几帧中能从
370          *点云获得该特征点的深度,则使用点云的深度信息,三角测量的结果仍通过计算出的转移矩阵
371          *进行维护,仍然存储在*ipDepthLast)[i]中,一旦无法从点云获得深度,则仍使用
372          *ipDepthLast)[i]中保存的三角测量进行融合
373          */
374         if (ipr.v == 2) {
375           if ((*ipDepthLast)[i] > 0) {
376               //这一步进行的是多次三角测量的融合,低通滤波
377             ipr.s = 3 * ipr.s * (*ipDepthLast)[i] / (ipr.s + 2 * (*ipDepthLast)[i]);
378           }
379           (*ipDepthLast)[i] = ipr.s;
380         } else if ((*ipDepthLast)[i] > 0) {
381           ipr.s = (*ipDepthLast)[i];
382           ipr.v = 2;
383         }
384       }
385 
386       ipRelations->push_back(ipr);
387       ipInd.push_back(imagePointsLast->points[i].ind);
388     }
389   }
390 
391   //迭代收敛获得两帧图像间的转移矩阵
392   int iterNum = 100;
393   pcl::PointXYZHSV ipr2, ipr3, ipr4;
394   int ipRelationsNum = ipRelations->points.size();
395   int ptNumNoDepthRec = 0;
396   int ptNumWithDepthRec = 0;
397   double meanValueWithDepthRec = 100000;
398   for (int iterCount = 0; iterCount < iterNum; iterCount++) {
399     ipRelations2->clear();
400     ipy2.clear();
401     int ptNumNoDepth = 0;
402     int ptNumWithDepth = 0;
403     double meanValueNoDepth = 0;
404     double meanValueWithDepth = 0;
405     for (int i = 0; i < ipRelationsNum; i++) {
406       ipr = ipRelations->points[i];
407 
408       double u0 = ipr.x;
409       double v0 = ipr.y;
410       double u1 = ipr.z;
411       double v1 = ipr.h;
412 
413       double srx = sin(transform[0]);
414       double crx = cos(transform[0]);
415       double sry = sin(transform[1]);
416       double cry = cos(transform[1]);
417       double srz = sin(transform[2]);
418       double crz = cos(transform[2]);
419       double tx = transform[3];
420       double ty = transform[4];
421       double tz = transform[5];
422 
423       if (fabs(ipr.v) < 0.5) {
424     /*
425      * 这里R矩阵使用欧拉角roll,pitch yaw来表示的,下面六个公式是论文公式(6)在对roll,pitch,yaw和tx,ty,tz求偏导
426      *transform[0]存储的是绕x轴旋转的角度,transform[1]存绕y轴角度,transform[2]存绕z轴角度
427      *这里计算的R、T矩阵是k-1时刻旋转到k时刻的R、T矩阵,注意这个主次关系,参考坐标系是current,即k时刻坐标系
428      *从k-1旋转到k的顺序是:z轴->x轴->y轴,注意顺序
429      *R_cl=[crz -srz 0;srz crz 0;0 0 1]*[1 0 0;0 crx -srx;0 srx crx]*[cry 0 sry;0 1 0;-sry 0 cry];
430     */
431         ipr2.x = v0*(crz*srx*(tx - tz*u1) - crx*(ty*u1 - tx*v1) + srz*srx*(ty - tz*v1)) 
432                - u0*(sry*srx*(ty*u1 - tx*v1) + crz*sry*crx*(tx - tz*u1) + sry*srz*crx*(ty - tz*v1)) 
433                + cry*srx*(ty*u1 - tx*v1) + cry*crz*crx*(tx - tz*u1) + cry*srz*crx*(ty - tz*v1);
434 
435         ipr2.y = u0*((tx - tz*u1)*(srz*sry - crz*srx*cry) - (ty - tz*v1)*(crz*sry + srx*srz*cry) 
436                + crx*cry*(ty*u1 - tx*v1)) - (tx - tz*u1)*(srz*cry + crz*srx*sry) 
437                + (ty - tz*v1)*(crz*cry - srx*srz*sry) + crx*sry*(ty*u1 - tx*v1);
438 
439         ipr2.z = -u0*((tx - tz*u1)*(cry*crz - srx*sry*srz) + (ty - tz*v1)*(cry*srz + srx*sry*crz)) 
440                - (tx - tz*u1)*(sry*crz + cry*srx*srz) - (ty - tz*v1)*(sry*srz - cry*srx*crz) 
441                - v0*(crx*crz*(ty - tz*v1) - crx*srz*(tx - tz*u1));
442 
443         ipr2.h = cry*crz*srx - v0*(crx*crz - srx*v1) - u0*(cry*srz + crz*srx*sry + crx*sry*v1) 
444                - sry*srz + crx*cry*v1;
445 
446         ipr2.s = crz*sry - v0*(crx*srz + srx*u1) + u0*(cry*crz + crx*sry*u1 - srx*sry*srz) 
447                - crx*cry*u1 + cry*srx*srz;
448 
449         ipr2.v = u1*(sry*srz - cry*crz*srx) - v1*(crz*sry + cry*srx*srz) + u0*(u1*(cry*srz + crz*srx*sry) 
450                - v1*(cry*crz - srx*sry*srz)) + v0*(crx*crz*u1 + crx*srz*v1);
451     //将六个变量值代入论文(公式6)计算得到残差值
452         double y2 = (ty - tz*v1)*(crz*sry + cry*srx*srz) - (tx - tz*u1)*(sry*srz - cry*crz*srx) 
453                   - v0*(srx*(ty*u1 - tx*v1) + crx*crz*(tx - tz*u1) + crx*srz*(ty - tz*v1)) 
454                   + u0*((ty - tz*v1)*(cry*crz - srx*sry*srz) - (tx - tz*u1)*(cry*srz + crz*srx*sry) 
455                   + crx*sry*(ty*u1 - tx*v1)) - crx*cry*(ty*u1 - tx*v1);
456 
457         if (ptNumNoDepthRec < 50 || iterCount < 25 || fabs(y2) < 2 * meanValueWithDepthRec / 10000) {
458           double scale = 100;
459           ipr2.x *= scale;
460           ipr2.y *= scale;
461           ipr2.z *= scale;
462           ipr2.h *= scale;
463           ipr2.s *= scale;
464           ipr2.v *= scale;
465           y2 *= scale;
466 
467           ipRelations2->push_back(ipr2);
468           ipy2.push_back(y2);
469 
470           ptNumNoDepth++;
471         } else {
472           ipRelations->points[i].v = -1;
473         }
474       } else if (fabs(ipr.v - 1) < 0.5 || fabs(ipr.v - 2) < 0.5) {
475 
476         double d0 = ipr.s;
477 
478         ipr3.x = d0*(cry*srz*crx + cry*u1*srx) - d0*u0*(sry*srz*crx + sry*u1*srx) 
479                - d0*v0*(u1*crx - srz*srx);
480 
481         ipr3.y = d0*(crz*cry + crx*u1*sry - srx*srz*sry) - d0*u0*(crz*sry - crx*u1*cry + srx*srz*cry);
482 
483         ipr3.z = -d0*(sry*srz - cry*srx*crz) - d0*u0*(cry*srz + srx*sry*crz) - crx*d0*v0*crz;
484 
485         ipr3.h = 1;
486 
487         ipr3.s = 0;
488 
489         ipr3.v = -u1;
490 
491         double y3 = tx - tz*u1 + d0*(crz*sry - crx*cry*u1 + cry*srx*srz) - d0*v0*(crx*srz + srx*u1) 
492                   + d0*u0*(cry*crz + crx*sry*u1 - srx*sry*srz);
493 
494         ipr4.x = d0*(cry*v1*srx - cry*crz*crx) + d0*u0*(crz*sry*crx - sry*v1*srx) 
495                - d0*v0*(crz*srx + v1*crx);
496 
497         ipr4.y = d0*(srz*cry + crz*srx*sry + crx*v1*sry) + d0*u0*(crz*srx*cry - srz*sry + crx*v1*cry);
498 
499         ipr4.z = d0*(sry*crz + cry*srx*srz) + d0*u0*(cry*crz - srx*sry*srz) - crx*d0*v0*srz;
500 
501         ipr4.h = 0;
502 
503         ipr4.s = 1;
504 
505         ipr4.v = -v1;
506 
507         double y4 = ty - tz*v1 - d0*(cry*crz*srx - sry*srz + crx*cry*v1) + d0*v0*(crx*crz - srx*v1) 
508                   + d0*u0*(cry*srz + crz*srx*sry + crx*sry*v1);
509 
510         if (ptNumWithDepthRec < 50 || iterCount < 25 || 
511             sqrt(y3 * y3 + y4 * y4) < 2 * meanValueWithDepthRec) {
512           ipRelations2->push_back(ipr3);
513           ipy2.push_back(y3);
514 
515           ipRelations2->push_back(ipr4);
516           ipy2.push_back(y4);
517 
518           ptNumWithDepth++;
519           meanValueWithDepth += sqrt(y3 * y3 + y4 * y4);
520         } else {
521           ipRelations->points[i].v = -1;
522         }
523       }
524     }
525     //加 0.01 是为了防止 ptNumWithDepth 为 0
526     meanValueWithDepth /= (ptNumWithDepth + 0.01);
527     ptNumNoDepthRec = ptNumNoDepth;
528     ptNumWithDepthRec = ptNumWithDepth;
529     meanValueWithDepthRec = meanValueWithDepth;
530 
531     int ipRelations2Num = ipRelations2->points.size();
532     if (ipRelations2Num > 10) {
533       cv::Mat matA(ipRelations2Num, 6, CV_32F, cv::Scalar::all(0));
534       cv::Mat matAt(6, ipRelations2Num, CV_32F, cv::Scalar::all(0));
535       cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
536       cv::Mat matB(ipRelations2Num, 1, CV_32F, cv::Scalar::all(0));
537       cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
538       cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));
539 
540       for (int i = 0; i < ipRelations2Num; i++) {
541         ipr2 = ipRelations2->points[i];
542 
543         matA.at<float>(i, 0) = ipr2.x;
544         matA.at<float>(i, 1) = ipr2.y;
545         matA.at<float>(i, 2) = ipr2.z;
546         matA.at<float>(i, 3) = ipr2.h;
547         matA.at<float>(i, 4) = ipr2.s;
548         matA.at<float>(i, 5) = ipr2.v;
549         matB.at<float>(i, 0) = -0.2 * ipy2[i];
550       }
551       cv::transpose(matA, matAt);
552       matAtA = matAt * matA;
553       matAtB = matAt * matB;
554       //根据《14讲》式(6.21),这里用的是高斯牛顿法而不是LM算法
555       cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
556 
557       //if (fabs(matX.at<float>(0, 0)) < 0.1 && fabs(matX.at<float>(1, 0)) < 0.1 && 
558       //    fabs(matX.at<float>(2, 0)) < 0.1) {
559         transform[0] += matX.at<float>(0, 0);
560         transform[1] += matX.at<float>(1, 0);
561         transform[2] += matX.at<float>(2, 0);
562         transform[3] += matX.at<float>(3, 0);
563         transform[4] += matX.at<float>(4, 0);
564         transform[5] += matX.at<float>(5, 0);
565       //}
566 
567       float deltaR = sqrt(matX.at<float>(0, 0) * 180 / PI * matX.at<float>(0, 0) * 180 / PI
568                    + matX.at<float>(1, 0) * 180 / PI * matX.at<float>(1, 0) * 180 / PI
569                    + matX.at<float>(2, 0) * 180 / PI * matX.at<float>(2, 0) * 180 / PI);
570       float deltaT = sqrt(matX.at<float>(3, 0) * 100 * matX.at<float>(3, 0) * 100
571                    + matX.at<float>(4, 0) * 100 * matX.at<float>(4, 0) * 100
572                    + matX.at<float>(5, 0) * 100 * matX.at<float>(5, 0) * 100);
573 
574       if (deltaR < 0.00001 && deltaT < 0.00001) {
575         break;
576       }
577 
578       //ROS_INFO ("iter: %d, deltaR: %f, deltaT: %f", iterCount, deltaR, deltaT);
579     }
580   }
581 
582   if (!imuInited) {
583     imuYawInit = imuYawCur;
584     transform[0] -= imuPitchCur;
585     transform[2] -= imuRollCur;
586 
587     imuInited = true;
588   }
589 
590   //rx,ry,rz表示当前帧与初始帧的pitch,yaw,roll角度
591   double rx, ry, rz;
592   accumulateRotation(transformSum[0], transformSum[1], transformSum[2], 
593                     -transform[0], -transform[1], -transform[2], rx, ry, rz);
594 
595   if (imuPointerLast >= 0) {
596     double drx, dry, drz;
597     diffRotation(imuPitchCur, imuYawCur - imuYawInit, imuRollCur, rx, ry, rz, drx, dry, drz);
598 
599     transform[0] -= 0.1 * drx;
600     /*if (dry > PI) {
601       transform[1] -= 0.1 * (dry - 2 * PI);
602     } else if (imuYawCur - imuYawInit - ry < -PI) {
603       transform[1] -= 0.1 * (dry + 2 * PI);
604     } else {
605       transform[1] -= 0.1 * dry;
606     }*/
607     transform[2] -= 0.1 * drz;
608 
609     accumulateRotation(transformSum[0], transformSum[1], transformSum[2], 
610                       -transform[0], -transform[1], -transform[2], rx, ry, rz);
611   }
612 
613   double x1 = cos(rz) * transform[3] - sin(rz) * transform[4];
614   double y1 = sin(rz) * transform[3] + cos(rz) * transform[4];
615   double z1 = transform[5];
616 
617   double x2 = x1;
618   double y2 = cos(rx) * y1 - sin(rx) * z1;
619   double z2 = sin(rx) * y1 + cos(rx) * z1;
620 
621   //当前帧与上一帧的位移量通过rx,ry,rz的旋转,计算当前帧和初始帧的位移增量,叠加到transformSum[]中
622   //该增量计算得到的是last帧相对于current帧在世界坐标系下的位移,所以current相对于Last在世界坐标系下的位移为负值,所以是减去
623   double tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);
624   double ty = transformSum[4] - y2;
625   double tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);
626   //当前帧与初始帧的转移矩阵保存在transformSum中,transformSum中存的是将当前帧旋转到起始帧的旋转矩阵
627   transformSum[0] = rx;
628   transformSum[1] = ry;
629   transformSum[2] = rz;
630   transformSum[3] = tx;
631   transformSum[4] = ty;
632   transformSum[5] = tz;
633 
634   pcl::PointXYZHSV spc;
635   spc.x = transformSum[0];
636   spc.y = transformSum[1];
637   spc.z = transformSum[2];
638   spc.h = transformSum[3];
639   spc.s = transformSum[4];
640   spc.v = transformSum[5];
641 
642   double crx = cos(transform[0]);
643   double srx = sin(transform[0]);
644   double cry = cos(transform[1]);
645   double sry = sin(transform[1]);
646 
647   j = 0;
648   //这里是以imagePointsCur为基准进行查找,会遍历imagePointsCur中的每一个点
649   for (int i = 0; i < imagePointsCurNum; i++) {
650     bool ipFound = false;
651     for (; j < imagePointsLastNum; j++) {
652       if (imagePointsLast->points[j].ind == imagePointsCur->points[i].ind) {
653         ipFound = true;
654       }
655       if (imagePointsLast->points[j].ind >= imagePointsCur->points[i].ind) {
656         break;
657       }
658     }
659 
660     if (ipFound) {
661       /*
662         *如果在连续的多帧特征图像之间,imagePointsCur中的某个特征点能够与上一帧
663         *imagePointsLast匹配到,代表该特征点能够被连续观测到,则将该特征点第一次出现在这些连续帧
664         *的坐标以及该坐标相对于初始帧的转移矩阵保存在startPointsCur中,用作三角测量时的第一帧
665         *特征点;若该特征点一旦与前一帧匹配失败,就表示该特征点为一系列特征点帧中的一个新出现的
666         *特征点,则将该特征点当前的坐标与转移矩阵保存在startPointsCur,认为它第一次出现,并在后续帧中
667         *若能一直观测到该特征点,startPointsCur中仍保存第一次出现的坐标与转移矩阵
668        */
669       startPointsCur->push_back(startPointsLast->points[j]);
670       startTransCur->push_back(startTransLast->points[j]);
671 
672       if ((*ipDepthLast)[j] > 0) {
673       /*
674       *transform[]里存的就是T_cl,所以将Last坐标系的点按照zxy(从右往左看)的顺序旋转,再加上位移就变换到了current坐标系
675       *而R_lc就是把transform[0]~transform[2]的pitch,yaw,roll角取负值然后按照yxz(从右往左看)的顺序变换就可得到
676       *这里有一点注意的是,将transform[0]~[2]按照yxz相乘得到的R_lc和直接将R_cl取转置得到的R_lc差了三个角度的负值
677       *所以通过旋转相乘得到R_lc时transform[0]~[2]要先取负值
678       */
679         double ipz = (*ipDepthLast)[j];
680         double ipx = imagePointsLast->points[j].u * ipz;
681         double ipy = imagePointsLast->points[j].v * ipz;
682 
683         x1 = cry * ipx + sry * ipz;
684         y1 = ipy;
685         z1 = -sry * ipx + cry * ipz;
686 
687         x2 = x1;
688         y2 = crx * y1 - srx * z1;
689         z2 = srx * y1 + crx * z1;
690 
691         ipDepthCur->push_back(z2 + transform[5]);
692       } else {
693         ipDepthCur->push_back(-1);
694       }
695     } else {
696       startPointsCur->push_back(imagePointsCur->points[i]);
697       startTransCur->push_back(spc);
698       ipDepthCur->push_back(-1);
699     }
700   }
701   startPointsLast->clear();
702   startTransLast->clear();
703   ipDepthLast->clear();
704 
705   angleSum[0] -= transform[0];
706   angleSum[1] -= transform[1];
707   angleSum[2] -= transform[2];
708 
709   /* rz,rx,ry分别对应着标准右手坐标系中的roll,pitch,yaw角,通过查看createQuaternionMsgFromRollPitchYaw()的函数定义可以发现.
710    * 当pitch和yaw角给负值后,四元数中的y和z会变成负值,x和w不受影响.由四元数定义可以知道,x,y,z是指旋转轴在三个轴上的投影,w影响
711    * 旋转角度,所以由createQuaternionMsgFromRollPitchYaw()计算得到四元数后,其在一般右手坐标系中的x,y,z分量对应到该应用场景下
712    * 的坐标系中,geoQuat.x对应实际坐标系下的z轴分量,geoQuat.y对应x轴分量,geoQuat.z对应实际的y轴分量,而由于rx和ry在计算四元数
713    * 时给的是负值,所以geoQuat.y和geoQuat.z取负值,这样就等于没变
714   */
715 
716   geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rz, -rx, -ry);
717   //geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rz, rx, ry);
718 
719   nav_msgs::Odometry voData;
720   voData.header.frame_id = "/camera_init";
721   voData.child_frame_id = "/camera";
722   voData.header.stamp = imagePoints2->header.stamp;
723   voData.pose.pose.orientation.x = -geoQuat.y;
724   voData.pose.pose.orientation.y = -geoQuat.z;
725   //voData.pose.pose.orientation.x = geoQuat.y;
726   //voData.pose.pose.orientation.y = geoQuat.z;
727   voData.pose.pose.orientation.z = geoQuat.x;
728   voData.pose.pose.orientation.w = geoQuat.w;
729   voData.pose.pose.position.x = tx;
730   voData.pose.pose.position.y = ty;
731   voData.pose.pose.position.z = tz;
732   voData.twist.twist.angular.x = angleSum[0];
733   voData.twist.twist.angular.y = angleSum[1];
734   voData.twist.twist.angular.z = angleSum[2];
735   voDataPubPointer->publish(voData);
736 
737   tf::StampedTransform voTrans;
738   voTrans.frame_id_ = "/camera_init";
739   voTrans.child_frame_id_ = "/camera";
740   voTrans.stamp_ = imagePoints2->header.stamp;
741   voTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
742   //voTrans.setRotation(tf::Quaternion(geoQuat.y, geoQuat.z, geoQuat.x, geoQuat.w));
743   voTrans.setOrigin(tf::Vector3(tx, ty, tz));
744   tfBroadcasterPointer->sendTransform(voTrans);
745 
746   pcl::PointCloud<DepthPoint>::Ptr depthPointsTemp = depthPointsLast;
747   depthPointsLast = depthPointsCur;
748   depthPointsCur = depthPointsTemp;
749 
750   DepthPoint ipd;
751   depthPointsCur->clear();
752 
753   ipd.u = transformSum[0];
754   ipd.v = transformSum[1];
755   ipd.depth = transformSum[2];
756   ipd.ind = -2;
757   depthPointsCur->push_back(ipd);
758 
759   ipd.u = transformSum[3];
760   ipd.v = transformSum[4];
761   ipd.depth = transformSum[5];
762   ipd.ind = -1;
763   depthPointsCur->push_back(ipd);
764 
765   depthPointsLastNum = depthPointsCurNum;
766   depthPointsCurNum = 2;
767 
768   j = 0;
769   pcl::PointXYZ ipp;
770   depthPointsSend->clear();
771   imagePointsProj->clear();
772   for (int i = 0; i < ipRelationsNum; i++) {
773     if (fabs(ipRelations->points[i].v - 1) < 0.5 || fabs(ipRelations->points[i].v - 2) < 0.5) {
774 
775       ipd.u = ipRelations->points[i].z;
776       ipd.v = ipRelations->points[i].h;
777       //这一步是对标号为ind的特征点深度进行的一个粗略估计,后面如果该特征点可以直接从点云或者三角测量获得深度值,
778       //则这个估计值失效,如果后面不能得到该特征深度值,则仍使用该估计值
779       ////////////////////////////////////////////////////////////////////
780       /*double ipz = ipRelations->points[i].s;
781       double ipx = ipRelations->points[i].x * ipz;
782       double ipy = ipRelations->points[i].y * ipz;
783 
784       double x1 = cry * ipx + sry * ipz;
785       double y1 = ipy;
786       double z1 = -sry * ipx + cry * ipz;
787 
788       double x2 = x1;
789       double y2 = crx * y1 - srx * z1;
790       double z2 = srx * y1 + crx * z1;
791       ipd.depth = z2 + transform[5];
792       */
793       ////////////////////////////////////////////////////////////////////
794 
795       ipd.depth = ipRelations->points[i].s + transform[5];
796       ipd.label = ipRelations->points[i].v;
797       ipd.ind = ipInd[i];
798 
799       depthPointsCur->push_back(ipd);
800       depthPointsCurNum++;
801 
802       for (; j < depthPointsLastNum; j++) {
803         if (depthPointsLast->points[j].ind < ipInd[i]) {
804           depthPointsSend->push_back(depthPointsLast->points[j]);
805         } else if (depthPointsLast->points[j].ind > ipInd[i]) {
806           break;
807         }
808       }
809 
810       ipd.u = ipRelations->points[i].x;
811       ipd.v = ipRelations->points[i].y;
812       ipd.depth = ipRelations->points[i].s;
813 
814       //现在有Last帧和Cur帧,depthPointsSend中存储的是对Last帧中特征点深度的估计和确切计算到的深度
815       depthPointsSend->push_back(ipd);
816 
817       ipp.x = ipRelations->points[i].x * ipRelations->points[i].s;
818       ipp.y = ipRelations->points[i].y * ipRelations->points[i].s;
819       ipp.z = ipRelations->points[i].s;
820 
821       imagePointsProj->push_back(ipp);
822     }
823   }
824 
825   sensor_msgs::PointCloud2 depthPoints2;
826   pcl::toROSMsg(*depthPointsSend, depthPoints2);
827   depthPoints2.header.frame_id = "camera2";
828   depthPoints2.header.stamp = ros::Time().fromSec(imagePointsLastTime);
829   depthPointsPubPointer->publish(depthPoints2);
830 
831   sensor_msgs::PointCloud2 imagePointsProj2;
832   pcl::toROSMsg(*imagePointsProj, imagePointsProj2);
833   imagePointsProj2.header.frame_id = "camera2";
834   imagePointsProj2.header.stamp = ros::Time().fromSec(imagePointsLastTime);
835   imagePointsProjPubPointer->publish(imagePointsProj2);
836 }
837 
838 void depthCloudHandler(const sensor_msgs::PointCloud2ConstPtr& depthCloud2)
839 {
840   depthCloudTime = depthCloud2->header.stamp.toSec();
841 
842   depthCloud->clear();
843   pcl::fromROSMsg(*depthCloud2, *depthCloud);
844   depthCloudNum = depthCloud->points.size();
845   //将整个点云投影到焦距为单位距离=10的平面上
846   if (depthCloudNum > 10) {
847     for (int i = 0; i < depthCloudNum; i++) {
848       depthCloud->points[i].intensity = depthCloud->points[i].z;
849       depthCloud->points[i].x *= 10 / depthCloud->points[i].z;
850       depthCloud->points[i].y *= 10 / depthCloud->points[i].z;
851       depthCloud->points[i].z = 10;
852     }
853 
854     kdTree->setInputCloud(depthCloud);
855   }
856 }
857 
858 void imuDataHandler(const sensor_msgs::Imu::ConstPtr& imuData)
859 {
860   double roll, pitch, yaw;
861   tf::Quaternion orientation;
862   tf::quaternionMsgToTF(imuData->orientation, orientation);
863   tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
864 
865   imuPointerLast = (imuPointerLast + 1) % imuQueLength;
866 
867   imuTime[imuPointerLast] = imuData->header.stamp.toSec() - 0.1068;
868   imuRoll[imuPointerLast] = roll;
869   imuPitch[imuPointerLast] = pitch;
870   imuYaw[imuPointerLast] = yaw;
871 }
872 
873 void imageDataHandler(const sensor_msgs::Image::ConstPtr& imageData) 
874 {
875   cv_bridge::CvImagePtr bridge = cv_bridge::toCvCopy(imageData, "bgr8");
876 
877   int ipRelationsNum = ipRelations->points.size();
878   for (int i = 0; i < ipRelationsNum; i++) {
879     if (fabs(ipRelations->points[i].v) < 0.5) {
880         //这里运用《14讲》公式5.5将归一化坐标平面上的点变换到图像平面,这里的相减是因为在featureTrack部分将图像坐标
881         //变到归一化图像平面时加了个负号,又考虑到显示的图像是缩小一倍后的,所以要再除以showDSRate
882       cv::circle(bridge->image, cv::Point((kImage[2] - ipRelations->points[i].z * kImage[0]) / showDSRate,
883                 (kImage[5] - ipRelations->points[i].h * kImage[4]) / showDSRate), 1, CV_RGB(255, 0, 0), 2);
884     } else if (fabs(ipRelations->points[i].v - 1) < 0.5) {
885       cv::circle(bridge->image, cv::Point((kImage[2] - ipRelations->points[i].z * kImage[0]) / showDSRate,
886                 (kImage[5] - ipRelations->points[i].h * kImage[4]) / showDSRate), 1, CV_RGB(0, 255, 0), 2);
887     } else if (fabs(ipRelations->points[i].v - 2) < 0.5) {
888       cv::circle(bridge->image, cv::Point((kImage[2] - ipRelations->points[i].z * kImage[0]) / showDSRate,
889                 (kImage[5] - ipRelations->points[i].h * kImage[4]) / showDSRate), 1, CV_RGB(0, 0, 255), 2);
890     } /*else {
891       cv::circle(bridge->image, cv::Point((kImage[2] - ipRelations->points[i].z * kImage[0]) / showDSRate,
892                 (kImage[5] - ipRelations->points[i].h * kImage[4]) / showDSRate), 1, CV_RGB(0, 0, 0), 2);
893     }*/
894   }
895 
896   sensor_msgs::Image::Ptr imagePointer = bridge->toImageMsg();
897   imageShowPubPointer->publish(imagePointer);
898 }
899 
900 int main(int argc, char** argv)
901 {
902   ros::init(argc, argv, "visualOdometry");
903   ros::NodeHandle nh;
904 
905   ros::Subscriber imagePointsSub = nh.subscribe<sensor_msgs::PointCloud2>
906                                    ("/image_points_last", 5, imagePointsHandler);
907 
908   ros::Subscriber depthCloudSub = nh.subscribe<sensor_msgs::PointCloud2> 
909                                   ("/depth_cloud", 5, depthCloudHandler);
910 
911   /*
912    * whether you need IMU information
913   */
914   //ros::Subscriber imuDataSub = nh.subscribe<sensor_msgs::Imu> ("/imu/data", 5, imuDataHandler);
915 
916   ros::Publisher voDataPub = nh.advertise<nav_msgs::Odometry> ("/cam_to_init", 5);
917   voDataPubPointer = &voDataPub;
918 
919   tf::TransformBroadcaster tfBroadcaster;
920   tfBroadcasterPointer = &tfBroadcaster;
921 
922   ros::Publisher depthPointsPub = nh.advertise<sensor_msgs::PointCloud2> ("/depth_points_last", 5);
923   depthPointsPubPointer = &depthPointsPub;
924 
925   ros::Publisher imagePointsProjPub = nh.advertise<sensor_msgs::PointCloud2> ("/image_points_proj", 1);
926   imagePointsProjPubPointer = &imagePointsProjPub;
927 
928   ros::Subscriber imageDataSub = nh.subscribe<sensor_msgs::Image>("/image/show", 1, imageDataHandler);
929 
930   ros::Publisher imageShowPub = nh.advertise<sensor_msgs::Image>("/image/show_2", 1);
931   imageShowPubPointer = &imageShowPub;
932 
933   ros::spin();
934 
935   return 0;
936 }

 

posted on 2018-04-07 22:07  zhch_pan  阅读(4766)  评论(1编辑  收藏  举报

导航