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