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
