V-LOAM源碼解析（四）

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

 

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

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

 

---

 

節點：stackDepthPoint

功能：將視覺里程計中用於定位的特徵點三維座標分批存儲，用於後面的局部BA優化

 

 1 #include <math.h>
 2 #include <stdio.h>
 3 #include <stdlib.h>
 4 #include <ros/ros.h>
 5 
 6 #include "pointDefinition.h"
 7 
 8 const double PI = 3.1415926;
 9 
10 const int keyframeNum = 5;
11 pcl::PointCloud<DepthPoint>::Ptr depthPoints[keyframeNum];
12 double depthPointsTime[keyframeNum];
13 int keyframeCount = 0;
14 int frameCount = 0;
15 
16 pcl::PointCloud<DepthPoint>::Ptr depthPointsStacked(new pcl::PointCloud<DepthPoint>());
17 ros::Publisher *depthPointsPubPointer = NULL;
18 
19 double lastPubTime = 0;
20 
21 void depthPointsHandler(const sensor_msgs::PointCloud2ConstPtr& depthPoints2)
22 {
23   //每隔五幀保留一幀
24   frameCount = (frameCount + 1) % 5;
25   if (frameCount != 0) {
26     return;
27   }
28 
29   pcl::PointCloud<DepthPoint>::Ptr depthPointsCur = depthPoints[0];
30   depthPointsCur->clear();
31   pcl::fromROSMsg(*depthPoints2, *depthPointsCur);
32 
33   for (int i = 0; i < keyframeNum - 1; i++) {
34     depthPoints[i] = depthPoints[i + 1];
35     depthPointsTime[i] = depthPointsTime[i + 1];
36   }
37   depthPoints[keyframeNum - 1] = depthPointsCur;
38   depthPointsTime[keyframeNum - 1] = depthPoints2->header.stamp.toSec();
39 
40   keyframeCount++;
41   if (keyframeCount >= keyframeNum && depthPointsTime[0] >= lastPubTime) {
42     depthPointsStacked->clear();
43     for (int i = 0; i < keyframeNum; i++) {
44       *depthPointsStacked += *depthPoints[i];
45     }
46 
47     sensor_msgs::PointCloud2 depthPoints3;
48     pcl::toROSMsg(*depthPointsStacked, depthPoints3);
49     depthPoints3.header.frame_id = "camera";
50     depthPoints3.header.stamp = depthPoints2->header.stamp;
51     depthPointsPubPointer->publish(depthPoints3);
52 
53     lastPubTime = depthPointsTime[keyframeNum - 1];
54   }
55 }
56 
57 int main(int argc, char** argv)
58 {
59   ros::init(argc, argv, "stackDepthPoint");
60   ros::NodeHandle nh;
61 
62   for (int i = 0; i < keyframeNum; i++) {
63     pcl::PointCloud<DepthPoint>::Ptr depthPointsTemp(new pcl::PointCloud<DepthPoint>());
64     depthPoints[i] = depthPointsTemp;
65   }
66 
67   ros::Subscriber depthPointsSub = nh.subscribe<sensor_msgs::PointCloud2>
68                                    ("/depth_points_last", 5, depthPointsHandler);
69 
70   ros::Publisher depthPointsPub = nh.advertise<sensor_msgs::PointCloud2> ("/depth_points_stacked", 1);
71   depthPointsPubPointer = &depthPointsPub;
72 
73   ros::spin();
74 
75   return 0;
76 }