v0.1.11 优化抓取角度

ReadMe.txt

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

tcv_conf.yml

@@ -6,13 +6,13 @@ conf_parameters:
    image_save: 1
    image_depository: "D:\\logs\\algo_img"
    image_backup_days: 7
-   model_path_grab: "D:/projects/graft/py_code/retina_tea5/TeaDetector_grab_20231126110149.onnx"
+   model_path_grab: "D:/projects/graft/py_code/retina_tea5/TeaDetector_grab_20231205234724.onnx"
    object_threshold_grab: 0.6
    nms_threshold_grab: 1.0000000149011612e-01
    grid_row_grab: 2
    grid_col_grab: 3
    grid_padding_grab: 100
-   offset_grab: 100
+   offset_grab: 230
    min_distance_grab: 501.
    min_area_ratio_grab: 0.0
    max_area_ratio_grab: 0.1

tea_cv_api.cpp

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

tea_sorter.cpp

@@ -167,6 +167,12 @@ int CTeaSort::detect(
 		area_ratio /= static_cast<double>(m_raw_img.cols);
 		tbox.area = area_ratio;
 		if (area_ratio < min_area_th || area_ratio > max_area_th) { continue; }
+		//检查box边界是否在图像内，如果没有，修改之
+		if (tbox.x1 < 0) { tbox.x1 = 0; }
+		if (tbox.y1 < 0) { tbox.y1 = 0; }
+		if (tbox.x2 >= m_raw_img.cols) { tbox.x2 = m_raw_img.cols - 1; }
+		if (tbox.y2 >= m_raw_img.rows) { tbox.y2 = m_raw_img.rows - 1; }
+
 		droplets.push_back(tbox);
 	}
 	if (m_pLogger) {
@@ -197,7 +203,7 @@ int CTeaSort::detect(
 				continue;
 			}
 			double grab_x, grab_y;
-			double angle = calculate_angle(b, grab_x, grab_y);
+			double angle = calculate_angle(b, true, grab_x, grab_y);
 			
 			//grab point 
 			if (valid_cnt == 0) {
@@ -223,7 +229,7 @@ int CTeaSort::detect(
 			Bbox&b = droplets.at(i);
 			b.status = 1; // selected
 			double grab_x, grab_y;
-			double angle = calculate_angle(b, grab_x, grab_y);
+			double angle = calculate_angle(b, true, grab_x, grab_y);
 			valid_cnt += 1;			
 			if (i == 0) {				
 				// 切割点是3、4的中间的点
@@ -273,7 +279,8 @@ int CTeaSort::detect(
 			//grab points
 			if (m_dtype == img_type::tea_grab) {
 				double grab_x, grab_y;
-				calculate_angle(b, grab_x, grab_y);
+				bool need_precise = b.status == 1;
+				double grab_angle = calculate_angle(b, need_precise, grab_x, grab_y);
 				//cv::circle(img_rst, cv::Point(int(grab_x), int(grab_y)), 4, cv::Scalar(0, 215, 255), -1, 3, 0);
 				//lines, p4-p5, p5-grab
 				cv::line(img_rst, 
@@ -291,6 +298,14 @@ int CTeaSort::detect(
 				cv::line(img_rst, cv::Point(cx - radius, cy - radius), cv::Point(cx + radius, cy + radius), cv::Scalar(0, 215, 255), 2);
 				cv::line(img_rst, cv::Point(cx - radius, cy + radius), cv::Point(cx + radius, cy - radius), cv::Scalar(0, 215, 255), 2);
 
+				//grab point angle
+				int radius_dir = m_cp.offset_grab / 2;
+				grab_angle *= (CV_PI / 180.0);
+				double dx = radius_dir * sin(grab_angle);
+				double dy = radius_dir * cos(grab_angle);
+				int dir_x = int(grab_x + dx);
+				int dir_y = int(grab_y + dy);
+				cv::line(img_rst, cv::Point(cx, cy), cv::Point(dir_x, dir_y), cv::Scalar(20, 255, 20), 2);
 			}
 			//cut points
 			if (m_dtype == img_type::tea_cut) {	
@@ -366,6 +381,7 @@ int CTeaSort::detect_impl(
 }
 double CTeaSort::calculate_angle(
 	Bbox&b,				//input
+	bool need_precise_angle,//input
 	double& grab_x,		//output
 	double& grab_y		//output
 )
@@ -380,24 +396,25 @@ double CTeaSort::calculate_angle(
 	x5 = b.ppoint[8];
 	y5 = b.ppoint[9];
 	if (m_dtype == img_type::tea_grab) {
-		calculate_stem_grab_position(b, grab_x, grab_y);
-		//calculate line of p4 ans p5
-		double r45 = sqrt((x4 - x5)*(x4 - x5) + (y4 - y5)*(y4 - y5));
-		if (r45 < 15.0) {
-			angle = atan2(x5 - x3, y5 - y3);
+		angle = atan2(x5 - x3, y5 - y3);
+		if (need_precise_angle) {
+			calculate_stem_grab_position(b, grab_x, grab_y, angle);
+			//计算抓取点
+			if (grab_x < 0 && grab_y < 0) {
+				double pr = (double)m_cp.offset_grab;
+				double dx = pr * sin(angle);
+				double dy = pr * cos(angle);
+				grab_x = x5 + dx;
+				grab_y = y5 + dy;
+			}
 		}
 		else {
-			angle = atan2(x5 - x4, y5 - y4);
-		}
-		//计算抓取点
-		if (grab_x < 0 && grab_y < 0) {		
 			double pr = (double)m_cp.offset_grab;
 			double dx = pr * sin(angle);
 			double dy = pr * cos(angle);
 			grab_x = x5 + dx;
 			grab_y = y5 + dy;
 		}
-		
 	}
 	else {
 		//tea cut, calculate line of p3 ans p4
@@ -543,7 +560,8 @@ int CTeaSort::generate_detect_windows(vector<Rect>&boxes)
 void CTeaSort::calculate_stem_grab_position(
 	Bbox&b,
 	double& grab_x,		//output
-	double& grab_y		//output
+	double& grab_y,		//output
+	double& grab_angle //output
 )
 {
 
@@ -557,8 +575,12 @@ void CTeaSort::calculate_stem_grab_position(
 	cv::Point p5(int(b.ppoint[8] - b.x1), int(b.ppoint[9] - b.y1));
 	cv::Mat crop_img;
 	if (y5 > y3) {
-		// Y position		
-		crop_img = m_raw_img(cv::Range(b.y1, b.y2 + padding), cv::Range(b.x1, b.x2)).clone();		
+		// Y position	
+		int ymax = b.y2 + padding;
+		if (ymax > m_raw_img.rows) { 
+			ymax = m_raw_img.rows; 
+		}
+		crop_img = m_raw_img(cv::Range(b.y1, ymax), cv::Range(b.x1, b.x2)).clone();
 	}
 	else {
 		// ^ position
@@ -630,8 +652,8 @@ void CTeaSort::calculate_stem_grab_position(
 	for (auto&p : candidate_pts) {
 		double angle_to_p3 = atan2(p.x - p3.x, p.y - p3.y);
 		//计算夹角
-		double fabs_angle = 0;
-		if (ref_angle > 0.5 * CV_PI) {
+		double fabs_angle = intersection_angle(ref_angle, angle_to_p3);
+		/*if (ref_angle > 0.5 * CV_PI) {
 			if (angle_to_p3 < 0) {
 				angle_to_p3 += 2 * CV_PI;				
 			}
@@ -647,7 +669,7 @@ void CTeaSort::calculate_stem_grab_position(
 			else {
 				fabs_angle = std::fabs(angle_to_p3 - ref_angle);
 			}
-		}
+		}*/
 		if (fabs_angle > CV_PI / 4.0) { continue; }
 		if (fabs_angle < min_angle) {
 			min_angle = fabs_angle;
@@ -672,6 +694,11 @@ void CTeaSort::calculate_stem_grab_position(
 		}
 		imshow_wait("skeleton img label", ske_img_tmp);
 	}
+	
+	//计算grab点的抓取角度
+	if (p_min_angle.x > 0 && p_min_angle.y > 0) {
+		grab_angle = get_grab_position(ske_img, p_min_angle, ref_angle);
+	}
 
 	//重新得到grab_x,grab_y的坐标
 	if (grab_x > 0 && grab_y > 0) {
@@ -680,6 +707,7 @@ void CTeaSort::calculate_stem_grab_position(
 		grab_y += b.y1;
 		grab_x += b.x1;
 	}
+
 }
 
 /**
@@ -797,23 +825,55 @@ void CTeaSort::thinning(const cv::Mat& src, cv::Mat& dst)
 	dst *= 255;
 }
 
-
+/**
+计算 [-pi,pi]间的两个角间的夹角
+*/
+double CTeaSort::intersection_angle(
+	double ref_angle, 
+	double angle_to_p3
+)
+{
+	//计算夹角
+	double fabs_angle = 0;
+	if (ref_angle > 0.5 * CV_PI) {
+		if (angle_to_p3 < 0) {
+			angle_to_p3 += 2 * CV_PI;
+		}
+		fabs_angle = std::fabs(angle_to_p3 - ref_angle);
+	}
+	else {
+		if (ref_angle < -0.5 * CV_PI) {
+			if (angle_to_p3 > 0) {
+				angle_to_p3 -= 2 * CV_PI;
+			}
+			fabs_angle = std::fabs(angle_to_p3 - ref_angle);
+		}
+		else {
+			fabs_angle = std::fabs(angle_to_p3 - ref_angle);
+		}
+	}
+	return fabs_angle;
+}
 /**
 * 
 */
-void CTeaSort::get_grab_position(
+double CTeaSort::get_grab_position(
 	const cv::Mat& skele_img, 
 	cv::Point&vertex, 
-	double&ref_angle
+	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);
+	double grab_point_angle = CV_2PI;
+	cv::Point pt0, pt1, pt2, pt3;
+	double radius = static_cast<double>(m_cp.offset_grab) * 0.5;
+	calc_bottom_vertex(vertex, ref_angle, radius, pt0, pt1);
+	calc_bottom_vertex(vertex, ref_angle+CV_PI, radius, pt2, pt3);
+
+	std::vector<cv::Point> triangle_region;	
 	triangle_region.push_back(pt0);
 	triangle_region.push_back(pt1);
-	
+	triangle_region.push_back(pt2);
+	triangle_region.push_back(pt3);	
 	
 	//构建多边形，然后判别骨架图中在多边形内的骨架像素
 	std::vector<cv::Point> curve_pts;
@@ -831,10 +891,57 @@ void CTeaSort::get_grab_position(
 			}
 		}
 	}
-	
 	//根据curve_pts进行曲线拟合，得到茎的曲线
-	cv::Mat curve_model;
-	poly_fit_cv(curve_pts,2,curve_model);
+	//cv::Mat curve_model;
+	//poly_fit_cv(curve_pts, 1, curve_model);
+	cv::Vec4f line_model;//[vx,vy, x0,y0], vx,vy---方向的归一化向量，x0,y0---直线上任意一点
+	line_fit(curve_pts, line_model);
+		//double k = curve_model.at<double>(1, 0);
+		//double y_angle = 0.5 * CV_PI - atan(k); // y_angle in range [0, pi]
+	double y_angle = atan2(line_model[0], line_model[1]);// y_angle in range [-pi, pi]
+	double fabs_angle = intersection_angle(ref_angle, y_angle);
+	double y_angle_inv = atan2(-line_model[0], -line_model[1]);;	//y_angle_inv in range [-pi, pi]
+	double fabs_angle_inv = intersection_angle(ref_angle, y_angle_inv);
+
+	grab_point_angle = y_angle;
+	if (fabs_angle_inv < fabs_angle) {
+		grab_point_angle = y_angle_inv;
+	}
+
+
+	//可视化
+	if (m_cp.image_show) {
+		cv::Mat ske_img_tmp = skele_img.clone();
+		for (auto&p : curve_pts) {
+			ske_img_tmp.at<unsigned char>(p) = 100;
+		}
+		cv::circle(ske_img_tmp, vertex, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
+		cv::circle(ske_img_tmp, pt0, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
+		cv::circle(ske_img_tmp, pt1, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
+		cv::circle(ske_img_tmp, pt2, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
+		cv::circle(ske_img_tmp, pt3, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
+
+		cv::line(ske_img_tmp, pt0, pt1, cv::Scalar(255, 215, 255), 2);
+		cv::line(ske_img_tmp, pt0, pt3, cv::Scalar(255, 215, 255), 2);
+		cv::line(ske_img_tmp, pt1, pt2, cv::Scalar(255, 215, 255), 2);
+		cv::line(ske_img_tmp, pt2, pt3, cv::Scalar(255, 215, 255), 2);
+		
+		double dcx = radius * sin(grab_point_angle);
+		double dcy = radius * cos(grab_point_angle);
+
+		cv::Point dir_o;
+		cv::Point dir_p;
+		dir_o.x = vertex.x + 10;
+		dir_o.y = vertex.y;
+
+		dir_p.x = int(vertex.x + 10 + dcx);
+		dir_p.y = int(vertex.y + dcy);
+		cv::line(ske_img_tmp, dir_o, dir_p, cv::Scalar(255, 215, 255), 2);
+
+		
+		imshow_wait("grab angle", ske_img_tmp);
+	}
+	return grab_point_angle;
 
 }
 
@@ -844,13 +951,14 @@ void CTeaSort::get_grab_position(
 */
 void CTeaSort::calc_bottom_vertex(
 	cv::Point&vertex,	//input
-	double&ref_angle,	//input
+	double ref_angle,	//input, rad
+	double radius,		//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 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;
@@ -890,6 +998,18 @@ void CTeaSort::calc_bottom_vertex(
 //	return a;	
 //}
 
+void CTeaSort::line_fit(std::vector<cv::Point>& key_point, cv::Vec4f& lines)
+{
+	std::vector<cv::Point2f> pts;
+	for (auto&p : key_point) {
+		pts.push_back(cv::Point2f(p.x, p.y));
+	}
+	double param = 0;
+	double reps = 0.01;
+	double aeps = 0.01;
+	//cv::Vec4f lines;//[vx,vy, x0,y0], vx,vy---方向的归一化向量，x0,y0---直线上任意一点
+	cv::fitLine(pts, lines, DIST_L1, param, reps, aeps);	
+}
 bool CTeaSort::poly_fit_cv(
 std::vector<cv::Point>& key_point,
 int n,

tea_sorter.h

@@ -43,17 +43,23 @@ namespace graft_cv{
 		int generate_detect_windows(vector<Rect>&boxes);
 		int detect_impl(cv::Mat& img, vector<Rect>&drop_regions, vector<Bbox> &droplets_raw);
 		void clear_imginfo();
-		double calculate_angle(Bbox&b, double& grab_x, double&grab_y);
-		void calculate_stem_grab_position(Bbox&b, double& grab_x, double&grab_y);
+		double calculate_angle(Bbox&b, bool need_precise_angle, double& grab_x, double&grab_y);
+		void calculate_stem_grab_position(Bbox&b, double& grab_x, double&grab_y, double& grab_angle);
 
 		//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);
+		double get_grab_position(const cv::Mat& skele_img, cv::Point&vertex, double ref_angle);
+		void calc_bottom_vertex(cv::Point&vertex, double ref_angle, double radius,	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);
+		double calc_fit_y(double x,cv::Mat& A);
+
+		void line_fit(std::vector<cv::Point>& key_point, cv::Vec4f& lines);
+		/**
+		计算 [-pi,pi]间的两个角间的夹角
+		*/
+		double intersection_angle(double ref_angle, double angle_to_p3);
 	};
 };