学习PCl点云

1、点云格式转化:

 从.ASC 到 .pcd 直接在文件开头 加上pcd格式的文件头

# .PCD v0.7 - Point Cloud Data file format
VERSION 0.7
FIELDS x y z
SIZE 4 4 4
TYPE F F F
COUNT 1 1 1
WIDTH xxxxxx     // 实际的点云数
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS xxxxxxx     // 实际的点云数

DATA ascii 

...     .....    ......

2、利用RANSAC  平面拟合 

#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>


int
main(int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCDReader reader;
reader.read("3d_data5.pcd", *cloud);
//std::cout << "第一个点的X坐标是 " << cloud->points[1].x << std::endl;
std::cout << "PointCloud before filtering has: " << cloud->points.size() << " data points." << std::endl;


//// Fill in the cloud data
//cloud->width = 15;
//cloud->height = 1;
//cloud->points.resize(cloud->width * cloud->height);


//// Generate the data
//for (size_t i = 0; i < cloud->points.size(); ++i)
//{
// cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
// cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
// cloud->points[i].z = 1.0;
//}


//// Set a few outliers
//cloud->points[0].z = 2.0;
//cloud->points[3].z = -2.0;
//cloud->points[6].z = 4.0;


//std::cerr << "Point cloud data: " << cloud->points.size() << " points" << std::endl;
//for (size_t i = 0; i < cloud->points.size(); ++i)
// std::cerr << "    " << cloud->points[i].x << " "
// << cloud->points[i].y << " "
// << cloud->points[i].z << std::endl;


pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);      // 定义系数对象
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);     
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients(true);
// Mandatory
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.01);


seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficients);


if (inliers->indices.size() == 0)
{
PCL_ERROR("Could not estimate a planar model for the given dataset.");
return (-1);
}


std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
std::cout << "平面方程为:" << coefficients->values[0] <<"X +" << coefficients->values[1]<< "Y +" << coefficients->values[2]<<"Z +"
                        <<coefficients->values[3] <<"= 0 " << std::endl;


std::cerr << "Model inliers: " << inliers->indices.size() << std::endl;
/*for (size_t i = 0; i < inliers->indices.size(); ++i)
std::cerr << inliers->indices[i] << "    " << cloud->points[inliers->indices[i]].x << " "
<< cloud->points[inliers->indices[i]].y << " "
<< cloud->points[inliers->indices[i]].z << std::endl;
*/
return (0);
}

阅读更多

没有更多推荐了,返回首页