def main():
toFind = [5]
if PublishToROS:
RP = RosPublisher(toFind)
cd = CameraDriver(toFind, defaultKernelSize=15, cameraNumber=0)
signal.signal(signal.SIGINT, cd.signalHandler)
pointLocationsInImage = [[325, 0], [1045, 0], [1045, 720], [325, 720]]
realCoordinates = [[0, 0], [300, 0], [300, 300], [0, 300]]
perspectiveConverter = PerspectiveCorrecter(pointLocationsInImage,
realCoordinates)
while cd.running:
cd.getImage()
cd.processFrame()
y = cd.returnPositions()
if PublishToROS:
RP.publishMarkerLocations(perspectiveConverter.convertPose(y[0]))
cd.publishImageFrame(RP)
else:
cd.drawDetectedMarkers()
cd.handleKeyboardEvents()
cd.showProcessedFrame()
#poseCorrected = perspectiveConverter.convertPose(y[0])
#print "x: ", poseCorrected.x, " y: ", poseCorrected.y
print("Stopping")
def main():
toFind = [5]
if PublishToROS:
RP = RosPublisher(toFind)
cd = CameraDriver(toFind, defaultKernelSize = 15, cameraNumber = 0)
signal.signal(signal.SIGINT, cd.signalHandler)
pointLocationsInImage = [[325, 0], [1045, 0], [1045, 720], [325, 720]]
realCoordinates = [[0, 0], [300, 0], [300, 300], [0, 300]]
perspectiveConverter = PerspectiveCorrecter(pointLocationsInImage, realCoordinates)
while cd.running:
cd.getImage()
cd.processFrame()
y = cd.returnPositions()
if PublishToROS:
RP.publishMarkerLocations(perspectiveConverter.convertPose(y[0]))
cd.publishImageFrame(RP)
else:
cd.drawDetectedMarkers()
cd.handleKeyboardEvents()
cd.showProcessedFrame()
#poseCorrected = perspectiveConverter.convertPose(y[0])
#print "x: ", poseCorrected.x, " y: ", poseCorrected.y
print("Stopping")
def main():
t0 = time()
t1 = time()
t2 = time()
print "function vers1 takes %f" % (t1 - t0)
print "function vers2 takes %f" % (t2 - t1)
toFind = [4, 6]
if PublishToROS:
RP = RosPublisher(toFind)
# cd = CameraDriver(toFind, defaultKernelSize = 25) # Best in robolab.
cd = CameraDriver(toFind, defaultKernelSize=25)
t0 = time()
# 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]]
realCoordinates = [[0, 0], [0, 109], [150.1, 109], [150.1, 0]]
perspectiveConverter = PerspectiveCorrecter(pointLocationsInImage, realCoordinates)
while cd.running:
(t1, t0) = (t0, time())
# print "time for one iteration: %f" % (t0 - t1)
cd.getImage()
cd.processFrame()
cd.drawDetectedMarkers()
cd.showProcessedFrame()
cd.handleKeyboardEvents()
y = cd.returnPositions()
if PublishToROS:
RP.publishMarkerLocations(y)
else:
pass
# print y
for k in range(len(y)):
try:
poseCorrected = perspectiveConverter.convertPose(y[k])
print (
"%8.3f %8.3f %8.3f %8.3f %s"
% (
poseCorrected.x,
poseCorrected.y,
poseCorrected.theta,
poseCorrected.quality,
poseCorrected.order,
)
)
# print("%3d %3d %8.3f" % (y[0][0], y[0][1], y[0][2]))
except:
pass
print ("Stopping")
def main():
t0 = time()
t1 = time()
t2 = time()
print 'function vers1 takes %f' % (t1 - t0)
print 'function vers2 takes %f' % (t2 - t1)
toFind = [6]
if PublishToROS:
RP = RosPublisher(toFind)
#cd = CameraDriver(toFind, defaultKernelSize = 25) # Best in robolab.
# cd = CameraDriver(toFind, defaultKernelSize = 25, cameraDevice="drone_flight.mkv")
cd = CameraDriver(
toFind,
defaultKernelSize=25,
cameraDevice="recording_flight_with_5_marker_afternoon.mkv")
# cd = CameraDriver(toFind, defaultKernelSize = 25, cameraDevice="square.mkv")
t0 = time()
pointLocationsInImage = [[1328, 340], [874, 346], [856, 756], [1300, 762]]
realCoordinates = [[0, 0], [300, 0], [300, 250], [0, 250]]
perspectiveConverter = PerspectiveCorrecter(pointLocationsInImage,
realCoordinates)
while cd.running:
(t1, t0) = (t0, time())
#print "time for one iteration: %f" % (t0 - t1)
cd.getImage()
cd.processFrame()
cd.drawDetectedMarkers()
cd.showProcessedFrame()
cd.handleKeyboardEvents()
y = cd.returnPositions()
if PublishToROS:
RP.publishMarkerLocations(y)
cd.publishImageFrame(RP)
else:
pass
#print y
for k in range(len(y)):
try:
poseCorrected = perspectiveConverter.convertPose(y[k])
print(
"x: %8.3f y: %8.3f angle: %8.3f quality: %8.3f order: %s"
%
(poseCorrected.x, poseCorrected.y, poseCorrected.theta,
poseCorrected.quality, poseCorrected.order))
#print("%3d %3d %8.3f" % (y[0][0], y[0][1], y[0][2]))
except:
pass
print("Stopping")
def main():
t0 = time()
t1 = time()
t2 = time()
print 'function vers1 takes %f' %(t1-t0)
print 'function vers2 takes %f' %(t2-t1)
toFind = [6]
if PublishToROS:
RP = RosPublisher(toFind)
#cd = CameraDriver(toFind, defaultKernelSize = 25) # Best in robolab.
# cd = CameraDriver(toFind, defaultKernelSize = 25, cameraDevice="drone_flight.mkv")
cd = CameraDriver(toFind, defaultKernelSize = 25, cameraDevice="recording_flight_with_5_marker_afternoon.mkv")
# cd = CameraDriver(toFind, defaultKernelSize = 25, cameraDevice="square.mkv")
t0 = time()
pointLocationsInImage = [[1328, 340], [874, 346], [856, 756], [1300, 762]]
realCoordinates = [[0, 0], [300, 0], [300, 250], [0, 250]]
perspectiveConverter = PerspectiveCorrecter(pointLocationsInImage, realCoordinates)
while cd.running:
(t1, t0) = (t0, time())
#print "time for one iteration: %f" % (t0 - t1)
cd.getImage()
cd.processFrame()
cd.drawDetectedMarkers()
cd.showProcessedFrame()
cd.handleKeyboardEvents()
y = cd.returnPositions()
if PublishToROS:
RP.publishMarkerLocations(y)
cd.publishImageFrame(RP)
else:
pass
#print y
for k in range(len(y)):
try:
poseCorrected = perspectiveConverter.convertPose(y[k])
print("x: %8.3f y: %8.3f angle: %8.3f quality: %8.3f order: %s" % (poseCorrected.x, poseCorrected.y, poseCorrected.theta, poseCorrected.quality, poseCorrected.order))
#print("%3d %3d %8.3f" % (y[0][0], y[0][1], y[0][2]))
except:
pass
print("Stopping")
def main():
if publish_to_ros:
ros_publisher = RosPublisher(list_of_markers_to_find,
markerpose_ros_topic)
ld = LocatorDriver(list_of_markers_to_find,
default_kernel_size=14,
scaling_parameter=1000,
downscale_factor=2)
# ld = ImageDriver(list_of_markers_to_find, defaultKernelSize = 21)
t0 = time()
# Calibration of setup in robolab, so that the
# coordinates correspond to real world coordinates.
reference_point_locations_in_image = [[1328, 340], [874, 346], [856, 756],
[1300, 762]]
reference_point_loc_in_world_coord = [[0, 0], [300, 0], [300, 250],
[0, 250]]
persp_corr = PerspectiveCorrecter(reference_point_locations_in_image,
reference_point_loc_in_world_coord)
while ld.running and stop_flag is False:
(t1, t0) = (t0, time())
if print_debug_messages is True:
print "time for one iteration: %f" % (t0 - t1)
ld.get_image()
ld.process_frame()
ld.draw_detected_markers()
ld.show_processed_frame()
ld.handle_keyboard_events()
y = ld.return_positions()
if publish_to_ros:
w, h = ld.get_image_size()
ros_publisher.publish_marker_locations(y, w, h, ld)
else:
for k in range(len(y)):
try:
pose_corr = persp_corr.convertPose(y[k])
print("%8.3f %8.3f %8.3f %8.3f %s" %
(pose_corr.x, pose_corr.y, pose_corr.theta,
pose_corr.quality, pose_corr.order))
except Exception as e:
print("%s" % e)
print("Stopping")
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)
def main():
createLogFile = False
createLogPictures = False
if(createLogFile):
fileName = strftime("%m-%d-%Y %H:%M:%S")
textFile = open("log/%s.txt" % fileName, "w")
toFind = [5]
cd = CameraDriver(toFind, defaultKernelSize = 21, cameraNumber = 0)
pointLocationsInImage = [[1328, 340], [874, 346], [856, 756], [1300, 762]]
realCoordinates = [[0, 0], [300, 0], [300, 250], [0, 250]]
perspectiveConverter = PerspectiveCorrecter(pointLocationsInImage, realCoordinates)
print('Running..')
imageCounter = 0
while cd.running:
cd.getImage()
timestamp = time()
cd.processFrame()
cd.drawDetectedMarkers()
cd.handleKeyboardEvents()
y = cd.returnPositions()
for k in range(len(y)):
try:
poseCorrected = perspectiveConverter.convertPose(y[k])
if(createLogFile):
textFile.write(poseToString(poseCorrected, timestamp) + "\n")
if(createLogPictures):
#if(imageCounter>0):
cd.savePicture()
#imageCounter = 0
#else:
#imageCounter = imageCounter+1
except:
pass
cd.showProcessedFrame()
if(createLogFile):
textFile.close()
print("Stopping..")
def __init__(self):
rospy.init_node('MarkerLocator')
self.rate = rospy.Rate(10) # Hz
# needed to convert image
self.bridge = CvBridge()
# read topic names
markerimage_sub_topic = rospy.get_param("~markerimage_sub",'/markerlocator/image_raw')
markerpose_pub_topic = rospy.get_param("~markerpose_pub",'/markerlocator/markerpose')
# read parameters
self.show_image = rospy.get_param("~show_image", False)
self.print_debug_messages = rospy.get_param("~print_debug_messages", False)
down_scale_factor = rospy.get_param("/image_downscale_factor", 1.0)
self.marker_order = rospy.get_param("~marker_order", 0)
marker_size = rospy.get_param("~marker_size", 0) / down_scale_factor
calib_a_wld = [rospy.get_param("/calibrate_a_world_x", 0), rospy.get_param("/calibrate_a_world_y", 0)]
calib_a_img = [rospy.get_param("/calibrate_a_image_x", 0) / down_scale_factor,
rospy.get_param("/calibrate_a_image_y", 0) / down_scale_factor]
calib_b_wld = [rospy.get_param("/calibrate_b_world_x", 0), rospy.get_param("/calibrate_b_world_y", 0)]
calib_b_img = [rospy.get_param("/calibrate_b_image_x", 0) / down_scale_factor,
rospy.get_param("/calibrate_b_image_y", 0) / down_scale_factor]
calib_c_wld = [rospy.get_param("/calibrate_c_world_x", 0), rospy.get_param("/calibrate_c_world_y", 0)]
calib_c_img = [rospy.get_param("/calibrate_c_image_x", 0) / down_scale_factor,
rospy.get_param("/calibrate_c_image_y", 0) / down_scale_factor]
calib_d_wld = [rospy.get_param("/calibrate_d_world_x", 0), rospy.get_param("/calibrate_d_world_y", 0)]
calib_d_img = [rospy.get_param("/calibrate_d_image_x", 0) / down_scale_factor,
rospy.get_param("/calibrate_d_image_y", 0) / down_scale_factor]
# static parameters
scaling_parameter = 1000 # only for (debug) plotting purposes
# Calibration of setup, so that the coordinates correspond to real world coordinates.
calib_pts_image = [calib_a_img, calib_b_img, calib_c_img, calib_d_img]
calib_pts_world = [calib_a_wld, calib_b_wld, calib_c_wld, calib_d_wld]
self.perspective_corrector = PerspectiveCorrecter(calib_pts_image, calib_pts_world)
if self.print_debug_messages:
print 'Calibration points image:', calib_pts_image
print 'Calibration points world:', calib_pts_world
# instantiate camera driver
self.cd = CameraDriver(self.marker_order, marker_size, scaling_parameter)
if self.show_image:
self.cd.open_image_window()
# instantiate markerpose publisher
self.markerpose_msg = markerpose()
self.markerpose_pub = rospy.Publisher(markerpose_pub_topic, markerpose, queue_size=0)
# instantiate image subscribers
self.time_prev_image = time()
rospy.Subscriber(markerimage_sub_topic, Image, self.on_new_image)
# call updater routine
self.updater()
def main():
if publish_to_ros:
ros_publisher = RosPublisher(list_of_markers_to_find, markerpose_ros_topic)
cd = CameraDriver(list_of_markers_to_find, default_kernel_size=55, scaling_parameter=1000, downscale_factor=2) # Best in robolab.
# cd = ImageDriver(list_of_markers_to_find, defaultKernelSize = 21)
t0 = time()
# Calibration of setup in robolab, so that the coordinates correspond to real world coordinates.
reference_point_locations_in_image = [[1328, 340], [874, 346], [856, 756], [1300, 762]]
reference_point_locations_in_world_coordinates = [[0, 0], [300, 0], [300, 250], [0, 250]]
perspective_corrector = PerspectiveCorrecter(reference_point_locations_in_image,
reference_point_locations_in_world_coordinates)
while cd.running and stop_flag is False:
(t1, t0) = (t0, time())
if print_debug_messages is True:
print "time for one iteration: %f" % (t0 - t1)
cd.get_image()
cd.process_frame()
cd.draw_detected_markers()
cd.show_processed_frame()
cd.handle_keyboard_events()
y = cd.return_positions()
if publish_to_ros:
ros_publisher.publish_marker_locations(y)
else:
for k in range(len(y)):
try:
pose_corrected = perspective_corrector.convertPose(y[k])
print("%8.3f %8.3f %8.3f %8.3f %s" % (pose_corrected.x,
pose_corrected.y,
pose_corrected.theta,
pose_corrected.quality,
pose_corrected.order))
except Exception as e:
print("%s" % e)
print("Stopping")
def main():
t0 = time()
t1 = time()
toFind = [4, 5, 6, 7]
#toFind = [7]
# 4 Is not optimal, can be confused by 5
# 5 works fine
#cd = CameraDriver(toFind, defaultKernelSize = 25) # Best in robolab.
#cd = CameraDriver(toFind, defaultKernelSize = 21)
cd = CameraDriver(toFind, defaultKernelSize=21)
t0 = time()
pointLocationsInImage = [[1328, 340], [874, 346], [856, 756], [1300, 762]]
realCoordinates = [[0, 0], [300, 0], [300, 250], [0, 250]]
perspectiveConverter = PerspectiveCorrecter(pointLocationsInImage,
realCoordinates)
while cd.running:
(t1, t0) = (t0, time())
#print "time for one iteration: %f" % (t0 - t1)
cd.getImage()
timestamp = time()
cd.processFrame()
cd.drawDetectedMarkers()
cd.showProcessedFrame()
cd.handleKeyboardEvents()
y = cd.returnPositions()
for k in range(len(y)):
try:
poseCorrected = perspectiveConverter.convertPose(y[k])
#print("%8.3f %8.3f %8.3f %8.3f %s" % (poseCorrected.x, poseCorrected.y, poseCorrected.theta, poseCorrected.quality, poseCorrected.order))
print(poseToString(poseCorrected, timestamp))
#print("%3d %3d %8.3f" % (y[0][0], y[0][1], y[0][2]))
except:
pass
print("Stopping")
def main():
t0 = time()
t1 = time()
toFind = [4, 5, 6, 7]
#toFind = [7]
# 4 Is not optimal, can be confused by 5
# 5 works fine
#cd = CameraDriver(toFind, defaultKernelSize = 25) # Best in robolab.
#cd = CameraDriver(toFind, defaultKernelSize = 21)
cd = CameraDriver(toFind, defaultKernelSize = 21)
t0 = time()
pointLocationsInImage = [[1328, 340], [874, 346], [856, 756], [1300, 762]]
realCoordinates = [[0, 0], [300, 0], [300, 250], [0, 250]]
perspectiveConverter = PerspectiveCorrecter(pointLocationsInImage, realCoordinates)
while cd.running:
(t1, t0) = (t0, time())
#print "time for one iteration: %f" % (t0 - t1)
cd.getImage()
timestamp = time()
cd.processFrame()
cd.drawDetectedMarkers()
cd.showProcessedFrame()
cd.handleKeyboardEvents()
y = cd.returnPositions()
for k in range(len(y)):
try:
poseCorrected = perspectiveConverter.convertPose(y[k])
#print("%8.3f %8.3f %8.3f %8.3f %s" % (poseCorrected.x, poseCorrected.y, poseCorrected.theta, poseCorrected.quality, poseCorrected.order))
print(poseToString(poseCorrected, timestamp))
#print("%3d %3d %8.3f" % (y[0][0], y[0][1], y[0][2]))
except:
pass
print("Stopping")
def main():
toFind = [5]
cd = CameraDriver(toFind, defaultKernelSize = 21, cameraNumber = 0)
pointLocationsInImage = [[1328, 340], [874, 346], [856, 756], [1300, 762]]
realCoordinates = [[0, 0], [300, 0], [300, 250], [0, 250]]
perspectiveConverter = PerspectiveCorrecter(pointLocationsInImage, realCoordinates)
while cd.running:
cd.getImage()
timestamp = time()
cd.processFrame()
#cd.drawDetectedMarkers()
#cd.handleKeyboardEvents()
y = cd.returnPositions()
for k in range(len(y)):
try:
poseCorrected = perspectiveConverter.convertPose(y[k])
print "x: ", poseCorrected.x, " y:", poseCorrected.y, " timestamp: ", timestamp
except:
pass
def main():
toFind = [5]
cd = CameraDriver(toFind, defaultKernelSize=21, cameraNumber=0)
pointLocationsInImage = [[1328, 340], [874, 346], [856, 756], [1300, 762]]
realCoordinates = [[0, 0], [300, 0], [300, 250], [0, 250]]
perspectiveConverter = PerspectiveCorrecter(pointLocationsInImage,
realCoordinates)
while cd.running:
cd.getImage()
timestamp = time()
cd.processFrame()
#cd.drawDetectedMarkers()
#cd.handleKeyboardEvents()
y = cd.returnPositions()
for k in range(len(y)):
try:
poseCorrected = perspectiveConverter.convertPose(y[k])
print "x: ", poseCorrected.x, " y:", poseCorrected.y, " timestamp: ", timestamp
except:
pass
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
class marker_locator_node():
def __init__(self):
rospy.init_node('MarkerLocator')
self.rate = rospy.Rate(10) # Hz
# needed to convert image
self.bridge = CvBridge()
# read topic names
markerimage_sub_topic = rospy.get_param("~markerimage_sub",
'/markerlocator/image_raw')
markerpose_pub_topic = rospy.get_param("~markerpose_pub",
'/markerlocator/markerpose')
# read parameters
self.show_image = rospy.get_param("~show_image", False)
self.print_debug_messages = rospy.get_param("~print_debug_messages",
False)
down_scale_factor = rospy.get_param("/image_downscale_factor", 1.0)
self.marker_order = rospy.get_param("~marker_order", 0)
marker_size = rospy.get_param("~marker_size", 0) / down_scale_factor
calib_a_wld = [
rospy.get_param("/calibrate_a_world_x", 0),
rospy.get_param("/calibrate_a_world_y", 0)
]
calib_a_img = [
rospy.get_param("/calibrate_a_image_x", 0) / down_scale_factor,
rospy.get_param("/calibrate_a_image_y", 0) / down_scale_factor
]
calib_b_wld = [
rospy.get_param("/calibrate_b_world_x", 0),
rospy.get_param("/calibrate_b_world_y", 0)
]
calib_b_img = [
rospy.get_param("/calibrate_b_image_x", 0) / down_scale_factor,
rospy.get_param("/calibrate_b_image_y", 0) / down_scale_factor
]
calib_c_wld = [
rospy.get_param("/calibrate_c_world_x", 0),
rospy.get_param("/calibrate_c_world_y", 0)
]
calib_c_img = [
rospy.get_param("/calibrate_c_image_x", 0) / down_scale_factor,
rospy.get_param("/calibrate_c_image_y", 0) / down_scale_factor
]
calib_d_wld = [
rospy.get_param("/calibrate_d_world_x", 0),
rospy.get_param("/calibrate_d_world_y", 0)
]
calib_d_img = [
rospy.get_param("/calibrate_d_image_x", 0) / down_scale_factor,
rospy.get_param("/calibrate_d_image_y", 0) / down_scale_factor
]
# static parameters
scaling_parameter = 1000 # only for (debug) plotting purposes
# Calibration of setup, so that the coordinates correspond to real world coordinates.
calib_pts_image = [calib_a_img, calib_b_img, calib_c_img, calib_d_img]
calib_pts_world = [calib_a_wld, calib_b_wld, calib_c_wld, calib_d_wld]
self.perspective_corrector = PerspectiveCorrecter(
calib_pts_image, calib_pts_world)
if self.print_debug_messages:
print 'Calibration points image:', calib_pts_image
print 'Calibration points world:', calib_pts_world
# instantiate camera driver
self.cd = CameraDriver(self.marker_order, marker_size,
scaling_parameter)
if self.show_image:
self.cd.open_image_window()
# instantiate markerpose publisher
self.markerpose_msg = markerpose()
self.markerpose_pub = rospy.Publisher(markerpose_pub_topic,
markerpose,
queue_size=0)
# instantiate image subscribers
self.time_prev_image = time()
rospy.Subscriber(markerimage_sub_topic, Image, self.on_new_image)
# call updater routine
self.updater()
def on_new_image(self, msg):
self.cd.current_frame = self.bridge.imgmsg_to_cv2(msg, "8UC1")
self.cd.process_frame()
if self.show_image:
self.cd.show_processed_frame()
self.cd.handle_keyboard_events()
# publish the marker position
self.markerpose_msg.header.stamp = rospy.get_rostime()
pose_corrected = self.perspective_corrector.convertPose(
self.cd.location)
self.markerpose_msg.order = pose_corrected.order
self.markerpose_msg.x = pose_corrected.x
self.markerpose_msg.y = pose_corrected.y
self.markerpose_msg.theta = pose_corrected.theta
self.markerpose_msg.quality = pose_corrected.quality
self.markerpose_msg.timestamp = msg.header.stamp
self.markerpose_pub.publish(self.markerpose_msg)
# print debug info
if self.print_debug_messages == True:
print(
"Time: %.3f Marker: %s x: %8.3f y:%8.3f theta: %8.3f quality: %8.3f interval: %.3f"
% (msg.header.stamp.to_sec(), pose_corrected.order,
pose_corrected.x, pose_corrected.y, pose_corrected.theta,
pose_corrected.quality, (time() - self.time_prev_image)))
self.time_prev_image = time()
def updater(self):
while not rospy.is_shutdown():
self.rate.sleep()
class marker_locator_node():
def __init__(self):
rospy.init_node('MarkerLocator')
self.rate = rospy.Rate(10) # Hz
# needed to convert image
self.bridge = CvBridge()
# read topic names
markerimage_sub_topic = rospy.get_param("~markerimage_sub",'/markerlocator/image_raw')
markerpose_pub_topic = rospy.get_param("~markerpose_pub",'/markerlocator/markerpose')
# read parameters
self.show_image = rospy.get_param("~show_image", False)
self.print_debug_messages = rospy.get_param("~print_debug_messages", False)
down_scale_factor = rospy.get_param("/image_downscale_factor", 1.0)
self.marker_order = rospy.get_param("~marker_order", 0)
marker_size = rospy.get_param("~marker_size", 0) / down_scale_factor
calib_a_wld = [rospy.get_param("/calibrate_a_world_x", 0), rospy.get_param("/calibrate_a_world_y", 0)]
calib_a_img = [rospy.get_param("/calibrate_a_image_x", 0) / down_scale_factor,
rospy.get_param("/calibrate_a_image_y", 0) / down_scale_factor]
calib_b_wld = [rospy.get_param("/calibrate_b_world_x", 0), rospy.get_param("/calibrate_b_world_y", 0)]
calib_b_img = [rospy.get_param("/calibrate_b_image_x", 0) / down_scale_factor,
rospy.get_param("/calibrate_b_image_y", 0) / down_scale_factor]
calib_c_wld = [rospy.get_param("/calibrate_c_world_x", 0), rospy.get_param("/calibrate_c_world_y", 0)]
calib_c_img = [rospy.get_param("/calibrate_c_image_x", 0) / down_scale_factor,
rospy.get_param("/calibrate_c_image_y", 0) / down_scale_factor]
calib_d_wld = [rospy.get_param("/calibrate_d_world_x", 0), rospy.get_param("/calibrate_d_world_y", 0)]
calib_d_img = [rospy.get_param("/calibrate_d_image_x", 0) / down_scale_factor,
rospy.get_param("/calibrate_d_image_y", 0) / down_scale_factor]
# static parameters
scaling_parameter = 1000 # only for (debug) plotting purposes
# Calibration of setup, so that the coordinates correspond to real world coordinates.
calib_pts_image = [calib_a_img, calib_b_img, calib_c_img, calib_d_img]
calib_pts_world = [calib_a_wld, calib_b_wld, calib_c_wld, calib_d_wld]
self.perspective_corrector = PerspectiveCorrecter(calib_pts_image, calib_pts_world)
if self.print_debug_messages:
print 'Calibration points image:', calib_pts_image
print 'Calibration points world:', calib_pts_world
# instantiate camera driver
self.cd = CameraDriver(self.marker_order, marker_size, scaling_parameter)
if self.show_image:
self.cd.open_image_window()
# instantiate markerpose publisher
self.markerpose_msg = markerpose()
self.markerpose_pub = rospy.Publisher(markerpose_pub_topic, markerpose, queue_size=0)
# instantiate image subscribers
self.time_prev_image = time()
rospy.Subscriber(markerimage_sub_topic, Image, self.on_new_image)
# call updater routine
self.updater()
def on_new_image(self, msg):
self.cd.current_frame = self.bridge.imgmsg_to_cv2(msg, "8UC1")
self.cd.process_frame()
if self.show_image:
self.cd.show_processed_frame()
self.cd.handle_keyboard_events()
# publish the marker position
self.markerpose_msg.header.stamp = rospy.get_rostime()
pose_corrected = self.perspective_corrector.convertPose(self.cd.location)
self.markerpose_msg.order = pose_corrected.order
self.markerpose_msg.x = pose_corrected.x
self.markerpose_msg.y = pose_corrected.y
self.markerpose_msg.theta = pose_corrected.theta
self.markerpose_msg.quality = pose_corrected.quality
self.markerpose_msg.timestamp = msg.header.stamp
self.markerpose_pub.publish(self.markerpose_msg)
# print debug info
if self.print_debug_messages == True:
print("Time: %.3f Marker: %s x: %8.3f y:%8.3f theta: %8.3f quality: %8.3f interval: %.3f" % (msg.header.stamp.to_sec(), pose_corrected.order, pose_corrected.x, pose_corrected.y, pose_corrected.theta, pose_corrected.quality, (time() - self.time_prev_image)))
self.time_prev_image = time()
def updater(self):
while not rospy.is_shutdown():
self.rate.sleep()
def __init__(self):
rospy.init_node('MarkerLocator')
self.rate = rospy.Rate(10) # Hz
# needed to convert image
self.bridge = CvBridge()
# read topic names
markerimage_sub_topic = rospy.get_param("~markerimage_sub",
'/markerlocator/image_raw')
markerpose_pub_topic = rospy.get_param("~markerpose_pub",
'/markerlocator/markerpose')
# read parameters
self.show_image = rospy.get_param("~show_image", False)
self.print_debug_messages = rospy.get_param("~print_debug_messages",
False)
down_scale_factor = rospy.get_param("/image_downscale_factor", 1.0)
self.marker_order = rospy.get_param("~marker_order", 0)
marker_size = rospy.get_param("~marker_size", 0) / down_scale_factor
calib_a_wld = [
rospy.get_param("/calibrate_a_world_x", 0),
rospy.get_param("/calibrate_a_world_y", 0)
]
calib_a_img = [
rospy.get_param("/calibrate_a_image_x", 0) / down_scale_factor,
rospy.get_param("/calibrate_a_image_y", 0) / down_scale_factor
]
calib_b_wld = [
rospy.get_param("/calibrate_b_world_x", 0),
rospy.get_param("/calibrate_b_world_y", 0)
]
calib_b_img = [
rospy.get_param("/calibrate_b_image_x", 0) / down_scale_factor,
rospy.get_param("/calibrate_b_image_y", 0) / down_scale_factor
]
calib_c_wld = [
rospy.get_param("/calibrate_c_world_x", 0),
rospy.get_param("/calibrate_c_world_y", 0)
]
calib_c_img = [
rospy.get_param("/calibrate_c_image_x", 0) / down_scale_factor,
rospy.get_param("/calibrate_c_image_y", 0) / down_scale_factor
]
calib_d_wld = [
rospy.get_param("/calibrate_d_world_x", 0),
rospy.get_param("/calibrate_d_world_y", 0)
]
calib_d_img = [
rospy.get_param("/calibrate_d_image_x", 0) / down_scale_factor,
rospy.get_param("/calibrate_d_image_y", 0) / down_scale_factor
]
# static parameters
scaling_parameter = 1000 # only for (debug) plotting purposes
# Calibration of setup, so that the coordinates correspond to real world coordinates.
calib_pts_image = [calib_a_img, calib_b_img, calib_c_img, calib_d_img]
calib_pts_world = [calib_a_wld, calib_b_wld, calib_c_wld, calib_d_wld]
self.perspective_corrector = PerspectiveCorrecter(
calib_pts_image, calib_pts_world)
if self.print_debug_messages:
print 'Calibration points image:', calib_pts_image
print 'Calibration points world:', calib_pts_world
# instantiate camera driver
self.cd = CameraDriver(self.marker_order, marker_size,
scaling_parameter)
if self.show_image:
self.cd.open_image_window()
# instantiate markerpose publisher
self.markerpose_msg = markerpose()
self.markerpose_pub = rospy.Publisher(markerpose_pub_topic,
markerpose,
queue_size=0)
# instantiate image subscribers
self.time_prev_image = time()
rospy.Subscriber(markerimage_sub_topic, Image, self.on_new_image)
# call updater routine
self.updater()
#!/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)
