From c0bf80d019d19a6adbd05f7a3c2d59df4c75588b Mon Sep 17 00:00:00 2001 From: Luc Maisonobe <luc@orekit.org> Date: Fri, 25 Apr 2014 16:53:27 +0200 Subject: [PATCH] Added an option to use Duvenhage algorithm with flat-Earth hypothesis. This option is mainly intended for comparison with existing systems. It should not be used for operational systems and the full Duvenhage algorithm with line-of-sight bending in geodetic coordinates should be used instead. --- .../org/orekit/rugged/api/AlgorithmId.java | 17 +++ .../java/org/orekit/rugged/api/Rugged.java | 16 +- .../core/duvenhage/DuvenhageAlgorithm.java | 105 +++++++++---- .../org/orekit/rugged/api/RuggedTest.java | 69 +++++++++ .../duvenhage/DuvenhageAlgorithmTest.java | 2 +- .../core/raster/RandomLandscapeUpdater.java | 141 ++++++++++++++++++ 6 files changed, 316 insertions(+), 34 deletions(-) create mode 100644 src/test/java/org/orekit/rugged/core/raster/RandomLandscapeUpdater.java diff --git a/src/main/java/org/orekit/rugged/api/AlgorithmId.java b/src/main/java/org/orekit/rugged/api/AlgorithmId.java index a689d4de..3ad245be 100644 --- a/src/main/java/org/orekit/rugged/api/AlgorithmId.java +++ b/src/main/java/org/orekit/rugged/api/AlgorithmId.java @@ -31,6 +31,23 @@ public enum AlgorithmId { */ DUVENHAGE, + /** Fast algorithm due to Bernardt Duvenhage. + * <p> + * The algorithm is described in the 2009 paper: + * <a href="http://researchspace.csir.co.za/dspace/bitstream/10204/3041/1/Duvenhage_2009.pdf">Using + * An Implicit Min/Max KD-Tree for Doing Efficient Terrain Line of Sight Calculations</a>. + * </p> + * <p> + * This version of the duvenhage's algorithm considers the body to be flat, i.e. lines computed + * from entry/exit points in the Digital Elevation Model are considered to be straight lines + * also in geodetic coordinates. The sagitta resulting from real ellipsoid curvature is therefore + * <em>not</em> corrected in this case. As this computation is not costly (a few percents overhead), + * the full {@link #DUVENHAGE} is recommended instead of this one. This choice is mainly intended + * for comparison purposes with other systems. + * </p> + */ + DUVENHAGE_FLAT_BODY, + /** Basic, <em>very slow</em> algorithm, designed only for tests and validation purposes. * <p> * The algorithm simply computes entry and exit points at high and low altitudes, diff --git a/src/main/java/org/orekit/rugged/api/Rugged.java b/src/main/java/org/orekit/rugged/api/Rugged.java index a57ebaf7..403c0592 100644 --- a/src/main/java/org/orekit/rugged/api/Rugged.java +++ b/src/main/java/org/orekit/rugged/api/Rugged.java @@ -128,9 +128,8 @@ public class Rugged { * @param updater updater used to load Digital Elevation Model tiles * @param maxCachedTiles maximum number of tiles stored in the cache * @param algorithmID identifier of algorithm to use for Digital Elevation Model intersection - * @param ellipsoidID identifier of reference ellipsoid - * @param inertialFrameID identifier of inertial frame - * @param bodyRotatingFrameID identifier of body rotating frame + * @param ellipsoid reference ellipsoid + * @param inertialFrame inertial frame * @param positionsVelocities satellite position and velocity * @param pvInterpolationOrder order to use for position/velocity interpolation * @param quaternions satellite quaternions @@ -404,7 +403,7 @@ public class Rugged { } - /** Convert a {@link OneAxisEllipsoid} into a {@link ExtendedEllipsoid} + /** Convert a {@link OneAxisEllipsoid} into a {@link ExtendedEllipsoid}. * @param ellipsoid ellpsoid to extend * @return extended ellipsoid */ @@ -444,7 +443,8 @@ public class Rugged { // set up the ephemeris final List<Orbit> orbits = new ArrayList<Orbit>(positionsVelocities.size()); for (final Pair<AbsoluteDate, PVCoordinates> pv : positionsVelocities) { - final CartesianOrbit orbit = new CartesianOrbit(pv.getSecond(), inertialFrame, pv.getFirst(), Constants.EIGEN5C_EARTH_MU); + final CartesianOrbit orbit = new CartesianOrbit(pv.getSecond(), inertialFrame, + pv.getFirst(), Constants.EIGEN5C_EARTH_MU); orbits.add(orbit); } @@ -455,7 +455,7 @@ public class Rugged { /** {@inhritDoc} */ @Override public PVCoordinates getPVCoordinates(final AbsoluteDate date, final Frame f) - throws OrekitException { + throws OrekitException { final List<Orbit> sample = cache.getNeighbors(date); final Orbit interpolated = sample.get(0).interpolate(date, sample); return interpolated.getPVCoordinates(date, f); @@ -477,7 +477,9 @@ public class Rugged { // set up the algorithm switch (algorithmID) { case DUVENHAGE : - return new DuvenhageAlgorithm(updater, maxCachedTiles); + return new DuvenhageAlgorithm(updater, maxCachedTiles, false); + case DUVENHAGE_FLAT_BODY : + return new DuvenhageAlgorithm(updater, maxCachedTiles, true); case BASIC_SLOW_EXHAUSTIVE_SCAN_FOR_TESTS_ONLY : return new BasicScanAlgorithm(updater, maxCachedTiles); case IGNORE_DEM_USE_ELLIPSOID : diff --git a/src/main/java/org/orekit/rugged/core/duvenhage/DuvenhageAlgorithm.java b/src/main/java/org/orekit/rugged/core/duvenhage/DuvenhageAlgorithm.java index a1a51530..33e38ea7 100644 --- a/src/main/java/org/orekit/rugged/core/duvenhage/DuvenhageAlgorithm.java +++ b/src/main/java/org/orekit/rugged/core/duvenhage/DuvenhageAlgorithm.java @@ -44,12 +44,23 @@ public class DuvenhageAlgorithm implements IntersectionAlgorithm { /** Cache for DEM tiles. */ private final TilesCache<MinMaxTreeTile> cache; + /** Flag for flat-body hypothesis. */ + private final boolean flatBody; + /** Simple constructor. * @param updater updater used to load Digital Elevation Model tiles * @param maxCachedTiles maximum number of tiles stored in the cache + * @param flatBody if true, the body is considered flat, i.e. lines computed + * from entry/exit points in the DEM are considered to be straight lines also + * in geodetic coordinates. The sagitta resulting from real ellipsoid curvature + * is therefore <em>not</em> corrected in this case. As this computation is not + * costly (a few percents overhead), it is not recommended to set this parameter + * to {@code true}. This flag is mainly intended for comparison purposes with other systems. */ - public DuvenhageAlgorithm(final TileUpdater updater, final int maxCachedTiles) { - cache = new TilesCache<MinMaxTreeTile>(new MinMaxTreeTileFactory(), updater, maxCachedTiles); + public DuvenhageAlgorithm(final TileUpdater updater, final int maxCachedTiles, + final boolean flatBody) { + this.cache = new TilesCache<MinMaxTreeTile>(new MinMaxTreeTileFactory(), updater, maxCachedTiles); + this.flatBody = flatBody; } /** {@inheritDoc} */ @@ -174,7 +185,7 @@ public class DuvenhageAlgorithm implements IntersectionAlgorithm { // we have narrowed the search down to a single Digital Elevation Model pixel GeodeticPoint intersection = tile.pixelIntersection(entry, ellipsoid.convertLos(entry, los), exitLat, exitLon); - if (intersection != null) { + if (intersection != null && !flatBody) { // improve the point, by projecting it back on the 3D line, // fixing the small body curvature remaining at pixel level final Vector3D delta = ellipsoid.transform(intersection).subtract(position); @@ -212,20 +223,34 @@ public class DuvenhageAlgorithm implements IntersectionAlgorithm { if (longitude >= FastMath.min(entry.getLongitude(), exit.getLongitude()) && longitude <= FastMath.max(entry.getLongitude(), exit.getLongitude())) { - final Vector3D crossingP = ellipsoid.pointAtLongitude(position, los, longitude); - final GeodeticPoint crossingGP = ellipsoid.transform(crossingP, ellipsoid.getBodyFrame(), null); - final int crossingLat = tile.getLatitudeIndex(crossingGP.getLatitude()); + final GeodeticPoint crossingGP; + if (flatBody) { + // linear approximation of crossing point + final double d = exit.getLongitude() - entry.getLongitude(); + final double cN = (exit.getLongitude() - longitude) / d; + final double cX = (longitude - entry.getLongitude()) / d; + crossingGP = new GeodeticPoint(cN * entry.getLatitude() + cX * exit.getLatitude(), + longitude, + cN * entry.getAltitude() + cX * exit.getAltitude()); + } else { + // full computation of crossing point + final Vector3D crossingP = ellipsoid.pointAtLongitude(position, los, longitude); + crossingGP = ellipsoid.transform(crossingP, ellipsoid.getBodyFrame(), null); + } + final int crossingLat = tile.getLatitudeIndex(crossingGP.getLatitude()); // adjust indices as the crossing point is by definition between the sub-tiles final int crossingLonBefore = crossingLon - (entryLon <= exitLon ? 1 : 0); final int crossingLonAfter = crossingLon - (entryLon <= exitLon ? 0 : 1); - // look for intersection - final GeodeticPoint intersection = recurseIntersection(depth + 1, ellipsoid, position, los, tile, - previousGP, previousLat, previousLon, - crossingGP, crossingLat, crossingLonBefore); - if (intersection != null) { - return intersection; + if (inRange(crossingLonBefore, entryLon, exitLon)) { + // look for intersection + final GeodeticPoint intersection = recurseIntersection(depth + 1, ellipsoid, position, los, tile, + previousGP, previousLat, previousLon, + crossingGP, crossingLat, crossingLonBefore); + if (intersection != null) { + return intersection; + } } // prepare next segment @@ -246,21 +271,35 @@ public class DuvenhageAlgorithm implements IntersectionAlgorithm { if (latitude >= FastMath.min(entry.getLatitude(), exit.getLatitude()) && latitude <= FastMath.max(entry.getLatitude(), exit.getLatitude())) { - final Vector3D crossingP = ellipsoid.pointAtLatitude(position, los, - tile.getLatitudeAtIndex(crossingLat)); - final GeodeticPoint crossingGP = ellipsoid.transform(crossingP, ellipsoid.getBodyFrame(), null); - final int crossingLon = tile.getLongitudeIndex(crossingGP.getLongitude()); + final GeodeticPoint crossingGP; + if (flatBody) { + // linear approximation of crossing point + final double d = exit.getLatitude() - entry.getLatitude(); + final double cN = (exit.getLatitude() - latitude) / d; + final double cX = (latitude - entry.getLatitude()) / d; + crossingGP = new GeodeticPoint(latitude, + cN * entry.getLongitude() + cX * exit.getLongitude(), + cN * entry.getAltitude() + cX * exit.getAltitude()); + } else { + // full computation of crossing point + final Vector3D crossingP = ellipsoid.pointAtLatitude(position, los, + tile.getLatitudeAtIndex(crossingLat)); + crossingGP = ellipsoid.transform(crossingP, ellipsoid.getBodyFrame(), null); + } + final int crossingLon = tile.getLongitudeIndex(crossingGP.getLongitude()); // adjust indices as the crossing point is by definition between the sub-tiles final int crossingLatBefore = crossingLat - (entryLat <= exitLat ? 1 : 0); final int crossingLatAfter = crossingLat - (entryLat <= exitLat ? 0 : 1); - // look for intersection - final GeodeticPoint intersection = recurseIntersection(depth + 1, ellipsoid, position, los, tile, - previousGP, previousLat, previousLon, - crossingGP, crossingLatBefore, crossingLon); - if (intersection != null) { - return intersection; + if (inRange(crossingLatBefore, entryLat, exitLat)) { + // look for intersection + final GeodeticPoint intersection = recurseIntersection(depth + 1, ellipsoid, position, los, tile, + previousGP, previousLat, previousLon, + crossingGP, crossingLatBefore, crossingLon); + if (intersection != null) { + return intersection; + } } // prepare next segment @@ -273,11 +312,25 @@ public class DuvenhageAlgorithm implements IntersectionAlgorithm { } } - // last part of the segment, up to exit point - return recurseIntersection(depth + 1, ellipsoid, position, los, tile, - previousGP, previousLat, previousLon, - exit, exitLat, exitLon); + if (inRange(previousLat, entryLat, exitLat) && inRange(previousLon, entryLon, exitLon)) { + // last part of the segment, up to exit point + return recurseIntersection(depth + 1, ellipsoid, position, los, tile, + previousGP, previousLat, previousLon, + exit, exitLat, exitLon); + } else { + return null; + } + + } + /** Check if an index is inside a range. + * @param i index to check + * @param a first bound of the range (may be either below or above b) + * @param b second bound of the range (may be either below or above a) + * @return true if i is between a and b (inclusive) + */ + private boolean inRange(final int i, final int a, final int b) { + return i >= FastMath.min(a, b) && i <= FastMath.max(a, b); } /** Compute a line-of-sight exit point from a tile. diff --git a/src/test/java/org/orekit/rugged/api/RuggedTest.java b/src/test/java/org/orekit/rugged/api/RuggedTest.java index c9126563..741bfd09 100644 --- a/src/test/java/org/orekit/rugged/api/RuggedTest.java +++ b/src/test/java/org/orekit/rugged/api/RuggedTest.java @@ -31,6 +31,7 @@ import java.util.Locale; import org.apache.commons.math3.geometry.euclidean.threed.Rotation; import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; import org.apache.commons.math3.ode.nonstiff.DormandPrince853Integrator; +import org.apache.commons.math3.stat.descriptive.SummaryStatistics; import org.apache.commons.math3.util.FastMath; import org.apache.commons.math3.util.Pair; import org.junit.Assert; @@ -61,6 +62,7 @@ import org.orekit.propagation.Propagator; import org.orekit.propagation.SpacecraftState; import org.orekit.propagation.analytical.KeplerianPropagator; import org.orekit.propagation.numerical.NumericalPropagator; +import org.orekit.rugged.core.raster.RandomLandscapeUpdater; import org.orekit.rugged.core.raster.VolcanicConeElevationUpdater; import org.orekit.time.AbsoluteDate; import org.orekit.time.TimeScalesFactory; @@ -387,6 +389,73 @@ public class RuggedTest { } + @Test + public void testFlatBodyCorrection() + throws RuggedException, OrekitException, URISyntaxException { + + int dimension = 200; + + String path = getClass().getClassLoader().getResource("orekit-data").toURI().getPath(); + DataProvidersManager.getInstance().addProvider(new DirectoryCrawler(new File(path))); + final BodyShape earth = createEarth(); + final Orbit orbit = createOrbit(Constants.EIGEN5C_EARTH_MU); + + AbsoluteDate crossing = new AbsoluteDate("2012-01-01T12:30:00.000", TimeScalesFactory.getUTC()); + + // one line sensor + // position: 1.5m in front (+X) and 20 cm above (-Z) of the S/C center of mass + // los: swath in the (YZ) plane, looking at 50° roll, ±1° aperture + List<PixelLOS> los = createLOS(new Vector3D(1.5, 0, -0.2), + new Rotation(Vector3D.PLUS_I, FastMath.toRadians(50.0)).applyTo(Vector3D.PLUS_K), + Vector3D.PLUS_I, + FastMath.toRadians(1.0), dimension); + + // linear datation model: at reference time we get line 100, and the rate is one line every 1.5ms + LineDatation lineDatation = new LinearLineDatation(dimension / 2, 1.0 / 1.5e-3); + int firstLine = 0; + int lastLine = dimension; + + Propagator propagator = new KeplerianPropagator(orbit); + propagator.setAttitudeProvider(new YawCompensation(new NadirPointing(earth))); + propagator.propagate(crossing.shiftedBy(lineDatation.getDate(firstLine) - 1.0)); + propagator.setEphemerisMode(); + propagator.propagate(crossing.shiftedBy(lineDatation.getDate(lastLine) + 1.0)); + Propagator ephemeris = propagator.getGeneratedEphemeris(); + + TileUpdater updater = + new RandomLandscapeUpdater(0.0, 9000.0, 0.5, 0xf0a401650191f9f6l, + FastMath.toRadians(1.0), 257); + + Rugged ruggedFull = new Rugged(crossing, updater, 8, + AlgorithmId.DUVENHAGE, + EllipsoidId.WGS84, + InertialFrameId.EME2000, + BodyRotatingFrameId.ITRF, + ephemeris); + ruggedFull.setLineSensor("line", los, lineDatation); + GeodeticPoint[] gpWithFlatBodyCorrection = ruggedFull.directLocalization("line", 100); + + Rugged ruggedFlat = new Rugged(crossing, updater, 8, + AlgorithmId.DUVENHAGE_FLAT_BODY, + EllipsoidId.WGS84, + InertialFrameId.EME2000, + BodyRotatingFrameId.ITRF, + ephemeris); + ruggedFlat.setLineSensor("line", los, lineDatation); + GeodeticPoint[] gpWithoutFlatBodyCorrection = ruggedFlat.directLocalization("line", 100); + + SummaryStatistics stats = new SummaryStatistics(); + for (int i = 0; i < gpWithFlatBodyCorrection.length; ++i) { + Vector3D pWith = earth.transform(gpWithFlatBodyCorrection[i]); + Vector3D pWithout = earth.transform(gpWithoutFlatBodyCorrection[i]); + stats.addValue(Vector3D.distance(pWith, pWithout)); + } + Assert.assertEquals(0.005, stats.getMin(), 1.0e-3); + Assert.assertEquals(6.503, stats.getMax(), 1.0e-3); + Assert.assertEquals(2.199, stats.getMean(), 1.0e-3); + + } + private BodyShape createEarth() throws OrekitException { return new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS, diff --git a/src/test/java/org/orekit/rugged/core/duvenhage/DuvenhageAlgorithmTest.java b/src/test/java/org/orekit/rugged/core/duvenhage/DuvenhageAlgorithmTest.java index 2d0d4045..c7946d17 100644 --- a/src/test/java/org/orekit/rugged/core/duvenhage/DuvenhageAlgorithmTest.java +++ b/src/test/java/org/orekit/rugged/core/duvenhage/DuvenhageAlgorithmTest.java @@ -29,7 +29,7 @@ import org.orekit.rugged.core.raster.IntersectionAlgorithm; public class DuvenhageAlgorithmTest extends AbstractAlgorithmTest { protected IntersectionAlgorithm createAlgorithm(final TileUpdater updater, final int maxCachedTiles) { - return new DuvenhageAlgorithm(updater, maxCachedTiles); + return new DuvenhageAlgorithm(updater, maxCachedTiles, false); } @Test diff --git a/src/test/java/org/orekit/rugged/core/raster/RandomLandscapeUpdater.java b/src/test/java/org/orekit/rugged/core/raster/RandomLandscapeUpdater.java new file mode 100644 index 00000000..05efab82 --- /dev/null +++ b/src/test/java/org/orekit/rugged/core/raster/RandomLandscapeUpdater.java @@ -0,0 +1,141 @@ +/* Copyright 2013-2014 CS Systèmes d'Information + * Licensed to CS Systèmes d'Information (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package org.orekit.rugged.core.raster; + +import java.io.IOException; + +import org.apache.commons.math3.exception.MathIllegalArgumentException; +import org.apache.commons.math3.exception.util.LocalizedFormats; +import org.apache.commons.math3.random.RandomGenerator; +import org.apache.commons.math3.random.Well19937a; +import org.apache.commons.math3.util.ArithmeticUtils; +import org.apache.commons.math3.util.FastMath; +import org.orekit.rugged.api.RuggedException; +import org.orekit.rugged.api.TileUpdater; +import org.orekit.rugged.api.UpdatableTile; + +public class RandomLandscapeUpdater implements TileUpdater { + + private double size; + private int n; + private double[][] heightMap; + + public RandomLandscapeUpdater(double baseH, double initialScale, double reductionFactor, + long seed, double size, int n) + throws MathIllegalArgumentException { + + if (!ArithmeticUtils.isPowerOfTwo(n - 1)) { + throw new MathIllegalArgumentException(LocalizedFormats.SIMPLE_MESSAGE, + "tile size must be a power of two plus one"); + } + + this.size = size; + this.n = n; + + // as we want to use this for testing and comparison purposes, + // and as we don't control when tiles are generated, we generate + // only ONCE a height map with continuity at the edges, and + // reuse this single height map for ALL generated tiles + heightMap = new double[n][n]; + RandomGenerator random = new Well19937a(seed); + + // initialize corners in diamond-square algorithm + heightMap[0][0] = baseH; + heightMap[0][n - 1] = baseH; + heightMap[n - 1][0] = baseH; + heightMap[n - 1][n - 1] = baseH; + + double scale = initialScale; + for (int span = n - 1; span > 1; span = span / 2) { + + // perform squares step + for (int i = span / 2; i < n; i += span) { + for (int j = span / 2; j < n; j += span) { + double middleH = mean(i - span / 2, j - span / 2, + i - span / 2, j + span / 2, + i + span / 2, j - span / 2, + i + span / 2, j + span / 2) + + scale * (random.nextDouble() - 0.5); + heightMap[i][j] = middleH; + } + } + + // perform diamonds step + boolean flipFlop = false; + for (int i = 0; i < n - 1; i += span / 2) { + for (int j = flipFlop ? 0 : span / 2; j < n - 1; j += span) { + double middleH = mean(i - span / 2, j, + i + span / 2, j, + i, j - span / 2, + i, j + span / 2) + + scale * (random.nextDouble() - 0.5); + heightMap[i][j] = middleH; + if (i == 0) { + heightMap[n - 1][j] = middleH; + } + if (j == 0) { + heightMap[i][n - 1] = middleH; + } + } + flipFlop = !flipFlop; + } + + // reduce scale + scale *= reductionFactor; + + } + + try { + java.io.PrintStream out = new java.io.PrintStream("/home/luc/x.dat"); + for (int i = 0; i < n; ++i) { + for (int j = 0; j < n; ++j) { + out.format(java.util.Locale.US, "%d %d %12.3f%n", + i, j, heightMap[i][j]); + } + out.format("%n"); + } + out.close(); + } catch (IOException ioe) { + ioe.printStackTrace(System.err); + System.exit(1); + } + + } + + public void updateTile(double latitude, double longitude, UpdatableTile tile) + throws RuggedException { + + double step = size / (n - 1); + double minLatitude = size * FastMath.floor(latitude / size); + double minLongitude = size * FastMath.floor(longitude / size); + tile.setGeometry(minLatitude, minLongitude, step, step, n, n); + for (int i = 0; i < n; ++i) { + for (int j = 0; j < n; ++j) { + tile.setElevation(i, j, heightMap[i][j]); + } + } + + } + + private double mean(int i1, int j1, int i2, int j2, int i3, int j3, int i4, int j4) { + return (heightMap[(i1 + n) % n][(j1 + n) % n] + + heightMap[(i2 + n) % n][(j2 + n) % n] + + heightMap[(i3 + n) % n][(j3 + n) % n] + + heightMap[(i4 + n) % n][(j4 + n) % n]) / 4; + } + +} -- GitLab