v0.1.12 优化抓取角度，提高效率，提高准确率

ReadMe.txt

@@ -9,4 +9,5 @@ v0.1.7
 v0.1.8 改用传统方法获取抓取位置
 v0.1.9 增加面积大小过滤（通过面积占比系数）
 v0.1.10 优化抓取位置识别（夹角最小点作为抓取点）
-v0.1.11 优化抓取角度
+v0.1.11 优化抓取角度
+v0.1.12 优化抓取角度，提高效率，提高准确率

tea_cv_api.cpp

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

tea_sorter.cpp

@@ -203,7 +203,7 @@ int CTeaSort::detect(
 				continue;
 			}
 			double grab_x, grab_y;
-			double angle = calculate_angle(b, true, grab_x, grab_y);
+			double angle = calculate_angle(b,/* true, */grab_x, grab_y);
 			
 			//grab point 
 			if (valid_cnt == 0) {
@@ -229,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, true, grab_x, grab_y);
+			double angle = calculate_angle(b,/* true,*/ grab_x, grab_y);
 			valid_cnt += 1;			
 			if (i == 0) {				
 				// 切割点是3、4的中间的点
@@ -279,8 +279,8 @@ int CTeaSort::detect(
 			//grab points
 			if (m_dtype == img_type::tea_grab) {
 				double grab_x, grab_y;
-				bool need_precise = b.status == 1;
-				double grab_angle = calculate_angle(b, need_precise, 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, 
@@ -381,7 +381,7 @@ int CTeaSort::detect_impl(
 }
 double CTeaSort::calculate_angle(
 	Bbox&b,				//input
-	bool need_precise_angle,//input
+	//bool need_precise_angle,//input
 	double& grab_x,		//output
 	double& grab_y		//output
 )
@@ -397,8 +397,8 @@ double CTeaSort::calculate_angle(
 	y5 = b.ppoint[9];
 	if (m_dtype == img_type::tea_grab) {
 		angle = atan2(x5 - x3, y5 - y3);
-		if (need_precise_angle) {
-			calculate_stem_grab_position(b, grab_x, grab_y, angle);
+		//if (need_precise_angle) {
+			calculate_stem_grab_position_opt(b, grab_x, grab_y, angle);
 			//计算抓取点
 			if (grab_x < 0 && grab_y < 0) {
 				double pr = (double)m_cp.offset_grab;
@@ -407,14 +407,14 @@ double CTeaSort::calculate_angle(
 				grab_x = x5 + dx;
 				grab_y = y5 + dy;
 			}
-		}
+		/*}
 		else {
 			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
@@ -866,8 +866,8 @@ double CTeaSort::get_grab_position(
 	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);
+	calc_bottom_vertex(vertex, ref_angle, CV_PI / 8.0, radius, pt0, pt1);
+	calc_bottom_vertex(vertex, ref_angle+CV_PI, CV_PI / 8.0, radius, pt2, pt3);
 
 	std::vector<cv::Point> triangle_region;	
 	triangle_region.push_back(pt0);
@@ -952,12 +952,13 @@ double CTeaSort::get_grab_position(
 void CTeaSort::calc_bottom_vertex(
 	cv::Point&vertex,	//input
 	double ref_angle,	//input, rad
+	double delta_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 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;
@@ -1010,52 +1011,381 @@ void CTeaSort::line_fit(std::vector<cv::Point>& key_point, cv::Vec4f& lines)
 	//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,
-cv::Mat& 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;	
+//}
+//}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// calculate_stem_grab_position_opt（）替代calculate_stem_grab_position函数
+// 1）采用局部thinning方法提高效率
+// 2) 重新用局部线性拟合的方向替代ref_angle(原始是p5和p3点连线与y正方向的夹角)
+
+void CTeaSort::calculate_stem_grab_position_opt(
+	Bbox&b,
+	double& grab_x,		//output
+	double& grab_y,		//output
+	double& grab_angle //input-output
 )
 {
-	//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);
+
+	grab_x = grab_y = -1.0;
+	//crop image
+	int padding = 2 * m_cp.offset_grab;
+	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) {
+		// 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
+		if (b.y1 - padding < 0) {
+			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, 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);
+	}
+
+	//to gray
+	cv::Mat gray_img;
+	if (crop_img.channels() == 1) { gray_img = crop_img; }
+	else {
+		cv::cvtColor(crop_img, gray_img, cv::COLOR_BGR2GRAY);
+	}
+	//binary
+	cv::Mat bin_img;
+	double th = cv::threshold(gray_img, bin_img, 255, 255, cv::THRESH_OTSU);
+	cv::bitwise_not(bin_img, bin_img);
+	if (m_cp.image_show) {
+		imshow_wait("cropped binary img", bin_img);
+	}
+
+	vector<vector<cv::Point>> contours;
+	vector<cv::Vec4i> hierarchy;
+	contours.clear();
+	hierarchy.clear();
+	findContours(bin_img, contours, hierarchy, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
+	cv::Mat con_img = cv::Mat::zeros(bin_img.size(), CV_8UC1);
+	for (auto&c : contours) {
+		for (auto&p : c) {
+			con_img.at<unsigned char>(p.y, p.x) = 255;
+		}
+	}
+	if (m_cp.image_show) {	
+		imshow_wait("findContours", con_img);
+	}
+	//在grab_angle的指导下找到最优方向，截图，进行局部thinning
+	double ref_angle_init = grab_angle;
+	double delta_angle = CV_PI / 24.0;
+	double radius = static_cast<double>(m_cp.offset_grab);
+	cv::Point pt0, pt1, pt2, pt3;
+	double step_angle = CV_PI / 36.0; // 5 degree
+	int max_pixels = 0;
+	cv::Point pt0_opt, pt1_opt, pt2_opt, pt3_opt, center_opt;
+	int minx_opt, maxx_opt, miny_opt, maxy_opt;
+	double target_angle_opt;
+	for (int i = -8; i <= 8; ++i) { //-30 degree ----  30 degree
+		//在指定方向的矩形框内，找到内部点最多的方向，作为主方向
+		double target_angle = ref_angle_init + i*step_angle;
+		cv::Point center_pt;
+		center_pt.x = p5.x + static_cast<int>(radius * sin(target_angle));
+		center_pt.y = p5.y + static_cast<int>(radius * cos(target_angle));
+		calc_bottom_vertex(center_pt, target_angle, delta_angle, radius, pt0, pt1);
+		calc_bottom_vertex(center_pt, target_angle + CV_PI, delta_angle, 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);
+
+		//外接4边形
+		int minx, maxx, miny, maxy;
+		minx = maxx = pt0.x;
+		miny = maxy = pt0.y;
+		for (auto& pt : triangle_region) {
+			minx = minx > pt.x ? pt.x : minx;
+			maxx = maxx > pt.x ? maxx : pt.x;
+			miny = miny > pt.y ? pt.y : miny;
+			maxy = maxy > pt.y ? maxy : pt.y;
+		}
+
+		//counting
+		int pixel_num = 0;
+		for (int r = miny; r <= maxy; ++r) {
+			if (r < 0) { continue; }
+			if (r >= bin_img.rows) { continue; }
+			for (int c = minx; c <= maxx; ++c) {
+				if (c < 0) { continue; }
+				if (c >= bin_img.cols) { continue; }
+				if (con_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) {
+					pixel_num++;
+				}				
 			}
 		}
+		if (pixel_num > max_pixels) {
+			max_pixels = pixel_num;
+			pt0_opt = pt0; 
+			pt1_opt = pt1;
+			pt2_opt = pt2;
+			pt3_opt = pt3;
+			center_opt = center_pt;
+			minx_opt = minx;
+			maxx_opt = maxx;
+			miny_opt = miny;
+			maxy_opt = maxy;
+			target_angle_opt = target_angle;
+		}
+		/*if (m_cp.image_show) {
+			cv::Mat bin_tmp = bin_img.clone();
+
+			cv::circle(bin_tmp, p5, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
+			cv::circle(bin_tmp, pt0, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
+			cv::circle(bin_tmp, pt1, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
+			cv::circle(bin_tmp, pt2, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
+			cv::circle(bin_tmp, pt3, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
+
+			cv::line(bin_tmp, pt0, pt1, cv::Scalar(180, 215, 255), 2);
+			cv::line(bin_tmp, pt0, pt3, cv::Scalar(180, 215, 255), 2);
+			cv::line(bin_tmp, pt1, pt2, cv::Scalar(180, 215, 255), 2);
+			cv::line(bin_tmp, pt2, pt3, cv::Scalar(180, 215, 255), 2);
+			
+			imshow_wait("binary img box", bin_tmp);
+		}*/
 	}
-	
-	//构造矩阵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;
+
+	//opt box process
+	if (m_cp.image_show) {
+		cv::Mat bin_tmp = bin_img.clone();
+
+		cv::circle(bin_tmp, p5, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
+		cv::circle(bin_tmp, pt0_opt, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
+		cv::circle(bin_tmp, pt1_opt, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
+		cv::circle(bin_tmp, pt2_opt, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
+		cv::circle(bin_tmp, pt3_opt, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
+
+		cv::line(bin_tmp, pt0_opt, pt1_opt, cv::Scalar(180, 215, 255), 2);
+		cv::line(bin_tmp, pt0_opt, pt3_opt, cv::Scalar(180, 215, 255), 2);
+		cv::line(bin_tmp, pt1_opt, pt2_opt, cv::Scalar(180, 215, 255), 2);
+		cv::line(bin_tmp, pt2_opt, pt3_opt, cv::Scalar(180, 215, 255), 2);
+
+		imshow_wait("binary img box opt", bin_tmp);
+	}
+
+
+	// skeletonize() or medial_axis()	
+	cv::Mat ske_img;
+	cv::Mat roi_bin_img = cv::Mat::zeros(bin_img.size(), CV_8UC1);	
+	for (int r = miny_opt; r <= maxy_opt; ++r) {
+		if (r < 0) { continue; }
+		if (r >= bin_img.rows) { continue; }
+		for (int c = minx_opt; c <= maxx_opt; ++c) {
+			if (c < 0) { continue; }
+			if (c >= bin_img.cols) { continue; }
+			if (bin_img.at<unsigned char>(r, c) == 0) { continue; }
+			roi_bin_img.at<unsigned char>(r, c) = bin_img.at<unsigned char>(r, c);			
 		}
 	}
-	
-	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);
+
+	thinning(roi_bin_img, ske_img);
+	if (m_cp.image_show) {
+		imshow_wait("skeleton img", ske_img);
+	}
+
+	//通过区域内的骨架点计算ref_angle
+	std::vector<cv::Point> triangle_region;
+	triangle_region.push_back(pt0_opt);
+	triangle_region.push_back(pt1_opt);
+	triangle_region.push_back(pt2_opt);
+	triangle_region.push_back(pt3_opt);
+	std::vector<cv::Point> in_region_pts;	
+	for (int r = miny_opt; r <= maxy_opt; ++r) {
+		if (r < 0) { continue; }
+		if (r >= ske_img.rows) { continue; }
+		for (int c = minx_opt; c <= maxx_opt; ++c) {
+			if (c < 0) { continue; }
+			if (c >= ske_img.cols) { continue; }
+			if (ske_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) {
+				in_region_pts.push_back(cv::Point(c, r));
+			}			
+		}
+	}
+	//计算ref_angle
+	cv::Vec4f line_model;//[vx,vy, x0,y0], vx,vy---方向的归一化向量，x0,y0---直线上任意一点
+	line_fit(in_region_pts, line_model);	
+	double y_angle = atan2(line_model[0], line_model[1]);// y_angle in range [-pi, pi]
+	double fabs_angle = intersection_angle(target_angle_opt, 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(target_angle_opt, y_angle_inv);
+
+	double ref_angle = y_angle;
+	if (fabs_angle_inv < fabs_angle) {
+		ref_angle = y_angle_inv;
+	}
+
+	//可视化
+	/*if (m_cp.image_show) {
+		cv::Mat ske_img_tmp = ske_img.clone();
+		for (auto&p : in_region_pts) {
+			ske_img_tmp.at<unsigned char>(p) = 100;
+		}	
+
+		double dcx = radius * sin(ref_angle);
+		double dcy = radius * cos(ref_angle);
+
+		cv::Point dir_o;
+		cv::Point dir_p;
+		dir_o.x = center_opt.x + 10;
+		dir_o.y = center_opt.y;
+
+		dir_p.x = int(center_opt.x + 10 + dcx);
+		dir_p.y = int(center_opt.y + dcy);
+		cv::line(ske_img_tmp, dir_o, dir_p, cv::Scalar(255, 215, 255), 2);
+
+
+		imshow_wait("ref angle", ske_img_tmp);
+	}*/
+
+
+	//遍历所有点，找到距离等于指定距离的点的位置, 以及距离p5最近的骨架上的点
+	std::vector<cv::Point> candidate_pts;
+	cv::Point p5_nearst;
+	double dist_th = 5;
+	double dist_min = 1.0e6;
+	for (auto& pt : in_region_pts) {
+		int c = pt.x;
+		int r = pt.y;
+		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));
+		}
+	}	
+
+	//按与参考角度的差，找到有效的候选点集合
+	std::vector<cv::Point> valid_candidate_pts;	
+	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);
+		//计算夹角
+		double fabs_angle = intersection_angle(ref_angle, angle_to_p3);
+		
+		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 (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) {
+			ske_img_tmp.at<unsigned char>(p) = 100;
+		}
+		cv::circle(ske_img_tmp, p5, 4, cv::Scalar(255, 0, 255), 1, 3, 0);
+		if (grab_x > 0 && grab_y > 0) {
+			cv::circle(ske_img_tmp, cv::Point(int(grab_x), int(grab_y)), 4, cv::Scalar(156, 0, 255), 1, 3, 0);
+		}
+		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) {
+		int real_padding_y = p5.y - int(b.ppoint[9] - b.y1);
+		grab_y -= real_padding_y;
+		grab_y += b.y1;
+		grab_x += b.x1;
 	}
-	return y;	
 }
+
 }

tea_sorter.h

@@ -43,8 +43,9 @@ 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, bool need_precise_angle, 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);
+		void calculate_stem_grab_position_opt(Bbox&b, double& grab_x, double&grab_y, double& grab_angle);
 
 		//thinning
 		void thinningIteration(cv::Mat& img, int iter);
@@ -52,9 +53,9 @@ namespace graft_cv{
 
 		//find grab position
 		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 calc_fit_y(double x,cv::Mat& A);
+		void calc_bottom_vertex(cv::Point&vertex, double ref_angle, double delta_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 calc_fit_y(double x,cv::Mat& A);
 
 		void line_fit(std::vector<cv::Point>& key_point, cv::Vec4f& lines);
 		/**