First commit

CircleDetector.py

@@ -0,0 +1,352 @@
+
+import cv2
+import numpy as np
+import math
+import time
+import sc_config
+current_milli_time = lambda: int(round(time.time() * 1000))
+
+
+'''
+TODO:
+make ECCENTRICITY dependent on craftAttitude
+remove pixel to distance and pixel to angle methods; import them
+'''
+
+
+class CircleDetector(object):
+
+
+
+	def __init__(self):
+		#load algorithm constants
+		#how round a circle needs to be. Perfect circle = 1
+		self.eccentricity = sc_config.config.get_float('algorithm', 'eccentricity', 0.6)
+		#acceptable distance(pixels) between cocentric circle centers
+		self.distance_threshold = sc_config.config.get_integer('algorithm','distance_threshold', 15)
+		# number of circles needed for a valid target(times 2); 2 circles are often overlayed
+		self.min_circles = sc_config.config.get_integer('algorithm','min_circles',5)
+		#pixels: used to identify repeat circles(stacked circles). Problem caused by findContours()
+		self.radius_tolerance = sc_config.config.get_integer('algorithm', 'radius_tolerance', 2)
+		#Tolerance used in comparing actaul ratios and preceived ratios 
+		self.ratio_tolerance = sc_config.config.get_float('algorithm', 'ratio_tolerance', 0.015)
+
+
+		#target specific data
+		#target_code is the unique ratio between rings
+		target_code_def = np.array([0.8,0.91,0.76,0.84,0.7,0.66,0.49])
+		self.target_code = sc_config.config.get_array('algorithm', 'target_code',target_code_def)
+		#the outer_ring is a scaling factor for targets of various sizes; radius of outer ring in meters
+		self.outer_ring = sc_config.config.get_float('algorithm', 'outer_ring', 0.08255)
+
+		#define field of view
+		self.cam_hfov = sc_config.config.get_float('camera', 'horizontal-fov', 70.42)
+		self.cam_vfov = sc_config.config.get_float('camera', 'vertical-fov', 43.3)
+
+
+	#analyze_frame - process an frame and look for a bulleye
+	#params -child_conn: child pipe connection
+	#		-img: raw image to be processed
+	#		-craftAttitude: roll and pitch of aircraft
+	#return -runtime: time in millis to process an image
+	#		-center: tuple(x,y) of the objects position on; 'None' when no target
+	#		-distance: distance in meters to the target; -1 when unable to calculate
+	#		-targetEllipses: ellipses that compose the detected target 'None' when no target
+	def analyze_frame(self, child_conn, img, craftAttitude):
+		#start timer
+		start = current_milli_time()
+
+
+		#blur image and grayscale
+		#img = cv2.medianBlur(img,5)
+
+		#grayscale image
+		cimg = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
+
+
+		#canny edge detector
+		edges = cv2.Canny(cimg,100,200,3)
+
+		if edges is not None:
+
+			#locate contours
+			contours, hierarchy = cv2.findContours(edges,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
+
+			#turn contours into ellipses
+			circles = np.empty((len(contours)),object)
+			circlesCnt = 0
+			for i in range(0,len(contours)):
+				contour = contours[i]
+				#make sure contour contains enough point for an ellipse
+				if(len(contour) > 4):
+					#detect an ellipse
+					ellipse = cv2.fitEllipse(contour)
+					#only take ellipses which are round
+					if self.checkEccentricity(ellipse,self.eccentricity):
+						circles[circlesCnt] = ellipse
+						circlesCnt += 1
+
+
+			#if circles were found then we look for nested circles
+			if circlesCnt > 0:
+
+				#get rid of null elements
+				circles = np.resize(circles,circlesCnt)
+				#look for nested ellipses
+				nestedCircles = self.detectNested(circles)
+
+				#if at least min_circles circles are nested look for target
+				#Times min_circles by two because we haven't removed repeat/stacked circles yet
+				if len(nestedCircles) > (self.min_circles * 2):
+
+					#look for circles with a common center
+					self.finalTarget, center = self.findCommonCenter(nestedCircles)
+
+					#we found the target position on xy-plane
+					if self.finalTarget is not None:
+
+						#decode the target rings for a list of ring ratios
+						ratios = self.tagAspectRatio(self.finalTarget)
+
+
+						#try to calculate distance to target
+						if ratios is not None:
+							distance = self.calcDistToTarget(self.finalTarget,ratios)
+
+							stop = current_milli_time()
+							child_conn.send((stop-start,center, distance, self.finalTarget))
+							return
+
+						#unable to calculate distance due to invalid data
+						else: 
+							stop = current_milli_time()
+							child_conn.send(( stop-start, center, -1, self.finalTarget))
+							return
+
+
+		#unable to locate target
+		stop = current_milli_time()
+		child_conn.send((stop-start,None,-1,None))
+		return
+
+	#distCenters - distance between two ellipses
+	def distCenters(self,ellipse1,ellipse2):
+		#distance between centers
+		distance = math.sqrt(math.pow((ellipse1[0][0]-ellipse2[0][0]),2) + math.pow((ellipse1[0][1] - ellipse2[0][1]),2))
+		return distance
+
+	#detectNested- return circles which are nested within other circles
+	def detectNested(self,rawCircles):
+		size = len(rawCircles)
+		nestedCircles = np.empty(size, object)
+		nestedCnt = 0
+		for i in range(0,size):
+			nested = False
+			for j in range(i, size):
+				if i != j:
+					circle1 = rawCircles[i]
+					circle2 = rawCircles[j]
+					#average major and minor axises
+					radius1 = (circle1[1][0] + circle1[1][1]) /2.0
+					radius2 = (circle2[1][0] + circle2[1][1]) /2.0
+
+					distance = self.distCenters(circle1,circle2)
+
+					#check if a circle is nested within another circle
+					if(distance < math.fabs(radius1 - radius2)):
+						nested = True
+			#add the base circle if it is nested
+			if nested:
+				nestedCircles[nestedCnt] = rawCircles[i]
+				nestedCnt += 1	
+		#remove null objects
+		nestedCircles  = np.resize(nestedCircles,nestedCnt)
+
+		return nestedCircles
+
+
+	#checkEccentricity - checks if an ellipse is 'round' enough
+	def checkEccentricity(self,ellipse, threshold):
+		#threshold = 1 for perfect circles
+		if ellipse[1][0] * 1.0/ ellipse[1][1] > threshold:
+			return True
+		return False
+	'''
+	def findCommonCenter(self,ellipses):
+
+		size = len(ellipses)
+		minDistance = 640 #image width
+		center = 0,0
+		for i in range(0,size):
+			nested = False
+			for j in range(i, size):
+				ellipse1 = ellipses[i]
+				ellipse2 = ellipses[j]
+				distance = math.sqrt(math.pow((ellipse1[0][0]-ellipse2[0][0]),2) + math.pow((ellipse1[0][1] - ellipse2[0][1]),2))
+				if distance <= minDistance and i != j:
+					minDistance = distance
+					center = ellipse1[0][0], ellipse1[0][1]
+		
+		return center
+
+	def ellipsesAroundCenter(ellipses, center, threshold):
+		size = len(ellipses)
+		centeredEllipses = np.empty(size,object)
+		centeredCnt = 0
+		for i in range(0,size):
+				distance = math.sqrt(math.pow((ellipses[i][0][0]-center[0]),2) + math.pow((ellipses[i][0][1] - center[1]),2))
+				if distance <= threshold:
+					centeredEllipses[centeredCnt] = ellipses[i]
+					centeredCnt += 1
+		centeredEllipses = np.resize(centeredEllipses,centeredCnt)
+		return centeredEllipses
+	'''
+	#findCommonCenter - locates a group of circles which share a the most common center. Returns the group and the center point 
+	def findCommonCenter(self,nestedCircles):
+
+		size = len(nestedCircles)
+
+		#sort by radius
+		for i in range(0,size):
+			baseCircle = nestedCircles[i]
+			smallestRadius = (baseCircle[1][0] + baseCircle[1][1]) /2.0
+			smallest = i
+
+			for j in range(i,size):
+				circle = nestedCircles[j]
+				radius = (circle[1][0] + circle[1][1]) /2.0
+				if(radius < smallestRadius):
+					smallestRadius = radius
+					smallest = j
+
+			nestedCircles[i] = nestedCircles[smallest]
+			nestedCircles[smallest] = baseCircle
+
+		#look at all circles
+		#add all circles that are within a certain threshold distance
+		#compare circle pairs and see which one has the most circles
+		concentricCombos = np.empty([size,size],object)
+
+
+		#start with the largest circle and scan all smaller circles and see if it is concentric with the large circle
+		maxConcentricCnt = 1
+		maxConcentricIndex = 0
+
+		#stores circle centers
+		xSum = np.zeros(size)
+		ySum = np.zeros(size)
+
+		for i in range(size-1,0,-1):
+			outer = nestedCircles[i]
+			concentricCombos[i][0] = outer
+			cnt = 1
+
+
+			for j in range(i, 0, -1):
+				inner = nestedCircles[j]
+				#outer circle and inner circle have same center, are different
+				if (self.distCenters(outer,inner) < self.distance_threshold) and (i != j):
+					#check that the circle isn't a repeat(a problem with findContours)
+					previous = concentricCombos[i][cnt -1]
+					radPrev = (previous[1][0] + previous[1][1]) /2.0
+					radCurr = (inner[1][0] + inner[1][1]) /2.0
+					#if the circle is cocentric and unique, add it
+					if(radPrev - radCurr) > self.radius_tolerance:
+						concentricCombos[i][cnt] = inner
+
+						xSum[i] += inner[0][0]
+						ySum[i] += inner[0][1]
+
+						cnt += 1
+
+			if(cnt > maxConcentricCnt):
+				maxConcentricCnt = cnt
+				maxConcentricIndex = i
+
+		#no concentric circles
+		if(maxConcentricCnt < self.min_circles):
+			return None,None
+
+		#choose the circle set with the most concentric circles
+		mostConcentric = concentricCombos[maxConcentricIndex]
+		mostConcentric = np.resize(mostConcentric, maxConcentricCnt)
+
+		#calculate meanCenter
+		meanCenter = xSum[maxConcentricIndex] / maxConcentricCnt, ySum[maxConcentricIndex]/maxConcentricCnt
+
+		return mostConcentric, meanCenter
+
+	#tagAspectRatio- processes the final target and calculates the ratio between rings. returns an array of ratios
+	def tagAspectRatio(self,target):
+		size = len(target)
+		#ratios = np.empty((size-1)*size/2.0, float)
+		ratios = np.empty(size-1,float)
+		cnt = 0
+
+		for i in range(0,size-1):
+			circle1 = target[i]
+			circle2 = target[i+1]
+			radius1 = (circle1[1][0] + circle1[1][1]) /2.0
+			radius2 = (circle2[1][0] + circle2[1][1]) /2.0
+
+
+			ratio = radius2 / radius1
+			ratios[cnt] = round(ratio,3)
+			cnt += 1
+		return ratios			
+
+
+
+	# pixels_to_angle_x - converts a number of pixels into an angle in radians 
+	def pixels_to_angle_x(self, num_pixels):
+		return num_pixels * math.radians(self.cam_hfov) / 640
+
+	# get_distance_from_pixels - returns distance to balloon in meters given number of pixels in image and expected 0.5m radius
+	#    size_in_pizels : diameter or radius of the object on the image (in pixels)
+	#    actual_size : diameter or radius of the object in meters
+	def get_distance_from_pixels(self,size_in_pixels, actual_size):
+		 # avoid divide by zero by returning 9999.9 meters for zero sized object 
+	    if (size_in_pixels == 0):
+	        return 9999.9
+	    # convert num_pixels to angular size
+	    return actual_size / self.pixels_to_angle_x(size_in_pixels)
+
+	#calculateRingSize - based on ring ID number and target size, calculate the size of a specific ring
+	def calculateRingSize(self,ringNumber):
+		radius = self.outer_ring #in meters
+
+		#actualRadius Outer ring size * ratio[n] * ratios[n + 1] ...
+		for i in range(0,ringNumber):
+			radius = radius * self.target_code[i]
+
+		return radius #in meters
+
+
+	#calcDistToTarget - processes a target and calculates distance to the target
+	def calcDistToTarget(self,target, ratios):
+		distance = 0
+		readings = 0
+		for i in range(0,len(ratios)):
+			ratio = ratios[i]
+			for j in range(0,len(self.target_code)):
+
+
+				if(math.fabs(self.target_code[j] - ratio) <= self.ratio_tolerance):
+					circle1 = target[i] #outer ring
+					circle2 = target[i+1] #inner ring
+					radius1 = (circle1[1][0] + circle1[1][1]) /2.0
+					radius2 = (circle2[1][0] + circle2[1][1]) /2.0
+
+					dist1 = self.get_distance_from_pixels(radius1, self.calculateRingSize(j))
+					dist2 = self.get_distance_from_pixels(radius2, self.calculateRingSize(j+1))
+					distance += (dist1 + dist2 )/2.0
+
+
+					readings += 1
+
+		#can not decode target	
+		if(readings == 0):
+			return -1
+		#average all distance readings
+		return distance/(readings * 1.0)
+

OpticalFlow.py

@@ -0,0 +1,19 @@
+import sc_config
+from sc_logger import sc_logger
+
+
+class OpticalFlow(object):
+
+	def __init__(self,args):
+		#load config file
+		sc_config.config.get_file(self.name())
+
+		#get camera index
+		self.camera_index = int(args.camera)
+
+	def name(self):
+		return "Optical_Flow"
+
+	def run(self):
+		sc_logger.text(sc_logger.GENERAL, 'running {0}'.format(self.name()))
+