SensorMeanPlaneCrossing.java
/* Copyright 2013-2019 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.linesensor;
import java.util.ArrayList;
import java.util.List;
import java.util.stream.Stream;
import org.hipparchus.analysis.UnivariateFunction;
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.Vector3D;
import org.hipparchus.linear.Array2DRowRealMatrix;
import org.hipparchus.linear.ArrayRealVector;
import org.hipparchus.linear.DecompositionSolver;
import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.QRDecomposition;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.linear.RealVector;
import org.hipparchus.linear.SingularValueDecomposition;
import org.hipparchus.util.FastMath;
import org.hipparchus.util.Precision;
import org.orekit.frames.Transform;
import org.orekit.rugged.errors.RuggedException;
import org.orekit.rugged.errors.RuggedInternalError;
import org.orekit.rugged.utils.SpacecraftToObservedBody;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.Constants;
import org.orekit.utils.PVCoordinates;
/** Class dedicated to find when ground point crosses mean sensor plane.
* <p>
* This class is used in the first stage of inverse location.
* </p>
* @author Luc Maisonobe
*/
public class SensorMeanPlaneCrossing {
/** Number of cached results for guessing the line number. */
private static final int CACHED_RESULTS = 6;
/** Converter between spacecraft and body. */
private final SpacecraftToObservedBody scToBody;
/** Middle line. */
private final double midLine;
/** Body to inertial transform for middle line. */
private final Transform midBodyToInert;
/** Spacecraft to inertial transform for middle line. */
private final Transform midScToInert;
/** 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 final boolean lightTimeCorrection;
/** Flag for aberration of light correction. */
private final boolean aberrationOfLightCorrection;
/** Line sensor. */
private final LineSensor sensor;
/** Sensor mean plane normal. */
private final Vector3D meanPlaneNormal;
/** Maximum number of evaluations. */
private final int maxEval;
/** Accuracy to use for finding crossing line number. */
private final double accuracy;
/** Cached results. */
private final List<CrossingResult> cachedResults;
/** Simple constructor.
* @param sensor sensor to consider
* @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
*/
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, scToBody, minLine, maxLine, lightTimeCorrection, aberrationOfLightCorrection,
maxEval, accuracy, computeMeanPlaneNormal(sensor, minLine, maxLine),
Stream.<CrossingResult>empty());
}
/** Simple constructor.
* @param sensor sensor to consider
* @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
* @param meanPlaneNormal mean plane normal
* @param cachedResults cached results
*/
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,
final Vector3D meanPlaneNormal,
final Stream<CrossingResult> cachedResults) {
this.sensor = sensor;
this.minLine = minLine;
this.maxLine = maxLine;
this.lightTimeCorrection = lightTimeCorrection;
this.aberrationOfLightCorrection = aberrationOfLightCorrection;
this.maxEval = maxEval;
this.accuracy = accuracy;
this.scToBody = scToBody;
this.midLine = 0.5 * (minLine + maxLine);
final AbsoluteDate midDate = sensor.getDate(midLine);
this.midBodyToInert = scToBody.getBodyToInertial(midDate);
this.midScToInert = scToBody.getScToInertial(midDate);
this.meanPlaneNormal = meanPlaneNormal;
this.cachedResults = new ArrayList<CrossingResult>(CACHED_RESULTS);
cachedResults.forEach(crossingResult -> {
if (crossingResult != null && this.cachedResults.size() < CACHED_RESULTS) {
this.cachedResults.add(crossingResult);
}
});
}
/** Compute the plane containing origin that best fits viewing directions point cloud.
* @param sensor line sensor
* @param minLine minimum line number
* @param maxLine maximum line number
* <p>
* The normal is oriented such that traversing pixels in increasing indices
* order corresponds to trigonometric order (i.e. counterclockwise).
* </p>
* @return normal of the mean plane
*/
private static Vector3D computeMeanPlaneNormal(final LineSensor sensor, final int minLine, final int maxLine) {
final AbsoluteDate midDate = sensor.getDate(0.5 * (minLine + maxLine));
// build a centered data matrix
// (for each viewing direction, we add both the direction and its
// opposite, thus ensuring the plane will contain origin)
final RealMatrix matrix = MatrixUtils.createRealMatrix(3, 2 * sensor.getNbPixels());
for (int i = 0; i < sensor.getNbPixels(); ++i) {
final Vector3D l = sensor.getLOS(midDate, i);
matrix.setEntry(0, 2 * i, l.getX());
matrix.setEntry(1, 2 * i, l.getY());
matrix.setEntry(2, 2 * i, l.getZ());
matrix.setEntry(0, 2 * i + 1, -l.getX());
matrix.setEntry(1, 2 * i + 1, -l.getY());
matrix.setEntry(2, 2 * i + 1, -l.getZ());
}
// compute Singular Value Decomposition
final SingularValueDecomposition svd = new SingularValueDecomposition(matrix);
// extract the left singular vector corresponding to least singular value
// (i.e. last vector since Hipparchus returns the values
// in non-increasing order)
final Vector3D singularVector = new Vector3D(svd.getU().getColumn(2)).normalize();
// check rotation order
final Vector3D first = sensor.getLOS(midDate, 0);
final Vector3D last = sensor.getLOS(midDate, sensor.getNbPixels() - 1);
if (Vector3D.dotProduct(singularVector, Vector3D.crossProduct(first, last)) >= 0) {
return singularVector;
} else {
return singularVector.negate();
}
}
/** Get the underlying sensor.
* @return underlying sensor
*/
public LineSensor getSensor() {
return sensor;
}
/** Get converter between spacecraft and body.
* @return converter between spacecraft and body
*/
public SpacecraftToObservedBody getScToBody() {
return scToBody;
}
/** 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;
}
/** Get the maximum number of evaluations.
* @return maximum number of evaluations
*/
public int getMaxEval() {
return maxEval;
}
/** Get the accuracy to use for finding crossing line number.
* @return accuracy to use for finding crossing line number
*/
public double getAccuracy() {
return accuracy;
}
/** Get the mean plane normal.
* <p>
* The normal is oriented such traversing pixels in increasing indices
* order corresponds is consistent with trigonometric order (i.e.
* counterclockwise).
* </p>
* @return mean plane normal
*/
public Vector3D getMeanPlaneNormal() {
return meanPlaneNormal;
}
/** Get cached previous results.
* @return cached previous results
*/
public Stream<CrossingResult> getCachedResults() {
return cachedResults.stream();
}
/** Container for mean plane crossing result. */
public static class CrossingResult {
/** Crossing date. */
private final AbsoluteDate crossingDate;
/** Crossing line. */
private final double crossingLine;
/** Target ground point. */
private final Vector3D target;
/** Target direction in spacecraft frame. */
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 Vector3D targetDirection,
final Vector3D targetDirectionDerivative) {
this.crossingDate = crossingDate;
this.crossingLine = crossingLine;
this.target = target;
this.targetDirection = targetDirection;
this.targetDirectionDerivative = targetDirectionDerivative;
}
/** Get the crossing date.
* @return crossing date
*/
public AbsoluteDate getDate() {
return crossingDate;
}
/** Get the crossing line.
* @return crossing line
*/
public double getLine() {
return crossingLine;
}
/** Get the target ground point.
* @return target ground point
*/
public Vector3D getTarget() {
return target;
}
/** Get the normalized target direction in spacecraft frame at crossing.
* @return normalized target direction in spacecraft frame at crossing
* with respect to line number
*/
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
* @since 2.0
*/
public Vector3D getTargetDirectionDerivative() {
return targetDirectionDerivative;
}
}
/** Find mean plane crossing.
* @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
*/
public CrossingResult find(final Vector3D target) {
double crossingLine = midLine;
Transform bodyToInert = midBodyToInert;
Transform scToInert = midScToInert;
// count the number of available results
if (cachedResults.size() >= 4) {
// we already have computed at lest 4 values, we attempt to build a linear
// model to guess a better start line
final double guessedCrossingLine = guessStartLine(target);
if (guessedCrossingLine >= minLine && guessedCrossingLine <= maxLine) {
crossingLine = guessedCrossingLine;
final AbsoluteDate date = sensor.getDate(crossingLine);
bodyToInert = scToBody.getBodyToInertial(date);
scToInert = scToBody.getScToInertial(date);
}
}
final PVCoordinates targetPV = new PVCoordinates(target, Vector3D.ZERO);
// we don't use an Hipparchus 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 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 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 Vector3D[] targetDirection =
evaluateLine(crossingLine, targetPV, bodyToInert, scToInert);
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]) / betaDerHistory[i];
crossingLine += deltaL;
} else {
// inverse cubic iteration
final double a0 = betaHistory[i - 1] - 0.5 * FastMath.PI;
final double l0 = crossingLineHistory[i - 1];
final double d0 = betaDerHistory[i - 1];
final double a1 = betaHistory[i] - 0.5 * FastMath.PI;
final double l1 = crossingLineHistory[i];
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
) / (a1Ma0 * a1Ma0 * a1Ma0);
deltaL = crossingLine - l1;
}
if (FastMath.abs(deltaL) <= accuracy) {
// return immediately, without doing any additional evaluation!
final CrossingResult crossingResult =
new CrossingResult(sensor.getDate(crossingLine), crossingLine, target,
targetDirection[0], targetDirection[1]);
boolean isNew = true;
for (final CrossingResult existing : cachedResults) {
isNew = isNew && FastMath.abs(crossingLine - existing.crossingLine) > accuracy;
}
if (isNew) {
// this result is different from the existing ones,
// it brings new sampling data to the cache
if (cachedResults.size() >= CACHED_RESULTS) {
cachedResults.remove(cachedResults.size() - 1);
}
cachedResults.add(0, crossingResult);
}
return crossingResult;
}
for (int j = 0; j < i; ++j) {
if (FastMath.abs(crossingLine - crossingLineHistory[j]) <= 1.0) {
// rare case: we are stuck in a loop!
// switch to a more robust (but slower) algorithm in this case
final CrossingResult slowResult = slowFind(targetPV, crossingLine);
if (slowResult == null) {
return null;
}
if (cachedResults.size() >= CACHED_RESULTS) {
cachedResults.remove(cachedResults.size() - 1);
}
cachedResults.add(0, slowResult);
return cachedResults.get(0);
}
}
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.getBodyToInertial(date);
scToInert = scToBody.getScToInertial(date);
}
return null;
}
/** Guess a start line using the last four results.
* @param target target ground point
* @return guessed start line
*/
private double guessStartLine(final Vector3D target) {
try {
// assume a linear model of the form: l = ax + by + cz + d
final int n = cachedResults.size();
final RealMatrix m = new Array2DRowRealMatrix(n, 4);
final RealVector v = new ArrayRealVector(n);
for (int i = 0; i < n; ++i) {
final CrossingResult crossingResult = cachedResults.get(i);
m.setEntry(i, 0, crossingResult.getTarget().getX());
m.setEntry(i, 1, crossingResult.getTarget().getY());
m.setEntry(i, 2, crossingResult.getTarget().getZ());
m.setEntry(i, 3, 1.0);
v.setEntry(i, crossingResult.getLine());
}
final DecompositionSolver solver = new QRDecomposition(m, Precision.SAFE_MIN).getSolver();
final RealVector coefficients = solver.solve(v);
// apply the linear model
return target.getX() * coefficients.getEntry(0) +
target.getY() * coefficients.getEntry(1) +
target.getZ() * coefficients.getEntry(2) +
coefficients.getEntry(3);
} catch (MathIllegalStateException sme) {
// the points are not independent
return Double.NaN;
}
}
/** Find mean plane crossing using a slow but robust method.
* @param targetPV target ground point
* @param initialGuess initial guess for the crossing line
* @return line number and target direction at mean plane crossing,
* or null if search interval does not bracket a solution
*/
private CrossingResult slowFind(final PVCoordinates targetPV, final double initialGuess) {
try {
// safety check
final double startValue;
if (initialGuess < minLine || initialGuess > maxLine) {
startValue = midLine;
} else {
startValue = initialGuess;
}
final UnivariateSolver solver = new BracketingNthOrderBrentSolver(accuracy, 5);
final double crossingLine = solver.solve(maxEval, new UnivariateFunction() {
/** {@inheritDoc} */
@Override
public double value(final double x) {
try {
final AbsoluteDate date = sensor.getDate(x);
final Vector3D[] targetDirection =
evaluateLine(x, targetPV, scToBody.getBodyToInertial(date), scToBody.getScToInertial(date));
return 0.5 * FastMath.PI - FastMath.acos(Vector3D.dotProduct(targetDirection[0], meanPlaneNormal));
} catch (RuggedException re) {
throw new RuggedInternalError(re);
}
}
}, minLine, maxLine, startValue);
final AbsoluteDate date = sensor.getDate(crossingLine);
final Vector3D[] targetDirection =
evaluateLine(crossingLine, targetPV, scToBody.getBodyToInertial(date), scToBody.getScToInertial(date));
return new CrossingResult(sensor.getDate(crossingLine), crossingLine, targetPV.getPosition(),
targetDirection[0], targetDirection[1]);
} catch (MathIllegalArgumentException nbe) {
return null;
}
}
/** 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 current line
* @param scToInert transform from inertial frame to spacecraft frame, for current line
* @return target direction in spacecraft frame, with its first derivative
* with respect to line number
*/
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 =
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 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 Vector3D[] {
dir, dirDot.scalarMultiply(1.0 / rate)
};
}
/** Compute the derivative of normalized vector.
* @param u base vector
* @param uDot derivative of the base vector
* @return vDot, where v = u / ||u||
*/
private Vector3D normalizedDot(final Vector3D u, final Vector3D uDot) {
final double n = u.getNorm();
return new Vector3D(1.0 / n, uDot, -Vector3D.dotProduct(u, uDot) / (n * n * n), u);
}
}