halcon and PCL 法向量找边

一、简介

深度图中由于高度起伏比较小，导致在找边的时候很不好找，所以可以使用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;
}