def localize(self, tags): global arr_ball_pos tag_positions = Tag_Positions() for t in tags.tags: current_tag_position = Tag_Position() current_tag_position.id = t.id #This is all Open CV matrix stuff that is more complicated #than it should be. a = cv.fromarray(numpy.array([[[float(t.x), float(t.y)]]])) b = cv.fromarray(numpy.empty((1, 1, 2))) cv.PerspectiveTransform(a, b, self.transform) current_tag_position.x = numpy.asarray(b)[0, 0, 0] current_tag_position.y = numpy.asarray(b)[0, 0, 1] current_tag_position.theta = t.zRot - self.zRot_offset #Adjust tag angle based on its position current_tag_position.theta += correct(current_tag_position.x, current_tag_position.y, arr_correct_theta) if (current_tag_position.theta < -pi): current_tag_position.theta += 2 * pi if (current_tag_position.theta > pi): current_tag_position.theta -= 2 * pi tag_positions.tag_positions.append(current_tag_position) for tcache in arr_ball_pos: tag_positions.tag_positions.append(tcache) self.pub.publish(tag_positions)
def localize(self, tags): global arr_ball_pos tag_positions = Tag_Positions() for t in tags.tags: current_tag_position = Tag_Position() current_tag_position.id = t.id #This is all Open CV matrix stuff that is more complicated #than it should be. a = cv.fromarray(numpy.array([[[float(t.x), float(t.y)]]])) b = cv.fromarray(numpy.empty((1,1,2))) cv.PerspectiveTransform(a, b, self.transform) current_tag_position.x = numpy.asarray(b)[0,0,0] current_tag_position.y = numpy.asarray(b)[0,0,1] current_tag_position.theta = t.zRot - self.zRot_offset #Adjust tag angle based on its position current_tag_position.theta += correct(current_tag_position.x, current_tag_position.y, arr_correct_theta) if (current_tag_position.theta < -pi): current_tag_position.theta += 2*pi if (current_tag_position.theta > pi): current_tag_position.theta -= 2*pi tag_positions.tag_positions.append(current_tag_position) for tcache in arr_ball_pos: tag_positions.tag_positions.append(tcache) self.pub.publish(tag_positions)
def arTagPositionResolution(artagsTPositions): # averages several AR Tag position reports into one count = len(artagsTPositions) position = Tag_Position() position.id = artagsTPositions[0].id position.x = 1. * sum(map(lambda (atp): atp.x, artagsTPositions)) / count position.y = 1. * sum(map(lambda (atp): atp.y, artagsTPositions)) / count # break each theta into its vector representation, and use # atan2 to reconstruct an "average" theta thetaCosSum = 1. * sum(map(lambda (atp): cos(atp.theta), artagsTPositions)) thetaSinSum = 1. * sum(map(lambda (atp): sin(atp.theta), artagsTPositions)) position.theta = atan2(thetaSinSum, thetaCosSum) # (y,x) return position
def arTagPositionResolution(artagsTPositions): # averages several AR Tag position reports into one count = len(artagsTPositions) position = Tag_Position() position.id = artagsTPositions[0].id position.x = 1.*sum(map(lambda(atp): atp.x, artagsTPositions))/count position.y = 1.*sum(map(lambda(atp): atp.y, artagsTPositions))/count # break each theta into its vector representation, and use # atan2 to reconstruct an "average" theta thetaCosSum = 1.*sum(map(lambda(atp): cos(atp.theta), artagsTPositions)) thetaSinSum = 1.*sum(map(lambda(atp): sin(atp.theta), artagsTPositions)) position.theta = atan2(thetaSinSum, thetaCosSum) # (y,x) return position
def track_ball(self, blobs): global arr_ball_pos arr_ball_pos = [] tag_positions = Tag_Positions() for blob in blobs.blobs: if (blob.red == 255 and blob.green == 0 and blob.blue == 0 and blob.area > area_crit): current_tag_position = Tag_Position() current_tag_position.id = ball_id a = cv.fromarray(numpy.array([[[float(blob.x), float(blob.y)]]])) b = cv.fromarray(numpy.empty((1,1,2))) cv.PerspectiveTransform(a, b, self.transform) current_tag_position.x = numpy.asarray(b)[0,0,0] current_tag_position.y = numpy.asarray(b)[0,0,1] arr_ball_pos.append(current_tag_position)
def track_ball(self, blobs): global arr_ball_pos arr_ball_pos = [] tag_positions = Tag_Positions() for blob in blobs.blobs: if (blob.red == 255 and blob.green == 0 and blob.blue == 0 and blob.area > area_crit): current_tag_position = Tag_Position() current_tag_position.id = ball_id a = cv.fromarray( numpy.array([[[float(blob.x), float(blob.y)]]])) b = cv.fromarray(numpy.empty((1, 1, 2))) cv.PerspectiveTransform(a, b, self.transform) current_tag_position.x = numpy.asarray(b)[0, 0, 0] current_tag_position.y = numpy.asarray(b)[0, 0, 1] arr_ball_pos.append(current_tag_position)