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]
