halcon and PCL normal vector edge finding

1. Introduction

Due to the relatively small height fluctuation in the depth map, it is very difficult to find edges, so you can use the normal vector in halcon to find edges.

2. Halcon code

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)

 3. PCL code

Since the project requirement is to find a positioning edge, it is very troublesome to turn around with the tif image, so the idea I adopt here is to calculate the normal vector of the point cloud, and then calculate the angle between the normal vector and the horizontal direction and the vertical direction respectively. Brush to select the points on the point cloud, fit a straight line, calculate the centroid of the point cloud, reversely connect the position in the X direction of the straight line, and start positioning.

step:

1. Calculate the normal vector of the entire point cloud

2. Calculate the angle between the horizontal and vertical directions through the angle of the normal vector

3. Fitting a straight line

4. Calculate the centroid

5. Reverse the position of X

Parameter analysis of pcl fitting straight line:

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;
}