From c3a117b284158bd014ea821446f8e16b9b1c9f29 Mon Sep 17 00:00:00 2001
From: Luc Maisonobe <luc@orekit.org>
Date: Mon, 12 May 2014 18:00:30 +0200
Subject: [PATCH] Improved inverse localization speed again!
---
.../java/org/orekit/rugged/api/Rugged.java | 46 ++-
.../rugged/api/SensorMeanPlaneCrossing.java | 338 +++++++++++-------
.../org/orekit/rugged/api/RuggedTest.java | 5 +-
3 files changed, 236 insertions(+), 153 deletions(-)
diff --git a/src/main/java/org/orekit/rugged/api/Rugged.java b/src/main/java/org/orekit/rugged/api/Rugged.java
index db7fa8bd..b6d841f4 100644
--- a/src/main/java/org/orekit/rugged/api/Rugged.java
+++ b/src/main/java/org/orekit/rugged/api/Rugged.java
@@ -77,6 +77,9 @@ public class Rugged {
/** Sensors. */
private final Map<String, LineSensor> sensors;
+ /** Mean plane crossing finders. */
+ private final Map<String, SensorMeanPlaneCrossing> finders;
+
/** DEM intersection algorithm. */
private final IntersectionAlgorithm algorithm;
@@ -229,6 +232,8 @@ public class Rugged {
this.algorithm = selectAlgorithm(algorithmID, updater, maxCachedTiles);
this.sensors = new HashMap<String, LineSensor>();
+ this.finders = new HashMap<String, SensorMeanPlaneCrossing>();
+
setLightTimeCorrection(true);
setAberrationOfLightCorrection(true);
@@ -565,7 +570,7 @@ public class Rugged {
*/
public SensorPixel inverseLocalization(final String sensorName,
final double latitude, final double longitude,
- final double minLine, final double maxLine)
+ final int minLine, final int maxLine)
throws RuggedException {
final GeodeticPoint groundPoint =
new GeodeticPoint(latitude, longitude, algorithm.getElevation(latitude, longitude));
@@ -582,26 +587,37 @@ public class Rugged {
* @exception RuggedException if line cannot be localized, or sensor is unknown
*/
public SensorPixel inverseLocalization(final String sensorName, final GeodeticPoint point,
- final double minLine, final double maxLine)
+ final int minLine, final int maxLine)
throws RuggedException {
final LineSensor sensor = getLineSensor(sensorName);
- final Vector3D target = ellipsoid.transform(point);
+ SensorMeanPlaneCrossing planeCrossing = finders.get(sensorName);
+ if (planeCrossing == null ||
+ planeCrossing.getMinLine() != minLine ||
+ planeCrossing.getMaxLine() != maxLine) {
+
+ // create a new finder for the specified sensor and range
+ planeCrossing = new SensorMeanPlaneCrossing(sensor, scToBody, minLine, maxLine,
+ lightTimeCorrection, aberrationOfLightCorrection,
+ MAX_EVAL, COARSE_INVERSE_LOCALIZATION_ACCURACY);
+
+ // store the finder, in order to reuse it
+ // (and save some computation done in its constructor)
+ finders.put(sensorName, planeCrossing);
+
+ }
// find approximately the sensor line at which ground point crosses sensor mean plane
- final SensorMeanPlaneCrossing planeCrossing =
- new SensorMeanPlaneCrossing(sensor, target, scToBody,
- lightTimeCorrection, aberrationOfLightCorrection,
- MAX_EVAL, COARSE_INVERSE_LOCALIZATION_ACCURACY);
- if (!planeCrossing.find(minLine, maxLine)) {
+ final Vector3D target = ellipsoid.transform(point);
+ final SensorMeanPlaneCrossing.CrossingResult crossingResult = planeCrossing.find(target);
+ if (crossingResult == null) {
// target is out of search interval
return null;
}
- final FieldVector3D<DerivativeStructure> coarseDirection = planeCrossing.getTargetDirection();
// find approximately the pixel along this sensor line
final SensorPixelCrossing pixelCrossing =
- new SensorPixelCrossing(sensor, coarseDirection.toVector3D(),
+ new SensorPixelCrossing(sensor, crossingResult.getTargetDirection().toVector3D(),
MAX_EVAL, COARSE_INVERSE_LOCALIZATION_ACCURACY);
final double coarsePixel = pixelCrossing.locatePixel();
if (Double.isNaN(coarsePixel)) {
@@ -612,12 +628,12 @@ public class Rugged {
// fix line by considering the closest pixel exact position and line-of-sight
// (this pixel might point towards a direction slightly above or below the mean sensor plane)
final int closestIndex = (int) FastMath.rint(coarsePixel);
- final DerivativeStructure beta = FieldVector3D.angle(coarseDirection, sensor.getMeanPlaneNormal());
+ final DerivativeStructure beta = FieldVector3D.angle(crossingResult.getTargetDirection(), sensor.getMeanPlaneNormal());
final double deltaL = (0.5 * FastMath.PI - beta.getValue()) / beta.getPartialDerivative(1);
- final double fixedLine = planeCrossing.getLine() + deltaL;
- final Vector3D fixedDirection = new Vector3D(coarseDirection.getX().taylor(deltaL),
- coarseDirection.getY().taylor(deltaL),
- coarseDirection.getZ().taylor(deltaL)).normalize();
+ final double fixedLine = crossingResult.getLine() + deltaL;
+ final Vector3D fixedDirection = new Vector3D(crossingResult.getTargetDirection().getX().taylor(deltaL),
+ crossingResult.getTargetDirection().getY().taylor(deltaL),
+ crossingResult.getTargetDirection().getZ().taylor(deltaL)).normalize();
// fix pixel
final double alpha = sensor.getAzimuth(fixedDirection, closestIndex);
diff --git a/src/main/java/org/orekit/rugged/api/SensorMeanPlaneCrossing.java b/src/main/java/org/orekit/rugged/api/SensorMeanPlaneCrossing.java
index 4c05fd76..cc7e6c20 100644
--- a/src/main/java/org/orekit/rugged/api/SensorMeanPlaneCrossing.java
+++ b/src/main/java/org/orekit/rugged/api/SensorMeanPlaneCrossing.java
@@ -33,192 +33,262 @@ class SensorMeanPlaneCrossing {
/** Converter between spacecraft and body. */
private final SpacecraftToObservedBody scToBody;
+ /** Minimum line number in the search interval. */
+ private final int minLine;
+
+ /** Maximum line number in the search interval. */
+ private final int maxLine;
+
/** Flag for light time correction. */
- private boolean lightTimeCorrection;
+ private final boolean lightTimeCorrection;
/** Flag for aberration of light correction. */
- private boolean aberrationOfLightCorrection;
+ private final boolean aberrationOfLightCorrection;
/** Line sensor. */
private final LineSensor sensor;
- /** Target ground point. */
- private final PVCoordinates target;
-
/** Maximum number of evaluations. */
private final int maxEval;
/** Accuracy to use for finding crossing line number. */
private final double accuracy;
- /** Crossing line. */
- private double crossingLine;
+ /** Middle line. */
+ private final double midLine;
- /** Target direction in spacecraft frame. */
- private FieldVector3D<DerivativeStructure> targetDirection;
+ /** Transform from observed body to inertial frame, for middle line. */
+ private final Transform midLineBodyToInert;
+
+ /** Transform from inertial frame to spacecraft frame, for middle line. */
+ private final Transform midLineScToInert;
/** Simple constructor.
* @param sensor sensor to consider
- * @param target target ground point
* @param scToBody converter between spacecraft and body
+ * @param minLine minimum line number
+ * @param maxLine maximum line number
* @param lightTimeCorrection flag for light time correction
* @param aberrationOfLightCorrection flag for aberration of light correction.
* @param maxEval maximum number of evaluations
* @param accuracy accuracy to use for finding crossing line number
+ * @exception RuggedException if some frame conversion fails
*/
- public SensorMeanPlaneCrossing(final LineSensor sensor, final Vector3D target,
+ public SensorMeanPlaneCrossing(final LineSensor sensor,
final SpacecraftToObservedBody scToBody,
+ final int minLine, final int maxLine,
final boolean lightTimeCorrection,
final boolean aberrationOfLightCorrection,
- final int maxEval, final double accuracy) {
- this.sensor = sensor;
- this.target = new PVCoordinates(target, Vector3D.ZERO);
- this.scToBody = scToBody;
- this.lightTimeCorrection = lightTimeCorrection;
- this.aberrationOfLightCorrection = aberrationOfLightCorrection;
- this.accuracy = accuracy;
- this.maxEval = maxEval;
+ final int maxEval, final double accuracy)
+ throws RuggedException {
+ try {
+
+ this.sensor = sensor;
+ this.scToBody = scToBody;
+ this.minLine = minLine;
+ this.maxLine = maxLine;
+ this.lightTimeCorrection = lightTimeCorrection;
+ this.aberrationOfLightCorrection = aberrationOfLightCorrection;
+ this.maxEval = maxEval;
+ this.accuracy = accuracy;
+
+ // compute one the transforms for middle line, as they will be reused
+ // as the start point for all searches
+ midLine = 0.5 * (minLine + maxLine);
+ final AbsoluteDate date = sensor.getDate(midLine);
+ midLineBodyToInert = scToBody.getInertialToBody(date).getInverse();
+ midLineScToInert = scToBody.getScToInertial(date);
+
+ } catch (OrekitException oe) {
+ throw new RuggedException(oe, oe.getSpecifier(), oe.getParts());
+ }
+ }
+
+ /** Get the minimum line number in the search interval.
+ * @return minimum line number in the search interval
+ */
+ public int getMinLine() {
+ return minLine;
+ }
+
+ /** Get the maximum line number in the search interval.
+ * @return maximum line number in the search interval
+ */
+ public int getMaxLine() {
+ return maxLine;
+ }
+
+ /** Container for mean plane crossing result. */
+ public static class CrossingResult {
+
+ /** Crossing line. */
+ private final double crossingLine;
+
+ /** Target direction in spacecraft frame. */
+ private final FieldVector3D<DerivativeStructure> targetDirection;
+
+ /** Simple constructor.
+ * @param crossingLine crossing line
+ * @param targetDirection target direction in spacecraft frame
+ */
+ private CrossingResult(final double crossingLine,
+ final FieldVector3D<DerivativeStructure> targetDirection) {
+ this.crossingLine = crossingLine;
+ this.targetDirection = targetDirection;
+ }
+
+ /** Get the crossing line.
+ * @return crossing line
+ */
+ public double getLine() {
+ return crossingLine;
+ }
+
+ /** Get the normalized target direction in spacecraft frame at crossing.
+ * @return normalized target direction in spacecraft frame at crossing, with its first derivative
+ * with respect to line number
+ */
+ public FieldVector3D<DerivativeStructure> getTargetDirection() {
+ return targetDirection;
+ }
+
}
/** Find mean plane crossing.
- * @param minLine minimum line number
- * @param maxLine maximum line number
- * @return true if a solution has been found in the search interval,
- * false if search interval does not bracket a solution
+ * @param target target ground point
+ * @return line number and target direction at mean plane crossing,
+ * or null if search interval does not bracket a solution
* @exception RuggedException if geometry cannot be computed for some line or
* if the maximum number of evaluations is exceeded
*/
- public boolean find(final double minLine, final double maxLine)
+ public CrossingResult find(final Vector3D target)
throws RuggedException {
+ try {
- // we don't use an Apache Commons Math solver here because we are more
- // interested in reducing the number of evaluations than being accurate,
- // as we know the solution is improved in the second stage of inverse localization.
- // We expect two or three evaluations only. Each new evaluation shows up quickly in
- // the performances as it involves frames conversions
- crossingLine = 0.5 * (minLine + maxLine);
- boolean atMin = false;
- boolean atMax = false;
- for (int i = 0; i < maxEval; ++i) {
- targetDirection = evaluateLine(crossingLine);
- final DerivativeStructure beta = FieldVector3D.angle(targetDirection, sensor.getMeanPlaneNormal());
- final double deltaL = (0.5 * FastMath.PI - beta.getValue()) / beta.getPartialDerivative(1);
- if (FastMath.abs(deltaL) <= accuracy) {
- // return immediately, without doing any additional evaluation!
- return true;
- }
- crossingLine += deltaL;
- if (crossingLine < minLine) {
- if (atMin) {
- // we were already trying at minLine and we need to go below that
- // give up as the solution is out of search interval
- return false;
- }
- atMin = true;
- crossingLine = minLine;
- } else if (crossingLine > maxLine) {
- if (atMax) {
- // we were already trying at maxLine and we need to go above that
- // give up as the solution is out of search interval
- return false;
+ final PVCoordinates targetPV = new PVCoordinates(target, Vector3D.ZERO);
+
+ // we don't use an Apache Commons Math solver here because we are more
+ // interested in reducing the number of evaluations than being accurate,
+ // as we know the solution is improved in the second stage of inverse localization.
+ // We expect two or three evaluations only. Each new evaluation shows up quickly in
+ // the performances as it involves frames conversions
+ double crossingLine = midLine;
+ Transform bodyToInert = midLineBodyToInert;
+ Transform scToInert = midLineScToInert;
+ boolean atMin = false;
+ boolean atMax = false;
+ for (int i = 0; i < maxEval; ++i) {
+
+ final FieldVector3D<DerivativeStructure> targetDirection =
+ evaluateLine(crossingLine, targetPV, bodyToInert, scToInert);
+ final DerivativeStructure beta = FieldVector3D.angle(targetDirection, sensor.getMeanPlaneNormal());
+
+ final double deltaL = (0.5 * FastMath.PI - beta.getValue()) / beta.getPartialDerivative(1);
+ if (FastMath.abs(deltaL) <= accuracy) {
+ // return immediately, without doing any additional evaluation!
+ return new CrossingResult(crossingLine, targetDirection);
}
- atMax = true;
- crossingLine = maxLine;
- } else {
- // the next evaluation will be a regular point
- atMin = false;
- atMax = false;
- }
- }
+ crossingLine += deltaL;
- return false;
+ if (crossingLine < minLine) {
+ if (atMin) {
+ // we were already trying at minLine and we need to go below that
+ // give up as the solution is out of search interval
+ return null;
+ }
+ atMin = true;
+ crossingLine = minLine;
+ } else if (crossingLine > maxLine) {
+ if (atMax) {
+ // we were already trying at maxLine and we need to go above that
+ // give up as the solution is out of search interval
+ return null;
+ }
+ atMax = true;
+ crossingLine = maxLine;
+ } else {
+ // the next evaluation will be a regular point
+ atMin = false;
+ atMax = false;
+ }
- }
+ final AbsoluteDate date = sensor.getDate(crossingLine);
+ bodyToInert = scToBody.getInertialToBody(date).getInverse();
+ scToInert = scToBody.getScToInertial(date);
+ }
- /** Get the crossing line.
- * @return crossing line
- */
- public double getLine() {
- return crossingLine;
- }
+ return null;
- /** Get the normalized target direction in spacecraft frame at crossing.
- * @return normalized target direction in spacecraft frame at crossing, with its first derivative
- * with respect to line number
- */
- public FieldVector3D<DerivativeStructure> getTargetDirection() {
- return targetDirection;
+ } catch (OrekitException oe) {
+ throw new RuggedException(oe, oe.getSpecifier(), oe.getParts());
+ }
}
/** Evaluate geometry for a given line number.
* @param lineNumber current line number
+ * @param targetPV target ground point
+ * @param bodyToInert transform from observed body to inertial frame, for middle line
+ * @param scToInert transform from inertial frame to spacecraft frame, for middle line
* @return target direction in spacecraft frame, with its first derivative
* with respect to line number
- * @exception RuggedException if some frame conversion fails
*/
- public FieldVector3D<DerivativeStructure> evaluateLine(final double lineNumber)
- throws RuggedException {
- try {
+ private FieldVector3D<DerivativeStructure> evaluateLine(final double lineNumber, final PVCoordinates targetPV,
+ final Transform bodyToInert, final Transform scToInert) {
- // compute the transform between spacecraft and observed body
- final AbsoluteDate date = sensor.getDate(lineNumber);
- final Transform bodyToInert = scToBody.getInertialToBody(date).getInverse();
- final Transform scToInert = scToBody.getScToInertial(date);
- final PVCoordinates refInert = scToInert.transformPVCoordinates(new PVCoordinates(sensor.getPosition(), Vector3D.ZERO));
-
- final PVCoordinates targetInert;
- if (lightTimeCorrection) {
- // apply light time correction
- final Vector3D iT = bodyToInert.transformPosition(target.getPosition());
- final double deltaT = refInert.getPosition().distance(iT) / Constants.SPEED_OF_LIGHT;
- targetInert = bodyToInert.shiftedBy(-deltaT).transformPVCoordinates(target);
- } else {
- // don't apply light time correction
- targetInert = bodyToInert.transformPVCoordinates(target);
- }
+ // compute the transform between spacecraft and observed body
+ final PVCoordinates refInert =
+ scToInert.transformPVCoordinates(new PVCoordinates(sensor.getPosition(), Vector3D.ZERO));
+
+ final PVCoordinates targetInert;
+ if (lightTimeCorrection) {
+ // apply light time correction
+ final Vector3D iT = bodyToInert.transformPosition(targetPV.getPosition());
+ final double deltaT = refInert.getPosition().distance(iT) / Constants.SPEED_OF_LIGHT;
+ targetInert = bodyToInert.shiftedBy(-deltaT).transformPVCoordinates(targetPV);
+ } else {
+ // don't apply light time correction
+ targetInert = bodyToInert.transformPVCoordinates(targetPV);
+ }
- final PVCoordinates lInert = new PVCoordinates(refInert, targetInert);
- final Transform inertToSc = scToInert.getInverse();
- final Vector3D obsLInert;
- final Vector3D obsLInertDot;
- if (aberrationOfLightCorrection) {
-
- // apply aberration of light correction
- // as the spacecraft velocity is small with respect to speed of light,
- // we use classical velocity addition and not relativistic velocity addition
- // we have: c * lInert + vsat = k * obsLInert
- final PVCoordinates spacecraftPV = scToInert.transformPVCoordinates(PVCoordinates.ZERO);
- final Vector3D l = lInert.getPosition().normalize();
- final Vector3D lDot = normalizedDot(lInert.getPosition(), lInert.getVelocity());
- final Vector3D kObs = new Vector3D(Constants.SPEED_OF_LIGHT, l, +1.0, spacecraftPV.getVelocity());
- obsLInert = kObs.normalize();
-
- // the following derivative is computed under the assumption the spacecraft velocity
- // is constant in inertial frame ... It is obviously not true, but as this velocity
- // is very small with respect to speed of light, the error is expected to remain small
- obsLInertDot = normalizedDot(kObs, new Vector3D(Constants.SPEED_OF_LIGHT, lDot));
-
- } else {
-
- // don't apply aberration of light correction
- obsLInert = lInert.getPosition().normalize();
- obsLInertDot = normalizedDot(lInert.getPosition(), lInert.getVelocity());
+ final PVCoordinates lInert = new PVCoordinates(refInert, targetInert);
+ final Transform inertToSc = scToInert.getInverse();
+ final Vector3D obsLInert;
+ final Vector3D obsLInertDot;
+ if (aberrationOfLightCorrection) {
- }
- final Vector3D dir = inertToSc.transformVector(obsLInert);
- final Vector3D dirDot = new Vector3D(+1.0, inertToSc.transformVector(obsLInertDot),
- -1.0, Vector3D.crossProduct(inertToSc.getRotationRate(),
- dir));
+ // apply aberration of light correction
+ // as the spacecraft velocity is small with respect to speed of light,
+ // we use classical velocity addition and not relativistic velocity addition
+ // we have: c * lInert + vsat = k * obsLInert
+ final PVCoordinates spacecraftPV = scToInert.transformPVCoordinates(PVCoordinates.ZERO);
+ final Vector3D l = lInert.getPosition().normalize();
+ final Vector3D lDot = normalizedDot(lInert.getPosition(), lInert.getVelocity());
+ final Vector3D kObs = new Vector3D(Constants.SPEED_OF_LIGHT, l, +1.0, spacecraftPV.getVelocity());
+ obsLInert = kObs.normalize();
- // combine vector value and derivative
- final double rate = sensor.getRate(lineNumber);
- return new FieldVector3D<DerivativeStructure>(new DerivativeStructure(1, 1, dir.getX(), dirDot.getX() / rate),
- new DerivativeStructure(1, 1, dir.getY(), dirDot.getY() / rate),
- new DerivativeStructure(1, 1, dir.getZ(), dirDot.getZ() / rate));
+ // the following derivative is computed under the assumption the spacecraft velocity
+ // is constant in inertial frame ... It is obviously not true, but as this velocity
+ // is very small with respect to speed of light, the error is expected to remain small
+ obsLInertDot = normalizedDot(kObs, new Vector3D(Constants.SPEED_OF_LIGHT, lDot));
+
+ } else {
+
+ // don't apply aberration of light correction
+ obsLInert = lInert.getPosition().normalize();
+ obsLInertDot = normalizedDot(lInert.getPosition(), lInert.getVelocity());
- } catch (OrekitException oe) {
- throw new RuggedException(oe, oe.getSpecifier(), oe.getParts());
}
+ final Vector3D dir = inertToSc.transformVector(obsLInert);
+ final Vector3D dirDot = new Vector3D(+1.0, inertToSc.transformVector(obsLInertDot),
+ -1.0, Vector3D.crossProduct(inertToSc.getRotationRate(),
+ dir));
+
+ // combine vector value and derivative
+ final double rate = sensor.getRate(lineNumber);
+ return new FieldVector3D<DerivativeStructure>(new DerivativeStructure(1, 1, dir.getX(), dirDot.getX() / rate),
+ new DerivativeStructure(1, 1, dir.getY(), dirDot.getY() / rate),
+ new DerivativeStructure(1, 1, dir.getZ(), dirDot.getZ() / rate));
+
}
/** Compute the derivative of normalized vector.
diff --git a/src/test/java/org/orekit/rugged/api/RuggedTest.java b/src/test/java/org/orekit/rugged/api/RuggedTest.java
index e53d7e30..07f3d4d4 100644
--- a/src/test/java/org/orekit/rugged/api/RuggedTest.java
+++ b/src/test/java/org/orekit/rugged/api/RuggedTest.java
@@ -66,8 +66,6 @@ import org.orekit.propagation.analytical.KeplerianPropagator;
import org.orekit.propagation.numerical.NumericalPropagator;
import org.orekit.propagation.sampling.OrekitFixedStepHandler;
import org.orekit.rugged.core.raster.RandomLandscapeUpdater;
-import org.orekit.rugged.core.raster.SimpleTileFactory;
-import org.orekit.rugged.core.raster.Tile;
import org.orekit.rugged.core.raster.VolcanicConeElevationUpdater;
import org.orekit.time.AbsoluteDate;
import org.orekit.time.TimeScalesFactory;
@@ -433,9 +431,8 @@ public class RuggedTest {
// the following test is disabled by default
// it is only used to check timings, and also create a small (2M) temporary file
@Test
- @Ignore
public void testInverseLocalizationTiming()
- throws RuggedException, OrekitException, URISyntaxException {
+ throws RuggedException, OrekitException, URISyntaxException {
long t0 = System.currentTimeMillis();
int dimension = 1000;
--
GitLab