def plot_data(in_data: Classes.InData, out_data: Classes.OutData):
pos_ned = in_data.gnss
x_h = out_data.x_h
[lat_ref, lon_ref] = [[el[0] for el in in_data.gnssref],
[el[1] for el in in_data.gnssref]]
[lat_ref,
lon_ref] = [sum(lat_ref) / len(lat_ref),
sum(lon_ref) / len(lon_ref)]
x_ned = nv.ned2lla(
[[el[0][0] for el in x_h], [el[1][0]
for el in x_h], [el[2][0] for el in x_h]],
lat_ref, lon_ref, 0) # convert ned to lat lon for output to html
x_ned_list = []
for i in range(len(x_ned[0])):
x_ned_list.append([x_ned[2][i], x_ned[0][i], x_ned[1][i]
]) # append the lat long for the output format
with open('../Output/output.csv', 'wt',
newline='') as file: # write the data as lat long to output file
filewc = csv.writer(file, dialect='excel')
filewc.writerows(x_ned_list)
plt.figure(1)
aided = plt.plot(
[el[1] for el in x_h], [el[0] for el in x_h],
'r-.',
label='GNSS aided INS trajectory') # plot the GPS aided INS trajectory
x_ned_1 = nv.ned2lla(
[[el[0][0] for el in x_h], [el[1][0]
for el in x_h], [el[2][0] for el in x_h]],
lat_ref, lon_ref, 0)
x_ned_list_limited = []
for i in range(len(x_ned_1[0])):
x_ned_list_limited.append(
[x_ned_1[2][i], x_ned_1[0][i], x_ned_1[1][i]])
# --------for file with GPS signal------------------------------------
# for files with no GPS signals comment this section
GPS = plt.plot([el[1] for el in pos_ned], [el[0] for el in pos_ned],
'b--',
label='GNSS position estimate') # plot the GPS signal
start = plt.plot(pos_ned[0][1],
pos_ned[0][0],
'ks',
linewidth=4.0,
label='Start point') # plot the start point
# ------------------------------------------
plt.title('Trajectory')
plt.ylabel('North [m]')
plt.xlabel('East [m]')
plt.legend()
plt.grid(True)
plt.axis('equal')
plt.show()
def get_corners(self, lon_lat_h, C_n_v):
"""
Returns the corners of the image in WGS Coordinates and NED
"""
# Find point on terrain directly below lon_lat_h
ref_lla = self.terrain_handler.add_heights(lon_lat_h[:2].reshape(1, 2))
ref_lla = ref_lla.flatten()
c_w_0 = navpy.lla2ned(lon_lat_h[1], lon_lat_h[0], lon_lat_h[2],
ref_lla[1], ref_lla[0], ref_lla[2])
R_w_c = np.dot(C_n_v, np.dot(self.C_b_v.T, self.C_b_cam))
n = np.array([0.0, 0.0, 1.0])
K = self.K
Ki = np.linalg.inv(K)
corners_pix = np.hstack((self.corners_pix, np.ones((4, 1)))).T
cvec = np.dot(Ki, corners_pix)
pclos = cvec / np.linalg.norm(cvec, axis=0)
pwlos = np.dot(R_w_c, pclos)
dd = (np.dot(n, c_w_0) / np.dot(n, pwlos))
corners_local = ((-1 * dd * pwlos) + c_w_0.reshape(3, 1)).T
# Return these for KML Style (clockwise, starting with Lower Left)
corners_local = corners_local[[1, 2, 3, 0], :]
out_pix = (corners_pix.T)[[1, 2, 3, 0], :]
c_wgs = np.vstack(navpy.ned2lla(corners_local,
ref_lla[1], ref_lla[0], ref_lla[2])).T
return corners_local, c_wgs[:, [1, 0, 2]], out_pix
def features_from_slice(ophoto,
slc,
detector,
desc_extract,
terrain_handler,
zoom=15,
bias=None):
"""
Takes in an orthophoto, slice tuple, detector, and extractor
"""
sub_photo = ophoto.get_img_from_slice(slc)
kp1, desc1 = f2d.extract_features(sub_photo.img, detector, desc_extract)
if len(kp1) > 0:
pix = f2d.keypoint_pixels(kp1)
meta = np.array([(kp.angle, kp.response, kp.size) for kp in kp1])
packed = f2d.numpySIFTScale(kp1)
pts_wgs84 = sub_photo.pix_to_wgs84(pix)
pts_hae = terrain_handler.add_heights(pts_wgs84)
if bias is not None:
print(bias)
ref = pts_hae[0, [1, 0, 2]]
pts_ned = navpy.lla2ned(pts_hae[:, 1], pts_hae[:, 0],
pts_hae[:, 2], *ref)
pts_ned -= bias
pts_hae = np.vstack(navpy.ned2lla(pts_ned, *ref)).T
pts_hae = pts_hae[:, [1, 0, 2]]
tile_coords = np.array(
[np.array(mercantile.tile(pt[0], pt[1], zoom)) for pt in pts_hae])
return (np.hstack((tile_coords, pts_hae, packed, meta)), desc1)
else:
return (None, None)
def update_rover_virtual_PosVelEcef(self, dt):
#calculate virtual position in ecef frame with noise
self.rover_ned_noise = self.add_gps_noise(self.white_noise_3d,
self.rover_ned_noise,
self.sigma_rover_pos)
rover_ned_w_noise = self.rover_ned + self.rover_ned_noise
self.rover_virtual_lla = navpy.ned2lla(rover_ned_w_noise,
self.ref_lla[0],
self.ref_lla[1],
self.ref_lla[2])
self.rover_virtual_pos_ecef = navpy.lla2ecef(self.rover_virtual_lla[0],
self.rover_virtual_lla[1],
self.rover_virtual_lla[2])
#calculate virtual velocity in ecef frame with noise
#make sure we do not divide by zero
if dt != 0.0:
rover_vel = (self.rover_ned - self.rover_ned_prev) / dt
else:
rover_vel = np.zeros(3)
self.rover_vel_noise = self.add_noise_3d(np.zeros(3),
self.white_noise_3d)
self.rover_vel_lpf = self.lpf(self.rover_vel_noise,
self.rover_vel_noise_prev, self.Ts,
self.sigma_rover_vel)
rover_vel_w_noise = rover_vel + self.rover_vel_lpf
self.rover_virtual_vel_ecef = navpy.ned2ecef(
rover_vel_w_noise, self.ref_lla[0], self.ref_lla[1],
self.ref_lla[2]) #self.ned2ecef(rover_vel_w_noise, self.ref_lla)
#update histories
self.rover_ned_prev = self.rover_ned
self.rover_vel_prev = rover_vel
self.rover_vel_noise_prev = self.rover_vel_noise
def N_to_NED(data, static_x, lat, long, h):
cn.it += 1
static_NED = pd.DataFrame(index=data.index,
columns=['x', 'y', 'z', 'lat', 'long', 'alt'])
for i in range(len(static_x)):
if cn.it != 4:
ned = [
float(static_x[i][0]),
float(static_x[i][1]),
float(static_x[i][2])
]
else:
ned = [
float(static_x.iloc[i, 0]),
float(static_x.iloc[i, 1]),
float(static_x.iloc[i, 2])
]
static_NED.iloc[i, 0] = ned[0]
static_NED.iloc[i, 1] = ned[1]
static_NED.iloc[i, 2] = ned[2]
coord = navpy.ned2lla(ned,
lat * 180 / m.pi,
long * 180 / m.pi,
h,
latlon_unit='deg',
alt_unit='m',
model='wgs84')
static_NED.iloc[i, 3] = coord[0]
static_NED.iloc[i, 4] = coord[1]
static_NED.iloc[i, 5] = coord[2]
return static_NED
def load(path, extern_ref):
result = []
# load matches
matches = pickle.load(open(path, "rb"))
print("loaded features:", len(matches))
# load project file to get ned reference for match coordinates
dir = os.path.dirname(path)
project_file = dir + "/Project.json"
try:
f = open(project_file, 'r')
project_dict = json.load(f)
f.close()
ref = project_dict['ned-reference-lla']
print('features ned reference:', ref)
except:
print("error: cannot load", project_file)
# convert match coords to lla, then back to external ned reference
# so the calling layer can have the points in the calling
# reference coordinate system.
print("converting feature coordinates to movie ned coordinate frame")
for m in matches:
zombie_door = random.randint(0, 49)
if zombie_door == 0:
ned = m[0]
if ned != None:
lla = navpy.ned2lla(ned, ref[0], ref[1], ref[2])
newned = navpy.lla2ned(lla[0], lla[1], lla[2], extern_ref[0],
extern_ref[1], extern_ref[2])
result.append(newned)
return result
def find_center_point(self, lon_lat_h, C_n_v):
"""
Get the world points of the 4 corners of the image and return them
:param np.ndarray lon_lat_h: Lon, Lat, Height in (3,) in degrees
:param np.ndarray att: (3,3) of DCM representing C_n_v
"""
# Find point on terrain directly below lon_lat_h
ref_lla = self.terrain_handler.add_heights(lon_lat_h[:2].reshape(1, 2))
ref_lla = ref_lla.flatten()
c_w_0 = navpy.lla2ned(lon_lat_h[1], lon_lat_h[0], lon_lat_h[2],
ref_lla[1], ref_lla[0], ref_lla[2])
R_w_c = np.dot(C_n_v, np.dot(self.C_b_v.T, self.C_b_cam))
n = np.array([0.0, 0.0, 1.0])
K = self.K
Ki = np.linalg.inv(K)
center_pix = np.array([self.cam_width / 2.0,
self.cam_height / 2.0,
1.0])
cvec = np.dot(Ki, center_pix)
pclos = cvec / np.linalg.norm(cvec, axis=0)
pwlos = np.dot(R_w_c, pclos)
dd = (np.dot(n, c_w_0) / np.dot(n, pwlos))
center_local = ((-1 * dd * pwlos) + c_w_0)
# Return these for KML Style (clockwise, starting with Lower Left)
c_wgs = navpy.ned2lla(center_local, ref_lla[1], ref_lla[0], ref_lla[2])
c_wgs = np.array(c_wgs)
return c_wgs[[1, 0, 2]]
def __init__(self, Var1, Var2, Var3, type, referencePoint):
Var1 = float(Var1)
Var2 = float(Var2)
Var3 = float(Var3)
if type == 1:
self.lat = Var1
self.long = Var2
self.alt = Var3
nedArray = lla2ned(Var1, Var2, Var3, referencePoint[0],
referencePoint[1], referencePoint[2])
self.north = nedArray[0]
self.east = nedArray[1]
self.down = nedArray[2]
elif type == 2:
self.north = Var1
self.east = Var2
self.down = Var3
llaArray = ned2lla(numpy.array([Var1, Var2,
Var3]), referencePoint[0],
referencePoint[1], referencePoint[2])
self.lat = llaArray[0]
self.long = llaArray[1]
self.alt = llaArray[2]
def load_tiles(self, lla_ref, width_m, height_m):
print "Notice: loading DEM tiles"
ll_ned = np.array([[-height_m*0.5, -width_m*0.5, 0.0]])
ur_ned = np.array([[height_m*0.5, width_m*0.5, 0.0]])
ll_lla = navpy.ned2lla(ll_ned, lla_ref[0], lla_ref[1], lla_ref[2])
ur_lla = navpy.ned2lla(ur_ned, lla_ref[0], lla_ref[1], lla_ref[2])
#print ll_lla
#print ur_lla
lat1, lon1 = lla_ll_corner( ll_lla[0], ll_lla[1] )
lat2, lon2 = lla_ll_corner( ur_lla[0], ur_lla[1] )
for lat in range(lat1, lat2+1):
for lon in range(lon1, lon2+1):
srtm = SRTM(lat, lon, '../srtm')
if srtm.parse():
srtm.make_lla_interpolator()
#srtm.plot_raw()
tile_name = make_tile_name(lat, lon)
self.tile_dict[tile_name] = srtm
def load_tiles(self, lla_ref, width_m, height_m):
print("Notice: loading DEM tiles")
ll_ned = np.array([[-height_m * 0.5, -width_m * 0.5, 0.0]])
ur_ned = np.array([[height_m * 0.5, width_m * 0.5, 0.0]])
ll_lla = navpy.ned2lla(ll_ned, lla_ref[0], lla_ref[1], lla_ref[2])
ur_lla = navpy.ned2lla(ur_ned, lla_ref[0], lla_ref[1], lla_ref[2])
#print ll_lla
#print ur_lla
lat1, lon1 = lla_ll_corner(ll_lla[0], ll_lla[1])
lat2, lon2 = lla_ll_corner(ur_lla[0], ur_lla[1])
for lat in range(lat1, lat2 + 1):
for lon in range(lon1, lon2 + 1):
srtm = SRTM(lat, lon, '../srtm')
if srtm.parse():
srtm.make_lla_interpolator()
#srtm.plot_raw()
tile_name = make_tile_name(lat, lon)
self.tile_dict[tile_name] = srtm
def features_from_slice(self,
slc,
bias=None,
convert_to_gray=True,
color_code=cv2.COLOR_RGBA2GRAY):
"""
Takes in an orthophoto, slice tuple, detector, and extractor
"""
sub_photo = self._ophoto.get_img_from_slice(slc)
if (sub_photo.img.ndim > 2) and convert_to_gray:
sub_img = cv2.cvtColor(sub_photo.img, color_code)
elif (sub_photo.img.ndim > 2):
raise TypeError(
"Underlying image is greater than 2 dimensions and convert_to_gray is set to false"
)
else:
sub_img = sub_photo.img
kp, desc = f2d.extract_features(sub_img, self._detector,
self._desc_extractor)
if len(kp) > 0:
pix = f2d.keypoint_pixels(kp)
meta = np.array([(pt.angle, pt.response, pt.size) for pt in kp])
packed = f2d.numpySIFTScale(kp)
img_df = pd.DataFrame(np.hstack((pix, meta, packed)),
columns=[
'pix_x', 'pix_y', 'angle', 'response',
'size', 'octave', 'layer', 'scale'
])
pts_lon_lat = sub_photo.pix_to_wgs84(pix)
pts_wgs = self._terrain_handler.add_heights(pts_lon_lat)
if bias is not None:
print(bias)
ref = pts_wgs[0, [1, 0, 2]]
pts_ned = navpy.lla2ned(pts_wgs[:, 1], pts_wgs[:, 0],
pts_wgs[:, 2], *ref)
pts_ned -= bias
pts_wgs = np.vstack(navpy.ned2lla(pts_ned, *ref)).T
pts_wgs = pts_wgs[:, [1, 0, 2]]
df = pd.concat([
img_df,
pd.DataFrame(pts_wgs, columns=['lon', 'lat', 'height'])
],
axis=1)
# Sort by feature response
df.sort_values('response', ascending=False, inplace=True)
pp = np.copy(df.index)
df.index = np.arange(df.shape[0])
return df, desc[pp, :]
else:
return None, None
def compute_rover_gps(roverTruth, gps, latRef, lonRef, altRef, originEcef):
gps.lla = navpy.ned2lla(roverTruth.position, latRef, lonRef, altRef)
ecefPositionRelative = navpy.ned2ecef(roverTruth.position, latRef, lonRef,
altRef)
gps.positionEcef = ecefPositionRelative + originEcef
gps.velocityEcef = navpy.ned2ecef(roverTruth.velocity, latRef, lonRef,
altRef)
gps.fix = 3
async def run_mission(drone, mission_items, lla_ref, gz_sub):
end_point = [38, 0, 1]
# The ned values are slightly skewed because the lla reference is at the start location, not world origin
# lla_ref_off = navpy.lla2ned(0, 0, 0, lla_ref[0], lla_ref[1], lla_ref[2])
lla_ref_off = [0, 0, 0]
[lat, lon, alt] = lla_ref # Set default mission item to start location
print("-- Comencing mission")
while (1):
lsr_val = await gz_sub.get_LaserScanStamped()
# Get next wp and determine if done
print("-- Getting next wp")
[x, y, z, speed] = get_next_wp(lsr_val, lla_ref_off)
if (not np.isnan(x)):
[lat, lon, alt] = navpy.ned2lla([y, x, -z], lla_ref[0], lla_ref[1],
lla_ref[2])
await drone.action.set_maximum_speed(speed)
await drone.action.goto_location(lat, lon, alt, 90)
else:
own_x = lsr_val.scan.world_pose.position.x + lla_ref_off[1]
own_z = lsr_val.scan.world_pose.position.z - lla_ref_off[2]
print("-- Going to the end")
# print("-- Mission is done")
await drone.action.set_maximum_speed(1)
[lat, lon,
alt] = navpy.ned2lla([end_point[1], end_point[0], -end_point[2]],
lla_ref[0], lla_ref[1], lla_ref[2])
if (own_x >= end_point[0] - 5
and abs(own_z - end_point[2]) <= 0.5):
print("-- landing")
await drone.action.land()
break
else:
await drone.action.goto_location(lat, lon, alt, 90)
def calc_least_squares(data, H, rho, lat, long, h):
rover_pos = pd.DataFrame(index=data.index, columns=['x','y','z','lat','long','alt', 'p_inv'])
for i in range(848):
p_inv = la.inv(H[i].T @ H[i])
x_hat = p_inv @ H[i].T @ rho[i]
NED = [float(x_hat[0]), float(x_hat[1]), float(x_hat[2])]
rover_pos.iloc[i, 0] = float(x_hat[0])
rover_pos.iloc[i, 1] = float(x_hat[1])
rover_pos.iloc[i, 2] = float(x_hat[2])
coord = navpy.ned2lla(NED, lat * 180 / m.pi, long * 180 / m.pi, h, latlon_unit='deg', alt_unit='m',
model='wgs84')
rover_pos.iloc[i, 3] = coord[0]
rover_pos.iloc[i, 4] = coord[1]
rover_pos.iloc[i, 5] = coord[2]
rover_pos.iloc[i, 6] = p_inv
return rover_pos
def project_points(self, lon_lat_h, C_n_v, points, return_local=False):
"""
This is the key Function to project points from the image frame into
the Local Level (NED) frame, and then convert into WGS-84. The origin
of the local level (NED) frame is located as the (Lon, Lat) of the
aircraft, with the height being located at h = Dem(lon, lat) (eg,
terrain height). This function computes the points by solving for
the ray-local level plane intersection
:param lon_lat_h: [3,] Position of the camera in Lon, Lat (deg),
height (m)
:param C_n_v: [3x3] np.ndarray that translates a vector in the vehicle
frame into the local-level NED frame
:param points: [Nx2] [np.ndarray] of points to project
:return: Returns an [Nx3] np.ndarray of Lon, Lat, Height per point in
[points]
"""
if self.K is None:
raise ValueError("Camera Model Not Initialized")
if points.ndim == 1:
points = points.reshape(1, 2)
ref_lla = self.terrain_handler.add_heights(lon_lat_h[:2].reshape(1, 2))
ref_lla = ref_lla.flatten()
c_w_0 = navpy.lla2ned(lon_lat_h[1], lon_lat_h[0], lon_lat_h[2],
ref_lla[1], ref_lla[0], ref_lla[2])
R_w_c = np.dot(C_n_v, np.dot(self.C_b_v.T, self.C_b_cam))
n = np.array([0.0, 0.0, 1.0])
K = self.K
Ki = np.linalg.inv(K)
pix = np.hstack((points, np.ones((points.shape[0], 1)))).T
cvec = np.dot(Ki, pix)
pclos = cvec / np.linalg.norm(cvec, axis=0)
pwlos = np.dot(R_w_c, pclos)
dd = (np.dot(n, c_w_0) / np.dot(n, pwlos))
points_local = ((-1 * dd * pwlos) + c_w_0.reshape(3, 1)).T
c_wgs = np.vstack(
navpy.ned2lla(points_local, ref_lla[1], ref_lla[0], ref_lla[2])).T
if return_local:
return c_wgs[:, [1, 0, 2]], points_local
else:
return c_wgs[:, [1, 0, 2]]
def save(self):
filename = os.path.join(self.project_dir, 'annotations.json')
print('Saving annotations:', filename)
lla_list = []
for m in self.markers:
lla = navpy.ned2lla( [m[1], m[0], 0.0],
self.ned_ref[0],
self.ned_ref[1],
self.ned_ref[2] )
lla_list.append(lla)
f = open(filename, 'w')
json.dump(lla_list, f, indent=4)
f.close()
# write simple csv version
filename = os.path.join(self.project_dir, 'annotations.csv')
with open(filename, 'w') as f:
fieldnames = ['lat_deg', 'lon_deg']
writer = csv.DictWriter(f, fieldnames=fieldnames)
writer.writeheader()
for lla in lla_list:
writer.writerow({'lat_deg': lla[0], 'lon_deg': lla[1]})
def test_ned2lla(self):
"""
Test conversion from NED to LLA.
Data Source: derived from "test_lla2ned" above.
"""
# A point near Los Angeles, CA
lat_ref = +( 34. + 0./60 + 0.00174/3600) # North
lon_ref = -(117. + 20./60 + 0.84965/3600) # West
alt_ref = 251.702 # [meters]
# Point near by with known NED position
lat = +(34. + 0./60 + 0.19237/3600) # North
lon = -(117. +20./60 + 0.77188/3600) # West
alt = 234.052 # [meters]
ned = [5.8738, 1.9959, 17.6498]
# Do conversion and check result
# Note: default assumption on units is deg and meters
lla_computed = navpy.ned2lla(ned, lat_ref, lon_ref, alt_ref)
for e1, e2 in zip(lla_computed, [lat, lon, alt]):
self.assertAlmostEqual(e1, e2, places=3)
def test_ned2lla(self):
"""
Test conversion from NED to LLA.
Data Source: derived from "test_lla2ned" above.
"""
# A point near Los Angeles, CA
lat_ref = +(34. + 0. / 60 + 0.00174 / 3600) # North
lon_ref = -(117. + 20. / 60 + 0.84965 / 3600) # West
alt_ref = 251.702 # [meters]
# Point near by with known NED position
lat = +(34. + 0. / 60 + 0.19237 / 3600) # North
lon = -(117. + 20. / 60 + 0.77188 / 3600) # West
alt = 234.052 # [meters]
ned = [5.8738, 1.9959, 17.6498]
# Do conversion and check result
# Note: default assumption on units is deg and meters
lla_computed = navpy.ned2lla(ned, lat_ref, lon_ref, alt_ref)
for e1, e2 in zip(lla_computed, [lat, lon, alt]):
self.assertAlmostEqual(e1, e2, places=3)
def main():
# TEST
# u = 1
# v = 2
# uv = np.array([u, v, 1])
# y = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]])
# print(np.dot(y, uv))
# print(y)
A = getCameraIntrinsicMatrix()
# print(A)
#test = np.array([[4, 7], [2, 6]])
#print(np.linalg.inv(test))
# Get camera's GPS positions and height
lat, lon, alt = getCameraPosition()
zc = getCameraHeight()
# Transform camera position from Geodetic (lla) to NED
# A point near Los Angeles, CA, given in https://github.com/NavPy/NavPy/blob/master/navpy/core/tests/test_navpy.py
# (this will change to the location of the experiment later on)
lat_ref = +(34. + 0./60 + 0.00174/3600) # North
lon_ref = -(117. + 20./60 + 0.84965/3600) # West
alt_ref = 251.702 # in meters
rnnc = navpy.lla2ned(lat, lon, alt, lat_ref, lon_ref, alt_ref, latlon_unit='deg', alt_unit='m', model='wgs84')
#print(rnnc)
# TEST
#lat_ref = 34.004924
#lon_ref = -117.897842
#alt_ref = 129.9972
rnnc = np.array([0, 0, -zc])
#rnnc = np.array([0, 0, -1.3462])
# Get the body frame’s orientation angles and gimbal angles from the GPS
roll, pitch, yaw = getCameraOrientation()
tilt, pan = getGimbalAngles()
# print(roll, pitch, yaw, tilt, pan)
# Compute Rcn from roll, pitch, yaw, tilt, pan angles
Rnb = np.array([[m.cos(yaw)*m.cos(pitch), -m.sin(yaw)*m.cos(roll) + m.cos(yaw)*m.sin(roll)*m.sin(pitch), m.sin(yaw)*m.sin(roll) + m.cos(yaw)*m.cos(roll)*m.sin(pitch)],
[m.sin(yaw)*m.cos(pitch), m.cos(yaw)*m.cos(roll) + m.sin(roll)*m.sin(pitch)*m.sin(yaw), -m.cos(yaw)*m.sin(roll) + m.sin(pitch)*m.sin(yaw)*m.cos(roll)],
[-m.sin(pitch), m.cos(pitch)*m.sin(roll), m.cos(pitch)*m.cos(roll)]])
Rcm = np.array([[0, 1, 0],
[-1, 0, 0],
[0, 0, 1]])
Rmb = np.array([[m.cos(pan)*m.cos(tilt), m.sin(pan)*m.cos(tilt), -m.sin(tilt)],
[-m.sin(pan), m.cos(pan), 0],
[m.cos(pan)*m.sin(tilt), m.sin(pan)*m.sin(tilt), m.cos(tilt)]])
Rmb = np.array([[1, 0, 0],
[0, 1, 0],
[0, 0, 1]])
# Rmb is simply the identity matrix if camera is strapped directly to airframe
Rcn = np.linalg.inv(np.dot(Rnb, np.linalg.inv(np.dot(Rcm, Rmb))))
#print(Rcn)
# Compute G(NE) from Rcn and rnnc
Gne = Rcn.copy()
Gne[:, 2] = -np.dot(Rcn, rnnc)
#print(Gne)
# Obtain the pixel positions (u,v) of hot spots from fire detection process
image = Image.open("location1_fire.jpg")
pixels = image.load()
width = image.size[0]
height = image.size[1]
# print(width, height)
for i in range(0, height):
for j in range(0, width):
if (pixels[j, i] != 0):
uv = np.array([j, i, 1])
# Testing the pinhole camera model
#xyzc = zc * np.dot(np.linalg.inv(A), uv)
#print(m.sqrt(xyzc[0] * xyzc[0] + xyzc[1] * xyzc[1] + xyzc[2] * xyzc[2]))
# Compute the NE coordinates of the hot spots from G(NE)
NE = zc * np.dot(np.dot(np.linalg.inv(Gne), np.linalg.inv(A)), uv)
#print(m.sqrt(NE[0] * NE[0] + NE[1] * NE[1]))
# Converts hot spots' coordinates from NED to lla (GPS) coordinates
result = navpy.ned2lla([NE[0], NE[1], 0], lat_ref, lon_ref, alt_ref, latlon_unit='deg', alt_unit='m', model='wgs84')
print(result)
camw, camh = proj.cam.get_image_params()
for image in proj.image_list:
print image.name
scale = float(image.width) / float(camw)
K = proj.cam.get_K(scale)
IK = np.linalg.inv(K)
corner_list = []
corner_list.append([0, image.height])
corner_list.append([image.width, image.height])
corner_list.append([image.width, 0])
corner_list.append([0, 0])
proj_list = proj.projectVectors(IK, image, corner_list)
print "proj_list:\n", proj_list
# pts = proj.intersectVectorsWithGroundPlane(image.camera_pose,
# g, proj_list)
pts = sss.interpolate_vectors(image.camera_pose, proj_list)
# print "pts (ned):\n", pts
corners_lonlat = []
for ned in pts:
print ned
lla = navpy.ned2lla([ned], ref[0], ref[1], ref[2])
corners_lonlat.append([lla[1], lla[0]])
ground = kml.newgroundoverlay(name=image.name)
ground.icon.href = "Images/" + image.name
ground.gxlatlonquad.coords.addcoordinates(corners_lonlat)
filename = args.project + "/GroundOverlay.kml"
kml.save(filename)
proj_list = proj.projectVectors( IK, image, corner_list, pose=args.pose )
#print "proj_list:\n", proj_list
if args.pose == 'direct':
pts_ned = sss.interpolate_vectors(image.camera_pose, proj_list)
elif args.pose == 'sba':
pts_ned = sss.interpolate_vectors(image.camera_pose_sba, proj_list)
# print "pts (ned):\n", pts_ned
image.corner_list_ned = []
image.corner_list_lla = []
image.corner_list_xy = []
for ned in pts_ned:
#print p
image.corner_list_ned.append( [ned[0], ned[1]] )
image.corner_list_lla.append( navpy.ned2lla([ned], ref[0], ref[1], ref[2]) )
image.corner_list_xy.append( [ned[1], ned[0]] )
dst_dir = proj.project_dir + "/Warped/"
if not os.path.exists(dst_dir):
print "Notice: creating rubber sheeted texture directory =", dst_dir
os.makedirs(dst_dir)
for image in proj.image_list:
basename, ext = os.path.splitext(image.name)
dst = dst_dir + basename + ".png"
#if os.path.exists(dst):
# continue
# print image.name
scale = float(image.width) / float(camw)
def setup_pnp_problem(self, matches, idx, init_wgs, C_nav_v):
"""
If you've got 99 problems, give yourself one more
"""
K = self._PnP__camera_matrix
keypoints = matches.keypoints[idx, :]
img_pts = self.undistort_keypoints(keypoints)
# First we need to create a local level Cartesian frame (NED),
lon_lat_h = matches.world_coordinates[idx, :]
ref = lon_lat_h.mean(0)
ref[2] = ref[2] - 150.0
world_n = navpy.lla2ned(lon_lat_h[:, 1], lon_lat_h[:, 0],
lon_lat_h[:, 2], ref[1], ref[0], ref[2])
if self._PnP__db_bias is not None:
world_n = world_n - self._PnP__db_bias
t_n = navpy.lla2ned(init_wgs[0], init_wgs[1], init_wgs[2], ref[1],
ref[0], ref[2])
C_nav_b = np.dot(self.C_b_v, C_nav_v.T).T
R_c_w = np.dot(self.C_b_cam.T, C_nav_b.T)
X_hat = world_n.T
x_img = img_pts.T
#Setup Constrained PnP
metadata = {'K': K, 'R_c_w': R_c_w}
prob = Problem()
trans_state = StateBlock('translation', t_n.flatten())
world_pt_states = [
StateBlock('pt_%d' % ii, X_hat[:, ii].flatten())
for ii in np.arange(X_hat.shape[1])
]
prob.add_state_block(trans_state)
for state in world_pt_states:
prob.add_state_block(state)
resids = []
for ii in np.arange(keypoints.shape[0]):
x_i = x_img[:, ii].T
states_measured = ['translation', 'pt_%d' % ii]
resid = ResidualBlock(states_measured,
image_point_translation_worldpt_meas_func,
image_point_trans_worldpt_jacobian_func)
resid.set_meas(x_i, 4.0 * np.eye(2))
resid.update_metadata(metadata)
resids.append(resid)
prob.add_residual_block(resid)
# Add in Ground Control Meas....
for ii in np.arange(keypoints.shape[0]):
X_w = X_hat[:, ii].T
states_measured = ['pt_%d' % ii]
resid = ResidualBlock(states_measured, worldpt_meas_func,
worldpt_jac_func)
resid.set_meas(X_w, np.diag([25.0, 25.0, 25.0]))
resid.update_metadata(None)
resids.append(resid)
prob.add_residual_block(resid)
niter = prob.solve()
R = np.array([])
for r in resids:
R = np.append(R, np.diag(r.R))
R = np.diag(R)
try:
cov = np.linalg.pinv(
np.dot(prob.J.T, np.dot(np.linalg.inv(R), prob.J)))
pnp_n = prob.states['translation'].get_current_val()
cov_n = np.abs(np.diag(cov)[0:3])
pnp_wgs = np.array(navpy.ned2lla(pnp_n, ref[1], ref[0], ref[2]))
return pnp_wgs, cov_n, niter
except:
return (np.array([]), np.array([]), 0)
async def run_mission(drone, mission_items, lla_ref, gz_sub):
max_speed = 2 # m/s
done = False # Flag to signal when we're done with our mission
# The ned values are slightly skewed because the lla reference is at the start location, not world origin
# lla_ref_off = navpy.lla2ned(0, 0, 0, lla_ref[0], lla_ref[1], lla_ref[2])
lla_ref_off = [0, 0, 0]
[lat, lon, alt] = lla_ref # Set default mission item to start location
mission_item_idx = 0 # Keep track which mission item we're currently on
async for mission_progress in drone.mission.mission_progress():
if (not mission_progress.current == -1):
print(f"Mission progress: "
f"{mission_progress.current}/"
f"{mission_progress.total}")
# Processing one point at a time, when have reached the end of the current set of waypoints
if (mission_progress.current == mission_progress.total - 1
and not done):
print("-- Pause and clear mission")
# Pause mission while calculating
await drone.mission.pause_mission()
await drone.mission.clear_mission()
# Get current mission item index
mission_item_idx = mission_progress.current
print("-- Get Lidar Readings")
# Get Lidar readings
lsr_val = await gz_sub.get_LaserScanStamped()
print("-- Register readings")
registered_scans = get_world_obst(lsr_val, lla_ref_off)
print(registered_scans)
print("-- Get new lla")
####### Replace this code with your algorithm
if (mission_item_idx == 0):
[lat, lon, alt] = navpy.ned2lla([0, 0, -1], lla_ref[0],
lla_ref[1], lla_ref[2])
elif (mission_item_idx == 1):
[lat, lon, alt] = navpy.ned2lla([0, 10, -3], lla_ref[0],
lla_ref[1], lla_ref[2])
elif (mission_item_idx == 2):
[lat, lon, alt] = navpy.ned2lla([0, 15, -5.5], lla_ref[0],
lla_ref[1], lla_ref[2])
elif (mission_item_idx == 3):
[lat, lon, alt] = navpy.ned2lla([0, 30, -5.5], lla_ref[0],
lla_ref[1], lla_ref[2])
####### Replace this code with your algorithm
else:
# Make sure that you set done to True when your algorithm has reached the endpoint
# and the drone is ready to land
done = True
# Insert the mission item created into the overall mission
print("-- Making mission plan")
mission_items.insert(
mission_item_idx,
MissionItem(lat, lon, alt, max_speed, True, float('nan'),
float('nan'), MissionItem.CameraAction.NONE,
float('nan'), float('nan')))
mission_plan = MissionPlan(mission_items)
# Upload updated mission to the drone
print("-- Uploading updated mission")
await drone.mission.upload_mission(mission_plan)
# Wait for the upload to finish, otherwise it can't resume
await asyncio.sleep(1)
# Start the mission from the item that we just added
print("-- Resuming mission")
await drone.mission.set_current_mission_item(mission_item_idx)
await drone.mission.start_mission()
# Land the drone at the end location
if (mission_progress.current == mission_progress.total and done):
# Get Lidar readings
lsr_val = await gz_sub.get_LaserScanStamped()
registered_scans = get_world_obst(lsr_val, lla_ref_off)
print(registered_scans)
print("-- Returning Home and Landing")
await drone.action.land()
def make_interpolator(self, lla_ref, width_m, height_m, step_m):
print "Notice: constructing NED area interpolator"
rows = (height_m / step_m) + 1
cols = (width_m / step_m) + 1
#ned_pts = np.zeros((cols, rows))
#for r in range(0,rows):
# for c in range(0,cols):
# idx = (cols*r)+c
# #va = self.srtm_z[idx]
# #if va == 65535 or va < 0 or va > 10000:
# # va = 0.0
# #z = va
# ned_pts[c,r] = 0.0
# build regularly gridded x,y coordinate list and ned_pts array
n_list = np.linspace(-height_m*0.5, height_m*0.5, rows)
e_list = np.linspace(-width_m*0.5, width_m*0.5, cols)
#print "e's:", e_list
#print "n's:", n_list
ned_pts = []
for e in e_list:
for n in n_list:
ned_pts.append( [n, e, 0] )
# convert ned_pts list to lla coordinates (so it's not
# necessarily an exact grid anymore, but we can now
# interpolate elevations out of the lla interpolators for each
# tile.
navpy_pts = navpy.ned2lla(ned_pts, lla_ref[0], lla_ref[1], lla_ref[2])
#print navpy_pts
# build list of (lat, lon) points for doing actual lla
# elevation lookup
ll_pts = []
for i in range( len(navpy_pts[0]) ):
lat = navpy_pts[0][i]
lon = navpy_pts[1][i]
ll_pts.append( [ lon, lat ] )
#print "ll_pts:", ll_pts
# set all the elevations in the ned_ds list to the extreme
# minimum value. (rows,cols) might seem funny, but (ne)d is
# reversed from (xy)z ... don't think about it too much or
# you'll get a headache. :-)
ned_ds = np.zeros((rows,cols))
ned_ds[:][:] = -32768
#print "ned_ds:", ned_ds
# for each tile loaded, interpolate as many elevation values
# as we can, then copy the good values into ned_ds. When we
# finish all the loaded tiles, we should have elevations for
# the entire range of points.
for tile in self.tile_dict:
zs = self.tile_dict[tile].lla_interpolate(np.array(ll_pts))
#print zs
# copy the good altitudes back to the corresponding ned points
for r in range(0,rows):
for c in range(0,cols):
idx = (rows*c)+r
if zs[idx] > -10000:
ned_ds[r,c] = zs[idx]
# quick sanity check
for r in range(0,rows):
for c in range(0,cols):
idx = (rows*c)+r
if ned_ds[r,c] < -10000:
print "Problem interpolating elevation for:", ll_pts[idx]
ned_ds[r,c] = 0.0
#print "ned_ds:", ned_ds
# now finally build the actual grid interpolator with evenly
# spaced ned n, e values and elevations interpolated out of
# the srtm lla interpolator.
self.interp = scipy.interpolate.RegularGridInterpolator((n_list, e_list), ned_ds, bounds_error=False, fill_value=-32768)
do_plot = False
if do_plot:
imshow(ned_ds, interpolation='bilinear', origin='lower', cmap=cm.gray, alpha=1.0)
grid(False)
show()
do_test = False
if do_test:
for i in range(40):
ned = [(random.random()-0.5)*height_m,
(random.random()-0.5)*width_m,
0.0]
lla = navpy.ned2lla(ned, lla_ref[0], lla_ref[1], lla_ref[2])
#print "ned=%s, lla=%s" % (ned, lla)
nedz = self.interp([ned[0], ned[1]])
tile = make_tile_name(lla[0], lla[1])
llaz = self.tile_dict[tile].lla_interpolate(np.array([lla[1], lla[0]]))
print "nedz=%.2f llaz=%.2f" % (nedz, llaz)
def coverage_lla(self, ref):
xmin, ymin, xmax, ymax = self.coverage_xy()
minlla = navpy.ned2lla([ymin, xmin, 0.0], ref[0], ref[1], ref[2])
maxlla = navpy.ned2lla([ymax, xmax, 0.0], ref[0], ref[1], ref[2])
return (minlla[1], minlla[0], maxlla[1], maxlla[0])
bins[index].append(np.array(ned))
else:
bins[index] = [np.array(ned)]
for index in bins:
sum = np.zeros(3)
for p in bins[index]:
sum += p
avg = sum / len(bins[index])
print(index, len(bins[index]), avg)
# write out simple csv version
filename = os.path.join(project_path, "ImageAnalysis", "zooniverse.csv")
with open(filename, 'w') as f:
fieldnames = ['id', 'lat_deg', 'lon_deg', 'alt_m', 'comment']
writer = csv.DictWriter(f, fieldnames=fieldnames)
writer.writeheader()
for index in bins:
sum = np.zeros(3)
for p in bins[index]:
sum += p
avg = sum / len(bins[index])
lla = navpy.ned2lla([avg], ref[0], ref[1], ref[2])
tmp = {}
tmp['id'] = index
tmp['lat_deg'] = lla[0]
tmp['lon_deg'] = lla[1]
tmp['alt_m'] = lla[2]
tmp['comment'] = "zooniverse"
writer.writerow(tmp)
async def run_mission(drone, mission_items, lla_ref, gz_sub):
done = False # Flag to signal when we're done with our mission
# The ned values are slightly skewed because the lla reference is at the start location, not world origin
# lla_ref_off = navpy.lla2ned(0, 0, 0, lla_ref[0], lla_ref[1], lla_ref[2])
lla_ref_off = [0, 0, 0]
[lat, lon, alt] = lla_ref # Set default mission item to start location
mission_item_idx = 0 # Keep track which mission item we're currently on
async for mission_progress in drone.mission.mission_progress():
if (not mission_progress.current == -1):
print(f"Mission progress: "
f"{mission_progress.current}/"
f"{mission_progress.total}")
# Processing one point at a time, when have reached the end of the current set of waypoints
if (mission_progress.current == mission_progress.total - 1
and not done):
print("-- Pause and clear mission")
# Pause mission while calculating
await drone.mission.pause_mission()
await drone.mission.clear_mission()
# Get current mission item index
mission_item_idx = mission_progress.current
print("-- Get Lidar Readings")
# Get Lidar readings
lsr_val = await gz_sub.get_LaserScanStamped()
# Get next wp and determine if done
print("-- Getting next wp")
[x, y, z, speed] = get_next_wp(lsr_val, lla_ref_off)
if (not np.isnan(x)):
[lat, lon, alt] = navpy.ned2lla([y, x, -z], lla_ref[0],
lla_ref[1], lla_ref[2])
else:
print("-- Mission is done")
done = True
# Insert the mission item created into the overall mission
print("-- Making mission plan")
mission_items.insert(
mission_item_idx,
MissionItem(lat, lon, alt, speed, True, float('nan'),
float('nan'), MissionItem.CameraAction.NONE,
float('nan'), float('nan')))
mission_plan = MissionPlan(mission_items)
# Upload updated mission to the drone
print("-- Uploading updated mission")
await drone.mission.upload_mission(mission_plan)
# Wait for the upload to finish, otherwise it can't resume
await asyncio.sleep(1)
# Start the mission from the item that we just added
print("-- Resuming mission")
await drone.mission.set_current_mission_item(mission_item_idx)
await drone.mission.start_mission()
# Land the drone at the end location
if (mission_progress.current == mission_progress.total and done):
print("-- Landing")
await drone.action.land()
async def run():
# Some of the values are inf and numpy complains when doing math with them, so turn off warnings
np.seterr(all='ignore')
np.set_printoptions(suppress=True)
# Make the Gazebo subscriber object
gz_sub = GazeboMessageSubscriber(HOST, PORT)
# Actually do the subscription
gz_task = asyncio.ensure_future(gz_sub.connect())
# Get a test message - this will speed up all of the subsequent requests
await gz_sub.get_LaserScanStamped()
# Initialize drone and connect
drone = System()
await drone.connect(system_address="udp://:14540")
print("Waiting for drone to connect...")
async for state in drone.core.connection_state():
if state.is_connected:
print(f"Drone discovered with UUID: {state.uuid}")
break
# Make sure telemetry is good so that we know we can trust the position and orientation values
print("Waiting for drone to have a global position estimate...")
async for health in drone.telemetry.health():
if health.is_global_position_ok:
print("Global position estimate ok")
break
# Get our starting position and use that as reference for conversions between lla and ned
print("Fetching amsl altitude at home location....")
lla_ref = []
async for terrain_info in drone.telemetry.position():
lla_ref = [
terrain_info.latitude_deg, terrain_info.longitude_deg,
terrain_info.absolute_altitude_m
]
print(lla_ref)
break
# First mission item is to takeoff to just above our starting location
mission_items = []
mission_items.append(
MissionItem(lla_ref[0], lla_ref[1], lla_ref[2] + 1, 1, True,
float('nan'), float('nan'), MissionItem.CameraAction.NONE,
float('nan'), float('nan')))
# Last mission item is at the end of the terrain, just above the terrain
# All of your mission items go between the the start point above and the end point below
[lat, lon, alt] = navpy.ned2lla([0, 40, -1], lla_ref[0], lla_ref[1],
lla_ref[2])
mission_items.append(
MissionItem(lat, lon, alt, 1, True, float('nan'), float('nan'),
MissionItem.CameraAction.NONE, float('nan'), float('nan')))
mission_plan = MissionPlan(mission_items)
# Spool up the mission and mission monitor
mission_task = asyncio.ensure_future(
run_mission(drone, mission_items, lla_ref, gz_sub))
termination_task = asyncio.ensure_future(
observe_is_in_air(drone, [mission_task, gz_task]))
# Make the drone dangerous (arm it)
print("-- Arming")
await drone.action.arm()
await asyncio.sleep(1)
# Upload mission with our first item (more will be added in the mission)
print("-- Uploading mission")
await drone.mission.upload_mission(mission_plan)
await asyncio.sleep(1)
await drone.action.set_takeoff_altitude(lla_ref[2] + 1)
# Start running through the mission
# The mission task will watch the progression and add new points as we reach the old ones
# Once the end condition is met, the drone will land
print("-- Starting mission")
await drone.mission.start_mission()
await termination_task
def ned2lla(self, n, e, d):
lla = navpy.ned2lla([n, e, d], self.ned_ref[0], self.ned_ref[1],
self.ned_ref[2])
# print(n, e, d, lla)
return lla
def build(self, cam_pos, view_size):
alpha = 0.6
depth = 0
a1 = view_size / 20
a2 = view_size / 5
props = base.win.getProperties()
y = props.getYSize()
pxm = float(y) / view_size
text_scale = 42 / y
# print("y:", y, "view_size:", view_size, "pxm:", pxm)
# center reticle
ls = LineSegs()
ls.setThickness(1)
ls.setColor(0.0, 1.0, 0.0, alpha)
ls.moveTo(cam_pos[0] + a1, cam_pos[1], 0)
ls.drawTo(cam_pos[0] + a2, cam_pos[1], 0)
ls.moveTo(cam_pos[0] - a1, cam_pos[1], 0)
ls.drawTo(cam_pos[0] - a2, cam_pos[1], 0)
ls.moveTo(cam_pos[0], cam_pos[1] + a1, 0)
ls.drawTo(cam_pos[0], cam_pos[1] + a2, 0)
ls.moveTo(cam_pos[0], cam_pos[1] - a1, 0)
ls.drawTo(cam_pos[0], cam_pos[1] - a2, 0)
self.node = NodePath(ls.create())
self.node.setDepthTest(False)
self.node.setDepthWrite(False)
self.node.setBin("unsorted", depth)
self.node.reparentTo(self.render)
# measurement marker
h_size = view_size * base.getAspectRatio()
h = math.pow(2, int(round(math.log2(h_size / 10.0))))
# print("h_size:", h_size, h)
ls = LineSegs()
ls.setThickness(2)
ls.setColor(0.0, 1.0, 0.0, alpha)
ls.moveTo(cam_pos[0] - 0.48 * h_size, cam_pos[1] - 0.48 * view_size, 0)
ls.drawTo(cam_pos[0] - 0.48 * h_size + h,
cam_pos[1] - 0.48 * view_size, 0)
ls.moveTo(cam_pos[0] - 0.48 * h_size, cam_pos[1] - 0.48 * view_size, 0)
ls.drawTo(cam_pos[0] - 0.48 * h_size, cam_pos[1] - 0.46 * view_size, 0)
ls.moveTo(cam_pos[0] - 0.48 * h_size + h,
cam_pos[1] - 0.48 * view_size, 0)
ls.drawTo(cam_pos[0] - 0.48 * h_size + h,
cam_pos[1] - 0.46 * view_size, 0)
self.node1 = NodePath(ls.create())
self.node1.setDepthTest(False)
self.node1.setDepthWrite(False)
self.node1.setBin("unsorted", depth)
self.node1.reparentTo(self.render)
if h >= 1.0:
dist_text = "%.0f m" % (h)
elif h >= 0.1:
dist_text = "%.1f cm" % (h * 100)
else:
dist_text = "%.1f mm" % (h * 1000)
self.text2 = OnscreenText(text=dist_text,
pos=(-0.95 * base.getAspectRatio(), -0.94),
scale=text_scale,
fg=(0.0, 1.0, 0.0, 1.0),
shadow=(0.1, 0.1, 0.1, 0.8),
align=TextNode.ALeft)
# position display
z = self.surface.get_elevation(cam_pos[0], cam_pos[1])
lla = navpy.ned2lla([cam_pos[1], cam_pos[0], z], self.ned_ref[0],
self.ned_ref[1], self.ned_ref[2])
pos_str = "Lat: %.7f Lon: %.7f Alt(m): %.1f" % (lla[0], lla[1],
lla[2])
self.text1 = OnscreenText(text=pos_str,
pos=(0.95 * base.getAspectRatio(), -0.95),
scale=text_scale,
fg=(0.0, 1.0, 0.0, 1.0),
shadow=(0.1, 0.1, 0.1, 0.8),
align=TextNode.ARight)
def __opencv_pnp(self, new_matches, ref=None):
"""
This function is a callback which listens for new\
vision_nav_msgs/FeatureCorrespondence2D3D messages, and computes pose \
(3D position and orientation) of the camera in the world frame \
associated with new_matches.world_coordinate_frame. For now \
we're assuming this is WGS-84, [Longitude (deg), Latitude (deg), \
Height Above Ellipsoid (meters)]. Need to add logic in to transform\
these points to some arbitrary frame later on.
"""
if new_matches.num_correspondences > 5:
keypoints = new_matches.keypoints
lon_lat_h = new_matches.world_coordinates
# First we need to create a local level Cartesian frame (NED),
# and get the world_coordinates into that frame
# Use the first world point as the reference for NED frame
if ref is None:
ref = lon_lat_h.mean(0)
ref[2] = ref[2] - 150.0
world_pts_ned = navpy.lla2ned(lon_lat_h[:, 1], lon_lat_h[:, 0],
lon_lat_h[:,
2], ref[1], ref[0], ref[2])
if self.__db_bias is not None:
world_pts_ned = world_pts_ned - self.__db_bias
# Apply the 2D geometric constraint
idx = self.__apply_geometric_constraint(
world_pts_ned[:, 0:2].astype(np.float32),
keypoints.astype(np.float32))
print("%d kp from Matcher :: %d passed geometric constraint" %
(new_matches.num_correspondences, idx.shape[0]))
# Do PnP
if idx.shape[0] > 6:
success, rvec, tvec, pnp_status = \
cv2.solvePnPRansac(
world_pts_ned[idx, :].astype(np.float32).reshape(idx.shape[0], 1, 3),
keypoints[idx, :].astype(np.float32).reshape(idx.shape[0], 1, 2),
self.__camera_matrix,
self.__distortion,
reprojectionError=2.0)
if (success) and (pnp_status.shape[0] >= 6):
# Rvec is a rodrigues vector from world to cam, so need to
# transpose
C_n_b = (cv2.Rodrigues(rvec)[0]).transpose()
# Then tvec is from world to cam, so need to rotate into
# world frame and negate
t_nav = -1 * np.dot(C_n_b, tvec.reshape(3, 1)).flatten()
t_wgs = navpy.ned2lla(t_nav, ref[1], ref[0], ref[2])
t_wgs = np.array(t_wgs)
return t_wgs, idx[pnp_status].flatten()
else:
print("Bailing Not Enough Matches")
return np.array([]), np.array([])
def enu2lla(enu, gps_ref, latlon_unit='deg', alt_unit='m', model='wgs84'):
ned = np.dot(GlobalVals.C_ENU_NED, enu)
lla = ned2lla(ned, gps_ref.lat, gps_ref.lon, gps_ref.alt, latlon_unit,
alt_unit, model)
return lla
proj_list = project.projectVectors(IK, image, corner_list, pose=args.pose)
#print "proj_list:\n", proj_list
if args.pose == 'direct':
pts_ned = srtm.interpolate_vectors(image.camera_pose, proj_list)
elif args.pose == 'sba':
pts_ned = srtm.interpolate_vectors(image.camera_pose_sba, proj_list)
# print "pts (ned):\n", pts_ned
image.corner_list_ned = []
image.corner_list_lla = []
image.corner_list_xy = []
for ned in pts_ned:
#print p
image.corner_list_ned.append([ned[0], ned[1]])
image.corner_list_lla.append(
navpy.ned2lla([ned], ref[0], ref[1], ref[2]))
image.corner_list_xy.append([ned[1], ned[0]])
dst_dir = proj.project_dir + "/Warped/"
if not os.path.exists(dst_dir):
print "Notice: creating rubber sheeted texture directory =", dst_dir
os.makedirs(dst_dir)
for image in proj.image_list:
basename, ext = os.path.splitext(image.name)
dst = dst_dir + basename + ".png"
#if os.path.exists(dst):
# continue
# print image.name
scale = float(image.width) / float(camw)
K = proj.cam.get_K(scale)
def ConvNED2LLA(self, NED):
# NED should be 1X3 array
lat, lon, alt = navpy.ned2lla(NED, self.LLAref[0], self.LLAref[1], self.LLAref[2])
return lat, lon, alt
def make_interpolator(self, lla_ref, width_m, height_m, step_m):
print("Notice: constructing NED area interpolator")
rows = int(height_m / step_m) + 1
cols = int(width_m / step_m) + 1
#ned_pts = np.zeros((cols, rows))
#for r in range(0,rows):
# for c in range(0,cols):
# idx = (cols*r)+c
# #va = self.srtm_z[idx]
# #if va == 65535 or va < 0 or va > 10000:
# # va = 0.0
# #z = va
# ned_pts[c,r] = 0.0
# build regularly gridded x,y coordinate list and ned_pts array
n_list = np.linspace(-height_m * 0.5, height_m * 0.5, rows)
e_list = np.linspace(-width_m * 0.5, width_m * 0.5, cols)
#print "e's:", e_list
#print "n's:", n_list
ned_pts = []
for e in e_list:
for n in n_list:
ned_pts.append([n, e, 0])
# convert ned_pts list to lla coordinates (so it's not
# necessarily an exact grid anymore, but we can now
# interpolate elevations out of the lla interpolators for each
# tile.
navpy_pts = navpy.ned2lla(ned_pts, lla_ref[0], lla_ref[1], lla_ref[2])
#print navpy_pts
# build list of (lat, lon) points for doing actual lla
# elevation lookup
ll_pts = []
for i in range(len(navpy_pts[0])):
lat = navpy_pts[0][i]
lon = navpy_pts[1][i]
ll_pts.append([lon, lat])
#print "ll_pts:", ll_pts
# set all the elevations in the ned_ds list to the extreme
# minimum value. (rows,cols) might seem funny, but (ne)d is
# reversed from (xy)z ... don't think about it too much or
# you'll get a headache. :-)
ned_ds = np.zeros((rows, cols))
ned_ds[:][:] = -32768
#print "ned_ds:", ned_ds
# for each tile loaded, interpolate as many elevation values
# as we can, then copy the good values into ned_ds. When we
# finish all the loaded tiles, we should have elevations for
# the entire range of points.
for tile in self.tile_dict:
zs = self.tile_dict[tile].lla_interpolate(np.array(ll_pts))
#print zs
# copy the good altitudes back to the corresponding ned points
for r in range(0, rows):
for c in range(0, cols):
idx = (rows * c) + r
if zs[idx] > -10000:
ned_ds[r, c] = zs[idx]
# quick sanity check
for r in range(0, rows):
for c in range(0, cols):
idx = (rows * c) + r
if ned_ds[r, c] < -10000:
print("Problem interpolating elevation for:", ll_pts[idx])
ned_ds[r, c] = 0.0
#print "ned_ds:", ned_ds
# now finally build the actual grid interpolator with evenly
# spaced ned n, e values and elevations interpolated out of
# the srtm lla interpolator.
self.interp = scipy.interpolate.RegularGridInterpolator(
(n_list, e_list), ned_ds, bounds_error=False, fill_value=-32768)
do_plot = False
if do_plot:
imshow(ned_ds,
interpolation='bilinear',
origin='lower',
cmap=cm.gray,
alpha=1.0)
grid(False)
show()
do_test = False
if do_test:
for i in range(40):
ned = [(random.random() - 0.5) * height_m,
(random.random() - 0.5) * width_m, 0.0]
lla = navpy.ned2lla(ned, lla_ref[0], lla_ref[1], lla_ref[2])
#print "ned=%s, lla=%s" % (ned, lla)
nedz = self.interp([ned[0], ned[1]])
tile = make_tile_name(lla[0], lla[1])
llaz = self.tile_dict[tile].lla_interpolate(
np.array([lla[1], lla[0]]))
print("nedz=%.2f llaz=%.2f" % (nedz, llaz))
def coverage_lla(self, ref):
xmin, ymin, xmax, ymax = self.coverage_xy()
minlla = navpy.ned2lla([ymin, xmin, 0.0], ref[0], ref[1], ref[2])
maxlla = navpy.ned2lla([ymax, xmax, 0.0], ref[0], ref[1], ref[2])
return (minlla[1], minlla[0], maxlla[1], maxlla[0])