def callback(self, data): try: cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) # Display the image #cv2.imshow("Image window", cv_image) frame_gray = cv2.cvtColor(cv_image, cv2.COLOR_RGB2GRAY) order = 10 kernel_size = 25 scale_factor = 100 mt = MarkerTracker(order, kernel_size, scale_factor) self.pose = mt.locate_marker(frame_gray) cv2.rectangle(cv_image, (self.pose.x + 100, self.pose.y + 100), (self.pose.x - 100, self.pose.y - 100), (0, 255, 0), 3) cv2.imshow("Box", cv_image) cv2.waitKey(3) print("{} {} {} {} {} {}".format("x-pose:", self.pose.x, "y-pose:", self.pose.y, "theta:", self.pose.theta)) height, width = cv_image.shape[:2] try: self.image_pub.publish( self.bridge.cv2_to_imgmsg(frame_gray, "8UC1")) self.x_pose_pub.publish(self.pose.x - (width / 2)) self.y_pose_pub.publish(self.pose.y - (height / 2)) self.theta_pose_pub.publish(self.pose.theta) except CvBridgeError as e: print(e)
def __init__(self, marker_orders=[6], default_kernel_size=21, scaling_parameter=2500, downscale_factor=1): # Initialize camera driver. # Open output window. if show_image is True: cv2.namedWindow('filterdemo', cv2.WINDOW_AUTOSIZE) # Select the camera where the images should be grabbed from. set_camera_focus() self.camera = cv2.VideoCapture(0) self.set_camera_resolution() # Storage for image processing. self.current_frame = None self.processed_frame = None self.running = True self.downscale_factor = downscale_factor # Storage for trackers. self.trackers = [] self.old_locations = [] # Initialize trackers. for marker_order in marker_orders: temp = MarkerTracker(marker_order, default_kernel_size, scaling_parameter) temp.track_marker_with_missing_black_leg = False self.trackers.append(temp) self.old_locations.append(MarkerPose(None, None, None, None, None))
def __init__(self, order = 7, defaultKernelSize = 21): #cv.NamedWindow('reducedWindow', cv.CV_WINDOW_AUTOSIZE) self.windowWidth = 100 self.windowHeight = 100 self.frameGray = cv.CreateImage ((self.windowWidth, self.windowHeight), cv.IPL_DEPTH_32F, 1) self.originalImage = cv.CreateImage((self.windowWidth, self.windowHeight), cv.IPL_DEPTH_32F, 3) self.markerTracker = MarkerTracker(order, defaultKernelSize, 2500) self.trackerIsInitialized = False self.subImagePosition = None pass
def __init__(self, order=7, defaultKernelSize=21): self.window_width = 100 self.window_height = 100 self.frameGray = np.zeros((self.window_height, self.window_width, 1), dtype=np.float32) self.originalImage = np.zeros( (self.window_height, self.window_width, 3), dtype=np.float32) self.markerTracker = MarkerTracker(order, defaultKernelSize, 2500) self.trackerIsInitialized = False self.subImagePosition = None self.reducedImage = None
def __init__(self, marker_order, marker_size, scaling_parameter): # Storage for image processing. self.current_frame = None self.processed_frame = None self.running = True # Initialize trackers. self.tracker = MarkerTracker(marker_order, marker_size, scaling_parameter) self.location = MarkerPose(None, None, None, None, None)
class CameraDriver: def __init__(self, marker_order, marker_size, scaling_parameter): # Storage for image processing. self.current_frame = None self.processed_frame = None self.running = True # Initialize trackers. self.tracker = MarkerTracker(marker_order, marker_size, scaling_parameter) self.location = MarkerPose(None, None, None, None, None) def open_image_window(self): cv2.namedWindow('filterdemo', cv2.cv.CV_WINDOW_AUTOSIZE) def process_frame(self): self.processed_frame = self.current_frame frame_gray = self.current_frame self.processed_frame = cv2.cvtColor(self.current_frame, cv2.cv.CV_GRAY2BGR) # Previous marker location is unknown, search in the entire image. self.current_frame = self.tracker.locate_marker(frame_gray) self.location = self.tracker.pose def show_processed_frame(self): xm = self.location.x ym = self.location.y orientation = self.location.theta if self.location.quality < 0.9: cv2.circle(self.processed_frame, (xm, ym), 4, (55, 55, 255), 1) else: cv2.circle(self.processed_frame, (xm, ym), 4, (55, 55, 255), 3) xm2 = int(xm + 50 * math.cos(orientation)) ym2 = int(ym + 50 * math.sin(orientation)) cv2.line(self.processed_frame, (xm, ym), (xm2, ym2), (255, 0, 0), 2) cv2.imshow('filterdemo', self.processed_frame) def reset_location(self): # Reset all markers locations, forcing a full search on the next iteration. self.location = MarkerPose(None, None, None, None, None) def handle_keyboard_events(self): # Listen for keyboard events and take relevant actions. key = cv2.waitKey(100) # Discard higher order bit, http://permalink.gmane.org/gmane.comp.lib.opencv.devel/410 key = key & 0xff if key == 27: # Esc self.running = False if key == 114: # R print("Resetting") self.reset_location() if key == 115: # S # save image print("Saving image") filename = strftime("%Y-%m-%d %H-%M-%S") cv2.imwrite("output/%s.png" % filename, self.current_frame)
class TrackerInWindowMode: def __init__(self, order=7, defaultKernelSize=21): self.window_width = 100 self.window_height = 100 self.frameGray = np.zeros((self.window_height, self.window_width, 1), dtype=np.float32) self.originalImage = np.zeros((self.window_height, self.window_width, 3), dtype=np.float32) self.markerTracker = MarkerTracker(order, defaultKernelSize, 2500) self.trackerIsInitialized = False self.subImagePosition = None self.reducedImage = None def crop_frame(self, frame, last_marker_location_x, last_marker_location_y): if not self.trackerIsInitialized: self.reducedImage = np.zeros((self.window_height, self.window_width, 3), frame.dtype) x_corner_pos = last_marker_location_x - self.window_width / 2 y_corner_pos = last_marker_location_y - self.window_height / 2 # Ensure that extracted window is inside the original image. if x_corner_pos < 1: x_corner_pos = 1 if y_corner_pos < 1: y_corner_pos = 1 if x_corner_pos > frame.shape[1] - self.window_width: x_corner_pos = frame.shape[1] - self.window_width if y_corner_pos > frame.shape[0] - self.window_height: y_corner_pos = frame.shape[0] - self.window_height try: self.subImagePosition = (x_corner_pos, y_corner_pos, self.window_width, self.window_height) self.reducedImage = frame[self.subImagePosition[1]:self.subImagePosition[1]+self.subImagePosition[3], self.subImagePosition[0]:self.subImagePosition[0]+self.subImagePosition[2], :] self.frameGray = cv2.cvtColor(self.reducedImage, cv2.cv.CV_RGB2GRAY) except: print("frame: ", frame.dtype) print("originalImage: ", self.originalImage.shape[0], self.originalImage.shape[1], self.originalImage) print("frameGray: ", self.frameGray.shape[0], self.frameGray.shape[1], self.frameGray.dtype) print "Unexpected error:", sys.exc_info()[0] pass def locate_marker(self): (xm, ym) = self.markerTracker.locate_marker(self.frameGray) red_color = (55, 55, 255) blue_color = (255, 0, 0) orientation = self.markerTracker.orientation cv2.circle(self.reducedImage, (xm, ym), 4, red_color, 2) xm2 = int(xm + 50*math.cos(orientation)) ym2 = int(ym + 50*math.sin(orientation)) cv2.line(self.reducedImage, (xm, ym), (xm2, ym2), blue_color, 2) xm = xm + self.subImagePosition[0] ym = ym + self.subImagePosition[1] return MarkerPose(xm, ym, orientation, self.markerTracker.quality, self.markerTracker.order) def show_cropped_image(self): pass
def __init__(self, order=7, defaultKernelSize=21): self.window_width = 100 self.window_height = 100 self.frameGray = np.zeros((self.window_height, self.window_width, 1), dtype=np.float32) self.originalImage = np.zeros((self.window_height, self.window_width, 3), dtype=np.float32) self.markerTracker = MarkerTracker(order, defaultKernelSize, 2500) self.trackerIsInitialized = False self.subImagePosition = None self.reducedImage = None
def __init__(self, order = 7, defaultKernelSize = 21): #cv.NamedWindow('reducedWindow', cv.CV_WINDOW_AUTOSIZE) self.windowWidth = 100 self.windowHeight = 100 self.frameGray = np.zeros((self.windowHeight, self.windowWidth,1), dtype=np.float32) self.originalImage = np.zeros((self.windowHeight, self.windowWidth,3), dtype=np.float32) self.markerTracker = MarkerTracker(order, defaultKernelSize, 2500) self.trackerIsInitialized = False self.subImagePosition = None pass
def callback(self, data): try: cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) # Display the image # cv2.imshow("Image window", cv_image) frame_gray = cv2.cvtColor(cv_image, cv2.COLOR_RGB2GRAY) order = 9 kernel_size = 25 scale_factor = 100 mt = MarkerTracker(order, kernel_size, scale_factor) self.pose = mt.locate_marker(frame_gray) cv2.rectangle(frame_gray, (self.pose.x + 100, self.pose.y + 100), (self.pose.x - 100, self.pose.y - 100), (0, 255, 0), 3) # cv2.imshow("Box",cv_image) # cv2.waitKey(3) # print("{} {} {} {} {} {}".format("x-pose:", self.pose.x, "y-pose:", self.pose.y, "theta:", self.pose.theta)) height, width = cv_image.shape[:2] try: # position out currX = self.pose.x - (width / 2) currY = self.pose.y - (height / 2) msg = MarkerLocation(x=currX, y=currY, theta=self.pose.theta, quality=self.pose.quality) self.marker_pose_pub.publish(msg) #camera feed with marker detection self.image_pub.publish( self.bridge.cv2_to_imgmsg(frame_gray, '8UC1')) except CvBridgeError as e: print(e)
def __init__(self, marker_orders=[4], default_kernel_size=14, scaling_parameter=2500, downscale_factor=1): # Initialize camera driver. # Open output window. if show_image is True: cv2.namedWindow('filterdemo', cv2.WINDOW_AUTOSIZE) # Storage for image processing. self.current_frame = None self.processed_frame = None self.running = True self.downscale_factor = downscale_factor self.cv_image = None # Storage for trackers. self.trackers = [] self.old_locations = [] self.bridge = CvBridge() self.image_sub = rospy.Subscriber(markerlocator_sub_topic, Image, self.callback) self.heading_sub = rospy.Subscriber( "/mavros/global_position/compass_hdg", Float64, self.callback_heading) self.heading = 0 # self.image_sub = rospy.Subscriber("/markerlocator/image_raw", Image, # self.callback) # cam params self.fov = 1.3962634 self.pix_w = 800 # Initialize trackers. for marker_order in marker_orders: temp = MarkerTracker(marker_order, default_kernel_size, scaling_parameter) self.trackers.append(temp) self.old_locations.append(MarkerPose(None, None, None, None, None))
class TrackerInWindowMode: def __init__(self, order=7, defaultKernelSize=21): #cv.NamedWindow('reducedWindow', cv.CV_WINDOW_AUTOSIZE) self.windowWidth = 100 self.windowHeight = 100 self.frameGray = cv.CreateImage((self.windowWidth, self.windowHeight), cv.IPL_DEPTH_32F, 1) self.originalImage = cv.CreateImage( (self.windowWidth, self.windowHeight), cv.IPL_DEPTH_32F, 3) self.markerTracker = MarkerTracker(order, defaultKernelSize, 2500) self.trackerIsInitialized = False self.subImagePosition = None pass def cropFrame(self, frame, lastMarkerLocationX, lastMarkerLocationY): if (not self.trackerIsInitialized): self.markerTracker.allocateSpaceGivenFirstFrame(self.originalImage) self.reducedImage = cv.CreateImage( (self.windowWidth, self.windowHeight), frame.depth, 3) xCornerPos = lastMarkerLocationX - self.windowWidth / 2 yCornerPos = lastMarkerLocationY - self.windowHeight / 2 # Ensure that extracted window is inside the original image. if (xCornerPos < 1): xCornerPos = 1 if (yCornerPos < 1): yCornerPos = 1 if (xCornerPos > frame.width - self.windowWidth): xCornerPos = frame.width - self.windowWidth if (yCornerPos > frame.height - self.windowHeight): yCornerPos = frame.height - self.windowHeight try: self.subImagePosition = (xCornerPos, yCornerPos, self.windowWidth, self.windowHeight) self.reducedImage = cv.GetSubRect(frame, self.subImagePosition) cv.ConvertScale(self.reducedImage, self.originalImage) cv.CvtColor(self.originalImage, self.frameGray, cv.CV_RGB2GRAY) except: print("frame: ", frame.depth) print("originalImage: ", self.originalImage.height, self.originalImage.width, self.originalImage) print("frameGray: ", self.frameGray.height, self.frameGray.width, self.frameGray.depth) print "Unexpected error:", sys.exc_info()[0] #quit(0) pass def locateMarker(self): (xm, ym) = self.markerTracker.locateMarker(self.frameGray) #xm = 50 #ym = 50 #cv.Line(self.reducedImage, (0, ym), (self.originalImage.width, ym), (0, 0, 255)) # B, G, R #cv.Line(self.reducedImage, (xm, 0), (xm, self.originalImage.height), (0, 0, 255)) redColor = (55, 55, 255) blueColor = (255, 0, 0) orientation = self.markerTracker.orientation cv.Circle(self.reducedImage, (xm, ym), 4, redColor, 2) xm2 = int(xm + 50 * math.cos(orientation)) ym2 = int(ym + 50 * math.sin(orientation)) cv.Line(self.reducedImage, (xm, ym), (xm2, ym2), blueColor, 2) xm = xm + self.subImagePosition[0] ym = ym + self.subImagePosition[1] #print((xm, ym)) #return [xm, ym, orientation, self.markerTracker.quality] return MarkerPose(xm, ym, orientation, self.markerTracker.quality, self.markerTracker.order) def showCroppedImage(self): pass
class CameraDriver: def __init__(self, marker_order, marker_size, scaling_parameter): # Storage for image processing. self.current_frame = None self.processed_frame = None self.running = True self.busy = False # Initialize trackers. self.tracker = MarkerTracker(marker_order, marker_size, scaling_parameter) self.location = MarkerPose(None, None, None, None, None) def process_frame(self): if self.busy == False: self.busy = True self.processed_frame = self.current_frame frame_gray = self.current_frame if LooseVersion(cv2.__version__).version[0] == 2: self.processed_frame = cv2.cvtColor(self.current_frame, cv2.cv.CV_GRAY2BGR) else: self.processed_frame = cv2.cvtColor(self.current_frame, cv2.COLOR_GRAY2BGR) # Previous marker location is unknown, search in the entire image. #self.current_frame = self.tracker.locate_marker(frame_gray) current_pose = self.tracker.locate_marker(frame_gray) self.location = self.tracker.pose else: print 'skipping' def show_processed_frame(self): xm = self.location.x ym = self.location.y orientation = self.location.theta if self.location.quality < 0.9: cv2.circle(self.processed_frame, (xm, ym), 4, (55, 55, 255), 1) else: cv2.circle(self.processed_frame, (xm, ym), 4, (55, 55, 255), 3) xm2 = int(xm + 50 * math.cos(orientation)) ym2 = int(ym + 50 * math.sin(orientation)) cv2.line(self.processed_frame, (xm, ym), (xm2, ym2), (255, 0, 0), 2) cv2.imshow('filterdemo', self.processed_frame) key = cv2.waitKey(1) # Discard higher order bit, http://docs.opencv.org/3.0-beta/doc/py_tutorials/py_gui/py_image_display/py_image_display.html key = key & 0xff if key == 27: # Esc self.running = False if key == 114: # R print("Resetting") self.reset_location() if key == 115: # S # save image print("Saving image") filename = strftime("%Y-%m-%d %H-%M-%S") cv2.imwrite("markerlocator_image_%s.png" % filename, self.current_frame) self.busy = False def reset_location(self): # Reset all markers locations, forcing a full search on the next iteration. self.location = MarkerPose(None, None, None, None, None)
from MarkerTracker import MarkerTracker import numpy as np import cv2 import cv from time import sleep kernelSize = 31 order=5 markerTracker = MarkerTracker(5, kernelSize, 1) (kernel_order_n_Real, kernel_order_n_Imag) = markerTracker.generateSymmetryDetectorKernel(order, kernelSize) (kernel_order_1_Real, kernel_order_1_Imag) = markerTracker.generateSymmetryDetectorKernel(1, kernelSize) kernel_order_1_array = np.array(kernel_order_1_Real + 1j*kernel_order_1_Imag, dtype=complex) kernel_order_n_array = np.array(kernel_order_n_Real + 1j*kernel_order_n_Imag, dtype=complex) absolute = np.absolute(kernel_order_n_array) threshold = 0.4*absolute.max() phase = np.exp(0.1*3.14*1j) t1_temp = kernel_order_n_array*np.power(phase, order) t1 = t1_temp.real > threshold t2_temp = kernel_order_n_array*np.power(phase, order) t2 = t2_temp.real < -threshold img_t1_t2_diff = t1.astype(np.float32)-t2.astype(np.float32)
def addMarkerToTrack(self, order, kernelSize, scaleFactor): self.markerTrackers.append( MarkerTracker(order, kernelSize, scaleFactor)) self.markerLocationsX.append(0) self.markerLocationsY.append(0) self.subClassesInitialized = False
class TrackerInWindowMode: def __init__(self, order = 7, defaultKernelSize = 21): #cv.NamedWindow('reducedWindow', cv.CV_WINDOW_AUTOSIZE) self.windowWidth = 100 self.windowHeight = 100 self.frameGray = cv.CreateImage ((self.windowWidth, self.windowHeight), cv.IPL_DEPTH_32F, 1) self.originalImage = cv.CreateImage((self.windowWidth, self.windowHeight), cv.IPL_DEPTH_32F, 3) self.markerTracker = MarkerTracker(order, defaultKernelSize, 2500) self.trackerIsInitialized = False self.subImagePosition = None pass def cropFrame(self, frame, lastMarkerLocationX, lastMarkerLocationY): if(not self.trackerIsInitialized): self.markerTracker.allocateSpaceGivenFirstFrame(self.originalImage) self.reducedImage = cv.CreateImage((self.windowWidth, self.windowHeight), frame.depth, 3) xCornerPos = lastMarkerLocationX - self.windowWidth / 2 yCornerPos = lastMarkerLocationY - self.windowHeight / 2 # Ensure that extracted window is inside the original image. if(xCornerPos < 1): xCornerPos = 1 if(yCornerPos < 1): yCornerPos = 1 if(xCornerPos > frame.width - self.windowWidth): xCornerPos = frame.width - self.windowWidth if(yCornerPos > frame.height - self.windowHeight): yCornerPos = frame.height - self.windowHeight try: self.subImagePosition = (xCornerPos, yCornerPos, self.windowWidth, self.windowHeight) self.reducedImage = cv.GetSubRect(frame, self.subImagePosition) cv.ConvertScale(self.reducedImage, self.originalImage) cv.CvtColor(self.originalImage, self.frameGray, cv.CV_RGB2GRAY) except: print("frame: ", frame.depth) print("originalImage: ", self.originalImage.height, self.originalImage.width, self.originalImage) print("frameGray: ", self.frameGray.height, self.frameGray.width, self.frameGray.depth) print "Unexpected error:", sys.exc_info()[0] #quit(0) pass def locateMarker(self): (xm, ym) = self.markerTracker.locateMarker(self.frameGray) #xm = 50 #ym = 50 #cv.Line(self.reducedImage, (0, ym), (self.originalImage.width, ym), (0, 0, 255)) # B, G, R #cv.Line(self.reducedImage, (xm, 0), (xm, self.originalImage.height), (0, 0, 255)) redColor = (55, 55, 255) blueColor = (255, 0, 0) orientation = self.markerTracker.orientation cv.Circle(self.reducedImage, (xm, ym), 4, redColor, 2) xm2 = int(xm + 50*math.cos(orientation)) ym2 = int(ym + 50*math.sin(orientation)) cv.Line(self.reducedImage, (xm, ym), (xm2, ym2), blueColor, 2) xm = xm + self.subImagePosition[0] ym = ym + self.subImagePosition[1] #print((xm, ym)) #return [xm, ym, orientation, self.markerTracker.quality] return MarkerPose(xm, ym, orientation, self.markerTracker.quality, self.markerTracker.order) def showCroppedImage(self): pass
class TrackerInWindowMode: def __init__(self, order=7, defaultKernelSize=21): self.window_width = 100 self.window_height = 100 self.frameGray = np.zeros((self.window_height, self.window_width, 1), dtype=np.float32) self.originalImage = np.zeros( (self.window_height, self.window_width, 3), dtype=np.float32) self.markerTracker = MarkerTracker(order, defaultKernelSize, 2500) self.trackerIsInitialized = False self.subImagePosition = None self.reducedImage = None def crop_frame(self, frame, last_marker_location_x, last_marker_location_y): if not self.trackerIsInitialized: self.reducedImage = np.zeros( (self.window_height, self.window_width, 3), frame.dtype) x_corner_pos = last_marker_location_x - self.window_width / 2 y_corner_pos = last_marker_location_y - self.window_height / 2 # Ensure that extracted window is inside the original image. if x_corner_pos < 1: x_corner_pos = 1 if y_corner_pos < 1: y_corner_pos = 1 if x_corner_pos > frame.shape[1] - self.window_width: x_corner_pos = frame.shape[1] - self.window_width if y_corner_pos > frame.shape[0] - self.window_height: y_corner_pos = frame.shape[0] - self.window_height try: self.subImagePosition = (x_corner_pos, y_corner_pos, self.window_width, self.window_height) self.reducedImage = frame[ self.subImagePosition[1]:self.subImagePosition[1] + self.subImagePosition[3], self.subImagePosition[0]:self.subImagePosition[0] + self.subImagePosition[2], :] self.frameGray = cv2.cvtColor(self.reducedImage, cv2.cv.CV_RGB2GRAY) except: print("frame: ", frame.dtype) print("originalImage: ", self.originalImage.shape[0], self.originalImage.shape[1], self.originalImage) print("frameGray: ", self.frameGray.shape[0], self.frameGray.shape[1], self.frameGray.dtype) print "Unexpected error:", sys.exc_info()[0] pass def locate_marker(self): (xm, ym) = self.markerTracker.locate_marker(self.frameGray) red_color = (55, 55, 255) blue_color = (255, 0, 0) orientation = self.markerTracker.orientation cv2.circle(self.reducedImage, (xm, ym), 4, red_color, 2) xm2 = int(xm + 50 * math.cos(orientation)) ym2 = int(ym + 50 * math.sin(orientation)) cv2.line(self.reducedImage, (xm, ym), (xm2, ym2), blue_color, 2) xm = xm + self.subImagePosition[0] ym = ym + self.subImagePosition[1] return MarkerPose(xm, ym, orientation, self.markerTracker.quality, self.markerTracker.order) def show_cropped_image(self): pass
# main function if __name__ == "__main__": print "Running.." print 'Loading image..' img = cv2.imread('marker_image.jpg', 0) ''' plt.imshow(img, cmap='gray', interpolation='bicubic') #plt.xticks([]), plt.yticks([]) # to hide tick values on X and Y axis #plt.show() #print "image plotted.." ''' # Number of n-fold edges: order = 3 kernel_size = 25 scale_factor = 100 #print 'Finding marker..' mt = MarkerTracker(order, kernel_size, scale_factor) while True: mt.locate_marker(img) mt.determine_marker_orientation(img) mt.determine_marker_quality(img) #print 'Marker found..' #print 'Plotting marker..' #img2 = mt.extract_window_around_maker_location(img) #plt.imshow(img2, cmap='gray', interpolation='bicubic') #plt.xticks([]), plt.yticks([]) # to hide tick values on X and Y axis #plt.show()