2021-10-12

import cv2 as cv
import numpy as np
import math

# img = cv.imread("/home/oision/Downloads/1633882892520.jpg")
# img = cv.imread("/home/oision/Downloads/1633955688430.jpg")
# img = cv.imread("/home/oision/Downloads/1633955760577.jpg")
# img = cv.imread("/home/oision/Downloads/1633955688411.jpg")
# img = cv.imread("/home/oision/Downloads/2.png")
# img = cv.imread("/home/oision/Downloads/3.png")
# img = cv.imread("/home/oision/Downloads/4.png")
# img = cv.imread("/home/oision/Downloads/5.png")
# img = cv.imread("/home/oision/Downloads/6.png")
# img = cv.imread("/home/oision/Downloads/7.png")
img = cv.imread("/home/oision/Downloads/8.png")
# img = cv.imread("/home/oision/Downloads/one.png")
# img = cv.resize(img, (300, 400))
cv.imshow("img", img)
img_cp1 = img.copy()
img_cp2 = img.copy()
img_cp3 = img.copy()
img_cp4 = img.copy()

img_cp1 = cv.cvtColor(img_cp1, cv.COLOR_BGR2HSV)
# LowerBlue = np.array([95, 90, 80])
# UpperBlue = np.array([130, 255, 255])
LowerBlue = np.array([95, 145, 90])
UpperBlue = np.array([130, 255, 255])
mask = cv.inRange(img_cp1, LowerBlue, UpperBlue)
img_mask = cv.bitwise_and(img_cp1, img_cp1, mask=mask)
img_mask = cv.cvtColor(img_mask, cv.COLOR_HSV2BGR)
cv.imshow("img_mask", img_mask)


img_canny = cv.Canny(img_mask, 0, 250)
cv.imshow("img_canny", img_canny)

# img_gray = cv.cvtColor(img_mask, cv.COLOR_BGR2GRAY)
# cv.imshow("img_gray", img_gray)
#
# # medianBlur
# img_blur = cv.medianBlur(img_gray, 11)
# cv.imshow("img_blur", img_blur)
#
# img_sobelx = cv.Sobel(img_blur, cv.CV_16S, 1, 0, ksize=5)
# img_sobely = cv.Sobel(img_blur, cv.CV_16S, 0, 1, ksize=5)
# absX = cv.convertScaleAbs(img_sobelx)
# absY = cv.convertScaleAbs(img_sobely)
# img_sobel = cv.addWeighted(absX, 0.5, absY, 0.5, 0)
# cv.imshow("img_sobel", img_sobel)
#
# ret, img_threshold = cv.threshold(img_sobel, 60, 255, cv.THRESH_BINARY)
# cv.imshow("img_threshold", img_threshold)
# # convertScaleAbs(grad_x, abs_grad_x);
# # abs_grad_x.convertTo(grayImage_x, CV_8U);
#
# img_th_blur = cv.medianBlur(img_threshold, 5)
# cv.imshow("img_th_blur", img_th_blur)

# kernel1 = cv.getStructuringElement(cv.MORPH_CROSS, (50, 50))
# kernel2 = cv.getStructuringElement(cv.MORPH_CROSS, (3, 3))
# img_dilated = cv.dilate(img_th_blur, kernel1)
# cv.imshow("img_dilated", img_dilated)
# img_eroded = cv.erode(img_dilated, kernel2)
# cv.imshow("img_eroded", img_eroded)


kernel1 = cv.getStructuringElement(cv.MORPH_CROSS, (40, 40))
kernel2 = cv.getStructuringElement(cv.MORPH_CROSS, (3, 3))
# compare
img_canny_dilated = cv.dilate(img_canny, kernel1)
cv.imshow("img_canny_dilated", img_canny_dilated)
img_canny_eroded = cv.erode(img_canny_dilated, kernel2)
cv.imshow("img_canny_eroded", img_canny_eroded)

# binary, contours, hierarchy = cv.findContours(img_eroded, cv.RETR_LIST, cv.CHAIN_APPROX_SIMPLE)
# # c = sorted(contours, key=cv.contourArea, reverse=True)
# # print contours
# img_cp1 = cv.cvtColor(img_cp1, cv.COLOR_HSV2BGR)
# cv.drawContours(img_cp1, contours, -1, (0, 0, 255), 3)
# cv.imshow("img_cp1", img_cp1)


binary, contours, hierarchy = cv.findContours(img_canny_eroded, cv.RETR_LIST, cv.CHAIN_APPROX_SIMPLE)
# binary, contours, hierarchy = cv.findContours(img_eroded, cv.RETR_LIST, cv.CHAIN_APPROX_SIMPLE)
# c = sorted(contours, key=cv.contourArea, reverse=True)
# print contours
area_contours = []
for i in range(len(contours)):
    area = cv.contourArea(contours[i])
    print area
    if (area > 50) and (area < 10000):
        area_contours.append(contours[i])
# print len(area_contours)
# print area_contours
# cv.drawContours(img_contours[i], contours, i, (255, 255, 255), -1)
cv.drawContours(img_cp2, area_contours, -1, (0, 0, 255), 3)
cv.imshow("img_cp2", img_cp2)
# ep = 0.1 * cv.arcLength(c[len(c) - 1], True)
# approx = cv.approxPolyDP(c[len(c) - 1], ep, True)
# img_approx = cv.drawContours(img.copy(), [approx], 0, (0, 255, 0), 2)
# cv.imshow("img_approx", img_approx)
max_contours = []
area = cv.contourArea(area_contours[0])
max_contour = area
max = 0
for i in range(len(area_contours)):
    # print area
    area_i = cv.contourArea(area_contours[i])
    if area_i > max_contour:
        max_contour = area_i
        max = i
max_contours.append(area_contours[max])
cv.drawContours(img_cp3, max_contours, -1, (0, 0, 255), 3)
cv.imshow("img_cp3", img_cp3)

# if find contours, pick the biggest box
# if len(contours) > 0:
# size = []
# size_max = 0
# for i, c in enumerate(max_contours):
rect = cv.minAreaRect(area_contours[max])
box = cv.boxPoints(rect)
box = np.int0(box)
x_mid = (box[0][0] + box[2][0] + box[1][0] + box[3][0]) / 4
y_mid = (box[0][1] + box[2][1] + box[1][1] + box[3][1]) / 4
w = math.sqrt((box[0][0] - box[1][0]) ** 2 + (box[0][1] - box[1][1]) ** 2)
h = math.sqrt((box[0][0] - box[3][0]) ** 2 + (box[0][1] - box[3][1]) ** 2)
# size.append(w * h)
# print x_mid, y_mid, w, h
point = (x_mid, y_mid)
cv.circle(img_cp4, point, 2, (0, 0, 255), 2)
text = "key point"
cv.putText(img_cp4, text, (x_mid, y_mid + 20), cv.FONT_HERSHEY_PLAIN, 1, (0, 0, 255), 1)
cv.imshow("img_cp4", img_cp4)
# if size[i] > size_max:
#     size_max = size[i]
#     index = i
#     xc = x_mid
#     yc = y_mid
# if box is not moving for 20 times
# print found_count
# if found_count >= 30:
#     self.is_found_object = True
#     cmd_vel = Twist()
#     self.cmd_vel_pub.publish(cmd_vel)
# else:
#     # if box is not moving
#     if abs(xc - xc_prev) <= 2 and abs(yc - yc_prev) <= 2:
#         found_count = found_count + 1
#     else:
#         found_count = 0
# else:
# found_count = 0
# xc_prev = xc
# yc_prev = yc

cv.waitKey(0)