|
|
@@ -565,158 +565,158 @@ int CTeaSort::generate_detect_windows(vector<Rect>&boxes)
|
|
|
return boxes.size();
|
|
|
}
|
|
|
|
|
|
-void CTeaSort::calculate_stem_grab_position(
|
|
|
- Bbox&b,
|
|
|
- double& grab_x, //output
|
|
|
- double& grab_y, //output
|
|
|
- double& grab_angle //output
|
|
|
-)
|
|
|
-{
|
|
|
-
|
|
|
- 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);
|
|
|
- }
|
|
|
-
|
|
|
- // skeletonize() or medial_axis()
|
|
|
- cv::Mat ske_img;
|
|
|
- thinning(bin_img, ske_img);
|
|
|
- /*if (m_cp.image_show) {
|
|
|
- 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));
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- //按与参考角度的差，找到有效的候选点集合
|
|
|
- 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);
|
|
|
- //计算夹角
|
|
|
- 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;
|
|
|
- }
|
|
|
- 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);
|
|
|
- }
|
|
|
- }*/
|
|
|
- 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;
|
|
|
- }
|
|
|
-
|
|
|
-}
|
|
|
+//void CTeaSort::calculate_stem_grab_position(
|
|
|
+// Bbox&b,
|
|
|
+// double& grab_x, //output
|
|
|
+// double& grab_y, //output
|
|
|
+// double& grab_angle //output
|
|
|
+//)
|
|
|
+//{
|
|
|
+//
|
|
|
+// 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);
|
|
|
+// }
|
|
|
+//
|
|
|
+// // skeletonize() or medial_axis()
|
|
|
+// cv::Mat ske_img;
|
|
|
+// thinning(bin_img, ske_img);
|
|
|
+// /*if (m_cp.image_show) {
|
|
|
+// 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));
|
|
|
+// }
|
|
|
+// }
|
|
|
+// }
|
|
|
+//
|
|
|
+// //按与参考角度的差，找到有效的候选点集合
|
|
|
+// 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);
|
|
|
+// //计算夹角
|
|
|
+// 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;
|
|
|
+// }
|
|
|
+// 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);
|
|
|
+// }
|
|
|
+// }*/
|
|
|
+// 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;
|
|
|
+// }
|
|
|
+//
|
|
|
+//}
|
|
|
|
|
|
/**
|
|
|
* Code for thinning a binary image using Zhang-Suen algorithm.
|
|
|
@@ -833,6 +833,62 @@ void CTeaSort::thinning(const cv::Mat& src, cv::Mat& dst)
|
|
|
dst *= 255;
|
|
|
}
|
|
|
|
|
|
+/**
|
|
|
+ distance_thinning()
|
|
|
+ distance transform based thinning
|
|
|
+
|
|
|
+ -----disused
|
|
|
+
|
|
|
+*/
|
|
|
+//void CTeaSort::distance_thinning(const cv::Mat& src, cv::Mat& dst)
|
|
|
+//{
|
|
|
+//
|
|
|
+// cv::Mat dist_mat(src.size(), CV_32FC1);
|
|
|
+// cv::distanceTransform(src, dist_mat, DIST_L2, 3);
|
|
|
+//
|
|
|
+// float max_dist = *max_element(dist_mat.begin<float>(), dist_mat.end<float>());
|
|
|
+// double r = 1.0;
|
|
|
+// if (max_dist > 1.0e-3) {
|
|
|
+// r = 255.0 / max_dist;
|
|
|
+// }
|
|
|
+// cv::Mat dist_img;
|
|
|
+// dist_mat.convertTo(dist_img, CV_8UC1, r, 0.0);
|
|
|
+//
|
|
|
+// cv::Canny(dist_img, dst, 50, 100, 7);
|
|
|
+//
|
|
|
+// unsigned char udist = *max_element(dst.begin<unsigned char>(), dst.end<unsigned char>());
|
|
|
+// if (m_cp.image_show) {
|
|
|
+// imshow_wait("dist_img", dist_img);
|
|
|
+// imshow_wait("canny", dst);
|
|
|
+// }
|
|
|
+//
|
|
|
+//
|
|
|
+//}
|
|
|
+
|
|
|
+/**
|
|
|
+part_thinning()
|
|
|
+将图片缩小，thinning， 然后放大得到，用以提高效率
|
|
|
+
|
|
|
+*/
|
|
|
+void CTeaSort::part_thinning(const cv::Mat& src, cv::Mat& dst)
|
|
|
+{
|
|
|
+
|
|
|
+ cv::Mat part_img;
|
|
|
+ cv::resize(src, part_img, cv::Size(src.cols / 2, src.rows / 2));
|
|
|
+
|
|
|
+ cv::Mat part_ske_img;
|
|
|
+ thinning(part_img, part_ske_img);
|
|
|
+
|
|
|
+ cv::Mat gray_img;
|
|
|
+ cv::resize(part_ske_img, gray_img, src.size());
|
|
|
+ double th = cv::threshold(gray_img, dst, 255, 255, cv::THRESH_OTSU);
|
|
|
+ /*if (m_cp.image_show) {
|
|
|
+ imshow_wait("part_img", part_img);
|
|
|
+ imshow_wait("part_ske_img", part_ske_img);
|
|
|
+ imshow_wait("dst", dst);
|
|
|
+ }*/
|
|
|
+}
|
|
|
+
|
|
|
/**
|
|
|
计算 [-pi,pi]间的两个角间的夹角
|
|
|
*/
|
|
|
@@ -866,6 +922,7 @@ double CTeaSort::intersection_angle(
|
|
|
*
|
|
|
*/
|
|
|
double CTeaSort::get_grab_position(
|
|
|
+ const std::vector<cv::Point2f>& inner_pixels,
|
|
|
const cv::Mat& skele_img,
|
|
|
cv::Point&vertex,
|
|
|
double ref_angle
|
|
|
@@ -884,28 +941,17 @@ double CTeaSort::get_grab_position(
|
|
|
triangle_region.push_back(pt3);
|
|
|
|
|
|
//构建多边形，然后判别骨架图中在多边形内的骨架像素
|
|
|
- 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));
|
|
|
- }
|
|
|
+ std::vector<cv::Point2f> curve_pts;
|
|
|
+ for (auto&pt : inner_pixels) {
|
|
|
+ double d = cv::pointPolygonTest(triangle_region, pt, false);
|
|
|
+ // d 1-内部点， 0-边缘点 -1-外部点
|
|
|
+ if (d > 0) {
|
|
|
+ curve_pts.push_back(pt);
|
|
|
}
|
|
|
}
|
|
|
//根据curve_pts进行曲线拟合，得到茎的曲线
|
|
|
- //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]
|
|
|
+ line_fit(curve_pts, line_model);
|
|
|
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]
|
|
|
@@ -916,7 +962,6 @@ double CTeaSort::get_grab_position(
|
|
|
grab_point_angle = y_angle_inv;
|
|
|
}
|
|
|
|
|
|
-
|
|
|
//可视化
|
|
|
if (m_cp.image_show) {
|
|
|
cv::Mat ske_img_tmp = skele_img.clone();
|
|
|
@@ -1007,17 +1052,17 @@ void CTeaSort::calc_bottom_vertex(
|
|
|
// return a;
|
|
|
//}
|
|
|
|
|
|
-void CTeaSort::line_fit(std::vector<cv::Point>& key_point, cv::Vec4f& lines)
|
|
|
+void CTeaSort::line_fit(std::vector<cv::Point2f>& key_point, cv::Vec4f& lines)
|
|
|
{
|
|
|
- std::vector<cv::Point2f> pts;
|
|
|
+ /*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);
|
|
|
+ cv::fitLine(key_point, lines, DIST_L1, param, reps, aeps);
|
|
|
}
|
|
|
//bool CTeaSort::poly_fit_cv(
|
|
|
//std::vector<cv::Point>& key_point,
|
|
|
@@ -1081,6 +1126,7 @@ void CTeaSort::calculate_stem_grab_position_opt(
|
|
|
double& grab_angle //input-output
|
|
|
)
|
|
|
{
|
|
|
+ //扩展box的范围，4个方向全部扩展
|
|
|
Bbox b(b_original);
|
|
|
int padding_border = m_cp.offset_grab;
|
|
|
b.x1 -= padding_border;
|
|
|
@@ -1145,29 +1191,23 @@ void CTeaSort::calculate_stem_grab_position_opt(
|
|
|
imshow_wait("cropped binary img", bin_img);
|
|
|
}
|
|
|
|
|
|
- //contours image
|
|
|
- /*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);
|
|
|
- }*/
|
|
|
-
|
|
|
// skeletonize() or medial_axis()
|
|
|
cv::Mat ske_img;
|
|
|
- thinning(bin_img, ske_img);
|
|
|
+ //thinning(bin_img, ske_img);
|
|
|
+ part_thinning(bin_img, ske_img);
|
|
|
/*if (m_cp.image_show) {
|
|
|
imshow_wait("skeleton img", ske_img);
|
|
|
}*/
|
|
|
|
|
|
+ //获取ske_img中骨架上的点坐标
|
|
|
+ std::vector<cv::Point2f> ske_pixels;
|
|
|
+ for (int r = 1; r < ske_img.rows-1; ++r) {
|
|
|
+ for (int c = 1; c < ske_img.cols-1; ++c) {
|
|
|
+ if (ske_img.at<unsigned char>(r, c) == 0) { continue; }
|
|
|
+ ske_pixels.push_back(cv::Point2f(c, r));
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
|
|
|
//在grab_angle的指导下找到最优方向，截图，进行局部thinning
|
|
|
double ref_angle_init = grab_angle;
|
|
|
@@ -1177,7 +1217,8 @@ void CTeaSort::calculate_stem_grab_position_opt(
|
|
|
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;
|
|
|
+ //int minx_opt, maxx_opt, miny_opt, maxy_opt;
|
|
|
+ std::vector<cv::Point2f> ske_pixels_opt;
|
|
|
double target_angle_opt;
|
|
|
for (int i = -8; i <= 8; ++i) { //-30 degree ---- 30 degree
|
|
|
//在指定方向的矩形框内，找到内部点最多的方向，作为主方向
|
|
|
@@ -1192,35 +1233,18 @@ void CTeaSort::calculate_stem_grab_position_opt(
|
|
|
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;
|
|
|
- }
|
|
|
+ triangle_region.push_back(pt3);
|
|
|
|
|
|
//counting
|
|
|
int pixel_num = 0;
|
|
|
- for (int r = miny; r <= maxy; ++r) {
|
|
|
- if (r < 0) { continue; }
|
|
|
- if (r >= ske_img.rows) { continue; }
|
|
|
- for (int c = minx; c <= maxx; ++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) {
|
|
|
- pixel_num++;
|
|
|
- }
|
|
|
- }
|
|
|
+ std::vector<cv::Point2f> inner_pixels;
|
|
|
+ for (auto&pt : ske_pixels) {
|
|
|
+ double d = cv::pointPolygonTest(triangle_region, pt, false);
|
|
|
+ // d 1-内部点， 0-边缘点 -1-外部点
|
|
|
+ if (d >= 0) {
|
|
|
+ pixel_num++;
|
|
|
+ inner_pixels.push_back(pt);
|
|
|
+ }
|
|
|
}
|
|
|
if (pixel_num > max_pixels) {
|
|
|
max_pixels = pixel_num;
|
|
|
@@ -1229,10 +1253,8 @@ void CTeaSort::calculate_stem_grab_position_opt(
|
|
|
pt2_opt = pt2;
|
|
|
pt3_opt = pt3;
|
|
|
center_opt = center_pt;
|
|
|
- minx_opt = minx;
|
|
|
- maxx_opt = maxx;
|
|
|
- miny_opt = miny;
|
|
|
- maxy_opt = maxy;
|
|
|
+ ske_pixels_opt.clear();
|
|
|
+ ske_pixels_opt.insert(ske_pixels_opt.begin(), inner_pixels.begin(), inner_pixels.end());
|
|
|
target_angle_opt = target_angle;
|
|
|
}
|
|
|
/*if (m_cp.image_show) {
|
|
|
@@ -1270,52 +1292,10 @@ void CTeaSort::calculate_stem_grab_position_opt(
|
|
|
|
|
|
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);
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- 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);
|
|
|
+ line_fit(ske_pixels_opt, 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]
|
|
|
@@ -1355,9 +1335,9 @@ void CTeaSort::calculate_stem_grab_position_opt(
|
|
|
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;
|
|
|
+ for (auto& pt : ske_pixels_opt) {
|
|
|
+ int c = int(pt.x);
|
|
|
+ int r = int(pt.y);
|
|
|
double dist = std::powf((p5.x - c), 2) + std::powf((p5.y - r), 2);
|
|
|
dist = std::sqrtf(dist);
|
|
|
if (dist < dist_min) {
|
|
|
@@ -1406,7 +1386,7 @@ void CTeaSort::calculate_stem_grab_position_opt(
|
|
|
|
|
|
//计算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_angle = get_grab_position(ske_pixels_opt, ske_img, p_min_angle, ref_angle);
|
|
|
}
|
|
|
|
|
|
//重新得到grab_x,grab_y的坐标
|
chenhongjiang