1 /* 2 * Copyright (C) 2022 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 package android.location.altitude; 18 19 import android.annotation.FlaggedApi; 20 import android.annotation.NonNull; 21 import android.annotation.WorkerThread; 22 import android.content.Context; 23 import android.frameworks.location.altitude.GetGeoidHeightRequest; 24 import android.frameworks.location.altitude.GetGeoidHeightResponse; 25 import android.location.Location; 26 import android.location.flags.Flags; 27 28 import com.android.internal.location.altitude.GeoidMap; 29 import com.android.internal.location.altitude.S2CellIdUtils; 30 import com.android.internal.location.altitude.nano.MapParamsProto; 31 import com.android.internal.util.Preconditions; 32 33 import java.io.IOException; 34 35 /** 36 * Converts altitudes reported above the World Geodetic System 1984 (WGS84) reference ellipsoid 37 * into ones above Mean Sea Level. 38 * 39 * <p>Reference: 40 * 41 * <pre> 42 * Brian Julian and Michael Angermann. 43 * "Resource efficient and accurate altitude conversion to Mean Sea Level." 44 * 2023 IEEE/ION Position, Location and Navigation Symposium (PLANS). 45 * </pre> 46 */ 47 public final class AltitudeConverter { 48 49 private static final double MAX_ABS_VALID_LATITUDE = 90; 50 private static final double MAX_ABS_VALID_LONGITUDE = 180; 51 52 /** Manages a mapping of geoid heights and expiration distances associated with S2 cells. */ 53 private final GeoidMap mGeoidMap = new GeoidMap(); 54 55 /** 56 * Creates an instance that manages an independent cache to optimized conversions of locations 57 * in proximity to one another. 58 */ AltitudeConverter()59 public AltitudeConverter() { 60 } 61 62 /** 63 * Throws an {@link IllegalArgumentException} if the {@code location} has an invalid latitude, 64 * longitude, or altitude above WGS84. 65 */ validate(@onNull Location location)66 private static void validate(@NonNull Location location) { 67 Preconditions.checkArgument( 68 isFiniteAndAtAbsMost(location.getLatitude(), MAX_ABS_VALID_LATITUDE), 69 "Invalid latitude: %f", location.getLatitude()); 70 Preconditions.checkArgument( 71 isFiniteAndAtAbsMost(location.getLongitude(), MAX_ABS_VALID_LONGITUDE), 72 "Invalid longitude: %f", location.getLongitude()); 73 Preconditions.checkArgument(location.hasAltitude(), "Missing altitude above WGS84"); 74 Preconditions.checkArgument(Double.isFinite(location.getAltitude()), 75 "Invalid altitude above WGS84: %f", location.getAltitude()); 76 } 77 isFiniteAndAtAbsMost(double value, double rhs)78 private static boolean isFiniteAndAtAbsMost(double value, double rhs) { 79 return Double.isFinite(value) && Math.abs(value) <= rhs; 80 } 81 82 /** 83 * Returns the four S2 cell IDs for the map square associated with the {@code location}. 84 * 85 * <p>The first map cell, denoted z11 in the appendix of the referenced paper above, contains 86 * the location. The others are the map cells denoted z21, z12, and z22, in that order. 87 */ findMapSquare(@onNull MapParamsProto geoidHeightParams, @NonNull Location location)88 private static long[] findMapSquare(@NonNull MapParamsProto geoidHeightParams, 89 @NonNull Location location) { 90 long s2CellId = S2CellIdUtils.fromLatLngDegrees(location.getLatitude(), 91 location.getLongitude()); 92 93 // Cell-space properties and coordinates. 94 int sizeIj = 1 << (S2CellIdUtils.MAX_LEVEL - geoidHeightParams.mapS2Level); 95 int maxIj = 1 << S2CellIdUtils.MAX_LEVEL; 96 long z11 = S2CellIdUtils.getParent(s2CellId, geoidHeightParams.mapS2Level); 97 int f11 = S2CellIdUtils.getFace(s2CellId); 98 int i1 = S2CellIdUtils.getI(s2CellId); 99 int j1 = S2CellIdUtils.getJ(s2CellId); 100 int i2 = i1 + sizeIj; 101 int j2 = j1 + sizeIj; 102 103 // Non-boundary region calculation - simplest and most common case. 104 if (i2 < maxIj && j2 < maxIj) { 105 return new long[]{z11, S2CellIdUtils.getParent(S2CellIdUtils.fromFij(f11, i2, j1), 106 geoidHeightParams.mapS2Level), S2CellIdUtils.getParent( 107 S2CellIdUtils.fromFij(f11, i1, j2), geoidHeightParams.mapS2Level), 108 S2CellIdUtils.getParent(S2CellIdUtils.fromFij(f11, i2, j2), 109 geoidHeightParams.mapS2Level)}; 110 } 111 112 // Boundary region calculation 113 long[] edgeNeighbors = new long[4]; 114 S2CellIdUtils.getEdgeNeighbors(z11, edgeNeighbors); 115 long z11W = edgeNeighbors[0]; 116 long z11S = edgeNeighbors[1]; 117 long z11E = edgeNeighbors[2]; 118 long z11N = edgeNeighbors[3]; 119 120 long[] otherEdgeNeighbors = new long[4]; 121 S2CellIdUtils.getEdgeNeighbors(z11W, otherEdgeNeighbors); 122 S2CellIdUtils.getEdgeNeighbors(z11S, edgeNeighbors); 123 long z11Sw = findCommonNeighbor(edgeNeighbors, otherEdgeNeighbors, z11); 124 S2CellIdUtils.getEdgeNeighbors(z11E, otherEdgeNeighbors); 125 long z11Se = findCommonNeighbor(edgeNeighbors, otherEdgeNeighbors, z11); 126 S2CellIdUtils.getEdgeNeighbors(z11N, edgeNeighbors); 127 long z11Ne = findCommonNeighbor(edgeNeighbors, otherEdgeNeighbors, z11); 128 129 long z21 = (f11 % 2 == 1 && i2 >= maxIj) ? z11Sw : z11S; 130 long z12 = (f11 % 2 == 0 && j2 >= maxIj) ? z11Ne : z11E; 131 long z22 = (z21 == z11Sw) ? z11S : (z12 == z11Ne) ? z11E : z11Se; 132 133 // Reuse edge neighbors' array to avoid an extra allocation. 134 edgeNeighbors[0] = z11; 135 edgeNeighbors[1] = z21; 136 edgeNeighbors[2] = z12; 137 edgeNeighbors[3] = z22; 138 return edgeNeighbors; 139 } 140 141 /** 142 * Returns the first common non-z11 neighbor found between the two arrays of edge neighbors. If 143 * such a common neighbor does not exist, returns z11. 144 */ findCommonNeighbor(long[] edgeNeighbors, long[] otherEdgeNeighbors, long z11)145 private static long findCommonNeighbor(long[] edgeNeighbors, long[] otherEdgeNeighbors, 146 long z11) { 147 for (long edgeNeighbor : edgeNeighbors) { 148 if (edgeNeighbor == z11) { 149 continue; 150 } 151 for (long otherEdgeNeighbor : otherEdgeNeighbors) { 152 if (edgeNeighbor == otherEdgeNeighbor) { 153 return edgeNeighbor; 154 } 155 } 156 } 157 return z11; 158 } 159 160 /** 161 * Adds to {@code location} the bilinearly interpolated Mean Sea Level altitude. In addition, a 162 * Mean Sea Level altitude accuracy is added if the {@code location} has a valid vertical 163 * accuracy; otherwise, does not add a corresponding accuracy. 164 */ addMslAltitude(@onNull MapParamsProto geoidHeightParams, @NonNull double[] geoidHeightsMeters, @NonNull Location location)165 private static void addMslAltitude(@NonNull MapParamsProto geoidHeightParams, 166 @NonNull double[] geoidHeightsMeters, @NonNull Location location) { 167 double h0 = geoidHeightsMeters[0]; 168 double h1 = geoidHeightsMeters[1]; 169 double h2 = geoidHeightsMeters[2]; 170 double h3 = geoidHeightsMeters[3]; 171 172 // Bilinear interpolation on an S2 square of size equal to that of a map cell. wi and wj 173 // are the normalized [0,1] weights in the i and j directions, respectively, allowing us to 174 // employ the simplified unit square formulation. 175 long s2CellId = S2CellIdUtils.fromLatLngDegrees(location.getLatitude(), 176 location.getLongitude()); 177 double sizeIj = 1 << (S2CellIdUtils.MAX_LEVEL - geoidHeightParams.mapS2Level); 178 double wi = (S2CellIdUtils.getI(s2CellId) % sizeIj) / sizeIj; 179 double wj = (S2CellIdUtils.getJ(s2CellId) % sizeIj) / sizeIj; 180 double offsetMeters = h0 + (h1 - h0) * wi + (h2 - h0) * wj + (h3 - h1 - h2 + h0) * wi * wj; 181 182 location.setMslAltitudeMeters(location.getAltitude() - offsetMeters); 183 if (location.hasVerticalAccuracy()) { 184 double verticalAccuracyMeters = location.getVerticalAccuracyMeters(); 185 if (Double.isFinite(verticalAccuracyMeters) && verticalAccuracyMeters >= 0) { 186 location.setMslAltitudeAccuracyMeters((float) Math.hypot(verticalAccuracyMeters, 187 geoidHeightParams.modelRmseMeters)); 188 } 189 } 190 } 191 192 /** 193 * Adds a Mean Sea Level altitude to the {@code location}. In addition, adds a Mean Sea Level 194 * altitude accuracy if the {@code location} has a finite and non-negative vertical accuracy; 195 * otherwise, does not add a corresponding accuracy. 196 * 197 * <p>Must be called off the main thread as data may be loaded from raw assets. 198 * 199 * @throws IOException if an I/O error occurs when loading data from raw assets. 200 * @throws IllegalArgumentException if the {@code location} has an invalid latitude, longitude, 201 * or altitude above WGS84. Specifically, the latitude must be 202 * between -90 and 90 (both inclusive), the longitude must be 203 * between -180 and 180 (both inclusive), and the altitude 204 * above WGS84 must be finite. 205 */ 206 @WorkerThread addMslAltitudeToLocation(@onNull Context context, @NonNull Location location)207 public void addMslAltitudeToLocation(@NonNull Context context, @NonNull Location location) 208 throws IOException { 209 validate(location); 210 MapParamsProto geoidHeightParams = GeoidMap.getGeoidHeightParams(context); 211 long[] mapCells = findMapSquare(geoidHeightParams, location); 212 double[] geoidHeightsMeters = mGeoidMap.readGeoidHeights(geoidHeightParams, context, 213 mapCells); 214 addMslAltitude(geoidHeightParams, geoidHeightsMeters, location); 215 } 216 217 /** 218 * Same as {@link #addMslAltitudeToLocation(Context, Location)} except that this method can be 219 * called on the main thread as data will not be loaded from raw assets. Returns true if a Mean 220 * Sea Level altitude is added to the {@code location}; otherwise, returns false and leaves the 221 * {@code location} unchanged. 222 * 223 * <p>Prior calls to {@link #addMslAltitudeToLocation(Context, Location)} off the main thread 224 * are necessary to load data from raw assets. Example code on the main thread is as follows: 225 * 226 * <pre>{@code 227 * if (!mAltitudeConverter.tryAddMslAltitudeToLocation(location)) { 228 * // Queue up only one call off the main thread. 229 * if (mIsAltitudeConverterIdle) { 230 * mIsAltitudeConverterIdle = false; 231 * executeOffMainThread(() -> { 232 * try { 233 * // Load raw assets for next call attempt on main thread. 234 * mAltitudeConverter.addMslAltitudeToLocation(mContext, location); 235 * } catch (IOException e) { 236 * Log.e(TAG, "Not loading raw assets: " + e); 237 * } 238 * mIsAltitudeConverterIdle = true; 239 * }); 240 * } 241 * } 242 * }</pre> 243 */ 244 @FlaggedApi(Flags.FLAG_GEOID_HEIGHTS_VIA_ALTITUDE_HAL) tryAddMslAltitudeToLocation(@onNull Location location)245 public boolean tryAddMslAltitudeToLocation(@NonNull Location location) { 246 validate(location); 247 MapParamsProto geoidHeightParams = GeoidMap.getGeoidHeightParams(); 248 if (geoidHeightParams == null) { 249 return false; 250 } 251 252 long[] mapCells = findMapSquare(geoidHeightParams, location); 253 double[] geoidHeightsMeters = mGeoidMap.readGeoidHeights(geoidHeightParams, mapCells); 254 if (geoidHeightsMeters == null) { 255 return false; 256 } 257 258 addMslAltitude(geoidHeightParams, geoidHeightsMeters, location); 259 return true; 260 } 261 262 /** 263 * Returns the geoid height (a.k.a. geoid undulation) at the location specified in {@code 264 * request}. The geoid height at a location is defined as the difference between an altitude 265 * measured above the World Geodetic System 1984 reference ellipsoid (WGS84) and its 266 * corresponding Mean Sea Level altitude. 267 * 268 * <p>Must be called off the main thread as data may be loaded from raw assets. 269 * 270 * @throws IOException if an I/O error occurs when loading data from raw assets. 271 * @throws IllegalArgumentException if the {@code request} has an invalid latitude or longitude. 272 * Specifically, the latitude must be between -90 and 90 (both 273 * inclusive), and the longitude must be between -180 and 180 274 * (both inclusive). 275 * @hide 276 */ 277 @WorkerThread getGeoidHeight(@onNull Context context, @NonNull GetGeoidHeightRequest request)278 public @NonNull GetGeoidHeightResponse getGeoidHeight(@NonNull Context context, 279 @NonNull GetGeoidHeightRequest request) throws IOException { 280 // Create a valid location from which the geoid height and its accuracy will be extracted. 281 Location location = new Location(""); 282 location.setLatitude(request.latitudeDegrees); 283 location.setLongitude(request.longitudeDegrees); 284 location.setAltitude(0.0); 285 location.setVerticalAccuracyMeters(0.0f); 286 287 addMslAltitudeToLocation(context, location); 288 // The geoid height for a location with zero WGS84 altitude is equal in value to the 289 // negative of corresponding MSL altitude. 290 double geoidHeightMeters = -location.getMslAltitudeMeters(); 291 // The geoid height error for a location with zero vertical accuracy is equal in value to 292 // the corresponding MSL altitude accuracy. 293 float geoidHeightErrorMeters = location.getMslAltitudeAccuracyMeters(); 294 295 MapParamsProto expirationDistanceParams = GeoidMap.getExpirationDistanceParams(context); 296 long s2CellId = S2CellIdUtils.fromLatLngDegrees(location.getLatitude(), 297 location.getLongitude()); 298 long[] mapCell = {S2CellIdUtils.getParent(s2CellId, expirationDistanceParams.mapS2Level)}; 299 double expirationDistanceMeters = mGeoidMap.readExpirationDistances( 300 expirationDistanceParams, context, mapCell)[0]; 301 float additionalGeoidHeightErrorMeters = (float) expirationDistanceParams.modelRmseMeters; 302 303 GetGeoidHeightResponse response = new GetGeoidHeightResponse(); 304 response.geoidHeightMeters = geoidHeightMeters; 305 response.geoidHeightErrorMeters = geoidHeightErrorMeters; 306 response.expirationDistanceMeters = expirationDistanceMeters; 307 response.additionalGeoidHeightErrorMeters = additionalGeoidHeightErrorMeters; 308 response.success = true; 309 return response; 310 } 311 } 312