效果如图:
环境如下:
c++
pcl:1.81
opencv:3.2
float hight_min=-0.1;
float hight_max=0.3;
float SAC_max_error=0.2;
int line_points_num_min=50;
float down_size_leaf=0.05;
float line_distance_max=2.0;
float line_angle_max_error = 0.01;
bool get_zxx(PointCloudType_::Ptr cloud, PointCloudType_::Ptr out ){
PointCloudType_ cloud_out_line;
PointCloudType_ cloud_in;
std::vector<PointCloudType_> pc_line;
int point_num = cloud->size();
for(int i=0;i<point_num;i++){
PointType_ added_pt;
if(cloud->points[i].z<hight_min||cloud->points[i].z>hight_max)
continue;
added_pt.x = cloud->points[i].x;
added_pt.y = cloud->points[i].y;
added_pt.z = 0;
cloud_in.push_back(added_pt);
}
pcl::VoxelGrid<PointType_> sor;
sor.setInputCloud(cloud_in.makeShared());
sor.setLeafSize(down_size_leaf, down_size_leaf, down_size_leaf);
sor.filter(cloud_in);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
pcl::SACSegmentation<PointType_> seg; // 创建拟合对象
seg.setOptimizeCoefficients(true); // 设置对估计模型参数进行优化处理
seg.setModelType(pcl::SACMODEL_LINE); // 设置拟合模型为直线模型
seg.setMethodType(pcl::SAC_RANSAC); // 设置拟合方法为RANSAC
seg.setMaxIterations(100); // 设置最大迭代次数
seg.setDistanceThreshold(SAC_max_error); // 判断是否为模型内点的距离阀值/设置误差容忍范围
point_num = cloud_in.size();
int k=0;
while (k < 2 && cloud_in.points.size() > 0.2 * point_num){
pcl::ModelCoefficients coefficients;
seg.setInputCloud(cloud_in.makeShared()); // 输入点云
seg.segment(*inliers, coefficients); // 内点的索引,模型系数
PointCloudType_::Ptr cloud_line(new PointCloudType_);
PointCloudType_::Ptr outside(new PointCloudType_);
if (inliers->indices.size() > line_points_num_min) // 判断提取直线上的点数是否小于20个点,小于的话该直线不合格
{
// lineCoff.push_back(coefficients); // 将参数保存进vector中
pcl::ExtractIndices<PointType_> extract; // 创建点云提取对象
extract.setInputCloud(cloud_in.makeShared());
extract.setIndices(inliers);
extract.setNegative(false); // 设置为false,表示提取内点
extract.filter(*cloud_line);
extract.setNegative(true); // true提取外点(该直线之外的点)
extract.filter(*outside); // outside为外点点云
cloud_in.makeShared() .swap(outside); // 将cloud_f中的点云赋值给cloud
line_point[k]=cloud_line->size();
pc_line.push_back(*cloud_line);
}
else
{
break;
}
k++;
}
if(pc_line.size()!=2){
pc_line.clear();
return 0;
}
int i=0;
float k_[2];
float dis[2];
cv::Mat matA1(2, 2, CV_32F, cv::Scalar::all(0));
cv::Mat matD1(1, 2, CV_32F, cv::Scalar::all(0));
cv::Mat matV1(2, 2, CV_32F, cv::Scalar::all(0));
int index=0;
float x[2];
float y[2];
for(i=0;i<pc_line.size();i++){
float cx = 0, cy = 0, cz = 0;
int num = pc_line[i].size();
for(int j=0;j<num;j++){
cx += pc_line[i].points[j].x;
cy += pc_line[i].points[j].y;
}
if(pc_line.size()==0) return 0;
cx/=num;
cy/=num;
float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
for (int j = 0; j < num; j++) {
float ax = pc_line[i].points[j].x - cx;
float ay = pc_line[i].points[j].y - cy;
a11 += ax * ax;
a12 += ax * ay;
a22 += ay * ay;
}
a11/=num;
a12/=num;
a22/=num;
matA1.at<float>(0, 0) = a11; matA1.at<float>(0, 1) = a12;
matA1.at<float>(1, 0) = a12; matA1.at<float>(1, 1) = a22;
cv::eigen(matA1, matD1, matV1);
if (matD1.at<float>(0, 0) < 3 * matD1.at<float>(0, 1)) {
return 0;
}
float x1 = cx + 0.1 * matV1.at<float>(0, 0);
float y1 = cy + 0.1 * matV1.at<float>(0, 1);
float x2 = cx - 0.1 * matV1.at<float>(0, 0);
float y2 = cy - 0.1 * matV1.at<float>(0, 1);
float x0=0;
float y0=0;
float a012 = sqrt(abs(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)))) ;
float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2));
float ld2 = a012 / l12;
if(ld2>line_distance_max)continue;
k_[index] = (y2-y1)/(x2-x1);
dis[index] = ld2;
x[index] = cx;
y[index] = cy;
index++;
}
pc_line.clear();
if(abs(atan(k_[0])-atan(k_[1]))>line_angle_max_error)
return 0;
float angle=(atan(k_[0])+atan(k_[1]))/2;
float y_ =(dis[1]-dis[0])*sin(angle);
float x_ = (dis[1]-dis[0])*cos(angle);
for(i=-100;i<100;i++){
PointType_ added_pt;
added_pt.x = x_+ 0.1*i*cos(angle);
added_pt.y = y_+ 0.1*i*sin(angle);
added_pt.z = 0;
out->push_back(added_pt);
}
return 1;
}