点云算法解密-栅格压缩法

点云压缩不仅可以提升后期处理的效率,同时还可以去除部分冗余点。栅格压缩法是一种降低点云密度,而且可以有效去除多站点云数据拼接后的重叠数据。这种算法的思想是将点云里的点划分到多个长宽高相等的正方体中,然后计算每个正方体中的所有点的坐标平均值点,以平均值点替代正方体中所有点来完成对点云的压缩。

算法的关键在于如何把点分配到正方体中,实际上,就是以正方体的边长s作为区间长度,然后将点的坐标分量的x、y、z分别除于s,然后取整,便可以求得点在x、y、z上所处的区间号,有了三个区间号,便可以确定其所处的正方体了。算法中正方体边长的设置是关键,一般要设置的比点云的相邻点间隔要大一些,才能达到压缩的效果。具体代码如下:

[code lang=”cpp”]

#include "stdafx.h"
#include <iostream>
#include <vector>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>

struct PointID
{
int idx;
int idy;
int idz;

int index;

bool operator < (const PointID &p) const
{
if (idx<p.idx) { return true; } else if (idx>p.idx)
{
return false;
}
else
{
if (idy<p.idy) { return true; } else if (idy>p.idy)
{
return false;
}
else
{
return (idz < p.idz);
}
}
}

bool operator==(const PointID&p)const
{
return idx == p.idx&&idy == p.idy&&idz == p.idz;
}
};

pcl::PointCloud<pcl::PointXYZ> sample(pcl::PointCloud<pcl::PointXYZ>& cloud,
float leafSize)
{
//计算每个点的序号
std::vector<PointID> pids;
pids.resize(cloud.size());

PointID id;
float inverseLeafSize = 1.0 / leafSize;

for (int i = 0; i < cloud.size(); i++)
{
id.idx = static_cast<int>(cloud[i].x *inverseLeafSize);
id.idy = static_cast<int>(cloud[i].y *inverseLeafSize);
id.idz = static_cast<int>(cloud[i].z *inverseLeafSize);
id.index = i;

pids[i] = id;
}

//序号排序
std::sort(pids.begin(), pids.end());

//序号分组
std::vector<std::pair<int, int> > startAndEndIndices;
int startIndex = 0;

while (startIndex < pids.size())
{
int endIndex = startIndex + 1;

while (endIndex < pids.size() && pids[endIndex] == pids[startIndex])
{
endIndex++;
}

startAndEndIndices.push_back(std::pair<int, int>(startIndex, endIndex – 1));

startIndex = endIndex;
}

//求得每个小盒的平均值点
pcl::PointCloud<pcl::PointXYZ> cloudout;

for (int i = 0; i < startAndEndIndices.size(); i++)
{
pcl::PointXYZ point;

int startIndex = startAndEndIndices[i].first;
int endIndex = startAndEndIndices[i].second;

int n = endIndex – startIndex + 1;

while (startIndex <= endIndex)
{
point.x += cloud[pids[startIndex].index].x;
point.y += cloud[pids[startIndex].index].y;
point.z += cloud[pids[startIndex].index].z;
startIndex++;
}

point.x /= n;
point.y /= n;
point.z /= n;

cloudout.push_back(point);
}

return cloudout;
}

int main()
{
//读取点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

if (pcl::io::loadPCDFile("D:\\Bunny.pcd", *cloud))
{
std::cout << "error";
}

//栅格压缩
pcl::PointCloud<pcl::PointXYZ> cloudout = sample(*cloud, 7.5);

//点云显示
pcl::visualization::PCLVisualizer viewer("Bunny 3D");

viewer.addPointCloud(cloudout.makeShared(), "Bunny");

while (!viewer.wasStopped())
{
viewer.spinOnce();
}

return (0);
}
[/code]

发表评论

电子邮件地址不会被公开。 必填项已用*标注