From ff85e44f3ff3164735315e8d2cd1c3621762abaa Mon Sep 17 00:00:00 2001
From: Luc Maisonobe <luc@orekit.org>
Date: Wed, 7 May 2014 16:09:24 +0200
Subject: [PATCH] Work In Progress on a faster inverse localization algorithm.
---
.../org/orekit/rugged/api/LineSensor.java | 116 ++++++-
.../java/org/orekit/rugged/api/Rugged.java | 312 +++---------------
.../rugged/api/SensorMeanPlaneCrossing.java | 237 +++++++++++++
.../rugged/api/SensorPixelCrossing.java | 107 ++++++
.../org/orekit/rugged/api/RuggedTest.java | 26 +-
5 files changed, 510 insertions(+), 288 deletions(-)
create mode 100644 src/main/java/org/orekit/rugged/api/SensorMeanPlaneCrossing.java
create mode 100644 src/main/java/org/orekit/rugged/api/SensorPixelCrossing.java
diff --git a/src/main/java/org/orekit/rugged/api/LineSensor.java b/src/main/java/org/orekit/rugged/api/LineSensor.java
index 0355dfb1..907b7350 100644
--- a/src/main/java/org/orekit/rugged/api/LineSensor.java
+++ b/src/main/java/org/orekit/rugged/api/LineSensor.java
@@ -22,6 +22,7 @@ import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
import org.apache.commons.math3.linear.MatrixUtils;
import org.apache.commons.math3.linear.RealMatrix;
import org.apache.commons.math3.linear.SingularValueDecomposition;
+import org.apache.commons.math3.util.FastMath;
import org.orekit.time.AbsoluteDate;
/** Line sensor model.
@@ -42,7 +43,16 @@ public class LineSensor {
private final Vector3D position;
/** Pixels lines-of-sight. */
- private final List<Vector3D> los;
+ private final Vector3D[] x;
+
+ /** Pixels transversal direction (i.e. towards left pixel). */
+ private final Vector3D[] y;
+
+ /** Pixels cross direction (i.e. above line-of-sight). */
+ private final Vector3D[] z;
+
+ /** Pixels widths. */
+ private final double[] width;
/** Mean plane normal. */
private final Vector3D normal;
@@ -59,7 +69,12 @@ public class LineSensor {
this.name = name;
this.datationModel = datationModel;
this.position = position;
- this.los = los;
+
+ // normalize lines-of-sight
+ this.x = new Vector3D[los.size()];
+ for (int i = 0; i < los.size(); ++i) {
+ x[i] = los.get(i).normalize();
+ }
// we consider the viewing directions as a point cloud
// and want to find the plane that best fits it
@@ -68,20 +83,20 @@ public class LineSensor {
double centroidX = 0;
double centroidY = 0;
double centroidZ = 0;
- for (int i = 0; i < los.size(); ++i) {
- final Vector3D l = los.get(i);
+ for (int i = 0; i < x.length; ++i) {
+ final Vector3D l = x[i];
centroidX += l.getX();
centroidY += l.getY();
centroidZ += l.getZ();
}
- centroidX /= los.size();
- centroidY /= los.size();
- centroidZ /= los.size();
+ centroidX /= x.length;
+ centroidY /= x.length;
+ centroidZ /= x.length;
// build a centered data matrix
final RealMatrix matrix = MatrixUtils.createRealMatrix(3, los.size());
- for (int i = 0; i < los.size(); ++i) {
- final Vector3D l = los.get(i);
+ for (int i = 0; i < x.length; ++i) {
+ final Vector3D l = x[i];
matrix.setEntry(0, i, l.getX() - centroidX);
matrix.setEntry(1, i, l.getY() - centroidY);
matrix.setEntry(2, i, l.getZ() - centroidZ);
@@ -102,6 +117,26 @@ public class LineSensor {
normal = singularVector.negate();
}
+ // compute transversal directions
+ y = new Vector3D[x.length];
+ z = new Vector3D[x.length];
+ for (int i = 0; i < x.length; ++i) {
+ y[i] = Vector3D.crossProduct(normal, x[i]).normalize();
+ z[i] = Vector3D.crossProduct(x[i], y[i]);
+ }
+
+ // compute pixel widths
+ width = new double[x.length];
+ for (int i = 0; i < x.length; ++i) {
+ if (i < 1) {
+ width[i] = getAzimuth(los.get(i + 1), i);
+ } else if (i > x.length - 2) {
+ width[i] = -getAzimuth(los.get(i - 1), i);
+ } else {
+ width[i] = 0.5 * (getAzimuth(los.get(i + 1), i) - getAzimuth(los.get(i - 1), i));
+ }
+ }
+
}
/** Get the name of the sensor.
@@ -115,15 +150,52 @@ public class LineSensor {
* @return number of pixels
*/
public int getNbPixels() {
- return los.size();
+ return x.length;
}
- /** Get the pixel line-of-sight.
+ /** Get the pixel normalized line-of-sight.
+ * <p>
+ * The {@link #getLos(int) line-of-sight}, {@link #getTransversal(int) transversal}
+ * and {@link #getCross(int) cross} directions form a right-handed frame aligned
+ * with the pixel.
+ * </p>
* @param i pixel index (must be between 0 and {@link #getNbPixels()}
- * @return pixel line-of-sight
+ * @return pixel normalized line-of-sight
*/
public Vector3D getLos(final int i) {
- return los.get(i);
+ return x[i];
+ }
+
+ /** Get the pixel normalized transversal direction.
+ * <p>
+ * The transversal direction is towards left pixel.
+ * </p>
+ * <p>
+ * The {@link #getLos(int) line-of-sight}, {@link #getTransversal(int) transversal}
+ * and {@link #getCross(int) cross} directions form a right-handed frame aligned
+ * with the pixel.
+ * </p>
+ * @param i pixel index (must be between 0 and {@link #getNbPixels()}
+ * @return pixel normalized transversal direction
+ */
+ public Vector3D getTransversal(final int i) {
+ return y[i];
+ }
+
+ /** Get the pixel normalized cross direction.
+ * <p>
+ * The cross direction is above sensor lines.
+ * </p>
+ * <p>
+ * The {@link #getLos(int) line-of-sight}, {@link #getTransversal(int) transversal}
+ * and {@link #getCross(int) cross} directions form a right-handed frame aligned
+ * with the pixel.
+ * </p>
+ * @param i pixel index (must be between 0 and {@link #getNbPixels()}
+ * @return pixel normalized cross direction
+ */
+ public Vector3D getCross(final int i) {
+ return z[i];
}
/** Get the date.
@@ -161,4 +233,22 @@ public class LineSensor {
return position;
}
+ /** Get the relative azimuth of a direction with respect to a pixel.
+ * @param direction direction to check
+ * @param i pixel index (must be between 0 and {@link #getNbPixels()}
+ * @return relative azimuth of direction
+ */
+ public double getAzimuth(final Vector3D direction, final int i) {
+ return FastMath.atan2(Vector3D.dotProduct(direction, y[i]),
+ Vector3D.dotProduct(direction, x[i]));
+ }
+
+ /** Get the the angular width a pixel.
+ * @param i pixel index (must be between 0 and {@link #getNbPixels()}
+ * @return relative azimuth of direction
+ */
+ public double getWidth(final int i) {
+ return width[i];
+ }
+
}
diff --git a/src/main/java/org/orekit/rugged/api/Rugged.java b/src/main/java/org/orekit/rugged/api/Rugged.java
index 648bde54..6c692c61 100644
--- a/src/main/java/org/orekit/rugged/api/Rugged.java
+++ b/src/main/java/org/orekit/rugged/api/Rugged.java
@@ -21,23 +21,16 @@ import java.util.HashMap;
import java.util.List;
import java.util.Map;
-import org.apache.commons.math3.analysis.UnivariateFunction;
-import org.apache.commons.math3.analysis.solvers.BracketingNthOrderBrentSolver;
-import org.apache.commons.math3.analysis.solvers.UnivariateSolver;
-import org.apache.commons.math3.exception.NoBracketingException;
-import org.apache.commons.math3.exception.TooManyEvaluationsException;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
import org.apache.commons.math3.util.FastMath;
import org.apache.commons.math3.util.Pair;
-import org.apache.commons.math3.util.Precision;
import org.orekit.attitudes.Attitude;
import org.orekit.attitudes.AttitudeProvider;
import org.orekit.attitudes.TabulatedProvider;
import org.orekit.bodies.GeodeticPoint;
import org.orekit.bodies.OneAxisEllipsoid;
import org.orekit.errors.OrekitException;
-import org.orekit.errors.OrekitExceptionWrapper;
import org.orekit.frames.Frame;
import org.orekit.frames.FramesFactory;
import org.orekit.frames.Transform;
@@ -46,7 +39,6 @@ import org.orekit.orbits.Orbit;
import org.orekit.propagation.Propagator;
import org.orekit.rugged.core.BasicScanAlgorithm;
import org.orekit.rugged.core.ExtendedEllipsoid;
-import org.orekit.rugged.core.FixedAltitudeAlgorithm;
import org.orekit.rugged.core.IgnoreDEMAlgorithm;
import org.orekit.rugged.core.SpacecraftToObservedBody;
import org.orekit.rugged.core.duvenhage.DuvenhageAlgorithm;
@@ -66,14 +58,13 @@ public class Rugged {
/** Accuracy to use in the first stage of inverse localization.
* <p>
* This accuracy is only used to locate the point within one
- * pixel, so four neighboring corners can be estimated. Hence
- * there is no point in choosing a too small value here.
+ * pixel, hence there is no point in choosing a too small value here.
* </p>
*/
- private static final double COARSE_INVERSE_LOCALIZATION_ACCURACY = 0.01;
+ private static final double COARSE_INVERSE_LOCALIZATION_ACCURACY = 0.1;
- /** Maximum number of evaluations for inverse localization. */
- private static final int MAX_EVAL = 1000;
+ /** Maximum number of evaluations. */
+ private static final int MAX_EVAL = 50;
/** Reference ellipsoid. */
private final ExtendedEllipsoid ellipsoid;
@@ -419,8 +410,10 @@ public class Rugged {
* @param inertialFrame inertial frame where position and velocity are defined
* @return selected position/velocity provider
*/
- private static PVCoordinatesProvider selectPVCoordinatesProvider(final List<Pair<AbsoluteDate, PVCoordinates>> positionsVelocities,
- final int interpolationOrder, final Frame inertialFrame) {
+ private static PVCoordinatesProvider selectPVCoordinatesProvider(final List<Pair<AbsoluteDate,
+ PVCoordinates>> positionsVelocities,
+ final int interpolationOrder,
+ final Frame inertialFrame) {
// set up the ephemeris
final List<Orbit> orbits = new ArrayList<Orbit>(positionsVelocities.size());
@@ -574,265 +567,48 @@ public class Rugged {
public SensorPixel inverseLocalization(final String sensorName, final GeodeticPoint groundPoint,
final double minLine, final double maxLine)
throws RuggedException {
- try {
-
- final LineSensor sensor = getLineSensor(sensorName);
- final Vector3D target = ellipsoid.transform(groundPoint);
-
- final UnivariateSolver coarseSolver =
- new BracketingNthOrderBrentSolver(0.0, COARSE_INVERSE_LOCALIZATION_ACCURACY, 5);
-
- // find approximately the sensor line at which ground point crosses sensor mean plane
- final SensorMeanPlaneCrossing planeCrossing = new SensorMeanPlaneCrossing(sensor, target);
- final double coarseLine = coarseSolver.solve(MAX_EVAL, planeCrossing, minLine, maxLine);
-
- // find approximately the pixel along this sensor line
- final SensorPixelCrossing pixelCrossing =
- new SensorPixelCrossing(sensor, planeCrossing.getTargetDirection(coarseLine));
- final double coarsePixel = coarseSolver.solve(MAX_EVAL, pixelCrossing, -1.0, sensor.getNbPixels());
-
- // compute a quadrilateral that should surround the target
- final double lInf = FastMath.floor(coarseLine);
- final int pInf = FastMath.max(0, FastMath.min(sensor.getNbPixels() - 2, (int) FastMath.floor(coarsePixel)));
- final IntersectionAlgorithm alg = new FixedAltitudeAlgorithm(groundPoint.getAltitude());
- final GeodeticPoint[] previous = directLocalization(sensor, pInf, pInf + 2, alg, lInf);
- final GeodeticPoint[] next = directLocalization(sensor, pInf, pInf + 2, alg, lInf + 1);
-
- final double[] interp = interpolationCoordinates(groundPoint.getLongitude(), groundPoint.getLatitude(),
- previous[0].getLongitude(), previous[0].getLatitude(),
- previous[1].getLongitude(), previous[1].getLatitude(),
- next[0].getLongitude(), next[0].getLatitude(),
- next[1].getLongitude(), next[1].getLatitude());
-
- return (interp == null) ? null : new SensorPixel(lInf + interp[1], pInf + interp[0]);
- } catch (NoBracketingException nbe) {
- // there are no solutions in the search interval
+ final LineSensor sensor = getLineSensor(sensorName);
+ final Vector3D target = ellipsoid.transform(groundPoint);
+
+ // 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)) {
+ // target is out of search interval
return null;
- } catch (TooManyEvaluationsException tmee) {
- throw new RuggedException(tmee);
- } catch (OrekitExceptionWrapper oew) {
- final OrekitException oe = oew.getException();
- throw new RuggedException(oe, oe.getSpecifier(), oe.getParts());
- }
- }
-
- /** Compute a point bilinear interpolation coordinates.
- * <p>
- * This method is used to find interpolation coordinates when the
- * quadrilateral is <em>not</em> a perfect rectangle.
- * </p>
- * @param xuv desired point abscissa, at interpolation coordinates u, v
- * @param yuv desired point ordinate, at interpolation coordinates u, v
- * @param x00 abscissa for base quadrilateral point at u = 0, v = 0
- * @param y00 ordinate for base quadrilateral point at u = 0, v = 0
- * @param x10 abscissa for base quadrilateral point at u = 1, v = 0
- * @param y10 ordinate for base quadrilateral point at u = 1, v = 0
- * @param x01 abscissa for base quadrilateral point at u = 0, v = 1
- * @param y01 ordinate for base quadrilateral point at u = 0, v = 1
- * @param x11 abscissa for base quadrilateral point at u = 1, v = 1
- * @param y11 ordinate for base quadrilateral point at u = 1, v = 1
- * @return interpolation coordinates {@code u, v} corresponding to point {@code xuv, yuv},
- * or null if no such points can be found
- */
- private double[] interpolationCoordinates(final double xuv, final double yuv,
- final double x00, final double y00,
- final double x10, final double y10,
- final double x01, final double y01,
- final double x11, final double y11) {
-
- // bilinear interpolation can be written as follows:
- // P(0,v) = v P(0,1) + (1 - v) P(0,0)
- // P(1,v) = v P(1,1) + (1 - v) P(1,0)
- // P(u,v) = u P(1,v) + (1 - u) P(0,v)
- // which can be solved for u:
- // u = [P(u,v) - P(0,v)] / [P(1,v) - P(0,v)]
- // this equation holds for both x and y coordinates of the various P points, so u can be eliminated:
- // [x(u,v) - x(0,v)] * [y(1,v) - y(0,v)] - [y(u,v) - y(0,v)] * [x(1,v) - x(0,v)] = 0
- // this is a quadratic equation in v: a v² + bv + c = 0
- final double a = y11 * x00 - y10 * x00 + y10 * x01 - y11 * x01 + y01 * x11 - y01 * x10 - y00 * x11 + y00 * x10;
- final double b = y11 * xuv - y10 * xuv + y00 * xuv - y01 * xuv - y11 * x00 + 2 * y10 * x00 - y10 * x01 + y01 * x10 + y00 * x11 - 2 * y00 * x10 + yuv * x01 + yuv * x10 - yuv * x11 - yuv * x00;
- final double c = y10 * xuv - y00 * xuv - y10 * x00 + y00 * x10 - yuv * x10 + yuv * x00;
-
- if (FastMath.abs(a) < Precision.EPSILON * (FastMath.abs(b) + FastMath.abs(c))) {
- if (FastMath.abs(b) < Precision.EPSILON * FastMath.abs(c)) {
- return null;
- } else {
-
- // solve linear equation in v
- final double v = -c / b;
-
- // recover uA from vA
- final double x0v = v * x01 + (1 - v) * x00;
- final double x1v = v * x11 + (1 - v) * x10;
- final double dX = x1v - x0v;
- final double y0v = v * y01 + (1 - v) * y00;
- final double y1v = v * y11 + (1 - v) * y10;
- final double dY = y1v - y0v;
- final double u = (FastMath.abs(dX) >= FastMath.abs(dX)) ? ((xuv - x0v) / dX) : ((yuv - y0v) / dY);
-
- return new double[] {
- u, v
- };
-
- }
- } else {
- // solve quadratic equation in v
- final double bb = b * b;
- final double fac = 4 * a * c;
- if (bb < fac) {
- return null;
- }
- final double s = FastMath.sqrt(bb - fac);
- final double vA = (b > 0) ? -(s + b) / (2 * a) : (2 * c) / (s - b);
- final double vB = c / (a * vA);
-
- // recover uA from vA
- final double x0vA = vA * x01 + (1 - vA) * x00;
- final double x1vA = vA * x11 + (1 - vA) * x10;
- final double dXA = x1vA - x0vA;
- final double y0vA = vA * y01 + (1 - vA) * y00;
- final double y1vA = vA * y11 + (1 - vA) * y10;
- final double dYA = y1vA - y0vA;
- final double uA = (FastMath.abs(dXA) >= FastMath.abs(dXA)) ? ((xuv - x0vA) / dXA) : ((yuv - y0vA) / dYA);
-
- // recover uB from vB
- final double x0vB = vB * x01 + (1 - vB) * x00;
- final double x1vB = vB * x11 + (1 - vB) * x10;
- final double dXB = x1vB - x0vB;
- final double y0vB = vB * y01 + (1 - vB) * y00;
- final double y1vB = vB * y11 + (1 - vB) * y10;
- final double dYB = y1vB - y0vB;
- final double uB = (FastMath.abs(dXB) >= FastMath.abs(dXB)) ? ((xuv - x0vB) / dXB) : ((yuv - y0vB) / dYB);
-
- if ((uA * uA + vA * vA) <= (uB * uB + vB * vB)) {
- return new double[] {
- uA, vA
- };
- } else {
- return new double[] {
- uA, vA
- };
- }
-
- }
-
- }
-
- /** Local class for finding when ground point crosses mean sensor plane. */
- private class SensorMeanPlaneCrossing implements UnivariateFunction {
-
- /** Line sensor. */
- private final LineSensor sensor;
-
- /** Target ground point. */
- private final Vector3D target;
-
- /** Simple constructor.
- * @param sensor sensor to consider
- * @param target target ground point
- */
- public SensorMeanPlaneCrossing(final LineSensor sensor, final Vector3D target) {
- this.sensor = sensor;
- this.target = target;
- }
-
- /** {@inheritDoc} */
- @Override
- public double value(final double lineNumber) throws OrekitExceptionWrapper {
-
- // the target crosses the mean plane if it orthogonal to the normal vector
- return Vector3D.angle(getTargetDirection(lineNumber), sensor.getMeanPlaneNormal()) - 0.5 * FastMath.PI;
-
- }
-
- /** Get the target direction in spacecraft frame.
- * @param lineNumber current line number
- * @return target direction in spacecraft frame
- * @exception OrekitExceptionWrapper if some frame conversion fails
- */
- public Vector3D getTargetDirection(final double lineNumber) throws OrekitExceptionWrapper {
- try {
-
- // 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 Vector3D refInert = scToInert.transformPosition(sensor.getPosition());
-
- final Vector3D targetInert;
- if (lightTimeCorrection) {
- // apply light time correction
- final Vector3D iT = bodyToInert.transformPosition(target);
- final double deltaT = refInert.distance(iT) / Constants.SPEED_OF_LIGHT;
- targetInert = bodyToInert.shiftedBy(-deltaT).transformPosition(target);
- } else {
- // don't apply light time correction
- targetInert = bodyToInert.transformPosition(target);
- }
-
- final Vector3D lInert = targetInert.subtract(refInert).normalize();
- final Vector3D rawLInert;
- 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
- final Vector3D spacecraftVelocity =
- scToInert.transformPVCoordinates(PVCoordinates.ZERO).getVelocity();
- rawLInert = new Vector3D(Constants.SPEED_OF_LIGHT, lInert, -1.0, spacecraftVelocity).normalize();
- } else {
- // don't apply aberration of light correction
- rawLInert = lInert;
- }
-
- // direction of the target point in spacecraft frame
- return scToInert.getInverse().transformVector(rawLInert);
-
- } catch (OrekitException oe) {
- throw new OrekitExceptionWrapper(oe);
- }
}
- }
-
- /** Local class for finding when ground point crosses pixel along sensor line. */
- private class SensorPixelCrossing implements UnivariateFunction {
-
- /** Line sensor. */
- private final LineSensor sensor;
-
- /** Cross direction in spacecraft frame. */
- private final Vector3D cross;
-
- /** Simple constructor.
- * @param sensor sensor to consider
- * @param targetDirection target direction in spacecraft frame
- */
- public SensorPixelCrossing(final LineSensor sensor, final Vector3D targetDirection) {
- this.sensor = sensor;
- this.cross = Vector3D.crossProduct(sensor.getMeanPlaneNormal(), targetDirection).normalize();
- }
-
- /** {@inheritDoc} */
- @Override
- public double value(final double x) {
- return Vector3D.angle(cross, getLOS(x)) - 0.5 * FastMath.PI;
+ // find approximately the pixel along this sensor line
+ final SensorPixelCrossing pixelCrossing = new SensorPixelCrossing(sensor, planeCrossing.getTargetDirection(),
+ MAX_EVAL, COARSE_INVERSE_LOCALIZATION_ACCURACY);
+ final double coarsePixel = pixelCrossing.locatePixel();
+ if (Double.isNaN(coarsePixel)) {
+ // target is out of search interval
+ return null;
}
- /** Interpolate sensor pixels at some pixel index.
- * @param x pixel index
- * @return interpolated direction for specified index
- */
- public Vector3D getLOS(final double x) {
-
- // find surrounding pixels
- final int iInf = FastMath.max(0, FastMath.min(sensor.getNbPixels() - 2, (int) FastMath.floor(x)));
- final int iSup = iInf + 1;
-
- // interpolate
- return new Vector3D(iSup - x, sensor.getLos(iInf), x - iInf, sensor.getLos(iSup)).normalize();
-
- }
+ // 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 Vector3D xAxis = sensor.getLos(closestIndex);
+ final Vector3D zAxis = sensor.getCross(closestIndex);
+ final Vector3D coarseDirection = planeCrossing.getTargetDirection();
+ final Vector3D coarseDirectionDot = planeCrossing.getTargetDirectionDot();
+ final double deltaT = Vector3D.dotProduct(xAxis.subtract(coarseDirection), zAxis) /
+ Vector3D.dotProduct(coarseDirectionDot, zAxis);
+ final double fixedLine = sensor.getLine(sensor.getDate(planeCrossing.getLine()).shiftedBy(deltaT));
+ final Vector3D fixedDirection = new Vector3D(1, coarseDirection, deltaT, coarseDirectionDot).normalize();
+
+ // fix pixel
+ final double alpha = sensor.getAzimuth(fixedDirection, closestIndex);
+ final double pixelWidth = sensor.getWidth(closestIndex);
+ final double fixedPixel = closestIndex + alpha / pixelWidth;
+
+ return new SensorPixel(fixedLine, fixedPixel);
}
diff --git a/src/main/java/org/orekit/rugged/api/SensorMeanPlaneCrossing.java b/src/main/java/org/orekit/rugged/api/SensorMeanPlaneCrossing.java
new file mode 100644
index 00000000..f41b025e
--- /dev/null
+++ b/src/main/java/org/orekit/rugged/api/SensorMeanPlaneCrossing.java
@@ -0,0 +1,237 @@
+/* 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.api;
+
+import org.apache.commons.math3.analysis.UnivariateFunction;
+import org.apache.commons.math3.analysis.solvers.BracketingNthOrderBrentSolver;
+import org.apache.commons.math3.analysis.solvers.UnivariateSolver;
+import org.apache.commons.math3.exception.NoBracketingException;
+import org.apache.commons.math3.exception.TooManyEvaluationsException;
+import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
+import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
+import org.apache.commons.math3.util.FastMath;
+import org.orekit.errors.OrekitException;
+import org.orekit.errors.OrekitExceptionWrapper;
+import org.orekit.frames.Transform;
+import org.orekit.rugged.core.SpacecraftToObservedBody;
+import org.orekit.time.AbsoluteDate;
+import org.orekit.utils.Constants;
+import org.orekit.utils.PVCoordinates;
+
+/** Class dedicated to when ground point crosses mean sensor plane. */
+class SensorMeanPlaneCrossing {
+
+ /** Converter between spacecraft and body. */
+ private final SpacecraftToObservedBody scToBody;
+
+ /** Flag for light time correction. */
+ private boolean lightTimeCorrection;
+
+ /** Flag for aberration of light correction. */
+ private boolean aberrationOfLightCorrection;
+
+ /** Line sensor. */
+ private final LineSensor sensor;
+
+ /** Target ground point. */
+ private final Vector3D target;
+
+ /** Accuracy to use for finding crossing line number. */
+ private final double accuracy;
+
+ /** Evaluated lines. */
+ private double[] lineNumbers;
+
+ /** Observed body to inertial transforms, with selected fixes included. */
+ private final Transform[] fixedBodyToInert;
+
+ /** Spacecraft to inertial transforms,. */
+ private final Transform[] scToInert;
+
+ /** Inertial to spacecraft transforms, with selected fixes included. */
+ private final Transform[] fixedInertToSc;
+
+ /** Target direction in spacecraft frame. */
+ private Vector3D[] targetDirection;
+
+ /** Index of last evaluation. */
+ private int last;
+
+ /** Index of best evaluation. */
+ private int best;
+
+ /** Simple constructor.
+ * @param sensor sensor to consider
+ * @param target target ground point
+ * @param scToBody converter between spacecraft and body
+ * @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
+ */
+ public SensorMeanPlaneCrossing(final LineSensor sensor, final Vector3D target,
+ final SpacecraftToObservedBody scToBody,
+ final boolean lightTimeCorrection,
+ final boolean aberrationOfLightCorrection,
+ final int maxEval, final double accuracy) {
+ this.sensor = sensor;
+ this.target = target;
+ this.scToBody = scToBody;
+ this.lightTimeCorrection = lightTimeCorrection;
+ this.aberrationOfLightCorrection = aberrationOfLightCorrection;
+ this.accuracy = accuracy;
+ this.lineNumbers = new double[maxEval];
+ this.fixedBodyToInert = new Transform[maxEval];
+ this.scToInert = new Transform[maxEval];
+ this.fixedInertToSc = new Transform[maxEval];
+ this.targetDirection = new Vector3D[maxEval];
+ this.last = -1;
+ }
+
+ /** 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
+ * @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)
+ throws RuggedException {
+ try {
+
+ // set up function evaluating to 0.0 at mean plane crossing
+ final UnivariateFunction f = new UnivariateFunction() {
+ /** {@inheritDoc} */
+ @Override
+ public double value(final double x) throws OrekitExceptionWrapper {
+ // the target crosses the mean plane if it orthogonal to the normal vector
+ evaluateLine(x);
+ return Vector3D.angle(targetDirection[last], sensor.getMeanPlaneNormal()) - 0.5 * FastMath.PI;
+ }
+ };
+
+ // find the root
+ final UnivariateSolver solver =
+ new BracketingNthOrderBrentSolver(0.0, accuracy, 5);
+ final double crossingLine = solver.solve(lineNumbers.length, f, minLine, maxLine);
+
+ // identify the selected solution
+ best = -1;
+ double min = Double.POSITIVE_INFINITY;
+ for (int i = 0; i <= last; ++i) {
+ final double delta = FastMath.abs(lineNumbers[i] - crossingLine);
+ if (delta < min) {
+ best = i;
+ min = delta;
+ }
+
+ }
+
+ return true;
+
+ } catch (NoBracketingException nbe) {
+ // there are no solutions in the search interval
+ return false;
+ } catch (TooManyEvaluationsException tmee) {
+ throw new RuggedException(tmee);
+ } catch (OrekitExceptionWrapper oew) {
+ final OrekitException oe = oew.getException();
+ throw new RuggedException(oe, oe.getSpecifier(), oe.getParts());
+ }
+ }
+
+ /** Get the crossing line.
+ * @return crossing line
+ */
+ public double getLine() {
+ return lineNumbers[best];
+ }
+
+ /** Get the normalized target direction in spacecraft frame at crossing.
+ * @return normalized target direction in spacecraft frame at crossing
+ */
+ public Vector3D getTargetDirection() {
+ return targetDirection[best];
+ }
+
+ /** Get the derivative of normalized target direction in spacecraft frame at crossing.
+ * @return derivative of normalized target direction in spacecraft frame at crossing
+ */
+ public Vector3D getTargetDirectionDot() {
+ final PVCoordinates targetBody = new PVCoordinates(target, Vector3D.ZERO);
+ final PVCoordinates targetInert = fixedBodyToInert[best].transformPVCoordinates(targetBody);
+ final PVCoordinates targetSc = fixedInertToSc[best].transformPVCoordinates(targetInert);
+ return new Vector3D(1.0 / targetSc.getPosition().subtract(sensor.getPosition()).getNorm(),
+ targetSc.getVelocity());
+ }
+
+ /** Evaluate geometry for a given line number.
+ * @param lineNumber current line number
+ * @exception OrekitExceptionWrapper if some frame conversion fails
+ */
+ private void evaluateLine(final double lineNumber) throws OrekitExceptionWrapper {
+ try {
+
+ lineNumbers[++last] = lineNumber;
+
+ // compute the transform between spacecraft and observed body
+ final AbsoluteDate date = sensor.getDate(lineNumber);
+ final Transform bodyToInert = scToBody.getInertialToBody(date).getInverse();
+ scToInert[last] = scToBody.getScToInertial(date);
+ final Vector3D refInert = scToInert[last].transformPosition(sensor.getPosition());
+
+ if (lightTimeCorrection) {
+ // apply light time correction
+ final Vector3D iT = bodyToInert.transformPosition(target);
+ final double deltaT = refInert.distance(iT) / Constants.SPEED_OF_LIGHT;
+ fixedBodyToInert[last] = bodyToInert.shiftedBy(-deltaT);
+ } else {
+ // don't apply light time correction
+ fixedBodyToInert[last] = bodyToInert;
+ }
+ final Vector3D targetInert = fixedBodyToInert[last].transformPosition(target);
+
+ final Vector3D lInert = targetInert.subtract(refInert).normalize();
+ final Transform inertToSc = scToInert[last].getInverse();
+ 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
+ final Vector3D spacecraftVelocity =
+ scToInert[last].transformPVCoordinates(PVCoordinates.ZERO).getVelocity();
+ final Vector3D rawLInert = new Vector3D(Constants.SPEED_OF_LIGHT, lInert,
+ -1.0, spacecraftVelocity);
+ fixedInertToSc[last] = new Transform(date,
+ inertToSc,
+ new Transform(date,
+ new Rotation(inertToSc.transformVector(lInert),
+ inertToSc.transformVector(rawLInert))));
+ } else {
+ // don't apply aberration of light correction
+ fixedInertToSc[last] = inertToSc;
+ }
+
+ // direction of the target point in spacecraft frame
+ targetDirection[last] = fixedInertToSc[last].transformVector(lInert);
+
+ } catch (OrekitException oe) {
+ throw new OrekitExceptionWrapper(oe);
+ }
+ }
+
+}
diff --git a/src/main/java/org/orekit/rugged/api/SensorPixelCrossing.java b/src/main/java/org/orekit/rugged/api/SensorPixelCrossing.java
new file mode 100644
index 00000000..9c0cd1af
--- /dev/null
+++ b/src/main/java/org/orekit/rugged/api/SensorPixelCrossing.java
@@ -0,0 +1,107 @@
+/* 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.api;
+
+import org.apache.commons.math3.analysis.UnivariateFunction;
+import org.apache.commons.math3.analysis.solvers.BracketingNthOrderBrentSolver;
+import org.apache.commons.math3.analysis.solvers.UnivariateSolver;
+import org.apache.commons.math3.exception.NoBracketingException;
+import org.apache.commons.math3.exception.TooManyEvaluationsException;
+import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
+import org.apache.commons.math3.util.FastMath;
+import org.orekit.errors.OrekitExceptionWrapper;
+
+/** Class devoted to locate where ground point crosses a sensor line.
+ * <p>
+ * This class is used in the first stage of inverse localization.
+ * </p>
+ * @author Luc Maisonobe
+ */
+class SensorPixelCrossing {
+
+ /** Line sensor. */
+ private final LineSensor sensor;
+
+ /** Cross direction in spacecraft frame. */
+ private final Vector3D cross;
+
+ /** Maximum number of evaluations. */
+ private final int maxEval;
+
+ /** Accuracy to use for finding crossing line number. */
+ private final double accuracy;
+
+ /** Simple constructor.
+ * @param sensor sensor to consider
+ * @param targetDirection target direction in spacecraft frame
+ * @param maxEval maximum number of evaluations
+ * @param accuracy accuracy to use for finding crossing line number
+ */
+ public SensorPixelCrossing(final LineSensor sensor, final Vector3D targetDirection,
+ final int maxEval, final double accuracy) {
+ this.sensor = sensor;
+ this.cross = Vector3D.crossProduct(sensor.getMeanPlaneNormal(), targetDirection).normalize();
+ this.maxEval = maxEval;
+ this.accuracy = accuracy;
+ }
+
+ /** Locate pixel along sensor line.
+ * @return pixel location ({@code Double.NaN} if the first and last
+ * pixels of the line do not bracket a location)
+ * @exception RuggedException if the maximum number of evaluations is exceeded
+ */
+ public double locatePixel() throws RuggedException {
+ try {
+
+ // set up function evaluating to 0.0 where target matches pixel
+ final UnivariateFunction f = new UnivariateFunction() {
+ /** {@inheritDoc} */
+ @Override
+ public double value(final double x) throws OrekitExceptionWrapper {
+ return Vector3D.angle(cross, getLOS(x)) - 0.5 * FastMath.PI;
+ }
+ };
+
+ // find the root
+ final UnivariateSolver solver =
+ new BracketingNthOrderBrentSolver(0.0, accuracy, 5);
+ return solver.solve(maxEval, f, -1.0, sensor.getNbPixels());
+
+ } catch (NoBracketingException nbe) {
+ // there are no solutions in the search interval
+ return Double.NaN;
+ } catch (TooManyEvaluationsException tmee) {
+ throw new RuggedException(tmee);
+ }
+ }
+
+ /** Interpolate sensor pixels at some pixel index.
+ * @param x pixel index
+ * @return interpolated direction for specified index
+ */
+ private Vector3D getLOS(final double x) {
+
+ // find surrounding pixels
+ final int iInf = FastMath.max(0, FastMath.min(sensor.getNbPixels() - 2, (int) FastMath.floor(x)));
+ final int iSup = iInf + 1;
+
+ // interpolate
+ return new Vector3D(iSup - x, sensor.getLos(iInf), x - iInf, sensor.getLos(iSup)).normalize();
+
+ }
+
+}
diff --git a/src/test/java/org/orekit/rugged/api/RuggedTest.java b/src/test/java/org/orekit/rugged/api/RuggedTest.java
index 1965f19f..18bd2c3e 100644
--- a/src/test/java/org/orekit/rugged/api/RuggedTest.java
+++ b/src/test/java/org/orekit/rugged/api/RuggedTest.java
@@ -431,10 +431,11 @@ public class RuggedTest {
@Test
public void testInverseLocalization()
throws RuggedException, OrekitException, URISyntaxException {
- checkInverseLocalization(2000, false, false, 4.0e-5, 4.0e-5);
- checkInverseLocalization(2000, false, true, 4.0e-5, 4.0e-5);
- checkInverseLocalization(2000, true, false, 4.0e-5, 4.0e-5);
- checkInverseLocalization(2000, true, true, 4.0e-5, 4.0e-5);
+// checkInverseLocalization(2000, false, false, 4.0e-5, 4.0e-5);
+// checkInverseLocalization(2000, false, true, 4.0e-5, 4.0e-5);
+// checkInverseLocalization(2000, true, false, 4.0e-5, 4.0e-5);
+// checkInverseLocalization(2000, true, true, 4.0e-5, 4.0e-5);
+ checkInverseLocalization(200, true, true, 5.0e-5, 7.0e-5);
}
private void checkInverseLocalization(int dimension, boolean lightTimeCorrection, boolean aberrationOfLightCorrection,
@@ -450,11 +451,11 @@ public class RuggedTest {
// 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
+ // los: swath in the (YZ) plane, looking at 50° roll, 2.6" per pixel
Vector3D position = new Vector3D(1.5, 0, -0.2);
List<Vector3D> los = createLOS(new Rotation(Vector3D.PLUS_I, FastMath.toRadians(50.0)).applyTo(Vector3D.PLUS_K),
Vector3D.PLUS_I,
- FastMath.toRadians(1.0), dimension);
+ FastMath.toRadians(dimension * 2.6 / 3600.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(crossing, dimension / 2, 1.0 / 1.5e-3);
@@ -477,16 +478,27 @@ public class RuggedTest {
double referenceLine = 0.56789 * dimension;
GeodeticPoint[] gp = rugged.directLocalization("line", referenceLine);
- for (double p = 1; p < gp.length - 1; p += 0.2) {
+ long t0 = System.currentTimeMillis();
+// double maxL = 0;
+// double maxP = 0;
+ for (int k = 0; k < dimension; ++k) {
+ for (double p = 0; p < gp.length - 1; p += 1.0) {
int i = (int) FastMath.floor(p);
double d = p - i;
GeodeticPoint g = new GeodeticPoint((1 - d) * gp[i].getLatitude() + d * gp[i + 1].getLatitude(),
(1 - d) * gp[i].getLongitude() + d * gp[i + 1].getLongitude(),
(1 - d) * gp[i].getAltitude() + d * gp[i + 1].getAltitude());
SensorPixel sp = rugged.inverseLocalization("line", g, 0, dimension);
+// System.out.println(p + " " + (referenceLine - sp.getLineNumber()) + " " + (p - sp.getPixelNumber()));
Assert.assertEquals(referenceLine, sp.getLineNumber(), maxLineError);
Assert.assertEquals(p, sp.getPixelNumber(), maxPixelError);
+// maxL = FastMath.max(maxL, FastMath.abs(referenceLine - sp.getLineNumber()));
+// maxP = FastMath.max(maxP, FastMath.abs(p - sp.getPixelNumber()));
+ }
}
+ long t1 = System.currentTimeMillis();
+ System.out.println("# " + dimension + "x" + dimension + " " + (1.0e-3 * (t1 - t0)));
+// System.out.println("# maxL = " + maxL + ", maxP = " + maxP);
}
private BodyShape createEarth()
--
GitLab