From 2ea9c1d2052e76c16ae07b5461a3c330f887db42 Mon Sep 17 00:00:00 2001
From: Luc Maisonobe <luc@orekit.org>
Date: Tue, 6 May 2014 16:11:38 +0200
Subject: [PATCH] First working version of inverse localization.
---
.../java/org/orekit/rugged/api/Rugged.java | 137 ++++++++++++++----
.../org/orekit/rugged/api/RuggedTest.java | 32 ++--
2 files changed, 126 insertions(+), 43 deletions(-)
diff --git a/src/main/java/org/orekit/rugged/api/Rugged.java b/src/main/java/org/orekit/rugged/api/Rugged.java
index 3995ee58..7c07c450 100644
--- a/src/main/java/org/orekit/rugged/api/Rugged.java
+++ b/src/main/java/org/orekit/rugged/api/Rugged.java
@@ -30,6 +30,7 @@ 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.apache.commons.math3.util.Pair;
+import org.apache.commons.math3.util.Precision;
import org.orekit.attitudes.Attitude;
import org.orekit.attitudes.AttitudeProvider;
import org.orekit.attitudes.TabulatedProvider;
@@ -539,17 +540,17 @@ public class Rugged {
final Vector3D eP2 = ellipsoid.transform(gp1);
final double deltaT2 = eP2.distance(sP) / Constants.SPEED_OF_LIGHT;
final Transform shifted2 = inertToBody.shiftedBy(-deltaT2);
- gp[i] = alg.refineIntersection(ellipsoid,
- shifted2.transformPosition(pInert),
- shifted2.transformVector(lInert),
- gp1);
+ gp[i - start] = alg.refineIntersection(ellipsoid,
+ shifted2.transformPosition(pInert),
+ shifted2.transformVector(lInert),
+ gp1);
} else {
// compute DEM intersection without light time correction
final Vector3D pBody = inertToBody.transformPosition(pInert);
final Vector3D lBody = inertToBody.transformVector(lInert);
- gp[i] = alg.refineIntersection(ellipsoid, pBody, lBody,
- alg.intersection(ellipsoid, pBody, lBody));
+ gp[i - start] = alg.refineIntersection(ellipsoid, pBody, lBody,
+ alg.intersection(ellipsoid, pBody, lBody));
}
}
@@ -592,12 +593,10 @@ public class Rugged {
// compute a quadrilateral that should surround the target
final double lInf = FastMath.floor(coarseLine);
- final double lSup = lInf + 1;
final int pInf = FastMath.max(0, FastMath.min(sensor.getNbPixels() - 2, (int) FastMath.floor(coarsePixel)));
- final int pSup = pInf + 1;
final IntersectionAlgorithm alg = new FixedAltitudeAlgorithm(groundPoint.getAltitude());
- final GeodeticPoint[] previous = directLocalization(sensor, pInf, pSup, alg, lInf);
- final GeodeticPoint[] next = directLocalization(sensor, pInf, pSup, alg, lSup);
+ 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(),
@@ -605,7 +604,7 @@ public class Rugged {
next[0].getLongitude(), next[0].getLatitude(),
next[1].getLongitude(), next[1].getLatitude());
- return new SensorPixel(lInf + interp[1], pInf + interp[0]);
+ return (interp == null) ? null : new SensorPixel(lInf + interp[1], pInf + interp[0]);
} catch (NoBracketingException nbe) {
// there are no solutions in the search interval
@@ -621,29 +620,103 @@ public class Rugged {
/** Compute a point bilinear interpolation coordinates.
* <p>
* This method is used to find interpolation coordinates when the
- * quadrilateral is <em>not</em> an exact rectangle.
+ * quadrilateral is <em>not</em> a perfect rectangle.
* </p>
- * @param x point abscissa
- * @param y point ordinate
- * @param xA lower left abscissa
- * @param yA lower left ordinate
- * @param xB lower right abscissa
- * @param yB lower right ordinate
- * @param xC upper left abscissa
- * @param yC upper left ordinate
- * @param xD upper right abscissa
- * @param yD upper right ordinate
- * @return interpolation coordinates corresponding to point {@code x}, {@code y}
+ * @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 x, final double y,
- final double xA, final double yA,
- final double xB, final double yB,
- final double xC, final double yC,
- final double xD, final double yD) {
- // TODO: compute coordinates
- return new double[] {
- Double.NaN, Double.NaN
- };
+ 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. */
diff --git a/src/test/java/org/orekit/rugged/api/RuggedTest.java b/src/test/java/org/orekit/rugged/api/RuggedTest.java
index 6770f2df..242e9930 100644
--- a/src/test/java/org/orekit/rugged/api/RuggedTest.java
+++ b/src/test/java/org/orekit/rugged/api/RuggedTest.java
@@ -427,8 +427,15 @@ public class RuggedTest {
@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);
+ }
- int dimension = 3;
+ private void checkInverseLocalization(int dimension, boolean lightTimeCorrection, boolean aberrationOfLightCorrection,
+ double maxLineError, double maxPixelError)
+ throws RuggedException, OrekitException, URISyntaxException {
String path = getClass().getClassLoader().getResource("orekit-data").toURI().getPath();
DataProvidersManager.getInstance().addProvider(new DirectoryCrawler(new File(path)));
@@ -459,21 +466,24 @@ public class RuggedTest {
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(false);
- rugged.setAberrationOfLightCorrection(true);
+ rugged.setLightTimeCorrection(lightTimeCorrection);
+ rugged.setAberrationOfLightCorrection(aberrationOfLightCorrection);
rugged.setLineSensor(lineSensor);
- double referenceLine = dimension / 2;
+ double referenceLine = 0.56789 * dimension;
GeodeticPoint[] gp = rugged.directLocalization("line", referenceLine);
- for (int i = 1; i < gp.length - 1; ++i) {
- SensorPixel sp = rugged.inverseLocalization("line", gp[i], 0, dimension);
- System.out.println(i + " " + (referenceLine - sp.getLineNumber()) + " " + (i - sp.getPixelNumber()));
-// Assert.assertEquals(referenceLine, sp.getLineNumber(), 5.0e-6);
-// Assert.assertEquals(i, sp.getPixelNumber(), 1.0e-7);
+ for (double p = 1; p < gp.length - 1; p += 0.2) {
+ int i = (int) FastMath.floor(p);
+ double d = p - i;
+ 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].getAltitude() + d * gp[i + 1].getAltitude());
+ SensorPixel sp = rugged.inverseLocalization("line", g, 0, dimension);
+ Assert.assertEquals(referenceLine, sp.getLineNumber(), maxLineError);
+ Assert.assertEquals(p, sp.getPixelNumber(), maxPixelError);
}
-
- }
+ }
private BodyShape createEarth()
throws OrekitException {
--
GitLab