From af6748d13dab23c0063ad9cdb0fa583e6e9bd42f Mon Sep 17 00:00:00 2001
From: Luc Maisonobe <luc@orekit.org>
Date: Mon, 22 Aug 2016 17:04:29 +0200
Subject: [PATCH] Set up partial derivatives for inverse location.
---
.../java/org/orekit/rugged/api/Rugged.java | 158 ++++++++++--
.../java/org/orekit/rugged/errors/Dump.java | 12 +-
.../orekit/rugged/errors/DumpReplayer.java | 25 +-
.../orekit/rugged/linesensor/LineSensor.java | 87 +------
.../linesensor/SensorMeanPlaneCrossing.java | 80 ++++---
.../java/org/orekit/rugged/TestUtils.java | 4 +-
.../org/orekit/rugged/api/RuggedTest.java | 226 ++++++++++++++++--
.../orekit/rugged/errors/DumpManagerTest.java | 2 +-
.../SensorMeanPlaneCrossingTest.java | 96 ++++++++
9 files changed, 510 insertions(+), 180 deletions(-)
diff --git a/src/main/java/org/orekit/rugged/api/Rugged.java b/src/main/java/org/orekit/rugged/api/Rugged.java
index b785eebf..4e4e85df 100644
--- a/src/main/java/org/orekit/rugged/api/Rugged.java
+++ b/src/main/java/org/orekit/rugged/api/Rugged.java
@@ -27,6 +27,8 @@ import java.util.Set;
import org.hipparchus.analysis.differentiation.DerivativeStructure;
import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
+import org.hipparchus.linear.Array2DRowRealMatrix;
+import org.hipparchus.linear.ArrayRealVector;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.linear.RealVector;
import org.hipparchus.optim.ConvergenceChecker;
@@ -76,6 +78,9 @@ public class Rugged {
/** Maximum number of evaluations. */
private static final int MAX_EVAL = 50;
+ /** Margin used in parameters estimation for the inverse location lines range. */
+ private static final int ESTIMATION_LINE_RANGE_MARGIN = 100;
+
/** Reference ellipsoid. */
private final ExtendedEllipsoid ellipsoid;
@@ -535,7 +540,7 @@ public class Rugged {
// find approximately the pixel along this sensor line
final SensorPixelCrossing pixelCrossing =
new SensorPixelCrossing(sensor, planeCrossing.getMeanPlaneNormal(),
- crossingResult.getTargetDirection().toVector3D(),
+ crossingResult.getTargetDirection(),
MAX_EVAL, COARSE_INVERSE_LOCATION_ACCURACY);
final double coarsePixel = pixelCrossing.locatePixel(crossingResult.getDate());
if (Double.isNaN(coarsePixel)) {
@@ -545,22 +550,25 @@ 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 lowIndex = FastMath.max(0, FastMath.min(sensor.getNbPixels() - 2, (int) FastMath.floor(coarsePixel)));
- final Vector3D lowLOS = sensor.getLOS(crossingResult.getDate(), lowIndex);
- final Vector3D highLOS = sensor.getLOS(crossingResult.getDate(), lowIndex + 1);
- final Vector3D localZ = Vector3D.crossProduct(lowLOS, highLOS);
- final DerivativeStructure beta = FieldVector3D.angle(crossingResult.getTargetDirection(), localZ);
- final double deltaL = (0.5 * FastMath.PI - beta.getValue()) / beta.getPartialDerivative(1);
- 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();
+ final int lowIndex = FastMath.max(0, FastMath.min(sensor.getNbPixels() - 2, (int) FastMath.floor(coarsePixel)));
+ final Vector3D lowLOS = sensor.getLOS(crossingResult.getDate(), lowIndex);
+ final Vector3D highLOS = sensor.getLOS(crossingResult.getDate(), lowIndex + 1);
+ final Vector3D localZ = Vector3D.crossProduct(lowLOS, highLOS).normalize();
+ final double beta = FastMath.acos(Vector3D.dotProduct(crossingResult.getTargetDirection(),
+ localZ));
+ final double s = Vector3D.dotProduct(crossingResult.getTargetDirectionDerivative(),
+ localZ);
+ final double betaDer = -s / FastMath.sqrt(1 - s * s);
+ final double deltaL = (0.5 * FastMath.PI - beta) / betaDer;
+ final double fixedLine = crossingResult.getLine() + deltaL;
+ final Vector3D fixedDirection = new Vector3D(1, crossingResult.getTargetDirection(),
+ deltaL, crossingResult.getTargetDirectionDerivative()).normalize();
// fix neighbouring pixels
- final AbsoluteDate fixedDate = sensor.getDate(fixedLine);
- final Vector3D fixedX = sensor.getLOS(fixedDate, lowIndex);
- final Vector3D fixedZ = Vector3D.crossProduct(fixedX, sensor.getLOS(fixedDate, lowIndex + 1));
- final Vector3D fixedY = Vector3D.crossProduct(fixedZ, fixedX);
+ final AbsoluteDate fixedDate = sensor.getDate(fixedLine);
+ final Vector3D fixedX = sensor.getLOS(fixedDate, lowIndex);
+ final Vector3D fixedZ = Vector3D.crossProduct(fixedX, sensor.getLOS(fixedDate, lowIndex + 1));
+ final Vector3D fixedY = Vector3D.crossProduct(fixedZ, fixedX);
// fix pixel
final double pixelWidth = FastMath.atan2(Vector3D.dotProduct(highLOS, fixedY),
@@ -616,6 +624,82 @@ public class Rugged {
finders.put(planeCrossing.getSensor().getName(), planeCrossing);
}
+ /** Inverse location of a point with derivatives.
+ * @param sensorName name of the line sensor
+ * @param point point to localize
+ * @param minLine minimum line number
+ * @param maxLine maximum line number
+ * @return sensor pixel seeing point with derivatives, or null if point cannot be seen between the
+ * prescribed line numbers
+ * @exception RuggedException if line cannot be localized, or sensor is unknown
+ * @see #inverseLocation(String, GeodeticPoint, int, int)
+ */
+ private DerivativeStructure[] inverseLocationDerivatives(final String sensorName,
+ final GeodeticPoint point,
+ final int minLine,
+ final int maxLine)
+ throws RuggedException {
+
+ final LineSensor sensor = getLineSensor(sensorName);
+
+ final SensorMeanPlaneCrossing planeCrossing = getPlaneCrossing(sensorName, minLine, maxLine);
+
+ // find approximately the sensor line at which ground point crosses sensor mean plane
+ final Vector3D target = ellipsoid.transform(point);
+ final SensorMeanPlaneCrossing.CrossingResult crossingResult = planeCrossing.find(target);
+ if (crossingResult == null) {
+ // target is out of search interval
+ return null;
+ }
+
+ // find approximately the pixel along this sensor line
+ final SensorPixelCrossing pixelCrossing =
+ new SensorPixelCrossing(sensor, planeCrossing.getMeanPlaneNormal(),
+ crossingResult.getTargetDirection(),
+ MAX_EVAL, COARSE_INVERSE_LOCATION_ACCURACY);
+ final double coarsePixel = pixelCrossing.locatePixel(crossingResult.getDate());
+ if (Double.isNaN(coarsePixel)) {
+ // target is out of search interval
+ return null;
+ }
+
+ // 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 lowIndex = FastMath.max(0, FastMath.min(sensor.getNbPixels() - 2, (int) FastMath.floor(coarsePixel)));
+ final FieldVector3D<DerivativeStructure> lowLOS =
+ sensor.getLOSDerivatives(crossingResult.getDate(), lowIndex);
+ final FieldVector3D<DerivativeStructure> highLOS = sensor.getLOSDerivatives(crossingResult.getDate(), lowIndex + 1);
+ final FieldVector3D<DerivativeStructure> localZ = FieldVector3D.crossProduct(lowLOS, highLOS).normalize();
+ final DerivativeStructure beta = FieldVector3D.dotProduct(crossingResult.getTargetDirection(), localZ).acos();
+ final DerivativeStructure s = FieldVector3D.dotProduct(crossingResult.getTargetDirectionDerivative(), localZ);
+ final DerivativeStructure minusBetaDer = s.divide(s.multiply(s).subtract(1).negate().sqrt());
+ final DerivativeStructure deltaL = beta.subtract(0.5 * FastMath.PI) .divide(minusBetaDer);
+ final DerivativeStructure fixedLine = deltaL.add(crossingResult.getLine());
+ final FieldVector3D<DerivativeStructure> fixedDirection =
+ new FieldVector3D<DerivativeStructure>(deltaL.getField().getOne(), crossingResult.getTargetDirection(),
+ deltaL, crossingResult.getTargetDirectionDerivative()).normalize();
+
+ // fix neighbouring pixels
+ final AbsoluteDate fixedDate = sensor.getDate(fixedLine.getValue());
+ final FieldVector3D<DerivativeStructure> fixedX = sensor.getLOSDerivatives(fixedDate, lowIndex);
+ final FieldVector3D<DerivativeStructure> fixedZ = FieldVector3D.crossProduct(fixedX, sensor.getLOSDerivatives(fixedDate, lowIndex + 1));
+ final FieldVector3D<DerivativeStructure> fixedY = FieldVector3D.crossProduct(fixedZ, fixedX);
+
+ // fix pixel
+ final DerivativeStructure hY = FieldVector3D.dotProduct(highLOS, fixedY);
+ final DerivativeStructure hX = FieldVector3D.dotProduct(highLOS, fixedX);
+ final DerivativeStructure pixelWidth = hY.atan2(hX);
+ final DerivativeStructure fY = FieldVector3D.dotProduct(fixedDirection, fixedY);
+ final DerivativeStructure fX = FieldVector3D.dotProduct(fixedDirection, fixedX);
+ final DerivativeStructure alpha = fY.atan2(fX);
+ final DerivativeStructure fixedPixel = alpha.divide(pixelWidth).add(lowIndex);
+
+ return new DerivativeStructure[] {
+ fixedLine, fixedPixel
+ };
+
+ }
+
/** Estimate the free parameters in viewing model to match specified sensor
* to ground mappings.
* <p>
@@ -705,13 +789,20 @@ public class Rugged {
n += reference.getMappings().size();
}
final double[] target = new double[2 * n];
+ double min = Double.POSITIVE_INFINITY;
+ double max = Double.NEGATIVE_INFINITY;
int k = 0;
for (final SensorToGroundMapping reference : references) {
for (final Map.Entry<SensorPixel, GeodeticPoint> mapping : reference.getMappings()) {
- target[k++] = mapping.getKey().getLineNumber();
- target[k++] = mapping.getKey().getPixelNumber();
+ final SensorPixel sp = mapping.getKey();
+ target[k++] = sp.getLineNumber();
+ target[k++] = sp.getPixelNumber();
+ min = FastMath.min(min, sp.getLineNumber());
+ max = FastMath.max(max, sp.getLineNumber());
}
}
+ int minLine = (int) FastMath.floor(min - ESTIMATION_LINE_RANGE_MARGIN);
+ int maxLine = (int) FastMath.ceil(max - ESTIMATION_LINE_RANGE_MARGIN);
// prevent parameters to exceed their prescribed bounds
final ParameterValidator validator = params -> {
@@ -743,9 +834,38 @@ public class Rugged {
driver.setNormalizedValue(point.getEntry(i++));
}
- // TODO: compute inverse loc and its partial derivatives
- return new Pair<RealVector, RealMatrix>(null, null);
+ // compute inverse loc and its partial derivatives
+ final RealVector value = new ArrayRealVector(target.length);
+ final RealMatrix jacobian = new Array2DRowRealMatrix(target.length, freeParameters.size());
+ int l = 0;
+ for (final SensorToGroundMapping reference : references) {
+ for (final Map.Entry<SensorPixel, GeodeticPoint> mapping : reference.getMappings()) {
+ final GeodeticPoint gp = mapping.getValue();
+ final DerivativeStructure[] ilResult =
+ inverseLocationDerivatives(reference.getSensor().getName(),
+ gp, minLine, maxLine);
+
+ // extract the value
+ value.setEntry(l, ilResult[0].getValue());
+ value.setEntry(l + 1, ilResult[1].getValue());
+
+ // extract the Jacobian
+ final int[] orders = new int[freeParameters.size()];
+ for (int m = 0; m < freeParameters.size(); ++m) {
+ orders[m] = 1;
+ jacobian.setEntry(l, m, ilResult[0].getPartialDerivative(orders));
+ jacobian.setEntry(l + 1, m, ilResult[1].getPartialDerivative(orders));
+ orders[m] = 0;
+ }
+
+ }
+ }
+
+ // inverse loc result with Jacobian for all reference points
+ return new Pair<RealVector, RealMatrix>(value, jacobian);
+ } catch (RuggedException re) {
+ throw new RuggedExceptionWrapper(re);
} catch (OrekitException oe) {
throw new OrekitExceptionWrapper(oe);
}
diff --git a/src/main/java/org/orekit/rugged/errors/Dump.java b/src/main/java/org/orekit/rugged/errors/Dump.java
index ea538a7a..a58960fd 100644
--- a/src/main/java/org/orekit/rugged/errors/Dump.java
+++ b/src/main/java/org/orekit/rugged/errors/Dump.java
@@ -516,12 +516,12 @@ class Dump {
" lineNumber %22.15e date %s target %22.15e %22.15e %22.15e targetDirection %22.15e %22.15e %22.15e %22.15e %22.15e %22.15e",
result.getLine(), convertDate(result.getDate()),
result.getTarget().getX(), result.getTarget().getY(), result.getTarget().getZ(),
- result.getTargetDirection().getX().getValue(),
- result.getTargetDirection().getY().getValue(),
- result.getTargetDirection().getZ().getValue(),
- result.getTargetDirection().getZ().getPartialDerivative(1),
- result.getTargetDirection().getY().getPartialDerivative(1),
- result.getTargetDirection().getZ().getPartialDerivative(1));
+ result.getTargetDirection().getX(),
+ result.getTargetDirection().getY(),
+ result.getTargetDirection().getZ(),
+ result.getTargetDirectionDerivative().getZ(),
+ result.getTargetDirectionDerivative().getY(),
+ result.getTargetDirectionDerivative().getZ());
} catch (RuggedException re) {
throw new RuggedExceptionWrapper(re);
}
diff --git a/src/main/java/org/orekit/rugged/errors/DumpReplayer.java b/src/main/java/org/orekit/rugged/errors/DumpReplayer.java
index 0e1506c1..40f6c2d4 100644
--- a/src/main/java/org/orekit/rugged/errors/DumpReplayer.java
+++ b/src/main/java/org/orekit/rugged/errors/DumpReplayer.java
@@ -868,23 +868,18 @@ public class DumpReplayer {
!fields[base + 8].equals(TARGET_DIRECTION)) {
throw new RuggedException(RuggedMessages.CANNOT_PARSE_LINE, l, file, line);
}
- final double ln = Double.parseDouble(fields[base + 1]);
- final AbsoluteDate date = new AbsoluteDate(fields[base + 3], TimeScalesFactory.getUTC());
- final Vector3D target = new Vector3D(Double.parseDouble(fields[base + 5]),
- Double.parseDouble(fields[base + 6]),
- Double.parseDouble(fields[base + 7]));
- final DerivativeStructure tdX = new DerivativeStructure(1, 1,
- Double.parseDouble(fields[base + 9]),
- Double.parseDouble(fields[base + 12]));
- final DerivativeStructure tdY = new DerivativeStructure(1, 1,
+ final double ln = Double.parseDouble(fields[base + 1]);
+ final AbsoluteDate date = new AbsoluteDate(fields[base + 3], TimeScalesFactory.getUTC());
+ final Vector3D target = new Vector3D(Double.parseDouble(fields[base + 5]),
+ Double.parseDouble(fields[base + 6]),
+ Double.parseDouble(fields[base + 7]));
+ final Vector3D targetDirection = new Vector3D(Double.parseDouble(fields[base + 9]),
Double.parseDouble(fields[base + 10]),
- Double.parseDouble(fields[base + 13]));
- final DerivativeStructure tdZ = new DerivativeStructure(1, 1,
- Double.parseDouble(fields[base + 11]),
+ Double.parseDouble(fields[base + 11]));
+ final Vector3D targetDirectionDerivative = new Vector3D(Double.parseDouble(fields[base + 12]),
+ Double.parseDouble(fields[base + 13]),
Double.parseDouble(fields[base + 14]));
- final FieldVector3D<DerivativeStructure> targetDirection =
- new FieldVector3D<DerivativeStructure>(tdX, tdY, tdZ);
- cachedResults[i] = new CrossingResult(date, ln, target, targetDirection);
+ cachedResults[i] = new CrossingResult(date, ln, target, targetDirection, targetDirectionDerivative);
base += 15;
}
global.getSensor(sensorName).setMeanPlane(new ParsedMeanPlane(minLine, maxLine, maxEval, accuracy, normal, cachedResults));
diff --git a/src/main/java/org/orekit/rugged/linesensor/LineSensor.java b/src/main/java/org/orekit/rugged/linesensor/LineSensor.java
index 88aa9137..5705184f 100644
--- a/src/main/java/org/orekit/rugged/linesensor/LineSensor.java
+++ b/src/main/java/org/orekit/rugged/linesensor/LineSensor.java
@@ -21,29 +21,17 @@ import java.util.stream.Stream;
import org.hipparchus.analysis.differentiation.DerivativeStructure;
import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
-import org.hipparchus.util.FastMath;
-import org.orekit.errors.OrekitException;
import org.orekit.rugged.errors.DumpManager;
import org.orekit.rugged.errors.RuggedException;
import org.orekit.rugged.los.TimeDependentLOS;
import org.orekit.rugged.utils.ExtendedParameterDriver;
import org.orekit.time.AbsoluteDate;
-import org.orekit.utils.ParameterDriver;
-import org.orekit.utils.ParameterObserver;
/** Line sensor model.
* @author Luc Maisonobe
*/
public class LineSensor {
- /** Parameters scaling factor.
- * <p>
- * We use a power of 2 to avoid numeric noise introduction
- * in the multiplications/divisions sequences.
- * </p>
- */
- private final double SCALE = FastMath.scalb(1.0, -3);
-
/** Name of the sensor. */
private final String name;
@@ -51,23 +39,11 @@ public class LineSensor {
private final LineDatation datationModel;
/** Sensor position. */
- private Vector3D position;
-
- /** Sensor position, with derivatives. */
- private FieldVector3D<DerivativeStructure> positionDS;
+ private final Vector3D position;
/** Pixels lines-of-sight. */
private final TimeDependentLOS los;
- /** Driver for the sensor position parameter along X. */
- private final ExtendedParameterDriver xPos;
-
- /** Driver for the sensor position parameter along Y. */
- private final ExtendedParameterDriver yPos;
-
- /** Driver for the sensor position parameter along Z. */
- private final ExtendedParameterDriver zPos;
-
/** Simple constructor.
* @param name name of the sensor
* @param datationModel datation model
@@ -80,32 +56,9 @@ public class LineSensor {
this.name = name;
this.datationModel = datationModel;
+ this.position = position;
this.los = los;
- final ParameterObserver resettingObserver = new ParameterObserver() {
- /** {@inheritDoc} */
- @Override
- public void valueChanged(final double previousValue, final ParameterDriver driver) {
- LineSensor.this.position = null;
- LineSensor.this.positionDS = null;
- }
- };
-
- try {
- xPos = new ExtendedParameterDriver(name + "-X", position.getX(), SCALE,
- Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);
- xPos.addObserver(resettingObserver);
- yPos = new ExtendedParameterDriver(name + "-Y", position.getY(), SCALE,
- Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);
- yPos.addObserver(resettingObserver);
- zPos = new ExtendedParameterDriver(name + "-Z", position.getZ(), SCALE,
- Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);
- zPos.addObserver(resettingObserver);
- } catch (OrekitException oe) {
- // this should never happen
- throw RuggedException.createInternalError(oe);
- }
-
}
/** Get the name of the sensor.
@@ -127,8 +80,7 @@ public class LineSensor {
* @since 2.0
*/
public Stream<ExtendedParameterDriver> getExtendedParametersDrivers() {
- return Stream.<ExtendedParameterDriver>concat(Stream.of(xPos, yPos, zPos),
- los.getExtendedParametersDrivers());
+ return los.getExtendedParametersDrivers();
}
/** Get the pixel normalized line-of-sight at some date.
@@ -192,40 +144,7 @@ public class LineSensor {
* @return position
*/
public Vector3D getPosition() {
- if (position == null) {
- // lazy evaluation of the position
- position = new Vector3D(xPos.getValue(), yPos.getValue(), zPos.getValue());
- }
return position;
}
- /** Get the sensor position,
- * and its derivatives with respect to estimated parameters.
- * @return position
- */
- public FieldVector3D<DerivativeStructure> getPositionDerivatives() {
- if (positionDS == null) {
- // lazy evaluation of the position
- positionDS = new FieldVector3D<DerivativeStructure>(getCoordinate(xPos),
- getCoordinate(yPos),
- getCoordinate(zPos));
- }
- return positionDS;
- }
-
- /** Get a coordinate and its derivatives.
- * @param driver coordinate driver
- * @return coordinate value and its derivatives
- */
- private DerivativeStructure getCoordinate(final ExtendedParameterDriver driver) {
- final double value = driver.getValue();
- if (driver.isSelected()) {
- // the x coordinate of the sensor is estimated
- return new DerivativeStructure(driver.getNbEstimated(), 1, driver.getIndex(), value);
- } else {
- // the x coordinate of the sensor is not estimated
- return new DerivativeStructure(driver.getNbEstimated(), 1, value);
- }
- }
-
}
diff --git a/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java b/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java
index e6f523f5..96bf4d97 100644
--- a/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java
+++ b/src/main/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossing.java
@@ -21,12 +21,10 @@ import java.util.List;
import java.util.stream.Stream;
import org.hipparchus.analysis.UnivariateFunction;
-import org.hipparchus.analysis.differentiation.DerivativeStructure;
import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
import org.hipparchus.analysis.solvers.UnivariateSolver;
import org.hipparchus.exception.MathIllegalArgumentException;
import org.hipparchus.exception.MathIllegalStateException;
-import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.linear.Array2DRowRealMatrix;
import org.hipparchus.linear.ArrayRealVector;
@@ -290,21 +288,27 @@ public class SensorMeanPlaneCrossing {
private final Vector3D target;
/** Target direction in spacecraft frame. */
- private final FieldVector3D<DerivativeStructure> targetDirection;
+ private final Vector3D targetDirection;
+
+ /** Derivative of the target direction in spacecraft frame with respect to line number. */
+ private final Vector3D targetDirectionDerivative;
/** Simple constructor.
* @param crossingDate crossing date
* @param crossingLine crossing line
* @param target target ground point
* @param targetDirection target direction in spacecraft frame
+ * @param targetDirectionDerivative derivative of the target direction in spacecraft frame with respect to line number
*/
public CrossingResult(final AbsoluteDate crossingDate, final double crossingLine,
final Vector3D target,
- final FieldVector3D<DerivativeStructure> targetDirection) {
- this.crossingDate = crossingDate;
- this.crossingLine = crossingLine;
- this.target = target;
- this.targetDirection = targetDirection;
+ final Vector3D targetDirection,
+ final Vector3D targetDirectionDerivative) {
+ this.crossingDate = crossingDate;
+ this.crossingLine = crossingLine;
+ this.target = target;
+ this.targetDirection = targetDirection;
+ this.targetDirectionDerivative = targetDirectionDerivative;
}
/** Get the crossing date.
@@ -329,13 +333,21 @@ public class SensorMeanPlaneCrossing {
}
/** Get the normalized target direction in spacecraft frame at crossing.
- * @return normalized target direction in spacecraft frame at crossing, with its first derivative
+ * @return normalized target direction in spacecraft frame at crossing
* with respect to line number
*/
- public FieldVector3D<DerivativeStructure> getTargetDirection() {
+ public Vector3D getTargetDirection() {
return targetDirection;
}
+ /** Get the derivative of the normalized target direction in spacecraft frame at crossing.
+ * @return derivative of the normalized target direction in spacecraft frame at crossing
+ * with respect to line number
+ */
+ public Vector3D getTargetDirectionDerivative() {
+ return targetDirectionDerivative;
+ }
+
}
/** Find mean plane crossing.
@@ -372,30 +384,33 @@ public class SensorMeanPlaneCrossing {
// as we know the solution is improved in the second stage of inverse location.
// We expect two or three evaluations only. Each new evaluation shows up quickly in
// the performances as it involves frames conversions
- final double[] crossingLineHistory = new double[maxEval];
- final DerivativeStructure[] betaHistory = new DerivativeStructure[maxEval];
- boolean atMin = false;
- boolean atMax = false;
+ final double[] crossingLineHistory = new double[maxEval];
+ final double[] betaHistory = new double[maxEval];
+ final double[] betaDerHistory = new double[maxEval];
+ boolean atMin = false;
+ boolean atMax = false;
for (int i = 0; i < maxEval; ++i) {
crossingLineHistory[i] = crossingLine;
- final FieldVector3D<DerivativeStructure> targetDirection =
+ final Vector3D[] targetDirection =
evaluateLine(crossingLine, targetPV, bodyToInert, scToInert);
- betaHistory[i] = FieldVector3D.angle(targetDirection, meanPlaneNormal);
+ betaHistory[i] = FastMath.acos(Vector3D.dotProduct(targetDirection[0], meanPlaneNormal));
+ final double s = Vector3D.dotProduct(targetDirection[1], meanPlaneNormal);
+ betaDerHistory[i] = -s / FastMath.sqrt(1 - s * s);
final double deltaL;
if (i == 0) {
// simple Newton iteration
- deltaL = (0.5 * FastMath.PI - betaHistory[i].getValue()) / betaHistory[i].getPartialDerivative(1);
+ deltaL = (0.5 * FastMath.PI - betaHistory[i]) / betaDerHistory[i];
crossingLine += deltaL;
} else {
// inverse cubic iteration
- final double a0 = betaHistory[i - 1].getValue() - 0.5 * FastMath.PI;
+ final double a0 = betaHistory[i - 1] - 0.5 * FastMath.PI;
final double l0 = crossingLineHistory[i - 1];
- final double d0 = betaHistory[i - 1].getPartialDerivative(1);
- final double a1 = betaHistory[i].getValue() - 0.5 * FastMath.PI;
+ final double d0 = betaDerHistory[i - 1];
+ final double a1 = betaHistory[i] - 0.5 * FastMath.PI;
final double l1 = crossingLineHistory[i];
- final double d1 = betaHistory[i].getPartialDerivative(1);
+ final double d1 = betaDerHistory[i];
final double a1Ma0 = a1 - a0;
crossingLine = ((l0 * (a1 - 3 * a0) - a0 * a1Ma0 / d0) * a1 * a1 +
(l1 * (3 * a1 - a0) - a1 * a1Ma0 / d1) * a0 * a0
@@ -407,7 +422,8 @@ public class SensorMeanPlaneCrossing {
if (cachedResults.size() >= CACHED_RESULTS) {
cachedResults.remove(cachedResults.size() - 1);
}
- cachedResults.add(0, new CrossingResult(sensor.getDate(crossingLine), crossingLine, target, targetDirection));
+ cachedResults.add(0, new CrossingResult(sensor.getDate(crossingLine), crossingLine, target,
+ targetDirection[0], targetDirection[1]));
return cachedResults.get(0);
}
for (int j = 0; j < i; ++j) {
@@ -519,10 +535,9 @@ public class SensorMeanPlaneCrossing {
public double value(final double x) throws RuggedExceptionWrapper {
try {
final AbsoluteDate date = sensor.getDate(x);
- final FieldVector3D<DerivativeStructure> targetDirection =
+ final Vector3D[] targetDirection =
evaluateLine(x, targetPV, scToBody.getBodyToInertial(date), scToBody.getScToInertial(date));
- final DerivativeStructure beta = FieldVector3D.angle(targetDirection, meanPlaneNormal);
- return 0.5 * FastMath.PI - beta.getValue();
+ return 0.5 * FastMath.PI - FastMath.acos(Vector3D.dotProduct(targetDirection[0], meanPlaneNormal));
} catch (RuggedException re) {
throw new RuggedExceptionWrapper(re);
}
@@ -530,9 +545,10 @@ public class SensorMeanPlaneCrossing {
}, minLine, maxLine, startValue);
final AbsoluteDate date = sensor.getDate(crossingLine);
- final FieldVector3D<DerivativeStructure> targetDirection =
+ final Vector3D[] targetDirection =
evaluateLine(crossingLine, targetPV, scToBody.getBodyToInertial(date), scToBody.getScToInertial(date));
- return new CrossingResult(sensor.getDate(crossingLine), crossingLine, targetPV.getPosition(), targetDirection);
+ return new CrossingResult(sensor.getDate(crossingLine), crossingLine, targetPV.getPosition(),
+ targetDirection[0], targetDirection[1]);
} catch (MathIllegalArgumentException nbe) {
return null;
@@ -549,8 +565,8 @@ public class SensorMeanPlaneCrossing {
* @return target direction in spacecraft frame, with its first derivative
* with respect to line number
*/
- private FieldVector3D<DerivativeStructure> evaluateLine(final double lineNumber, final PVCoordinates targetPV,
- final Transform bodyToInert, final Transform scToInert) {
+ private Vector3D[] evaluateLine(final double lineNumber, final PVCoordinates targetPV,
+ final Transform bodyToInert, final Transform scToInert) {
// compute the transform between spacecraft and observed body
final PVCoordinates refInert =
@@ -601,9 +617,9 @@ public class SensorMeanPlaneCrossing {
// 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));
+ return new Vector3D[] {
+ dir, dirDot.scalarMultiply(1.0 / rate)
+ };
}
diff --git a/src/test/java/org/orekit/rugged/TestUtils.java b/src/test/java/org/orekit/rugged/TestUtils.java
index e66380f5..13b365a3 100644
--- a/src/test/java/org/orekit/rugged/TestUtils.java
+++ b/src/test/java/org/orekit/rugged/TestUtils.java
@@ -151,13 +151,13 @@ public class TestUtils {
}
- public static TimeDependentLOS createLOSPerfectLine(Vector3D center, Vector3D normal, double halfAperture, int n) {
+ public static LOSBuilder createLOSPerfectLine(Vector3D center, Vector3D normal, double halfAperture, int n) {
List<Vector3D> list = new ArrayList<Vector3D>(n);
for (int i = 0; i < n; ++i) {
double alpha = (halfAperture * (2 * i + 1 - n)) / (n - 1);
list.add(new Rotation(normal, alpha, RotationConvention.VECTOR_OPERATOR).applyTo(center));
}
- return new LOSBuilder(list).build();
+ return new LOSBuilder(list);
}
public static TimeDependentLOS createLOSCurvedLine(Vector3D center, Vector3D normal,
diff --git a/src/test/java/org/orekit/rugged/api/RuggedTest.java b/src/test/java/org/orekit/rugged/api/RuggedTest.java
index 2b8928e6..12761a26 100644
--- a/src/test/java/org/orekit/rugged/api/RuggedTest.java
+++ b/src/test/java/org/orekit/rugged/api/RuggedTest.java
@@ -17,22 +17,27 @@
package org.orekit.rugged.api;
-import org.hipparchus.geometry.euclidean.threed.Rotation;
-import org.hipparchus.geometry.euclidean.threed.RotationConvention;
-import org.hipparchus.geometry.euclidean.threed.Vector3D;
-import org.hipparchus.stat.descriptive.StreamingStatistics;
-import org.hipparchus.stat.descriptive.rank.Percentile;
-import org.hipparchus.util.FastMath;
import java.io.File;
import java.io.IOException;
import java.io.RandomAccessFile;
+import java.lang.reflect.InvocationTargetException;
+import java.lang.reflect.Method;
import java.net.URISyntaxException;
import java.nio.MappedByteBuffer;
import java.nio.channels.FileChannel;
import java.util.ArrayList;
+import java.util.HashMap;
import java.util.List;
import java.util.Locale;
+import java.util.Map;
+import org.hipparchus.analysis.differentiation.DerivativeStructure;
+import org.hipparchus.geometry.euclidean.threed.Rotation;
+import org.hipparchus.geometry.euclidean.threed.RotationConvention;
+import org.hipparchus.geometry.euclidean.threed.Vector3D;
+import org.hipparchus.stat.descriptive.StreamingStatistics;
+import org.hipparchus.stat.descriptive.rank.Percentile;
+import org.hipparchus.util.FastMath;
import org.junit.Assert;
import org.junit.Ignore;
import org.junit.Rule;
@@ -55,11 +60,13 @@ import org.orekit.rugged.linesensor.LineDatation;
import org.orekit.rugged.linesensor.LineSensor;
import org.orekit.rugged.linesensor.LinearLineDatation;
import org.orekit.rugged.linesensor.SensorPixel;
+import org.orekit.rugged.los.FixedRotation;
import org.orekit.rugged.los.LOSBuilder;
import org.orekit.rugged.los.TimeDependentLOS;
import org.orekit.rugged.raster.RandomLandscapeUpdater;
import org.orekit.rugged.raster.TileUpdater;
import org.orekit.rugged.raster.VolcanicConeElevationUpdater;
+import org.orekit.rugged.utils.ExtendedParameterDriver;
import org.orekit.time.AbsoluteDate;
import org.orekit.time.TimeScale;
import org.orekit.time.TimeScalesFactory;
@@ -105,7 +112,7 @@ public class RuggedTest {
// los: swath in the (YZ) plane, centered at +Z, ±10° aperture, 960 pixels
Vector3D position = new Vector3D(1.5, 0, -0.2);
TimeDependentLOS los = TestUtils.createLOSPerfectLine(Vector3D.PLUS_K, Vector3D.PLUS_I,
- FastMath.toRadians(10.0), dimension);
+ FastMath.toRadians(10.0), dimension).build();
// linear datation model: at reference time we get line 1000, and the rate is one line every 1.5ms
LineDatation lineDatation = new LinearLineDatation(crossing, dimension / 2, 1.0 / 1.5e-3);
@@ -186,7 +193,7 @@ public class RuggedTest {
// los: swath in the (YZ) plane, centered at +Z, ±10° aperture, 960 pixels
Vector3D position = new Vector3D(1.5, 0, -0.2);
TimeDependentLOS los = TestUtils.createLOSPerfectLine(Vector3D.PLUS_K, Vector3D.PLUS_I,
- FastMath.toRadians(10.0), dimension);
+ FastMath.toRadians(10.0), dimension).build();
// linear datation model: at reference time we get line 200, and the rate is one line every 1.5ms
LineDatation lineDatation = new LinearLineDatation(crossing, dimension / 2, 1.0 / 1.5e-3);
@@ -264,7 +271,7 @@ public class RuggedTest {
// los: swath in the (YZ) plane, centered at +Z, ±10° aperture, 960 pixels
Vector3D position = new Vector3D(1.5, 0, -0.2);
TimeDependentLOS los = TestUtils.createLOSPerfectLine(Vector3D.PLUS_K, Vector3D.PLUS_I,
- FastMath.toRadians(10.0), dimension);
+ FastMath.toRadians(10.0), dimension).build();
// linear datation model: at reference time we get line 200, and the rate is one line every 1.5ms
LineDatation lineDatation = new LinearLineDatation(crossing, dimension / 2, 1.0 / 1.5e-3);
@@ -324,7 +331,7 @@ public class RuggedTest {
TimeDependentLOS los = TestUtils.createLOSPerfectLine(new Rotation(Vector3D.PLUS_I,
FastMath.toRadians(50.0),
RotationConvention.VECTOR_OPERATOR).applyTo(Vector3D.PLUS_K),
- Vector3D.PLUS_I, FastMath.toRadians(1.0), dimension);
+ Vector3D.PLUS_I, FastMath.toRadians(1.0), dimension).build();
// 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);
@@ -386,7 +393,7 @@ public class RuggedTest {
TimeDependentLOS los = TestUtils.createLOSPerfectLine(new Rotation(Vector3D.PLUS_I,
FastMath.toRadians(50.0),
RotationConvention.VECTOR_OPERATOR).applyTo(Vector3D.PLUS_K),
- Vector3D.PLUS_I, FastMath.toRadians(1.0), dimension);
+ Vector3D.PLUS_I, FastMath.toRadians(1.0), dimension).build();
// 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);
@@ -444,7 +451,7 @@ public class RuggedTest {
TimeDependentLOS los = TestUtils.createLOSPerfectLine(new Rotation(Vector3D.PLUS_I,
FastMath.toRadians(50.0),
RotationConvention.VECTOR_OPERATOR).applyTo(Vector3D.PLUS_K),
- Vector3D.PLUS_I, FastMath.toRadians(1.0), dimension);
+ Vector3D.PLUS_I, FastMath.toRadians(1.0), dimension).build();
// 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);
@@ -505,7 +512,7 @@ public class RuggedTest {
TimeDependentLOS los = TestUtils.createLOSPerfectLine(new Rotation(Vector3D.PLUS_I,
FastMath.toRadians(50.0),
RotationConvention.VECTOR_OPERATOR).applyTo(Vector3D.PLUS_K),
- Vector3D.PLUS_I, FastMath.toRadians(1.0), dimension);
+ Vector3D.PLUS_I, FastMath.toRadians(1.0), dimension).build();
// 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);
@@ -572,7 +579,7 @@ public class RuggedTest {
TimeDependentLOS los = TestUtils.createLOSPerfectLine(new Rotation(Vector3D.PLUS_I,
FastMath.toRadians(50.0 - 0.001 * i),
RotationConvention.VECTOR_OPERATOR).applyTo(Vector3D.PLUS_K),
- Vector3D.PLUS_I, FastMath.toRadians(dimension * 2.6 / 3600.0), dimension);
+ Vector3D.PLUS_I, FastMath.toRadians(dimension * 2.6 / 3600.0), dimension).build();
// linear datation model: at reference time we get roughly middle line, and the rate is one line every 1.5ms
LineDatation lineDatation = new LinearLineDatation(crossing, i + dimension / 2, 1.0 / 1.5e-3);
@@ -661,7 +668,7 @@ public class RuggedTest {
@Test
public void testInverseLocation()
throws RuggedException, OrekitException, URISyntaxException {
- checkInverseLocation(2000, false, false, 3.0e-7, 5.0e-6);
+ checkInverseLocation(2000, false, false, 4.0e-7, 5.0e-6);
checkInverseLocation(2000, false, true, 1.0e-5, 2.0e-7);
checkInverseLocation(2000, true, false, 4.0e-7, 4.0e-7);
checkInverseLocation(2000, true, true, 2.0e-5, 3.0e-7);
@@ -761,19 +768,19 @@ public class RuggedTest {
Rugged rugged = builder.build();
GeodeticPoint point1 = new GeodeticPoint(0.7053784581520293, -1.7354535645320581, 691.856741468848);
SensorPixel sensorPixel1 = rugged.inverseLocation(lineSensor.getName(), point1, 1, 131328);
- Assert.assertEquals( 2.01472, sensorPixel1.getLineNumber(), 1.0e-5);
+ Assert.assertEquals( 2.01474, sensorPixel1.getLineNumber(), 1.0e-5);
Assert.assertEquals( 2.09271, sensorPixel1.getPixelNumber(), 1.0e-5);
GeodeticPoint point2 = new GeodeticPoint(0.704463899881073, -1.7303503789334154, 648.9200602492216);
SensorPixel sensorPixel2 = rugged.inverseLocation(lineSensor.getName(), point2, 1, 131328);
- Assert.assertEquals( 2.02184, sensorPixel2.getLineNumber(), 1.0e-5);
+ Assert.assertEquals( 2.02185, sensorPixel2.getLineNumber(), 1.0e-5);
Assert.assertEquals( 27.53008, sensorPixel2.getPixelNumber(), 1.0e-5);
GeodeticPoint point3 = new GeodeticPoint(0.7009593480939814, -1.7314283804521957, 588.3075485689468);
SensorPixel sensorPixel3 = rugged.inverseLocation(lineSensor.getName(), point3, 1, 131328);
- Assert.assertEquals(2305.26174, sensorPixel3.getLineNumber(), 1.0e-5);
+ Assert.assertEquals(2305.26101, sensorPixel3.getLineNumber(), 1.0e-5);
Assert.assertEquals( 27.18381, sensorPixel3.getPixelNumber(), 1.0e-5);
GeodeticPoint point4 = new GeodeticPoint(0.7018731669637096, -1.73651769725183, 611.2759403696498);
SensorPixel sensorPixel4 = rugged.inverseLocation(lineSensor.getName(), point4, 1, 131328);
- Assert.assertEquals(2305.25506, sensorPixel4.getLineNumber(), 1.0e-5);
+ Assert.assertEquals(2305.25436, sensorPixel4.getLineNumber(), 1.0e-5);
Assert.assertEquals( 1.54447, sensorPixel4.getPixelNumber(), 1.0e-5);
}
@@ -953,7 +960,7 @@ public class RuggedTest {
FastMath.toRadians(50.0),
RotationConvention.VECTOR_OPERATOR).applyTo(Vector3D.PLUS_K),
Vector3D.PLUS_I,
- FastMath.toRadians(dimension * 2.6 / 3600.0), dimension);
+ FastMath.toRadians(dimension * 2.6 / 3600.0), dimension).build();
// linear datation model: at reference time we get the middle line, and the rate is one line every 1.5ms
LineDatation lineDatation = new LinearLineDatation(crossing, dimension / 2, 1.0 / 1.5e-3);
@@ -1026,6 +1033,183 @@ public class RuggedTest {
}
+ @Test
+ public void testInverseLocationDerivativesWithoutCorrections()
+ throws RuggedException, OrekitException {
+ doTestInverseLocationDerivatives(2000, false, false,
+ 7.0e-9, 4.0e-11, 1.0e-20, 3.0e-5);
+ }
+
+ @Test
+ public void testInverseLocationDerivativesWithLightTimeCorrection()
+ throws RuggedException, OrekitException {
+ doTestInverseLocationDerivatives(2000, true, false,
+ 3.0e-9, 9.0e-9, 1.0e-20, 2.0e-6);
+ }
+
+ @Test
+ public void testInverseLocationDerivativesWithAberrationOfLightCorrection()
+ throws RuggedException, OrekitException {
+ doTestInverseLocationDerivatives(2000, false, true,
+ 3.0e-10, 3.0e-10, 1.0e-20, 2.0e-6);
+ }
+
+ @Test
+ public void testInverseLocationDerivativesWithAllCorrections()
+ throws RuggedException, OrekitException {
+ doTestInverseLocationDerivatives(2000, true, true,
+ 3.0e-10, 5.0e-10, 1.0e-20, 2.0e-6);
+ }
+
+ private void doTestInverseLocationDerivatives(int dimension,
+ boolean lightTimeCorrection,
+ boolean aberrationOfLightCorrection,
+ double lineTolerance,
+ double pixelTolerance,
+ double lineDerivativeTolerance,
+ double pixelDerivativeTolerance)
+ throws RuggedException, OrekitException {
+ try {
+
+ String path = getClass().getClassLoader().getResource("orekit-data").toURI().getPath();
+ DataProvidersManager.getInstance().addProvider(new DirectoryCrawler(new File(path)));
+ final BodyShape earth = TestUtils.createEarth();
+ final Orbit orbit = TestUtils.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, 2.6" per pixel
+ Vector3D position = new Vector3D(1.5, 0, -0.2);
+ LOSBuilder losBuilder =
+ TestUtils.createLOSPerfectLine(new Rotation(Vector3D.PLUS_I,
+ FastMath.toRadians(50.0),
+ RotationConvention.VECTOR_OPERATOR).applyTo(Vector3D.PLUS_K),
+ Vector3D.PLUS_I,
+ FastMath.toRadians(dimension * 2.6 / 3600.0), dimension);
+ losBuilder.addTransform(new FixedRotation("roll", Vector3D.MINUS_I, 0.0));
+ losBuilder.addTransform(new FixedRotation("pitch", Vector3D.MINUS_J, 0.0));
+ TimeDependentLOS los = losBuilder.build();
+
+ // linear datation model: at reference time we get the middle line, and the rate is one line every 1.5ms
+ LineDatation lineDatation = new LinearLineDatation(crossing, dimension / 2, 1.0 / 1.5e-3);
+ int firstLine = 0;
+ int lastLine = dimension;
+ LineSensor lineSensor = new LineSensor("line", lineDatation, position, los);
+ AbsoluteDate minDate = lineSensor.getDate(firstLine).shiftedBy(-1.0);
+ AbsoluteDate maxDate = lineSensor.getDate(lastLine).shiftedBy(+1.0);
+
+ TileUpdater updater =
+ new RandomLandscapeUpdater(0.0, 9000.0, 0.3, 0xf0a401650191f9f6l,
+ FastMath.toRadians(1.0), 257);
+
+ Rugged rugged = new RuggedBuilder().
+ setDigitalElevationModel(updater, 8).
+ setAlgorithm(AlgorithmId.DUVENHAGE).
+ setEllipsoid(EllipsoidId.WGS84, BodyRotatingFrameId.ITRF).
+ setTimeSpan(minDate, maxDate, 0.001, 5.0).
+ setTrajectory(InertialFrameId.EME2000,
+ TestUtils.orbitToPV(orbit, earth, minDate.shiftedBy(-1.0), maxDate.shiftedBy(+1.0), 0.25),
+ 8, CartesianDerivativesFilter.USE_PV,
+ TestUtils.orbitToQ(orbit, earth, minDate.shiftedBy(-1.0), maxDate.shiftedBy(+1.0), 0.25),
+ 2, AngularDerivativesFilter.USE_R).
+ setLightTimeCorrection(lightTimeCorrection).
+ setAberrationOfLightCorrection(aberrationOfLightCorrection).
+ addLineSensor(lineSensor).
+ build();
+
+ // we want to adjust sensor roll and pitch angles
+ ExtendedParameterDriver rollDriver =
+ rugged.getLineSensor("line").getExtendedParametersDrivers().
+ filter(driver -> driver.getName().equals("roll")).findFirst().get();
+ rollDriver.setSelected(true);
+ ExtendedParameterDriver pitchDriver =
+ rugged.getLineSensor("line").getExtendedParametersDrivers().
+ filter(driver -> driver.getName().equals("pitch")).findFirst().get();
+ pitchDriver.setSelected(true);
+
+ // prepare parameters (this is normally done by Rugged.estimateFreeParameters)
+ // this setup may seem complex, but DerivativeStructure are really an
+ // internal feature, user do not need to deal with them
+ final Map<String, int[]> map = new HashMap<>();
+ rugged.getLineSensor("line").getExtendedParametersDrivers().
+ filter(driver -> driver.isSelected()).
+ forEach(driver -> {
+ driver.setNbEstimated(2);
+ driver.setIndex(map.size());
+ int[] orders = new int[2];
+ orders[map.size()] = 1;
+ map.put(driver.getName(), orders);
+ });
+
+ double referenceLine = 0.87654 * dimension;
+ GeodeticPoint[] gp = rugged.directLocation("line", referenceLine);
+
+ Method inverseLoc = Rugged.class.getDeclaredMethod("inverseLocationDerivatives",
+ String.class, GeodeticPoint.class,
+ Integer.TYPE, Integer.TYPE);
+ inverseLoc.setAccessible(true);
+ int referencePixel = (3 * dimension) / 4;
+ DerivativeStructure[] result =
+ (DerivativeStructure[]) inverseLoc.invoke(rugged,
+ "line", gp[referencePixel], 0, dimension);
+ Assert.assertEquals(referenceLine, result[0].getValue(), lineTolerance);
+ Assert.assertEquals(referencePixel, result[1].getValue(), pixelTolerance);
+ Assert.assertEquals(2, result[0].getFreeParameters());
+ Assert.assertEquals(1, result[0].getOrder());
+
+ // check the partial derivatives
+ double h = 1.0e-6;
+
+ rollDriver.setValue(-h);
+ pitchDriver.setValue(0);
+ SensorPixel rMp0 = rugged.inverseLocation("line", gp[referencePixel], 0, dimension);
+ rollDriver.setValue(+h);
+ pitchDriver.setValue(0);
+ SensorPixel rPp0 = rugged.inverseLocation("line", gp[referencePixel], 0, dimension);
+
+// rollDriver.setValue(0);
+// pitchDriver.setValue(-h);
+// SensorPixel r0pM = rugged.inverseLocation("line", gp[referencePixel], 0, dimension);
+// rollDriver.setValue(0);
+// pitchDriver.setValue(+h);
+ // TODO: the following line leads to a singular matrix
+// SensorPixel r0pP = rugged.inverseLocation("line", gp[referencePixel], 0, dimension);
+
+ Assert.assertEquals("dLine/dRoll error : " +
+ ((rPp0.getLineNumber() - rMp0.getLineNumber()) / h -
+ result[0].getPartialDerivative(map.get("roll"))),
+ (rPp0.getLineNumber() - rMp0.getLineNumber()) / h,
+ result[0].getPartialDerivative(map.get("roll")),
+ lineDerivativeTolerance);
+ Assert.assertEquals("dPixel/dRoll error : " +
+ ((rPp0.getPixelNumber() - rMp0.getPixelNumber()) / h -
+ result[1].getPartialDerivative(map.get("roll"))),
+ (rPp0.getPixelNumber() - rMp0.getPixelNumber()) / h,
+ result[1].getPartialDerivative(map.get("roll")),
+ pixelDerivativeTolerance);
+// Assert.assertEquals("dLine/dPitch error : " +
+// ((r0pP.getLineNumber() - r0pM.getLineNumber()) / h -
+// result[0].getPartialDerivative(map.get("pitch"))),
+// (r0pP.getLineNumber() - r0pM.getLineNumber()) / h,
+// result[0].getPartialDerivative(map.get("pitch")),
+// lineDerivativeTolerance);
+// Assert.assertEquals("dPixel/dPitch error : " +
+// ((r0pP.getPixelNumber() - r0pM.getPixelNumber()) / h -
+// result[1].getPartialDerivative(map.get("pitch"))),
+// (r0pP.getPixelNumber() - r0pM.getPixelNumber()) / h,
+// result[1].getPartialDerivative(map.get("picth")),
+// pixelDerivativeTolerance);
+
+ } catch (InvocationTargetException | NoSuchMethodException |
+ SecurityException | IllegalAccessException |
+ IllegalArgumentException | URISyntaxException e) {
+ e.printStackTrace();
+ Assert.fail(e.getLocalizedMessage());
+ }
+ }
+
private void checkDateLocation(int dimension, boolean lightTimeCorrection, boolean aberrationOfLightCorrection,
double maxDateError)
throws RuggedException, OrekitException, URISyntaxException {
@@ -1045,7 +1229,7 @@ public class RuggedTest {
FastMath.toRadians(50.0),
RotationConvention.VECTOR_OPERATOR).applyTo(Vector3D.PLUS_K),
Vector3D.PLUS_I,
- FastMath.toRadians(dimension * 2.6 / 3600.0), dimension);
+ FastMath.toRadians(dimension * 2.6 / 3600.0), dimension).build();
// 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);
diff --git a/src/test/java/org/orekit/rugged/errors/DumpManagerTest.java b/src/test/java/org/orekit/rugged/errors/DumpManagerTest.java
index e9de4c43..98774339 100644
--- a/src/test/java/org/orekit/rugged/errors/DumpManagerTest.java
+++ b/src/test/java/org/orekit/rugged/errors/DumpManagerTest.java
@@ -149,7 +149,7 @@ public class DumpManagerTest {
TimeDependentLOS los = TestUtils.createLOSPerfectLine(new Rotation(Vector3D.PLUS_I,
FastMath.toRadians(50.0),
RotationConvention.VECTOR_OPERATOR).applyTo(Vector3D.PLUS_K),
- Vector3D.PLUS_I, FastMath.toRadians(1.0), dimension);
+ Vector3D.PLUS_I, FastMath.toRadians(1.0), dimension).build();
// 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);
diff --git a/src/test/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossingTest.java b/src/test/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossingTest.java
index 24dd8bd7..3f5125b0 100644
--- a/src/test/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossingTest.java
+++ b/src/test/java/org/orekit/rugged/linesensor/SensorMeanPlaneCrossingTest.java
@@ -16,6 +16,7 @@
*/
package org.orekit.rugged.linesensor;
+import org.hipparchus.geometry.euclidean.threed.Line;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.random.RandomGenerator;
import org.hipparchus.random.Well19937a;
@@ -31,11 +32,13 @@ import org.junit.Test;
import org.orekit.attitudes.NadirPointing;
import org.orekit.attitudes.YawCompensation;
import org.orekit.bodies.BodyShape;
+import org.orekit.bodies.GeodeticPoint;
import org.orekit.bodies.OneAxisEllipsoid;
import org.orekit.data.DataProvidersManager;
import org.orekit.data.DirectoryCrawler;
import org.orekit.errors.OrekitException;
import org.orekit.frames.FramesFactory;
+import org.orekit.frames.Transform;
import org.orekit.orbits.CircularOrbit;
import org.orekit.orbits.Orbit;
import org.orekit.orbits.PositionAngle;
@@ -120,6 +123,99 @@ public class SensorMeanPlaneCrossingTest {
}
+ @Test
+ public void testDerivativeWithoutCorrections() throws RuggedException, OrekitException {
+ doTestDerivative(false, false, 2.2e-11);
+ }
+
+ @Test
+ public void testDerivativeLightTimeCorrection() throws RuggedException, OrekitException {
+ doTestDerivative(true, false, 2.4e-7);
+ }
+
+ @Test
+ public void testDerivativeAberrationOfLightCorrection() throws RuggedException, OrekitException {
+ doTestDerivative(false, true, 1.1e-7);
+ }
+
+ @Test
+ public void testDerivativeWithAllCorrections() throws RuggedException, OrekitException {
+ doTestDerivative(true, true, 1.4e-7);
+ }
+
+ private void doTestDerivative(boolean lightTimeCorrection,
+ boolean aberrationOfLightCorrection,
+ double tol)
+ throws RuggedException, OrekitException {
+
+ final Vector3D position = new Vector3D(1.5, Vector3D.PLUS_I);
+ final Vector3D normal = Vector3D.PLUS_I;
+ final Vector3D fovCenter = Vector3D.PLUS_K;
+ final Vector3D cross = Vector3D.crossProduct(normal, fovCenter);
+
+ // build lists of pixels regularly spread on a perfect plane
+ final List<Vector3D> los = new ArrayList<Vector3D>();
+ for (int i = -1000; i <= 1000; ++i) {
+ final double alpha = i * 0.17 / 1000;
+ los.add(new Vector3D(FastMath.cos(alpha), fovCenter, FastMath.sin(alpha), cross));
+ }
+
+ final LineSensor sensor = new LineSensor("perfect line",
+ new LinearLineDatation(AbsoluteDate.J2000_EPOCH, 0.0, 1.0 / 1.5e-3),
+ position, new LOSBuilder(los).build());
+
+ Assert.assertEquals("perfect line", sensor.getName());
+ Assert.assertEquals(AbsoluteDate.J2000_EPOCH, sensor.getDate(0.0));
+ Assert.assertEquals(0.0, Vector3D.distance(position, sensor.getPosition()), 1.0e-15);
+
+ SensorMeanPlaneCrossing mean = new SensorMeanPlaneCrossing(sensor, createInterpolator(sensor),
+ 0, 2000, lightTimeCorrection,
+ aberrationOfLightCorrection, 50, 1.0e-6);
+
+ double refLine = 1200.0;
+ AbsoluteDate refDate = sensor.getDate(refLine);
+ int refPixel= 1800;
+ Transform b2i = mean.getScToBody().getBodyToInertial(refDate);
+ Transform sc2i = mean.getScToBody().getScToInertial(refDate);
+ Transform sc2b = new Transform(refDate, sc2i, b2i.getInverse());
+ Vector3D p1 = sc2b.transformPosition(position);
+ Vector3D p2 = sc2b.transformPosition(new Vector3D(1, position,
+ 1.0e6, los.get(refPixel)));
+ Line line = new Line(p1, p2, 0.001);
+ BodyShape earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
+ Constants.WGS84_EARTH_FLATTENING,
+ mean.getScToBody().getBodyFrame());
+ GeodeticPoint groundPoint = earth.getIntersectionPoint(line, p1, mean.getScToBody().getBodyFrame(), refDate);
+ Vector3D gpCartesian = earth.transform(groundPoint);
+ SensorMeanPlaneCrossing.CrossingResult result = mean.find(gpCartesian);
+
+ if (lightTimeCorrection) {
+ // applying corrections shifts the point with respect
+ // to the reference result computed from a simple model above
+ Assert.assertTrue(result.getLine() - refLine > 0.02);
+ } else if (aberrationOfLightCorrection) {
+ // applying corrections shifts the point with respect
+ // to the reference result computed from a simple model above
+ Assert.assertTrue(result.getLine() - refLine > 1.9);
+ } else {
+ // the simple model from which reference results have been compute applies here
+ Assert.assertEquals(refLine, result.getLine(), 7.0e-14 * refLine);
+ Assert.assertEquals(0.0, result.getDate().durationFrom(refDate), 2.0e-13);
+ Assert.assertEquals(0.0, Vector3D.angle(los.get(refPixel), result.getTargetDirection()), 5.1e-15);
+ }
+
+ double deltaL = 0.5;
+ Transform b2scPlus = sc2b.getInverse().shiftedBy(deltaL / sensor.getRate(refLine));
+ Vector3D dirPlus = b2scPlus.transformPosition(gpCartesian).subtract(position).normalize();
+ Transform b2scMinus = sc2b.getInverse().shiftedBy(-deltaL / sensor.getRate(refLine));
+ Vector3D dirMinus = b2scMinus.transformPosition(gpCartesian).subtract(position).normalize();
+ Vector3D dirDer = new Vector3D(1.0 / (2 * deltaL), dirPlus.subtract(dirMinus));
+ Assert.assertEquals(0.0,
+ Vector3D.distance(result.getTargetDirectionDerivative(), dirDer),
+ tol * dirDer.getNorm());
+
+ }
+
private SpacecraftToObservedBody createInterpolator(LineSensor sensor)
throws RuggedException, OrekitException {
Orbit orbit = new CircularOrbit(7173352.811913891,
--
GitLab