先将点云投影到二维网格上,每个网格存有该网格对应的所有点的信息,选取密度大于阈值的网格,取其及其邻域内所有点,用pcl中RANSAC算法拟合圆柱体,提取出内点并输出。
代码如下:
#include <iostream>
#include <fstream>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <math.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d.h>
using namespace std;
float length = 0.3;//每个栅格的边长
int min_density = 200;//候选网格中最少点数
int min_points = 400;//ransac后,内点最少数
class xyz {
public:
float x;
float y;
float z;
xyz(float a, float b, float c) {
x = a;
y = b;
z = c;
}
};
class mesh
{
public:
float high = -100;
float low = 100;
int density = 0;//存储网格密度,这里是该网格中点的数量
int state = 0;//0表示不是候选区(圆柱体处),1表示是
vector<xyz> point;
mesh() {}
};
int main(int argc, char** argv) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::Poin