def __init__(self):
self.node_name = "Line Detector"
self.image_size = rospy.get_param('~img_size')
self.top_cutoff = rospy.get_param('~top_cutoff')
self.bridge = CvBridge()
self.detector = LineDetector()
self.detector.hsv_white1 = np.array(rospy.get_param('~hsv_white1'))
self.detector.hsv_white2 = np.array(rospy.get_param('~hsv_white2'))
self.detector.hsv_yellow1 = np.array(rospy.get_param('~hsv_yellow1'))
self.detector.hsv_yellow2 = np.array(rospy.get_param('~hsv_yellow2'))
self.detector.hsv_red1 = np.array(rospy.get_param('~hsv_red1'))
self.detector.hsv_red2 = np.array(rospy.get_param('~hsv_red2'))
self.detector.hsv_red3 = np.array(rospy.get_param('~hsv_red3'))
self.detector.hsv_red4 = np.array(rospy.get_param('~hsv_red4'))
self.detector.dilation_kernel_size = rospy.get_param('~dilation_kernel_size')
self.detector.canny_thresholds = rospy.get_param('~canny_thresholds')
self.detector.hough_min_line_length = rospy.get_param('~hough_min_line_length')
self.detector.hough_max_line_gap = rospy.get_param('~hough_max_line_gap')
self.detector.hough_threshold = rospy.get_param('~hough_threshold')
self.sub_image = rospy.Subscriber("~image", CompressedImage, self.processImage)
self.pub_lines = rospy.Publisher("~segment_list", SegmentList, queue_size=1)
self.pub_image = rospy.Publisher("~image_with_lines", Image, queue_size=1)
def __init__(self):
self.node_name = "Line Detector"
# Thread lock
self.thread_lock = threading.Lock()
# Constructor of line detector
self.bridge = CvBridge()
self.detector = LineDetector()
self.wb = WhiteBalance()
self.flag_wb_ref = False
# Parameters
self.flag_wb = rospy.get_param('~white_balance')
self.image_size = rospy.get_param('~img_size')
self.top_cutoff = rospy.get_param('~top_cutoff')
self.detector.hsv_white1 = np.array(rospy.get_param('~hsv_white1'))
self.detector.hsv_white2 = np.array(rospy.get_param('~hsv_white2'))
self.detector.hsv_yellow1 = np.array(rospy.get_param('~hsv_yellow1'))
self.detector.hsv_yellow2 = np.array(rospy.get_param('~hsv_yellow2'))
self.detector.hsv_red1 = np.array(rospy.get_param('~hsv_red1'))
self.detector.hsv_red2 = np.array(rospy.get_param('~hsv_red2'))
self.detector.hsv_red3 = np.array(rospy.get_param('~hsv_red3'))
self.detector.hsv_red4 = np.array(rospy.get_param('~hsv_red4'))
self.detector.dilation_kernel_size = rospy.get_param(
'~dilation_kernel_size')
self.detector.canny_thresholds = rospy.get_param('~canny_thresholds')
self.detector.hough_min_line_length = rospy.get_param(
'~hough_min_line_length')
self.detector.hough_max_line_gap = rospy.get_param(
'~hough_max_line_gap')
self.detector.hough_threshold = rospy.get_param('~hough_threshold')
# Publishers
self.pub_lines = rospy.Publisher("~segment_list",
SegmentList,
queue_size=1)
self.pub_image = rospy.Publisher("~image_with_lines",
Image,
queue_size=1)
# Verbose option
self.verbose = rospy.get_param('~verbose')
if self.verbose:
self.toc_pre = rospy.get_time()
# Subscribers
self.sub_image = rospy.Subscriber("~image",
CompressedImage,
self.cbImage,
queue_size=1)
rospy.loginfo("[%s] Initialized." % (self.node_name))
def main():
line_finder = LineDetector(**settings)
if args.file_.split('.')[-1] == 'jpg':
img = mpimg.imread(args.path + args.file_)
output_img = line_finder.forward(img)
plt.imshow(output_img)
plt.show()
elif args.file_.split('.')[-1] == 'mp4':
white_output = 'test_videos_output/output_video.mp4'
clip1 = VideoFileClip(args.path + args.file_).subclip(0, args.subclip)
white_clip = clip1.fl_image(line_finder.forward)
white_clip.write_videofile(white_output, audio=False)
else:
print('Error {}: Not supported type.'.format(
args.file_.split('.')[-1]))
quit()
def __init__(self):
self.node_name = "Line Detector"
self.image_size = rospy.get_param('~img_size')
self.top_cutoff = rospy.get_param('~top_cutoff')
self.bridge = CvBridge()
self.detector = LineDetector()
self.detector.hsv_white1 = np.array(rospy.get_param('~hsv_white1'))
self.detector.hsv_white2 = np.array(rospy.get_param('~hsv_white2'))
self.detector.hsv_yellow1 = np.array(rospy.get_param('~hsv_yellow1'))
self.detector.hsv_yellow2 = np.array(rospy.get_param('~hsv_yellow2'))
self.detector.hsv_red1 = np.array(rospy.get_param('~hsv_red1'))
self.detector.hsv_red2 = np.array(rospy.get_param('~hsv_red2'))
self.detector.hsv_red3 = np.array(rospy.get_param('~hsv_red3'))
self.detector.hsv_red4 = np.array(rospy.get_param('~hsv_red4'))
self.detector.dilation_kernel_size = rospy.get_param('~dilation_kernel_size')
self.detector.canny_thresholds = rospy.get_param('~canny_thresholds')
self.detector.hough_min_line_length = rospy.get_param('~hough_min_line_length')
self.detector.hough_max_line_gap = rospy.get_param('~hough_max_line_gap')
self.detector.hough_threshold = rospy.get_param('~hough_threshold')
self.sub_image = rospy.Subscriber("~image", CompressedImage, self.processImage)
self.pub_lines = rospy.Publisher("~segment_list", SegmentList, queue_size=1)
self.pub_image = rospy.Publisher("~image_with_lines", Image, queue_size=1)
self.timer_param = rospy.Timer(rospy.Duration.from_sec(3.0),self.cbParamUpdate)
def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing......" % (self.node_name))
self.bridge = CvBridge()
self.cv_image = None
self.detector = LineDetector()
self.visualization = True
self.size = (320, 240)
self.pid_enabled = False
self.base_speed = 50
# self.car = CarDriver()
# self.config_file = os.path.dirname(os.path.abspath(__file__)) + "/default.yaml"
self.image_msg = None
self.lost_count = 0
self.line_msg = GetLineDetectionResponse()
self.pub_detections = rospy.Publisher("~image_color",
CompressedImage,
queue_size=1)
self.pub_line_detection = rospy.Publisher("~line_detection",
LineDetection,
queue_size=1)
self.detect_line_srv = rospy.Service('~detect_line', GetLineDetection,
self.cbGetLineDetection)
self.set_color_srv = rospy.Service('~set_color_threshold',
SetColorThreshold,
self.cbSetColorThreshold)
self.get_color_srv = rospy.Service('~get_color_threshold',
GetColorThreshold,
self.cbGetColorThreshold)
self.set_color_list_srv = rospy.Service('~get_color_list', GetStrings,
self.cbGetColorList)
# self.pid_set_enable_srv = rospy.Service('~pid_set_enable', SetInt32, self.cbPidSetEnable)
# self.sub_image = rospy.Subscriber("~image_raw", Image, self.cbImg ,queue_size=1)
self.sub_image = rospy.Subscriber("~image_raw/compressed",
CompressedImage,
self.cbImg,
queue_size=1)
# rospy.loginfo("[%s] wait_for_service : camera_get_frame..." % (self.node_name))
# rospy.wait_for_service('~camera_get_frame')
# self.get_frame = rospy.ServiceProxy('~camera_get_frame', GetFrame)
rospy.loginfo("[%s] Initialized." % (self.node_name))
def __init__(self):
self.node_name = "Line Detector"
self.hei_image = self.setupParam("~hei_image", 240)
self.wid_image = self.setupParam("~wid_image", 320)
self.top_cutoff = self.setupParam("~top_cutoff", 90)
self.bridge = CvBridge()
self.detector = LineDetector()
self.sub_image = rospy.Subscriber("~image", CompressedImage, self.processImage)
self.pub_lines = rospy.Publisher("~segment_list", SegmentList, queue_size=1)
self.pub_image = rospy.Publisher("~image_with_lines", Image, queue_size=1)
def __init__(self):
self.bridge = CvBridge()
self.sub_image = rospy.Subscriber("~image", Image, self.processImage)
self.pub_lines = rospy.Publisher("~segment_list", SegmentList, queue_size=1)
self.pub_image = rospy.Publisher("~image_with_lines", Image, queue_size=1)
self.detector = LineDetector()
self.segmentList = SegmentList()
self.segment = Segment()
self.pixel1 = Pixel()
self.pixel2 = Pixel()
self.point1 = Point()
self.point2 = Point()
def __init__(self):
self.node_name = "Line Detector"
# Thread lock
self.thread_lock = threading.Lock()
# Constructor of line detector
self.bridge = CvBridge()
self.detector = LineDetector()
self.wb = WhiteBalance()
self.flag_wb_ref = False
# Parameters
self.flag_wb = rospy.get_param('~white_balance')
self.image_size = rospy.get_param('~img_size')
self.top_cutoff = rospy.get_param('~top_cutoff')
self.detector.hsv_white1 = np.array(rospy.get_param('~hsv_white1'))
self.detector.hsv_white2 = np.array(rospy.get_param('~hsv_white2'))
self.detector.hsv_yellow1 = np.array(rospy.get_param('~hsv_yellow1'))
self.detector.hsv_yellow2 = np.array(rospy.get_param('~hsv_yellow2'))
self.detector.hsv_red1 = np.array(rospy.get_param('~hsv_red1'))
self.detector.hsv_red2 = np.array(rospy.get_param('~hsv_red2'))
self.detector.hsv_red3 = np.array(rospy.get_param('~hsv_red3'))
self.detector.hsv_red4 = np.array(rospy.get_param('~hsv_red4'))
self.detector.dilation_kernel_size = rospy.get_param('~dilation_kernel_size')
self.detector.canny_thresholds = rospy.get_param('~canny_thresholds')
self.detector.hough_min_line_length = rospy.get_param('~hough_min_line_length')
self.detector.hough_max_line_gap = rospy.get_param('~hough_max_line_gap')
self.detector.hough_threshold = rospy.get_param('~hough_threshold')
# Publishers
self.pub_lines = rospy.Publisher("~segment_list", SegmentList, queue_size=1)
self.pub_image = rospy.Publisher("~image_with_lines", Image, queue_size=1)
# Verbose option
self.verbose = rospy.get_param('~verbose')
if self.verbose:
self.toc_pre = rospy.get_time()
# Subscribers
self.sub_image = rospy.Subscriber("~image", CompressedImage, self.cbImage, queue_size=1)
rospy.loginfo("[%s] Initialized." %(self.node_name))
class LineDetectorNode(object):
def __init__(self):
self.bridge = CvBridge()
self.sub_image = rospy.Subscriber("~image", Image, self.processImage)
self.pub_lines = rospy.Publisher("~segment_list", SegmentList, queue_size=1)
self.pub_image = rospy.Publisher("~image_with_lines", Image, queue_size=1)
self.detector = LineDetector()
self.segmentList = SegmentList()
self.segment = Segment()
self.pixel1 = Pixel()
self.pixel2 = Pixel()
self.point1 = Point()
self.point2 = Point()
def processImage(self, image_msg):
image_cv = self.bridge.imgmsg_to_cv2(image_msg, "bgr8")
# Resize image
image_cv = cv2.resize(image_cv, (200, 150))
image_cv = image_cv[image_cv.shape[0] / 2 :, :, :]
# Detect lines and normals
lines_white, normals_white = self.detector.detectLines(image_cv, "white")
lines_yellow, normals_yellow = self.detector.detectLines(image_cv, "yellow")
lines_red, normals_red = self.detector.detectLines(image_cv, "red")
# Draw lines and normals
image_with_lines_cv = image_cv
self.detector.drawLines(image_with_lines_cv, lines_white, (0, 0, 0))
self.detector.drawLines(image_with_lines_cv, lines_yellow, (255, 0, 0))
self.detector.drawLines(image_with_lines_cv, lines_red, (0, 255, 0))
self.detector.drawNormals(image_with_lines_cv, lines_white, normals_white)
self.detector.drawNormals(image_with_lines_cv, lines_yellow, normals_yellow)
self.detector.drawNormals(image_with_lines_cv, lines_red, normals_red)
# TODO: Pixel frame to body frame covnersion
# Publish the segments
if len(lines_white) > 0:
self.publishSegmentList(lines_white, normals_white, self.segmentList.WHITE)
rospy.loginfo("[LineDetectorNode] number of white lines = %s" % (len(lines_white)))
if len(lines_yellow) > 0:
self.publishSegmentList(lines_yellow, normals_yellow, self.segmentList.YELLOW)
rospy.loginfo("[LineDetectorNode] number of yellow lines = %s" % (len(lines_yellow)))
if len(lines_red) > 0:
self.publishSegmentList(lines_red, normals_red, self.segmentList.RED)
rospy.loginfo("[LineDetectorNode] number of red lines = %s" % (len(lines_red)))
# Publish the frame with lines
image_msg = self.bridge.cv2_to_imgmsg(image_with_lines_cv, "bgr8")
self.pub_image.publish(image_msg)
def onShutdown(self):
rospy.loginfo("[LineDetectorNode] Shutdown.")
def publishSegmentList(self, lines, normals, color):
self.segmentList.segments = []
self.segmentList.color = color
for u1, v1, u2, v2, norm_u, norm_v in np.hstack(lines, normals):
self.pixel1.u = int(u1)
self.pixel1.v = int(v1)
self.pixel2.u = int(u2)
self.pixel2.v = int(v2)
self.segment.pixels[0] = self.pixel1
self.segment.pixels[1] = self.pixel2
self.segment.normal_u = norm_u
self.segment.normal_v = norm_v
# TODO: assign segment.points
self.segmentList.segments.append(self.segment)
self.pub_lines.publish(self.segmentList)
class LineDetectorNode(object):
def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing......" % (self.node_name))
self.bridge = CvBridge()
self.cv_image = None
self.detector = LineDetector()
self.visualization = True
self.size = (320, 240)
self.pid_enabled = False
self.base_speed = 50
# self.car = CarDriver()
# self.config_file = os.path.dirname(os.path.abspath(__file__)) + "/default.yaml"
self.image_msg = None
self.lost_count = 0
self.line_msg = GetLineDetectionResponse()
self.pub_detections = rospy.Publisher("~image_color",
CompressedImage,
queue_size=1)
self.pub_line_detection = rospy.Publisher("~line_detection",
LineDetection,
queue_size=1)
self.detect_line_srv = rospy.Service('~detect_line', GetLineDetection,
self.cbGetLineDetection)
self.set_color_srv = rospy.Service('~set_color_threshold',
SetColorThreshold,
self.cbSetColorThreshold)
self.get_color_srv = rospy.Service('~get_color_threshold',
GetColorThreshold,
self.cbGetColorThreshold)
self.set_color_list_srv = rospy.Service('~get_color_list', GetStrings,
self.cbGetColorList)
# self.pid_set_enable_srv = rospy.Service('~pid_set_enable', SetInt32, self.cbPidSetEnable)
# self.sub_image = rospy.Subscriber("~image_raw", Image, self.cbImg ,queue_size=1)
self.sub_image = rospy.Subscriber("~image_raw/compressed",
CompressedImage,
self.cbImg,
queue_size=1)
# rospy.loginfo("[%s] wait_for_service : camera_get_frame..." % (self.node_name))
# rospy.wait_for_service('~camera_get_frame')
# self.get_frame = rospy.ServiceProxy('~camera_get_frame', GetFrame)
rospy.loginfo("[%s] Initialized." % (self.node_name))
def cbImg(self, image_msg):
self.image_msg = image_msg
# self.line_msg = self.detectLine()
if self.pid_enabled:
self.cbPid(self.line_msg)
# self.pub_line_detection.publish(self.line_msg)
'''
def cbPid(self,line_msg):
center = line_msg.center
if center[0] == 0:
self.lost_count = self.lost_count + 1
print('lost count %d' % (self.lost_count))
if self.lost_count > 25:
self.pid_enabled = False
print('yellow line lost,stop')
self.car.setWheelsSpeed(0,0)
return
self.lost_count = 0
offset = 70 - center[0]
bias = offset/160.0
speed = self.base_speed/100.0*(1-bias)
if speed < 0.4:
speed = 0.4
left = speed*(1-bias)
right = speed*(1+bias)
self.car.setWheelsSpeed(left,right)
'''
def detectLine(self, params):
# start = time.time()
# print(params)
color = params.color.decode('utf-8')
if not self.detector.colors.has_key(color):
return GetLineDetectionResponse()
image_msg = self.image_msg
if image_msg == None:
print('[%s]: camera msg not received' % (self.node_name))
return GetLineDetectionResponse()
if hasattr(image_msg, 'format'): # CompressedImage
try:
cv_image = bgr_from_jpg(image_msg.data)
except ValueError as e:
rospy.loginfo('[%s] cannot decode image: %s' %
(self.node_name, e))
return GetLineDetectionResponse()
else: # Image
try:
cv_image = self.bridge.imgmsg_to_cv2(image_msg,
desired_encoding="bgr8")
except Exception as e:
rospy.loginfo('[%s] cannot convert image: %s' %
(self.node_name, e))
return GetLineDetectionResponse()
# if cv_image.shape[0]!=self.size[1] or cv_image.shape[1]!=self.size[0]:
# cv_image = cv2.resize(cv_image,self.size)
if params.y1 < params.y2 and params.x1 < params.x2:
rect_image = cv_image[params.y1:params.y2, params.x1:params.x2]
detection, image = self.detector.detect_hsv(
rect_image, self.detector.colors[color])
if self.visualization:
cv_image[params.y1:params.y2, params.x1:params.x2] = image
cv2.rectangle(cv_image, (params.x1, params.y1),
(params.x2, params.y2), (0, 0, 0), 1)
# cv_image = cv2.resize(cv_image,(480,360))
# imgmsg = self.bridge.cv2_to_imgmsg(cv_image,"bgr8")
# self.pub_detections.publish(imgmsg)
self.pubImage(cv_image)
end = time.time()
# print('time cost in detect line: %.2f ms' % ((end - start)*1000))
return GetLineDetectionResponse(detection[0:2], detection[2:4],
detection[4])
def toLineDetections(self, cnt):
if cnt is not None:
center, wh, angle = cv2.minAreaRect(cnt)
return GetLineDetectionResponse(center, wh, angle)
return GetLineDetectionResponse()
def cbGetLineDetection(self, params):
try:
line_msg = self.detectLine(params)
return line_msg
except Exception as e:
print(self.node_name)
print(e)
return GetLineDetectionResponse()
# return self.line_msg
def cbSetColorThreshold(self, params):
print(params)
try:
self.detector.colors[params.color.decode('utf-8')] = [{
"min": [params.low_h, params.low_s, params.low_v],
"max": [params.high_h, params.high_s, params.high_v]
}]
return SetColorThresholdResponse("设置成功")
except Exception as e:
print(self.node_name)
print(e)
return SetColorThresholdResponse("设置失败,请检查参数")
def cbGetColorThreshold(self, params):
try:
threshold = self.detector.colors[params.color.decode('utf-8')]
return GetColorThresholdResponse(str(threshold))
except Exception as e:
print(self.node_name)
print(e)
return GetColorThresholdResponse()
def cbGetColorList(self, params):
return GetStringsResponse(self.detector.colors.keys())
'''
def cbPidSetEnable(self,params):
if params.port == 1:
self.pid_enabled = True
self.base_speed = params.value
else:
self.pid_enabled = True
self.base_speed = 0
self.car.setWheelsSpeed(0,0)
return SetInt32Response(params.port,params.value)
'''
def pubImage(self, image):
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = jpg_from_bgr(image)
self.pub_detections.publish(msg)
def onShutdown(self):
rospy.loginfo("[%s] Shutdown." % (self.node_name))
# Note for vz:
# vz < 0 => ascend
# vz > 0 => descend
# UP = -0.5
# DOWN = 0.5
UP = 0
DOWN = 0
PRECISION = 3
YAW_CONTROL = True
ABS_MAX_TURNING_ANGLE = 75
ld = LineDetector(PRECISION)
arm_and_takeoff(1.5)
print("!!!!!!!!!!! MOVING START !!!!!!!!!")
while True:
# getTurnDir returns
dir = ld.getTurnDir()
print("getTurnDir returns %d" % dir)
# If there is no line, land the vehicle
if dir == (PRECISION + 1):
break
# If YAW_CONTROL is true, it is possible to trace curves.
class LineDetectorNode(object):
def __init__(self):
self.node_name = "Line Detector"
# Thread lock
self.thread_lock = threading.Lock()
# Constructor of line detector
self.bridge = CvBridge()
self.detector = LineDetector()
self.wb = WhiteBalance()
self.flag_wb_ref = False
# Parameters
self.flag_wb = rospy.get_param('~white_balance')
self.image_size = rospy.get_param('~img_size')
self.top_cutoff = rospy.get_param('~top_cutoff')
self.detector.hsv_white1 = np.array(rospy.get_param('~hsv_white1'))
self.detector.hsv_white2 = np.array(rospy.get_param('~hsv_white2'))
self.detector.hsv_yellow1 = np.array(rospy.get_param('~hsv_yellow1'))
self.detector.hsv_yellow2 = np.array(rospy.get_param('~hsv_yellow2'))
self.detector.hsv_red1 = np.array(rospy.get_param('~hsv_red1'))
self.detector.hsv_red2 = np.array(rospy.get_param('~hsv_red2'))
self.detector.hsv_red3 = np.array(rospy.get_param('~hsv_red3'))
self.detector.hsv_red4 = np.array(rospy.get_param('~hsv_red4'))
self.detector.dilation_kernel_size = rospy.get_param('~dilation_kernel_size')
self.detector.canny_thresholds = rospy.get_param('~canny_thresholds')
self.detector.hough_min_line_length = rospy.get_param('~hough_min_line_length')
self.detector.hough_max_line_gap = rospy.get_param('~hough_max_line_gap')
self.detector.hough_threshold = rospy.get_param('~hough_threshold')
# Publishers
self.pub_lines = rospy.Publisher("~segment_list", SegmentList, queue_size=1)
self.pub_image = rospy.Publisher("~image_with_lines", Image, queue_size=1)
# Verbose option
self.verbose = rospy.get_param('~verbose')
if self.verbose:
self.toc_pre = rospy.get_time()
# Subscribers
self.sub_image = rospy.Subscriber("~image", CompressedImage, self.cbImage, queue_size=1)
rospy.loginfo("[%s] Initialized." %(self.node_name))
def cbImage(self,image_msg):
# Start a daemon thread to process the image
thread = threading.Thread(target=self.processImage,args=(image_msg,))
thread.setDaemon(True)
thread.start()
# Returns rightaway
def processImage(self,image_msg):
if not self.thread_lock.acquire(False):
# Return immediately if the thread is locked
return
# Verbose
if self.verbose:
rospy.loginfo("[%s] Latency received = %.3f ms" %(self.node_name, (rospy.get_time()-image_msg.header.stamp.to_sec()) * 1000.0))
# time_start = rospy.Time.now()
# time_start = event.last_real
# msg_age = time_start - image_msg.header.stamp
# rospy.loginfo("[LineDetector] image age: %s" %msg_age.to_sec())
# Decode from compressed image
# with OpenCV
image_cv = cv2.imdecode(np.fromstring(image_msg.data, np.uint8), cv2.CV_LOAD_IMAGE_COLOR)
# with PIL Image
# image_cv = jpeg.JPEG(np.fromstring(image_msg.data, np.uint8)).decode()
# with libjpeg-turbo
# Convert from uncompressed image message
# image_cv = self.bridge.imgmsg_to_cv2(image_msg, "bgr8")
# Verbose
if self.verbose:
self.tic = rospy.get_time()
rospy.loginfo("[%s] Latency image decompressed = %.3f ms" %(self.node_name, (self.tic-image_msg.header.stamp.to_sec()) * 1000.0))
# White balancing: set reference image to estimate parameters
if self.flag_wb and (not self.flag_wb_ref):
# set reference image to estimate parameters
self.wb.setRefImg(image_cv)
rospy.loginfo("[%s] White balance: parameters computed." %(self.node_name))
print self.wb.norm_bgr
self.flag_wb_ref = True
# Resize and crop image
hei_original = image_cv.shape[0]
wid_original = image_cv.shape[1]
if self.image_size[0]!=hei_original or self.image_size[1]!=wid_original:
# image_cv = cv2.GaussianBlur(image_cv, (5,5), 2)
image_cv = cv2.resize(image_cv, (self.image_size[1], self.image_size[0]), interpolation=cv2.INTER_NEAREST)
image_cv = image_cv[self.top_cutoff:,:,:]
# White balancing
if self.flag_wb and self.flag_wb_ref:
self.wb.correctImg(image_cv)
# Set the image to be detected
self.detector.setImage(image_cv)
# Detect lines and normals
lines_white, normals_white = self.detector.detectLines('white')
lines_yellow, normals_yellow = self.detector.detectLines('yellow')
lines_red, normals_red = self.detector.detectLines('red')
# Draw lines and normals
self.detector.drawLines(lines_white, (0,0,0))
self.detector.drawLines(lines_yellow, (255,0,0))
self.detector.drawLines(lines_red, (0,255,0))
#self.detector.drawNormals(lines_white, normals_white)
#self.detector.drawNormals(lines_yellow, normals_yellow)
#self.detector.drawNormals(lines_red, normals_red)
# SegmentList constructor
segmentList = SegmentList()
segmentList.header.stamp = image_msg.header.stamp
# Convert to normalized pixel coordinates, and add segments to segmentList
arr_cutoff = np.array((0, self.top_cutoff, 0, self.top_cutoff))
arr_ratio = np.array((1./self.image_size[1], 1./self.image_size[0], 1./self.image_size[1], 1./self.image_size[0]))
if len(lines_white)>0:
lines_normalized_white = ((lines_white + arr_cutoff) * arr_ratio)
segmentList.segments.extend(self.toSegmentMsg(lines_normalized_white, normals_white, Segment.WHITE))
if len(lines_yellow)>0:
lines_normalized_yellow = ((lines_yellow + arr_cutoff) * arr_ratio)
segmentList.segments.extend(self.toSegmentMsg(lines_normalized_yellow, normals_yellow, Segment.YELLOW))
if len(lines_red)>0:
lines_normalized_red = ((lines_red + arr_cutoff) * arr_ratio)
segmentList.segments.extend(self.toSegmentMsg(lines_normalized_red, normals_red, Segment.RED))
# Verbose
if self.verbose:
self.toc = rospy.get_time()
rospy.loginfo("[%s] Image processing time: %.3f ms" %(self.node_name, (self.toc-self.tic)*1000.0))
rospy.loginfo("[%s] Number of white segments = %d" %(self.node_name, len(lines_white)))
rospy.loginfo("[%s] number of yellow segments = %d" %(self.node_name, len(lines_yellow)))
rospy.loginfo("[%s] number of red segments = %d" %(self.node_name, len(lines_red)))
self.toc_pre = self.toc
# Publish segmentList
self.pub_lines.publish(segmentList)
# time_spent = rospy.Time.now() - time_start
# rospy.loginfo("[LineDetectorNode] Spent: %s" %(time_spent.to_sec()))
# Publish the frame with lines
image_msg_out = self.bridge.cv2_to_imgmsg(self.detector.getImage(), "bgr8")
image_msg_out.header.stamp = image_msg.header.stamp
self.pub_image.publish(image_msg_out)
# time_spent = rospy.Time.now() - time_start
# rospy.loginfo("[LineDetectorNode] Spent on img: %s" %(time_spent.to_sec()))
# Verbose
if self.verbose:
rospy.loginfo("[%s] Latency sent = %.3f ms" %(self.node_name, (rospy.get_time()-image_msg.header.stamp.to_sec()) * 1000.0))
# Release the thread lock
self.thread_lock.release()
def onShutdown(self):
rospy.loginfo("[LineDetectorNode] Shutdown.")
def toSegmentMsg(self, lines, normals, color):
segmentMsgList = []
for x1,y1,x2,y2,norm_x,norm_y in np.hstack((lines,normals)):
segment = Segment()
segment.color = color
segment.pixels_normalized[0].x = x1
segment.pixels_normalized[0].y = y1
segment.pixels_normalized[1].x = x2
segment.pixels_normalized[1].y = y2
segment.normal.x = norm_x
segment.normal.y = norm_y
segmentMsgList.append(segment)
return segmentMsgList
class LineDetectorNode(object):
def __init__(self):
self.node_name = "Line Detector"
# Thread lock
self.thread_lock = threading.Lock()
# Constructor of line detector
self.bridge = CvBridge()
self.detector = LineDetector()
self.wb = WhiteBalance()
self.flag_wb_ref = False
# Parameters
self.flag_wb = rospy.get_param('~white_balance')
self.image_size = rospy.get_param('~img_size')
self.top_cutoff = rospy.get_param('~top_cutoff')
self.detector.hsv_white1 = np.array(rospy.get_param('~hsv_white1'))
self.detector.hsv_white2 = np.array(rospy.get_param('~hsv_white2'))
self.detector.hsv_yellow1 = np.array(rospy.get_param('~hsv_yellow1'))
self.detector.hsv_yellow2 = np.array(rospy.get_param('~hsv_yellow2'))
self.detector.hsv_red1 = np.array(rospy.get_param('~hsv_red1'))
self.detector.hsv_red2 = np.array(rospy.get_param('~hsv_red2'))
self.detector.hsv_red3 = np.array(rospy.get_param('~hsv_red3'))
self.detector.hsv_red4 = np.array(rospy.get_param('~hsv_red4'))
self.detector.dilation_kernel_size = rospy.get_param(
'~dilation_kernel_size')
self.detector.canny_thresholds = rospy.get_param('~canny_thresholds')
self.detector.hough_min_line_length = rospy.get_param(
'~hough_min_line_length')
self.detector.hough_max_line_gap = rospy.get_param(
'~hough_max_line_gap')
self.detector.hough_threshold = rospy.get_param('~hough_threshold')
# Publishers
self.pub_lines = rospy.Publisher("~segment_list",
SegmentList,
queue_size=1)
self.pub_image = rospy.Publisher("~image_with_lines",
Image,
queue_size=1)
# Verbose option
self.verbose = rospy.get_param('~verbose')
if self.verbose:
self.toc_pre = rospy.get_time()
# Subscribers
self.sub_image = rospy.Subscriber("~image",
CompressedImage,
self.cbImage,
queue_size=1)
rospy.loginfo("[%s] Initialized." % (self.node_name))
def cbImage(self, image_msg):
# Start a daemon thread to process the image
thread = threading.Thread(target=self.processImage, args=(image_msg, ))
thread.setDaemon(True)
thread.start()
# Returns rightaway
def processImage(self, image_msg):
if not self.thread_lock.acquire(False):
# Return immediately if the thread is locked
return
# Verbose
if self.verbose:
rospy.loginfo(
"[%s] Latency received = %.3f ms" %
(self.node_name,
(rospy.get_time() - image_msg.header.stamp.to_sec()) *
1000.0))
# time_start = rospy.Time.now()
# time_start = event.last_real
# msg_age = time_start - image_msg.header.stamp
# rospy.loginfo("[LineDetector] image age: %s" %msg_age.to_sec())
# Decode from compressed image
# with OpenCV
image_cv = cv2.imdecode(np.fromstring(image_msg.data, np.uint8),
cv2.CV_LOAD_IMAGE_COLOR)
# with PIL Image
# image_cv = jpeg.JPEG(np.fromstring(image_msg.data, np.uint8)).decode()
# with libjpeg-turbo
# Convert from uncompressed image message
# image_cv = self.bridge.imgmsg_to_cv2(image_msg, "bgr8")
# Verbose
if self.verbose:
self.tic = rospy.get_time()
rospy.loginfo(
"[%s] Latency image decompressed = %.3f ms" %
(self.node_name,
(self.tic - image_msg.header.stamp.to_sec()) * 1000.0))
# White balancing: set reference image to estimate parameters
if self.flag_wb and (not self.flag_wb_ref):
# set reference image to estimate parameters
self.wb.setRefImg(image_cv)
rospy.loginfo("[%s] White balance: parameters computed." %
(self.node_name))
print self.wb.norm_bgr
self.flag_wb_ref = True
# Resize and crop image
hei_original = image_cv.shape[0]
wid_original = image_cv.shape[1]
if self.image_size[0] != hei_original or self.image_size[
1] != wid_original:
# image_cv = cv2.GaussianBlur(image_cv, (5,5), 2)
image_cv = cv2.resize(image_cv,
(self.image_size[1], self.image_size[0]),
interpolation=cv2.INTER_NEAREST)
image_cv = image_cv[self.top_cutoff:, :, :]
# White balancing
if self.flag_wb and self.flag_wb_ref:
self.wb.correctImg(image_cv)
# Set the image to be detected
self.detector.setImage(image_cv)
# Detect lines and normals
lines_white, normals_white = self.detector.detectLines('white')
lines_yellow, normals_yellow = self.detector.detectLines('yellow')
lines_red, normals_red = self.detector.detectLines('red')
# Draw lines and normals
self.detector.drawLines(lines_white, (0, 0, 0))
self.detector.drawLines(lines_yellow, (255, 0, 0))
self.detector.drawLines(lines_red, (0, 255, 0))
#self.detector.drawNormals(lines_white, normals_white)
#self.detector.drawNormals(lines_yellow, normals_yellow)
#self.detector.drawNormals(lines_red, normals_red)
# SegmentList constructor
segmentList = SegmentList()
segmentList.header.stamp = image_msg.header.stamp
# Convert to normalized pixel coordinates, and add segments to segmentList
arr_cutoff = np.array((0, self.top_cutoff, 0, self.top_cutoff))
arr_ratio = np.array(
(1. / self.image_size[1], 1. / self.image_size[0],
1. / self.image_size[1], 1. / self.image_size[0]))
if len(lines_white) > 0:
lines_normalized_white = ((lines_white + arr_cutoff) * arr_ratio)
segmentList.segments.extend(
self.toSegmentMsg(lines_normalized_white, normals_white,
Segment.WHITE))
if len(lines_yellow) > 0:
lines_normalized_yellow = ((lines_yellow + arr_cutoff) * arr_ratio)
segmentList.segments.extend(
self.toSegmentMsg(lines_normalized_yellow, normals_yellow,
Segment.YELLOW))
if len(lines_red) > 0:
lines_normalized_red = ((lines_red + arr_cutoff) * arr_ratio)
segmentList.segments.extend(
self.toSegmentMsg(lines_normalized_red, normals_red,
Segment.RED))
# Verbose
if self.verbose:
self.toc = rospy.get_time()
rospy.loginfo("[%s] Image processing time: %.3f ms" %
(self.node_name, (self.toc - self.tic) * 1000.0))
rospy.loginfo("[%s] Number of white segments = %d" %
(self.node_name, len(lines_white)))
rospy.loginfo("[%s] number of yellow segments = %d" %
(self.node_name, len(lines_yellow)))
rospy.loginfo("[%s] number of red segments = %d" %
(self.node_name, len(lines_red)))
self.toc_pre = self.toc
# Publish segmentList
self.pub_lines.publish(segmentList)
# time_spent = rospy.Time.now() - time_start
# rospy.loginfo("[LineDetectorNode] Spent: %s" %(time_spent.to_sec()))
# Publish the frame with lines
image_msg_out = self.bridge.cv2_to_imgmsg(self.detector.getImage(),
"bgr8")
image_msg_out.header.stamp = image_msg.header.stamp
self.pub_image.publish(image_msg_out)
# time_spent = rospy.Time.now() - time_start
# rospy.loginfo("[LineDetectorNode] Spent on img: %s" %(time_spent.to_sec()))
# Verbose
if self.verbose:
rospy.loginfo(
"[%s] Latency sent = %.3f ms" %
(self.node_name,
(rospy.get_time() - image_msg.header.stamp.to_sec()) *
1000.0))
# Release the thread lock
self.thread_lock.release()
def onShutdown(self):
rospy.loginfo("[LineDetectorNode] Shutdown.")
def toSegmentMsg(self, lines, normals, color):
segmentMsgList = []
for x1, y1, x2, y2, norm_x, norm_y in np.hstack((lines, normals)):
segment = Segment()
segment.color = color
segment.pixels_normalized[0].x = x1
segment.pixels_normalized[0].y = y1
segment.pixels_normalized[1].x = x2
segment.pixels_normalized[1].y = y2
segment.normal.x = norm_x
segment.normal.y = norm_y
segmentMsgList.append(segment)
return segmentMsgList
def __init__(self, node_name):
# Initialize the DTROS parent class
super(LineDetectorNode, self).__init__(node_name=node_name,
node_type=NodeType.PERCEPTION)
# Define parameters
self._line_detector_parameters = rospy.get_param(
"~line_detector_parameters", None)
self._colors = rospy.get_param("~colors", None)
self._img_size = rospy.get_param("~img_size", None)
self._top_cutoff = rospy.get_param("~top_cutoff", None)
self.bridge = CvBridge()
# The thresholds to be used for AntiInstagram color correction
self.ai_thresholds_received = False
self.anti_instagram_thresholds = dict()
self.ai = AntiInstagram()
# This holds the colormaps for the debug/ranges images after they are computed once
self.colormaps = dict()
# Create a new LineDetector object with the parameters from the Parameter Server / config file
self.detector = LineDetector(**self._line_detector_parameters)
# Update the color ranges objects
self.color_ranges = {
color: ColorRange.fromDict(d)
for color, d in list(self._colors.items())
}
# Publishers
self.pub_lines = rospy.Publisher("~segment_list",
SegmentList,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION)
self.pub_d_segments = rospy.Publisher("~debug/segments/compressed",
CompressedImage,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
self.pub_d_edges = rospy.Publisher("~debug/edges/compressed",
CompressedImage,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
self.pub_d_maps = rospy.Publisher("~debug/maps/compressed",
CompressedImage,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
# these are not compressed because compression adds undesired blur
self.pub_d_ranges_HS = rospy.Publisher("~debug/ranges_HS",
Image,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
self.pub_d_ranges_SV = rospy.Publisher("~debug/ranges_SV",
Image,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
self.pub_d_ranges_HV = rospy.Publisher("~debug/ranges_HV",
Image,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
# Subscribers
self.sub_image = rospy.Subscriber("~image/compressed",
CompressedImage,
self.image_cb,
buff_size=10000000,
queue_size=1)
self.sub_thresholds = rospy.Subscriber("~thresholds",
AntiInstagramThresholds,
self.thresholds_cb,
queue_size=1)
class LineDetectorNode(DTROS):
"""
The ``LineDetectorNode`` is responsible for detecting the line white, yellow and red line segment in an image and
is used for lane localization.
Upon receiving an image, this node reduces its resolution, cuts off the top part so that only the
road-containing part of the image is left, extracts the white, red, and yellow segments and publishes them.
The main functionality of this node is implemented in the :py:class:`line_detector.LineDetector` class.
The performance of this node can be very sensitive to its configuration parameters. Therefore, it also provides a
number of debug topics which can be used for fine-tuning these parameters. These configuration parameters can be
changed dynamically while the node is running via ``rosparam set`` commands.
Args:
node_name (:obj:`str`): a unique, descriptive name for the node that ROS will use
Configuration:
~line_detector_parameters (:obj:`dict`): A dictionary with the parameters for the detector. The full list can be found in :py:class:`line_detector.LineDetector`.
~colors (:obj:`dict`): A dictionary of colors and color ranges to be detected in the image. The keys (color names) should match the ones in the Segment message definition, otherwise an exception will be thrown! See the ``config`` directory in the node code for the default ranges.
~img_size (:obj:`list` of ``int``): The desired downsized resolution of the image. Lower resolution would result in faster detection but lower performance, default is ``[120,160]``
~top_cutoff (:obj:`int`): The number of rows to be removed from the top of the image _after_ resizing, default is 40
Subscriber:
~camera_node/image/compressed (:obj:`sensor_msgs.msg.CompressedImage`): The camera images
~anti_instagram_node/thresholds(:obj:`duckietown_msgs.msg.AntiInstagramThresholds`): The thresholds to do color correction
Publishers:
~segment_list (:obj:`duckietown_msgs.msg.SegmentList`): A list of the detected segments. Each segment is an :obj:`duckietown_msgs.msg.Segment` message
~debug/segments/compressed (:obj:`sensor_msgs.msg.CompressedImage`): Debug topic with the segments drawn on the input image
~debug/edges/compressed (:obj:`sensor_msgs.msg.CompressedImage`): Debug topic with the Canny edges drawn on the input image
~debug/maps/compressed (:obj:`sensor_msgs.msg.CompressedImage`): Debug topic with the regions falling in each color range drawn on the input image
~debug/ranges_HS (:obj:`sensor_msgs.msg.Image`): Debug topic with a histogram of the colors in the input image and the color ranges, Hue-Saturation projection
~debug/ranges_SV (:obj:`sensor_msgs.msg.Image`): Debug topic with a histogram of the colors in the input image and the color ranges, Saturation-Value projection
~debug/ranges_HV (:obj:`sensor_msgs.msg.Image`): Debug topic with a histogram of the colors in the input image and the color ranges, Hue-Value projection
"""
def __init__(self, node_name):
# Initialize the DTROS parent class
super(LineDetectorNode, self).__init__(node_name=node_name,
node_type=NodeType.PERCEPTION)
# Define parameters
self._line_detector_parameters = rospy.get_param(
"~line_detector_parameters", None)
self._colors = rospy.get_param("~colors", None)
self._img_size = rospy.get_param("~img_size", None)
self._top_cutoff = rospy.get_param("~top_cutoff", None)
self.bridge = CvBridge()
# The thresholds to be used for AntiInstagram color correction
self.ai_thresholds_received = False
self.anti_instagram_thresholds = dict()
self.ai = AntiInstagram()
# This holds the colormaps for the debug/ranges images after they are computed once
self.colormaps = dict()
# Create a new LineDetector object with the parameters from the Parameter Server / config file
self.detector = LineDetector(**self._line_detector_parameters)
# Update the color ranges objects
self.color_ranges = {
color: ColorRange.fromDict(d)
for color, d in list(self._colors.items())
}
# Publishers
self.pub_lines = rospy.Publisher("~segment_list",
SegmentList,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION)
self.pub_d_segments = rospy.Publisher("~debug/segments/compressed",
CompressedImage,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
self.pub_d_edges = rospy.Publisher("~debug/edges/compressed",
CompressedImage,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
self.pub_d_maps = rospy.Publisher("~debug/maps/compressed",
CompressedImage,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
# these are not compressed because compression adds undesired blur
self.pub_d_ranges_HS = rospy.Publisher("~debug/ranges_HS",
Image,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
self.pub_d_ranges_SV = rospy.Publisher("~debug/ranges_SV",
Image,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
self.pub_d_ranges_HV = rospy.Publisher("~debug/ranges_HV",
Image,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
# Subscribers
self.sub_image = rospy.Subscriber("~image/compressed",
CompressedImage,
self.image_cb,
buff_size=10000000,
queue_size=1)
self.sub_thresholds = rospy.Subscriber("~thresholds",
AntiInstagramThresholds,
self.thresholds_cb,
queue_size=1)
def thresholds_cb(self, thresh_msg):
self.anti_instagram_thresholds["lower"] = thresh_msg.low
self.anti_instagram_thresholds["higher"] = thresh_msg.high
self.ai_thresholds_received = True
def image_cb(self, image_msg):
"""
Processes the incoming image messages.
Performs the following steps for each incoming image:
#. Performs color correction
#. Resizes the image to the ``~img_size`` resolution
#. Removes the top ``~top_cutoff`` rows in order to remove the part of the image that doesn't include the road
#. Extracts the line segments in the image using :py:class:`line_detector.LineDetector`
#. Converts the coordinates of detected segments to normalized ones
#. Creates and publishes the resultant :obj:`duckietown_msgs.msg.SegmentList` message
#. Creates and publishes debug images if there is a subscriber to the respective topics
Args:
image_msg (:obj:`sensor_msgs.msg.CompressedImage`): The receive image message
"""
# Decode from compressed image with OpenCV
try:
image = self.bridge.compressed_imgmsg_to_cv2(image_msg)
except ValueError as e:
self.logerr(f"Could not decode image: {e}")
return
# Perform color correction
if self.ai_thresholds_received:
image = self.ai.apply_color_balance(
self.anti_instagram_thresholds["lower"],
self.anti_instagram_thresholds["higher"], image)
# Resize the image to the desired dimensions
height_original, width_original = image.shape[0:2]
img_size = (self._img_size[1], self._img_size[0])
if img_size[0] != width_original or img_size[1] != height_original:
image = cv2.resize(image,
img_size,
interpolation=cv2.INTER_NEAREST)
image = image[self._top_cutoff:, :, :]
# Extract the line segments for every color
self.detector.setImage(image)
detections = {
color: self.detector.detectLines(ranges)
for color, ranges in list(self.color_ranges.items())
}
# Construct a SegmentList
segment_list = SegmentList()
segment_list.header.stamp = image_msg.header.stamp
# Remove the offset in coordinates coming from the removing of the top part and
arr_cutoff = np.array([0, self._top_cutoff, 0, self._top_cutoff])
arr_ratio = np.array([
1.0 / self._img_size[1],
1.0 / self._img_size[0],
1.0 / self._img_size[1],
1.0 / self._img_size[0],
])
# Fill in the segment_list with all the detected segments
for color, det in list(detections.items()):
# Get the ID for the color from the Segment msg definition
# Throw and exception otherwise
if len(det.lines) > 0 and len(det.normals) > 0:
try:
color_id = getattr(Segment, color)
lines_normalized = (det.lines + arr_cutoff) * arr_ratio
segment_list.segments.extend(
self._to_segment_msg(lines_normalized, det.normals,
color_id))
except AttributeError:
self.logerr(
f"Color name {color} is not defined in the Segment message"
)
# Publish the message
self.pub_lines.publish(segment_list)
# If there are any subscribers to the debug topics, generate a debug image and publish it
if self.pub_d_segments.get_num_connections() > 0:
colorrange_detections = {
self.color_ranges[c]: det
for c, det in list(detections.items())
}
debug_img = plotSegments(image, colorrange_detections)
debug_image_msg = self.bridge.cv2_to_compressed_imgmsg(debug_img)
debug_image_msg.header = image_msg.header
self.pub_d_segments.publish(debug_image_msg)
if self.pub_d_edges.get_num_connections() > 0:
debug_image_msg = self.bridge.cv2_to_compressed_imgmsg(
self.detector.canny_edges)
debug_image_msg.header = image_msg.header
self.pub_d_edges.publish(debug_image_msg)
if self.pub_d_maps.get_num_connections() > 0:
colorrange_detections = {
self.color_ranges[c]: det
for c, det in list(detections.items())
}
debug_img = plotMaps(image, colorrange_detections)
debug_image_msg = self.bridge.cv2_to_compressed_imgmsg(debug_img)
debug_image_msg.header = image_msg.header
self.pub_d_maps.publish(debug_image_msg)
for channels in ["HS", "SV", "HV"]:
publisher = getattr(self, f"pub_d_ranges_{channels}")
if publisher.get_num_connections() > 0:
debug_img = self._plot_ranges_histogram(channels)
debug_image_msg = self.bridge.cv2_to_imgmsg(debug_img,
encoding="bgr8")
debug_image_msg.header = image_msg.header
publisher.publish(debug_image_msg)
@staticmethod
def _to_segment_msg(lines, normals, color):
"""
Converts line detections to a list of Segment messages.
Converts the resultant line segments and normals from the line detection to a list of Segment messages.
Args:
lines (:obj:`numpy array`): An ``Nx4`` array where each row represents a line.
normals (:obj:`numpy array`): An ``Nx2`` array where each row represents the normal of a line.
color (:obj:`str`): Color name string, should be one of the pre-defined in the Segment message definition.
Returns:
:obj:`list` of :obj:`duckietown_msgs.msg.Segment`: List of Segment messages
"""
segment_msg_list = []
for x1, y1, x2, y2, norm_x, norm_y in np.hstack((lines, normals)):
segment = Segment()
segment.color = color
segment.pixels_normalized[0].x = x1
segment.pixels_normalized[0].y = y1
segment.pixels_normalized[1].x = x2
segment.pixels_normalized[1].y = y2
segment.normal.x = norm_x
segment.normal.y = norm_y
segment_msg_list.append(segment)
return segment_msg_list
def _plot_ranges_histogram(self, channels):
"""Utility method for plotting color histograms and color ranges.
Args:
channels (:obj:`str`): The desired two channels, should be one of ``['HS','SV','HV']``
Returns:
:obj:`numpy array`: The resultant plot image
"""
channel_to_axis = {"H": 0, "S": 1, "V": 2}
axis_to_range = {0: 180, 1: 256, 2: 256}
# Get which is the third channel that will not be shown in this plot
missing_channel = "HSV".replace(channels[0],
"").replace(channels[1], "")
hsv_im = self.detector.hsv
# Get the pixels as a list (flatten the horizontal and vertical dimensions)
hsv_im = hsv_im.reshape((-1, 3))
channel_idx = [
channel_to_axis[channels[0]], channel_to_axis[channels[1]]
]
# Get only the relevant channels
x_bins = np.arange(0, axis_to_range[channel_idx[1]] + 1, 2)
y_bins = np.arange(0, axis_to_range[channel_idx[0]] + 1, 2)
h, _, _ = np.histogram2d(x=hsv_im[:, channel_idx[0]],
y=hsv_im[:, channel_idx[1]],
bins=[y_bins, x_bins])
# Log-normalized histogram
np.log(h, out=h, where=(h != 0))
h = (255 * h / np.max(h)).astype(np.uint8)
# Make a color map, for the missing channel, just take the middle of the range
if channels not in self.colormaps:
colormap_1, colormap_0 = np.meshgrid(x_bins[:-1], y_bins[:-1])
colormap_2 = np.ones_like(colormap_0) * (
axis_to_range[channel_to_axis[missing_channel]] / 2)
channel_to_map = {
channels[0]: colormap_0,
channels[1]: colormap_1,
missing_channel: colormap_2
}
self.colormaps[channels] = np.stack([
channel_to_map["H"], channel_to_map["S"], channel_to_map["V"]
],
axis=-1).astype(np.uint8)
self.colormaps[channels] = cv2.cvtColor(self.colormaps[channels],
cv2.COLOR_HSV2BGR)
# resulting histogram image as a blend of the two images
im = cv2.cvtColor(h[:, :, None], cv2.COLOR_GRAY2BGR)
im = cv2.addWeighted(im, 0.5, self.colormaps[channels], 1 - 0.5, 0.0)
# now plot the color ranges on top
for _, color_range in list(self.color_ranges.items()):
# convert HSV color to BGR
c = color_range.representative
c = np.uint8([[[c[0], c[1], c[2]]]])
color = cv2.cvtColor(c, cv2.COLOR_HSV2BGR).squeeze().astype(int)
for i in range(len(color_range.low)):
cv2.rectangle(
im,
pt1=(
(color_range.high[i, channel_idx[1]] / 2).astype(
np.uint8),
(color_range.high[i, channel_idx[0]] / 2).astype(
np.uint8),
),
pt2=(
(color_range.low[i, channel_idx[1]] / 2).astype(
np.uint8),
(color_range.low[i, channel_idx[0]] / 2).astype(
np.uint8),
),
color=color,
lineType=cv2.LINE_4,
)
# ---
return im
def __init__(self):
self.line_detector = LineDetector()
self.thresh_image = None
class LineDetectorNode(object):
def __init__(self):
self.node_name = "Line Detector"
self.image_size = rospy.get_param('~img_size')
self.top_cutoff = rospy.get_param('~top_cutoff')
self.bridge = CvBridge()
self.detector = LineDetector()
self.detector.hsv_white1 = np.array(rospy.get_param('~hsv_white1'))
self.detector.hsv_white2 = np.array(rospy.get_param('~hsv_white2'))
self.detector.hsv_yellow1 = np.array(rospy.get_param('~hsv_yellow1'))
self.detector.hsv_yellow2 = np.array(rospy.get_param('~hsv_yellow2'))
self.detector.hsv_red1 = np.array(rospy.get_param('~hsv_red1'))
self.detector.hsv_red2 = np.array(rospy.get_param('~hsv_red2'))
self.detector.hsv_red3 = np.array(rospy.get_param('~hsv_red3'))
self.detector.hsv_red4 = np.array(rospy.get_param('~hsv_red4'))
self.detector.dilation_kernel_size = rospy.get_param('~dilation_kernel_size')
self.detector.canny_thresholds = rospy.get_param('~canny_thresholds')
self.detector.hough_min_line_length = rospy.get_param('~hough_min_line_length')
self.detector.hough_max_line_gap = rospy.get_param('~hough_max_line_gap')
self.detector.hough_threshold = rospy.get_param('~hough_threshold')
self.sub_image = rospy.Subscriber("~image", CompressedImage, self.processImage)
self.pub_lines = rospy.Publisher("~segment_list", SegmentList, queue_size=1)
self.pub_image = rospy.Publisher("~image_with_lines", Image, queue_size=1)
self.timer_param = rospy.Timer(rospy.Duration.from_sec(3.0),self.cbParamUpdate)
def cbParamUpdate(self,event):
print '====Parameters Updated===='
# S of white
self.detector.hsv_white2 = (self.detector.hsv_white2)%256 + np.array([0, 5, 0])
print 'HSV_white1: ' + str(self.detector.hsv_white1)
print 'HSV_white2: ' + str(self.detector.hsv_white2)
"""
# S of white
self.detector.hsv_white2 = (self.detector.hsv_white2)%256 + np.array([0, 5, 0])
print 'HSV_white1: ' + str(self.detector.hsv_white1)
print 'HSV_white2: ' + str(self.detector.hsv_white2)
# V of white
self.detector.hsv_white1 = (self.detector.hsv_white1)%256 + np.array([0, 0, 5])
print 'HSV_white1: ' + str(self.detector.hsv_white1)
print 'HSV_white2: ' + str(self.detector.hsv_white2)
# H of yellow
self.detector.hsv_yellow1 = (self.detector.hsv_yellow1)%256 + np.array([1, 0, 0])
print 'HSV_yellow1: ' + str(self.detector.hsv_yellow1)
print 'HSV_yellow2: ' + str(self.detector.hsv_yellow2)
self.detector.hsv_yellow2 = (self.detector.hsv_yellow2)%256 - np.array([1, 0, 0])
print 'HSV_yellow1: ' + str(self.detector.hsv_yellow1)
print 'HSV_yellow2: ' + str(self.detector.hsv_yellow2)
# S of yellow
self.detector.hsv_yellow1 = (self.detector.hsv_yellow1)%256 + np.array([0, 5, 0])
print 'HSV_yellow1: ' + str(self.detector.hsv_yellow1)
print 'HSV_yellow2: ' + str(self.detector.hsv_yellow2)
# V of yellow
self.detector.hsv_yellow1 = (self.detector.hsv_yellow1)%256 + np.array([0, 0, 5])
print 'HSV_yellow1: ' + str(self.detector.hsv_yellow1)
print 'HSV_yellow2: ' + str(self.detector.hsv_yellow2)
# Lower threshold of Canny edge
self.detector.canny_thresholds = self.detector.canny_thresholds%256 + np.array([5, 0])
print 'Canny_thresholds: ' + str(self.detector.canny_thresholds)
# Higher threshold of Canny edge
self.detector.canny_thresholds = self.detector.canny_thresholds%256 + np.array([0, 5])
print 'Canny_thresholds: ' + str(self.detector.canny_thresholds)
# Minimum line length
self.detector.hough_min_line_length = self.detector.hough_min_line_length%10 + 1
print 'Minimum_line_length: ' + str(self.detector.hough_min_line_length)
# Maximum line gap
self.detector.hough_max_line_gap = self.detector.detector.hough_max_line_gap%10 + 1
print 'Maximum_line_gap: ' + str(self.detector.detector.hough_max_line_gap)
# Threshold of Hough transform
self.detector.hough_threshold = (self.detector.hough_threshold)%50 + 5
print 'Hough_threshold: ' + str(self.detector.hough_threshold)
"""
def processImage(self,image_msg):
image_cv = cv2.imdecode(np.fromstring(image_msg.data, np.uint8), cv2.CV_LOAD_IMAGE_COLOR)
#image_cv = self.bridge.imgmsg_to_cv2(image_msg, "bgr8")
# Resize and crop image
hei_original = image_cv.shape[0]
wid_original = image_cv.shape[1]
if self.image_size[0]!=hei_original or self.image_size[1]!=wid_original:
image_cv = cv2.resize(image_cv, (self.image_size[1], self.image_size[0]))
image_cv = image_cv[self.top_cutoff:,:,:]
# Set the image to be detected
self.detector.setImage(image_cv)
# Detect lines and normals
lines_white, normals_white = self.detector.detectLines('white')
lines_yellow, normals_yellow = self.detector.detectLines('yellow')
lines_red, normals_red = self.detector.detectLines('red')
# Draw lines and normals
self.detector.drawLines(lines_white, (0,0,0))
self.detector.drawLines(lines_yellow, (255,0,0))
self.detector.drawLines(lines_red, (0,255,0))
#self.detector.drawNormals(lines_white, normals_white)
#self.detector.drawNormals(lines_yellow, normals_yellow)
#self.detector.drawNormals(lines_red, normals_red)
# Convert to normalized pixel coordinates, and add segments to segmentList
segmentList = SegmentList()
arr_cutoff = np.array((0, self.top_cutoff, 0, self.top_cutoff))
arr_ratio = np.array((1./self.image_size[1], 1./self.image_size[0], 1./self.image_size[1], 1./self.image_size[0]))
if len(lines_white)>0:
#rospy.loginfo("[LineDetectorNode] number of white lines = %s" %(len(lines_white)))
lines_normalized_white = ((lines_white + arr_cutoff) * arr_ratio)
segmentList.segments.extend(self.toSegmentMsg(lines_normalized_white, normals_white, Segment.WHITE))
if len(lines_yellow)>0:
#rospy.loginfo("[LineDetectorNode] number of yellow lines = %s" %(len(lines_yellow)))
lines_normalized_yellow = ((lines_yellow + arr_cutoff) * arr_ratio)
segmentList.segments.extend(self.toSegmentMsg(lines_normalized_yellow, normals_yellow, Segment.YELLOW))
if len(lines_red)>0:
#rospy.loginfo("[LineDetectorNode] number of red lines = %s" %(len(lines_red)))
lines_normalized_red = ((lines_red + arr_cutoff) * arr_ratio)
segmentList.segments.extend(self.toSegmentMsg(lines_normalized_red, normals_red, Segment.RED))
# Publish segmentList
self.pub_lines.publish(segmentList)
# Publish the frame with lines
image_msg = self.bridge.cv2_to_imgmsg(self.detector.getImage(), "bgr8")
self.pub_image.publish(image_msg)
def onShutdown(self):
rospy.loginfo("[LineDetectorNode] Shutdown.")
def toSegmentMsg(self, lines, normals, color):
segmentMsgList = []
for x1,y1,x2,y2,norm_x,norm_y in np.hstack((lines,normals)):
segment = Segment()
segment.color = color
segment.pixels_normalized[0].x = x1
segment.pixels_normalized[0].y = y1
segment.pixels_normalized[1].x = x2
segment.pixels_normalized[1].y = y2
segment.normal.x = norm_x
segment.normal.y = norm_y
segmentMsgList.append(segment)
return segmentMsgList
class LineDetectorNode(object):
def __init__(self):
self.node_name = "Line Detector"
self.hei_image = self.setupParam("~hei_image", 240)
self.wid_image = self.setupParam("~wid_image", 320)
self.top_cutoff = self.setupParam("~top_cutoff", 90)
self.bridge = CvBridge()
self.detector = LineDetector()
self.sub_image = rospy.Subscriber("~image", CompressedImage, self.processImage)
self.pub_lines = rospy.Publisher("~segment_list", SegmentList, queue_size=1)
self.pub_image = rospy.Publisher("~image_with_lines", Image, queue_size=1)
def setupParam(self, param_name, default_value):
value = rospy.get_param(param_name, default_value)
rospy.set_param(param_name, value) # Write to parameter server for transparancy
rospy.loginfo("[%s] %s = %s " %(self.node_name,param_name,value))
return value
def processImage(self,image_msg):
image_cv = cv2.imdecode(np.fromstring(image_msg.data, np.uint8), cv2.CV_LOAD_IMAGE_COLOR)
#image_cv = self.bridge.imgmsg_to_cv2(image_msg, "bgr8")
# Resize and crop image
hei_original = image_cv.shape[0]
wid_original = image_cv.shape[1]
if self.hei_image!=hei_original or self.wid_image!=wid_original:
image_cv = cv2.resize(image_cv, (self.wid_image, self.hei_image))
image_cv = image_cv[self.top_cutoff:,:,:]
# Set the image to be detected
self.detector.setImage(image_cv)
# Detect lines and normals
lines_white, normals_white = self.detector.detectLines('white')
lines_yellow, normals_yellow = self.detector.detectLines('yellow')
lines_red, normals_red = self.detector.detectLines('red')
# Draw lines and normals
self.detector.drawLines(lines_white, (0,0,0))
self.detector.drawLines(lines_yellow, (255,0,0))
self.detector.drawLines(lines_red, (0,255,0))
#self.detector.drawNormals(lines_white, normals_white)
#self.detector.drawNormals(lines_yellow, normals_yellow)
#self.detector.drawNormals(lines_red, normals_red)
# Convert to position in original resolution, and add segments to segmentList
segmentList = SegmentList()
arr_cutoff = np.array((0, self.top_cutoff, 0, self.top_cutoff))
arr_ratio = np.array((1.*wid_original/self.wid_image, 1.*hei_original/self.hei_image, 1.*wid_original/self.wid_image, 1.*hei_original/self.hei_image))
if len(lines_white)>0:
rospy.loginfo("[LineDetectorNode] number of white lines = %s" %(len(lines_white)))
lines_white = ((lines_white + arr_cutoff) * arr_ratio).astype('int')
segmentList.segments.extend(self.toSegmentMsg(lines_white, normals_white, Segment.WHITE, wid_original, hei_original))
if len(lines_yellow)>0:
rospy.loginfo("[LineDetectorNode] number of yellow lines = %s" %(len(lines_yellow)))
lines_yellow = ((lines_yellow + arr_cutoff) * arr_ratio).astype('int')
segmentList.segments.extend(self.toSegmentMsg(lines_yellow, normals_yellow, Segment.YELLOW, wid_original, hei_original))
if len(lines_red)>0:
rospy.loginfo("[LineDetectorNode] number of red lines = %s" %(len(lines_red)))
lines_red = ((lines_red + arr_cutoff) * arr_ratio).astype('int')
segmentList.segments.extend(self.toSegmentMsg(lines_red, normals_red, Segment.RED, wid_original, hei_original))
# Publish segmentList
self.pub_lines.publish(segmentList)
# Publish the frame with lines
image_msg = self.bridge.cv2_to_imgmsg(self.detector.getImage(), "bgr8")
self.pub_image.publish(image_msg)
def onShutdown(self):
rospy.loginfo("[LineDetectorNode] Shutdown.")
def toSegmentMsg(self, lines, normals, color, wid, hei):
segmentMsgList = []
for u1,v1,u2,v2,norm_u,norm_v in np.hstack((lines,normals)):
segment = Segment()
segment.color = color
segment.pixels[0].u = int(u1)
segment.pixels[0].v = int(v1)
segment.pixels[1].u = int(u2)
segment.pixels[1].v = int(v2)
segment.pixels_normalized[0].x = u1/wid
segment.pixels_normalized[0].y = v1/hei
segment.pixels_normalized[1].x = u2/wid
segment.pixels_normalized[1].y = v2/hei
segment.normal.x = norm_u
segment.normal.y = norm_v
segmentMsgList.append(segment)
return segmentMsgList
if self.cv_image is None:
print('image msg received')
self.cv_image = self.bridge.imgmsg_to_cv2(image_msg,
desired_encoding="bgr8")
def getImage(self):
return self.cv_image
# print(__name__)
if __name__ == '__main__':
import cv2
import time
from line_detector import LineDetector
detector = LineDetector()
subscriber = RosImageSubscriber()
count = 1
start = time.time()
while True:
# image_frame = subscriber.get_frame(GetFrameRequest())
# cv_image = subscriber.bridge.imgmsg_to_cv2(image_frame.image, desired_encoding="bgr8")
cv_image = subscriber.cv_image
rect_image = cv_image
detection, image_color = detector.detect_hsv(rect_image,
detector.colors[u'黄色'])
count = count + 1
if (count > 100):
end = time.time()
class LineDetectorNode(object):
def __init__(self):
self.node_name = "Line Detector"
self.image_size = rospy.get_param('~img_size')
self.top_cutoff = rospy.get_param('~top_cutoff')
self.bridge = CvBridge()
self.detector = LineDetector()
self.detector.hsv_white1 = np.array(rospy.get_param('~hsv_white1'))
self.detector.hsv_white2 = np.array(rospy.get_param('~hsv_white2'))
self.detector.hsv_yellow1 = np.array(rospy.get_param('~hsv_yellow1'))
self.detector.hsv_yellow2 = np.array(rospy.get_param('~hsv_yellow2'))
self.detector.hsv_red1 = np.array(rospy.get_param('~hsv_red1'))
self.detector.hsv_red2 = np.array(rospy.get_param('~hsv_red2'))
self.detector.hsv_red3 = np.array(rospy.get_param('~hsv_red3'))
self.detector.hsv_red4 = np.array(rospy.get_param('~hsv_red4'))
self.detector.dilation_kernel_size = rospy.get_param(
'~dilation_kernel_size')
self.detector.canny_thresholds = rospy.get_param('~canny_thresholds')
self.detector.hough_min_line_length = rospy.get_param(
'~hough_min_line_length')
self.detector.hough_max_line_gap = rospy.get_param(
'~hough_max_line_gap')
self.detector.hough_threshold = rospy.get_param('~hough_threshold')
self.sub_image = rospy.Subscriber("~image", CompressedImage,
self.processImage)
self.pub_lines = rospy.Publisher("~segment_list",
SegmentList,
queue_size=1)
self.pub_image = rospy.Publisher("~image_with_lines",
Image,
queue_size=1)
self.timer_param = rospy.Timer(rospy.Duration.from_sec(3.0),
self.cbParamUpdate)
def cbParamUpdate(self, event):
print '====Parameters Updated===='
"""
# S of white
self.detector.hsv_white2 = (self.detector.hsv_white2)%256 + np.array([0, 5, 0])
print 'HSV_white1: ' + str(self.detector.hsv_white1)
print 'HSV_white2: ' + str(self.detector.hsv_white2)
# V of white
self.detector.hsv_white1 = (self.detector.hsv_white1)%256 - np.array([0, 0, 10])
print 'HSV_white1: ' + str(self.detector.hsv_white1)
print 'HSV_white2: ' + str(self.detector.hsv_white2)
# H of yellow
self.detector.hsv_yellow1 = (self.detector.hsv_yellow1)%256 + np.array([1, 0, 0])
print 'HSV_yellow1: ' + str(self.detector.hsv_yellow1)
print 'HSV_yellow2: ' + str(self.detector.hsv_yellow2)
self.detector.hsv_yellow2 = (self.detector.hsv_yellow2)%256 - np.array([1, 0, 0])
print 'HSV_yellow1: ' + str(self.detector.hsv_yellow1)
print 'HSV_yellow2: ' + str(self.detector.hsv_yellow2)
# S of yellow
self.detector.hsv_yellow1 = (self.detector.hsv_yellow1)%256 + np.array([0, 5, 0])
print 'HSV_yellow1: ' + str(self.detector.hsv_yellow1)
print 'HSV_yellow2: ' + str(self.detector.hsv_yellow2)
# V of yellow
self.detector.hsv_yellow1 = (self.detector.hsv_yellow1)%256 + np.array([0, 0, 5])
print 'HSV_yellow1: ' + str(self.detector.hsv_yellow1)
print 'HSV_yellow2: ' + str(self.detector.hsv_yellow2)
# Lower threshold of Canny edge
self.detector.canny_thresholds = np.array(self.detector.canny_thresholds)%256 + np.array([5, 0])
print 'Canny_thresholds: ' + str(self.detector.canny_thresholds)
# Higher threshold of Canny edge
self.detector.canny_thresholds = np.array(self.detector.canny_thresholds)%256 + np.array([0, 5])
print 'Canny_thresholds: ' + str(self.detector.canny_thresholds)
# Minimum line length
self.detector.hough_min_line_length = self.detector.hough_min_line_length%10 + 1
print 'Minimum_line_length: ' + str(self.detector.hough_min_line_length)
# Maximum line gap
self.detector.hough_max_line_gap = self.detector.detector.hough_max_line_gap%10 + 1
print 'Maximum_line_gap: ' + str(self.detector.detector.hough_max_line_gap)
# Threshold of Hough transform
self.detector.hough_threshold = (self.detector.hough_threshold)%50 + 5
print 'Hough_threshold: ' + str(self.detector.hough_threshold)
"""
def processImage(self, image_msg):
image_cv = cv2.imdecode(np.fromstring(image_msg.data, np.uint8),
cv2.CV_LOAD_IMAGE_COLOR)
#image_cv = self.bridge.imgmsg_to_cv2(image_msg, "bgr8")
# Resize and crop image
hei_original = image_cv.shape[0]
wid_original = image_cv.shape[1]
if self.image_size[0] != hei_original or self.image_size[
1] != wid_original:
image_cv = cv2.resize(image_cv,
(self.image_size[1], self.image_size[0]))
image_cv = image_cv[self.top_cutoff:, :, :]
# Set the image to be detected
self.detector.setImage(image_cv)
# Detect lines and normals
lines_white, normals_white = self.detector.detectLines('white')
lines_yellow, normals_yellow = self.detector.detectLines('yellow')
lines_red, normals_red = self.detector.detectLines('red')
# Draw lines and normals
self.detector.drawLines(lines_white, (0, 0, 0))
self.detector.drawLines(lines_yellow, (255, 0, 0))
self.detector.drawLines(lines_red, (0, 255, 0))
#self.detector.drawNormals(lines_white, normals_white)
#self.detector.drawNormals(lines_yellow, normals_yellow)
#self.detector.drawNormals(lines_red, normals_red)
# Convert to normalized pixel coordinates, and add segments to segmentList
segmentList = SegmentList()
arr_cutoff = np.array((0, self.top_cutoff, 0, self.top_cutoff))
arr_ratio = np.array(
(1. / self.image_size[1], 1. / self.image_size[0],
1. / self.image_size[1], 1. / self.image_size[0]))
if len(lines_white) > 0:
#rospy.loginfo("[LineDetectorNode] number of white lines = %s" %(len(lines_white)))
lines_normalized_white = ((lines_white + arr_cutoff) * arr_ratio)
segmentList.segments.extend(
self.toSegmentMsg(lines_normalized_white, normals_white,
Segment.WHITE))
if len(lines_yellow) > 0:
#rospy.loginfo("[LineDetectorNode] number of yellow lines = %s" %(len(lines_yellow)))
lines_normalized_yellow = ((lines_yellow + arr_cutoff) * arr_ratio)
segmentList.segments.extend(
self.toSegmentMsg(lines_normalized_yellow, normals_yellow,
Segment.YELLOW))
if len(lines_red) > 0:
#rospy.loginfo("[LineDetectorNode] number of red lines = %s" %(len(lines_red)))
lines_normalized_red = ((lines_red + arr_cutoff) * arr_ratio)
segmentList.segments.extend(
self.toSegmentMsg(lines_normalized_red, normals_red,
Segment.RED))
# Publish segmentList
self.pub_lines.publish(segmentList)
# Publish the frame with lines
image_msg = self.bridge.cv2_to_imgmsg(self.detector.getImage(), "bgr8")
self.pub_image.publish(image_msg)
def onShutdown(self):
rospy.loginfo("[LineDetectorNode] Shutdown.")
def toSegmentMsg(self, lines, normals, color):
segmentMsgList = []
for x1, y1, x2, y2, norm_x, norm_y in np.hstack((lines, normals)):
segment = Segment()
segment.color = color
segment.pixels_normalized[0].x = x1
segment.pixels_normalized[0].y = y1
segment.pixels_normalized[1].x = x2
segment.pixels_normalized[1].y = y2
segment.normal.x = norm_x
segment.normal.y = norm_y
segmentMsgList.append(segment)
return segmentMsgList
class GridDetector:
def __init__(self):
self.line_detector = LineDetector()
self.thresh_image = None
def find_grid(self, image, show_img=True):
self.thresh_image = self.preprocess_image(image)
filtered_image = self.filter_small_contours(self.thresh_image)
lines = self.line_detector.find_lines(filtered_image)
if lines is None:
return None
points_finder = PointsFinder(filtered_image, lines)
points = points_finder.get_points()
lines_with_points = points_finder.get_lines_with_points()
corner_finder = CornerFinder()
final_corners = corner_finder.estimate_corners(lines_with_points)
if show_img:
colored_image = self.draw_image(
filtered_image, lines, points,
corner_finder.get_middle_lines_ids(),
corner_finder.get_border_lines_ids())
cv2.imshow('Line & points', colored_image)
return final_corners
def draw_image(self, thresh, lines, points, middle_lines, border_lines):
colored_image = cv2.cvtColor(thresh, cv2.COLOR_GRAY2BGR)
self.draw_lines(colored_image, lines, middle_lines, border_lines)
self.draw_points(colored_image, points)
return colored_image
@staticmethod
def preprocess_image(raw_image):
gray = cv2.cvtColor(raw_image, cv2.COLOR_BGR2GRAY)
return cv2.adaptiveThreshold(gray, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C,
cv2.THRESH_BINARY, 57, 5)
@staticmethod
def filter_small_contours(thresh):
thresh = np.copy(thresh)
cnts = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
cnts = cnts[0] if len(cnts) == 2 else cnts[1]
for c in cnts:
area = cv2.contourArea(c)
if area < 1500:
cv2.drawContours(thresh, [c], -1, 255, -1)
return 255 - thresh
@staticmethod
def draw_lines(colored_image, lines, middle_lines_id, border_lines_id):
line_id = 0
for line in lines:
rho = line[0][0]
theta = line[0][1]
a = np.cos(theta)
b = np.sin(theta)
x0 = a * rho
y0 = b * rho
pt1 = (int(x0 + 1000 * (-b)), int(y0 + 1000 * a))
pt2 = (int(x0 - 1000 * (-b)), int(y0 - 1000 * a))
if line_id in middle_lines_id:
cv2.line(colored_image, pt1, pt2, (255, 0, 255), 1,
cv2.LINE_AA)
elif line_id in border_lines_id:
cv2.line(colored_image, pt1, pt2, (0, 255, 0), 1, cv2.LINE_AA)
else:
cv2.line(colored_image, pt1, pt2, (0, 0, 255), 1, cv2.LINE_AA)
line_id += 1
@staticmethod
def draw_points(img, points):
for ps, t, l1, l2 in points:
color = (0, 255, 0)
if t == PointsFinder.INTERSECTION_CORNER:
color = (0, 255, 255)
elif t == PointsFinder.INTERSECTION_CROSS:
color = (255, 0, 255)
cv2.circle(img, ps, 5, color, 2)