来用C++实现八叉树以及解决点云的均匀降采样问题

在使用点云时会遇到降采样问题，对于一般体量的点云可以用pcl的voxel filter降采样，但pcl提供的接口对输入的点云大小有限制，当点云过大时将会失败（其实一点也不大好吗），而且体素栅格采样求取的是网格内各个点云的质心，也就是说降采样完毕后，点云位置会有变化，并不是之前点云中的某个id所代表的点。另外，pcl1.9带的均匀采样uniform downsample可以做到保留点云原有的位置，但实测它也并不是真正的均匀采样，在不同区域的降采样效果并不均衡，有密一块疏一块的嫌疑。

对于大规模、须保留原位置的点云降采样需求，有一种比较暴力的方法是采用三维数组储存空间内的点云，按空间换时间的思想来说，效果很好，但是花费的内存实在是太大了，另外一种常用的方法就是八叉树，它可以递归地将点云分散为父与子的关系，类似于kd树，它的时间复杂度比暴力法小很多。

事不宜迟，马上撸一个八叉树的demo，头文件如下：

#ifndef OCTREE_H
#define OCTREE_H
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <std_msgs/Float32.h>
#include <stack>

#include <pcl/common/common.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/common/transforms.h>
#include <pcl/common/centroid.h>
#include <pcl_conversions/pcl_conversions.h>

using namespace std;

typedef pcl::PointXYZI NormalType;
typedef pcl::PointCloud<NormalType> NormalCloud;
typedef pcl::PointCloud<NormalType>::Ptr NormalPtr;

//定义八叉树的节点
struct Node
{
  float size;//此节点的栅格大小
  int depth = 0;//此节点在八叉树中的深度

  //此节点的栅格上下界
  float xMin, xMax;
  float yMin, yMax;
  float zMin, zMax;

  //在遍历树的过程中是否已经被访问，因此默认为否
  bool visited = false;

  //此节点的八个子节点
  std::vector<Node*> childs;

  //当前节点所囊括的点云下标
  std::vector<int> cloudIndices;

  Node(float xmin, float xmax, float ymin, float ymax,
        float zmin, float zmax)
      : xMin(xmin), xMax(xmax), yMin(ymin), yMax(ymax),
        zMin(zmin), zMax(zmax)
  {
    childs.resize(8);
  }

  //查找是否存在未访问过的子节点
  Node* nextChild()
  {
    for(auto& c:childs)
    {
      if(!c->visited)
        return c;
    }
    return nullptr;
  }
};

namespace octree_test
{
class OctreeTest
{
public:
  OctreeTest(ros::NodeHandle& nh);
  ~OctreeTest();

  void buildOctree();
  void setChild(Node*&, std::vector<int>&, int, float, float, float, float, float, float);
  void downsizeCloud();
  void visualize(NormalPtr&, ros::Publisher&);
private:
  //可动态调整点云降采样尺寸
  ros::Subscriber size_sub_;
  void sizeCb(const std_msgs::Float32::ConstPtr& msg);
  float current_downsize_;

  //建立八叉树时最小的栅格大小
  float min_downsize_;
  //八叉树根节点
  Node* root_node_;

  std::vector<Node*> node_list_;

  //点云可视化
  ros::Publisher cloud_pub_, downsize_pub_;

  //读取点云的路径
  std::string pcd_dir_;

  //要用到的点云
  NormalPtr current_cloud_, downsize_cloud_;
};
}/* end of namespace */
#endif

为了方便调试，我们用rviz可以观看原点云和降采样的点云的效果，并且在命令行中用ros topic pub “/this_downsize"就可以输入要降采样的尺寸。 在降采样的过程中，如果只取每个栅格的第一个点，也算是均匀采样，如果要找到最接近栅格中心的点，恐怕耗费的时间有点太长了。

#include "octree.h"

using namespace std;

namespace octree_test
{
OctreeTest::OctreeTest(ros::NodeHandle& nh)
  : pcd_dir_("global.pcd"), min_downsize_(0.04), root_node_(nullptr)
{
  cloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>("/original_cloud", 1, true);
  downsize_pub_ = nh.advertise<sensor_msgs::PointCloud2>("/downsize_cloud", 1, true);
  size_sub_ = nh.subscribe<std_msgs::Float32>("/this_downsize", 1, &OctreeTest::sizeCb, this);

  current_cloud_.reset(new NormalCloud());
  downsize_cloud_.reset(new NormalCloud());

  buildOctree();
}

OctreeTest::~OctreeTest()
{
  int i = 0;
  for(auto& ptr:node_list_)
  {
    cout<<"delete :"<<i<<" node"<<endl;
    ++i;
    delete ptr;
    ptr = nullptr;
  }
}

void OctreeTest::buildOctree()
{
  pcl::io::loadPCDFile<NormalType>(pcd_dir_, *current_cloud_);

  //去掉一些杂点，防止无意义的层级和分支
  NormalPtr tempCloud(new NormalCloud());
  for(auto& p:current_cloud_->points)
  {
    if(fabs(p.x)<10.0 && fabs(p.y)<10.0 && fabs(p.z)<5.0)//具体数值视情况决定
    {
      tempCloud->points.push_back(p);
    }
  }
  *current_cloud_ = *tempCloud;

  visualize(current_cloud_, cloud_pub_);

  NormalType max, min;
  pcl::getMinMax3D(*current_cloud_, min, max);

  //人类迷惑行为，获取点云的所有索引
  std::vector<int> indice((int)current_cloud_->size());
  for(int i = 0; i < (int)indice.size(); ++i)
  {
    indice[i] = i;
  }

  cout<<"indice:"<<indice.size()<<endl;

  setChild(root_node_, indice, 0, min.x, max.x, min.y, max.y, min.z, max.z);

  cout<<"the octree is built successfully"<<endl;
}

void OctreeTest::setChild(Node*& node, std::vector<int>& indices, int depth,
                           float xmin, float xmax, float ymin, float ymax,
                            float zmin, float zmax)
{
  //确定此栅格的尺寸，决定何时结束递归
  float this_downsize = std::min(zmax-zmin, std::min(ymax-ymin, xmax-xmin));

  if(this_downsize > min_downsize_)
  {
    //准备分割栅格的中点尺寸
    float xMiddle = 0.5*(xmax+xmin);
    float yMiddle = 0.5*(ymax+ymin);
    float zMiddle = 0.5*(zmax+zmin);
    node = new Node(xmin, xmax, ymin, ymax, zmin, zmax);

    node_list_.push_back(node);

    //记录当前节点的降采样尺寸和在树中的深度
    node->size = this_downsize;
    depth++;
    node->depth = depth;

    //将属于当前栅格的点云索引记录下来
    node->cloudIndices.clear();
    for(auto& id:indices)
    {
      auto& p = current_cloud_->points[id];
      if(p.x>=xmin && p.x<xmax && p.y>=ymin && p.y<ymax && p.z>=zmin && p.z<zmax)
      {
        node->cloudIndices.push_back(id);
      }
    }

    if(!node->cloudIndices.empty())
      cout<<"node depth:"<<node->depth<<", this_downsize:"<<this_downsize<<", node->cloudIndices:"<<node->cloudIndices.size()<<endl;

    //递归地建立八个子节点，按xyz坐标系区别前后左右上下
    setChild(node->childs[0], node->cloudIndices, depth, xMiddle, xmax, ymin, yMiddle, zMiddle, zmax);
    setChild(node->childs[1], node->cloudIndices, depth, xmin, xMiddle, ymin, yMiddle, zMiddle, zmax);
    setChild(node->childs[2], node->cloudIndices, depth, xMiddle, xmax, yMiddle, ymax, zMiddle, zmax);
    setChild(node->childs[3], node->cloudIndices, depth, xmin, xMiddle, yMiddle, ymax, zMiddle, zmax);
    setChild(node->childs[4], node->cloudIndices, depth, xMiddle, xmax, ymin, yMiddle, zmin, zMiddle);
    setChild(node->childs[5], node->cloudIndices, depth, xmin, xMiddle, ymin, yMiddle, zmin, zMiddle);
    setChild(node->childs[6], node->cloudIndices, depth, xMiddle, xmax, yMiddle, ymax, zmin, zMiddle);
    setChild(node->childs[7], node->cloudIndices, depth, xmin, xMiddle, yMiddle, ymax, zmin, zMiddle);

  }
}

void OctreeTest::downsizeCloud()
{
  //在八叉树中，相同深度的节点降采样尺寸相同
  //以需求的降采样尺寸，来寻找八叉树中适合的一层，若都没有完全相等的数值，则向下再找一层

  Node* thisNode = root_node_->childs[0];

  float ds = thisNode->size;

  while(ds>current_downsize_)
  {
    thisNode = thisNode->childs[0];

    ds = thisNode->size;
  }

  //这就是我们要访问八叉树所需的深度，当访问到该深度的栅格，则准备取点云id
  int depth = thisNode->depth;
  cout<<"visit depth:"<<depth<<endl;

  downsize_cloud_->clear();

  //在遍历树的过程中记录下小栅格中的点云id中的第一个，也就是每个小栅格保留一个点，这就是均匀降采样
  //TODO（或者求平均值？）
  std::vector<int> indices;

  //用栈遍历该树，当栈的深度等于该深度则记录，并退栈(退回父节点)，换下一个分支
  std::stack<Node*> visitStack;
  visitStack.push(root_node_);
  Node* tnode;
  while(!visitStack.empty())
  {
    //栈顶节点是当前处理的节点
    tnode = visitStack.top();
    cout<<"depth:"<<tnode->depth<<endl;
    //如果该节点的所有子节点均被访问过，则退栈回到它的父节点，否则将继续找下一个子节点访问
    auto tchild = tnode->nextChild();
    if(tchild)
    {
      tnode = tchild;
      visitStack.push(tnode);
      //标记为访问过
      tnode->visited = true;

      if(tnode->depth == depth)
      {
        //取该栅格点集中第一个点
        if(!tnode->cloudIndices.empty())
          indices.push_back(tnode->cloudIndices[0]);

        //退栈
        visitStack.pop();
      }
    }
    else
    {
      visitStack.pop();
    }
  }

  for(auto& id:indices)
  {
    downsize_cloud_->points.push_back(current_cloud_->points[id]);
  }
}

void OctreeTest::sizeCb(const std_msgs::Float32::ConstPtr& msg)
{
  float t1 = ros::Time::now().toSec();

  current_downsize_ = msg->data;
  downsizeCloud();
  visualize(downsize_cloud_, downsize_pub_);

  float t2 = ros::Time::now().toSec();
  cout<<"down size: "<<current_downsize_<<", use time:"<<t2-t1<<endl;
}

void OctreeTest::visualize(NormalPtr& cloudPtr, ros::Publisher& pub)
{
  sensor_msgs::PointCloud2 cloud;
  pcl::toROSMsg(*cloudPtr, cloud);
  cloud.header.frame_id = "map";
  pub.publish(cloud);
}

}/* end of namespace */

int main(int argc, char** argv)
{
  ros::init(argc, argv, "octree_test");
  ros::NodeHandle nh("~");
  octree_test::OctreeTest test(nh);
  ros::spin();
  return 0;
}

当然，这只是一个demo～权且抛砖引玉，只是展示了如何建立一棵树与遍历一棵树，同学们可以从中get到八叉树以及均匀降采样的方法，并且，如果要多次降采样的话，需要增加一个遍历节点的方法，将它们的是否被访问的flag给重置回来。