def reverse(self, pt):
'''Takes a point in gmap meters and converts it to image coordinates'''
lat, lon, alt, roll, pitch, yaw = self.params # camera parameters (location, orientation, focal length)
width = self.width # image width
height = self.height # image height
Fx = self.Fx
Fy = self.Fy
# convert input pt from meters to lat lon
ptlon, ptlat = metersToLatLon([pt[0], pt[1]])
ptalt = 0
px, py, pz = transformLonLatAltToEcef([ptlon, ptlat, ptalt])
pt = numpy.array([[px, py, pz,
1]]).transpose() # convert to column vector
cameraMatrix = numpy.matrix(
[
[Fx, 0, width / 2.0], # matrix of intrinsic camera parameters
[0, Fy, height / 2.0],
[0, 0, 1]
],
dtype='float64')
x, y, z = transformLonLatAltToEcef(
(lon, lat, alt)) # camera pose in ecef
rotation = rotFromEul(roll, pitch, yaw) # euler to matrix
rotation = numpy.transpose(rotation)
cameraPoseColVector = numpy.array([[x, y, z]]).transpose()
translation = -1 * rotation * cameraPoseColVector
rotTransMat = numpy.c_[
rotation,
translation] # append the translation matrix (3x1) to rotation matrix (3x3) -> becomes 3x4
ptInImage = cameraMatrix * rotTransMat * pt
u = ptInImage.item(0) / ptInImage.item(2)
v = ptInImage.item(1) / ptInImage.item(2)
ptInImage = [u, v]
return ptInImage
def testTransformClass():
lat, lon, alt, Fx, Fy, width, height = getInitialData()
camLonLatAlt = (lon, lat, alt)
rotMatrix = rotMatrixOfCameraInEcef(lon, transformLonLatAltToEcef(camLonLatAlt))
print "rotation Matrix is"
print rotMatrix
print "euler angle is"
r,p,y = eulFromRot(rotMatrix)
print [r,p,y]
print "back to rotation Matrix"
print rotFromEul(r,p,y)
"""
Forward
"""
# pt = [width / 2.0, height / 2.0] # if image coord is at center of image
# fwdRetval = forward(pt)
# print "ecef should be at 0, 0, some radius"
# print fwdRetval
#
# print "back to image coordinates"
# print reverse(fwdRetval)
#
# print"back to ecef"
# print forward(reverse(fwdRetval))
#
"""
def getInitParams(cls, toPts, fromPts, imageId):
mission, roll, frame = imageId.split('-')
issImage = ISSimage(mission, roll, frame, '')
try:
issLat = issImage.extras.nadirLat
issLon = issImage.extras.nadirLon
issAlt = issImage.extras.altitude
foLenX = issImage.extras.focalLength[0]
foLenY = issImage.extras.focalLength[1]
camLonLatAlt = (issLon, issLat, issAlt)
rotMatrix = rotMatrixOfCameraInEcef(
issLon, transformLonLatAltToEcef(
camLonLatAlt)) # initially nadir pointing
roll, pitch, yaw = eulFromRot(
rotMatrix) # initially set to nadir rotation
# these values are not going to be optimized. But needs to be passed to fromParams
# to set it as member vars.
width = issImage.extras.width
height = issImage.extras.height
except Exception as e:
print "Could not retrieve image metadata from the ISS MRF: " + str(
e)
return [
issLat, issLon, issAlt, roll, pitch, yaw, foLenX, foLenY, width,
height
]
def getCenterPoint(issImage):
"""
Center point is only available if the image has mission, roll, and frame.
"""
if issImage:
lat = issImage.extras.centerLat
lon = issImage.extras.centerLon
alt = issImage.extras.altitude
focalLength = issImage.extras.focalLength
sensorSize = issImage.extras.sensorSize
if (lat is None) or (lon is None):
nadirLat = issImage.extras.nadirLat
nadirLon = issImage.extras.nadirLon
nadirLonLatAlt = (nadirLon, nadirLat, alt)
centerCoords = [issImage.width / 2.0, issImage.height / 2.0]
opticalCenter = (int(issImage.width / 2.0),
int(issImage.height / 2.0))
rotMatrix = rotMatrixOfCameraInEcef(
nadirLon, transformLonLatAltToEcef(nadirLonLatAlt))
centerPointEcef = imageCoordToEcef(nadirLonLatAlt, centerCoords,
opticalCenter, focalLength,
rotMatrix)
lon, lat, alt = transformEcefToLonLatAlt(centerPointEcef)
else:
print "Error: image metadata is not available from the given ISS image ID"
return None
return {"lon": lon, "lat": lat, "alt": alt}
def imageCoordToEcef(cameraLonLatAlt, pixelCoord, opticalCenter, focalLength,
rotationMatrix):
"""
Given the camera position in ecef and image coordinates x,y
returns the coordinates in ecef frame (x,y,z)
rotationMatrix: from camera frame to ecef frame.
"""
cameraPoseEcef = transformLonLatAltToEcef(cameraLonLatAlt)
cameraPose = Point3(
cameraPoseEcef[0], cameraPoseEcef[1],
cameraPoseEcef[2]) # ray start is camera position in world coordinates
dirVector = pixelToVector(
opticalCenter, focalLength,
pixelCoord) # vector from center of projection to pixel on image.
dirVector_np = np.array([[dirVector.dx], [dirVector.dy], [dirVector.dz]])
dirVecEcef_np = rotationMatrix * dirVector_np
dirVectorEcef = Vector3(dirVecEcef_np[0], dirVecEcef_np[1],
dirVecEcef_np[2])
dirVectorEcef = dirVectorEcef.norm() # normalize the direction vector
ray = Ray3(cameraPose, dirVectorEcef) # construct the ray
earthCenter = Point3(0, 0, 0) # since we use ecef, earth center is 0 0 0
earth = Sphere(earthCenter, EARTH_RADIUS_METERS)
t = earth.intersect(ray) # intersect it with Earth
if t != float("inf"):
ecefCoords = ray.start + t * ray.dir
return pointToTuple(ecefCoords)
else:
return None
def reverse(pt):
"""
Takes a point in gmap meters and converts it to image coordinates
"""
lat, lon, alt, Fx, Fy, width, height = getInitialData()
camLonLatAlt = (lon,lat,alt)
rotMatrix = rotMatrixOfCameraInEcef(lon, transformLonLatAltToEcef(camLonLatAlt))
roll, pitch, yaw = eulFromRot(rotMatrix)
# #convert point to lat lon, and then to ecef
# ptlon, ptlat = transform.metersToLatLon([pt[0], pt[1]])
# ptalt = 0
# # convert lon lat alt to ecef
# px, py, pz = transformLonLatAltToEcef([ptlon, ptlat, ptalt])
# print ("0,0 lat lon to meters")
# print transform.lonLatToMeters([0,0])
# print (px, py, pz)
px, py, pz = (pt[0],pt[1], pt[2])
pt = numpy.array([[px, py, pz, 1]]).transpose()
cameraMatrix = numpy.matrix([[Fx, 0, width / 2.0], # matrix of intrinsic camera parameters
[0, Fy, height / 2.0],
[0, 0, 1]],
dtype='float64')
x,y,z = transformLonLatAltToEcef((lon,lat,alt)) # camera pose in ecef
# euler to matrix
rotation = rotFromEul(roll, pitch, yaw)
rotation = numpy.transpose(rotation) # can I do this?
# rotation = rotMatrixFromEcefToCamera(lon, [x,y,z]) # world to camera
cameraPoseColVector = numpy.array([[x, y, z]]).transpose()
translation = -1* rotation * cameraPoseColVector
# append the translation matrix (3x1) to rotation matrix (3x3) -> becomes 3x4
rotTransMat = numpy.c_[rotation, translation]
ptInImage = cameraMatrix * rotTransMat * pt
u = ptInImage.item(0) / ptInImage.item(2)
v = ptInImage.item(1) / ptInImage.item(2)
ptInImage = [u, v]
return ptInImage
def forward(pt):
"""
Takes in a point in pixel coordinate and returns point in gmap units (meters)
"""
lat, lon, alt, Fx, Fy, width, height = getInitialData()
camLonLatAlt = (lon,lat,alt)
rotMatrix = rotMatrixOfCameraInEcef(lon, transformLonLatAltToEcef(camLonLatAlt))
print "rotMatrix"
print rotMatrix
roll, pitch, yaw = eulFromRot(rotMatrix)
print 'roll pitch yaw'
print (roll * (180/numpy.pi), pitch * (180/numpy.pi), yaw * (180/numpy.pi))
rotMatrix = rotFromEul(roll, pitch, yaw)
print "rot matrix back"
print rotMatrix
opticalCenter = (int(width / 2.0), int(height / 2.0))
focalLength = (Fx, Fy)
# convert image pixel coordinates to ecef
ecef = imageCoordToEcef(camLonLatAlt, pt, opticalCenter, focalLength, rotMatrix)
return ecef