V-LOAM源碼解析（三）

轉載請註明出處：本文轉自zhch_pan的博客http://www.cnblogs.com/zhchp-blog/

 

本博客爲本人以前作項目時作的源碼閱讀工做，po到網上但願幫助其餘人更好的理解V-LOAM的工程實現，有些地方代碼作了修改，可能和原工程有出入，但對於該工程的總體流程理解沒有妨礙。

源碼下載連接：https://github.com/Jinqiang/demo_lidar

 

---

 

節點：processDepthmap

功能：根據視覺里程計topic("/cam_to_init")，和點雲數據（"/sync_scan_cloud_filtered"），將新點雲和已有點雲變換到當前相機座標系下，實現圖像與點雲數據的對齊

 

  1 /*
  2  * 數組voRx[]~voTz[]用於保存連續的相機位姿,depthCloud不斷把以前的點雲點變換到最近時刻的相機位姿,
  3  * 當再有新的點雲到來時,進入函數syncCloudHandler(),該函數先將該幀點雲變換到最近的相機位姿下,而後
  4  * 添加到depthCloud中;當相機位姿發生變化時,進入函數voDataHandler(),先將depthCloud變換到新的相機
  5  * 位姿下,而後進行濾波,而後發佈出去
  6  */
  7 #include <math.h>
  8 #include <stdio.h>
  9 #include <stdlib.h>
 10 #include <ros/ros.h>
 11 
 12 #include <nav_msgs/Odometry.h>
 13 
 14 #include <tf/transform_datatypes.h>
 15 #include <tf/transform_listener.h>
 16 #include <tf/transform_broadcaster.h>
 17 
 18 #include "pointDefinition.h"
 19 
 20 const double PI = 3.1415926;
 21 
 22 const int keepVoDataNum = 30;
 23 double voDataTime[keepVoDataNum] = {0};
 24 double voRx[keepVoDataNum] = {0};
 25 double voRy[keepVoDataNum] = {0};
 26 double voRz[keepVoDataNum] = {0};
 27 double voTx[keepVoDataNum] = {0};
 28 double voTy[keepVoDataNum] = {0};
 29 double voTz[keepVoDataNum] = {0};
 30 int voDataInd = -1;
 31 int voRegInd = 0;
 32 
 33 pcl::PointCloud<pcl::PointXYZI>::Ptr depthCloud(new pcl::PointCloud<pcl::PointXYZI>());
 34 pcl::PointCloud<pcl::PointXYZ>::Ptr syncCloud(new pcl::PointCloud<pcl::PointXYZ>());
 35 pcl::PointCloud<pcl::PointXYZI>::Ptr tempCloud(new pcl::PointCloud<pcl::PointXYZI>());
 36 pcl::PointCloud<pcl::PointXYZI>::Ptr tempCloud2(new pcl::PointCloud<pcl::PointXYZI>());
 37 
 38 double timeRec = 0;
 39 double rxRec = 0, ryRec = 0, rzRec = 0;
 40 double txRec = 0, tyRec = 0, tzRec = 0;
 41 
 42 bool systemInited = false;
 43 double initTime;
 44 
 45 int startCount = -1;
 46 const int startSkipNum = 5;
 47 
 48 ros::Publisher *depthCloudPubPointer = NULL;
 49 
 50 void voDataHandler(const nav_msgs::Odometry::ConstPtr& voData)
 51 {
 52   double time = voData->header.stamp.toSec();
 53 
 54   double roll, pitch, yaw;
 55   geometry_msgs::Quaternion geoQuat = voData->pose.pose.orientation;
 56   tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
 57 
 58   double rx = voData->twist.twist.angular.x - rxRec;
 59   double ry = voData->twist.twist.angular.y - ryRec;
 60   double rz = voData->twist.twist.angular.z - rzRec;
 61 
 62   if (ry < -PI) {
 63     ry += 2 * PI;
 64   } else if (ry > PI) {
 65     ry -= 2 * PI;
 66   }
 67 
 68   double tx = voData->pose.pose.position.x - txRec;
 69   double ty = voData->pose.pose.position.y - tyRec;
 70   double tz = voData->pose.pose.position.z - tzRec;
 71 
 72   rxRec = voData->twist.twist.angular.x;
 73   ryRec = voData->twist.twist.angular.y;
 74   rzRec = voData->twist.twist.angular.z;
 75 
 76   txRec = voData->pose.pose.position.x;
 77   tyRec = voData->pose.pose.position.y;
 78   tzRec = voData->pose.pose.position.z;
 79 
 80   //由於是把世界座標系旋轉到當前座標系,因此roll,pitch,yaw應該取負值,而繞x軸和繞y軸的旋轉角度在發佈與接收時已經被
 81   //添加了負值,因此旋轉矩陣沒變,而繞z軸的旋轉角沒取負值,因此在旋轉矩陣裏要把繞z軸角度取負值
 82   double x1 = cos(yaw) * tx + sin(yaw) * tz;
 83   double y1 = ty;
 84   double z1 = -sin(yaw) * tx + cos(yaw) * tz;
 85 
 86   double x2 = x1;
 87   double y2 = cos(pitch) * y1 - sin(pitch) * z1;
 88   double z2 = sin(pitch) * y1 + cos(pitch) * z1;
 89 
 90   tx = cos(roll) * x2 + sin(roll) * y2;
 91   ty = -sin(roll) * x2 + cos(roll) * y2;
 92   tz = z2;
 93 
 94   //voDataInd取值爲0~29
 95   voDataInd = (voDataInd + 1) % keepVoDataNum;
 96   voDataTime[voDataInd] = time;
 97   /*
 98    * rx~ry存的是R_lc中的旋轉量,旋轉方向是z->x->y,參考座標系是上一幀,因此也就是說上一幀按照R_lc=ry*rx*rz(旋轉方向自右向左)的
 99    * 順序旋轉能夠獲得當前幀的座標,在visualOdometry.cpp中能夠看到,transform[0]~[2]存儲的實際上是R_cl中的旋轉角度,而vo的
100    * twist中的旋轉角度存的是angleSum[0]~[2] -= transform[0]~[2],有一個取負值的操做,取負以後獲得的就是R_lc中的旋轉角,
101    * R_lc和R_cl的區別就是:
102    * R_lc=ry*rx*rz
103    * R_cl=-rz*-rx*-ry(旋轉順序從右往左看)
104    * tx~tz存的就是T_lc的位移量,當前座標系相對於上一幀座標系,在當前座標系下表示的位移增量,
105   */
106   voRx[voDataInd] = rx;
107   voRy[voDataInd] = ry;
108   voRz[voDataInd] = rz;
109   voTx[voDataInd] = tx;
110   voTy[voDataInd] = ty;
111   voTz[voDataInd] = tz;
112 
113   double cosrx = cos(rx);
114   double sinrx = sin(rx);
115   double cosry = cos(ry);
116   double sinry = sin(ry);
117   double cosrz = cos(rz);
118   double sinrz = sin(rz);
119 
120   if (time - timeRec < 0.5) {
121     pcl::PointXYZI point;
122     tempCloud->clear();
123     double x1, y1, z1, x2, y2, z2;
124     int depthCloudNum = depthCloud->points.size();
125     for (int i = 0; i < depthCloudNum; i++) {
126       point = depthCloud->points[i];
127 
128       x1 = cosry * point.x - sinry * point.z;
129       y1 = point.y;
130       z1 = sinry * point.x + cosry * point.z;
131 
132       x2 = x1;
133       y2 = cosrx * y1 + sinrx * z1;
134       z2 = -sinrx * y1 + cosrx * z1;
135 
136       /*
137        * tx~tz存的就是當前座標系相對於上一幀座標系,在當前座標系下表示的位移增量,T_lc,由於是
138        * rx = voData->twist.twist.angular.x - rxRec;因此基準座標系是上一幀,即rxRec.
139        * 因此上一幀的一個點P_l,經過P_c=R_cl*P_l+T_cl能夠變換到當前座標系,
140        * 而且R_cl=-rz*-rx*-ry  T_cl=-T_lc(即-tx,-ty,-tz),因此這裏是減去tx,ty,tz
141       */
142       point.x = cosrz * x2 + sinrz * y2 - tx;
143       point.y = -sinrz * x2 + cosrz * y2 - ty;
144       point.z = z2 - tz;
145 
146       double pointDis = sqrt(point.x * point.x + point.y * point.y + point.z * point.z);
147       double timeDis = time - initTime - point.intensity;
148       if (fabs(point.x / point.z) < 2 && fabs(point.y / point.z) < 1 && point.z > 0.5 && pointDis < 15 &&
149           timeDis < 5.0) {
150         tempCloud->push_back(point);
151       }
152     }
153 
154     depthCloud->clear();
155     pcl::VoxelGrid<pcl::PointXYZI> downSizeFilter;
156     downSizeFilter.setInputCloud(tempCloud);
157     downSizeFilter.setLeafSize(0.05, 0.05, 0.05);
158     downSizeFilter.filter(*depthCloud);
159     depthCloudNum = depthCloud->points.size();
160 
161     tempCloud->clear();
162     for (int i = 0; i < depthCloudNum; i++) {
163       point = depthCloud->points[i];
164 
165       if (fabs(point.x / point.z) < 1 && fabs(point.y / point.z) < 0.6) {
166         point.intensity = depthCloud->points[i].z;
167         point.x *= 10 / depthCloud->points[i].z;
168         point.y *= 10 / depthCloud->points[i].z;
169         point.z = 10;
170 
171         tempCloud->push_back(point);
172       }
173     }
174 
175     tempCloud2->clear();
176     downSizeFilter.setInputCloud(tempCloud);
177     downSizeFilter.setLeafSize(0.1, 0.1, 0.1);
178     downSizeFilter.filter(*tempCloud2);
179     int tempCloud2Num = tempCloud2->points.size();
180 
181     for (int i = 0; i < tempCloud2Num; i++) {
182       tempCloud2->points[i].z = tempCloud2->points[i].intensity;
183       tempCloud2->points[i].x *= tempCloud2->points[i].z / 10;
184       tempCloud2->points[i].y *= tempCloud2->points[i].z / 10;
185       tempCloud2->points[i].intensity = 10;
186     }
187 
188     sensor_msgs::PointCloud2 depthCloud2;
189     pcl::toROSMsg(*tempCloud2, depthCloud2);
190     depthCloud2.header.frame_id = "camera2";
191     depthCloud2.header.stamp = voData->header.stamp;
192     depthCloudPubPointer->publish(depthCloud2);
193   }
194 
195   timeRec = time;
196 }
197 
198 void syncCloudHandler(const sensor_msgs::PointCloud2ConstPtr& syncCloud2)
199 {
200   if (startCount < startSkipNum) {
201     startCount++;
202     return;
203   }
204 
205   if (!systemInited) {
206     initTime = syncCloud2->header.stamp.toSec();
207     systemInited = true;
208   }
209   double time = syncCloud2->header.stamp.toSec();
210   double timeLasted = time - initTime;
211 
212   syncCloud->clear();
213   pcl::fromROSMsg(*syncCloud2, *syncCloud);
214 
215   double scale = 0;
216   int voPreInd = keepVoDataNum - 1;
217   if (voDataInd >= 0) {
218     while (voDataTime[voRegInd] <= time && voRegInd != voDataInd) {
219       voRegInd = (voRegInd + 1) % keepVoDataNum;
220     }
221 
222     voPreInd = (voRegInd + keepVoDataNum - 1) % keepVoDataNum;
223     double voTimePre = voDataTime[voPreInd];
224     double voTimeReg = voDataTime[voRegInd];
225 
226     if (voTimeReg - voTimePre < 0.5) {
227       //modified at 2018/01/09
228       //double scale =  (voTimeReg - time) / (voTimeReg - voTimePre);
229       scale =  (voTimeReg - time) / (voTimeReg - voTimePre);
230       if (scale > 1) {
231         scale = 1;
232       } else if (scale < 0) {
233         scale = 0;
234       }
235     }
236   }
237 
238   //經過插值獲得與點雲對應的座標系,這個座標系下保存的rx2~rz2,tx2~tz2指的是點雲對應的幀與voRegInd指向的幀之間的R,T關係
239   double rx2 = voRx[voRegInd] * scale;
240   double ry2 = voRy[voRegInd] * scale;
241   double rz2 = voRz[voRegInd] * scale;
242 
243   double tx2 = voTx[voRegInd] * scale;
244   double ty2 = voTy[voRegInd] * scale;
245   double tz2 = voTz[voRegInd] * scale;
246 
247   double cosrx2 = cos(rx2);
248   double sinrx2 = sin(rx2);
249   double cosry2 = cos(ry2);
250   double sinry2 = sin(ry2);
251   double cosrz2 = cos(rz2);
252   double sinrz2 = sin(rz2);
253 
254   pcl::PointXYZI point;
255   double x1, y1, z1, x2, y2, z2;
256   int syncCloudNum = syncCloud->points.size();
257   for (int i = 0; i < syncCloudNum; i++) {
258     point.x = syncCloud->points[i].x;
259     point.y = syncCloud->points[i].y;
260     point.z = syncCloud->points[i].z;
261     point.intensity = timeLasted;
262 
263     //把插值獲得的座標系下的點轉換到voRegInd指向的那一幀
264     x1 = cosry2 * point.x - sinry2 * point.z;
265     y1 = point.y;
266     z1 = sinry2 * point.x + cosry2 * point.z;
267 
268     x2 = x1;
269     y2 = cosrx2 * y1 + sinrx2 * z1;
270     z2 = -sinrx2 * y1 + cosrx2 * z1;
271 
272     point.x = cosrz2 * x2 + sinrz2 * y2 - tx2;
273     point.y = -sinrz2 * x2 + cosrz2 * y2 - ty2;
274     point.z = z2 - tz2;
275 
276     //將點雲一直變換到最新的一幀座標系下
277     if (voDataInd >= 0) {
278       int voAftInd = (voRegInd + 1) % keepVoDataNum;
279       while (voAftInd != (voDataInd + 1) % keepVoDataNum) {
280         double rx = voRx[voAftInd];
281         double ry = voRy[voAftInd];
282         double rz = voRz[voAftInd];
283 
284         double tx = voTx[voAftInd];
285         double ty = voTy[voAftInd];
286         double tz = voTz[voAftInd];
287 
288         double cosrx = cos(rx);
289         double sinrx = sin(rx);
290         double cosry = cos(ry);
291         double sinry = sin(ry);
292         double cosrz = cos(rz);
293         double sinrz = sin(rz);
294 
295         x1 = cosry * point.x - sinry * point.z;
296         y1 = point.y;
297         z1 = sinry * point.x + cosry * point.z;
298 
299         x2 = x1;
300         y2 = cosrx * y1 + sinrx * z1;
301         z2 = -sinrx * y1 + cosrx * z1;
302 
303         point.x = cosrz * x2 + sinrz * y2 - tx;
304         point.y = -sinrz * x2 + cosrz * y2 - ty;
305         point.z = z2 - tz;
306 
307         voAftInd = (voAftInd + 1) % keepVoDataNum;
308       }
309     }
310 
311     double pointDis = sqrt(point.x * point.x + point.y * point.y + point.z * point.z);
312     if (fabs(point.x / point.z) < 2 && fabs(point.y / point.z) < 1.5 && point.z > 0.5 && pointDis < 15) {
313       depthCloud->push_back(point);
314     }
315   }
316 }
317 
318 int main(int argc, char** argv)
319 {
320   ros::init(argc, argv, "processDepthmap");
321   ros::NodeHandle nh;
322 
323   ros::Subscriber voDataSub = nh.subscribe<nav_msgs::Odometry> ("/cam_to_init", 5, voDataHandler);
324 
325   ros::Subscriber syncCloudSub = nh.subscribe<sensor_msgs::PointCloud2>
326                                  ("/sync_scan_cloud_filtered", 5, syncCloudHandler);
327 
328   ros::Publisher depthCloudPub = nh.advertise<sensor_msgs::PointCloud2> ("/depth_cloud", 5);
329   depthCloudPubPointer = &depthCloudPub;
330 
331   ros::spin();
332 
333   return 0;
334 }