PCL-Cpp (70%)
์ฝ๋๋ [์ด๊ณณ]์์ ๋ค์ด๋ก๋ ๊ฐ๋ฅํฉ๋๋ค. ์ํํ์ผ์ [tabletop_passthrough.pcd]์ ์ฌ์ฉํ์์ต๋๋ค. ์๋ณธ ์ฝ๋๋ [์ด๊ณณ]์ ์ฐธ๊ณ ํ์์ต๋๋ค.
#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>
#include <pcl/filters/extract_indices.h>
//Plane model segmentation
//http://pointclouds.org/documentation/tutorials/planar_segmentation.php#planar-segmentation
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>),
cloud_p (new pcl::PointCloud<pcl::PointXYZRGB>),
inlierPoints (new pcl::PointCloud<pcl::PointXYZRGB>),
inlierPoints_neg (new pcl::PointCloud<pcl::PointXYZRGB>);
// *.PCD ํ์ผ ์ฝ๊ธฐ (https://raw.githubusercontent.com/adioshun/gitBook_Tutorial_PCL/master/Beginner/sample/tabletop_passthrough.pcd)
pcl::io::loadPCDFile<pcl::PointXYZRGB> ("tabletop_passthrough.pcd", *cloud);
// ํฌ์ธํธ์ ์ถ๋ ฅ
std::cout << "Loaded :" << cloud->width * cloud->height << std::endl;
// Object for storing the plane model coefficients.
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
// ์คํ์ ํธ ์์ฑ Create the segmentation object.
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
seg.setOptimizeCoefficients (true); //(์ต์
) // Enable model coefficient refinement (optional).
seg.setInputCloud (cloud); //์
๋ ฅ
seg.setModelType (pcl::SACMODEL_PLANE); //์ ์ฉ ๋ชจ๋ธ // Configure the object to look for a plane.
seg.setMethodType (pcl::SAC_RANSAC); //์ ์ฉ ๋ฐฉ๋ฒ // Use RANSAC method.
seg.setMaxIterations (1000); //์ต๋ ์คํ ์
seg.setDistanceThreshold (0.01); //inlier๋ก ์ฒ๋ฆฌํ ๊ฑฐ๋ฆฌ ์ ๋ณด // Set the maximum allowed distance to the model.
//seg.setRadiusLimits(0, 0.1); // cylinder๊ฒฝ์ฐ, Set minimum and maximum radii of the cylinder.
seg.segment (*inliers, *coefficients); //์ธ๊ทธ๋ฉํ
์ด์
์ ์ฉ
//์ถ์ ๋ ํ๋ฉด ํ๋ผ๋ฏธํฐ ์ถ๋ ฅ (eg. ax + by + cz + d = 0 ).
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
pcl::copyPointCloud<pcl::PointXYZRGB>(*cloud, *inliers, *inlierPoints);
pcl::io::savePCDFile<pcl::PointXYZRGB>("SACSegmentation_result.pcd", *inlierPoints);
//[์ต์
]] ๋ฐ๋ฅ ์ ๊ฑฐ ๊ฒฐ๊ณผ ์ป๊ธฐ
//Extracting indices from a PointCloud
//http://pointclouds.org/documentation/tutorials/extract_indices.php
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud (cloud);
extract.setIndices (inliers);
extract.setNegative (true);//false
extract.filter (*inlierPoints_neg);
pcl::io::savePCDFile<pcl::PointXYZRGB>("SACSegmentation_result_neg.pcd", *inlierPoints_neg);
return (0);
}
์คํ ๊ฒฐ๊ณผ
Loaded :72823
Model coefficients: 3.88368e-05 0.000606678 1 -0.773654
์๊ฐํ & ๊ฒฐ๊ณผ
$ pcl_viewer tabletop_passthrough.pcd
$ pcl_viewer SACSegmentation_result.pcd
$ pcl_viewer SACSegmentation_result_neg.pcd
์๋ณธtabletop_passthrough.pcd
๊ฒฐ๊ณผsetNegative (false)
๊ฒฐ๊ณผ setNegative (true)
Last updated
Was this helpful?