V-LOAM源码解析（四）

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

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

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

---

节点：stackDepthPoint

功能：将视觉里程计中用于定位的特征点三维坐标分批存储，用于后面的局部BA优化

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <ros/ros.h>

#include "pointDefinition.h"

const double PI = 3.1415926;

const int keyframeNum = 5;
pcl::PointCloud<DepthPoint>::Ptr depthPoints[keyframeNum];
double depthPointsTime[keyframeNum];
int keyframeCount = 0;
int frameCount = 0;

pcl::PointCloud<DepthPoint>::Ptr depthPointsStacked(new pcl::PointCloud<DepthPoint>());
ros::Publisher *depthPointsPubPointer = NULL;

double lastPubTime = 0;

void depthPointsHandler(const sensor_msgs::PointCloud2ConstPtr& depthPoints2)
{
  //每隔五帧保留一帧
  frameCount = (frameCount + 1) % 5;
  if (frameCount != 0) {
    return;
  }

  pcl::PointCloud<DepthPoint>::Ptr depthPointsCur = depthPoints[0];
  depthPointsCur->clear();
  pcl::fromROSMsg(*depthPoints2, *depthPointsCur);

  for (int i = 0; i < keyframeNum - 1; i++) {
    depthPoints[i] = depthPoints[i + 1];
    depthPointsTime[i] = depthPointsTime[i + 1];
  }
  depthPoints[keyframeNum - 1] = depthPointsCur;
  depthPointsTime[keyframeNum - 1] = depthPoints2->header.stamp.toSec();

  keyframeCount++;
  if (keyframeCount >= keyframeNum && depthPointsTime[0] >= lastPubTime) {
    depthPointsStacked->clear();
    for (int i = 0; i < keyframeNum; i++) {
      *depthPointsStacked += *depthPoints[i];
    }

    sensor_msgs::PointCloud2 depthPoints3;
    pcl::toROSMsg(*depthPointsStacked, depthPoints3);
    depthPoints3.header.frame_id = "camera";
    depthPoints3.header.stamp = depthPoints2->header.stamp;
    depthPointsPubPointer->publish(depthPoints3);

    lastPubTime = depthPointsTime[keyframeNum - 1];
  }
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "stackDepthPoint");
  ros::NodeHandle nh;

  for (int i = 0; i < keyframeNum; i++) {
    pcl::PointCloud<DepthPoint>::Ptr depthPointsTemp(new pcl::PointCloud<DepthPoint>());
    depthPoints[i] = depthPointsTemp;
  }

  ros::Subscriber depthPointsSub = nh.subscribe<sensor_msgs::PointCloud2>
                                   ("/depth_points_last", 5, depthPointsHandler);

  ros::Publisher depthPointsPub = nh.advertise<sensor_msgs::PointCloud2> ("/depth_points_stacked", 1);
  depthPointsPubPointer = &depthPointsPub;

  ros::spin();

  return 0;
}