def ned2ecef(self, ned, origin): """ Converts ned local tangent plane coordinates into ecef coordinates using origin as the ecef point of tangency. Input: ned - (north, east, down) in (m, m, m) origin - (x0, y0, z0) in (m, m, m) Output: ecef - (x, y, z) in (m, m, m) """ llaOrigin = self.ecef2lla(origin) lat = geo.deg2rad(llaOrigin[0]) lon = geo.deg2rad(llaOrigin[1]) Rt2e = array([[-sin(lat)*cos(lon), -sin(lon), -cos(lat)*cos(lon)], [-sin(lat)*sin(lon), cos(lon), -cos(lat)*sin(lon)], [cos(lat), 0., -sin(lat)]]) return list(dot(Rt2e, array(ned)) + array(origin))
def ned2ecef(self, ned, origin): """ Converts ned local tangent plane coordinates into ecef coordinates using origin as the ecef point of tangency. Input: ned - (north, east, down) in (m, m, m) origin - (x0, y0, z0) in (m, m, m) Output: ecef - (x, y, z) in (m, m, m) """ llaOrigin = self.ecef2lla(origin) lat = geo.deg2rad(llaOrigin[0]) lon = geo.deg2rad(llaOrigin[1]) Rt2e = array([[-sin(lat) * cos(lon), -sin(lon), -cos(lat) * cos(lon)], [-sin(lat) * sin(lon), cos(lon), -cos(lat) * sin(lon)], [cos(lat), 0., -sin(lat)]]) return list(dot(Rt2e, array(ned)) + array(origin))
def lla2ecef(self, lla): """ Convert lat, lon, alt to Earth-centered, Earth-fixed coordinates. Input: lla - (lat, lon, alt) in (decimal degrees, decimal degees, m) Output: ecef - (x, y, z) in (m, m, m) """ # Decompose the input lat = geo.deg2rad(lla[0]) lon = geo.deg2rad(lla[1]) alt = lla[2] # Calculate length of the normal to the ellipsoid N = self.a / sqrt(1 - (self.e * sin(lat))**2) # Calculate ecef coordinates x = (N + alt) * cos(lat) * cos(lon) y = (N + alt) * cos(lat) * sin(lon) z = (N * (1 - self.e**2) + alt) * sin(lat) # Return the ecef coordinates return (x, y, z)
def ecef2ned(self, ecef, origin): """ Converts ecef coordinates into local tangent plane where the origin is the origin in ecef coordinates. Input: ecef - (x, y, z) in (m, m, m) origin - (x0, y0, z0) in (m, m, m) Output: ned - (north, east, down) in (m, m, m) """ # print('ecef = ', ecef) # print('origin = ', origin) llaOrigin = self.ecef2lla(origin) lat = geo.deg2rad(llaOrigin[0]) lon = geo.deg2rad(llaOrigin[1]) Re2t = array([[-sin(lat)*cos(lon), -sin(lat)*sin(lon), cos(lat)], [-sin(lon), cos(lon), 0], [-cos(lat)*cos(lon), -cos(lat)*sin(lon), -sin(lat)]]) # print('type ecef = %s' % type(ecef)) # print('type origin = %s' % type(origin)) return list(dot(Re2t, array(ecef) - array(origin)))
def ecef2ned(self, ecef, origin): """ Converts ecef coordinates into local tangent plane where the origin is the origin in ecef coordinates. Input: ecef - (x, y, z) in (m, m, m) origin - (x0, y0, z0) in (m, m, m) Output: ned - (north, east, down) in (m, m, m) """ # print('ecef = ', ecef) # print('origin = ', origin) llaOrigin = self.ecef2lla(origin) lat = geo.deg2rad(llaOrigin[0]) lon = geo.deg2rad(llaOrigin[1]) Re2t = array([[-sin(lat) * cos(lon), -sin(lat) * sin(lon), cos(lat)], [-sin(lon), cos(lon), 0], [-cos(lat) * cos(lon), -cos(lat) * sin(lon), -sin(lat)]]) # print('type ecef = %s' % type(ecef)) # print('type origin = %s' % type(origin)) return list(dot(Re2t, array(ecef) - array(origin)))
def get_variables(data): """ выдает основные переменные """ x, y = data['x'], data['y'] s, d = data['s'], data['d'] end_path_s, end_path_d = data['end_path_s'], data['end_path_d'] prev_x, prev_y = data['previous_path_x'], data['previous_path_y'] if len(prev_x) == 0: end_path_s, end_path_d = s, d ref_yaw = deg2rad(data['yaw']) other_cars = np.array([[a[-2], a[-1], sqrt(a[-3]**2 + a[-4]**2)] for a in data['sensor_fusion']]) #добавляем к s скорость * время for i in range(len(other_cars)): other_cars[i][0] += other_cars[i][2] * len(prev_x) * 0.02 #last_s,last_d=data['end_path_s'], data['end_path_d'] while len(last_coords) > len(prev_x): last_coords.pop(0) if len(last_coords) == 0: last_coords.append((s, d)) #todo - здесь должен быть cos/sin if len(prev_x) == 0: prev_x, prev_y = [x - 0.001, x], [y, y] if len(prev_x) == 1: prev_x, prev_y = [prev_x[0] - 0.001, prev_x[0]], [prev_y[0], prev_y[0]] car_speed = distance(prev_x[-1], prev_y[-1], prev_x[-2], prev_y[-2]) / 0.02 return prev_x, prev_y, end_path_s, end_path_d, car_speed, ref_yaw, other_cars
def lla2utm(self, lla): """ Converts lat, lon, alt to Universal Transverse Mercator coordinates Input: lla - (lat, lon, alt) in (decimal degrees, decimal degrees, m) Output: utm - (easting, northing, upping) in (m, m, m) info - (zone, scale factor) Algorithm from: Snyder, J. P., Map Projections-A Working Manual, U.S. Geol. Surv. Prof. Pap., 1395, 1987 Code segments from pygps project, Russ Nelson """ # Decompose lla lat = lla[0] lon = lla[1] alt = lla[2] # Determine the zone number zoneNumber = int((lon+180.)/6) + 1 # Special zone for Norway if (56. <= lat < 64.) and (3. <= lon < 12.): zoneNumber = 32 # Special zones for Svalbard if 72. <= lat < 84.: if 0. <= lon < 9.: zoneNumber = 31 elif 9. <= lon < 21.: zoneNumber = 33 elif 21. <= lon < 33.: zoneNumber = 35 elif 33. <= lon < 42.: zoneNumber = 37 # Format the zone zone = "%d%c" % (zoneNumber, self.utmLetterDesignator(lat)) # Determine longitude origin lonOrigin = (zoneNumber - 1) * 6 - 180 + 3 # Convert to radians latRad = geo.deg2rad(lat) lonRad = geo.deg2rad(lon) lonOriginRad = geo.deg2rad(lonOrigin) # Conversion constants k0 = 0.9996 eSquared = self.e**2 ePrimeSquared = eSquared/(1.-eSquared) N = self.a/sqrt(1.-eSquared*sin(latRad)**2) T = tan(latRad)**2 C = ePrimeSquared*cos(latRad)**2 A = (lonRad - lonOriginRad)*cos(latRad) M = self.a*( (1. - eSquared/4. - 3.*eSquared**2/64. - 5.*eSquared**3/256)*latRad - (3.*eSquared/8. + 3.*eSquared**2/32. + 45.*eSquared**3/1024.)*sin(2.*latRad) + (15.*eSquared**2/256. + 45.*eSquared**3/1024.)*sin(4.*latRad) - (35.*eSquared**3/3072.)*sin(6.*latRad)) M0 = 0. # Calculate coordinates x = k0*N*( A+(1-T+C)*A**3/6. + (5.-18.*T+T**2+72.*C-58.*ePrimeSquared)*A**5/120.) + 500000. y = k0*( M-M0+N*tan(latRad)*( A**2/2. + (5.-T+9.*C+4.*C**2)*A**4/24. + (61.-58.*T+T**2+600.*C-330.*ePrimeSquared)*A**6/720.)) # Calculate scale factor k = k0*(1 + (1+C)*A**2/2. + (5.-4.*T+42.*C+13.*C**2-28.*ePrimeSquared)*A**4/24. + (61.-148.*T+16.*T**2)*A**6/720.) utm = [x, y, alt] info = [zone, k] return utm, info
from geo import calc_dist, deg2rad, is_prop_latlon if __name__ == '__main__': """Start of program execution.""" # Read configuration file... config = configparser.ConfigParser() config.read('config.ini') # Read customer records from file... cust_records = read( join(config['DEFAULT']['DATA_DIR'], config['DEFAULT']['GIST_FILE'])) # Convert intercom office coordinates from degrees to radians... off_lat_deg = config['INPUTS']['OFFICE_LAT'] if is_prop_latlon(off_lat_deg, True): off_lat = deg2rad(float(off_lat_deg)) else: print("Intercom Office latitude is not in a proper float format.") print("Exitiing application.") exit() off_lon_deg = config['INPUTS']['OFFICE_LON'] if is_prop_latlon(off_lon_deg, False): off_lon = deg2rad(float(off_lon_deg)) else: print("Intercom Office longitude is not in a proper float format.") print("Exitiing application.") exit() # Loop through all records, find distance from intercom office, # and store the customer records in a dictionary who is within 100 kms...
def lla2utm(self, lla): """ Converts lat, lon, alt to Universal Transverse Mercator coordinates Input: lla - (lat, lon, alt) in (decimal degrees, decimal degrees, m) Output: utm - (easting, northing, upping) in (m, m, m) info - (zone, scale factor) Algorithm from: Snyder, J. P., Map Projections-A Working Manual, U.S. Geol. Surv. Prof. Pap., 1395, 1987 Code segments from pygps project, Russ Nelson """ # Decompose lla lat = lla[0] lon = lla[1] alt = lla[2] # Determine the zone number zoneNumber = int((lon + 180.) / 6) + 1 # Special zone for Norway if (56. <= lat < 64.) and (3. <= lon < 12.): zoneNumber = 32 # Special zones for Svalbard if 72. <= lat < 84.: if 0. <= lon < 9.: zoneNumber = 31 elif 9. <= lon < 21.: zoneNumber = 33 elif 21. <= lon < 33.: zoneNumber = 35 elif 33. <= lon < 42.: zoneNumber = 37 # Format the zone zone = "%d%c" % (zoneNumber, self.utmLetterDesignator(lat)) # Determine longitude origin lonOrigin = (zoneNumber - 1) * 6 - 180 + 3 # Convert to radians latRad = geo.deg2rad(lat) lonRad = geo.deg2rad(lon) lonOriginRad = geo.deg2rad(lonOrigin) # Conversion constants k0 = 0.9996 eSquared = self.e**2 ePrimeSquared = eSquared / (1. - eSquared) N = self.a / sqrt(1. - eSquared * sin(latRad)**2) T = tan(latRad)**2 C = ePrimeSquared * cos(latRad)**2 A = (lonRad - lonOriginRad) * cos(latRad) M = self.a * ((1. - eSquared / 4. - 3. * eSquared**2 / 64. - 5. * eSquared**3 / 256) * latRad - (3. * eSquared / 8. + 3. * eSquared**2 / 32. + 45. * eSquared**3 / 1024.) * sin(2. * latRad) + (15. * eSquared**2 / 256. + 45. * eSquared**3 / 1024.) * sin(4. * latRad) - (35. * eSquared**3 / 3072.) * sin(6. * latRad)) M0 = 0. # Calculate coordinates x = k0 * N * (A + (1 - T + C) * A**3 / 6. + (5. - 18. * T + T**2 + 72. * C - 58. * ePrimeSquared) * A**5 / 120.) + 500000. y = k0 * (M - M0 + N * tan(latRad) * (A**2 / 2. + (5. - T + 9. * C + 4. * C**2) * A**4 / 24. + (61. - 58. * T + T**2 + 600. * C - 330. * ePrimeSquared) * A**6 / 720.)) # Calculate scale factor k = k0 * (1 + (1 + C) * A**2 / 2. + (5. - 4. * T + 42. * C + 13. * C**2 - 28. * ePrimeSquared) * A**4 / 24. + (61. - 148. * T + 16. * T**2) * A**6 / 720.) utm = [x, y, alt] info = [zone, k] return utm, info