1.介绍
将点云格网化
读取每个格网内点云高程方向的统计特征,极值、均值、方差
判断与邻域内其他格网的相关性
例如均值之差<2cm等
2.代码
#include <string>
#include<iostream>
#include <cstdlib>
#include<fstream>
#include <sstream>
#include <Eigen/Core>
#include <opencv2/opencv.hpp>
#include <pcl/io/ply_io.h>
#include<pcl/io/pcd_io.h>
#include<pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/console/parse.h>
#include <pcl/registration/icp.h>
#include <pcl/common/common.h>
#include <pcl/filters/passthrough.h>
#include <boost/thread/thread.hpp>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/segmentation/progressive_morphological_filter.h>
int main()
{
/*读取点云文件*/
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ>("../data/subway.pcd", *cloud) == -1)
{
PCL_ERROR("Couldn't read file\n");
return(-1);
}
/*读取点云范围*/
double x_min = 10000; double y_min = 10000;
double x_max = 0; double y_max = 0;
for (size_t i = 0; i < cloud->points.size(); ++i)
{
if(cloud->points[i].x <= x_min) x_min = cloud->points[i].x;
if(cloud->points[i].y <= y_min) y_min = cloud->points[i].y;
if(cloud->points[i].x >= x_max) x_max = cloud->points[i].x;
if(cloud->points[i].y >= y_max) y_max = cloud->points[i].y;
}
// cout << (int)x_min << " , " << (int)y_min << endl;
// cout << (int)x_max + 1 << " , " << (int)y_max + 1 << endl;
/*设置格网参数*/
double pt0_x = (int)x_min; double pt0_y = (int)y_min;
double pt1_x = (int)x_max + 1; double pt1_y = (int)y_max + 1;
double grid_width = 0.05;
int grid_num = (int) (pt1_x - pt0_x) / grid_width * (pt1_y - pt0_y) / grid_width;
/*生成格网*/
double grid[grid_num][5];
for(int i = 0; i < grid_num; ++i)
{
int grid_num_y = (int) (pt1_y - pt0_y) / grid_width;
int grid_num_x_i = (int) (i / grid_num_y);
int grid_num_y_i = (int) (i - grid_num_x_i * grid_num_y);
double i_y_start = grid_num_y_i * grid_width + pt0_y;
double i_x_start = grid_num_x_i * grid_width + pt0_x;
std::vector<double> grid_pt_z;
int num_grid_pt_z = 0;
for (size_t j = 0; j < cloud->points.size(); ++j)
{
if(cloud->points[j].x <= i_x_start + grid_width && cloud->points[j].x >= i_x_start && cloud->points[j].y <= i_y_start + grid_width && cloud->points[j].y >= i_y_start)
{
grid_pt_z.push_back(cloud->points[j].z);
num_grid_pt_z ++;
}
}
if(num_grid_pt_z <= 1) continue;
double z_sum = 0; double z_min = 10000; double z_max = 0;
for(int k = 0; k < num_grid_pt_z; ++k)
{
z_sum = z_sum + grid_pt_z[k];
if(grid_pt_z[k] > z_max) z_max = grid_pt_z[k];
if(grid_pt_z[k] < z_min) z_min = grid_pt_z[k];
}
double z_avg = z_sum / num_grid_pt_z;
double z_sig_sum = 0;
for(int k2 = 0; k2 < num_grid_pt_z; ++k2) z_sig_sum = z_sig_sum + (grid_pt_z[k2] - z_avg) * (grid_pt_z[k2] - z_avg);
double z_sig = z_sig_sum / num_grid_pt_z;
grid[i][0] = num_grid_pt_z;
grid[i][1] = z_max;
grid[i][2] = z_min;
grid[i][3] = z_avg;
grid[i][4] = z_sig;
grid_pt_z.clear();
}
/*判断地面*/
pcl::PointCloud<pcl::PointXYZ> ground_cloud;
for(int i = 0; i < grid_num; ++i)
{
int grid_num_y = (int) (pt1_y - pt0_y) / grid_width;
if(i < grid_num_y) continue;
if(i > grid_num - grid_num_y) continue;
int mod_x = i - ((int) (i / grid_num_y)) * grid_num_y;
if(mod_x == 0 || mod_x == grid_num_y - 1) continue;
double z_avg_1 = grid[i][3];
double z_avg_2 = grid[i - 1][3];
double z_avg_3 = grid[i + 1][3];
double z_avg_4 = grid[i - grid_num_y][3];
double z_avg_5 = grid[i + grid_num_y][3];
if(abs(z_avg_1 - z_avg_2) <= 0.02 && abs(z_avg_1 - z_avg_3) <= 0.02 && abs(z_avg_1 - z_avg_4) <= 0.02 && abs(z_avg_1 - z_avg_5) <= 0.02)
{
int x_num_i = (int) i / grid_num_y;
int y_num_i = i - (int) i / grid_num_y * grid_num_y;
double x_start = x_num_i * grid_width + pt0_x;
double y_start = y_num_i * grid_width + pt0_y;
for (size_t j = 0; j < cloud->points.size(); ++j)
{
if(cloud->points[j].x <= x_start + grid_width && cloud->points[j].x >= x_start && cloud->points[j].y <= y_start + grid_width && cloud->points[j].y >= y_start) ground_cloud.push_back(cloud->points[j]);
}
}
}
/*存储与显示*/
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("cloud show"));
int v1 = 0;
viewer->createViewPort(0, 0, 1.0, 1.0, v1);
viewer->setBackgroundColor(255,255, 255, v1);
viewer->setBackgroundColor(255,255, 255);
pcl::PointCloud<pcl::PointXYZ>::Ptr g_cloud = ground_cloud.makeShared();
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> raw(cloud, 0, 0, 0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> g_pt(g_cloud, 255,0, 0);
viewer->addPointCloud(cloud, raw, "source cloud", v1);
viewer->addPointCloud(g_cloud, g_pt, "select ground point", v1);
// std::string path = "../ground.pcd";
// pcl::io::savePCDFile(path, *g_cloud);
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(10000));
}
system("pause");
return 0;
}
3.示例
(图中红色为地面点云)