前段时间写了一些代码，但是在运算过程中发现有些代码可以进行改进以提高运行效率，尤其是与PCL相关的部分，可以进行大幅度提高．特意在此进行记录，分享给大家，也供自己查看．
pcl::PointCloud< T >点云的加法
比如我有两个clusters，每个clusters都包括points_这个成员，类型为：
pcl::PointCloud<PointInfoType> points_;
我想要把两个clusters的points_成员加在一起，PCL是支持点云的加法的，如下所示．
cluster1.points_ = cluster1.points_ + cluster2.points_;
但是这样会很慢，我们可以通过逐点的push_back来替换，如下所示．
// update the cluster points
for(int i=0; i<cluster2.points_.size(); i++)
{
cluster1.points_.push_back(cluster2.points_[i]);
}
提前分配空间
比如，我们想对点云进行一个体素下采样，每个体素中保留一个点且已知体素的数量，那么我们可以提前分配给点云变量对应大小．这样在填充过程中就不用系统每次内部进行扩充，大大加快处理速度．
如下述代码中，提前为pl_feat分配了num_voxels大小，后续使用emplace_back函数，逐个添加点．
pl_feat.clear();
pl_feat.points.reserve(num_voxels); // 提前分配内存
for (const auto& voxel : voxel_map)
{
// pl_feat.points.push_back(voxel.second.points_[0]);
pl_feat.points.emplace_back(voxel.second.points_[0]);
}
pl_feat.width = pl_feat.points.size();
pl_feat.height = 1;
pl_feat.is_dense = true;
类似的，在构建voxelmap的时候也提前分配好一个比较大的空间，这样在向voxel_map中添加点云时就会大大加快速度．
std::unordered_map<UNI_VOXEL_LOC, M_POINT> voxel_map;
voxel_map.reserve(50000000);
在实时运行中kdtree最好只构建一次
在面对大量点云，或者说点云在逐渐增加时，最好只对点云构建一次kdtree或者说尽量降低构建kdtree的次数，或者更换其他数据结构，比如ikdtree来解决问题．
pcl中自带的kdtree构建难以处理大量点云，会非常耗时
// build a kd tree
pcl::KdTreeFLANN<pcl::PointXY> kdtree2d;
kdtree2d.setInputCloud(gnd_data2d);