【PCL】点云栅格化1

作为传统的障碍物或地面分割方法之一的第一步，栅格化显得基础又重要。这是我根据自己理解写的栅格化程序，目前还没有找到权威的版本。接下来几个版本的程序，代表了不断地改进。

//
// 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，应该只需要简单的一句。后面调好了会贴上来。

【PCL】点云栅格化1

猜你喜欢