v0.1.10 优化抓取位置识别（夹角最小点作为抓取点）;识别2个有效苗才返回0，否则返回-1，异常返回1

ReadMe.txt

@@ -7,4 +7,5 @@ v0.1.5 支
 v0.1.6 去除支持grab图片上下镜像检测，因为夹爪旋转角度只有180度范围
 v0.1.7 恢复支持grab图片上下镜像检测，增加抓取最小间隔设置
 v0.1.8 改用传统方法获取抓取位置
-v0.1.9 增加面积大小过滤（通过面积占比系数）
+v0.1.9 增加面积大小过滤（通过面积占比系数）
+v0.1.10 优化抓取位置识别（夹角最小点作为抓取点）

tcv_conf.yml

@@ -1,7 +1,7 @@
 %YAML:1.0
 ---
 conf_parameters:
-   image_show: 1
+   image_show: 0
    image_return: 1
    image_save: 1
    image_depository: "D:\\logs\\algo_img"

tea_cv_api.cpp

@@ -18,7 +18,7 @@ extern CRITICAL_SECTION g_cs;
 namespace graft_cv
 {
 
-	char *g_version_str = "0.1.9";
+	char *g_version_str = "0.1.10";
 
 	//configure
 	string g_conf_file = "./tcv_conf.yml";	

tea_sorter.cpp

@@ -321,7 +321,7 @@ int CTeaSort::detect(
 		}
 	}
 	//拍照无苗, 返回识别结果-1
-	if (valid_cnt == 0) { return -1; }
+	if (valid_cnt != 2) { return -1; }
 	return 0;
 }
 
@@ -553,6 +553,7 @@ void CTeaSort::calculate_stem_grab_position(
 	int y3 = int(b.ppoint[5]);	
 	int y5 = int(b.ppoint[9]);
 	cv::Point p3(int(b.ppoint[4] - b.x1), int(b.ppoint[5] - b.y1));
+	cv::Point p4(int(b.ppoint[6] - b.x1), int(b.ppoint[7] - b.y1));
 	cv::Point p5(int(b.ppoint[8] - b.x1), int(b.ppoint[9] - b.y1));
 	cv::Mat crop_img;
 	if (y5 > y3) {
@@ -565,13 +566,16 @@ void CTeaSort::calculate_stem_grab_position(
 			padding = b.y1;
 		}
 		p5.y = int(b.ppoint[9] - b.y1 + padding);
+		p4.y = int(b.ppoint[7] - b.y1 + padding);
 		p3.y = int(b.ppoint[5] - b.y1 + padding);
 		crop_img = m_raw_img(cv::Range(b.y1 - padding, b.y2), cv::Range(b.x1, b.x2)).clone();
 		
 	}
 	if (m_cp.image_show) {
 		cv::Mat crop_img_tmp = crop_img.clone();
-		cv::circle(crop_img_tmp, p5, 4, cv::Scalar(255, 0, 255), -1, 3, 0);
+		cv::circle(crop_img_tmp, p3, 4, cv::Scalar(255, 0, 0), -1, 3, 0);
+		cv::circle(crop_img_tmp, p4, 4, cv::Scalar(0, 255, 0), -1, 3, 0);
+		cv::circle(crop_img_tmp, p5, 4, cv::Scalar(0, 0, 255), -1, 3, 0);
 
 		imshow_wait("cropped box", crop_img_tmp);
 	}
@@ -597,14 +601,21 @@ void CTeaSort::calculate_stem_grab_position(
 		imshow_wait("skeleton img", ske_img);
 	}*/
 
-	//遍历所有点，找到距离等于指定距离的点的位置
+	//遍历所有点，找到距离等于指定距离的点的位置, 以及距离p5最近的骨架上的点
 	std::vector<cv::Point> candidate_pts;
+	cv::Point p5_nearst;
 	double dist_th = 5;
+	double dist_min = 1.0e6;
 	for (int r = 0; r < ske_img.rows; ++r) {
 		for (int c = 0; c < ske_img.cols; ++c) {
 			if (ske_img.at<unsigned char>(r, c) == 0) { continue; }
 			double dist = std::powf((p5.x - c), 2) + std::powf((p5.y - r),2);
 			dist = std::sqrtf(dist);
+			if (dist < dist_min) {
+				dist_min = dist;
+				p5_nearst.x = c;
+				p5_nearst.y = r;
+			}
 			if (std::fabs(dist - m_cp.offset_grab) < dist_th) {
 				candidate_pts.push_back(cv::Point(c, r));
 			}
@@ -614,6 +625,8 @@ void CTeaSort::calculate_stem_grab_position(
 	//按与参考角度的差，找到有效的候选点集合
 	std::vector<cv::Point> valid_candidate_pts;
 	double ref_angle = atan2(p5.x - p3.x, p5.y - p3.y);
+	cv::Point p_min_angle(-1,-1);
+	double min_angle = CV_PI;
 	for (auto&p : candidate_pts) {
 		double angle_to_p3 = atan2(p.x - p3.x, p.y - p3.y);
 		//计算夹角
@@ -636,31 +649,18 @@ void CTeaSort::calculate_stem_grab_position(
 			}
 		}
 		if (fabs_angle > CV_PI / 4.0) { continue; }
+		if (fabs_angle < min_angle) {
+			min_angle = fabs_angle;
+			p_min_angle.x = p.x;
+			p_min_angle.y = p.y;
+		}
 		valid_candidate_pts.push_back(p);
 	}	
-
-	// 找到离重心最近的点作为抓取点
-	if (valid_candidate_pts.size() > 0) {
-		cv::Point2f p_mu(0,0);
-		for (auto&p : valid_candidate_pts) {
-			p_mu.x += p.x;
-			p_mu.y += p.y;
-		}
-		p_mu.x /= (float)(valid_candidate_pts.size());
-		p_mu.y /= (float)(valid_candidate_pts.size());
-
-		double min_dist = 1.0e8;
-		for (auto&p : valid_candidate_pts) {
-			double dist = std::powf((p.x - p_mu.x), 2) + std::powf((p.y - p_mu.y), 2);
-			dist = std::sqrtf(dist);
-			if (dist < min_dist) {
-				min_dist = dist;
-				grab_x = p.x;
-				grab_y = p.y;
-			}
-		}
-
+	if (p_min_angle.x>0 && p_min_angle.y>0) {
+		grab_x = p_min_angle.x;
+		grab_y = p_min_angle.y;
 	}
+
 	if (m_cp.image_show) {
 		cv::Mat ske_img_tmp = ske_img.clone();
 		for (auto&p : valid_candidate_pts) {
@@ -796,4 +796,146 @@ void CTeaSort::thinning(const cv::Mat& src, cv::Mat& dst)
 
 	dst *= 255;
 }
+
+
+/**
+* 
+*/
+void CTeaSort::get_grab_position(
+	const cv::Mat& skele_img, 
+	cv::Point&vertex, 
+	double&ref_angle
+)
+{
+	cv::Point pt0, pt1;
+	calc_bottom_vertex(vertex, ref_angle, pt0, pt1);
+	std::vector<cv::Point> triangle_region;
+	triangle_region.push_back(vertex);
+	triangle_region.push_back(pt0);
+	triangle_region.push_back(pt1);
+	
+	
+	//构建多边形，然后判别骨架图中在多边形内的骨架像素
+	std::vector<cv::Point> curve_pts;
+	cv::Mat roi_img = skele_img.clone();
+	for(int r=0;r<roi_img.rows;++r){
+		for(int c=0;c<roi_img.cols;++c){
+			if(roi_img.at<unsigned char>(r,c)==0){continue;}
+			double d = cv::pointPolygonTest(triangle_region,cv::Point2f(c,r),false);
+			// d 1-内部点， 0-边缘点 -1-外部点
+			if(d<=0){
+				roi_img.at<unsigned char>(r,c)==0;
+			}
+			else{
+				curve_pts.push_back(cv::Point(c,r));
+			}
+		}
+	}
+	
+	//根据curve_pts进行曲线拟合，得到茎的曲线
+	cv::Mat curve_model;
+	poly_fit_cv(curve_pts,2,curve_model);
+
+}
+
+/**
+* calc_bottom_vertex
+* 找到三角形两个底角点
+*/
+void CTeaSort::calc_bottom_vertex(
+	cv::Point&vertex,	//input
+	double&ref_angle,	//input
+	cv::Point&bpt0,		//output
+	cv::Point&bpt1		//output
+) 
+{
+	double delta_angle = CV_PI / 8.0; // 22.5 degree
+	double radius = static_cast<double>(m_cp.offset_grab) * 1.5;
+
+	double angle = ref_angle - delta_angle;
+	int x = static_cast<int>(radius * sin(angle) + 0.5) + vertex.x;
+	int y = static_cast<int>(radius * cos(angle) + 0.5) + vertex.y;
+
+	bpt0.x = x;
+	bpt0.y = y;
+
+	angle = ref_angle + delta_angle;
+	x = static_cast<int>(radius * sin(angle) + 0.5) + vertex.x;
+	y = static_cast<int>(radius * cos(angle) + 0.5) + vertex.y;
+
+	bpt1.x = x;
+	bpt1.y = y;
+}
+//cv::Mat CTeaSort::poly_fit(
+//	std::vector<cv::Point2f>& chain,
+//	int n
+//)
+//{
+//	//https://blog.csdn.net/jpc20144055069/article/details/103232641
+//	cv::Mat y(chain.size(), 1, CV_32F, cv::Scalar::all(0));
+//	cv::Mat phy(chain.size(), n, CV_32F, cv::Scalar::all(0));
+//	for(int i=0;i<phy.rows;++i){
+//		float* pr = phy.ptr<float>(i);
+//		for(int j=0; j<phy.cols;++j){
+//			pr[j] = pow(chain[i].x,j);
+//		}
+//		y.at<float>(i) = chain[i].y;
+//	}
+//	
+//	cv::Mat phy_t = phy.t();
+//	cv::Mat phyMULphy_t = phy.t() * phy;
+//	cv::Mat phyMphyInv = phyMULphy_t.inv();
+//	cv::Mat a = phyMphyInv * phy_t;
+//	a = a*y;
+//	return a;	
+//}
+
+bool CTeaSort::poly_fit_cv(
+std::vector<cv::Point>& key_point,
+int n,
+cv::Mat& A
+)
+{
+	//https://blog.csdn.net/KYJL888/article/details/103073956
+	int N = key_point.size();
+	
+	//构造矩阵X
+	cv::Mat X = cv::Mat::zeros(n+1, n+1, CV_64FC1);
+	for(int i=0;i<n+1; ++i){
+		for(int j=0;j<n+1;++j){
+			for(int k=0;k<N;++k){
+				X.at<double>(i,j) = X.at<double>(i,j) +
+  				  std::pow(key_point[k].x, i+j);
+			}
+		}
+	}
+	
+	//构造矩阵Y
+	cv::Mat Y = cv::Mat::zeros(n+1, 1, CV_64FC1);
+	for(int i=0;i<n+1;++i){
+		for(int k=0;k<N;++k){
+			Y.at<double>(i,0) = Y.at<double>(i,0) +
+			std::pow(key_point[k].x, i) + key_point[k].y;
+		}
+	}
+	
+	A = cv::Mat::zeros(n+1, 1, CV_64FC1);
+	cv::solve(X,Y,A,cv::DECOMP_LU);	
+	return true;
+}
+double CTeaSort::calc_fit_y(
+double x,	//input
+cv::Mat& A	//input
+)
+{	
+	//double y = A.at<double>(0,0) + A.at<double>(1,0) * x +
+	//	A.at<double>(2,0) * std::pow(x,2) + A.at<double>(3,0) * std::pow(x,3);
+	//return y;	
+	
+	double y = 0.0;
+	for(int i=0; i<A.rows;++i){
+		y += A.at<double>(i,0) * std::pow(x,i);
+	}
+	return y;	
+}
 }

tea_sorter.h

@@ -49,5 +49,11 @@ namespace graft_cv{
 		//thinning
 		void thinningIteration(cv::Mat& img, int iter);
 		void thinning(const cv::Mat& src, cv::Mat& dst);
+
+		//find grab position
+		void get_grab_position(const cv::Mat& skele_img, cv::Point&vertex, double&ref_angle);
+		void calc_bottom_vertex(cv::Point&vertex, double&ref_angle, cv::Point&bpt0, cv::Point&bpt1);
+		bool poly_fit_cv(std::vector<cv::Point>& key_point, int n, cv::Mat& A);
+		double CTeaSort::calc_fit_y(double x,cv::Mat& A);
 	};
 };