#!/usr/bin/python from PerspectiveTransform import PerspectiveCorrecter from math import hypot pointLocationsInImage = [[952, 533], [946, 3], [1668, 3], [1667, 527]] realCoordinates = [[0, 0], [0, 109], [150.1, 109], [150.1, 0]] perspectiveConverter = PerspectiveCorrecter(pointLocationsInImage, realCoordinates) # not image 14 in /home/exchizz/ px_pkt = (208,377) meter_pktl = perspectiveConverter.convert(px_pkt) px_pkt = (95,319) meter_pktr = perspectiveConverter.convert(px_pkt) distance = hypot(meter_pktl[0] - meter_pktr[0],meter_pktl[1] - meter_pktr[1]) print "Test 1 distance is: ", distance, " error: ", 50-distance exit() # Image 15 px_pkt = (925,689) meter_pktl = perspectiveConverter.convert(px_pkt) px_pkt = (1246,839) meter_pktr = perspectiveConverter.convert(px_pkt)
class ROS:
def __init__(self):
# Instantiate ros publisher with information about the markers that
# will be tracked.
rospy.init_node('FrobitLocator')
self.toFind = rospy.get_param( '~to_find', 4)
rospy.logwarn("Looking for drone order: %d" % self.toFind);
self.pub = None
# self.markers = self.toFind
self.bridge = CvBridge()
pose_out = rospy.get_param( '~pose_out', 'pose')
self.pub = rospy.Publisher(pose_out, DronePose, queue_size = 10)
self.imagePub = rospy.Publisher("imagePublisher", Image, queue_size=10)
topic_camera_in = rospy.get_param( '~camera_in', '/camera/stream')
self.image_sub = rospy.Subscriber(topic_camera_in,Image,self.callbackFrame)
self.newFrame = False
self.frame = None
self.cd = CameraDriver(self.toFind, defaultKernelSize = 25)
# pointLocationsInImage = [[1328, 340], [874, 346], [856, 756], [1300, 762]]
# realCoordinates = [[0, 0], [300, 0], [300, 250], [0, 250]]
pointLocationsInImage = [[952, 533], [946, 3], [1668, 3], [1667, 527]]
# pointLocationsInImage = [[1668, 3], [1667, 527], [952, 533], [946, 3]]
realCoordinates = [[0, 0], [0, 109], [150.1, 109], [150.1, 0]]
self.perspectiveConverter = PerspectiveCorrecter(pointLocationsInImage, realCoordinates)
# while cd.running:
# pass
# cd.getImage()
# if RP.newFrame:
# RP.newFrame = False
def callbackFrame(self, frame):
try:
source = self.bridge.imgmsg_to_cv2(frame, "bgr8")
frame = cv.CreateImageHeader((source.shape[1], source.shape[0]), cv.IPL_DEPTH_8U, 3)
cv.SetData( frame, source.tostring(), source.dtype.itemsize * 3 * source.shape[1] )
except CvBridgeError as e:
print e
self.cd.processFrame(frame)
self.cd.drawDetectedMarkers()
self.cd.showProcessedFrame()
self.cd.handleKeyboardEvents()
y = self.cd.returnPositions()
self.publishMarkerLocations(y)
img = numpy.asarray(self.cd.processedFrame[:,:])
self.publishImage(img)
def publishMarkerLocations(self, locations):
# pkt = self.perspectiveConverter.convert(px_pkt)
# print "before homography:", locations[0].x, locations[0].y
x,y = self.perspectiveConverter.convert( (locations[0].x, locations[0].y) )
# print "after homography:", x, y
# print x,y
# print("x: %8.7f y: %8.7f angle: %8.7f quality: %8.7f order: %s lock: %d" % (x, y, locations[0].theta, locations[0].quality, locations[0].order, locations[0].order_match))
self.pub.publish( DronePose( x, y, locations[0].theta, locations[0].quality, locations[0].order_match ) )
def publishImage(self, Image):
try:
self.imagePub.publish(self.bridge.cv2_to_imgmsg(Image, 'bgr8'))
except CvBridgeError, e:
print e