def get_pos(ins_df, pos_df, vel_df, cur, v_n_cur, pos_cur, c_bn, dt, wn_ie,
wn_en, wb_ib, g_n, pos_gps, vel_gps, acc_bias):
f_b = get_fb(ins_df, cur, acc_bias)
wn_in = calc_wn_in(wn_ie, wn_en)
wn_in_df = pd.DataFrame(data=wn_in)
pos_ref = position.extract_data(pos_df, pos_df.index[0])
pos_cur_ned = navpy.lla2ned(pos_cur[0][0],
pos_cur[1][0],
pos_cur[2][0],
pos_ref[0][0],
pos_ref[1][0],
pos_ref[2][0],
latlon_unit='rad',
alt_unit='m',
model='wgs84')
pos_gps_ned = navpy.lla2ned(pos_gps[0][0],
pos_gps[1][0],
pos_gps[2][0],
pos_ref[0][0],
pos_ref[1][0],
pos_ref[2][0],
latlon_unit='rad',
alt_unit='m',
model='wgs84')
pos_cur_ned = pos_cur_ned.reshape((-1, 1))
pos_gps_ned = pos_gps_ned.reshape((-1, 1))
A = state_matrix_a(wn_en, g_n, wn_ie, c_bn, f_b, wn_in)
M = noise_model_matrix_m(c_bn)
F = get_f_matrix(A, dt)
Q = noise_covariance_q(A, M, dt)
p_kk = calc_position_p_kk(pos_cur_ned, F, Q)
return p_kk
def keypoint_callback(self, channel, lcm_kp):
kp = uncorrespondedopticalcamerafeatures().decode(lcm_kp)
logger.info("Got KP with: %d Features" % kp.num_features)
if (self.att is not None) and ((time.time() - self.current_time) > self.wait_time):
self.current_time = time.time()
obs_kp = np.vstack((kp.x, kp.y)).T
obs_desc = np.array([np.frombuffer(dx, dtype=np.uint8) for dx in kp.descriptor_vector])
pnpout = self.pnp.do_pnp(obs_kp,
obs_desc,
self.att,
return_matches=True,
return_cv=True)
twgs, cov_n, niter, matches, status, cv_wgs = pnpout
if status.shape[0] > 0 and twgs.shape[0] > 0:
lon_lat_h = self.llh
cv_error = navpy.lla2ned(cv_wgs[0], cv_wgs[1], cv_wgs[2],
lon_lat_h[1], lon_lat_h[0],
lon_lat_h[2])
logger.info("OCV Error: %s :: %f " % (repr(cv_error), np.linalg.norm(cv_error)))
ned_error = navpy.lla2ned(twgs[0], twgs[1], twgs[2],
lon_lat_h[1], lon_lat_h[0],
lon_lat_h[2])
logger.info("NED Error: %s :: %f " % (repr(ned_error), np.linalg.norm(ned_error)))
tstamp = kp.header.timestamp_valid
kp_time = tstamp.sec + (tstamp.nsec / 1E9)
aspn_pos = aspn_geo_pos_from_row(kp_time,
self.seq,
twgs)
self.lch.publish(aspn_pos.header.device_id, aspn_pos.encode())
self.seq += 1
def draw_lla_point(self, lla, label):
pt_ned = navpy.lla2ned(lla[0], lla[1], lla[2], self.ref[0],
self.ref[1], self.ref[2])
rel_ned = [
pt_ned[0] - self.ned[0], pt_ned[1] - self.ned[1],
pt_ned[2] - self.ned[2]
]
hdist = math.sqrt(rel_ned[0] * rel_ned[0] + rel_ned[1] * rel_ned[1])
dist = math.sqrt(rel_ned[0] * rel_ned[0] + rel_ned[1] * rel_ned[1] +
rel_ned[2] * rel_ned[2])
m2sm = 0.000621371
hdist_sm = hdist * m2sm
if hdist_sm <= 10.0:
scale = 0.7 - (hdist_sm / 10.0) * 0.4
if hdist_sm <= 7.5:
label += " (%.1f)" % hdist_sm
# normalize, and draw relative to aircraft ned so that label
# separation works better
rel_ned[0] /= dist
rel_ned[1] /= dist
rel_ned[2] /= dist
self.draw_ned_point([
self.ned[0] + rel_ned[0], self.ned[1] + rel_ned[1],
self.ned[2] + rel_ned[2]
],
label,
scale=scale,
vert='below')
def test_lla2ned(self):
"""
Test conversion from LLA to NED.
Data Source: Example generated using book GNSS Applications and Methods
Chapter 7 library functions: wgslla2xyz.m, wgsxyz2enu
"""
# 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
ned_computed = navpy.lla2ned(lat, lon, alt, lat_ref, lon_ref, alt_ref)
for e1, e2 in zip(ned_computed, ned):
self.assertAlmostEqual(e1, e2, places=3)
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 add_marker_dict(self, m):
ned = navpy.lla2ned(m['lat_deg'], m['lon_deg'], m['alt_m'],
self.ned_ref[0], self.ned_ref[1], self.ned_ref[2])
if m['alt_m'] < 1.0:
# estimate surface elevation if needed
pos = np.array([ned[1], ned[0]]) # x, y order
norm = np.linalg.norm(pos)
if norm > 0:
v = pos / norm
# walk towards the center 1m at a time until we get onto
# the interpolation surface
while True:
z = self.surface.get_elevation(pos[0], pos[1])
print("pos:", pos, "v:", v, "z:", z)
if z < -1.0:
ned[2] = z
break
elif np.linalg.norm(pos) < 5:
# getting too close to the (ned) ned reference pt, failed
break
else:
pos -= v
print("ned updated:", ned)
if 'id' in m:
id = m['id']
if id >= self.next_id:
self.next_id = id + 1
else:
id = self.next_id
self.next_id += 1
self.add_marker(ned, m['comment'], id)
def DataFrameLLA2Cartesian(df):
"""
Convert point cloud to local cartesian X-Y-Z. North-East-Down (NED) system is used
Params:
--------------
df: Dataframe with X,Y,Z columns representing longitude, latitude and altitude respectively
Returns:
--------------
df: with appended columns x_ned, y_ned, z_ned
"""
lon = df["X"].values
lat = df["Y"].values
alt = df["Z"].values
cartesian = navpy.lla2ned(lat,
lon,
alt,
LAT_REF,
LON_REF,
ALT_REF,
latlon_unit='deg',
alt_unit='m',
model='wgs84')
df['x_cart'] = cartesian[:, 0]
df['y_cart'] = cartesian[:, 1]
df['z_cart'] = cartesian[:, 2]
return df
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 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 test_lla2ned(self):
"""
Test conversion from LLA to NED.
Data Source: Example generated using book GNSS Applications and Methods
Chapter 7 library functions: wgslla2xyz.m, wgsxyz2enu
"""
# 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
ned_computed = navpy.lla2ned(lat, lon, alt, lat_ref, lon_ref, alt_ref)
for e1, e2 in zip(ned_computed, ned):
self.assertAlmostEqual(e1, e2, places=3)
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 compute_camera_poses(proj):
log("Setting camera poses (offset from aircraft pose.)")
images_node = getNode("/images", True)
ref_node = getNode("/config/ned_reference", True)
ref_lat = ref_node.getFloat("lat_deg")
ref_lon = ref_node.getFloat("lon_deg")
ref_alt = ref_node.getFloat("alt_m")
body2cam = camera.get_body2cam()
for image in proj.image_list:
print("camera pose:", image.name)
ac_pose_node = image.node.getChild("aircraft_pose", True)
#cam_pose_node = images_node.getChild(name + "/camera_pose", True)
aircraft_lat = ac_pose_node.getFloat("lat_deg")
aircraft_lon = ac_pose_node.getFloat("lon_deg")
aircraft_alt = ac_pose_node.getFloat("alt_m")
ned2body = []
for i in range(4):
ned2body.append(ac_pose_node.getFloatEnum("quat", i))
ned2cam = transformations.quaternion_multiply(ned2body, body2cam)
(yaw_rad, pitch_rad,
roll_rad) = transformations.euler_from_quaternion(ned2cam, "rzyx")
ned = navpy.lla2ned(aircraft_lat, aircraft_lon, aircraft_alt, ref_lat,
ref_lon, ref_alt)
image.set_camera_pose(ned, yaw_rad * r2d, pitch_rad * r2d,
roll_rad * r2d)
def MapFrameToPos(df_frames):
"""
Maps frame_id to gps coordinate of camera when the frame was captured.
Params:
--------------
df_frames: pandas dataframe containing frame information
Returns:
--------------
map_frame2pos: dict containing mapping from frame number -> position (x,y,z) in NED system
"""
lon = df_frames["long"].values
lat = df_frames["lat"].values
alt = df_frames["alt"].values
cartesian = navpy.lla2ned(lat,
lon,
alt,
LAT_REF,
LON_REF,
ALT_REF,
latlon_unit='deg',
alt_unit='m',
model='wgs84')
map_frame2pos = {}
for (frame_no, row) in enumerate(cartesian):
map_frame2pos[frame_no] = row
return map_frame2pos
def gps_callback(host, port):
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
try:
s.connect((host, port))
s.settimeout(GlobalVals.GPS_TIMEOUT)
except Exception as e:
print("Exception: " + str(e.__class__))
print(
"There was an error starting the distanceEKF socket. This thread will now stop."
)
with GlobalVals.BREAK_GPS_LOGGER_THREAD_MUTEX:
GlobalVals.BREAK_GPS_LOGGER_THREAD = True
return
while True:
with GlobalVals.BREAK_GPS_LOGGER_THREAD_MUTEX:
if GlobalVals.BREAK_GPS_LOGGER_THREAD:
break
try:
data_bytes = s.recv(GlobalVals.BUFFER)
except Exception as e:
print("Exception: " + str(e.__class__))
print(
"There was an error starting the distanceEKF socket. This thread will now stop."
)
break
if len(data_bytes) == 0:
continue
data_str = data_bytes.decode('utf-8')
string_list = []
iterator = data_str.find('{')
while data_str.find('}', iterator) != -1:
substring_end = data_str.find('}', iterator)
string_list.append(data_str[iterator:substring_end + 1])
iterator = substring_end + 1
if len(string_list) > 0:
gps_list = []
for string in string_list:
received, gps_i = stringToGPS(string)
if received:
gps_list.append(gps_i)
idx = 0
while idx < len(gps_list):
ned = lla2ned(gps_list[idx].lat, gps_list[idx].lon,
gps_list[idx].atl, GlobalVals.GPS_REF.lat,
GlobalVals.GPS_REF.lon, GlobalVals.GPS_REF.alt)
posXY = ned[0:2]
position_update(posXY, gps_list[idx])
idx += 1
s.close()
def build_poly(img_num, geo_df, ref_pt):
f_llh = geo_df.loc[img_num][10:-1].reshape(4, 3).astype(np.float)
f_llh[:, 2] = 0
f_ned = navpy.lla2ned(f_llh[:, 1], f_llh[:, 0], f_llh[:, 2], ref_pt[0],
ref_pt[1], ref_pt[2])
f_ned = np.vstack((f_ned, f_ned[0, :]))
f_xy = [tuple(l) for l in f_ned[:, 0:2].tolist()]
return Polygon(f_xy)
def sensors_data_read(data, in_data: Classes.InData,
settings: Classes.Settings, out_data: Classes.OutData,
kalman_filter: Classes.Filter):
if len(in_data.imu
) < 101: # gathering 100 IMU readings to find average bias
if data[4] != "": # check if its IMU data or GPS - data[4] is IMU
# append accelerometer and gyroscope data
in_data.imu.append(
np.array([
float(data[5]),
float(data[4]),
float(data[6]),
float(data[10]),
float(data[11]),
float(data[12])
]))
# append magnetometer data
in_data.magnetometer.append(
np.array([float(data[7]),
float(data[8]),
float(data[9])]))
in_data.t.append((float(data[0]) / 100)) # append dt
if data[16] != "": # GPS data
in_data.gnssref.append([float(data[16]), float(data[17])])
if len(in_data.imu) > 100 and len(
in_data.gnssref
) != 0: # after gathering 100 readings send the data to setup
gps_aided_ins_setup(in_data, settings, out_data, kalman_filter)
else: # normal run after setup
if data[4] != "": # check if its IMU data
in_data.imu.append(
np.array([
float(data[5]),
float(data[4]),
float(data[6]),
float(data[10]),
float(data[11]),
float(data[12])
]))
in_data.magnetometer.append(
np.array([float(data[7]),
float(data[8]),
float(data[9])]))
in_data.t.append((float(data[0]) / 100))
gps_aided_ins(in_data, out_data, kalman_filter,
1) # run navigation step with IMU data input
if data[16] != "":
# append GPS data after converting long/lat to NED coordinate
in_data.gnss.append(
navpy.lla2ned(float(data[16]), float(data[17]), 0,
in_data.gnssref[0][0], in_data.gnssref[0][1], 0))
in_data.tgnss.append((float(data[0]) / 100)) # dt
gps_aided_ins(in_data, out_data, kalman_filter,
0) # run navigation step with GPS data input
return
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 DataFrameLLA2Cartesian(df): lon = df["X"].values lat = df["Y"].values alt = df["Z"].values cartesian = navpy.lla2ned(lat, lon, alt, LAT_REF, LON_REF, ALT_REF, latlon_unit='deg', alt_unit='m', model='wgs84') df['x_cart'] = cartesian[:, 0] df['y_cart'] = cartesian[:, 1] df['z_cart'] = cartesian[:, 2] return df
def get_average_tile_size(extent):
ii = 0
box_size = np.zeros((extent.grid_size**2, 2))
for xid in np.arange(extent.xb[0], extent.xb[1] + 1):
for yid in np.arange(extent.yb[0], extent.yb[1] + 1):
bbox = mercantile.bounds(xid, yid, 15)
ned_box = navpy.lla2ned(bbox.north, bbox.west, 0.0, bbox.south,
bbox.east, 0.0)
box_size[ii, :] = np.copy(ned_box[0:2])
ii += 1
return np.abs(box_size.mean(0)).mean()
def LLA2NED(self, curr_lat, curr_lon, curr_alt):
NED = navpy.lla2ned(curr_lat,
curr_lon,
curr_alt,
self.Home.lat,
self.Home.lon,
self.Home.alt,
latlon_unit='deg',
alt_unit='m',
model='wgs84')
return LocationLocal(NED[0], NED[1], NED[2])
def DataFrameLLA2Cartesian(df):
LAT_REF = constants.LAT_REF
LON_REF = constants.LON_REF
ALT_REF = constants.ALT_REF
lon = df["lon"].values
lat = df["lat"].values
alt = df["alt"].values
cartesian = navpy.lla2ned(lat, lon, alt,LAT_REF, LON_REF, ALT_REF,latlon_unit='deg', alt_unit='m', model='wgs84')
df['x_cart'] = cartesian[:, 0]
df['y_cart'] = cartesian[:, 1]
df['z_cart'] = cartesian[:, 2]
return df
def get_state(ins_df, pos_df, vel_df, cur, v_n_cur, pos_cur, c_bn, dt, wn_ie,
wn_en, wb_ib, g_n, pos_gps, vel_gps, acc_bias):
f_b = get_fb(ins_df, cur, acc_bias)
wn_in = calc_wn_in(wn_ie, wn_en)
wn_in_df = pd.DataFrame(data=wn_in)
pos_cur_ned = navpy.lla2ned(pos_cur[0][0],
pos_cur[1][0],
pos_cur[2][0],
cn.pos_ref[0][0],
cn.pos_ref[1][0],
cn.pos_ref[2][0],
latlon_unit='rad',
alt_unit='m',
model='wgs84')
pos_gps_ned = navpy.lla2ned(pos_gps[0][0],
pos_gps[1][0],
pos_gps[2][0],
cn.pos_ref[0][0],
cn.pos_ref[1][0],
cn.pos_ref[2][0],
latlon_unit='rad',
alt_unit='m',
model='wgs84')
pos_cur_ned = pos_cur_ned.reshape((-1, 1))
pos_gps_ned = pos_gps_ned.reshape((-1, 1))
A = state_matrix_a(wn_en, g_n, wn_ie, c_bn, f_b, wn_in)
M = noise_model_matrix_m(c_bn)
F = get_f_matrix(A, dt)
Q = noise_covariance_q(A, M, dt)
p_kk = calc_position_p_kk(pos_cur_ned, F, Q)
K = noise_gain_matrix_k(p_kk)
del_x = loose_state_matrix(v_n_cur, pos_cur_ned, K, pos_gps_ned, vel_gps)
p_kk = (np.identity(15) - K @ cn.H) @ p_kk
cn.p_init = p_kk
return del_x
def gps_callback(host,port):
s = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
try:
s.connect((host,port))
s.settimeout(GlobalVals.GPS_TIMEOUT)
except Exception as e:
print("Exception: " + str(e.__class__))
print("There was an error starting the GPS socket. This thread will now stop.")
with GlobalVals.BREAK_GPS_LOGGER_THREAD_MUTEX:
GlobalVals.BREAK_GPS_LOGGER_THREAD = True
return
while True:
with GlobalVals.BREAK_GPS_LOGGER_THREAD_MUTEX:
if GlobalVals.BREAK_GPS_LOGGER_THREAD:
break
try:
data_bytes = s.recv(GlobalVals.BUFFER)
except Exception as e:
print("Exception: " + str(e.__class__))
print("There was an error starting the GPS socket. This thread will now stop.")
break
if len(data_bytes) == 0:
continue
data_str = data_bytes.decode('utf-8')
string_list = []
string_list = extract_str_btw_curly_brackets(data_str)
if len(string_list) > 0:
gps_list = []
for string in string_list:
received, gps_i = stringToGPS(string)
if received:
gps_list.append(gps_i)
idx = 0
with GlobalVals.POSXYZ_UPDATE_MUTEX:
while idx < len(gps_list):
ned = lla2ned(gps_list[idx].lat, gps_list[idx].lon, gps_list[idx].alt, GlobalVals.GPS_REF.lat, GlobalVals.GPS_REF.lon, GlobalVals.GPS_REF.alt)
posXYZ = POS_XYZ(ned[0],ned[1])
position_update(posXYZ, gps_list[idx])
idx += 1
s.close()
def load(self):
file = os.path.join(self.project_dir, 'annotations.json')
if os.path.exists(file):
print('Loading saved annotations:', file)
f = open(file, 'r')
lla_list = json.load(f)
f.close()
for lla in lla_list:
ned = navpy.lla2ned(lla[0], lla[1], lla[2],
self.ned_ref[0],
self.ned_ref[1],
self.ned_ref[2])
# print(lla, ned)
self.markers.append( [ned[1], ned[0]] )
else:
print('No annotations file found.')
def get_ned_pos(ins_df, imu_df):
ned_pos = pd.DataFrame(data=None, index=ins_df.index,
columns=['X', 'Y', 'Z'])
for time in ins_df.index:
NED = navpy.lla2ned(ins_df.loc[(time, 'lat')],
ins_df.loc[(time, 'long')],
ins_df.loc[(time, 'h')],
cn.pos_ref[0][0],
cn.pos_ref[1][0],
cn.pos_ref[2][0],
latlon_unit='rad', alt_unit='m', model='wgs84')
ned_pos.loc[(time, 'X')] = NED[0]
ned_pos.loc[(time, 'Y')] = NED[1]
ned_pos.loc[(time, 'Z')] = NED[2]
if time == 424:
break
return ned_pos
def load(file, ned_ref, range_m):
result = []
with open(file, 'r') as f:
reader = csv.DictReader(f)
for row in reader:
lat = float(row['Lat'])
lon = float(row['Lon'])
alt = float(row['Alt'])
pt_ned = navpy.lla2ned( lat, lon, alt,
ned_ref[0], ned_ref[1], ned_ref[2] )
dist = math.sqrt(pt_ned[0]*pt_ned[0] + pt_ned[1]*pt_ned[1]
+ pt_ned[2]*pt_ned[2])
if dist <= range_m:
print('found:', row['Ident'], 'dist: %.1f km' % (dist/1000))
result.append( [ row['Ident'], lat, lon, alt ] )
print('done!')
return result
def make_bucket_based_on_gps(self,lat,lon,alt):
self.ax1.clear()
check_list=[]
x_sign, y_sign, z_sign = navpy.lla2ned(float(lat),float(lon),float(alt),
LAT_REF, LON_REF, ALT_REF,
latlon_unit='deg', alt_unit='m', model='wgs84')
query_point = np.array([x_sign,y_sign,z_sign]).reshape(1,-1)
query_return = self.kd_tree.query_ball_point(query_point,r=20)
if len(query_return[0])>39:
for i in query_return[0]:
temp_list=[None,None,None,None,None,None,None,None,None,None,None,None]
temp_list[0]=lat
temp_list[1]=lon
temp_list[2]=alt
temp_list[3]=i
temp_list[4]=self.df_retro.iloc[i]['Y']
temp_list[5]=self.df_retro.iloc[i]['X']
temp_list[6]=self.df_retro.iloc[i]['Z']
temp_list[7]=self.df_retro.iloc[i]['Retro']
temp_list[8]=len(query_return[0])
temp_list[9]=self.df_retro.iloc[i]['x_cart']
temp_list[10]=self.df_retro.iloc[i]['y_cart']
temp_list[11]=self.df_retro.iloc[i]['z_cart']
check_list.append(temp_list)
else:
print('[INFO] Not enough points')
print("[INFO] Saving to file")
df_lidar = pd.DataFrame(check_list,columns=['query_lat','query_lon','query_alt','index','lidar_lat','lidar_long','lidar_alt','retro','count','x_cart','y_cart','z_cart',])
output_path='../Data/enter_gps/output_file_entered_gps_{}_{}_{}.csv'.format(lat,lon,alt)
df_lidar.to_csv(output_path,index=False,header=True)
print("[INFO] Finished extracting points and saved to output csv file")
df_plot=pd.read_csv(output_path)
self.ax1.scatter(df_plot['lidar_lat'], df_plot['lidar_long'],df_plot['lidar_alt'], s=30, c='b', marker='.')
self.ax1.scatter(df_plot['query_lat'],df_plot['query_lon'],df_plot['query_alt'],s=20,marker='*')
def read_bagfile(seed_bag, subsample = 1, home = [13.1916987, -59.6419202, 0.00000]):
# Hard coded bag file and topic names
bag = rosbag.Bag(seed_bag)
position_topic = '/slicklizard/gnc/mavros/global_position/global'
data_topic = '/slicklizard/sensors/micron_echo/data'
altitude = []
locations = []
latitude = []
longitude = []
times = []
prev_loc = current_loc = current_alt = None
for topic, msg, t in bag.read_messages(topics = [data_topic, position_topic]):
if topic == position_topic:
# If a more recent altitude data point has been recieved, save the following lat-long coordinate
if current_alt is not None:
# Only take data points in the correct quadrant
if msg.latitude > home[0] and msg.longitude > home[1]:
altitude.append(-(current_alt.range - 3.007621677259172))
loc = navpy.lla2ned(msg.latitude, msg.longitude, 0.0, home[0], home[1], 0.0)
locations.append([loc[1], loc[0]])
times.append(current_alt.header.stamp.secs)
current_alt = None
elif topic == '/slicklizard/sensors/micron_echo/data':
current_alt = msg
# Convert lists to ndarrays
locations = np.array(locations).reshape((-1, 2));
# altitude = np.array(altitude-np.mean(altitude)).reshape((-1, 1))
# altitude = np.array(altitude).reshape((-1, 1))
altitude = np.array(altitude)
FILT_N = 5
altitude = np.convolve(altitude, np.ones((FILT_N,))/FILT_N, mode='same').reshape((-1, 1))
# Reject outliers (more then 2 standard deviations from the mean) and subsamples the data
outlier_index = (abs(altitude - np.mean(altitude)) < 2.0 * np.std(altitude)).reshape(-1, )
# locations = locations[outlier_index, :][::10]
# altitude = altitude[outlier_index][::10]
locations = locations[outlier_index, :][::subsample]
altitude = altitude[outlier_index][::subsample]
return locations, altitude
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 get_pix_size(self, lon_lat_h, C_n_v):
"""
Given the current pose of the aircraft, calculate the GSD for each edge
of the image
:param lon_lat_h: [3,] Position of the camera: 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
:return: Returns an [4,] np.ndarray of the pixel size (m/pixel) for each
of the 4 edges of the image. (bottom, right, top, left)
"""
cwgs = self.project_corners(lon_lat_h, C_n_v)
cref = cwgs[0, :]
corners_ned = navpy.lla2ned(cwgs[:, 1], cwgs[:, 0], cwgs[:, 2],
cref[1], cref[0], cref[2])
corners_ned = np.vstack((corners_ned, corners_ned[0, :]))
dist = np.linalg.norm(np.diff(corners_ned[:, 0:2], axis=0), axis=1)
pix = np.tile([self.cam_width, self.cam_height], 2)
return dist / pix
def computeCameraPoseFromAircraft(image, cam, ref,
yaw_bias=0.0, roll_bias=0.0,
pitch_bias=0.0, alt_bias=0.0):
lla, ypr, ned2body = image.get_aircraft_pose()
aircraft_lat, aircraft_lon, aircraft_alt = lla
#print "aircraft quat =", world2body
msl = aircraft_alt + image.alt_bias + alt_bias
(camera_yaw, camera_pitch, camera_roll) = cam.get_mount_params()
body2cam = transformations.quaternion_from_euler(camera_yaw * d2r,
camera_pitch * d2r,
camera_roll * d2r,
'rzyx')
ned2cam = transformations.quaternion_multiply(ned2body, body2cam)
(yaw, pitch, roll) = transformations.euler_from_quaternion(ned2cam, 'rzyx')
ned = navpy.lla2ned( aircraft_lat, aircraft_lon, aircraft_alt,
ref[0], ref[1], ref[2] )
#print "aircraft=%s ref=%s ned=%s" % (image.get_aircraft_pose(), ref, ned)
return (ned.tolist(), [yaw/d2r, pitch/d2r, roll/d2r])
'rzyx')
#print 'att:', [yaw_rad, pitch_rad, roll_rad]
ned2body = transformations.quaternion_from_euler(yaw_rad,
pitch_rad,
roll_rad,
'rzyx')
body2ned = transformations.quaternion_inverse(ned2body)
#print 'ned2body(q):', ned2body
ned2cam_q = transformations.quaternion_multiply(ned2body, body2cam)
ned2cam = np.matrix(transformations.quaternion_matrix(np.array(ned2cam_q))[:3,:3]).T
#print 'ned2cam:', ned2cam
R = ned2proj.dot( ned2cam )
rvec, jac = cv2.Rodrigues(R)
ned = navpy.lla2ned( lat_deg, lon_deg, altitude_m,
ref[0], ref[1], ref[2] )
#print 'ned:', ned
tvec = -np.matrix(R) * np.matrix(ned).T
R, jac = cv2.Rodrigues(rvec)
# is this R the same as the earlier R?
PROJ = np.concatenate((R, tvec), axis=1)
#print 'PROJ:', PROJ
#print lat_deg, lon_deg, altitude, ref[0], ref[1], ref[2]
#print ned
method = cv2.INTER_AREA
#method = cv2.INTER_LANCZOS4
frame_scale = cv2.resize(frame, (0,0), fx=args.scale, fy=args.scale,
interpolation=method)
frame_undist = cv2.undistort(frame_scale, K, np.array(dist))
def run_filter(filter, imu_data, gps_data, filter_data, config=None):
data_dict = data_store.data_store()
errors = []
# Using while loop starting at k (set to kstart) and going to end
# of .mat file
gps_index = 0
filter_index = 0
new_gps = 0
if config and config['call_init']:
filter_init = False
else:
filter_init = True
if config and 'start_time' in config:
for k, imu_pt in enumerate(imu_data):
if imu_pt.time >= config['start_time']:
k_start = k
break
else:
k_start = 0
if config and 'end_time' in config:
for k, imu_pt in enumerate(imu_data):
if imu_pt.time >= config['end_time']:
k_end = k
break
else:
k_end = len(imu_data)
#print k_start, k_end
for k in range(k_start, k_end):
imupt = imu_data[k]
if gps_index < len(gps_data) - 1:
# walk the gps counter forward as needed
newData = 0
while gps_data[gps_index+1].time - gps_latency <= imupt.time:
gps_index += 1
newData = 1
gpspt = gps_data[gps_index]
gpspt.newData = newData
else:
# no more gps data, stay on the last record
gpspt = gps_data[gps_index]
gpspt.newData = 0
#print gpspt.time`
# walk the filter counter forward as needed
if len(filter_data):
if imupt.time > filter_data[filter_index].time:
filter_index += 1
if filter_index >= len(filter_data):
# no more filter data, stay on the last record
filter_index = len(filter_data)-1
filterpt = filter_data[filter_index]
else:
filterpt = None
#print "t(imu) = " + str(imupt.time) + " t(gps) = " + str(gpspt.time)
# If k is at the initialization time init_nav else get_nav
if not filter_init and gps_index > 0:
print "init:", imupt.time, gpspt.time
navpt = filter.init(imupt, gpspt, filterpt)
filter_init = True
elif filter_init:
navpt = filter.update(imupt, gpspt, filterpt)
# Store the desired results obtained from the compiled test
# navigation filter and the baseline filter
if filter_init:
data_dict.append(navpt)
if gpspt.newData:
# compute error metric with each new gps report
# full 3d distance error (in ecef)
p1 = navpy.lla2ecef(gpspt.lat, gpspt.lon, gpspt.alt,
latlon_unit='deg')
p2 = navpy.lla2ecef(navpt.lat, navpt.lon, navpt.alt,
latlon_unit='rad')
pe = np.linalg.norm(p1 - p2)
# weight horizontal error more highly than vertical error
ref = gps_data[0]
n1 = navpy.lla2ned(gpspt.lat, gpspt.lon, gpspt.alt,
ref.lat, ref.lon, 0.0,
latlon_unit='deg')
n2 = navpy.lla2ned(navpt.lat*r2d, navpt.lon*r2d, navpt.alt,
ref.lat, ref.lon, 0.0,
latlon_unit='deg')
dn = n2 - n1
ne = math.sqrt(dn[0]*dn[0] + dn[1]*dn[1] + dn[2]*dn[2]*0.5)
# it is always tempting to fit to the velocity vector
# (especially when seeing some of the weird velocity fits
# that the optimizer spews out), but it never helps
# ... seems to make the solution convergence much more
# shallow, ultimately never seems to produce a better fit
# than using position directly. Weird fits happen when
# the inertial errors just don't fit the gps errors.
# Fitting to velocity doesn't seem to improve that
# problem.
v1 = np.array( [gpspt.vn, gpspt.ve, gpspt.vd] )
v2 = np.array( [navpt.vn, navpt.ve, navpt.vd] )
ve = np.linalg.norm(v1 - v2)
errors.append(ne) # ned error weighted towards horizontal
# Increment time up one step for the next iteration of the
# while loop.
k += 1
# proper cleanup
filter.close()
return errors, data_dict
