|
|
@@ -181,7 +181,7 @@ int CTeaSort::detect(
|
|
|
continue;
|
|
|
}
|
|
|
double grab_x, grab_y;
|
|
|
- double angle = calalate_angle(b, grab_x, grab_y);
|
|
|
+ double angle = calculate_angle(b, grab_x, grab_y);
|
|
|
|
|
|
//grab point
|
|
|
if (valid_cnt == 0) {
|
|
|
@@ -207,7 +207,7 @@ int CTeaSort::detect(
|
|
|
Bbox&b = droplets.at(i);
|
|
|
b.status = 1; // selected
|
|
|
double grab_x, grab_y;
|
|
|
- double angle = calalate_angle(b, grab_x, grab_y);
|
|
|
+ double angle = calculate_angle(b, grab_x, grab_y);
|
|
|
valid_cnt += 1;
|
|
|
if (i == 0) {
|
|
|
// 切割点是3、4的中间的点
|
|
|
@@ -257,7 +257,7 @@ int CTeaSort::detect(
|
|
|
//grab points
|
|
|
if (m_dtype == img_type::tea_grab) {
|
|
|
double grab_x, grab_y;
|
|
|
- calalate_angle(b, grab_x, grab_y);
|
|
|
+ calculate_angle(b, 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,
|
|
|
@@ -348,7 +348,7 @@ int CTeaSort::detect_impl(
|
|
|
|
|
|
return droplets_raw.size();
|
|
|
}
|
|
|
-double CTeaSort::calalate_angle(
|
|
|
+double CTeaSort::calculate_angle(
|
|
|
Bbox&b, //input
|
|
|
double& grab_x, //output
|
|
|
double& grab_y //output
|
|
|
@@ -364,6 +364,7 @@ double CTeaSort::calalate_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) {
|
|
|
@@ -373,11 +374,14 @@ double CTeaSort::calalate_angle(
|
|
|
angle = atan2(x5 - x4, y5 - y4);
|
|
|
}
|
|
|
//计算抓取点
|
|
|
- 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;
|
|
|
+ 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
|
|
|
@@ -520,4 +524,260 @@ 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
|
|
|
+)
|
|
|
+{
|
|
|
+
|
|
|
+ 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 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();
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ // ^ position
|
|
|
+ if (b.y1 - padding < 0) {
|
|
|
+ padding = b.y1;
|
|
|
+ }
|
|
|
+ p5.y = int(b.ppoint[9] - 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);
|
|
|
+
|
|
|
+ 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);
|
|
|
+ }*/
|
|
|
+
|
|
|
+ //遍历所有点，找到距离等于指定距离的点的位置
|
|
|
+ std::vector<cv::Point> candidate_pts;
|
|
|
+ double dist_th = 5;
|
|
|
+ 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 (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);
|
|
|
+ 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) {
|
|
|
+ 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; }
|
|
|
+ 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 (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_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.
|
|
|
+*
|
|
|
+* Author: Nash (nash [at] opencv-code [dot] com)
|
|
|
+* Website: http://opencv-code.com
|
|
|
+*/
|
|
|
+
|
|
|
+/**
|
|
|
+* Perform one thinning iteration.
|
|
|
+* Normally you wouldn't call this function directly from your code.
|
|
|
+*
|
|
|
+* Parameters:
|
|
|
+* im Binary image with range = [0,1]
|
|
|
+* iter 0=even, 1=odd
|
|
|
+*/
|
|
|
+void CTeaSort::thinningIteration(cv::Mat& img, int iter)
|
|
|
+{
|
|
|
+ CV_Assert(img.channels() == 1);
|
|
|
+ CV_Assert(img.depth() != sizeof(uchar));
|
|
|
+ CV_Assert(img.rows > 3 && img.cols > 3);
|
|
|
+
|
|
|
+ cv::Mat marker = cv::Mat::zeros(img.size(), CV_8UC1);
|
|
|
+
|
|
|
+ int nRows = img.rows;
|
|
|
+ int nCols = img.cols;
|
|
|
+
|
|
|
+ if (img.isContinuous()) {
|
|
|
+ nCols *= nRows;
|
|
|
+ nRows = 1;
|
|
|
+ }
|
|
|
+
|
|
|
+ int x, y;
|
|
|
+ uchar *pAbove;
|
|
|
+ uchar *pCurr;
|
|
|
+ uchar *pBelow;
|
|
|
+ uchar *nw, *no, *ne; // north (pAbove)
|
|
|
+ uchar *we, *me, *ea;
|
|
|
+ uchar *sw, *so, *se; // south (pBelow)
|
|
|
+
|
|
|
+ uchar *pDst;
|
|
|
+
|
|
|
+ // initialize row pointers
|
|
|
+ pAbove = NULL;
|
|
|
+ pCurr = img.ptr<uchar>(0);
|
|
|
+ pBelow = img.ptr<uchar>(1);
|
|
|
+
|
|
|
+ for (y = 1; y < img.rows - 1; ++y) {
|
|
|
+ // shift the rows up by one
|
|
|
+ pAbove = pCurr;
|
|
|
+ pCurr = pBelow;
|
|
|
+ pBelow = img.ptr<uchar>(y + 1);
|
|
|
+
|
|
|
+ pDst = marker.ptr<uchar>(y);
|
|
|
+
|
|
|
+ // initialize col pointers
|
|
|
+ no = &(pAbove[0]);
|
|
|
+ ne = &(pAbove[1]);
|
|
|
+ me = &(pCurr[0]);
|
|
|
+ ea = &(pCurr[1]);
|
|
|
+ so = &(pBelow[0]);
|
|
|
+ se = &(pBelow[1]);
|
|
|
+
|
|
|
+ for (x = 1; x < img.cols - 1; ++x) {
|
|
|
+ // shift col pointers left by one (scan left to right)
|
|
|
+ nw = no;
|
|
|
+ no = ne;
|
|
|
+ ne = &(pAbove[x + 1]);
|
|
|
+ we = me;
|
|
|
+ me = ea;
|
|
|
+ ea = &(pCurr[x + 1]);
|
|
|
+ sw = so;
|
|
|
+ so = se;
|
|
|
+ se = &(pBelow[x + 1]);
|
|
|
+
|
|
|
+ int A = (*no == 0 && *ne == 1) + (*ne == 0 && *ea == 1) +
|
|
|
+ (*ea == 0 && *se == 1) + (*se == 0 && *so == 1) +
|
|
|
+ (*so == 0 && *sw == 1) + (*sw == 0 && *we == 1) +
|
|
|
+ (*we == 0 && *nw == 1) + (*nw == 0 && *no == 1);
|
|
|
+ int B = *no + *ne + *ea + *se + *so + *sw + *we + *nw;
|
|
|
+ int m1 = iter == 0 ? (*no * *ea * *so) : (*no * *ea * *we);
|
|
|
+ int m2 = iter == 0 ? (*ea * *so * *we) : (*no * *so * *we);
|
|
|
+
|
|
|
+ if (A == 1 && (B >= 2 && B <= 6) && m1 == 0 && m2 == 0)
|
|
|
+ pDst[x] = 1;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ img &= ~marker;
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+* Function for thinning the given binary image
|
|
|
+*
|
|
|
+* Parameters:
|
|
|
+* src The source image, binary with range = [0,255]
|
|
|
+* dst The destination image
|
|
|
+*/
|
|
|
+void CTeaSort::thinning(const cv::Mat& src, cv::Mat& dst)
|
|
|
+{
|
|
|
+ dst = src.clone();
|
|
|
+ dst /= 255; // convert to binary image
|
|
|
+
|
|
|
+ cv::Mat prev = cv::Mat::zeros(dst.size(), CV_8UC1);
|
|
|
+ cv::Mat diff;
|
|
|
+
|
|
|
+ do {
|
|
|
+ thinningIteration(dst, 0);
|
|
|
+ thinningIteration(dst, 1);
|
|
|
+ cv::absdiff(dst, prev, diff);
|
|
|
+ dst.copyTo(prev);
|
|
|
+ } while (cv::countNonZero(diff) > 0);
|
|
|
+
|
|
|
+ dst *= 255;
|
|
|
+}
|
|
|
}
|
chenhongjiang