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

First working version of inverse localization.

parent 0fd69dd0
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/Rugged.java
+ 105
− 32
View file @ 2ea9c1d2
...@@ -30,6 +30,7 @@ import org.apache.commons.math3.geometry.euclidean.threed.Rotation; ...@@ -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.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;
...@@ -539,17 +540,17 @@ public class Rugged { ...@@ -539,17 +540,17 @@ public class Rugged {
final Vector3D eP2 = ellipsoid.transform(gp1); final Vector3D eP2 = ellipsoid.transform(gp1);
final double deltaT2 = eP2.distance(sP) / Constants.SPEED_OF_LIGHT; final double deltaT2 = eP2.distance(sP) / Constants.SPEED_OF_LIGHT;
final Transform shifted2 = inertToBody.shiftedBy(-deltaT2); final Transform shifted2 = inertToBody.shiftedBy(-deltaT2);
gp[i] = alg.refineIntersection(ellipsoid, gp[i - start] = alg.refineIntersection(ellipsoid,
shifted2.transformPosition(pInert), shifted2.transformPosition(pInert),
shifted2.transformVector(lInert), shifted2.transformVector(lInert),
gp1); gp1);
} else { } else {
// compute DEM intersection without light time correction // compute DEM intersection without light time correction
final Vector3D pBody = inertToBody.transformPosition(pInert); final Vector3D pBody = inertToBody.transformPosition(pInert);
final Vector3D lBody = inertToBody.transformVector(lInert); final Vector3D lBody = inertToBody.transformVector(lInert);
gp[i] = alg.refineIntersection(ellipsoid, pBody, lBody, gp[i - start] = alg.refineIntersection(ellipsoid, pBody, lBody,
alg.intersection(ellipsoid, pBody, lBody)); alg.intersection(ellipsoid, pBody, lBody));
} }
} }
...@@ -592,12 +593,10 @@ public class Rugged { ...@@ -592,12 +593,10 @@ public class Rugged {
// compute a quadrilateral that should surround the target // compute a quadrilateral that should surround the target
final double lInf = FastMath.floor(coarseLine); 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 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 IntersectionAlgorithm alg = new FixedAltitudeAlgorithm(groundPoint.getAltitude());
final GeodeticPoint[] previous = directLocalization(sensor, pInf, pSup, alg, lInf); final GeodeticPoint[] previous = directLocalization(sensor, pInf, pInf + 2, alg, lInf);
final GeodeticPoint[] next = directLocalization(sensor, pInf, pSup, alg, lSup); final GeodeticPoint[] next = directLocalization(sensor, pInf, pInf + 2, alg, lInf + 1);
final double[] interp = interpolationCoordinates(groundPoint.getLongitude(), groundPoint.getLatitude(), final double[] interp = interpolationCoordinates(groundPoint.getLongitude(), groundPoint.getLatitude(),
previous[0].getLongitude(), previous[0].getLatitude(), previous[0].getLongitude(), previous[0].getLatitude(),
...@@ -605,7 +604,7 @@ public class Rugged { ...@@ -605,7 +604,7 @@ public class Rugged {
next[0].getLongitude(), next[0].getLatitude(), next[0].getLongitude(), next[0].getLatitude(),
next[1].getLongitude(), next[1].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) { } catch (NoBracketingException nbe) {
// there are no solutions in the search interval // there are no solutions in the search interval
...@@ -621,29 +620,103 @@ public class Rugged { ...@@ -621,29 +620,103 @@ public class Rugged {
/** Compute a point bilinear interpolation coordinates. /** Compute a point bilinear interpolation coordinates.
* <p> * <p>
* This method is used to find interpolation coordinates when the * 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> * </p>
* @param x point abscissa * @param xuv desired point abscissa, at interpolation coordinates u, v
* @param y point ordinate * @param yuv desired point ordinate, at interpolation coordinates u, v
* @param xA lower left abscissa * @param x00 abscissa for base quadrilateral point at u = 0, v = 0
* @param yA lower left ordinate * @param y00 ordinate for base quadrilateral point at u = 0, v = 0
* @param xB lower right abscissa * @param x10 abscissa for base quadrilateral point at u = 1, v = 0
* @param yB lower right ordinate * @param y10 ordinate for base quadrilateral point at u = 1, v = 0
* @param xC upper left abscissa * @param x01 abscissa for base quadrilateral point at u = 0, v = 1
* @param yC upper left ordinate * @param y01 ordinate for base quadrilateral point at u = 0, v = 1
* @param xD upper right abscissa * @param x11 abscissa for base quadrilateral point at u = 1, v = 1
* @param yD upper right ordinate * @param y11 ordinate for base quadrilateral point at u = 1, v = 1
* @return interpolation coordinates corresponding to point {@code x}, {@code y} * @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, private double[] interpolationCoordinates(final double xuv, final double yuv,
final double xA, final double yA, final double x00, final double y00,
final double xB, final double yB, final double x10, final double y10,
final double xC, final double yC, final double x01, final double y01,
final double xD, final double yD) { final double x11, final double y11) {
// TODO: compute coordinates
return new double[] { // bilinear interpolation can be written as follows:
Double.NaN, Double.NaN // 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. */ /** Local class for finding when ground point crosses mean sensor plane. */
......
src/test/java/org/orekit/rugged/api/RuggedTest.java
+ 21
− 11
View file @ 2ea9c1d2
...@@ -427,8 +427,15 @@ public class RuggedTest { ...@@ -427,8 +427,15 @@ 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, 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(); String path = getClass().getClassLoader().getResource("orekit-data").toURI().getPath();
DataProvidersManager.getInstance().addProvider(new DirectoryCrawler(new File(path))); DataProvidersManager.getInstance().addProvider(new DirectoryCrawler(new File(path)));
...@@ -459,21 +466,24 @@ public class RuggedTest { ...@@ -459,21 +466,24 @@ public class RuggedTest {
EllipsoidId.WGS84, InertialFrameId.EME2000, BodyRotatingFrameId.ITRF, EllipsoidId.WGS84, InertialFrameId.EME2000, BodyRotatingFrameId.ITRF,
orbitToPV(orbit, earth, lineDatation, firstLine, lastLine, 0.25), 8, orbitToPV(orbit, earth, lineDatation, firstLine, lastLine, 0.25), 8,
orbitToQ(orbit, earth, lineDatation, firstLine, lastLine, 0.25), 2); orbitToQ(orbit, earth, lineDatation, firstLine, lastLine, 0.25), 2);
rugged.setLightTimeCorrection(false); rugged.setLightTimeCorrection(lightTimeCorrection);
rugged.setAberrationOfLightCorrection(true); rugged.setAberrationOfLightCorrection(aberrationOfLightCorrection);
rugged.setLineSensor(lineSensor); rugged.setLineSensor(lineSensor);
double referenceLine = dimension / 2; double referenceLine = 0.56789 * dimension;
GeodeticPoint[] gp = rugged.directLocalization("line", referenceLine); GeodeticPoint[] gp = rugged.directLocalization("line", referenceLine);
for (int i = 1; i < gp.length - 1; ++i) { for (double p = 1; p < gp.length - 1; p += 0.2) {
SensorPixel sp = rugged.inverseLocalization("line", gp[i], 0, dimension); int i = (int) FastMath.floor(p);
System.out.println(i + " " + (referenceLine - sp.getLineNumber()) + " " + (i - sp.getPixelNumber())); double d = p - i;
// Assert.assertEquals(referenceLine, sp.getLineNumber(), 5.0e-6); GeodeticPoint g = new GeodeticPoint((1 - d) * gp[i].getLatitude() + d * gp[i + 1].getLatitude(),
// Assert.assertEquals(i, sp.getPixelNumber(), 1.0e-7); (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() private BodyShape createEarth()
throws OrekitException { throws OrekitException {
......
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