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

Improved inverse localization performances.

parent 296f1804
No related branches found
No related tags found
No related merge requests found
......@@ -48,9 +48,6 @@ public class LineSensor {
/** 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;
......@@ -117,12 +114,10 @@ public class LineSensor {
normal = singularVector.negate();
}
// compute transversal directions
// compute transversal direction
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
......@@ -166,38 +161,6 @@ public class LineSensor {
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.
* @param lineNumber line number
* @return date corresponding to line number
......
......@@ -21,6 +21,8 @@ import java.util.HashMap;
import java.util.List;
import java.util.Map;
import org.apache.commons.math3.analysis.differentiation.DerivativeStructure;
import org.apache.commons.math3.geometry.euclidean.threed.FieldVector3D;
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;
......@@ -61,7 +63,7 @@ public class Rugged {
* pixel, hence there is no point in choosing a too small value here.
* </p>
*/
private static final double COARSE_INVERSE_LOCALIZATION_ACCURACY = 0.1;
private static final double COARSE_INVERSE_LOCALIZATION_ACCURACY = 0.01;
/** Maximum number of evaluations. */
private static final int MAX_EVAL = 50;
......@@ -507,16 +509,24 @@ public class Rugged {
final GeodeticPoint[] gp = new GeodeticPoint[end - start];
for (int i = start; i < end; ++i) {
final Vector3D rawLInert = scToInert.transformVector(sensor.getLos(i));
final Vector3D obsLInert = scToInert.transformVector(sensor.getLos(i));
final Vector3D lInert;
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
lInert = new Vector3D(Constants.SPEED_OF_LIGHT, rawLInert, 1.0, spacecraftVelocity).normalize();
// we look for a positive k such that: c * lInert + vsat = k * obsLInert
// with lInert normalized
final double a = obsLInert.getNormSq();
final double b = -Vector3D.dotProduct(obsLInert, spacecraftVelocity);
final double c = spacecraftVelocity.getNormSq() - Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT;
final double s = FastMath.sqrt(b * b - a * c);
final double k = (b > 0) ? -c / (s + b) : (s - b) / a;
lInert = new Vector3D( k / Constants.SPEED_OF_LIGHT, obsLInert,
-1.0 / Constants.SPEED_OF_LIGHT, spacecraftVelocity);
} else {
// don't apply aberration of light correction
lInert = rawLInert;
lInert = obsLInert;
}
if (lightTimeCorrection) {
......@@ -572,19 +582,20 @@ public class Rugged {
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);
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;
}
final FieldVector3D<DerivativeStructure> coarseDirection = planeCrossing.getTargetDirection();
// find approximately the pixel along this sensor line
final SensorPixelCrossing pixelCrossing = new SensorPixelCrossing(sensor, planeCrossing.getTargetDirection(),
MAX_EVAL, COARSE_INVERSE_LOCALIZATION_ACCURACY);
final SensorPixelCrossing pixelCrossing =
new SensorPixelCrossing(sensor, coarseDirection.toVector3D(),
MAX_EVAL, COARSE_INVERSE_LOCALIZATION_ACCURACY);
final double coarsePixel = pixelCrossing.locatePixel();
if (Double.isNaN(coarsePixel)) {
// target is out of search interval
......@@ -593,15 +604,13 @@ 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 closestIndex = (int) FastMath.rint(coarsePixel);
final Vector3D xAxis = sensor.getLos(closestIndex);
final Vector3D zAxis = sensor.getCross(closestIndex);
final Vector3D coarseDirection = planeCrossing.getTargetDirection();
final Vector3D coarseDirectionDot = planeCrossing.getTargetDirectionDot();
final double deltaT = Vector3D.dotProduct(xAxis.subtract(coarseDirection), zAxis) /
Vector3D.dotProduct(coarseDirectionDot, zAxis);
final double fixedLine = sensor.getLine(sensor.getDate(planeCrossing.getLine()).shiftedBy(deltaT));
final Vector3D fixedDirection = new Vector3D(1, coarseDirection, deltaT, coarseDirectionDot).normalize();
final int closestIndex = (int) FastMath.rint(coarsePixel);
final DerivativeStructure beta = FieldVector3D.angle(coarseDirection, sensor.getMeanPlaneNormal());
final double deltaL = (0.5 * FastMath.PI - beta.getValue()) / beta.getPartialDerivative(1);
final double fixedLine = planeCrossing.getLine() + deltaL;
final Vector3D fixedDirection = new Vector3D(coarseDirection.getX().taylor(deltaL),
coarseDirection.getY().taylor(deltaL),
coarseDirection.getZ().taylor(deltaL)).normalize();
// fix pixel
final double alpha = sensor.getAzimuth(fixedDirection, closestIndex);
......
......@@ -16,16 +16,11 @@
*/
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.analysis.differentiation.DerivativeStructure;
import org.apache.commons.math3.geometry.euclidean.threed.FieldVector3D;
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;
......@@ -48,31 +43,19 @@ class SensorMeanPlaneCrossing {
private final LineSensor sensor;
/** Target ground point. */
private final Vector3D target;
private final PVCoordinates target;
/** Maximum number of evaluations. */
private final int maxEval;
/** 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;
/** Crossing line. */
private double crossingLine;
/** Target direction in spacecraft frame. */
private Vector3D[] targetDirection;
/** Index of last evaluation. */
private int last;
/** Index of best evaluation. */
private int best;
private FieldVector3D<DerivativeStructure> targetDirection;
/** Simple constructor.
* @param sensor sensor to consider
......@@ -89,17 +72,12 @@ class SensorMeanPlaneCrossing {
final boolean aberrationOfLightCorrection,
final int maxEval, final double accuracy) {
this.sensor = sensor;
this.target = target;
this.target = new PVCoordinates(target, Vector3D.ZERO);
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;
this.maxEval = maxEval;
}
/** Find mean plane crossing.
......@@ -112,126 +90,145 @@ class SensorMeanPlaneCrossing {
*/
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;
// 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 localization.
// We expect two or three evaluations only. Each new evaluation shows up quickly in
// the performances as it involves frames conversions
crossingLine = 0.5 * (minLine + maxLine);
boolean atMin = false;
boolean atMax = false;
for (int i = 0; i < maxEval; ++i) {
targetDirection = evaluateLine(crossingLine);
final DerivativeStructure beta = FieldVector3D.angle(targetDirection, sensor.getMeanPlaneNormal());
final double deltaL = (0.5 * FastMath.PI - beta.getValue()) / beta.getPartialDerivative(1);
if (FastMath.abs(deltaL) <= accuracy) {
// return immediately, without doing any additional evaluation!
return true;
}
crossingLine += deltaL;
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 false;
}
};
// 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;
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 false;
}
atMax = true;
crossingLine = maxLine;
} else {
// the next evaluation will be a regular point
atMin = false;
atMax = false;
}
}
return true;
return false;
} 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];
return crossingLine;
}
/** Get the normalized target direction in spacecraft frame at crossing.
* @return 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 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());
public FieldVector3D<DerivativeStructure> getTargetDirection() {
return targetDirection;
}
/** Evaluate geometry for a given line number.
* @param lineNumber current line number
* @exception OrekitExceptionWrapper if some frame conversion fails
* @return target direction in spacecraft frame, with its first derivative
* with respect to line number
* @exception RuggedException if some frame conversion fails
*/
private void evaluateLine(final double lineNumber) throws OrekitExceptionWrapper {
public FieldVector3D<DerivativeStructure> evaluateLine(final double lineNumber)
throws RuggedException {
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());
final AbsoluteDate date = sensor.getDate(lineNumber);
final Transform bodyToInert = scToBody.getInertialToBody(date).getInverse();
final Transform scToInert = scToBody.getScToInertial(date);
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(target);
final double deltaT = refInert.distance(iT) / Constants.SPEED_OF_LIGHT;
fixedBodyToInert[last] = bodyToInert.shiftedBy(-deltaT);
final Vector3D iT = bodyToInert.transformPosition(target.getPosition());
final double deltaT = refInert.getPosition().distance(iT) / Constants.SPEED_OF_LIGHT;
targetInert = bodyToInert.shiftedBy(-deltaT).transformPVCoordinates(target);
} else {
// don't apply light time correction
fixedBodyToInert[last] = bodyToInert;
targetInert = bodyToInert.transformPVCoordinates(target);
}
final Vector3D targetInert = fixedBodyToInert[last].transformPosition(target);
final Vector3D lInert = targetInert.subtract(refInert).normalize();
final Transform inertToSc = scToInert[last].getInverse();
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
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))));
// 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
fixedInertToSc[last] = inertToSc;
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));
// direction of the target point in spacecraft frame
targetDirection[last] = fixedInertToSc[last].transformVector(lInert);
// 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));
} catch (OrekitException oe) {
throw new OrekitExceptionWrapper(oe);
throw new RuggedException(oe, oe.getSpecifier(), oe.getParts());
}
}
/** 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);
}
}
......@@ -79,7 +79,7 @@ class SensorPixelCrossing {
// find the root
final UnivariateSolver solver =
new BracketingNthOrderBrentSolver(0.0, accuracy, 5);
return solver.solve(maxEval, f, -1.0, sensor.getNbPixels());
return solver.solve(maxEval, f, 0.0, sensor.getNbPixels() - 1.0);
} catch (NoBracketingException nbe) {
// there are no solutions in the search interval
......
......@@ -66,6 +66,8 @@ import org.orekit.propagation.analytical.KeplerianPropagator;
import org.orekit.propagation.numerical.NumericalPropagator;
import org.orekit.propagation.sampling.OrekitFixedStepHandler;
import org.orekit.rugged.core.raster.RandomLandscapeUpdater;
import org.orekit.rugged.core.raster.SimpleTileFactory;
import org.orekit.rugged.core.raster.Tile;
import org.orekit.rugged.core.raster.VolcanicConeElevationUpdater;
import org.orekit.time.AbsoluteDate;
import org.orekit.time.TimeScalesFactory;
......@@ -182,7 +184,7 @@ public class RuggedTest {
}
// the following test is disabled by default
// it is only used to check timings, and also create a large (366M) temporary file
// it is only used to check timings, and also create a large (66M) temporary file
@Test
@Ignore
public void testMayonVolcanoTiming()
......@@ -235,7 +237,7 @@ public class RuggedTest {
try {
int size = (lastLine - firstLine) * los.size() * 3 * Integer.SIZE;
int size = (lastLine - firstLine) * los.size() * 3 * Integer.SIZE / 8;
RandomAccessFile out = new RandomAccessFile(tempFolder.newFile(), "rw");
MappedByteBuffer buffer = out.getChannel().map(FileChannel.MapMode.READ_WRITE, 0, size);
......@@ -262,7 +264,7 @@ public class RuggedTest {
System.out.format(Locale.US,
"%n%n%5dx%5d:%n" +
" Orekit initialization and DEM creation : %5.1fs%n" +
" direct localization and %3dM grid writing: %5.1fs (%.0f px/s)%n",
" direct localization and %3dM grid writing: %5.1fs (%.1f px/s)%n",
lastLine - firstLine, los.size(),
1.0e-3 *(t1 - t0), sizeM, 1.0e-3 *(t2 - t1), pixels / (1.0e-3 * (t2 - t1)));
} catch (IOException ioe) {
......@@ -423,19 +425,112 @@ public class RuggedTest {
stats.addValue(Vector3D.distance(pWith, pWithout));
}
Assert.assertEquals( 0.005, stats.getMin(), 1.0e-3);
Assert.assertEquals(49.157, stats.getMax(), 1.0e-3);
Assert.assertEquals( 4.870, stats.getMean(), 1.0e-3);
Assert.assertEquals(39.924, stats.getMax(), 1.0e-3);
Assert.assertEquals( 4.823, stats.getMean(), 1.0e-3);
}
// the following test is disabled by default
// it is only used to check timings, and also create a small (2M) temporary file
@Test
@Ignore
public void testInverseLocalizationTiming()
throws RuggedException, OrekitException, URISyntaxException {
long t0 = System.currentTimeMillis();
int dimension = 1000;
String path = getClass().getClassLoader().getResource("orekit-data").toURI().getPath();
DataProvidersManager.getInstance().addProvider(new DirectoryCrawler(new File(path)));
final BodyShape earth = createEarth();
final Orbit orbit = 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);
List<Vector3D> los = createLOS(new Rotation(Vector3D.PLUS_I, FastMath.toRadians(50.0)).applyTo(Vector3D.PLUS_K),
Vector3D.PLUS_I,
FastMath.toRadians(dimension * 2.6 / 3600.0), dimension);
// linear datation model: at reference time we get line 100, and the rate is one line every 1.5ms
LineDatation lineDatation = new LinearLineDatation(crossing, dimension / 2, 1.0 / 1.5e-3);
int firstLine = 0;
int lastLine = dimension;
LineSensor lineSensor = new LineSensor("line", lineDatation, position, los);
TileUpdater updater =
new RandomLandscapeUpdater(0.0, 9000.0, 0.5, 0xf0a401650191f9f6l,
FastMath.toRadians(1.0), 257);
Rugged rugged = new Rugged(updater, 8, AlgorithmId.DUVENHAGE,
EllipsoidId.WGS84, InertialFrameId.EME2000, BodyRotatingFrameId.ITRF,
orbitToPV(orbit, earth, lineDatation, firstLine, lastLine, 0.25), 8,
orbitToQ(orbit, earth, lineDatation, firstLine, lastLine, 0.25), 2);
rugged.setLightTimeCorrection(true);
rugged.setAberrationOfLightCorrection(true);
rugged.addLineSensor(lineSensor);
double lat0 = FastMath.toRadians(-22.9);
double lon0 = FastMath.toRadians(142.4);
double delta = FastMath.toRadians(0.5);
Tile tile = new SimpleTileFactory().createTile();
updater.updateTile(lat0, lon0, tile);
try {
int size = dimension * dimension * 2 * Integer.SIZE / 8;
RandomAccessFile out = new RandomAccessFile(tempFolder.newFile(), "rw");
MappedByteBuffer buffer = out.getChannel().map(FileChannel.MapMode.READ_WRITE, 0, size);
long t1 = System.currentTimeMillis();
int goodPixels = 0;
int badPixels = 0;
for (int i = 0; i < dimension; ++i) {
double latitude = lat0 + (i * delta) / dimension;
for (int j = 0; j < dimension; ++j) {
double longitude = lon0 + (j * delta) / dimension;
GeodeticPoint gp = new GeodeticPoint(latitude, longitude,
tile.interpolateElevation(latitude, longitude));
SensorPixel sp = rugged.inverseLocalization("line", gp, 0, dimension);
if (sp == null) {
++badPixels;
buffer.putInt(-1);
buffer.putInt(-1);
} else {
++goodPixels;
final int lineCode = (int) FastMath.rint(FastMath.scalb(sp.getLineNumber(), 16));
final int pixelCode = (int) FastMath.rint(FastMath.scalb(sp.getPixelNumber(), 16));
buffer.putInt(lineCode);
buffer.putInt(pixelCode);
}
}
}
long t2 = System.currentTimeMillis();
out.close();
int sizeM = size / (1024 * 1024);
System.out.format(Locale.US,
"%n%n%5dx%5d:%n" +
" Orekit initialization and DEM creation : %5.1fs%n" +
" inverse localization and %3dM grid writing: %5.1fs (%.1f px/s, %.1f%% covered)%n",
dimension, dimension,
1.0e-3 *(t1 - t0), sizeM, 1.0e-3 *(t2 - t1),
(badPixels + goodPixels) / (1.0e-3 * (t2 - t1)),
(100.0 * goodPixels) / (goodPixels + badPixels));
} catch (IOException ioe) {
Assert.fail(ioe.getLocalizedMessage());
}
}
@Test
public void testInverseLocalization()
throws RuggedException, OrekitException, URISyntaxException {
// checkInverseLocalization(2000, false, false, 4.0e-5, 4.0e-5);
// checkInverseLocalization(2000, false, true, 4.0e-5, 4.0e-5);
// checkInverseLocalization(2000, true, false, 4.0e-5, 4.0e-5);
// checkInverseLocalization(2000, true, true, 4.0e-5, 4.0e-5);
checkInverseLocalization(200, true, true, 5.0e-5, 7.0e-5);
checkInverseLocalization(2000, false, false, 5.0e-5, 8.0e-5);
checkInverseLocalization(2000, false, true, 5.0e-5, 8.0e-5);
checkInverseLocalization(2000, true, false, 5.0e-5, 8.0e-5);
checkInverseLocalization(2000, true, true, 5.0e-5, 8.0e-5);
}
private void checkInverseLocalization(int dimension, boolean lightTimeCorrection, boolean aberrationOfLightCorrection,
......@@ -475,13 +570,9 @@ public class RuggedTest {
rugged.setAberrationOfLightCorrection(aberrationOfLightCorrection);
rugged.addLineSensor(lineSensor);
double referenceLine = 0.56789 * dimension;
double referenceLine = 0.87654 * dimension;
GeodeticPoint[] gp = rugged.directLocalization("line", referenceLine);
long t0 = System.currentTimeMillis();
// double maxL = 0;
// double maxP = 0;
for (int k = 0; k < dimension; ++k) {
for (double p = 0; p < gp.length - 1; p += 1.0) {
int i = (int) FastMath.floor(p);
double d = p - i;
......@@ -489,16 +580,9 @@ public class RuggedTest {
(1 - d) * gp[i].getLongitude() + d * gp[i + 1].getLongitude(),
(1 - d) * gp[i].getAltitude() + d * gp[i + 1].getAltitude());
SensorPixel sp = rugged.inverseLocalization("line", g, 0, dimension);
// System.out.println(p + " " + (referenceLine - sp.getLineNumber()) + " " + (p - sp.getPixelNumber()));
Assert.assertEquals(referenceLine, sp.getLineNumber(), maxLineError);
Assert.assertEquals(p, sp.getPixelNumber(), maxPixelError);
// maxL = FastMath.max(maxL, FastMath.abs(referenceLine - sp.getLineNumber()));
// maxP = FastMath.max(maxP, FastMath.abs(p - sp.getPixelNumber()));
}
}
long t1 = System.currentTimeMillis();
System.out.println("# " + dimension + "x" + dimension + " " + (1.0e-3 * (t1 - t0)));
// System.out.println("# maxL = " + maxL + ", maxP = " + maxP);
}
private BodyShape createEarth()
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment