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 }