作为传统的障碍物或地面分割方法之一的第一步,栅格化显得基础又重要。这是我根据自己理解写的栅格化程序,目前还没有找到权威的版本。接下来几个版本的程序,代表了不断地改进。
//
// Created by ethan on 18-6-6.
//
#include <iostream>
#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/console/print.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/visualization/cloud_viewer.h>
class Grid//将一个栅格定义为一个类对象
{
public:
bool road;//是不是路
float h_mean;//平均高度
float h_square;//高度方差
pcl::PointCloud<pcl::PointXYZ>::Ptr grid_cloud {new pcl::PointCloud<pcl::PointXYZ>};//指向栅格内的点云的指针
pcl::PointIndices::Ptr grid_inliers {new pcl::PointIndices};//栅格内点云索引的指针
int num;//点云点数
//std::vector<int> indices;//点云索引(定义了索引向量指针就不用定义它啦,因为后面想将得到的索引值赋给索引的时候,直接赋给*ptr就可以了)
};
int
main(int argc,char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);//输入点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);//输出点云
if (pcl::io::loadPCDFile<pcl::PointXYZ> ("/home/ethan/bags/pcl_2_bag/cloud9.pcd", *cloud_in) == -1)//读取pcd文件
{
PCL_ERROR ("Couldn't read file cloud9.pcd ^.^\n");
return (-1);
}
pcl::ExtractIndices<pcl::PointXYZ> extract;//定义一个抽取器
int column=10;
int row=10;
float grid_size=5.0;
Grid grid[column][row];
int j=0;
int i=0;
for (int m=0;m<cloud_in->size();m++)//这些点一个一个看
{
for( i=-column/2;i<column/2;i++)
{
if (((cloud_in->points[m].x>i*grid_size)&&(cloud_in->points[m].x<i*grid_size+grid_size)))//如果x在某范围内
{
for( j=-row/2;j<row/2;j++)
{
if((cloud_in->points[m].y>j*grid_size)&&(cloud_in->points[m].y<j*grid_size+grid_size))//如果y在某范围内
{
//grid_inliner是pointindices类型的指针,pointindices类型的内容,包括header和索引向量indices
//header就直接赋值为输入点云的header
grid[i+column/2][j+row/2].grid_inliers->indices.push_back(m);
grid[i+column/2][j+row/2].grid_inliers->header=cloud_in->header;
}
}
}
}
}
extract.setInputCloud (cloud_in);//从哪里抽取
extract.setIndices (grid[0][0].grid_inliers);//创建抽取出来的点的索引(编号)
extract.setNegative (false);//将平面保留还是去除
extract.filter (*cloud_out);//滤波(抽取)后结果送往cloud_out
pcl::visualization::CloudViewer viewer("resterization");//可视化窗口
viewer.showCloud(grid[4][4].grid_cloud);//看一下其中某个栅格
while(!viewer.wasStopped())
{ }
}
效果:
这里先定义个索引指针,再从输入点云中按照这些索引将点抽出来,送往一个点云指针。为什么会这么麻烦呢?因为我在想怎么把点放到同一个东西里的时候,就只知道点云分割有这种功能。点云分割里,就是分割完后得到一个pointindices(包括header和indices,header值和输入点云的header一样)和coefficient,indices就是符合条件的点的索引就pushback到它里面。再从原点云中按照这个indices将内点抽取出来。
那与其这样,访问点的时候,直接把符合条件的点pushback到栅格自己的点云指针就是了,何必先得到索引,在从原点云中按照索引拿呢。
所以if里面可以改成这样:
if((cloud_in->points[m].y>j*grid_size)&&(cloud_in->points[m].y<j*grid_size+grid_size))
{
grid[i+column/2][j+row/2].grid_cloud->points.push_back(cloud_in->points[m]);
}
还有一个想法是,不再预先设定范围然后判断点是否在此范围内,而是直接看point的坐标,然后除以栅格大小,就可以得到栅格的index,应该只需要简单的一句。后面调好了会贴上来。