pcl :: RANSACセグメンテーション、クラウド内のすべてのプレーンを取得しますか？
点群で最大の平面を検出する点群ライブラリ関数があります。これはうまくいきます。次に、この機能を拡張して、クラウド内のすべての平面をセグメント化し、それらのポイントを新しいクラウドにコピーします（たとえば、部屋の床に球があるシーンでは、床と壁が返されますが、平面ではないため、球ではありません）。以下のコードを拡張して、最大の飛行機だけでなく、すべての飛行機を取得するにはどうすればよいですか？ （ここではランタイムが要因なので、この同じコードをループで実行するのではなく、毎回新しい最大のプレーンを取り除きます）
ありがとうございました。
int
main(int argc, char** argv)
{
pcl::visualization::CloudViewer viewer("viewer1");
pcl::PCLPointCloud2::Ptr cloud_blob(new pcl::PCLPointCloud2), cloud_filtered_blob(new pcl::PCLPointCloud2);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>), cloud_p(new pcl::PointCloud<pcl::PointXYZ>), cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
// Fill in the cloud data
pcl::PCDReader reader;
reader.read("clouds/table.pcd", *cloud_blob);
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud(cloud_blob);
sor.setLeafSize(0.01f, 0.01f, 0.01f);
sor.filter(*cloud_filtered_blob);
// Convert to the templated PointCloud
pcl::fromPCLPointCloud2(*cloud_filtered_blob, *cloud_filtered);
std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << 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);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(1000);
seg.setDistanceThreshold(0.01);
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZ> extract;
int i = 0, nr_points = (int)cloud_filtered->points.size();
// While 30% of the original cloud is still there
while (cloud_filtered->points.size() > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud(cloud_filtered);
pcl::ScopeTime scopeTime("Test loop");
{
seg.segment(*inliers, *coefficients);
}
if (inliers->indices.size() == 0)
{
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the inliers
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers);
extract.setNegative(false);
extract.filter(*cloud_p);
std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl;
}
viewer.showCloud(cloud_p, "viewer1");
while (!viewer.wasStopped()) {}
return (0);
}
最初の平面を取得したら、ポイントを削除し、アルゴリズムを使用して、推定された平面の残りのポイントがなくなるまで、新しい平面を計算します。 2番目のケースは、RANSACを使用すると、十分なポイントがある限り、常に平面が見つかるためです。私はここで同様のことをしています（これはrosノードのコールバックです）：
void pointCloudCb(const sensor_msgs::PointCloud2::ConstPtr &msg){
// Convert to pcl point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_msg (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*msg,*cloud_msg);
ROS_DEBUG("%s: new ponitcloud (%i,%i)(%zu)",_name.c_str(),cloud_msg->width,cloud_msg->height,cloud_msg->size());
// Filter cloud
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(cloud_msg);
pass.setFilterFieldName ("z");
pass.setFilterLimits(0.001,10000);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pass.filter (*cloud);
// Get segmentation ready
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::ExtractIndices<pcl::PointXYZ> extract;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold(_max_distance);
// Create pointcloud to publish inliers
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_pub(new pcl::PointCloud<pcl::PointXYZRGB>);
int original_size(cloud->height*cloud->width);
int n_planes(0);
while (cloud->height*cloud->width>original_size*_min_percentage/100){
// Fit a plane
seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficients);
// Check result
if (inliers->indices.size() == 0)
break;
// Iterate inliers
double mean_error(0);
double max_error(0);
double min_error(100000);
std::vector<double> err;
for (int i=0;i<inliers->indices.size();i++){
// Get Point
pcl::PointXYZ pt = cloud->points[inliers->indices[i]];
// Compute distance
double d = point2planedistnace(pt,coefficients)*1000;// mm
err.Push_back(d);
// Update statistics
mean_error += d;
if (d>max_error) max_error = d;
if (d<min_error) min_error = d;
}
mean_error/=inliers->indices.size();
// Compute Standard deviation
ColorMap cm(min_error,max_error);
double sigma(0);
for (int i=0;i<inliers->indices.size();i++){
sigma += pow(err[i] - mean_error,2);
// Get Point
pcl::PointXYZ pt = cloud->points[inliers->indices[i]];
// Copy point to noew cloud
pcl::PointXYZRGB pt_color;
pt_color.x = pt.x;
pt_color.y = pt.y;
pt_color.z = pt.z;
uint32_t rgb;
if (_color_pc_with_error)
rgb = cm.getColor(err[i]);
else
rgb = colors[n_planes].getColor();
pt_color.rgb = *reinterpret_cast<float*>(&rgb);
cloud_pub->points.Push_back(pt_color);
}
sigma = sqrt(sigma/inliers->indices.size());
// Extract inliers
extract.setInputCloud(cloud);
extract.setIndices(inliers);
extract.setNegative(true);
pcl::PointCloud<pcl::PointXYZ> cloudF;
extract.filter(cloudF);
cloud->swap(cloudF);
// Display infor
ROS_INFO("%s: fitted plane %i: %fx%s%fy%s%fz%s%f=0 (inliers: %zu/%i)",
_name.c_str(),n_planes,
coefficients->values[0],(coefficients->values[1]>=0?"+":""),
coefficients->values[1],(coefficients->values[2]>=0?"+":""),
coefficients->values[2],(coefficients->values[3]>=0?"+":""),
coefficients->values[3],
inliers->indices.size(),original_size);
ROS_INFO("%s: mean error: %f(mm), standard deviation: %f (mm), max error: %f(mm)",_name.c_str(),mean_error,sigma,max_error);
ROS_INFO("%s: poitns left in cloud %i",_name.c_str(),cloud->width*cloud->height);
// Nest iteration
n_planes++;
}
// Publish points
sensor_msgs::PointCloud2 cloud_publish;
pcl::toROSMsg(*cloud_pub,cloud_publish);
cloud_publish.header = msg->header;
_pub_inliers.publish(cloud_publish);
}
あなたはノード全体を見つけることができます ここ
この目的のために、私はこのメソッドを作成しました：
1）最初の飛行機を探す
2）平面内のポイントに近い、ポイントクラウド内のすべてのポイントを取得します
3）線形回帰を使用して、この点を平面で近似します
4）新しい平面が小さすぎなくなるまで1〜3を繰り返します