def read_gps_ecef(gps_file): # Read and pre-process llh = read_gps_fields(gps_file, ['lat', 'long', 'height']) if len(llh[0]) == 0: return None llh = np.array(llh, dtype=np.float64).T # NOTE Working w/ ECEF xyz = WGS84toECEF(llh).T return xyz
if __name__ == '__main__': import argparse parser = argparse.ArgumentParser( description='Solve graphslam QP for positions') parser.add_argument( 'gps_file', help='gps file to read raw positions and velocities from') parser.add_argument('ref_gps_file', help='gps file to read for llh0 in conversion to ENU') parser.add_argument('out_file', help='file to save opt positions to') parser.add_argument('--debug', action='store_true', help='display plots or not') args = parser.parse_args() # NOTE We're reading un ENU order while by default it's in NEU gps_fields = read_gps_fields( args.gps_file, ['lat', 'long', 'height', 'v_east', 'v_north', 'v_up']) ref_gps_fields = read_gps_fields(args.ref_gps_file, ['lat', 'long', 'height']) llh = np.array(gps_fields[0:3], dtype=np.float64).T llh_ref = np.array(ref_gps_fields, dtype=np.float64).T vel = np.array(gps_fields[3:6], dtype=np.float64) itg_vel = integrateVelocity(vel.T) xyz = WGS84toENU(llh_ref[0, :], llh) N = xyz.shape[1] #N = 10000 model = gurobipy.Model('graphslam_qp') # Create variables # Positions
from graphslam_config import BIAS_GAMMA, dt from gps_viewer import read_gps_fields from GPSTransforms import R_to_i_from_w if __name__ == '__main__': import argparse parser = argparse.ArgumentParser(description='Solve graphslam QP for positions') parser.add_argument('gps_file', help='gps file to read raw positions and velocities from') parser.add_argument('ref_gps_file', help='gps file to read for llh0 in conversion to ENU') parser.add_argument('out_file', help='file to save opt positions to') parser.add_argument('--debug', action='store_true', help='display plots or not') args = parser.parse_args() # NOTE We're reading un ENU order while by default it's in NEU gps_fields = read_gps_fields(args.gps_file, ['lat', 'long', 'height', 'v_east', 'v_north', 'v_up']) ref_gps_fields = read_gps_fields(args.ref_gps_file, ['lat', 'long', 'height']) llh = np.array(gps_fields[0:3], dtype=np.float64).T llh_ref = np.array(ref_gps_fields, dtype=np.float64).T vel = np.array(gps_fields[3:6], dtype=np.float64) itg_vel = integrateVelocity(vel.T) xyz = WGS84toENU(llh_ref[0, :], llh) N = xyz.shape[1] #N = 10000 model = gurobipy.Model('graphslam_qp') # Create variables # Positions xs = np.zeros((3, N), dtype=object)
def get_enu0(gps_file, gps_ref_file): llh = read_gps_fields(gps_file, ['lat', 'long', 'height']) llh = np.array(llh, dtype=np.float64).T llh_ref = read_gps_fields(gps_ref_file, ['lat', 'long', 'height']) llh_ref = np.array(llh_ref, dtype=np.float64).T return WGS84toENU(llh_ref[0, :], llh)[:, 0]
def get_ecef0(gps_file): llh = read_gps_fields(gps_file, ['lat', 'long', 'height']) llh = np.array(llh, dtype=np.float64).T ecef = WGS84toECEF(llh) return ecef[:, 0]