def computeNormals(self, useExisting=False): if useExisting and 'normal' in self.vertAttr: #print("Skipping normal computation, normals are already defined") return # Expand out an indexed view of our vertices faceVerts = self.verts[self.tris] # Compute face normals as a cross product of the face vertices # TODO why the double colon...? faceNormals = np.cross(faceVerts[::, 1] - faceVerts[::, 0], faceVerts[::, 2] - faceVerts[::, 0]) # Area of each face is used for a weighted average below, so save that faceAreas = 0.5 * norm(faceNormals, axis=1).reshape((self.nTris, 1)) faceNormals = normalize(faceNormals) self.faceAttr['normal'] = faceNormals # Now compute vertex normals as a area-weighted average of the face normals vertNormals = np.zeros(self.verts.shape, dtype=self.verts.dtype) vertNormals[self.tris[:, 0]] += faceNormals * faceAreas vertNormals[self.tris[:, 1]] += faceNormals * faceAreas vertNormals[self.tris[:, 2]] += faceNormals * faceAreas normalize(vertNormals) self.vertAttr['normal'] = vertNormals
def ProcessFrame(frame): out = obj() frameOut = frame.copy() frame = norm(frame) mean, std = cv2.meanStdDev(frame) r = dist(frame, (mean[0], mean[1], mean[2])) mean, std = cv2.meanStdDev(r) print "m: %d, std %d" % (mean, std) #r = frame[:, :, 2] r = cv2.GaussianBlur(r, (9, 9), 0) debugFrame("ChannelOfInterest", r) edges = cv2.Canny(r, std * 3.5, 1.2* std) debugFrame("edges", edges) lines = cv2.HoughLinesP(edges, 4, math.pi/180, 200, minLineLength = 100, maxLineGap = 50) if isinstance(lines, list): print "numLines: %d" % len(lines[0]) for line in lines[0]: p1 = (line[0], line[1]) p2 = (line[2], line[3]) dx = p1[0] - p2[0] dy = abs(p1[1] - p2[1]) theta = math.atan2(dy, dx) if abs(theta - math.pi/2) < 10 *math.pi/180: cv2.line(frameOut, p1, p2, (255, 0, 255), 5) """ Below is the param info for HoughCircles image is the imaged being looked at by the function method cv2.CV_HOUGH_GRADIENT is the only method available dp the size of accumulator inverse relative to input image, dp = 2 means accumulator is half dim of input image minDist minimum distance between detected circles, too small and mult neighbor circ founds, big and neighbor circ counted as same param1 bigger value input into canny, unsure why it is useful therefore not using param2 vote threshold minRadius min Radius of circle to look for maxRadius max Radius of circle to look for """ maxrad = 300 minrad = 10 step = 50 for radius in range(minrad + step, maxrad + 1, step): circles = cv2.HoughCircles(image = r, method = cv2.cv.CV_HOUGH_GRADIENT, dp = .5, minDist = radius * 2, param2 = int((2 * radius * math.pi)/15), minRadius = radius - step, maxRadius = radius) msg = "minRadius: %d, maxRadius %d" % (radius - step, radius) if type(circles) != type(None): print msg + " found: %d" % (len(circles)) for circ in circles[0,:]: out.append(circ) out.draw(frameOut) else: print msg + " no circ found" frameOut = out.draw(frameOut) debugFrame("houghProb", frameOut) print "-------------------------------" return out
def ProcessFrame(frame): out = obj([False, 0, 0, 0]) frameOut = np.copy(frame) frame = norm(frame) m = cv2.mean(frame) red = dist(frame, [m[0], m[1], 255]) debugFrame("red", red) (row, col, pages) = frame.shape maxArea = (row * col) / 3 minArea = 200 red = cv2.medianBlur(red, ksize=3) ret, thresh = cv2.threshold(red, np.mean(red) - max(np.std(red), 10) * 2.65, 255, cv2.THRESH_BINARY_INV) debugFrame("threshOg", thresh) kernel = np.ones((7, 7), np.uint8) thresh = cv2.erode(thresh, kernel, iterations=2) debugFrame("thresh", thresh) contours, _ = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) for cont in contours: if len(cont) < 3: continue (center, (w, h), ang) = cv2.minAreaRect(cont) area = w * h aspectRatio = 0 if w != 0: aspectRatio = h / w if aspectRatio < 1 and aspectRatio != 0: aspectRatio = 1 / aspectRatio ang += 90 else: aspectRatio = 1 #caluclating solidity contArea = cv2.contourArea(cont) if contArea < .1: conArea = 1.0 if area == 0: area = 1 solidity = contArea / (1.0 * area) loc = (int(center[0]), int(center[1])) stringOut = "%f" % (solidity) #cv2.putText(frameOut, stringOut, loc, cv2.FONT_HERSHEY_SIMPLEX, .5, 255) if area > 1500 and aspectRatio > 3 and aspectRatio < 13 and solidity > .8: angp = -1 * ang #angp = angp - round((angp/180.0)) * 180 stringOut += ", ang: %d" % (angp) cv2.drawContours(frameOut, [cont], -1, (0, 0, 0), 3) out = obj([True, angp, center[0], center[1]]) frameOut = out.draw(frameOut) cv2.putText(frameOut, stringOut, loc, cv2.FONT_HERSHEY_SIMPLEX, .5, 255) print stringOut debugFrame("out", frameOut) return out
def ProcessFrame(frame): print("called") out = obj() frameOut = frame.copy() HEIGHT, WIDTH, _ = frame.shape contImg = np.zeros((HEIGHT, WIDTH, 3), np.uint8) frame = norm(frame) mean, std = cv2.meanStdDev(frame) r = dist(frame, (mean[0], mean[1], mean[2])) mean, std = cv2.meanStdDev(r) print "m: %d, std %d" % (mean, std) #r = frame[:, :, 2] r = cv2.GaussianBlur(r, (9, 9), 0) debugFrame("red", r) if std > 6: edges = cv2.Canny(r, std * 1.8, std * 1.2) else: edges = cv2.Canny(r, 30, 20) debugFrame("edges", edges) lines = cv2.HoughLinesP(edges, 4, math.pi / 180, 200, minLineLength=100, maxLineGap=50) if isinstance(lines, list): print "numLines: %d" % len(lines[0]) for line in lines[0]: p1 = (line[0], line[1]) p2 = (line[2], line[3]) dx = p1[0] - p2[0] dy = abs(p1[1] - p2[1]) theta = math.atan2(dy, dx) if abs(theta - math.pi / 2) < 10 * math.pi / 180: cv2.line(frameOut, p1, p2, (255, 0, 255), 5) contours, _ = cv2.findContours(edges, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) dice = [] for i in range(len(contours)): area = cv2.contourArea(contours[i]) if area < WIDTH * HEIGHT / 2: cv2.drawContours(contImg, contours, i, (255, 255, 255), 5) x, y, w, h = cv2.boundingRect(contours[i]) if w * h > 700 and h != 0 and w / h < 2 and w / h > 1 / 2: cv2.rectangle(frameOut, (x, y), (x + w, y + h), (0, 0, 255), 2) dice.append([x + w / 2, y + h / 2, (w + h) / 2]) debugFrame("contours", contImg) print "Contours: ", len(contours) if (len(dice) >= 2): for die in dice: out.append(die) frameOut = out.draw(frameOut) debugFrame("houghProb", frameOut) print "-------------------------------" return out
def computeNormals(self, useExisting=False): if useExisting and 'normal' in self.vertAttr: #print("Skipping normal computation, normals are already defined") return # Expand out an indexed view of our vertices faceVerts = self.verts[self.tris] # Compute face normals as a cross product of the face vertices # TODO why the double colon...? faceNormals = np.cross(faceVerts[::,1 ] - faceVerts[::,0], faceVerts[::,2 ] - faceVerts[::,0]) # Area of each face is used for a weighted average below, so save that faceAreas = 0.5 * norm(faceNormals, axis=1).reshape((self.nTris, 1)) faceNormals = normalize(faceNormals) self.faceAttr['normal'] = faceNormals # Now compute vertex normals as a area-weighted average of the face normals vertNormals = np.zeros( self.verts.shape, dtype=self.verts.dtype) vertNormals[ self.tris[:,0] ] += faceNormals * faceAreas vertNormals[ self.tris[:,1] ] += faceNormals * faceAreas vertNormals[ self.tris[:,2] ] += faceNormals * faceAreas normalize(vertNormals) self.vertAttr['normal'] = vertNormals
def ProcessFrame(frame): out = obj([False, 0, 0]) frameOut = frame.copy() frame = norm(frame) mean, std = cv2.meanStdDev(frame) r = dist(frame, (mean[0], mean[1], mean[2])) mean, std = cv2.meanStdDev(r) print "m: %d, std %d" % (mean, std) #r = frame[:, :, 2] r = cv2.GaussianBlur(r, (9, 9), 0) debugFrame("ChannelOfInterest", r) edges = cv2.Canny(r, std * 3.5, 1.2* std) debugFrame("edges", edges) lines = cv2.HoughLinesP(edges, 4, math.pi/180, 200, minLineLength = 100, maxLineGap = 50) if lines != None: print "numLines: %d" % len(lines[0]) for line in lines[0]: p1 = (line[0], line[1]) p2 = (line[2], line[3]) dx = p1[0] - p2[0] dy = abs(p1[1] - p2[1]) theta = math.atan2(dy, dx) if abs(theta - math.pi/2) < 10 *math.pi/180: cv2.line(frameOut, p1, p2, (255, 0, 255), 5) """ Below is the param info for HoughCircles image is the imaged being looked at by the function method cv2.CV_HOUGH_GRADIENT is the only method available dp the size of accumulator inverse relative to input image, dp = 2 means accumulator is half dim of input image minDist minimum distance between detected circles, too small and mult neighbor circ founds, big and neighbor circ counted as same param1 bigger value input into canny, unsure why it is useful therefore not using param2 vote threshold minRadius min Radius of circle to look for maxRadius max Radius of circle to look for """ maxrad = 300 minrad = 10 step = 50 for radius in range(minrad + step, maxrad + 1, step): circles = cv2.HoughCircles(image = r, method = cv2.cv.CV_HOUGH_GRADIENT, dp = .5, minDist = radius * 2, param2 = int((2 * radius * math.pi)/20), minRadius = radius - step, maxRadius = radius) msg = "minRadius: %d, maxRadius %d" % (radius - step, radius) if circles != None: print msg + " found: %d" % (len(circles)) for circ in circles[0,:]: cv2.circle(frameOut, (int(circ[0]), int(circ[1])), circ[2], (0, 255, 255), 2) cv2.circle(frameOut, (int(circ[0]), int(circ[1])), 2, (0, 0, 255), 2) else: print msg + " no circ found" frameOut = out.draw(frameOut) debugFrame("houghProb", frameOut) print "-------------------------------" return out.dict()
def ProcessFrame(frame): out = obj([False, 0, 0, 0]) frameOut = np.copy(frame) frame = norm(frame) m = cv2.mean(frame) red = dist(frame, [m[0], m[1], 255]) debugFrame("red", red) (row, col, pages) = frame.shape maxArea = (row * col)/3 minArea = 200 red = cv2.medianBlur(red, ksize = 3) ret, thresh = cv2.threshold(red, np.mean(red) - max(np.std(red), 10) * 2.65, 255, cv2.THRESH_BINARY_INV) debugFrame("threshOg", thresh) kernel = np.ones((7,7),np.uint8) thresh = cv2.erode(thresh,kernel, iterations = 2) debugFrame("thresh", thresh) contours, _ = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) for cont in contours: if len(cont) < 3: continue (center, (w, h), ang) = cv2.minAreaRect(cont) area = w * h aspectRatio = 0 if w != 0: aspectRatio = h / w if aspectRatio < 1 and aspectRatio != 0: aspectRatio = 1/aspectRatio ang+=90 else: aspectRatio = 1 #caluclating solidity contArea = cv2.contourArea(cont) if contArea < .1: conArea = 1.0 if area == 0: area = 1 solidity = contArea / (1.0 * area) loc = (int(center[0]), int(center[1])) stringOut = "%f" % (solidity) #cv2.putText(frameOut, stringOut, loc, cv2.FONT_HERSHEY_SIMPLEX, .5, 255) if area > 1500 and aspectRatio > 3 and aspectRatio < 13 and solidity > .8: angp = -1 * ang #angp = angp - round((angp/180.0)) * 180 stringOut += ", ang: %d" % (angp) cv2.drawContours(frameOut, [cont], -1, (0,0, 0), 3) out = obj([True, angp, center[0], center[1]]) frameOut = out.draw(frameOut) cv2.putText(frameOut, stringOut, loc, cv2.FONT_HERSHEY_SIMPLEX, .5, 255) print stringOut debugFrame("out", frameOut) return out
def ProcessFrame(frame): print("called") out = obj() frameOut = frame.copy() HEIGHT,WIDTH,_ = frame.shape contImg = np.zeros((HEIGHT,WIDTH,3), np.uint8) frame = norm(frame) mean, std = cv2.meanStdDev(frame) r = dist(frame, (mean[0], mean[1], mean[2])) mean, std = cv2.meanStdDev(r) print "m: %d, std %d" % (mean, std) #r = frame[:, :, 2] r = cv2.GaussianBlur(r, (9, 9), 0) debugFrame("red", r) if std > 6: edges = cv2.Canny(r, std * 1.8 , std * 1.2) else: edges = cv2.Canny(r, 30 , 20) debugFrame("edges", edges) lines = cv2.HoughLinesP(edges, 4, math.pi/180, 200, minLineLength = 100, maxLineGap = 50) if isinstance(lines, list): print "numLines: %d" % len(lines[0]) for line in lines[0]: p1 = (line[0], line[1]) p2 = (line[2], line[3]) dx = p1[0] - p2[0] dy = abs(p1[1] - p2[1]) theta = math.atan2(dy, dx) if abs(theta - math.pi/2) < 10 *math.pi/180: cv2.line(frameOut, p1, p2, (255, 0, 255), 5) contours, _ = cv2.findContours(edges, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) dice = [] for i in range(len(contours)): area = cv2.contourArea(contours[i]) if area < WIDTH * HEIGHT / 2: cv2.drawContours(contImg, contours, i, (255, 255, 255), 5) x,y,w,h = cv2.boundingRect(contours[i]) if w * h > 700 and h != 0 and w/h < 2 and w/h > 1/2: cv2.rectangle(frameOut,(x,y),(x+w,y+h),(0,0,255),2) dice.append([x + w/2, y + h/2, (w+h) / 2]) debugFrame("contours", contImg) print "Contours: ", len(contours) if(len(dice) >= 2): for die in dice: out.append(die) frameOut = out.draw(frameOut) debugFrame("houghProb", frameOut) print "-------------------------------" return out
def ProcessFrame(frame): out = obj([False, 0, 0]) frameOut = frame.copy() frame = norm(frame) mean, std = cv2.meanStdDev(frame) print mean r = cv2.cvtColor(frameOut, cv2.COLOR_BGR2GRAY) mean, std = cv2.meanStdDev(r) print "m: %d, std %d" % (mean, std) #r = frame[:, :, 2] debugFrame("COI", r) r = cv2.GaussianBlur(r, (7, 7), 0) edges = cv2.Canny(r, std * 2.0 , 1.3 * std) debugFrame("edges", edges) lines = cv2.HoughLines(edges, 3, math.pi/180, 110) poles = [] if lines is list: print "numLines: %d" % len(lines[0]) for line in lines[0]: r = line[0] theta = line[1] if ( abs(theta - round((theta/math.pi)) * math.pi) < 3.0 * math.pi / 180.0): a = math.cos(theta) b = math.sin(theta) x0 = a * r y0 = b * r pt1 = (int(x0 - 100*b), int(y0 + 100 * a)) pt2 = (int(x0 + 100 * b), int(y0 - 100 * a)) poles.append(((x0, y0), pt1, pt2)) cv2.line(frameOut, pt1, pt2, (255, 0, 255), 5) #filtering out the matching poles poles = sorted(poles, key = sortPoles) if len(poles) > 0: gatePoles = [poles[0]] for pole in poles: if abs(pole[0][0] - gatePoles[-1][0][0]) > 80: gatePoles.append(pole) for pole in gatePoles: cv2.line(frameOut, pole[1], pole[2], (0, 0, 255), 10) if len(gatePoles) == 2: out = obj([True, gatePoles[0][0][0], gatePoles[1][0][0]]) print "len Poles: %d, len gatePoles: %d" % (len(poles), len(gatePoles)) frameOut = out.draw(frameOut) debugFrame("houghReg", frameOut) print "-------------------------------" return out
def ProcessFrame(frame): out = {} out[ort] = -400 out[cpX] = 0 out[cpY] = 0 out[found] = False pRed = frame[:, :, 2] frame = norm(frame) red = frame[:,:,2] debugFrame("red", red) (row, col, pages) = frame.shape maxArea = (row * col)/3 minArea = 200 red = cv2.medianBlur(red, ksize = 3) ret, thresh = cv2.threshold(red, 120, 255, cv2.THRESH_BINARY) debugFrame("threshOg", thresh) kernel = np.ones((7,7),np.uint8) thresh = cv2.erode(thresh,kernel, iterations = 2) debugFrame("thresh", thresh) res, contours, _ = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) possiblePoles = [] for cont in contours: (center, (w, h), ang) = cv2.minAreaRect(cont) area = w * h aspectRatio = 0 if w != 0: aspectRatio = h / w if aspectRatio < 1 and aspectRatio != 0: aspectRatio = 1/aspectRatio if area > 1000 and aspectRatio > 3 and aspectRatio < 13: angp = abs(ang) angp = abs(angp - round((angp/90.0)) * 90) loc = (int(center[0]), int(center[1])) cv2.drawContours(frame, [cont], -1, (0,0, 0), 3) [vx,vy,x,y] = cv2.fitLine(cont, cv2.DIST_L2,0,0.01,0.01) if vx > .01: lefty = int((-x*vy/vx) + y) righty = int(((col-x)*vy/vx)+y) cv2.line(frame,(col-1,righty),(0,lefty),(0,255,0),2) angm = math.degrees(math.atan2(vy,vx)) angM = angm - round((angm/90.0)) * 90 stringOut = "angP %.2f, asp %.2f" % (angM, aspectRatio) cv2.putText(frame, stringOut, loc, cv2.FONT_HERSHEY_SIMPLEX, .5, 255) out[ort] = int(angM) out[cpY] = center[1] out[cpX] = center[0] out[found] = True debugFrame("out", frame) return out
def ProcessFrame(frame): out = obj([False, 0, 0]) frameOut = frame.copy() frame = norm(frame) mean, std = cv2.meanStdDev(frame) r = dist(frame, (mean[0], mean[1], 255)) mean, std = cv2.meanStdDev(r) print "m: %d, std %d" % (mean, std) #r = frame[:, :, 2] r = cv2.GaussianBlur(r, (9, 9), 0) debugFrame("red", r) edges = cv2.Canny(r, std * 1.8, 1.2 * std) debugFrame("edges", edges) lines = cv2.HoughLinesP(edges, 4, math.pi / 180, 145, minLineLength=200, maxLineGap=80) poles = [] if lines != None: print "numLines: %d" % len(lines[0]) for line in lines[0]: p1 = (line[0], line[1]) p2 = (line[2], line[3]) dx = p1[0] - p2[0] dy = abs(p1[1] - p2[1]) theta = math.atan2(dy, dx) if abs(theta - math.pi / 2) < 10 * math.pi / 180: cv2.line(frameOut, p1, p2, (255, 0, 255), 5) poles.append((p1, p2)) #filtering out the matching poles poles = sorted(poles, key=sortPoles) if len(poles) > 0: gatePoles = [poles[0]] for pole in poles: if abs(pole[0][0] - gatePoles[-1][0][0]) > 80: gatePoles.append(pole) for pole in gatePoles: cv2.line(frameOut, pole[0], pole[1], (0, 0, 255), 10) if len(gatePoles) == 2: out = obj([True, gatePoles[0][0][0], gatePoles[1][0][0]]) print "len Poles: %d, len gatePoles: %d" % (len(poles), len(gatePoles)) frameOut = out.draw(frameOut) debugFrame("houghProb", frameOut) print "-------------------------------" return out.dict()
def ProcessFrame(frame): out = {} out[lp] = 0 out[rp] = 0 out[cp] = 0 frame = norm(frame) red = frame[:, :, 2] debugFrame("red", red) (row, col, pages) = frame.shape maxArea = (row * col) / 3 minArea = 200 red = cv2.medianBlur(red, ksize=3) ret, thresh = cv2.threshold(red, 120, 255, cv2.THRESH_BINARY) kernel = np.ones((5, 5), np.uint8) thresh = cv2.dilate(thresh, kernel, iterations=1) debugFrame("thresh", thresh) _, contours, hierchy = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) possiblePoles = [] for cont in contours: (center, (w, h), ang) = cv2.minAreaRect(cont) area = w * h aspectRatio = h / w if aspectRatio < 1: aspectRatio = 1 / aspectRatio angp = abs(ang) angp = abs(angp - round((angp / 90.0)) * 90) if angp < 10.0 and area > minArea and abs(aspectRatio - 7) < 2: possiblePoles.append((cont, center)) possiblePoles = sorted(possiblePoles, key=sortPoles) if len(possiblePoles) > 1: prevPole = possiblePoles[0] poles = [prevPole] for pole in possiblePoles: diff = abs(pole[1][0] - prevPole[1][0]) if (diff > 20): poles.append(pole) prevPole = pole if (len(poles) == 2): left = poles[0] right = poles[1] out[rp] = int(right[1][0]) out[lp] = int(left[1][0]) out[cp] = out[lp] + (out[rp] - out[lp]) / 2 cv2.circle(frame, (out[cp], int((right[1][1] + left[1][1]) / 2)), 10, (0, 0, 0), 3) cv2.drawContours(frame, [left[0], right[0]], -1, (0, 0, 0), 3) debugFrame("out", frame) return out
def ProcessFrame(frame): out = obj([False, 0, 0]) frameOut = frame.copy() frame = norm(frame) mean, std = cv2.meanStdDev(frame) r = dist(frame, (mean[0], mean[1], 255)) r = cv2.cvtColor(frameOut, cv2.COLOR_BGR2GRAY) mean, std = cv2.meanStdDev(r) print "m: %d, std %d" % (mean, std) r = frame[:, :, 2] debugFrame("red", r) r = cv2.GaussianBlur(r, (7, 7), 0) edges = cv2.Canny(r, std * 2.0, 1.3 * std) debugFrame("edges", edges) lines = cv2.HoughLines(edges, 3, math.pi / 180, 110) poles = [] if lines != None: print "numLines: %d" % len(lines[0]) for line in lines[0]: r = line[0] theta = line[1] if (abs(theta - round((theta / math.pi)) * math.pi) < 3.0 * math.pi / 180.0): a = math.cos(theta) b = math.sin(theta) x0 = a * r y0 = b * r pt1 = (int(x0 - 100 * b), int(y0 + 100 * a)) pt2 = (int(x0 + 100 * b), int(y0 - 100 * a)) poles.append(((x0, y0), pt1, pt2)) cv2.line(frameOut, pt1, pt2, (255, 0, 255), 5) #filtering out the matching poles poles = sorted(poles, key=sortPoles) if len(poles) > 0: gatePoles = [poles[0]] for pole in poles: if abs(pole[0][0] - gatePoles[-1][0][0]) > 80: gatePoles.append(pole) for pole in gatePoles: cv2.line(frameOut, pole[1], pole[2], (0, 0, 255), 10) if len(gatePoles) == 2: out = obj([True, gatePoles[0][0][0], gatePoles[1][0][0]]) print "len Poles: %d, len gatePoles: %d" % (len(poles), len(gatePoles)) frameOut = out.draw(frameOut) debugFrame("houghReg", frameOut) print "-------------------------------" return out.dict()
def ProcessFrame(frame): out = {} out[lp] = 0 out[rp] = 0 out[cp] = 0 frame = norm(frame) red = frame[:,:,2] debugFrame("red", red) (row, col, pages) = frame.shape maxArea = (row * col)/3 minArea = 200 red = cv2.medianBlur(red, ksize = 3) ret, thresh = cv2.threshold(red, 120, 255, cv2.THRESH_BINARY) kernel = np.ones((5,5),np.uint8) thresh = cv2.dilate(thresh,kernel, iterations = 1) debugFrame("thresh", thresh) _, contours, hierchy = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) possiblePoles = [] for cont in contours: (center, (w, h), ang) = cv2.minAreaRect(cont) area = w * h aspectRatio = h / w if aspectRatio < 1: aspectRatio = 1/aspectRatio angp = abs(ang) angp = abs(angp - round((angp/90.0)) * 90) if angp < 10.0 and area > minArea and abs(aspectRatio - 7) < 2: possiblePoles.append((cont, center)) possiblePoles = sorted(possiblePoles, key = sortPoles) if len(possiblePoles) > 1: prevPole = possiblePoles[0] poles = [prevPole] for pole in possiblePoles: diff = abs( pole[1][0] - prevPole[1][0]) if (diff > 20): poles.append(pole) prevPole = pole if (len(poles) == 2): left = poles[0] right = poles[1] out[rp] = int(right[1][0]) out[lp] = int(left[1][0]) out[cp] = out[lp] + (out[rp] - out[lp])/2 cv2.circle(frame, (out[cp], int((right[1][1] + left[1][1])/2)), 10, (0, 0, 0), 3) cv2.drawContours(frame, [left[0], right[0]], -1, (0,0, 0), 3) debugFrame("out", frame) return out
def ProcessFrame(frame): out = obj([False, 0, 0, 0]) pRed = frame[:, :, 2] frame = norm(frame) red = frame[:,:,2] debugFrame("red", red) (row, col, pages) = frame.shape maxArea = (row * col)/3 minArea = 200 red = cv2.medianBlur(red, ksize = 3) ret, thresh = cv2.threshold(red, 120, 255, cv2.THRESH_BINARY) debugFrame("threshOg", thresh) kernel = np.ones((7,7),np.uint8) thresh = cv2.erode(thresh,kernel, iterations = 2) debugFrame("thresh", thresh) contours, _ = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) possiblePoles = [] if contours != None: for cont in contours: if len(cont) < 3: continue; (center, (w, h), ang) = cv2.minAreaRect(cont) area = w * h aspectRatio = 0 if w != 0: aspectRatio = h / w if aspectRatio < 1 and aspectRatio != 0: aspectRatio = 1/aspectRatio if area > 1000 and aspectRatio > 3 and aspectRatio < 13: angp = abs(ang) angp = abs(angp - round((angp/90.0)) * 90) loc = (int(center[0]), int(center[1])) cv2.drawContours(frame, [cont], -1, (0,0, 0), 3) [vx,vy,x,y] = cv2.fitLine(cont, 2,0,0.01,0.01) if vx > .01: lefty = int((-x*vy/vx) + y) righty = int(((col-x)*vy/vx)+y) cv2.line(frame,(col-1,righty),(0,lefty),(0,255,0),2) angm = math.degrees(math.atan2(vy,vx)) angM = angm - round((angm/90.0)) * 90 stringOut = "angP %.2f, asp %.2f" % (angM, aspectRatio) cv2.putText(frame, stringOut, loc, cv2.FONT_HERSHEY_SIMPLEX, .5, 255) out = obj([True, angM, center[0], center[1]]) out.draw(frame) debugFrame("out", frame) return out
def ProcessFrame(frame): out = obj() frameOut = frame.copy() frame = norm(frame) mean, std = cv2.meanStdDev(frame) r = dist(frame, (mean[0], mean[1], mean[2])) mean, std = cv2.meanStdDev(r) print "m: %d, std %d" % (mean, std) #r = frame[:, :, 2] r = cv2.GaussianBlur(r, (9, 9), 0) debugFrame("ChannelOfInterest", r) edges = cv2.Canny(r, std * 2.5, 1.2* std) debugFrame("edges", edges) height, width = frame.shape[:2] outImg = np.zeros((height,width,3), np.uint8) #frameOut = outImg """ Below is the param info for HoughCircles image is the imaged being looked at by the function method cv2.CV_HOUGH_GRADIENT is the only method available dp the size of accumulator inverse relative to input image, dp = 2 means accumulator is half dim of input image minDist minimum distance between detected circles, too small and mult neighbor circ founds, big and neighbor circ counted as same param1 bigger value input into canny, unsure why it is useful therefore not using param2 vote threshold minRadius min Radius of circle to look for maxRadius max Radius of circle to look for """ """ Find and make list of circles in the image """ drawAvg = False circleList = [] minDeg = 80.0 s = .8 d = 3.7 maxrad = 150 minrad = 30 step = 50 for radius in range(minrad + step, maxrad + 1, step): circles = cv2.HoughCircles(image = r, method = cv2.cv.CV_HOUGH_GRADIENT, dp = 4.2, minDist = 2 * radius, param2 = int((2 * radius * math.pi)/8.94), minRadius = radius - step, maxRadius = radius) msg = "minRadius: %d, maxRadius %d" % (radius - step, radius) if type(circles) != type(None): print msg + " found: %d" % (len(circles)) for circ in circles[0,:]: circleList.append(circ) if seeCircles: cv2.circle(frameOut, (circ[0], circ[1]), circ[2], (0,0,0), 2, 8, 0 ) else: print msg + " no circ found" if len(circleList) > 2: pts = [] for circle in circleList: #out.append(circle) if seeCircles: cv2.circle(frameOut, (int(circle[0]), int(circle[1])), int(circle[2]), (255,0,0), 2, 8, 0 ) cv2.circle(frameOut, (int(circle[0]), int(circle[1])), 7, (0,0,0), 2, 8, 0 ) pts.append([circle[0], circle[1]]) contour = np.array(pts, dtype=np.int32) (x,y), r = cv2.minEnclosingCircle(contour) out.append([int(x), int(y), int(r)]) #cv2.circle(frameOut, (int(x), int(y)), int(r), (0,0,0), 2, 8, 0 ) frameOut = out.draw(frameOut) out.draw(frameOut) debugFrame("houghProb", frameOut) print "-------------------------------" return out
def ProcessFrame(frame): out = obj([False, 0, 0]) frameOut = np.copy(frame) frame = norm(frame) m = cv2.mean(frame) red = dist(frame, [m[0], m[1], max(m[2] + 50, 255)]) debugFrame("red", red) (row, col, pages) = frame.shape maxArea = 30000 minArea = 1000 ret, thresh = cv2.threshold(red, np.mean(red) - np.std(red) * 1.5, 255, cv2.THRESH_BINARY_INV) kernel = np.ones((5,5),np.uint8) kernel[:, 0:1] = 0 kernel[:, 3:5] = 0 debugFrame("threshOg", thresh) thresh = cv2.erode(thresh,kernel, iterations = 1) thresh = cv2.dilate(thresh, kernel, iterations = 2) debugFrame("thresh", thresh) contours, _ = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) possiblePoles = [] for cont in contours: #cv2.drawContours(frame, [cont], -1, (0, 0, 255), 3) (center, (w, h), ang) = cv2.minAreaRect(cont) area = w * h if w > 0: aspectRatio = h / w if aspectRatio < 1 and aspectRatio != 0: aspectRatio = 1/aspectRatio ang+=90 else: aspectRation = 20 angp = abs(ang) angp = abs(angp - round((angp/180.0)) * 180) stringOut = "%d %d %d" % (angp, area, aspectRatio) cv2.putText(frameOut, stringOut, (int(center[0]), int(center[1])), cv2.FONT_HERSHEY_SIMPLEX, .5, 255) if (angp < 10 or angp > 170) and area > minArea and area < maxArea and abs(aspectRatio - 9) <= 4: possiblePoles.append((cont, center)) possiblePoles = sorted(possiblePoles, key = sortPoles) for pole in possiblePoles: cv2.drawContours(frameOut, [pole[0]], -1, (0, 0, 255), 3) (center, (w, h), ang) = cv2.minAreaRect(pole[0]) aspectRatio = h / w if aspectRatio < 1: ang+=90 angp = abs(ang) angp = abs(angp - round((angp/180)) * 180) area = h * w stringOut = "%d %d %d" % (angp, area, aspectRatio) cv2.putText(frameOut, stringOut, (int(center[0]), int(center[1])), cv2.FONT_HERSHEY_SIMPLEX, 1, 255) if len(possiblePoles) > 1: prevPole = possiblePoles[0] poles = [prevPole] for pole in possiblePoles: cv2.drawContours(frame, [pole[0]], -1, (0, 0, 255), 3) diff = abs( pole[1][0] - prevPole[1][0]) if (diff > 20): poles.append(pole) prevPole = pole if (len(poles) == 2): left = poles[0] right = poles[1] out = obj([True, int(right[1][0]), int(left[1][0])]) frameOut = out.draw(frameOut) cv2.drawContours(frameOut, [left[0], right[0]], -1, (0,0, 0), 3) debugFrame("out", frameOut) return out
def ProcessFrame(frame): out = obj([False, 0, 0]) frameOut = frame.copy() frame = norm(frame) mean, std = cv2.meanStdDev(frame) print "r mean: %d" % (mean[2]) r = dist(frame, (mean[0], mean[1], max(mean[2], 80))) mean, std = cv2.meanStdDev(r) print "m: %d, std %d" % (mean, std) #r = frame[:, :, 2] r = cv2.GaussianBlur(r, (9, 9), 0) debugFrame("red", r) if std > 6: edges = cv2.Canny(r, std * 2.0, std * 1.1) else: edges = cv2.Canny(r, 30 , 20) debugFrame("edges", edges) contours, _ = cv2.findContours(r, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) possiblePoles = [] for cont in contours: cv2.drawContours(frame, [cont], -1, (0, 0, 255), 3) (center, (w, h), ang) = cv2.minAreaRect(cont) area = w * h if w > 0: aspectRatio = h / w if aspectRatio < 1 and aspectRatio != 0: aspectRatio = 1/aspectRatio ang+=90 else: aspectRation = 20 angp = abs(ang) angp = abs(angp - round((angp/180.0)) * 180) stringOut = "%d %d %d" % (angp, area, aspectRatio) cv2.putText(frameOut, stringOut, (int(center[0]), int(center[1])), cv2.FONT_HERSHEY_SIMPLEX, .5, 255) if (angp < 10 or angp > 170) and area > minArea and area < maxArea and abs(aspectRatio) <= 2: possiblePoles.append((cont, center)) possiblePoles = sorted(possiblePoles, key = sortPoles) for pole in possiblePoles: cv2.drawContours(frameOut, [pole[0]], -1, (0, 0, 255), 3) (center, (w, h), ang) = cv2.minAreaRect(pole[0]) aspectRatio = h / w if aspectRatio < 1: ang+=90 angp = abs(ang) angp = abs(angp - round((angp/180)) * 180) area = h * w stringOut = "%d %d %d" % (angp, area, aspectRatio) cv2.putText(frameOut, stringOut, (int(center[0]), int(center[1])), cv2.FONT_HERSHEY_SIMPLEX, 1, 255) if len(possiblePoles) > 1: prevPole = possiblePoles[0] poles = [prevPole] for pole in possiblePoles: cv2.drawContours(frame, [pole[0]], -1, (0, 0, 255), 3) diff = abs( pole[1][0] - prevPole[1][0]) if (diff > 20): poles.append(pole) prevPole = pole if (len(poles) == 2): left = poles[0] right = poles[1] out = obj([True, int(right[1][0]), int(left[1][0])]) frameOut = out.draw(frameOut) cv2.drawContours(frameOut, [left[0], right[0]], -1, (0,0, 0), 3) debugFrame("out", frameOut) return out
def panel_control(data_array, mQ): n_program_state = 1 t_quickload_timer = 0 t_frame_start_time = time.time() BA_input_buffer = bytearray() f_first_pass = 1 x_trim = [0, 0, 0] throttle_inhib = False spd_err = 0 spd_err_p = 0 while 1: if time.time() - t_frame_start_time >= Settings.c_loop_frame_rate: # record the start of the processing so we can get timing data t_frame_start_time = time.time() # STATE = PANEL CONN - Connect to the panel if n_program_state == 1: mQ.put((0, 'Connecting to the panel....')) try: ser = serial.Serial('COM3', 115200, timeout=0.1) mQ.put((0, 'Connected to the panel')) time.sleep( 1 ) # serial needs a little bit of time to initialise, otherwise later code - esp CNIA fails n_program_state = 2 except serial.serialutil.SerialException: mQ.put((1, 'Could not connect to the panel')) time.sleep(5) # to avoid spamming the message queue pass # STATE = GAME CONN - Connect to the KRPC Server if n_program_state == 2: mQ.put((0, 'Connecting to the game server....')) try: conn = krpc.connect(name='Game Controller') mQ.put((0, 'Connected to the game server')) n_program_state = 3 except ConnectionRefusedError: mQ.put((1, 'Could not connect to the game server')) pass # STATE = LINKING - Link to the active Vessel if n_program_state == 3 and conn.krpc.current_game_scene == conn.krpc.current_game_scene.flight: mQ.put((0, 'Connecting to the vessel....')) try: vessel = conn.space_center.active_vessel mQ.put((0, 'Linked to ' + vessel.name)) n_program_state = 4 except krpc.client.RPCError: mQ.put((1, 'Could not connect to a vessel')) pass # STATE = Perform CNIA if n_program_state == 4: mQ.put((0, 'Starting CNIA...')) cnia(ser, conn, vessel) mQ.put((0, 'CNIA Complete')) n_program_state = 5 # STATE = Streams and objects- setup data input streams and reused objects if n_program_state == 5: # Camera object cam = conn.space_center.camera # Part temperatures part_temp_list = [] for x in vessel.parts.all: temp = [ conn.add_stream(getattr, x, 'temperature'), x.max_temperature, conn.add_stream(getattr, x, 'skin_temperature'), x.max_skin_temperature ] part_temp_list.append(temp) # Engine propellant status engine_propellant_status = [] for x in [ item for sublist in [p.propellants for p in vessel.parts.engines] for item in sublist ]: temp = [ conn.add_stream(getattr, x, 'total_resource_available'), conn.add_stream(getattr, x, 'total_resource_capacity') ] engine_propellant_status.append(temp) # Monoprop and electricity status resources = vessel.resources_in_decouple_stage( vessel.control.current_stage) mono_status = [ conn.add_stream(resources.amount, 'MonoPropellant'), conn.add_stream(resources.max, 'MonoPropellant') ] elec_status = [ conn.add_stream(resources.amount, 'ElectricCharge'), conn.add_stream(resources.max, 'ElectricCharge') ] # Gear Status gear_status = [] temp = [] for x in vessel.parts.landing_gear: temp.append(conn.add_stream(getattr, x, 'state')) gear_status.append(temp) temp = [] for x in vessel.parts.landing_legs: temp.append(conn.add_stream(getattr, x, 'state')) gear_status.append(temp) # Vessel and body list for map mode focus switching sorted_bodies = sorted(conn.space_center.bodies.items(), key=operator.itemgetter(1)) map_view_list = [(vessel.name, vessel)] map_view_list.extend(sorted_bodies) mQ.put((0, 'Stream setup complete')) n_program_state = 6 # STATE = RUNNING if n_program_state == 6: try: # catch RPC errors as they generally result from a scene change. Make more specific KRPC issue 256 # Send data to the arduino request it to process inputs - command byte = 0x00 BA_output_buffer = bytearray([0x00, 0x00, 0x00]) ser.write(BA_output_buffer) # Now while the Arduino is busy with inputs we processes the outputs - comamand byte = 0x01 BA_output_buffer = bytearray([0x01, 0x00, 0x00]) output_mapping(BA_output_buffer, conn, part_temp_list, engine_propellant_status, mono_status, elec_status, gear_status) # Make sure the Arduino has responded while ser.in_waiting != 40: pass # read back the data from the arduino BA_input_buffer_prev = BA_input_buffer BA_input_buffer = ser.read(40) # Now send the output date we calculated earlier ser.write(BA_output_buffer) if f_first_pass: # On the first pass copy the data in to avoid an error. BA_input_buffer_prev = BA_input_buffer f_first_pass = 0 # Check the status of the Arduino if BA_input_buffer[ 0] == 3: # status of 00000011 is fully powered # Action Groups for i in range(0, 10): if is_set(BA_input_buffer[int(i / 8) + 6], i % 8) and not is_set( BA_input_buffer_prev[int(i / 8) + 6], i % 8): vessel.control.toggle_action_group( (i + 1) % 10) if is_set(BA_input_buffer[11], 7) and not is_set( BA_input_buffer_prev[11], 7 ): # todo - Remove this when mux 0 pin A3 is resolved vessel.control.toggle_action_group(3) # Staging if is_set(BA_input_buffer[7], 7) and not is_set( BA_input_buffer_prev[7], 7) and is_set( BA_input_buffer[9], 7): vessel.control.activate_next_stage() n_program_state = 5 # trigger a stream refresh! # todo do we need this for decouple? # Systems if is_set(BA_input_buffer[11], 1) != is_set( BA_input_buffer_prev[11], 1): vessel.control.lights = is_set( BA_input_buffer[11], 1) if is_set(BA_input_buffer[11], 2) != is_set( BA_input_buffer_prev[11], 2): vessel.control.rcs = is_set(BA_input_buffer[11], 2) if is_set(BA_input_buffer[11], 4) != is_set( BA_input_buffer_prev[11], 4): vessel.control.gear = not is_set( BA_input_buffer[11], 4) # gear is opposite sense if is_set(BA_input_buffer[11], 5) != is_set( BA_input_buffer_prev[11], 5) or (is_set( BA_input_buffer[11], 6) != is_set( BA_input_buffer_prev[11], 6)): vessel.control.brakes = is_set( BA_input_buffer[11], 5) or is_set( BA_input_buffer[11], 6) # Navball Mode if is_set(BA_input_buffer[12], 1): vessel.control.speed_mode = vessel.control.speed_mode.target if is_set(BA_input_buffer[12], 2): vessel.control.speed_mode = vessel.control.speed_mode.orbit if is_set(BA_input_buffer[12], 3): vessel.control.speed_mode = vessel.control.speed_mode.surface # Landing Guidance if is_set(BA_input_buffer[12], 4) and not is_set( BA_input_buffer_prev[12], 4): ldg_guidance_draw(conn, vessel) elif not is_set(BA_input_buffer[12], 4) and is_set( BA_input_buffer_prev[12], 4): ldg_guidance_clear(conn) # Autopiliot spd_err_p = spd_err throttle_inhib, spd_err = Autopilot( BA_input_buffer, BA_input_buffer_prev, vessel, spd_err_p, 100) # Flight Control and Trims sas_overide = flight_control_inputs( BA_input_buffer, vessel, x_trim, throttle_inhib) # SAS SAS_inputs(BA_input_buffer, BA_input_buffer_prev, vessel, mQ, sas_overide) # Save/Load if is_set(BA_input_buffer[1], 0) and not is_set( BA_input_buffer_prev[1], 0): conn.space_center.quicksave() conn.ui.message('Quicksaving...', duration=5) if is_set(BA_input_buffer[1], 1) and not is_set( BA_input_buffer_prev[1], 1): t_quickload_timer = time.time() + 5 conn.ui.message( 'Hold for 5 seconds to Quickload...', duration=5) if not is_set(BA_input_buffer[1], 1): t_quickload_timer = 0 if time.time() >= t_quickload_timer > 0: conn.space_center.quickload() # Warp if is_set(BA_input_buffer[4], 7): conn.space_center.physics_warp_factor = 0 conn.space_center.rails_warp_factor = 0 elif is_set(BA_input_buffer[5], 1) and not is_set( BA_input_buffer_prev[5], 1 ) and conn.space_center.physics_warp_factor == 0: conn.space_center.rails_warp_factor = min( conn.space_center.rails_warp_factor + 1, conn.space_center.maximum_rails_warp_factor) elif is_set(BA_input_buffer[5], 3) and not is_set( BA_input_buffer_prev[5], 3): conn.space_center.rails_warp_factor = max( conn.space_center.rails_warp_factor - 1, 0) elif is_set(BA_input_buffer[5], 5) and not is_set( BA_input_buffer_prev[5], 5 ) and conn.space_center.rails_warp_factor == 0: conn.space_center.physics_warp_factor = min( conn.space_center.physics_warp_factor + 1, 3) elif is_set(BA_input_buffer[5], 7) and not is_set( BA_input_buffer_prev[5], 7): conn.space_center.physics_warp_factor = max( conn.space_center.physics_warp_factor - 1, 0) # Clear Target if is_set(BA_input_buffer[3], 1) and not is_set( BA_input_buffer_prev[3], 1): conn.space_center.clear_target() # Camera Control if Settings.G_cam_change_timer > 0: Settings.G_cam_change_timer = max( 0, Settings.G_cam_change_timer - Settings.c_loop_frame_rate) camera_inputs(cam, BA_input_buffer, mQ) # Map focus if cam.mode == cam.mode.map: if cam.focussed_vessel is not None: map_idx = [x[0] for x in map_view_list ].index(cam.focussed_vessel.name) elif cam.focussed_body is not None: map_idx = [x[0] for x in map_view_list ].index(cam.focussed_body.name) map_idx_new = map_idx if is_set(BA_input_buffer[2], 2) and not is_set( BA_input_buffer_prev[2], 2): map_idx_new = (map_idx + 1) % len(map_view_list) if is_set(BA_input_buffer[2], 0) and not is_set( BA_input_buffer_prev[2], 0): map_idx_new = (map_idx - 1) % len(map_view_list) if map_idx_new != map_idx: if map_idx_new == 0: # vessel cam.focussed_vessel = vessel cam.distance = cam.default_distance else: cam.focussed_body = map_view_list[ map_idx_new][1] cam.distance = cam.default_distance if is_set(BA_input_buffer[2], 1) and not is_set( BA_input_buffer_prev[2], 1): # always reset to te current vessel cam.focussed_vessel = vessel cam.distance = cam.default_distance # Vessel Switch if is_set(BA_input_buffer[2], 5) and not is_set( BA_input_buffer_prev[2], 5): vessel_list = [ v for v in conn.space_center.vessels if norm(v.position(vessel.reference_frame)) < 2000 ] conn.space_center.active_vessel = vessel_list[ (vessel_list.index(vessel) + 1) % len(vessel_list)] n_program_state = 4 elif is_set(BA_input_buffer[2], 7) and not is_set( BA_input_buffer_prev[2], 7): vessel_list = [ v for v in conn.space_center.vessels if norm(v.position(vessel.reference_frame)) < 2000 ] conn.space_center.active_vessel = vessel_list[ (vessel_list.index(vessel) - 1) % len(vessel_list)] n_program_state = 4 # put all the data onto the shared array for use by the GUI for i in range(len(BA_input_buffer)): data_array[i] = BA_input_buffer[i] except krpc.client.RPCError: n_program_state = 3 mQ.put((1, 'Main Loop Error')) # Check for Overuns and send a warning. if (time.time() - t_frame_start_time) > Settings.c_loop_frame_rate * 1.1: mQ.put( (1, 'OVERUN - ' + str(int( (time.time() - t_frame_start_time) * 1000)) + 'ms'))
def ProcessFrame(frame): import time #time.sleep(.1) out = obj([False, 0, 0, 0, 0, 0, 0]) frameOut = np.copy(frame) frame = norm(frame) m = cv2.mean(frame) red = dist(frame, [m[0], m[1], 255]) debugFrame("red", red) (row, col, pages) = frame.shape maxArea = (row * col) / 3 minArea = 200 red = cv2.medianBlur(red, ksize=3) ret, thresh = cv2.threshold(red, np.mean(red) - max(np.std(red), 10) * 2.65, 255, cv2.THRESH_BINARY_INV) debugFrame("threshOg", thresh) kernel = np.ones((7, 7), np.uint8) thresh = cv2.erode(thresh, kernel, iterations=2) debugFrame("thresh", thresh) contours, _ = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) mean, std = cv2.meanStdDev(frame) r = dist(frame, (mean[0], mean[1], mean[2])) mean, std = cv2.meanStdDev(r) r = cv2.GaussianBlur(r, (9, 9), 0) for i in range(len(contours)): epsilon = 0.045 * cv2.arcLength(contours[i], True) approx = cv2.approxPolyDP(contours[i], epsilon, True) #check if shape triangle cv2.fillConvexPoly(frameOut, approx, (255, 0, 0)) foundThreeOrFour = False """"if the approximated contour is a triangle""" if len(approx) == 3: #foundThreeOrFour = True """Finding the point opposite of the hypotenuse""" pts = [] for ptWrapper in approx: pts.append(ptWrapper[0]) #find hypotenuse maxLen = 0 maxIdx = 0 for j in range(3): #print j p1 = pts[j] #print (j+1) %3 p2 = pts[(j + 1) % 3] sideLen = math.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2) if sideLen > maxLen: maxLen = sideLen maxIdx = j p1 = pts[maxIdx] p2 = pts[(maxIdx + 1) % 3] center = pts[(maxIdx + 2) % 3] #cv2.circle(frameOut,(center[0], center[1]) , 5, color = (0,0,0), thickness = -1) elif len(approx) == 4: foundThreeOrFour = True """Figure out closest and furthest points""" pts = [] for ptWrapper in approx: pts.append(ptWrapper[0]) maxDist = 0 end1 = 0 end2 = 0 for i in range(0, len(pts)): for j in range(i, len(pts)): ptdist = math.sqrt((pts[i][0] - pts[j][0])**2 + (pts[i][1] - pts[j][1])**2) if (ptdist >= maxDist): maxDist = ptdist end1 = i end2 = j p1, p2 = pts[end1], pts[end2] centerPts = [] for i in range(len(pts)): if i != end1 and i != end2: centerPts.append(pts[i]) center = ((centerPts[0][0] + centerPts[1][0]) / 2, (centerPts[0][1] + centerPts[1][1]) / 2) #out = obj([True, int(center[0]), int(center[1]), int(endPts[0][0]), int(endPts[0][1]), int(endPts[1][0]), int(endPts[1][1])]) #frameOut = out.draw(frameOut) if foundThreeOrFour: v1 = [p1[0] - center[0], p1[1] - center[1]] v2 = [p2[0] - center[0], p2[1] - center[1]] dot = v1[0] * v2[0] + v1[1] * v2[1] """Finding magnitude of 2 shorter sides and the angle opposite of the hypotenuse""" mag1 = math.sqrt((v1[0])**2 + (v1[1])**2) mag2 = math.sqrt((v2[0])**2 + (v2[1])**2) #may increment mags and arg inside acos to be not 0 or in acos domain if mag1 == 0: mag1 += .001 if mag2 == 0: mag2 += .001 angle = math.acos(.999 * dot / (mag1 * mag2)) * 180.0 / math.pi """ checks that the biggest angle of the triangle is roughly 130 degrees and that the sides are similar sizes 130 degrees corresponds to the 45 degree bend """ MAX_ANG_DIF = 12 MAX_LEN_DIF = 60 MIN_LEN = 60 TARGET_ANGLE = 130 if mag1 > 100 or mag2 > 100: print "Angle, Mag1, Mag2: ", int(angle), int(mag1), int(mag2) #print angle if (abs(TARGET_ANGLE - abs(angle)) < MAX_ANG_DIF ) and abs(mag1 - mag2) < MAX_LEN_DIF and mag1 > MIN_LEN: out = obj([ True, int(center[0]), int(center[1]), int(p1[0]), int(p1[1]), int(p2[0]), int(p2[1]) ]) frameOut = out.draw(frameOut) debugFrame("out", frameOut) return out
def update(self, data_array, msgQ): # Manage notebook tab switching if self.panel_connected: if is_set(data_array[4], 6): self.notebook.select(7) elif is_set(data_array[5], 0): self.notebook.select(6) elif is_set(data_array[5], 2): self.notebook.select(5) elif is_set(data_array[5], 4): self.notebook.select(4) elif is_set(data_array[5], 6): self.notebook.select(3) elif is_set(data_array[2], 4): self.notebook.select(2) elif is_set(data_array[2], 6): self.notebook.select(1) elif is_set(data_array[3], 0): self.notebook.select(0) else: self.notebook.select(8) self.notebook_page = self.notebook.index(self.notebook.select()) # Update reused elements OI_data = [si_val(self.orbital_speed()) + 'm/s', si_val(self.apoapsis(), 2) + 'm', si_val(self.periapsis(), 2) + 'm', sec2time(self.orbital_period()), sec2time(self.time_to_apoapsis()), sec2time(self.time_to_periapsis()), '{0:.2f} deg'.format(degrees(self.inclination())), '{0:.4f}'.format(self.eccentricity()), '{0:.2f} deg'.format(degrees(self.long_asc_node()))] SI_data = [si_val(self.agl_altitude(), 3) + 'm', '{0:.1f} deg'.format(self.pitch()), '{0:.0f} deg'.format(self.heading()), '{0:.1f} deg'.format(self.roll()), si_val(self.speed(), 2) + 'm/s', si_val(self.v_speed(), 2) + 'm/s', si_val(self.h_speed(), 2) + 'm/s'] try: twr = self.max_thrust() / (self.mass() * self.surface_gravity()) except ZeroDivisionError: twr = 0 VI_data = [si_val(self.mass() * 1000) + 'g', si_val(self.max_thrust(), 3) + 'N', si_val(self.thrust(), 3) + 'N', '{0:.2f}'.format(twr)] # Tab 1 - Launch if self.notebook_page == 0: self.T1_OI.update(OI_data) self.T1_SI.update(SI_data) self.T1_VI.update(VI_data) self.T1_LP.animate(self.vessel, (self.latitude(), self.longitude()), self.mean_altitude()) # Tab 2 - Orbital if self.notebook_page == 1: self.T2_OI.update(OI_data) self.T2_VI.update(VI_data) self.T2_OP.animate(self.vessel, self.periapsis(), self.eccentricity(), self.ecc_anomaly(), self.inclination(), self.arg_periapsis()) # Tab 3 - Landing if self.notebook_page == 2: self.T3_OI.update(OI_data) self.T3_VI.update(VI_data) self.T3_SI.update(SI_data) # Tab 4 - Rondezvous if self.notebook_page == 3: self.T4_OI.update(OI_data) self.T4_VI.update(VI_data) self.T4_SI.update(SI_data) target_vessel = self.conn.space_center.target_vessel if target_vessel is None: T4_TI_data = ["No Target", '', '', '', '', '', '', '', '', '', '', ''] else: target_pos = target_vessel.position(self.vessel.reference_frame) target_vel = target_vessel.velocity(self.vessel.reference_frame) T4_TI_data = [target_vessel.name, '', '', '', si_val(norm(target_pos), 1) + 'm', si_val(norm(target_vel), 1) + 'm/s', '{0:.2f}m'.format(target_pos[0]), '{0:.2f}m'.format(target_pos[1]), '{0:.2f}m'.format(target_pos[2]), '{0:.2f}m/s'.format(target_vel[0]), '{0:.2f}m/s'.format(target_vel[1]), '{0:.2f}m/s'.format(target_vel[2])] self.T4_TI.update(T4_TI_data) # Tab 8 = Systems if self.notebook_page == 7: res_vals = [] for res in self.res_streams: res_vals.append('{0:.0f}'.format(res())) self.T8_TRD.update(self.res_names, res_vals) resIDS_vals = [] for res in self.resIDS_streams: resIDS_vals.append('{0:.0f}'.format(res())) self.T8_SRD.update(self.resIDS_names, resIDS_vals) # Tab 9 - Maintanance if self.notebook_page == 8: temp = ['{0:08b}'.format(data_array[0]), '{:d}ms'.format(data_array[27]), '{0:.0f}deg'.format(data_array[18] * 0.69310345 - 68.0241379), '{0:.0f}%'.format(data_array[28] / 255 * 100), '{0:.0f}%'.format(data_array[19] / 255 * 100)] self.T9_SD.update(temp) # Tab 9 --> Left Frame --> Digital Data SubframeLabel temp = ['{0:08b}'.format(data_array[1]), '{0:08b}'.format(data_array[2]), '{0:08b}'.format(data_array[3]), '{0:08b}'.format(data_array[4]), '{0:08b}'.format(data_array[5]), '{0:08b}'.format(data_array[6]), '{0:08b}'.format(data_array[7]), '{0:08b}'.format(data_array[8]), '{0:08b}'.format(data_array[9]), '{0:08b}'.format(data_array[10]), '{0:08b}'.format(data_array[33]), '{0:08b}'.format(data_array[34]), '{0:08b}'.format(data_array[35]), '{0:08b}'.format(data_array[36]), '{0:08b}'.format(data_array[37]), '{0:08b}'.format(data_array[38]) ] self.T9_DI.update(temp) # Tab 9 --> Left Frame --> Analogure Data SubframeLabel temp = [data_array[20], data_array[21], data_array[22], data_array[23], data_array[24], data_array[25], data_array[26]] self.T9_AI.update(temp) # Tab 9 --> Left Frame --> Analogure Data SubframeLabel temp = ["ON" if is_set(data_array[29], 0) else "OFF", "RUNNING" if is_set(data_array[29], 1) else "INITIATING", "CONNECTED" if is_set(data_array[29], 2) else "DISCON", data_array[30], bytes2int([data_array[33], data_array[34]]), "CONNECTED" if is_set(data_array[29], 3) else "DISCON", data_array[31], bytes2int([data_array[35], data_array[36]]), "CONNECTED" if is_set(data_array[29], 4) else "DISCON", data_array[32], bytes2int([data_array[37], data_array[38]])] self.T9_AP.update(temp) # Update message area if not msgQ.empty(): m = msgQ.get() self.msgL1.set(self.msgL2.get()) self.msgL2.set(self.msgL3.get()) self.msgL3.set(self.msgL4.get()) self.msgL4.set(msg_prefix[m[0]] + ': ' + m[1])
def update(self, data_array, msgQ): # Manage notebook tab switching if self.panel_connected: if is_set(data_array[4], 6): self.notebook.select(7) elif is_set(data_array[5], 0): self.notebook.select(6) elif is_set(data_array[5], 2): self.notebook.select(5) elif is_set(data_array[5], 4): self.notebook.select(4) elif is_set(data_array[5], 6): self.notebook.select(3) elif is_set(data_array[2], 4): self.notebook.select(2) elif is_set(data_array[2], 6): self.notebook.select(1) elif is_set(data_array[3], 0): self.notebook.select(0) else: self.notebook.select(8) self.notebook_page = self.notebook.index(self.notebook.select()) # Update reused elements OI_data = [ si_val(self.orbital_speed()) + 'm/s', si_val(self.apoapsis(), 2) + 'm', si_val(self.periapsis(), 2) + 'm', sec2time(self.orbital_period()), sec2time(self.time_to_apoapsis()), sec2time(self.time_to_periapsis()), '{0:.2f} deg'.format(degrees(self.inclination())), '{0:.4f}'.format(self.eccentricity()), '{0:.2f} deg'.format(degrees(self.long_asc_node())) ] SI_data = [ si_val(self.agl_altitude(), 3) + 'm', '{0:.1f} deg'.format(self.pitch()), '{0:.0f} deg'.format(self.heading()), '{0:.1f} deg'.format(self.roll()), si_val(self.speed(), 2) + 'm/s', si_val(self.v_speed(), 2) + 'm/s', si_val(self.h_speed(), 2) + 'm/s' ] try: twr = self.max_thrust() / (self.mass() * self.surface_gravity()) except ZeroDivisionError: twr = 0 VI_data = [ si_val(self.mass() * 1000) + 'g', si_val(self.max_thrust(), 3) + 'N', si_val(self.thrust(), 3) + 'N', '{0:.2f}'.format(twr) ] # Tab 1 - Launch if self.notebook_page == 0: self.T1_OI.update(OI_data) self.T1_SI.update(SI_data) self.T1_VI.update(VI_data) self.T1_LP.animate(self.vessel, (self.latitude(), self.longitude()), self.mean_altitude()) # Tab 2 - Orbital if self.notebook_page == 1: self.T2_OI.update(OI_data) self.T2_VI.update(VI_data) self.T2_OP.animate(self.vessel, self.periapsis(), self.eccentricity(), self.ecc_anomaly(), self.inclination(), self.arg_periapsis()) # Tab 3 - Landing if self.notebook_page == 2: self.T3_OI.update(OI_data) self.T3_VI.update(VI_data) self.T3_SI.update(SI_data) # Tab 4 - Rondezvous if self.notebook_page == 3: self.T4_OI.update(OI_data) self.T4_VI.update(VI_data) self.T4_SI.update(SI_data) target_vessel = self.conn.space_center.target_vessel if target_vessel is None: T4_TI_data = [ "No Target", '', '', '', '', '', '', '', '', '', '', '' ] else: target_pos = target_vessel.position( self.vessel.reference_frame) target_vel = target_vessel.velocity( self.vessel.reference_frame) T4_TI_data = [ target_vessel.name, '', '', '', si_val(norm(target_pos), 1) + 'm', si_val(norm(target_vel), 1) + 'm/s', '{0:.2f}m'.format( target_pos[0]), '{0:.2f}m'.format(target_pos[1]), '{0:.2f}m'.format(target_pos[2]), '{0:.2f}m/s'.format(target_vel[0]), '{0:.2f}m/s'.format(target_vel[1]), '{0:.2f}m/s'.format(target_vel[2]) ] self.T4_TI.update(T4_TI_data) # Tab 8 = Systems if self.notebook_page == 7: res_vals = [] for res in self.res_streams: res_vals.append('{0:.0f}'.format(res())) self.T8_TRD.update(self.res_names, res_vals) resIDS_vals = [] for res in self.resIDS_streams: resIDS_vals.append('{0:.0f}'.format(res())) self.T8_SRD.update(self.resIDS_names, resIDS_vals) # Tab 9 - Maintanance if self.notebook_page == 8: temp = [ '{0:08b}'.format(data_array[0]), '{:d}ms'.format(data_array[27]), '{0:.0f}deg'.format(data_array[18] * 0.69310345 - 68.0241379), '{0:.0f}%'.format(data_array[28] / 255 * 100), '{0:.0f}%'.format(data_array[19] / 255 * 100) ] self.T9_SD.update(temp) # Tab 9 --> Left Frame --> Digital Data SubframeLabel temp = [ '{0:08b}'.format(data_array[1]), '{0:08b}'.format( data_array[2]), '{0:08b}'.format(data_array[3]), '{0:08b}'.format(data_array[4]), '{0:08b}'.format( data_array[5]), '{0:08b}'.format(data_array[6]), '{0:08b}'.format(data_array[7]), '{0:08b}'.format( data_array[8]), '{0:08b}'.format(data_array[9]), '{0:08b}'.format(data_array[10]), '{0:08b}'.format( data_array[33]), '{0:08b}'.format(data_array[34]), '{0:08b}'.format(data_array[35]), '{0:08b}'.format( data_array[36]), '{0:08b}'.format(data_array[37]), '{0:08b}'.format(data_array[38]) ] self.T9_DI.update(temp) # Tab 9 --> Left Frame --> Analogure Data SubframeLabel temp = [ data_array[20], data_array[21], data_array[22], data_array[23], data_array[24], data_array[25], data_array[26] ] self.T9_AI.update(temp) # Tab 9 --> Left Frame --> Analogure Data SubframeLabel temp = [ "ON" if is_set(data_array[29], 0) else "OFF", "RUNNING" if is_set(data_array[29], 1) else "INITIATING", "CONNECTED" if is_set(data_array[29], 2) else "DISCON", data_array[30], bytes2int([data_array[33], data_array[34]]), "CONNECTED" if is_set(data_array[29], 3) else "DISCON", data_array[31], bytes2int([data_array[35], data_array[36]]), "CONNECTED" if is_set(data_array[29], 4) else "DISCON", data_array[32], bytes2int([data_array[37], data_array[38]]) ] self.T9_AP.update(temp) # Update message area if not msgQ.empty(): m = msgQ.get() self.msgL1.set(self.msgL2.get()) self.msgL2.set(self.msgL3.get()) self.msgL3.set(self.msgL4.get()) self.msgL4.set(msg_prefix[m[0]] + ': ' + m[1])
def ProcessFrame(frame): out = obj([False, 0, 0]) frameOut = np.copy(frame) frame = norm(frame) m = cv2.mean(frame) red = dist(frame, [m[0], m[1], max(m[2] + 50, 255)]) debugFrame("red", red) (row, col, pages) = frame.shape maxArea = 30000 minArea = 1000 ret, thresh = cv2.threshold(red, np.mean(red) - np.std(red) * 1.5, 255, cv2.THRESH_BINARY_INV) kernel = np.ones((5, 5), np.uint8) kernel[:, 0:1] = 0 kernel[:, 3:5] = 0 debugFrame("threshOg", thresh) thresh = cv2.erode(thresh, kernel, iterations=1) thresh = cv2.dilate(thresh, kernel, iterations=2) debugFrame("thresh", thresh) contours, _ = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) possiblePoles = [] for cont in contours: #cv2.drawContours(frame, [cont], -1, (0, 0, 255), 3) (center, (w, h), ang) = cv2.minAreaRect(cont) area = w * h if w > 0: aspectRatio = h / w if aspectRatio < 1 and aspectRatio != 0: aspectRatio = 1 / aspectRatio ang += 90 else: aspectRation = 20 angp = abs(ang) angp = abs(angp - round((angp / 180.0)) * 180) stringOut = "%d %d %d" % (angp, area, aspectRatio) cv2.putText(frameOut, stringOut, (int(center[0]), int(center[1])), cv2.FONT_HERSHEY_SIMPLEX, .5, 255) if ( angp < 10 or angp > 170 ) and area > minArea and area < maxArea and abs(aspectRatio - 9) <= 4: possiblePoles.append((cont, center)) possiblePoles = sorted(possiblePoles, key=sortPoles) for pole in possiblePoles: cv2.drawContours(frameOut, [pole[0]], -1, (0, 0, 255), 3) (center, (w, h), ang) = cv2.minAreaRect(pole[0]) aspectRatio = h / w if aspectRatio < 1: ang += 90 angp = abs(ang) angp = abs(angp - round((angp / 180)) * 180) area = h * w stringOut = "%d %d %d" % (angp, area, aspectRatio) cv2.putText(frameOut, stringOut, (int(center[0]), int(center[1])), cv2.FONT_HERSHEY_SIMPLEX, 1, 255) if len(possiblePoles) > 1: prevPole = possiblePoles[0] poles = [prevPole] for pole in possiblePoles: cv2.drawContours(frame, [pole[0]], -1, (0, 0, 255), 3) diff = abs(pole[1][0] - prevPole[1][0]) if (diff > 20): poles.append(pole) prevPole = pole if (len(poles) == 2): left = poles[0] right = poles[1] out = obj([True, int(right[1][0]), int(left[1][0])]) frameOut = out.draw(frameOut) cv2.drawContours(frameOut, [left[0], right[0]], -1, (0, 0, 0), 3) debugFrame("out", frameOut) return out
def panel_control(data_array, mQ): n_program_state = 1 t_quickload_timer = 0 t_frame_start_time = time.time() BA_input_buffer = bytearray() f_first_pass = 1 x_trim = [0, 0, 0] throttle_inhib = False spd_err = 0 spd_err_p = 0 while 1: if time.time() - t_frame_start_time >= Settings.c_loop_frame_rate: # record the start of the processing so we can get timing data t_frame_start_time = time.time() # STATE = PANEL CONN - Connect to the panel if n_program_state == 1: mQ.put((0, 'Connecting to the panel....')) try: ser = serial.Serial('COM3', 115200, timeout=0.1) mQ.put((0, 'Connected to the panel')) time.sleep(1) # serial needs a little bit of time to initialise, otherwise later code - esp CNIA fails n_program_state = 2 except serial.serialutil.SerialException: mQ.put((1, 'Could not connect to the panel')) time.sleep(5) # to avoid spamming the message queue pass # STATE = GAME CONN - Connect to the KRPC Server if n_program_state == 2: mQ.put((0, 'Connecting to the game server....')) try: conn = krpc.connect(name='Game Controller') mQ.put((0, 'Connected to the game server')) n_program_state = 3 except ConnectionRefusedError: mQ.put((1, 'Could not connect to the game server')) pass # STATE = LINKING - Link to the active Vessel if n_program_state == 3 and conn.krpc.current_game_scene == conn.krpc.current_game_scene.flight: mQ.put((0, 'Connecting to the vessel....')) try: vessel = conn.space_center.active_vessel mQ.put((0, 'Linked to ' + vessel.name)) n_program_state = 4 except krpc.client.RPCError: mQ.put((1, 'Could not connect to a vessel')) pass # STATE = Perform CNIA if n_program_state == 4: mQ.put((0, 'Starting CNIA...')) cnia(ser, conn, vessel) mQ.put((0, 'CNIA Complete')) n_program_state = 5 # STATE = Streams and objects- setup data input streams and reused objects if n_program_state == 5: # Camera object cam = conn.space_center.camera # Part temperatures part_temp_list = [] for x in vessel.parts.all: temp = [conn.add_stream(getattr, x, 'temperature'), x.max_temperature, conn.add_stream(getattr, x, 'skin_temperature'), x.max_skin_temperature] part_temp_list.append(temp) # Engine propellant status engine_propellant_status = [] for x in [item for sublist in [p.propellants for p in vessel.parts.engines] for item in sublist]: temp = [conn.add_stream(getattr, x, 'total_resource_available'), conn.add_stream(getattr, x, 'total_resource_capacity')] engine_propellant_status.append(temp) # Monoprop and electricity status resources = vessel.resources_in_decouple_stage(vessel.control.current_stage) mono_status = [conn.add_stream(resources.amount, 'MonoPropellant'), conn.add_stream(resources.max, 'MonoPropellant')] elec_status = [conn.add_stream(resources.amount, 'ElectricCharge'), conn.add_stream(resources.max, 'ElectricCharge')] # Gear Status gear_status = [] temp = [] for x in vessel.parts.landing_gear: temp.append(conn.add_stream(getattr, x, 'state')) gear_status.append(temp) temp = [] for x in vessel.parts.landing_legs: temp.append(conn.add_stream(getattr, x, 'state')) gear_status.append(temp) # Vessel and body list for map mode focus switching sorted_bodies = sorted(conn.space_center.bodies.items(), key=operator.itemgetter(1)) map_view_list = [(vessel.name, vessel)] map_view_list.extend(sorted_bodies) mQ.put((0, 'Stream setup complete')) n_program_state = 6 # STATE = RUNNING if n_program_state == 6: try: # catch RPC errors as they generally result from a scene change. Make more specific KRPC issue 256 # Send data to the arduino request it to process inputs - command byte = 0x00 BA_output_buffer = bytearray([0x00, 0x00, 0x00]) ser.write(BA_output_buffer) # Now while the Arduino is busy with inputs we processes the outputs - comamand byte = 0x01 BA_output_buffer = bytearray([0x01, 0x00, 0x00]) output_mapping(BA_output_buffer, conn, part_temp_list, engine_propellant_status, mono_status, elec_status, gear_status) # Make sure the Arduino has responded while ser.in_waiting != 40: pass # read back the data from the arduino BA_input_buffer_prev = BA_input_buffer BA_input_buffer = ser.read(40) # Now send the output date we calculated earlier ser.write(BA_output_buffer) if f_first_pass: # On the first pass copy the data in to avoid an error. BA_input_buffer_prev = BA_input_buffer f_first_pass = 0 # Check the status of the Arduino if BA_input_buffer[0] == 3: # status of 00000011 is fully powered # Action Groups for i in range(0, 10): if is_set(BA_input_buffer[int(i / 8) + 6], i % 8) and not is_set(BA_input_buffer_prev[int(i / 8) + 6], i % 8): vessel.control.toggle_action_group((i + 1) % 10) if is_set(BA_input_buffer[11], 7) and not is_set(BA_input_buffer_prev[11], 7): # todo - Remove this when mux 0 pin A3 is resolved vessel.control.toggle_action_group(3) # Staging if is_set(BA_input_buffer[7], 7) and not is_set(BA_input_buffer_prev[7], 7) and is_set(BA_input_buffer[9], 7): vessel.control.activate_next_stage() n_program_state = 5 # trigger a stream refresh! # todo do we need this for decouple? # Systems if is_set(BA_input_buffer[11], 1) != is_set(BA_input_buffer_prev[11], 1): vessel.control.lights = is_set(BA_input_buffer[11], 1) if is_set(BA_input_buffer[11], 2) != is_set(BA_input_buffer_prev[11], 2): vessel.control.rcs = is_set(BA_input_buffer[11], 2) if is_set(BA_input_buffer[11], 4) != is_set(BA_input_buffer_prev[11], 4): vessel.control.gear = not is_set(BA_input_buffer[11], 4) # gear is opposite sense if is_set(BA_input_buffer[11], 5) != is_set(BA_input_buffer_prev[11], 5) or (is_set(BA_input_buffer[11], 6) != is_set(BA_input_buffer_prev[11], 6)): vessel.control.brakes = is_set(BA_input_buffer[11], 5) or is_set(BA_input_buffer[11], 6) # Navball Mode if is_set(BA_input_buffer[12], 1): vessel.control.speed_mode = vessel.control.speed_mode.target if is_set(BA_input_buffer[12], 2): vessel.control.speed_mode = vessel.control.speed_mode.orbit if is_set(BA_input_buffer[12], 3): vessel.control.speed_mode = vessel.control.speed_mode.surface # Landing Guidance if is_set(BA_input_buffer[12], 4) and not is_set(BA_input_buffer_prev[12], 4): ldg_guidance_draw(conn, vessel) elif not is_set(BA_input_buffer[12], 4) and is_set(BA_input_buffer_prev[12], 4): ldg_guidance_clear(conn) # Autopiliot spd_err_p = spd_err throttle_inhib, spd_err = Autopilot(BA_input_buffer, BA_input_buffer_prev, vessel, spd_err_p, 100) # Flight Control and Trims sas_overide = flight_control_inputs(BA_input_buffer, vessel, x_trim, throttle_inhib) # SAS SAS_inputs(BA_input_buffer, BA_input_buffer_prev, vessel, mQ, sas_overide) # Save/Load if is_set(BA_input_buffer[1], 0) and not is_set(BA_input_buffer_prev[1], 0): conn.space_center.quicksave() conn.ui.message('Quicksaving...', duration=5) if is_set(BA_input_buffer[1], 1) and not is_set(BA_input_buffer_prev[1], 1): t_quickload_timer = time.time() + 5 conn.ui.message('Hold for 5 seconds to Quickload...', duration=5) if not is_set(BA_input_buffer[1], 1): t_quickload_timer = 0 if time.time() >= t_quickload_timer > 0: conn.space_center.quickload() # Warp if is_set(BA_input_buffer[4], 7): conn.space_center.physics_warp_factor = 0 conn.space_center.rails_warp_factor = 0 elif is_set(BA_input_buffer[5], 1) and not is_set(BA_input_buffer_prev[5], 1) and conn.space_center.physics_warp_factor == 0: conn.space_center.rails_warp_factor = min(conn.space_center.rails_warp_factor + 1, conn.space_center.maximum_rails_warp_factor) elif is_set(BA_input_buffer[5], 3) and not is_set(BA_input_buffer_prev[5], 3): conn.space_center.rails_warp_factor = max(conn.space_center.rails_warp_factor - 1, 0) elif is_set(BA_input_buffer[5], 5) and not is_set(BA_input_buffer_prev[5], 5) and conn.space_center.rails_warp_factor == 0: conn.space_center.physics_warp_factor = min(conn.space_center.physics_warp_factor + 1, 3) elif is_set(BA_input_buffer[5], 7) and not is_set(BA_input_buffer_prev[5], 7): conn.space_center.physics_warp_factor = max(conn.space_center.physics_warp_factor - 1, 0) # Clear Target if is_set(BA_input_buffer[3], 1) and not is_set(BA_input_buffer_prev[3], 1): conn.space_center.clear_target() # Camera Control if Settings.G_cam_change_timer > 0: Settings.G_cam_change_timer = max(0, Settings.G_cam_change_timer - Settings.c_loop_frame_rate) camera_inputs(cam, BA_input_buffer, mQ) # Map focus if cam.mode == cam.mode.map: if cam.focussed_vessel is not None: map_idx = [x[0] for x in map_view_list].index(cam.focussed_vessel.name) elif cam.focussed_body is not None: map_idx = [x[0] for x in map_view_list].index(cam.focussed_body.name) map_idx_new = map_idx if is_set(BA_input_buffer[2], 2) and not is_set(BA_input_buffer_prev[2], 2): map_idx_new = (map_idx + 1) % len(map_view_list) if is_set(BA_input_buffer[2], 0) and not is_set(BA_input_buffer_prev[2], 0): map_idx_new = (map_idx - 1) % len(map_view_list) if map_idx_new != map_idx: if map_idx_new == 0: # vessel cam.focussed_vessel = vessel cam.distance = cam.default_distance else: cam.focussed_body = map_view_list[map_idx_new][1] cam.distance = cam.default_distance if is_set(BA_input_buffer[2], 1) and not is_set(BA_input_buffer_prev[2], 1): # always reset to te current vessel cam.focussed_vessel = vessel cam.distance = cam.default_distance # Vessel Switch if is_set(BA_input_buffer[2], 5) and not is_set(BA_input_buffer_prev[2], 5): vessel_list = [v for v in conn.space_center.vessels if norm(v.position(vessel.reference_frame)) < 2000] conn.space_center.active_vessel = vessel_list[(vessel_list.index(vessel) + 1) % len(vessel_list)] n_program_state = 4 elif is_set(BA_input_buffer[2], 7) and not is_set(BA_input_buffer_prev[2], 7): vessel_list = [v for v in conn.space_center.vessels if norm(v.position(vessel.reference_frame)) < 2000] conn.space_center.active_vessel = vessel_list[(vessel_list.index(vessel) - 1) % len(vessel_list)] n_program_state = 4 # put all the data onto the shared array for use by the GUI for i in range(len(BA_input_buffer)): data_array[i] = BA_input_buffer[i] except krpc.client.RPCError: n_program_state = 3 mQ.put((1, 'Main Loop Error')) # Check for Overuns and send a warning. if (time.time() - t_frame_start_time) > Settings.c_loop_frame_rate * 1.1: mQ.put((1, 'OVERUN - ' + str(int((time.time() - t_frame_start_time) * 1000)) + 'ms'))
def ProcessFrame(frame): import time #time.sleep(.1) out = obj([False, 0, 0, 0, 0, 0, 0]) frameOut = np.copy(frame) frame = norm(frame) m = cv2.mean(frame) red = dist(frame, [m[0], m[1], 255]) debugFrame("red", red) (row, col, pages) = frame.shape maxArea = (row * col)/3 minArea = 200 red = cv2.medianBlur(red, ksize = 3) ret, thresh = cv2.threshold(red, np.mean(red) - max(np.std(red), 10) * 2.65, 255, cv2.THRESH_BINARY_INV) debugFrame("threshOg", thresh) kernel = np.ones((7,7),np.uint8) thresh = cv2.erode(thresh,kernel, iterations = 2) debugFrame("thresh", thresh) contours, _ = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) mean, std = cv2.meanStdDev(frame) r = dist(frame, (mean[0], mean[1], mean[2])) mean, std = cv2.meanStdDev(r) r = cv2.GaussianBlur(r, (9, 9), 0) for i in range(len(contours)): epsilon = 0.045*cv2.arcLength(contours[i],True) approx = cv2.approxPolyDP(contours[i],epsilon,True) #check if shape triangle cv2.fillConvexPoly(frameOut, approx, (255,0,0)) foundThreeOrFour = False """"if the approximated contour is a triangle""" if len(approx) == 3: #foundThreeOrFour = True """Finding the point opposite of the hypotenuse""" pts = [] for ptWrapper in approx: pts.append(ptWrapper[0]) #find hypotenuse maxLen = 0 maxIdx = 0 for j in range(3): #print j p1 = pts[j] #print (j+1) %3 p2 = pts[(j + 1)%3] sideLen = math.sqrt( (p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2 ) if sideLen > maxLen: maxLen = sideLen maxIdx = j p1 = pts[maxIdx] p2 = pts[(maxIdx + 1) % 3] center = pts[(maxIdx + 2) % 3] #cv2.circle(frameOut,(center[0], center[1]) , 5, color = (0,0,0), thickness = -1) elif len(approx) == 4: foundThreeOrFour = True """Figure out closest and furthest points""" pts = [] for ptWrapper in approx: pts.append(ptWrapper[0]) maxDist = 0 end1 = 0 end2 = 0 for i in range(0,len(pts)): for j in range(i,len(pts)): ptdist = math.sqrt( (pts[i][0] - pts[j][0]) ** 2 + (pts[i][1] - pts[j][1]) ** 2 ) if(ptdist >= maxDist): maxDist = ptdist end1 = i end2 = j p1, p2 = pts[end1], pts[end2] centerPts = [] for i in range(len(pts)): if i != end1 and i != end2: centerPts.append(pts[i]) center = ( (centerPts[0][0] + centerPts[1][0]) / 2, (centerPts[0][1] + centerPts[1][1]) / 2 ) #out = obj([True, int(center[0]), int(center[1]), int(endPts[0][0]), int(endPts[0][1]), int(endPts[1][0]), int(endPts[1][1])]) #frameOut = out.draw(frameOut) if foundThreeOrFour: v1 = [p1[0] - center[0], p1[1] - center[1]] v2 = [p2[0] - center[0], p2[1] - center[1]] dot = v1[0] * v2[0] + v1[1] * v2[1] """Finding magnitude of 2 shorter sides and the angle opposite of the hypotenuse""" mag1 = math.sqrt( (v1[0]) ** 2 + (v1[1]) ** 2 ) mag2 = math.sqrt( (v2[0]) ** 2 + (v2[1]) ** 2 ) #may increment mags and arg inside acos to be not 0 or in acos domain if mag1 == 0: mag1 += .001 if mag2 == 0: mag2 += .001 angle = math.acos(.999 * dot/(mag1 * mag2)) * 180.0 / math.pi """ checks that the biggest angle of the triangle is roughly 130 degrees and that the sides are similar sizes 130 degrees corresponds to the 45 degree bend """ MAX_ANG_DIF = 12 MAX_LEN_DIF = 60 MIN_LEN = 60 TARGET_ANGLE = 130 if mag1 > 100 or mag2 > 100: print "Angle, Mag1, Mag2: ", int(angle), int(mag1), int(mag2) #print angle if( abs(TARGET_ANGLE - abs(angle)) < MAX_ANG_DIF ) and abs(mag1 - mag2) < MAX_LEN_DIF and mag1 > MIN_LEN: out = obj([True, int(center[0]), int(center[1]), int(p1[0]), int(p1[1]), int(p2[0]), int(p2[1])]) frameOut = out.draw(frameOut) debugFrame("out", frameOut) return out