Lidar series articles
Sensor fusion is to fuse the data collected by multiple sensors to better perceive the surrounding environment; Firstly, the related contents of lidar are introduced, including the basic introduction of lidar (this section), laser point cloud data processing methods (point cloud data display, point cloud segmentation, point cloud clustering, obstacle recognition examples), etc.
Catalogue of series articles
1. Basic introduction of lidar
2. Laser point cloud data display
3. Laser point cloud segmentation based on RANSAC
Plane model segmentation based on PCL
This section mainly introduces how to do a simple plane segmentation for a group of point cloud data.
#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_ consensus/method_ types. h> / / random parameter estimation method header file
#include <pcl/sample_ consensus/model_ types. h> / / model definition header file
#include <pcl/segmentation/sac_ segmentation. h> / / header files of classes divided based on sampling consistency
int main (int argc, char** argv)
{
//1. Create point cloud data containing external points, and then print the coordinate values of these points on the screen.
pcl::PointCloud<pcl::PointXYZ> cloud;
//Fill in the point cloud data. Set the width of the point cloud to 15 and the height to 1, that is, it is an unordered point cloud
cloud.width = 15;
cloud.height = 1;
cloud.points.resize (cloud.width * cloud.height);
//Generate data and fill the x and y coordinates of the point cloud with random numbers, but they are all on the plane of z=1.0
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 several external points, that is, reset the z values of several points to make them deviate from the plane with z=1.0
cloud.points[0].z = 2.0;
cloud.points[3].z = -2.0;
cloud.points[6].z = 4.0;
//The coordinate values of each point in the point cloud are printed on the standard output to facilitate the reference after segmentation.
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;
//2. Create a random sampling consistency segmentation object, set the model type and random sampling consistency method type, and set the "distance threshold"
//Create the model coefficient object coefficients required for segmentation, and the point index collection object inliers for storing inner points.
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
//Create split objects
pcl::SACSegmentation<pcl::PointXYZ> seg;
//Optional configuration parameters. Setting model coefficients needs to be optimized
seg.setOptimizeCoefficients (true);
//Parameters must be configured to set the model type of segmentation, the random parameter estimation method used, the distance threshold and the input point cloud
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01); //The distance threshold determines the conditions that must be met when a point is considered as an internal point, which represents the maximum distance from the point to the estimation model
seg.setInputCloud (cloud.makeShared ());
//The segmentation is realized, and the segmentation results are stored to the point set inliers and the coefficients of the plane model
seg.segment (*inliers, *coefficients);
//3. Print out the estimated plane model parameters (in the form of ax+by+cz+d=0)
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;
//4. Print out the number of points and coordinates of local points
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);
}
The compilation and operation results are as follows. It can be seen that all internal points are extracted through point cloud segmentation, and the external points with z ≠ 1 are filtered out.
./planar_segmentation
Point cloud data: 15 points
0.352222 -0.151883 2
-0.106395 -0.397406 1
-0.473106 0.292602 1
-0.731898 0.667105 -2
0.441304 -0.734766 1
0.854581 -0.0361733 1
-0.4607 -0.277468 4
-0.916762 0.183749 1
0.968809 0.512055 1
-0.998983 -0.463871 1
0.691785 0.716053 1
0.525135 -0.523004 1
0.439387 0.56706 1
0.905417 -0.579787 1
0.898706 -0.504929 1
Model coefficients: 0 0 1 -1
Model inliers: 12
1 -0.106395 -0.397406 1
2 -0.473106 0.292602 1
4 0.441304 -0.734766 1
5 0.854581 -0.0361733 1
7 -0.916762 0.183749 1
8 0.968809 0.512055 1
9 -0.998983 -0.463871 1
10 0.691785 0.716053 1
11 0.525135 -0.523004 1
12 0.439387 0.56706 1
13 0.905417 -0.579787 1
14 0.898706 -0.504929 1
Cylinder model segmentation based on PCL
This section mainly introduces how to extract a cylinder model from a noisy point cloud using random sampling consistency estimation.
The processing flow of the whole program is as follows:
(1) Filter out data points farther than 1.5m;
(2) Estimate the surface normal of each point;
(3) Split the plane model and save it;
(4) Split the cylinder model and save it.
The point cloud data used in this paper are as follows:
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
typedef pcl::PointXYZ PointT;
int
main (int argc, char** argv)
{
// Read in the cloud data enables reading point cloud data from files
pcl::PCDReader reader; //pcd file read object
pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>); //Instantiate point cloud objects
reader.read ("../table_scene_mug_stereo_textured.pcd", *cloud);
std::cerr << "PointCloud has: " << cloud->points.size () << " data points." << std::endl;
// Build a passthrough filter to remove spurious NaNs
//Conduct through filtering, filter out the points whose z-axis value is not within the range of (0,1.5), and store the remaining points in cloud_ In the filtered object
pcl::PassThrough<PointT> pass; //Define pass through filter object
pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 1.5);
pass.filter (*cloud_filtered);
std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl;
// Estimate point normals
pcl::NormalEstimation<PointT, pcl::Normal> ne; //Define normal estimation object
pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>); //Point cloud normal
ne.setSearchMethod (tree);
ne.setInputCloud (cloud_filtered);
ne.setKSearch (50);
ne.compute (*cloud_normals);
//Segment the point cloud
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg; //Split object
pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients), coefficients_cylinder (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_cylinder (new pcl::PointIndices);
//Set the model type, method and related parameters used for segmentation
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
seg.setNormalDistanceWeight (0.1);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.03);
seg.setInputCloud (cloud_filtered);
seg.setInputNormals (cloud_normals);
// Obtain the plane inliers and coefficients
//Perform segmentation according to the above input parameters to obtain the plane model coefficients and the interior points on the plane
seg.segment (*inliers_plane, *coefficients_plane);
std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;
// Extract the planar inliers from the input cloud
//Extract the point set on the plane after segmentation from the point cloud
pcl::ExtractIndices<PointT> extract; //Point extraction object
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers_plane);
extract.setNegative (false);
// Write the planar inliers to disk
pcl::PointCloud<PointT>::Ptr cloud_plane (new pcl::PointCloud<PointT> ());
extract.filter (*cloud_plane);
std::cerr << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
//Store the point-to-point cloud file on the divided plane
pcl::PCDWriter writer; //Define pcd file write object
writer.write ("table_scene_mug_stereo_textured_plane.pcd", *cloud_plane, false);
// Remove the planar inliers, extract the rest
pcl::ExtractIndices<pcl::Normal> extract_normals; //Point extraction object
pcl::PointCloud<PointT>::Ptr cloud_filtered2 (new pcl::PointCloud<PointT>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
extract.setNegative (true);
extract.filter (*cloud_filtered2);
extract_normals.setNegative (true);
extract_normals.setInputCloud (cloud_normals);
extract_normals.setIndices (inliers_plane);
extract_normals.filter (*cloud_normals2);
//Create a split object for the cylinder split,
// Create the segmentation object for cylinder segmentation and set all the parameters
seg.setOptimizeCoefficients (true); //Setting the estimated model coefficients needs to be optimized
seg.setModelType (pcl::SACMODEL_CYLINDER); //Set the split model to cylinder
seg.setMethodType (pcl::SAC_RANSAC); //Set the parameter estimation method using RANSAC as the algorithm
seg.setNormalDistanceWeight (0.1); //Sets the surface normal weight factor
seg.setMaxIterations (10000); //Set the maximum number of iterations to 1000
seg.setDistanceThreshold (0.05); //Sets the maximum allowable distance from the interior point to the model
seg.setRadiusLimits (0, 0.1); //Set the radius range of the estimated cylinder model
seg.setInputCloud (cloud_filtered2);
seg.setInputNormals (cloud_normals2);
// Obtain the cylinder inliers and coefficients
seg.segment (*inliers_cylinder, *coefficients_cylinder);
std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;
// Write the cylinder inliers to disk
extract.setInputCloud (cloud_filtered2);
extract.setIndices (inliers_cylinder);
extract.setNegative (false);
pcl::PointCloud<PointT>::Ptr cloud_cylinder (new pcl::PointCloud<PointT> ());
extract.filter (*cloud_cylinder);
if (cloud_cylinder->points.empty ())
std::cerr << "Can't find the cylindrical component." << std::endl;
else
{
std::cerr << "PointCloud representing the cylindrical component: " << cloud_cylinder->points.size () << " data points." << std::endl;
writer.write ("table_scene_mug_stereo_textured_cylinder.pcd", *cloud_cylinder, false);
}
return (0);
}
In the following cylinder segmentation, RANSAC is used to obtain the cylinder model coefficients randomly, and the consistency robust estimation is used to limit each local point to the model threshold DistanceThreshold = 0.05m, set the influence weight of the surface normal NormalDistanceWeight = 0.1, and limit the radius range of the cylinder model RadiusLimits = 0~0.1, After estimating the coefficients of the cylindrical model and the corresponding interior points, the points are stored in the point cloud file.
The results of compiling and running the program are as follows:
./cylinder_segmentation PointCloud has: 307200 data points. PointCloud after filtering has: 139897 data points. Plane coefficients: header: seq: 0 stamp: 0 frame_id: values[] values[0]: 0.0161902 values[1]: -0.837667 values[2]: -0.545941 values[3]: 0.528862 PointCloud representing the planar component: 116300 data points. Cylinder coefficients: header: seq: 0 stamp: 0 frame_id: values[] values[0]: 0.0543319 values[1]: 0.100139 values[2]: 0.787577 values[3]: -0.0135876 values[4]: 0.834831 values[5]: 0.550338 values[6]: 0.0387446 PointCloud representing the cylindrical component: 11462 data points.
The split plane is as follows:
The split cylinder is as follows:
Resource link: Entry level instance learning of point cloud segmentation
reference resources:
Chapter 12 entry level example analysis of point cloud library PCL from entry to mastery.