def pixel_position_matt(xpos, ypos, height, pitch, roll, yaw, C):
'''
find the offset on the ground in meters of a pixel in a ground image
given height above the ground in meters, and pitch/roll/yaw in degrees, the
lens and image parameters
The xpos,ypos is from the top-left of the image
The height is in meters
The yaw is from grid north. Positive yaw is clockwise
The roll is from horiznotal. Positive roll is down on the right
The pitch is from horiznotal. Positive pitch is up in the front
return result is a tuple, with meters east and north of current GPS position
'''
from numpy import array, eye, zeros, uint64
from cuav.uav.uav import uavxfer
from math import pi
xfer = uavxfer()
xfer.setCameraMatrix(C.K)
xfer.setCameraOrientation(0.0, 0.0, pi / 2)
xfer.setFlatEarth(0)
xfer.setPlatformPose(0, 0, -height, math.radians(roll),
math.radians(pitch), math.radians(yaw))
# compute the undistorted points for the ideal camera matrix
#src = numpy.zeros((1,1,2), numpy.uint64) #cv.CreateMat(1, 1, cv.CV_64FC2)
src = numpy.zeros((1, 1, 2), numpy.float32)
src[0, 0] = (xpos, ypos)
#dst = cv.CreateMat(1, 1, cv.CV_64FC2)
R = eye(3)
K = C.K
D = C.D
dst = cv2.undistortPoints(src, K, D, R, K)
x = dst[0, 0][0]
y = dst[0, 0][1]
#print '(', xpos,',', ypos,') -> (', x, ',', y, ')'
# negative scale means camera pointing above horizon
# large scale means a long way away also unreliable
(joe_w, scale) = xfer.imageToWorld(x, y)
if (scale < 0 or scale > 500):
return None
#(te, tn) = pixel_position_tridge(xpos, ypos, height, pitch, roll, yaw, lens, sensorwidth, xresolution, yresolution)
#diff = (te-joe_w[1], tn-joe_w[0])
#print 'diff: ', diff
# east and north
return (joe_w[1], joe_w[0])
def pixel_position_matt(xpos, ypos, height, pitch, roll, yaw, C):
'''
find the offset on the ground in meters of a pixel in a ground image
given height above the ground in meters, and pitch/roll/yaw in degrees, the
lens and image parameters
The xpos,ypos is from the top-left of the image
The height is in meters
The yaw is from grid north. Positive yaw is clockwise
The roll is from horiznotal. Positive roll is down on the right
The pitch is from horiznotal. Positive pitch is up in the front
return result is a tuple, with meters east and north of current GPS position
'''
from numpy import array,eye, zeros, uint64
from cuav.uav.uav import uavxfer
from math import pi
xfer = uavxfer()
xfer.setCameraMatrix(C.K)
xfer.setCameraOrientation( 0.0, 0.0, pi/2 )
xfer.setFlatEarth(0);
xfer.setPlatformPose(0, 0, -height, math.radians(roll), math.radians(pitch), math.radians(yaw))
# compute the undistorted points for the ideal camera matrix
#src = numpy.zeros((1,1,2), numpy.uint64) #cv.CreateMat(1, 1, cv.CV_64FC2)
src = numpy.zeros((1,1, 2), numpy.float32)
src[0,0] = (xpos, ypos)
#dst = cv.CreateMat(1, 1, cv.CV_64FC2)
R = eye(3)
K = C.K
D = C.D
dst = cv2.undistortPoints(src, K, D, R, K)
x = dst[0,0][0]
y = dst[0,0][1]
#print '(', xpos,',', ypos,') -> (', x, ',', y, ')'
# negative scale means camera pointing above horizon
# large scale means a long way away also unreliable
(joe_w, scale) = xfer.imageToWorld(x, y)
if (scale < 0 or scale > 500):
return None
#(te, tn) = pixel_position_tridge(xpos, ypos, height, pitch, roll, yaw, lens, sensorwidth, xresolution, yresolution)
#diff = (te-joe_w[1], tn-joe_w[0])
#print 'diff: ', diff
# east and north
return (joe_w[1], joe_w[0])
def test_uavxfer():
xfer = uavxfer()
xfer.setCameraParams(200.0, 200.0, 512, 480)
xfer.setCameraOrientation(0.0, 0.0, -pi/2)
xfer.setPlatformPose(500.0, 1000.0, -700.0, 0.1, -0.1, 0.1)
p_w = array([500. +00., 1000. -00., -600.0])
p_p = xfer.worldToPlatform(p_w[0], p_w[1], p_w[2])
p_i = xfer.worldToImage(p_w[0], p_w[1], p_w[2])
(l_w, scale) = xfer.imageToWorld(p_i[0], p_i[1])
assert abs(500 - l_w[0]) < 0.01
assert abs(1000 - l_w[1]) < 0.01
assert abs(-600 - l_w[2]) < 0.01
assert abs(1 - l_w[3]) < 0.01
assert abs(99 - scale) < 0.01