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
from 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 = cv.CreateMat(1, 1, cv.CV_64FC2)
src[0,0] = (xpos, ypos)
dst = cv.CreateMat(1, 1, cv.CV_64FC2)
R = cv.fromarray(eye(3))
K = cv.fromarray(C.K)
D = cv.fromarray(C.D)
cv.UndistortPoints(src, dst, 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])
files = loadtxt(inputfile, dtype='string', usecols=(0,))
#parser = OptionParser("showjoe.py [options]")
#(opts, args) = parser.parse_args()
f_m = 4 # 4mm
#s_m = (4./5.)*25.4/3 #1/3" sensor width in mm
s_m = 5.05
s_p = 1280
f_p = s_p * f_m / s_m
par = 1.0
f_u = f_p*par
f_v = f_p/par
xfer = uavxfer()
xfer.setCameraParams(f_u, f_v, 640, 480)
xfer.setCameraOrientation( -0.1, 0.1, pi/2 )
height0 = 0.0
xfer.setFlatEarth(-height0);
g = geodetic()
lat0 = data[0,2]
lon0 = data[0,3]
(zone, band) = g.computeZoneAndBand(lat0, lon0)
(north0, east0) = g.geoToGrid(lat0, lon0, zone, band)
print 'zone/band:',zone,band
f = pyplot.figure(1)
f.clf()
files = loadtxt(inputfile, dtype='string', usecols=(0, ))
#parser = OptionParser("showjoe.py [options]")
#(opts, args) = parser.parse_args()
f_m = 4 # 4mm
#s_m = (4./5.)*25.4/3 #1/3" sensor width in mm
s_m = 5.05
s_p = 1280
f_p = s_p * f_m / s_m
par = 1.0
f_u = f_p * par
f_v = f_p / par
xfer = uavxfer()
xfer.setCameraParams(f_u, f_v, 640, 480)
xfer.setCameraOrientation(-0.1, 0.1, pi / 2)
height0 = 0.0
xfer.setFlatEarth(-height0)
g = geodetic()
lat0 = data[0, 2]
lon0 = data[0, 3]
(zone, band) = g.computeZoneAndBand(lat0, lon0)
(north0, east0) = g.geoToGrid(lat0, lon0, zone, band)
print 'zone/band:', zone, band
f = pyplot.figure(1)
f.clf()
