一、简介
深度图中由于高度起伏比较小,导致在找边的时候很不好找,所以可以使用halcon 中的法向量来找边。
二、Halcon代码
read_image (Sta5001, 'C:/Users/alber/Desktop/R1/2023-02-24_/Sta9_001.tif')
zoom_image_factor (Sta9_001, ImageZoomed, 1, 0.01/0.0025, 'nearest_neighbor')
threshold (ImageZoomed, Region, -3, 255)
reduce_domain (ImageZoomed, Region, ImageReduced)
crop_domain (ImageReduced, ImagePart)
get_image_size (ImagePart, Width, Height)
get_region_points (ImagePart, Rows, Columns)
get_grayval (ImagePart, Rows, Columns, Grayval)
gen_object_model_3d_from_points (Columns*0.0025, Rows*0.0025, Grayval, ObjectModel3D)
surface_normals_object_model_3d (ObjectModel3D, 'mls', [], [], ObjectModel3DNormals)
get_object_model_3d_params (ObjectModel3DNormals, 'point_normal_x', GenParamValueX)
get_object_model_3d_params (ObjectModel3DNormals, 'point_normal_y', GenParamValueY)
get_object_model_3d_params (ObjectModel3DNormals, 'point_normal_z', GenParamValueZ)
gen_image_const (Image, 'real', Width, Height)
copy_image (Image, DupImageY)
copy_image (Image, DupImageZ)
set_grayval (Image, Rows, Columns, GenParamValueX)
* 开始找边
* auto_threshold (Image, Regions, 0.14)
* opening_circle (Regions, RegionOpening, 5.5)
* connection (RegionOpening, ConnectedRegions)
* select_shape (ConnectedRegions, SelectedRegions, 'area', 'and', 10000, 109508)
* dilation_circle (SelectedRegions, RegionDilation, 18)
* gen_contour_region_xld (RegionDilation, Contours, 'border')
* segment_contours_xld (Contours, ContoursSplit, 'lines_circles', 5, 4, 2)
set_grayval (DupImageY, Rows, Columns, GenParamValueY)
set_grayval (DupImageZ, Rows, Columns, GenParamValueZ)
add_image (Image, DupImageY, ImageResult, 1, 0)
add_image (ImageResult, DupImageZ, ImageResult1,1, 0)
compose3 (Image, DupImageY, DupImageZ, MultiChannelImage)
三、PCL代码
由于项目需求是找一条定位的边,用tif图转来转去很麻烦,所以我这里采用的思路是计算点云的法向量,然后分别计算这个法向量与水平方向和垂直方向的夹角,刷选出点云上面的点,拟合直线,计算点云的质心,反接出那条直线X方向上的位置,开始定位。
步骤:
1、计算整个点云的法向量
2、通过法向量的夹角计算水平和竖直方向的夹角
3、拟合直线
4、计算质心
5、反接X的位置
pcl 拟合直线的参数解析:
double FindEdgeNormal(PointCloudT::Ptr cloud, float radius)
{
if (cloud->size() < 2000)
{
cout << "FindEdgeNormal the point is empty" << endl;
}
CG_Line line;
//pcl::cop
// 统计滤波,去除利群的点
SORFilter(cloud, cloud, 3);
int a = cloud->size();
cout << a << endl;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_line(new pcl::PointCloud<pcl::PointXYZRGB>);
//计算法向
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
ne.setInputCloud(cloud);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>());
ne.setSearchMethod(tree);
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
double normal_radius = 0.01; //参数,法向搜索半径//
ne.setRadiusSearch(normal_radius);
ne.compute(*normals);
cout << "法向量个数:" << normals->size() << endl;
//遍历所有的点
// 定义两个向量
Eigen::Vector3f p_horizontal(0, 0, 1);
Eigen::Vector3f p_vertical(1, 0, 0);
//Eigen::Vector3cf p_normal;
// 遍历整个点云,求出
PointCloudT point_t;
for (int i = 0; i < cloud->size(); i++)
{
float normal_x = normals->points[i].normal_x;
float normal_y = normals->points[i].normal_y;
float normal_z = normals->points[i].normal_z;
Eigen::Vector3f p_normal(normal_x, normal_y, normal_z);
//计算 夹角
p_normal.normalize();
float p_angle = pcl::getAngle3D(p_normal, p_vertical, true);
float p_angle2 = pcl::getAngle3D(p_normal, p_horizontal, true);
if (p_angle>50 && p_angle<70 && p_angle2>20 && p_angle2<50)
{
cloud->points[i].r = 255;
cloud->points[i].g = 0;
cloud->points[i].b = 0;
cloud_line->push_back(cloud->points[i]);
}
}
// 保存点云数据
pcl::io::savePCDFileBinaryCompressed("./Zudingwei.pcd", *cloud);
if (cloud_line->size()<100)
{
cout << "FindEdgeNormal the point is empty" << endl;
}
// 拟合直线
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices); //inliers表示误差能容忍的点 记录的是点云的序号
pcl::SACSegmentation<pcl::PointXYZRGB>seg;
seg.setOptimizeCoefficients(true); // // Optional,这个设置可以选定结果平面展示的点是分割掉的点还是分割剩下的点。
seg.setInputCloud(cloud_line);
seg.setDistanceThreshold(0.01);
seg.setModelType(pcl::SACMODEL_LINE); // Mandatory-设置目标几何形状
seg.setMethodType(pcl::SAC_RANSAC); // 随机一致采样的方法
seg.segment(*inliers, *coefficients);
// 打印出直线的方程参数
//打印直线方程
std::cout << "a:" << coefficients->values[0] << endl;
std::cout << "b:" << coefficients->values[1] << endl;
std::cout << "c:" << coefficients->values[2] << endl;
std::cout << "d:" << coefficients->values[3] << endl;
std::cout << "e:" << coefficients->values[4] << endl;
std::cout << "f:" << coefficients->values[5] << endl;
// 计算点云的重心
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*cloud_line, centroid);
std::cout << "质心: " << centroid(0) << endl;
std::cout << "质心: " << centroid(1) << endl;
std::cout << "质心: " << centroid(2) << endl;
// 用质心反接出X
double x = (centroid(1) - coefficients->values[1])* coefficients->values[3] / coefficients->values[4] + coefficients->values[0];
return x;
}