diff --git a/src/main/java/org/orekit/rugged/api/Rugged.java b/src/main/java/org/orekit/rugged/api/Rugged.java
index 04ebd74469ad05070c61466f83914e6be89eb219..aaf4043e7eb94ee09e2acb2d7a23318718ee8a7e 100644
--- a/src/main/java/org/orekit/rugged/api/Rugged.java
+++ b/src/main/java/org/orekit/rugged/api/Rugged.java
@@ -20,6 +20,7 @@ import java.util.ArrayList;
import java.util.Collection;
import java.util.HashMap;
import java.util.HashSet;
+import java.util.Iterator;
import java.util.List;
import java.util.Map;
import java.util.Set;
@@ -601,7 +602,7 @@ public class Rugged {
final AbsoluteDate dateA, final int pixelA,
final SpacecraftToObservedBody scToBodyA,
final LineSensor sensorB,
- final AbsoluteDate dateB, final int pixelB)
+ final AbsoluteDate dateB, final double pixelB)
throws RuggedException {
//final LineSensor sensorA = getLineSensor(sensorNameA);
@@ -611,17 +612,24 @@ public class Rugged {
final Transform scToInertA = scToBodyA.getScToInertial(dateA); // from rugged instance A
final Transform scToInertB = scToBody.getScToInertial(dateB); // from (current) rugged instance B
+ // find surrounding pixels of pixelB (in order to interpolate LOS from pixelB (that is not an integer)
+ final int iInf = FastMath.max(0, FastMath.min(sensorB.getNbPixels() - 2, (int) FastMath.floor(pixelB)));
+ final int iSup = iInf + 1;
+
// Get sensors's position and LOS into local frame
+ // LOS
final Vector3D vALocal = sensorA.getLOS(dateA, pixelA); // V_a : line of sight's vectorA
- final Vector3D vBLocal = sensorB.getLOS(dateB, pixelB); // V_b
+ final Vector3D vBLocal = new Vector3D(iSup - pixelB, sensorB.getLOS(dateB, iInf),
+ pixelB - iInf, sensorB.getLOS(dateB, iSup)); // V_b is interpolated
+ // positions
final Vector3D sALocal = sensorA.getPosition(); // S_a : sensorA 's position
final Vector3D sBLocal = sensorB.getPosition(); // S_b
// Get sensors's position and LOS into inertial frame
- final Vector3D sA = scToInertA.transformPosition(sALocal);
- final Vector3D vA = scToInertA.transformVector(vALocal);
- final Vector3D sB = scToInertB.transformPosition(sBLocal);
- final Vector3D vB = scToInertB.transformVector(vBLocal);
+ final Vector3D sA = scToInertA.transformPosition(sALocal).normalize();
+ final Vector3D vA = scToInertA.transformVector(vALocal).normalize();
+ final Vector3D sB = scToInertB.transformPosition(sBLocal).normalize();
+ final Vector3D vB = scToInertB.transformVector(vBLocal).normalize();
final Vector3D vBase= sB.subtract(sA); // S_b - S_a
final double svA = Vector3D.dotProduct(vBase, vA); // SV_a = (S_b - S_a).V_a
@@ -639,9 +647,9 @@ public class Rugged {
final Vector3D mB = sB.add(vB.scalarMultiply(lambdaB)); // M_b = S_b + lambda_b * V_b
final Vector3D vDistance = mB.subtract(mA); // M_b - M_a
- System.out.format("vDistance %f %f %f ",vDistance.getX(),vDistance.getY(),vDistance.getZ());
+ //System.out.format("vDistance %f %f %f ",vDistance.getX(),vDistance.getY(),vDistance.getZ());
// Get square of the euclidean norm
- final double d = vDistance.getNormSq();
+ final double d = vDistance.getNorm();
return d;
}
/** Compute distance between two line sensors with derivatives.
@@ -661,7 +669,7 @@ public class Rugged {
final AbsoluteDate dateA, final int pixelA,
final SpacecraftToObservedBody scToBodyA,
final LineSensor sensorB,
- final AbsoluteDate dateB, final int pixelB,
+ final AbsoluteDate dateB, final double pixelB,
final DSGenerator generator)
throws RuggedException {
@@ -672,13 +680,21 @@ public class Rugged {
final Transform scToInertA = scToBodyA.getScToInertial(dateA); // from rugged instance A
final Transform scToInertB = scToBody.getScToInertial(dateB); // from (current) rugged instance B
+ // find surrounding pixels of pixelB (in order to interpolate LOS from pixelB (that is not an integer)
+ final int iInf = FastMath.max(0, FastMath.min(sensorB.getNbPixels() - 2, (int) FastMath.floor(pixelB)));
+ final int iSup = iInf + 1;
+
// Get sensors's LOS into local frame
final FieldVector3D<DerivativeStructure> vALocal = sensorA.getLOSDerivatives(dateA, pixelA, generator);
- final FieldVector3D<DerivativeStructure> vBLocal = sensorB.getLOSDerivatives(dateB, pixelB, generator);
+ final FieldVector3D<DerivativeStructure> vBLocal = new FieldVector3D<DerivativeStructure> ( // V_b is interpolated
+ iSup - pixelB,
+ sensorB.getLOSDerivatives(dateB, iInf, generator),
+ pixelB - iInf,
+ sensorB.getLOSDerivatives(dateB, iSup, generator)).normalize();
// Get sensors's LOS into inertial frame
- final FieldVector3D<DerivativeStructure> vA = scToInertA.transformPosition(vALocal); // V_a : line of sight's vectorA
- final FieldVector3D<DerivativeStructure> vB = scToInertB.transformPosition(vBLocal); // V_b
+ final FieldVector3D<DerivativeStructure> vA = scToInertA.transformVector(vALocal).normalize(); // V_a : line of sight's vectorA
+ final FieldVector3D<DerivativeStructure> vB = scToInertB.transformVector(vBLocal).normalize(); // V_b
// Get sensors's position into local frame TODO: check if we have to implement getPositionDerivatives() method & CO
final Vector3D sAtmp = sensorA.getPosition(); // S_a : sensorA 's position
@@ -690,8 +706,8 @@ public class Rugged {
final FieldVector3D<DerivativeStructure> sBLocal = new FieldVector3D<DerivativeStructure>(scaleFactor, sBtmp);
// Get sensors's position into inertial frame
- final FieldVector3D<DerivativeStructure> sA = scToInertA.transformPosition(sALocal);
- final FieldVector3D<DerivativeStructure> sB = scToInertB.transformPosition(sBLocal);
+ final FieldVector3D<DerivativeStructure> sA = scToInertA.transformPosition(sALocal).normalize();
+ final FieldVector3D<DerivativeStructure> sB = scToInertB.transformPosition(sBLocal).normalize();
// final FieldVector3D<DerivativeStructure> sA = sensorA.getPositionDerivatives(); // S_a : sensorA 's position
// final FieldVector3D<DerivativeStructure> sB = sensorB.getPositionDerivatives(); // S_b
@@ -714,7 +730,7 @@ public class Rugged {
final FieldVector3D<DerivativeStructure> vDistance = mB.subtract(mA); // M_b - M_a
// Get square of the euclidean norm
- final DerivativeStructure d = vDistance.getNormSq();
+ final DerivativeStructure d = vDistance.getNorm();
return new DerivativeStructure[] {d};
}
/** Estimate the free parameters from two viewing models (A and B)
@@ -737,8 +753,6 @@ public class Rugged {
throws RuggedException {
try {
- // TODO BEGIN-----------------------
- // Verify that createGenerator's construction is ok with the use of two Rugged instance
final List<LineSensor> selectedSensors = new ArrayList<>();
for (final SensorToSensorMapping reference : references) {
selectedSensors.add(ruggedA.getLineSensor(reference.getSensorNameA())); // from ruggedA instance
@@ -750,8 +764,7 @@ public class Rugged {
if (selected.isEmpty()) {
throw new RuggedException(RuggedMessages.NO_PARAMETERS_SELECTED);
}
- // TODO END--------------------------
-
+
// get start point (as a normalized value)
final double[] start = new double[selected.size()];
for (int i = 0; i < start.length; ++i) {
@@ -763,7 +776,7 @@ public class Rugged {
for (final SensorToSensorMapping reference : references) {
n += reference.getMappings().size();
}
- System.out.format("Check nb TiePoints %d %n", n);
+
if (n == 0) {
throw new RuggedException(RuggedMessages.NO_REFERENCE_MAPPINGS);
}
@@ -819,7 +832,7 @@ public class Rugged {
final AbsoluteDate dateA = lineSensorA.getDate(spA.getLineNumber());
final AbsoluteDate dateB = lineSensorB.getDate(spB.getLineNumber());
final int pixelA = (int)spA.getPixelNumber(); // Note: Rugged don't deal with half-pixel
- final int pixelB = (int)spB.getPixelNumber();
+ final double pixelB = spB.getPixelNumber();
final SpacecraftToObservedBody scToBodyA = ruggedA.getScToBody();
final DerivativeStructure[] ilResult =
@@ -878,6 +891,7 @@ public class Rugged {
// set up the optimizer
final LeastSquaresOptimizer optimizer = new LevenbergMarquardtOptimizer();
+// final LeastSquaresOptimizer optimizer = new GaussNewtonOptimizer().withDecomposition(GaussNewtonOptimizer.Decomposition.QR);
// solve the least squares problem
return optimizer.optimize(problem);
@@ -1232,7 +1246,7 @@ public class Rugged {
map.put(driver.getName(), map.size());
});
}
-
+
return new DSGenerator() {
/** {@inheritDoc} */
diff --git a/src/tutorials/java/AffinagePleiades/AffinageRuggedLiaison.java b/src/tutorials/java/AffinagePleiades/AffinageRuggedLiaison.java
index ea9c2f4ec99d0908a58bf02d32af7627dedcfe58..688b401822073b74b7850682f4cfd85d8c7f0ca8 100644
--- a/src/tutorials/java/AffinagePleiades/AffinageRuggedLiaison.java
+++ b/src/tutorials/java/AffinagePleiades/AffinageRuggedLiaison.java
@@ -221,8 +221,8 @@ public class AffinageRuggedLiaison {
System.out.format(" **** Add roll / pitch / factor values **** %n");
- double rollValueA = FastMath.toRadians(0.01);//0.6
- double pitchValueA = FastMath.toRadians(0.0);
+ double rollValueA = FastMath.toRadians(-0.01);//0.6
+ double pitchValueA = FastMath.toRadians(0.00);//0.02
double factorValue = 1.00;
System.out.format("roll : %3.5f pitch : %3.5f factor : %3.5f \n",rollValueA,pitchValueA,factorValue);
@@ -245,7 +245,6 @@ public class AffinageRuggedLiaison {
filter(driver -> driver.getName().equals(SensorNameA+"_factor")).
findFirst().get().setValue(factorValue);
-
lineDateA = lineSensorA.getDate(pleiadesViewingModelA.dimension/2);
losA = lineSensorA.getLOS(lineDateA,pleiadesViewingModelA.dimension/2);
centerPointA = ruggedA.directLocation(lineDateA, positionA, losA);
@@ -283,7 +282,7 @@ public class AffinageRuggedLiaison {
// initialize ruggedA without perturbation
- System.out.format(" **** Reset Roll/Pitch/Factor **** %n");
+ System.out.format(" **** Reset Roll/Pitch/Factor (ruggedA) **** %n");
ruggedA.
getLineSensor(pleiadesViewingModelA.getSensorName()).
getParametersDrivers().
@@ -309,13 +308,37 @@ public class AffinageRuggedLiaison {
}
});
-
-
+ // initialize ruggedB with selected flag set to true
+// System.out.format(" **** Select parameters drivers (ruggedB) **** %n");
+// ruggedB.
+// getLineSensor(pleiadesViewingModelB.getSensorName()).
+// getParametersDrivers().
+// filter(driver -> driver.getName().equals(SensorNameB+"_roll") || driver.getName().equals(SensorNameB+"_pitch")).
+// forEach(driver -> {
+// try {
+// driver.setSelected(true);
+// driver.setValue(0.0);
+// } catch (OrekitException e) {
+// throw new OrekitExceptionWrapper(e);
+// }
+// });
+// ruggedB.
+// getLineSensor(pleiadesViewingModelB.getSensorName()).
+// getParametersDrivers().
+// filter(driver -> driver.getName().equals(SensorNameB+"_factor")).
+// forEach(driver -> {
+// try {
+// driver.setSelected(true);
+// driver.setValue(1.0);
+// } catch (OrekitException e) {
+// throw new OrekitExceptionWrapper(e);
+// }
+// });
System.out.format(" **** Start optimization **** %n");
// perform parameters estimation
int maxIterations = 100;
- double convergenceThreshold = 1e-14;
+ double convergenceThreshold = 1e-14;//1e-14;
System.out.format("iterations max %d convergence threshold %3.6e \n",maxIterations, convergenceThreshold);
@@ -353,8 +376,8 @@ public class AffinageRuggedLiaison {
System.out.format("Estimated factor A %3.5f, factor error %3.6e %n ", estFactorA, factorError);
// Viewing model B
- double rollValueB = FastMath.toRadians(0.0);
- double pitchValueB = FastMath.toRadians(0.0);
+ double rollValueB = FastMath.toRadians(-0.00);
+ double pitchValueB = FastMath.toRadians(0.00);
double estRollB = ruggedB.getLineSensor(pleiadesViewingModelB.getSensorName()).
getParametersDrivers().
filter(driver -> driver.getName().equals(SensorNameB+"_roll")).
@@ -374,7 +397,7 @@ public class AffinageRuggedLiaison {
filter(driver -> driver.getName().equals(SensorNameB+"_factor")).
findFirst().get().getValue();
double factorErrorB = (estFactorB - factorValue);
- System.out.format("Estimated factor %3.5f factor error %3.6e %n ", estFactorB, factorError);
+ System.out.format("Estimated factor B %3.5f factor error %3.6e %n ", estFactorB, factorErrorB);
} catch (OrekitException oe) {
diff --git a/src/tutorials/java/AffinagePleiades/SensorToSensorMeasureGenerator.java b/src/tutorials/java/AffinagePleiades/SensorToSensorMeasureGenerator.java
index 767c21b4fbaabfb094bfffba108a04abf8574091..d958141961eb8b67ed207b2a379df71c37c7a06e 100644
--- a/src/tutorials/java/AffinagePleiades/SensorToSensorMeasureGenerator.java
+++ b/src/tutorials/java/AffinagePleiades/SensorToSensorMeasureGenerator.java
@@ -132,10 +132,10 @@ public class SensorToSensorMeasureGenerator {
// we need to test if the sensor pixel is found in the
// prescribed lines otherwise the sensor pixel is null
if (sensorPixelB != null) {
- System.out.format(Locale.US,
- "Sensor Pixel found : line = %5.3f, pixel = %5.3f %n",
- sensorPixelB.getLineNumber(),
- sensorPixelB.getPixelNumber());
+// System.out.format(Locale.US,
+// "Sensor Pixel found : line = %5.3f, pixel = %5.3f %n",
+// sensorPixelB.getLineNumber(),
+// sensorPixelB.getPixelNumber());
// TODO test if distance is 0.0 with crossing LOS
final AbsoluteDate dateB = sensorB.getDate(sensorPixelB.getLineNumber());
@@ -144,16 +144,15 @@ public class SensorToSensorMeasureGenerator {
// Get spacecraft to body transform of rugged instance A
final SpacecraftToObservedBody scToBodyA = ruggedA.getScToBody();
- //TODO work with double pixel values
- double distance = ruggedB.distanceBetweenLOS(sensorA,dateA, pixelA, scToBodyA, sensorB ,dateB,(int) pixelB);
+ double distance = ruggedB.distanceBetweenLOS(sensorA,dateA, pixelA, scToBodyA, sensorB ,dateB, pixelB);
System.out.format(Locale.US,"distance %f %n",distance);
mapping.addMapping(new SensorPixel(line, pixelA),
sensorPixelB, 0.0);
measureCount++;
} else {
- System.out
- .println("Sensor Pixel is null: point cannot be seen between the prescribed line numbers\n");
+// System.out
+// .println("Sensor Pixel is null: point cannot be seen between the prescribed line numbers\n");
}