SensorMeanPlaneCrossing.java
/* Copyright 2013-2016 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 org.apache.commons.math3.analysis.UnivariateFunction;
import org.apache.commons.math3.analysis.differentiation.DerivativeStructure;
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.geometry.euclidean.threed.FieldVector3D;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
import org.apache.commons.math3.linear.Array2DRowRealMatrix;
import org.apache.commons.math3.linear.ArrayRealVector;
import org.apache.commons.math3.linear.DecompositionSolver;
import org.apache.commons.math3.linear.MatrixUtils;
import org.apache.commons.math3.linear.QRDecomposition;
import org.apache.commons.math3.linear.RealMatrix;
import org.apache.commons.math3.linear.RealVector;
import org.apache.commons.math3.linear.SingularMatrixException;
import org.apache.commons.math3.linear.SingularValueDecomposition;
import org.apache.commons.math3.util.FastMath;
import org.apache.commons.math3.util.Precision;
import org.orekit.frames.Transform;
import org.orekit.rugged.errors.RuggedException;
import org.orekit.rugged.errors.RuggedExceptionWrapper;
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 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
* @exception RuggedException if some frame conversion fails
*/
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)
throws RuggedException {
this(sensor, scToBody, minLine, maxLine, lightTimeCorrection, aberrationOfLightCorrection,
maxEval, accuracy, computeMeanPlaneNormal(sensor, minLine, maxLine), new CrossingResult[0]);
}
/** 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
* @exception RuggedException if some frame conversion fails
*/
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 CrossingResult[] cachedResults)
throws RuggedException {
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 CrossingResult[CACHED_RESULTS];
System.arraycopy(cachedResults, 0, this.cachedResults, 0, cachedResults.length);
}
/** 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
* @exception RuggedException if mid date cannot be handled
*/
private static Vector3D computeMeanPlaneNormal(final LineSensor sensor, final int minLine, final int maxLine)
throws RuggedException {
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 Apache Commons Math 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 the cached previous results.
* @return mean plane normal
*/
public CrossingResult[] getCachedResults() {
return cachedResults;
}
/** 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 FieldVector3D<DerivativeStructure> targetDirection;
/** Simple constructor.
* @param crossingDate crossing date
* @param crossingLine crossing line
* @param target target ground point
* @param targetDirection target direction in spacecraft frame
*/
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;
}
/** 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 its first derivative
* with respect to line number
*/
public FieldVector3D<DerivativeStructure> getTargetDirection() {
return targetDirection;
}
}
/** 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
* @exception RuggedException if geometry cannot be computed for some line or
* if the maximum number of evaluations is exceeded
*/
public CrossingResult find(final Vector3D target)
throws RuggedException {
double crossingLine = midLine;
Transform bodyToInert = midBodyToInert;
Transform scToInert = midScToInert;
// count the number of available results
int n = 0;
while (n < cachedResults.length && cachedResults[n] != null) {
++n;
}
if (n >= 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(n, 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 Apache Commons Math solver here because we are more
// interested in reducing the number of evaluations than being accurate,
// as we know the solution is improved in the second stage of inverse 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;
for (int i = 0; i < maxEval; ++i) {
crossingLineHistory[i] = crossingLine;
final FieldVector3D<DerivativeStructure> targetDirection =
evaluateLine(crossingLine, targetPV, bodyToInert, scToInert);
betaHistory[i] = FieldVector3D.angle(targetDirection, meanPlaneNormal);
final double deltaL;
if (i == 0) {
// simple Newton iteration
deltaL = (0.5 * FastMath.PI - betaHistory[i].getValue()) / betaHistory[i].getPartialDerivative(1);
crossingLine += deltaL;
} else {
// inverse cubic iteration
final double a0 = betaHistory[i - 1].getValue() - 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 l1 = crossingLineHistory[i];
final double d1 = betaHistory[i].getPartialDerivative(1);
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!
for (int k = cachedResults.length - 1; k > 0; --k) {
cachedResults[k] = cachedResults[k - 1];
}
cachedResults[0] = new CrossingResult(sensor.getDate(crossingLine), crossingLine, target, targetDirection);
return cachedResults[0];
}
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;
}
for (int k = cachedResults.length - 1; k > 0; --k) {
cachedResults[k] = cachedResults[k - 1];
}
cachedResults[0] = slowResult;
return cachedResults[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 n number of cached results available
* @param target target ground point
* @return guessed start line
*/
private double guessStartLine(final int n, final Vector3D target) {
try {
// assume a linear model of the form: l = ax + by + cz + d
final RealMatrix m = new Array2DRowRealMatrix(n, 4);
final RealVector v = new ArrayRealVector(n);
for (int i = 0; i < n; ++i) {
m.setEntry(i, 0, cachedResults[i].getTarget().getX());
m.setEntry(i, 1, cachedResults[i].getTarget().getY());
m.setEntry(i, 2, cachedResults[i].getTarget().getZ());
m.setEntry(i, 3, 1.0);
v.setEntry(i, cachedResults[i].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 (SingularMatrixException 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
* @exception RuggedException if geometry cannot be computed for some line or
* if the maximum number of evaluations is exceeded
*/
public CrossingResult slowFind(final PVCoordinates targetPV, final double initialGuess)
throws RuggedException {
try {
// safety check
final double startValue;
if (initialGuess < minLine || initialGuess > maxLine) {
startValue = midLine;
} else {
startValue = initialGuess;
}
final UnivariateSolver solver = new BracketingNthOrderBrentSolver(accuracy, 5);
double crossingLine = solver.solve(maxEval, new UnivariateFunction() {
/** {@inheritDoc} */
@Override
public double value(final double x) throws RuggedExceptionWrapper {
try {
final AbsoluteDate date = sensor.getDate(x);
final FieldVector3D<DerivativeStructure> targetDirection =
evaluateLine(x, targetPV, scToBody.getBodyToInertial(date), scToBody.getScToInertial(date));
final DerivativeStructure beta = FieldVector3D.angle(targetDirection, meanPlaneNormal);
return 0.5 * FastMath.PI - beta.getValue();
} catch (RuggedException re) {
throw new RuggedExceptionWrapper(re);
}
}
}, minLine, maxLine, startValue);
final AbsoluteDate date = sensor.getDate(crossingLine);
final FieldVector3D<DerivativeStructure> targetDirection =
evaluateLine(crossingLine, targetPV, scToBody.getBodyToInertial(date), scToBody.getScToInertial(date));
return new CrossingResult(sensor.getDate(crossingLine), crossingLine, targetPV.getPosition(), targetDirection);
} catch (NoBracketingException nbe) {
return null;
} catch (RuggedExceptionWrapper rew) {
throw rew.getException();
}
}
/** 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 FieldVector3D<DerivativeStructure> 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 FieldVector3D<DerivativeStructure>(new DerivativeStructure(1, 1, dir.getX(), dirDot.getX() / rate),
new DerivativeStructure(1, 1, dir.getY(), dirDot.getY() / rate),
new DerivativeStructure(1, 1, dir.getZ(), dirDot.getZ() / rate));
}
/** Compute the derivative of normalized vector.
* @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);
}
}