Skip to content
Snippets Groups Projects
Commit ff85e44f authored by Luc Maisonobe's avatar Luc Maisonobe
Browse files

Work In Progress on a faster inverse localization algorithm.

parent e8824cdd
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/main/java/org/orekit/rugged/api/LineSensor.java
+ 103
− 13
View file @ ff85e44f
...@@ -22,6 +22,7 @@ import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; ...@@ -22,6 +22,7 @@ import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
import org.apache.commons.math3.linear.MatrixUtils; import org.apache.commons.math3.linear.MatrixUtils;
import org.apache.commons.math3.linear.RealMatrix; import org.apache.commons.math3.linear.RealMatrix;
import org.apache.commons.math3.linear.SingularValueDecomposition; import org.apache.commons.math3.linear.SingularValueDecomposition;
import org.apache.commons.math3.util.FastMath;
import org.orekit.time.AbsoluteDate; import org.orekit.time.AbsoluteDate;
/** Line sensor model. /** Line sensor model.
...@@ -42,7 +43,16 @@ public class LineSensor { ...@@ -42,7 +43,16 @@ public class LineSensor {
private final Vector3D position; private final Vector3D position;
/** Pixels lines-of-sight. */ /** Pixels lines-of-sight. */
private final List<Vector3D> los; private final Vector3D[] x;
/** Pixels transversal direction (i.e. towards left pixel). */
private final Vector3D[] y;
/** Pixels cross direction (i.e. above line-of-sight). */
private final Vector3D[] z;
/** Pixels widths. */
private final double[] width;
/** Mean plane normal. */ /** Mean plane normal. */
private final Vector3D normal; private final Vector3D normal;
...@@ -59,7 +69,12 @@ public class LineSensor { ...@@ -59,7 +69,12 @@ public class LineSensor {
this.name = name; this.name = name;
this.datationModel = datationModel; this.datationModel = datationModel;
this.position = position; this.position = position;
this.los = los;
// normalize lines-of-sight
this.x = new Vector3D[los.size()];
for (int i = 0; i < los.size(); ++i) {
x[i] = los.get(i).normalize();
}
// we consider the viewing directions as a point cloud // we consider the viewing directions as a point cloud
// and want to find the plane that best fits it // and want to find the plane that best fits it
...@@ -68,20 +83,20 @@ public class LineSensor { ...@@ -68,20 +83,20 @@ public class LineSensor {
double centroidX = 0; double centroidX = 0;
double centroidY = 0; double centroidY = 0;
double centroidZ = 0; double centroidZ = 0;
for (int i = 0; i < los.size(); ++i) { for (int i = 0; i < x.length; ++i) {
final Vector3D l = los.get(i); final Vector3D l = x[i];
centroidX += l.getX(); centroidX += l.getX();
centroidY += l.getY(); centroidY += l.getY();
centroidZ += l.getZ(); centroidZ += l.getZ();
} }
centroidX /= los.size(); centroidX /= x.length;
centroidY /= los.size(); centroidY /= x.length;
centroidZ /= los.size(); centroidZ /= x.length;
// build a centered data matrix // build a centered data matrix
final RealMatrix matrix = MatrixUtils.createRealMatrix(3, los.size()); final RealMatrix matrix = MatrixUtils.createRealMatrix(3, los.size());
for (int i = 0; i < los.size(); ++i) { for (int i = 0; i < x.length; ++i) {
final Vector3D l = los.get(i); final Vector3D l = x[i];
matrix.setEntry(0, i, l.getX() - centroidX); matrix.setEntry(0, i, l.getX() - centroidX);
matrix.setEntry(1, i, l.getY() - centroidY); matrix.setEntry(1, i, l.getY() - centroidY);
matrix.setEntry(2, i, l.getZ() - centroidZ); matrix.setEntry(2, i, l.getZ() - centroidZ);
...@@ -102,6 +117,26 @@ public class LineSensor { ...@@ -102,6 +117,26 @@ public class LineSensor {
normal = singularVector.negate(); normal = singularVector.negate();
} }
// compute transversal directions
y = new Vector3D[x.length];
z = new Vector3D[x.length];
for (int i = 0; i < x.length; ++i) {
y[i] = Vector3D.crossProduct(normal, x[i]).normalize();
z[i] = Vector3D.crossProduct(x[i], y[i]);
}
// compute pixel widths
width = new double[x.length];
for (int i = 0; i < x.length; ++i) {
if (i < 1) {
width[i] = getAzimuth(los.get(i + 1), i);
} else if (i > x.length - 2) {
width[i] = -getAzimuth(los.get(i - 1), i);
} else {
width[i] = 0.5 * (getAzimuth(los.get(i + 1), i) - getAzimuth(los.get(i - 1), i));
}
}
} }
/** Get the name of the sensor. /** Get the name of the sensor.
...@@ -115,15 +150,52 @@ public class LineSensor { ...@@ -115,15 +150,52 @@ public class LineSensor {
* @return number of pixels * @return number of pixels
*/ */
public int getNbPixels() { public int getNbPixels() {
return los.size(); return x.length;
} }
/** Get the pixel line-of-sight. /** Get the pixel normalized line-of-sight.
* <p>
* The {@link #getLos(int) line-of-sight}, {@link #getTransversal(int) transversal}
* and {@link #getCross(int) cross} directions form a right-handed frame aligned
* with the pixel.
* </p>
* @param i pixel index (must be between 0 and {@link #getNbPixels()} * @param i pixel index (must be between 0 and {@link #getNbPixels()}
* @return pixel line-of-sight * @return pixel normalized line-of-sight
*/ */
public Vector3D getLos(final int i) { public Vector3D getLos(final int i) {
return los.get(i); return x[i];
}
/** Get the pixel normalized transversal direction.
* <p>
* The transversal direction is towards left pixel.
* </p>
* <p>
* The {@link #getLos(int) line-of-sight}, {@link #getTransversal(int) transversal}
* and {@link #getCross(int) cross} directions form a right-handed frame aligned
* with the pixel.
* </p>
* @param i pixel index (must be between 0 and {@link #getNbPixels()}
* @return pixel normalized transversal direction
*/
public Vector3D getTransversal(final int i) {
return y[i];
}
/** Get the pixel normalized cross direction.
* <p>
* The cross direction is above sensor lines.
* </p>
* <p>
* The {@link #getLos(int) line-of-sight}, {@link #getTransversal(int) transversal}
* and {@link #getCross(int) cross} directions form a right-handed frame aligned
* with the pixel.
* </p>
* @param i pixel index (must be between 0 and {@link #getNbPixels()}
* @return pixel normalized cross direction
*/
public Vector3D getCross(final int i) {
return z[i];
} }
/** Get the date. /** Get the date.
...@@ -161,4 +233,22 @@ public class LineSensor { ...@@ -161,4 +233,22 @@ public class LineSensor {
return position; return position;
} }
/** Get the relative azimuth of a direction with respect to a pixel.
* @param direction direction to check
* @param i pixel index (must be between 0 and {@link #getNbPixels()}
* @return relative azimuth of direction
*/
public double getAzimuth(final Vector3D direction, final int i) {
return FastMath.atan2(Vector3D.dotProduct(direction, y[i]),
Vector3D.dotProduct(direction, x[i]));
}
/** Get the the angular width a pixel.
* @param i pixel index (must be between 0 and {@link #getNbPixels()}
* @return relative azimuth of direction
*/
public double getWidth(final int i) {
return width[i];
}
} }
src/main/java/org/orekit/rugged/api/Rugged.java
+ 44
− 268
View file @ ff85e44f
...@@ -21,23 +21,16 @@ import java.util.HashMap; ...@@ -21,23 +21,16 @@ import java.util.HashMap;
import java.util.List; import java.util.List;
import java.util.Map; import java.util.Map;
import org.apache.commons.math3.analysis.UnivariateFunction;
import org.apache.commons.math3.analysis.solvers.BracketingNthOrderBrentSolver;
import org.apache.commons.math3.analysis.solvers.UnivariateSolver;
import org.apache.commons.math3.exception.NoBracketingException;
import org.apache.commons.math3.exception.TooManyEvaluationsException;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation; import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
import org.apache.commons.math3.util.FastMath; import org.apache.commons.math3.util.FastMath;
import org.apache.commons.math3.util.Pair; import org.apache.commons.math3.util.Pair;
import org.apache.commons.math3.util.Precision;
import org.orekit.attitudes.Attitude; import org.orekit.attitudes.Attitude;
import org.orekit.attitudes.AttitudeProvider; import org.orekit.attitudes.AttitudeProvider;
import org.orekit.attitudes.TabulatedProvider; import org.orekit.attitudes.TabulatedProvider;
import org.orekit.bodies.GeodeticPoint; import org.orekit.bodies.GeodeticPoint;
import org.orekit.bodies.OneAxisEllipsoid; import org.orekit.bodies.OneAxisEllipsoid;
import org.orekit.errors.OrekitException; import org.orekit.errors.OrekitException;
import org.orekit.errors.OrekitExceptionWrapper;
import org.orekit.frames.Frame; import org.orekit.frames.Frame;
import org.orekit.frames.FramesFactory; import org.orekit.frames.FramesFactory;
import org.orekit.frames.Transform; import org.orekit.frames.Transform;
...@@ -46,7 +39,6 @@ import org.orekit.orbits.Orbit; ...@@ -46,7 +39,6 @@ import org.orekit.orbits.Orbit;
import org.orekit.propagation.Propagator; import org.orekit.propagation.Propagator;
import org.orekit.rugged.core.BasicScanAlgorithm; import org.orekit.rugged.core.BasicScanAlgorithm;
import org.orekit.rugged.core.ExtendedEllipsoid; import org.orekit.rugged.core.ExtendedEllipsoid;
import org.orekit.rugged.core.FixedAltitudeAlgorithm;
import org.orekit.rugged.core.IgnoreDEMAlgorithm; import org.orekit.rugged.core.IgnoreDEMAlgorithm;
import org.orekit.rugged.core.SpacecraftToObservedBody; import org.orekit.rugged.core.SpacecraftToObservedBody;
import org.orekit.rugged.core.duvenhage.DuvenhageAlgorithm; import org.orekit.rugged.core.duvenhage.DuvenhageAlgorithm;
...@@ -66,14 +58,13 @@ public class Rugged { ...@@ -66,14 +58,13 @@ public class Rugged {
/** Accuracy to use in the first stage of inverse localization. /** Accuracy to use in the first stage of inverse localization.
* <p> * <p>
* This accuracy is only used to locate the point within one * This accuracy is only used to locate the point within one
* pixel, so four neighboring corners can be estimated. Hence * pixel, hence there is no point in choosing a too small value here.
* there is no point in choosing a too small value here.
* </p> * </p>
*/ */
private static final double COARSE_INVERSE_LOCALIZATION_ACCURACY = 0.01; private static final double COARSE_INVERSE_LOCALIZATION_ACCURACY = 0.1;
/** Maximum number of evaluations for inverse localization. */ /** Maximum number of evaluations. */
private static final int MAX_EVAL = 1000; private static final int MAX_EVAL = 50;
/** Reference ellipsoid. */ /** Reference ellipsoid. */
private final ExtendedEllipsoid ellipsoid; private final ExtendedEllipsoid ellipsoid;
...@@ -419,8 +410,10 @@ public class Rugged { ...@@ -419,8 +410,10 @@ public class Rugged {
* @param inertialFrame inertial frame where position and velocity are defined * @param inertialFrame inertial frame where position and velocity are defined
* @return selected position/velocity provider * @return selected position/velocity provider
*/ */
private static PVCoordinatesProvider selectPVCoordinatesProvider(final List<Pair<AbsoluteDate, PVCoordinates>> positionsVelocities, private static PVCoordinatesProvider selectPVCoordinatesProvider(final List<Pair<AbsoluteDate,
final int interpolationOrder, final Frame inertialFrame) { PVCoordinates>> positionsVelocities,
final int interpolationOrder,
final Frame inertialFrame) {
// set up the ephemeris // set up the ephemeris
final List<Orbit> orbits = new ArrayList<Orbit>(positionsVelocities.size()); final List<Orbit> orbits = new ArrayList<Orbit>(positionsVelocities.size());
...@@ -574,265 +567,48 @@ public class Rugged { ...@@ -574,265 +567,48 @@ public class Rugged {
public SensorPixel inverseLocalization(final String sensorName, final GeodeticPoint groundPoint, public SensorPixel inverseLocalization(final String sensorName, final GeodeticPoint groundPoint,
final double minLine, final double maxLine) final double minLine, final double maxLine)
throws RuggedException { throws RuggedException {
try {
final LineSensor sensor = getLineSensor(sensorName);
final Vector3D target = ellipsoid.transform(groundPoint);
final UnivariateSolver coarseSolver =
new BracketingNthOrderBrentSolver(0.0, COARSE_INVERSE_LOCALIZATION_ACCURACY, 5);
// find approximately the sensor line at which ground point crosses sensor mean plane
final SensorMeanPlaneCrossing planeCrossing = new SensorMeanPlaneCrossing(sensor, target);
final double coarseLine = coarseSolver.solve(MAX_EVAL, planeCrossing, minLine, maxLine);
// find approximately the pixel along this sensor line
final SensorPixelCrossing pixelCrossing =
new SensorPixelCrossing(sensor, planeCrossing.getTargetDirection(coarseLine));
final double coarsePixel = coarseSolver.solve(MAX_EVAL, pixelCrossing, -1.0, sensor.getNbPixels());
// compute a quadrilateral that should surround the target
final double lInf = FastMath.floor(coarseLine);
final int pInf = FastMath.max(0, FastMath.min(sensor.getNbPixels() - 2, (int) FastMath.floor(coarsePixel)));
final IntersectionAlgorithm alg = new FixedAltitudeAlgorithm(groundPoint.getAltitude());
final GeodeticPoint[] previous = directLocalization(sensor, pInf, pInf + 2, alg, lInf);
final GeodeticPoint[] next = directLocalization(sensor, pInf, pInf + 2, alg, lInf + 1);
final double[] interp = interpolationCoordinates(groundPoint.getLongitude(), groundPoint.getLatitude(),
previous[0].getLongitude(), previous[0].getLatitude(),
previous[1].getLongitude(), previous[1].getLatitude(),
next[0].getLongitude(), next[0].getLatitude(),
next[1].getLongitude(), next[1].getLatitude());
return (interp == null) ? null : new SensorPixel(lInf + interp[1], pInf + interp[0]);
} catch (NoBracketingException nbe) { final LineSensor sensor = getLineSensor(sensorName);
// there are no solutions in the search interval final Vector3D target = ellipsoid.transform(groundPoint);
// find approximately the sensor line at which ground point crosses sensor mean plane
final SensorMeanPlaneCrossing planeCrossing = new SensorMeanPlaneCrossing(sensor, target, scToBody,
lightTimeCorrection,
aberrationOfLightCorrection,
MAX_EVAL,
COARSE_INVERSE_LOCALIZATION_ACCURACY);
if (!planeCrossing.find(minLine, maxLine)) {
// target is out of search interval
return null; return null;
} catch (TooManyEvaluationsException tmee) {
throw new RuggedException(tmee);
} catch (OrekitExceptionWrapper oew) {
final OrekitException oe = oew.getException();
throw new RuggedException(oe, oe.getSpecifier(), oe.getParts());
}
}
/** Compute a point bilinear interpolation coordinates.
* <p>
* This method is used to find interpolation coordinates when the
* quadrilateral is <em>not</em> a perfect rectangle.
* </p>
* @param xuv desired point abscissa, at interpolation coordinates u, v
* @param yuv desired point ordinate, at interpolation coordinates u, v
* @param x00 abscissa for base quadrilateral point at u = 0, v = 0
* @param y00 ordinate for base quadrilateral point at u = 0, v = 0
* @param x10 abscissa for base quadrilateral point at u = 1, v = 0
* @param y10 ordinate for base quadrilateral point at u = 1, v = 0
* @param x01 abscissa for base quadrilateral point at u = 0, v = 1
* @param y01 ordinate for base quadrilateral point at u = 0, v = 1
* @param x11 abscissa for base quadrilateral point at u = 1, v = 1
* @param y11 ordinate for base quadrilateral point at u = 1, v = 1
* @return interpolation coordinates {@code u, v} corresponding to point {@code xuv, yuv},
* or null if no such points can be found
*/
private double[] interpolationCoordinates(final double xuv, final double yuv,
final double x00, final double y00,
final double x10, final double y10,
final double x01, final double y01,
final double x11, final double y11) {
// bilinear interpolation can be written as follows:
// P(0,v) = v P(0,1) + (1 - v) P(0,0)
// P(1,v) = v P(1,1) + (1 - v) P(1,0)
// P(u,v) = u P(1,v) + (1 - u) P(0,v)
// which can be solved for u:
// u = [P(u,v) - P(0,v)] / [P(1,v) - P(0,v)]
// this equation holds for both x and y coordinates of the various P points, so u can be eliminated:
// [x(u,v) - x(0,v)] * [y(1,v) - y(0,v)] - [y(u,v) - y(0,v)] * [x(1,v) - x(0,v)] = 0
// this is a quadratic equation in v: a v² + bv + c = 0
final double a = y11 * x00 - y10 * x00 + y10 * x01 - y11 * x01 + y01 * x11 - y01 * x10 - y00 * x11 + y00 * x10;
final double b = y11 * xuv - y10 * xuv + y00 * xuv - y01 * xuv - y11 * x00 + 2 * y10 * x00 - y10 * x01 + y01 * x10 + y00 * x11 - 2 * y00 * x10 + yuv * x01 + yuv * x10 - yuv * x11 - yuv * x00;
final double c = y10 * xuv - y00 * xuv - y10 * x00 + y00 * x10 - yuv * x10 + yuv * x00;
if (FastMath.abs(a) < Precision.EPSILON * (FastMath.abs(b) + FastMath.abs(c))) {
if (FastMath.abs(b) < Precision.EPSILON * FastMath.abs(c)) {
return null;
} else {
// solve linear equation in v
final double v = -c / b;
// recover uA from vA
final double x0v = v * x01 + (1 - v) * x00;
final double x1v = v * x11 + (1 - v) * x10;
final double dX = x1v - x0v;
final double y0v = v * y01 + (1 - v) * y00;
final double y1v = v * y11 + (1 - v) * y10;
final double dY = y1v - y0v;
final double u = (FastMath.abs(dX) >= FastMath.abs(dX)) ? ((xuv - x0v) / dX) : ((yuv - y0v) / dY);
return new double[] {
u, v
};
}
} else {
// solve quadratic equation in v
final double bb = b * b;
final double fac = 4 * a * c;
if (bb < fac) {
return null;
}
final double s = FastMath.sqrt(bb - fac);
final double vA = (b > 0) ? -(s + b) / (2 * a) : (2 * c) / (s - b);
final double vB = c / (a * vA);
// recover uA from vA
final double x0vA = vA * x01 + (1 - vA) * x00;
final double x1vA = vA * x11 + (1 - vA) * x10;
final double dXA = x1vA - x0vA;
final double y0vA = vA * y01 + (1 - vA) * y00;
final double y1vA = vA * y11 + (1 - vA) * y10;
final double dYA = y1vA - y0vA;
final double uA = (FastMath.abs(dXA) >= FastMath.abs(dXA)) ? ((xuv - x0vA) / dXA) : ((yuv - y0vA) / dYA);
// recover uB from vB
final double x0vB = vB * x01 + (1 - vB) * x00;
final double x1vB = vB * x11 + (1 - vB) * x10;
final double dXB = x1vB - x0vB;
final double y0vB = vB * y01 + (1 - vB) * y00;
final double y1vB = vB * y11 + (1 - vB) * y10;
final double dYB = y1vB - y0vB;
final double uB = (FastMath.abs(dXB) >= FastMath.abs(dXB)) ? ((xuv - x0vB) / dXB) : ((yuv - y0vB) / dYB);
if ((uA * uA + vA * vA) <= (uB * uB + vB * vB)) {
return new double[] {
uA, vA
};
} else {
return new double[] {
uA, vA
};
}
}
}
/** Local class for finding when ground point crosses mean sensor plane. */
private class SensorMeanPlaneCrossing implements UnivariateFunction {
/** Line sensor. */
private final LineSensor sensor;
/** Target ground point. */
private final Vector3D target;
/** Simple constructor.
* @param sensor sensor to consider
* @param target target ground point
*/
public SensorMeanPlaneCrossing(final LineSensor sensor, final Vector3D target) {
this.sensor = sensor;
this.target = target;
}
/** {@inheritDoc} */
@Override
public double value(final double lineNumber) throws OrekitExceptionWrapper {
// the target crosses the mean plane if it orthogonal to the normal vector
return Vector3D.angle(getTargetDirection(lineNumber), sensor.getMeanPlaneNormal()) - 0.5 * FastMath.PI;
}
/** Get the target direction in spacecraft frame.
* @param lineNumber current line number
* @return target direction in spacecraft frame
* @exception OrekitExceptionWrapper if some frame conversion fails
*/
public Vector3D getTargetDirection(final double lineNumber) throws OrekitExceptionWrapper {
try {
// compute the transform between spacecraft and observed body
final AbsoluteDate date = sensor.getDate(lineNumber);
final Transform bodyToInert = scToBody.getInertialToBody(date).getInverse();
final Transform scToInert = scToBody.getScToInertial(date);
final Vector3D refInert = scToInert.transformPosition(sensor.getPosition());
final Vector3D targetInert;
if (lightTimeCorrection) {
// apply light time correction
final Vector3D iT = bodyToInert.transformPosition(target);
final double deltaT = refInert.distance(iT) / Constants.SPEED_OF_LIGHT;
targetInert = bodyToInert.shiftedBy(-deltaT).transformPosition(target);
} else {
// don't apply light time correction
targetInert = bodyToInert.transformPosition(target);
}
final Vector3D lInert = targetInert.subtract(refInert).normalize();
final Vector3D rawLInert;
if (aberrationOfLightCorrection) {
// apply aberration of light correction
// as the spacecraft velocity is small with respect to speed of light,
// we use classical velocity addition and not relativistic velocity addition
final Vector3D spacecraftVelocity =
scToInert.transformPVCoordinates(PVCoordinates.ZERO).getVelocity();
rawLInert = new Vector3D(Constants.SPEED_OF_LIGHT, lInert, -1.0, spacecraftVelocity).normalize();
} else {
// don't apply aberration of light correction
rawLInert = lInert;
}
// direction of the target point in spacecraft frame
return scToInert.getInverse().transformVector(rawLInert);
} catch (OrekitException oe) {
throw new OrekitExceptionWrapper(oe);
}
} }
} // find approximately the pixel along this sensor line
final SensorPixelCrossing pixelCrossing = new SensorPixelCrossing(sensor, planeCrossing.getTargetDirection(),
/** Local class for finding when ground point crosses pixel along sensor line. */ MAX_EVAL, COARSE_INVERSE_LOCALIZATION_ACCURACY);
private class SensorPixelCrossing implements UnivariateFunction { final double coarsePixel = pixelCrossing.locatePixel();
if (Double.isNaN(coarsePixel)) {
/** Line sensor. */ // target is out of search interval
private final LineSensor sensor; return null;
/** Cross direction in spacecraft frame. */
private final Vector3D cross;
/** Simple constructor.
* @param sensor sensor to consider
* @param targetDirection target direction in spacecraft frame
*/
public SensorPixelCrossing(final LineSensor sensor, final Vector3D targetDirection) {
this.sensor = sensor;
this.cross = Vector3D.crossProduct(sensor.getMeanPlaneNormal(), targetDirection).normalize();
}
/** {@inheritDoc} */
@Override
public double value(final double x) {
return Vector3D.angle(cross, getLOS(x)) - 0.5 * FastMath.PI;
} }
/** Interpolate sensor pixels at some pixel index. // fix line by considering the closest pixel exact position and line-of-sight
* @param x pixel index // (this pixel might point towards a direction slightly above or below the mean sensor plane)
* @return interpolated direction for specified index final int closestIndex = (int) FastMath.rint(coarsePixel);
*/ final Vector3D xAxis = sensor.getLos(closestIndex);
public Vector3D getLOS(final double x) { final Vector3D zAxis = sensor.getCross(closestIndex);
final Vector3D coarseDirection = planeCrossing.getTargetDirection();
// find surrounding pixels final Vector3D coarseDirectionDot = planeCrossing.getTargetDirectionDot();
final int iInf = FastMath.max(0, FastMath.min(sensor.getNbPixels() - 2, (int) FastMath.floor(x))); final double deltaT = Vector3D.dotProduct(xAxis.subtract(coarseDirection), zAxis) /
final int iSup = iInf + 1; Vector3D.dotProduct(coarseDirectionDot, zAxis);
final double fixedLine = sensor.getLine(sensor.getDate(planeCrossing.getLine()).shiftedBy(deltaT));
// interpolate final Vector3D fixedDirection = new Vector3D(1, coarseDirection, deltaT, coarseDirectionDot).normalize();
return new Vector3D(iSup - x, sensor.getLos(iInf), x - iInf, sensor.getLos(iSup)).normalize();
// fix pixel
} final double alpha = sensor.getAzimuth(fixedDirection, closestIndex);
final double pixelWidth = sensor.getWidth(closestIndex);
final double fixedPixel = closestIndex + alpha / pixelWidth;
return new SensorPixel(fixedLine, fixedPixel);
} }
......
src/main/java/org/orekit/rugged/api/SensorMeanPlaneCrossing.java 0 → 100644
+ 237
− 0
View file @ ff85e44f
/* Copyright 2013-2014 CS Systèmes d'Information
* Licensed to CS Systèmes d'Information (CS) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* CS licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.orekit.rugged.api;
import org.apache.commons.math3.analysis.UnivariateFunction;
import org.apache.commons.math3.analysis.solvers.BracketingNthOrderBrentSolver;
import org.apache.commons.math3.analysis.solvers.UnivariateSolver;
import org.apache.commons.math3.exception.NoBracketingException;
import org.apache.commons.math3.exception.TooManyEvaluationsException;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
import org.apache.commons.math3.util.FastMath;
import org.orekit.errors.OrekitException;
import org.orekit.errors.OrekitExceptionWrapper;
import org.orekit.frames.Transform;
import org.orekit.rugged.core.SpacecraftToObservedBody;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.Constants;
import org.orekit.utils.PVCoordinates;
/** Class dedicated to when ground point crosses mean sensor plane. */
class SensorMeanPlaneCrossing {
/** Converter between spacecraft and body. */
private final SpacecraftToObservedBody scToBody;
/** Flag for light time correction. */
private boolean lightTimeCorrection;
/** Flag for aberration of light correction. */
private boolean aberrationOfLightCorrection;
/** Line sensor. */
private final LineSensor sensor;
/** Target ground point. */
private final Vector3D target;
/** Accuracy to use for finding crossing line number. */
private final double accuracy;
/** Evaluated lines. */
private double[] lineNumbers;
/** Observed body to inertial transforms, with selected fixes included. */
private final Transform[] fixedBodyToInert;
/** Spacecraft to inertial transforms,. */
private final Transform[] scToInert;
/** Inertial to spacecraft transforms, with selected fixes included. */
private final Transform[] fixedInertToSc;
/** Target direction in spacecraft frame. */
private Vector3D[] targetDirection;
/** Index of last evaluation. */
private int last;
/** Index of best evaluation. */
private int best;
/** Simple constructor.
* @param sensor sensor to consider
* @param target target ground point
* @param scToBody converter between spacecraft and body
* @param lightTimeCorrection flag for light time correction
* @param aberrationOfLightCorrection flag for aberration of light correction.
* @param maxEval maximum number of evaluations
* @param accuracy accuracy to use for finding crossing line number
*/
public SensorMeanPlaneCrossing(final LineSensor sensor, final Vector3D target,
final SpacecraftToObservedBody scToBody,
final boolean lightTimeCorrection,
final boolean aberrationOfLightCorrection,
final int maxEval, final double accuracy) {
this.sensor = sensor;
this.target = target;
this.scToBody = scToBody;
this.lightTimeCorrection = lightTimeCorrection;
this.aberrationOfLightCorrection = aberrationOfLightCorrection;
this.accuracy = accuracy;
this.lineNumbers = new double[maxEval];
this.fixedBodyToInert = new Transform[maxEval];
this.scToInert = new Transform[maxEval];
this.fixedInertToSc = new Transform[maxEval];
this.targetDirection = new Vector3D[maxEval];
this.last = -1;
}
/** Find mean plane crossing.
* @param minLine minimum line number
* @param maxLine maximum line number
* @return true if a solution has been found in the search interval,
* false if search interval does not bracket a solution
* @exception RuggedException if geometry cannot be computed for some line or
* if the maximum number of evaluations is exceeded
*/
public boolean find(final double minLine, final double maxLine)
throws RuggedException {
try {
// set up function evaluating to 0.0 at mean plane crossing
final UnivariateFunction f = new UnivariateFunction() {
/** {@inheritDoc} */
@Override
public double value(final double x) throws OrekitExceptionWrapper {
// the target crosses the mean plane if it orthogonal to the normal vector
evaluateLine(x);
return Vector3D.angle(targetDirection[last], sensor.getMeanPlaneNormal()) - 0.5 * FastMath.PI;
}
};
// find the root
final UnivariateSolver solver =
new BracketingNthOrderBrentSolver(0.0, accuracy, 5);
final double crossingLine = solver.solve(lineNumbers.length, f, minLine, maxLine);
// identify the selected solution
best = -1;
double min = Double.POSITIVE_INFINITY;
for (int i = 0; i <= last; ++i) {
final double delta = FastMath.abs(lineNumbers[i] - crossingLine);
if (delta < min) {
best = i;
min = delta;
}
}
return true;
} catch (NoBracketingException nbe) {
// there are no solutions in the search interval
return false;
} catch (TooManyEvaluationsException tmee) {
throw new RuggedException(tmee);
} catch (OrekitExceptionWrapper oew) {
final OrekitException oe = oew.getException();
throw new RuggedException(oe, oe.getSpecifier(), oe.getParts());
}
}
/** Get the crossing line.
* @return crossing line
*/
public double getLine() {
return lineNumbers[best];
}
/** Get the normalized target direction in spacecraft frame at crossing.
* @return normalized target direction in spacecraft frame at crossing
*/
public Vector3D getTargetDirection() {
return targetDirection[best];
}
/** Get the derivative of normalized target direction in spacecraft frame at crossing.
* @return derivative of normalized target direction in spacecraft frame at crossing
*/
public Vector3D getTargetDirectionDot() {
final PVCoordinates targetBody = new PVCoordinates(target, Vector3D.ZERO);
final PVCoordinates targetInert = fixedBodyToInert[best].transformPVCoordinates(targetBody);
final PVCoordinates targetSc = fixedInertToSc[best].transformPVCoordinates(targetInert);
return new Vector3D(1.0 / targetSc.getPosition().subtract(sensor.getPosition()).getNorm(),
targetSc.getVelocity());
}
/** Evaluate geometry for a given line number.
* @param lineNumber current line number
* @exception OrekitExceptionWrapper if some frame conversion fails
*/
private void evaluateLine(final double lineNumber) throws OrekitExceptionWrapper {
try {
lineNumbers[++last] = lineNumber;
// compute the transform between spacecraft and observed body
final AbsoluteDate date = sensor.getDate(lineNumber);
final Transform bodyToInert = scToBody.getInertialToBody(date).getInverse();
scToInert[last] = scToBody.getScToInertial(date);
final Vector3D refInert = scToInert[last].transformPosition(sensor.getPosition());
if (lightTimeCorrection) {
// apply light time correction
final Vector3D iT = bodyToInert.transformPosition(target);
final double deltaT = refInert.distance(iT) / Constants.SPEED_OF_LIGHT;
fixedBodyToInert[last] = bodyToInert.shiftedBy(-deltaT);
} else {
// don't apply light time correction
fixedBodyToInert[last] = bodyToInert;
}
final Vector3D targetInert = fixedBodyToInert[last].transformPosition(target);
final Vector3D lInert = targetInert.subtract(refInert).normalize();
final Transform inertToSc = scToInert[last].getInverse();
if (aberrationOfLightCorrection) {
// apply aberration of light correction
// as the spacecraft velocity is small with respect to speed of light,
// we use classical velocity addition and not relativistic velocity addition
final Vector3D spacecraftVelocity =
scToInert[last].transformPVCoordinates(PVCoordinates.ZERO).getVelocity();
final Vector3D rawLInert = new Vector3D(Constants.SPEED_OF_LIGHT, lInert,
-1.0, spacecraftVelocity);
fixedInertToSc[last] = new Transform(date,
inertToSc,
new Transform(date,
new Rotation(inertToSc.transformVector(lInert),
inertToSc.transformVector(rawLInert))));
} else {
// don't apply aberration of light correction
fixedInertToSc[last] = inertToSc;
}
// direction of the target point in spacecraft frame
targetDirection[last] = fixedInertToSc[last].transformVector(lInert);
} catch (OrekitException oe) {
throw new OrekitExceptionWrapper(oe);
}
}
}
src/main/java/org/orekit/rugged/api/SensorPixelCrossing.java 0 → 100644
+ 107
− 0
View file @ ff85e44f
/* Copyright 2013-2014 CS Systèmes d'Information
* Licensed to CS Systèmes d'Information (CS) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* CS licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.orekit.rugged.api;
import org.apache.commons.math3.analysis.UnivariateFunction;
import org.apache.commons.math3.analysis.solvers.BracketingNthOrderBrentSolver;
import org.apache.commons.math3.analysis.solvers.UnivariateSolver;
import org.apache.commons.math3.exception.NoBracketingException;
import org.apache.commons.math3.exception.TooManyEvaluationsException;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
import org.apache.commons.math3.util.FastMath;
import org.orekit.errors.OrekitExceptionWrapper;
/** Class devoted to locate where ground point crosses a sensor line.
* <p>
* This class is used in the first stage of inverse localization.
* </p>
* @author Luc Maisonobe
*/
class SensorPixelCrossing {
/** Line sensor. */
private final LineSensor sensor;
/** Cross direction in spacecraft frame. */
private final Vector3D cross;
/** Maximum number of evaluations. */
private final int maxEval;
/** Accuracy to use for finding crossing line number. */
private final double accuracy;
/** Simple constructor.
* @param sensor sensor to consider
* @param targetDirection target direction in spacecraft frame
* @param maxEval maximum number of evaluations
* @param accuracy accuracy to use for finding crossing line number
*/
public SensorPixelCrossing(final LineSensor sensor, final Vector3D targetDirection,
final int maxEval, final double accuracy) {
this.sensor = sensor;
this.cross = Vector3D.crossProduct(sensor.getMeanPlaneNormal(), targetDirection).normalize();
this.maxEval = maxEval;
this.accuracy = accuracy;
}
/** Locate pixel along sensor line.
* @return pixel location ({@code Double.NaN} if the first and last
* pixels of the line do not bracket a location)
* @exception RuggedException if the maximum number of evaluations is exceeded
*/
public double locatePixel() throws RuggedException {
try {
// set up function evaluating to 0.0 where target matches pixel
final UnivariateFunction f = new UnivariateFunction() {
/** {@inheritDoc} */
@Override
public double value(final double x) throws OrekitExceptionWrapper {
return Vector3D.angle(cross, getLOS(x)) - 0.5 * FastMath.PI;
}
};
// find the root
final UnivariateSolver solver =
new BracketingNthOrderBrentSolver(0.0, accuracy, 5);
return solver.solve(maxEval, f, -1.0, sensor.getNbPixels());
} catch (NoBracketingException nbe) {
// there are no solutions in the search interval
return Double.NaN;
} catch (TooManyEvaluationsException tmee) {
throw new RuggedException(tmee);
}
}
/** Interpolate sensor pixels at some pixel index.
* @param x pixel index
* @return interpolated direction for specified index
*/
private Vector3D getLOS(final double x) {
// find surrounding pixels
final int iInf = FastMath.max(0, FastMath.min(sensor.getNbPixels() - 2, (int) FastMath.floor(x)));
final int iSup = iInf + 1;
// interpolate
return new Vector3D(iSup - x, sensor.getLos(iInf), x - iInf, sensor.getLos(iSup)).normalize();
}
}
src/test/java/org/orekit/rugged/api/RuggedTest.java
+ 19
− 7
View file @ ff85e44f
...@@ -431,10 +431,11 @@ public class RuggedTest { ...@@ -431,10 +431,11 @@ public class RuggedTest {
@Test @Test
public void testInverseLocalization() public void testInverseLocalization()
throws RuggedException, OrekitException, URISyntaxException { throws RuggedException, OrekitException, URISyntaxException {
checkInverseLocalization(2000, false, false, 4.0e-5, 4.0e-5); // checkInverseLocalization(2000, false, false, 4.0e-5, 4.0e-5);
checkInverseLocalization(2000, false, true, 4.0e-5, 4.0e-5); // checkInverseLocalization(2000, false, true, 4.0e-5, 4.0e-5);
checkInverseLocalization(2000, true, false, 4.0e-5, 4.0e-5); // checkInverseLocalization(2000, true, false, 4.0e-5, 4.0e-5);
checkInverseLocalization(2000, true, true, 4.0e-5, 4.0e-5); // checkInverseLocalization(2000, true, true, 4.0e-5, 4.0e-5);
checkInverseLocalization(200, true, true, 5.0e-5, 7.0e-5);
} }
private void checkInverseLocalization(int dimension, boolean lightTimeCorrection, boolean aberrationOfLightCorrection, private void checkInverseLocalization(int dimension, boolean lightTimeCorrection, boolean aberrationOfLightCorrection,
...@@ -450,11 +451,11 @@ public class RuggedTest { ...@@ -450,11 +451,11 @@ public class RuggedTest {
// one line sensor // one line sensor
// position: 1.5m in front (+X) and 20 cm above (-Z) of the S/C center of mass // position: 1.5m in front (+X) and 20 cm above (-Z) of the S/C center of mass
// los: swath in the (YZ) plane, looking at 50° roll, ±1° aperture // los: swath in the (YZ) plane, looking at 50° roll, 2.6" per pixel
Vector3D position = new Vector3D(1.5, 0, -0.2); Vector3D position = new Vector3D(1.5, 0, -0.2);
List<Vector3D> los = createLOS(new Rotation(Vector3D.PLUS_I, FastMath.toRadians(50.0)).applyTo(Vector3D.PLUS_K), List<Vector3D> los = createLOS(new Rotation(Vector3D.PLUS_I, FastMath.toRadians(50.0)).applyTo(Vector3D.PLUS_K),
Vector3D.PLUS_I, Vector3D.PLUS_I,
FastMath.toRadians(1.0), dimension); FastMath.toRadians(dimension * 2.6 / 3600.0), dimension);
// linear datation model: at reference time we get line 100, and the rate is one line every 1.5ms // 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); LineDatation lineDatation = new LinearLineDatation(crossing, dimension / 2, 1.0 / 1.5e-3);
...@@ -477,16 +478,27 @@ public class RuggedTest { ...@@ -477,16 +478,27 @@ public class RuggedTest {
double referenceLine = 0.56789 * dimension; double referenceLine = 0.56789 * dimension;
GeodeticPoint[] gp = rugged.directLocalization("line", referenceLine); GeodeticPoint[] gp = rugged.directLocalization("line", referenceLine);
for (double p = 1; p < gp.length - 1; p += 0.2) { long t0 = System.currentTimeMillis();
// double maxL = 0;
// double maxP = 0;
for (int k = 0; k < dimension; ++k) {
for (double p = 0; p < gp.length - 1; p += 1.0) {
int i = (int) FastMath.floor(p); int i = (int) FastMath.floor(p);
double d = p - i; double d = p - i;
GeodeticPoint g = new GeodeticPoint((1 - d) * gp[i].getLatitude() + d * gp[i + 1].getLatitude(), GeodeticPoint g = new GeodeticPoint((1 - d) * gp[i].getLatitude() + d * gp[i + 1].getLatitude(),
(1 - d) * gp[i].getLongitude() + d * gp[i + 1].getLongitude(), (1 - d) * gp[i].getLongitude() + d * gp[i + 1].getLongitude(),
(1 - d) * gp[i].getAltitude() + d * gp[i + 1].getAltitude()); (1 - d) * gp[i].getAltitude() + d * gp[i + 1].getAltitude());
SensorPixel sp = rugged.inverseLocalization("line", g, 0, dimension); SensorPixel sp = rugged.inverseLocalization("line", g, 0, dimension);
// System.out.println(p + " " + (referenceLine - sp.getLineNumber()) + " " + (p - sp.getPixelNumber()));
Assert.assertEquals(referenceLine, sp.getLineNumber(), maxLineError); Assert.assertEquals(referenceLine, sp.getLineNumber(), maxLineError);
Assert.assertEquals(p, sp.getPixelNumber(), maxPixelError); Assert.assertEquals(p, sp.getPixelNumber(), maxPixelError);
// maxL = FastMath.max(maxL, FastMath.abs(referenceLine - sp.getLineNumber()));
// maxP = FastMath.max(maxP, FastMath.abs(p - sp.getPixelNumber()));
}
} }
long t1 = System.currentTimeMillis();
System.out.println("# " + dimension + "x" + dimension + " " + (1.0e-3 * (t1 - t0)));
// System.out.println("# maxL = " + maxL + ", maxP = " + maxP);
} }
private BodyShape createEarth() private BodyShape createEarth()
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment