From d3f50fd0de36ae30abe5586063c209218ad1a863 Mon Sep 17 00:00:00 2001
From: LabatAllee Lucie <lucie.labat-allee@c-s.fr>
Date: Mon, 14 Nov 2016 16:12:16 +0100
Subject: [PATCH] AffinageRuggedLiaison: the call to the optimization function
is added
---
.../java/org/orekit/rugged/api/Rugged.java | 5 +-
.../java/AffinagePleiades/AffinageRugged.java | 21 ++--
.../AffinageRuggedLiaison.java | 115 ++++++++++++++++--
.../PleiadesViewingModel.java | 6 +-
.../SensorToSensorMeasureGenerator.java | 6 +
5 files changed, 131 insertions(+), 22 deletions(-)
diff --git a/src/main/java/org/orekit/rugged/api/Rugged.java b/src/main/java/org/orekit/rugged/api/Rugged.java
index fa1cffd0..04ebd744 100644
--- a/src/main/java/org/orekit/rugged/api/Rugged.java
+++ b/src/main/java/org/orekit/rugged/api/Rugged.java
@@ -741,8 +741,8 @@ public class Rugged {
// 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(getLineSensor(reference.getSensorNameA())); // from ruggedA instance
- selectedSensors.add(getLineSensor(reference.getSensorNameB())); // from current ruggedB instance
+ selectedSensors.add(ruggedA.getLineSensor(reference.getSensorNameA())); // from ruggedA instance
+ selectedSensors.add(this.getLineSensor(reference.getSensorNameB())); // from current ruggedB instance
}
final DSGenerator generator = createGenerator(selectedSensors);
@@ -763,6 +763,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);
}
diff --git a/src/tutorials/java/AffinagePleiades/AffinageRugged.java b/src/tutorials/java/AffinagePleiades/AffinageRugged.java
index ad6f1164..e1546c45 100644
--- a/src/tutorials/java/AffinagePleiades/AffinageRugged.java
+++ b/src/tutorials/java/AffinagePleiades/AffinageRugged.java
@@ -65,8 +65,8 @@ public class AffinageRugged {
// Initialize Orekit, assuming an orekit-data folder is in user home directory
File home = new File(System.getProperty("user.home"));
- File orekitData = new File(home, "workspace/data/orekit-data");
- //File orekitData = new File(home, "COTS/orekit-data");
+ //File orekitData = new File(home, "workspace/data/orekit-data");
+ File orekitData = new File(home, "COTS/orekit-data");
DataProvidersManager.getInstance().addProvider(new DirectoryCrawler(orekitData));
@@ -189,19 +189,19 @@ public class AffinageRugged {
rugged.
getLineSensor("line").
getParametersDrivers().
- filter(driver -> driver.getName().equals("roll")).
+ filter(driver -> driver.getName().equals("line_roll")).
findFirst().get().setValue(rollValue);
rugged.
getLineSensor("line").
getParametersDrivers().
- filter(driver -> driver.getName().equals("pitch")).
+ filter(driver -> driver.getName().equals("line_pitch")).
findFirst().get().setValue(pitchValue);
rugged.
getLineSensor("line").
getParametersDrivers().
- filter(driver -> driver.getName().equals("factor")).
+ filter(driver -> driver.getName().equals("line_factor")).
findFirst().get().setValue(factorValue);
@@ -239,7 +239,7 @@ public class AffinageRugged {
rugged.
getLineSensor(pleiadesViewingModel.getSensorName()).
getParametersDrivers().
- filter(driver -> driver.getName().equals("roll") || driver.getName().equals("pitch")).
+ filter(driver -> driver.getName().equals("line_roll") || driver.getName().equals("line_pitch")).
forEach(driver -> {
try {
driver.setSelected(true);
@@ -251,7 +251,7 @@ public class AffinageRugged {
rugged.
getLineSensor(pleiadesViewingModel.getSensorName()).
getParametersDrivers().
- filter(driver -> driver.getName().equals("factor")).
+ filter(driver -> driver.getName().equals("line_factor")).
forEach(driver -> {
try {
driver.setSelected(true);
@@ -280,25 +280,26 @@ public class AffinageRugged {
System.out.format("max value %3.6e %n",optimum.getResiduals().getMaxValue());
System.out.format(" Optimization performed in %d iterations \n",optimum.getEvaluations());
+ System.out.format(" RMSE: %f \n",optimum.getRMS());
// check estimated values
double estimatedRoll = rugged.getLineSensor(pleiadesViewingModel.getSensorName()).
getParametersDrivers().
- filter(driver -> driver.getName().equals("roll")).
+ filter(driver -> driver.getName().equals("line_roll")).
findFirst().get().getValue();
double rollError = (estimatedRoll - rollValue);
System.out.format("Estimated roll %3.5f roll error %3.6e %n", estimatedRoll, rollError);
double estimatedPitch = rugged.getLineSensor(pleiadesViewingModel.getSensorName()).
getParametersDrivers().
- filter(driver -> driver.getName().equals("pitch")).
+ filter(driver -> driver.getName().equals("line_pitch")).
findFirst().get().getValue();
double pitchError = (estimatedPitch - pitchValue);
System.out.format("Estimated pitch %3.5f pitch error %3.6e %n ", estimatedPitch, pitchError);
double estimatedFactor = rugged.getLineSensor(pleiadesViewingModel.getSensorName()).
getParametersDrivers().
- filter(driver -> driver.getName().equals("factor")).
+ filter(driver -> driver.getName().equals("line_factor")).
findFirst().get().getValue();
double factorError = (estimatedFactor - factorValue);
System.out.format("Estimated factor %3.5f factor error %3.6e %n ", estimatedFactor, factorError);
diff --git a/src/tutorials/java/AffinagePleiades/AffinageRuggedLiaison.java b/src/tutorials/java/AffinagePleiades/AffinageRuggedLiaison.java
index 2e889963..ea9c2f4e 100644
--- a/src/tutorials/java/AffinagePleiades/AffinageRuggedLiaison.java
+++ b/src/tutorials/java/AffinagePleiades/AffinageRuggedLiaison.java
@@ -21,6 +21,7 @@ import org.hipparchus.util.FastMath;
import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer.Optimum;
import java.io.File;
import java.util.Locale;
+import java.util.Collection;
import java.util.Collections;
import java.io.FileWriter;
import java.io.StringWriter;
@@ -42,6 +43,7 @@ import org.orekit.rugged.api.EllipsoidId;
import org.orekit.rugged.api.InertialFrameId;
import org.orekit.rugged.api.Rugged;
import org.orekit.rugged.api.RuggedBuilder;
+import org.orekit.rugged.api.SensorToSensorMapping;
import org.orekit.rugged.errors.RuggedException;
import org.orekit.rugged.linesensor.LineSensor;
import org.orekit.rugged.linesensor.SensorPixel;
@@ -68,8 +70,8 @@ public class AffinageRuggedLiaison {
// Initialize Orekit, assuming an orekit-data folder is in user home directory
File home = new File(System.getProperty("user.home"));
- File orekitData = new File(home, "workspace/data/orekit-data");
- //File orekitData = new File(home, "COTS/orekit-data");
+ //File orekitData = new File(home, "workspace/data/orekit-data");
+ File orekitData = new File(home, "COTS/orekit-data");
DataProvidersManager.getInstance().addProvider(new DirectoryCrawler(orekitData));
@@ -82,6 +84,7 @@ public class AffinageRuggedLiaison {
final AbsoluteDate minDateA = pleiadesViewingModelA.getMinDate();
final AbsoluteDate maxDateA = pleiadesViewingModelA.getMaxDate();
final AbsoluteDate refDateA = pleiadesViewingModelA.getDatationReference();
+ System.out.format(Locale.US, "Viewing model A - date min: %s max: %s ref: %s %n", minDateA, maxDateA, refDateA);
//CreateOrbitA
OrbitModel orbitmodelA = new OrbitModel();
@@ -110,6 +113,7 @@ public class AffinageRuggedLiaison {
final AbsoluteDate minDateB = pleiadesViewingModelB.getMinDate();
final AbsoluteDate maxDateB = pleiadesViewingModelB.getMaxDate();
final AbsoluteDate refDateB = pleiadesViewingModelB.getDatationReference();
+ System.out.format(Locale.US, "Viewing model B - date min: %s max: %s ref: %s %n", minDateB, maxDateB, refDateB);
//CreateOrbitA
OrbitModel orbitmodelB = new OrbitModel();
@@ -216,7 +220,8 @@ public class AffinageRuggedLiaison {
System.out.format("distance %f meters %n",distance);
- double rollValueA = FastMath.toRadians(0.6);
+ System.out.format(" **** Add roll / pitch / factor values **** %n");
+ double rollValueA = FastMath.toRadians(0.01);//0.6
double pitchValueA = FastMath.toRadians(0.0);
double factorValue = 1.00;
@@ -225,19 +230,19 @@ public class AffinageRuggedLiaison {
ruggedA.
getLineSensor(SensorNameA).
getParametersDrivers().
- filter(driver -> driver.getName().equals("roll")).
+ filter(driver -> driver.getName().equals(SensorNameA+"_roll")).
findFirst().get().setValue(rollValueA);
ruggedA.
getLineSensor(SensorNameA).
getParametersDrivers().
- filter(driver -> driver.getName().equals("pitch")).
+ filter(driver -> driver.getName().equals(SensorNameA+"_pitch")).
findFirst().get().setValue(pitchValueA);
ruggedA.
getLineSensor(SensorNameA).
getParametersDrivers().
- filter(driver -> driver.getName().equals("factor")).
+ filter(driver -> driver.getName().equals(SensorNameA+"_factor")).
findFirst().get().setValue(factorValue);
@@ -267,7 +272,7 @@ public class AffinageRuggedLiaison {
//add mapping
-
+ System.out.format(" **** Generate Measures **** %n");
SensorToSensorMeasureGenerator measure = new SensorToSensorMeasureGenerator(pleiadesViewingModelA,ruggedA,pleiadesViewingModelB,ruggedB);
int lineSampling = 1000;
int pixelSampling = 1000;
@@ -275,6 +280,102 @@ public class AffinageRuggedLiaison {
System.out.format("nb TiePoints %d %n", measure.getMeasureCount());
+
+
+ // initialize ruggedA without perturbation
+ System.out.format(" **** Reset Roll/Pitch/Factor **** %n");
+ ruggedA.
+ getLineSensor(pleiadesViewingModelA.getSensorName()).
+ getParametersDrivers().
+ filter(driver -> driver.getName().equals(SensorNameA+"_roll") || driver.getName().equals(SensorNameA+"_pitch")).
+ forEach(driver -> {
+ try {
+ driver.setSelected(true);
+ driver.setValue(0.0);
+ } catch (OrekitException e) {
+ throw new OrekitExceptionWrapper(e);
+ }
+ });
+ ruggedA.
+ getLineSensor(pleiadesViewingModelA.getSensorName()).
+ getParametersDrivers().
+ filter(driver -> driver.getName().equals(SensorNameA+"_factor")).
+ forEach(driver -> {
+ try {
+ driver.setSelected(true);
+ driver.setValue(1.0); // default value: no Z scale factor applied
+ } catch (OrekitException e) {
+ throw new OrekitExceptionWrapper(e);
+ }
+ });
+
+
+
+
+ System.out.format(" **** Start optimization **** %n");
+ // perform parameters estimation
+ int maxIterations = 100;
+ double convergenceThreshold = 1e-14;
+
+ System.out.format("iterations max %d convergence threshold %3.6e \n",maxIterations, convergenceThreshold);
+
+ // Note: estimateFreeParams2Models() is supposed to be applying on ruggedB, not on ruggedA (to be compliant with notations)
+ // TODO: apply perturbations on ruggedB, not on ruggedA
+ Optimum optimum = ruggedB.estimateFreeParams2Models(Collections.singletonList(measure.getMapping()), maxIterations,convergenceThreshold, ruggedA);
+ System.out.format("max value %3.6e %n",optimum.getResiduals().getMaxValue());
+
+ System.out.format(" Optimization performed in %d iterations \n",optimum.getEvaluations());
+ System.out.format(" RMSE: %f \n",optimum.getRMS());
+
+
+ System.out.format(" **** Check estimated values **** %n");
+ // check estimated values
+ // Viewing model A
+ double estRollA = ruggedA.getLineSensor(pleiadesViewingModelA.getSensorName()).
+ getParametersDrivers().
+ filter(driver -> driver.getName().equals(SensorNameA+"_roll")).
+ findFirst().get().getValue();
+ double rollErrorA = (estRollA - rollValueA);
+ System.out.format("Estimated roll A %3.5f, roll error %3.6e %n", estRollA, rollErrorA);
+
+ double estPitchA = ruggedA.getLineSensor(pleiadesViewingModelA.getSensorName()).
+ getParametersDrivers().
+ filter(driver -> driver.getName().equals(SensorNameA+"_pitch")).
+ findFirst().get().getValue();
+ double pitchErrorA = (estPitchA - pitchValueA);
+ System.out.format("Estimated pitch A %3.5f, pitch error %3.6e %n ", estPitchA, pitchErrorA);
+
+ double estFactorA = ruggedA.getLineSensor(pleiadesViewingModelA.getSensorName()).
+ getParametersDrivers().
+ filter(driver -> driver.getName().equals(SensorNameA+"_factor")).
+ findFirst().get().getValue();
+ double factorError = (estFactorA - factorValue);
+ 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 estRollB = ruggedB.getLineSensor(pleiadesViewingModelB.getSensorName()).
+ getParametersDrivers().
+ filter(driver -> driver.getName().equals(SensorNameB+"_roll")).
+ findFirst().get().getValue();
+ double rollErrorB = (estRollB - rollValueB);
+ System.out.format("Estimated roll B %3.5f, roll error %3.6e %n", estRollB, rollErrorB);
+
+ double estPitchB = ruggedB.getLineSensor(pleiadesViewingModelB.getSensorName()).
+ getParametersDrivers().
+ filter(driver -> driver.getName().equals(SensorNameB+"_pitch")).
+ findFirst().get().getValue();
+ double pitchErrorB = (estPitchB - pitchValueB);
+ System.out.format("Estimated pitch B %3.5f, pitch error %3.6e %n ", estPitchB, pitchErrorB);
+
+ double estFactorB = ruggedB.getLineSensor(pleiadesViewingModelB.getSensorName()).
+ getParametersDrivers().
+ 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);
+
} catch (OrekitException oe) {
System.err.println(oe.getLocalizedMessage());
diff --git a/src/tutorials/java/AffinagePleiades/PleiadesViewingModel.java b/src/tutorials/java/AffinagePleiades/PleiadesViewingModel.java
index eeea2282..128772bd 100644
--- a/src/tutorials/java/AffinagePleiades/PleiadesViewingModel.java
+++ b/src/tutorials/java/AffinagePleiades/PleiadesViewingModel.java
@@ -111,9 +111,9 @@ public class PleiadesViewingModel {
FastMath.toRadians(this.angle),
RotationConvention.VECTOR_OPERATOR).applyTo(Vector3D.PLUS_K), Vector3D.PLUS_I, FastMath.toRadians(fov/2), dimension);
- losBuilder.addTransform(new FixedRotation("roll", Vector3D.MINUS_I, 0.00));
- losBuilder.addTransform(new FixedRotation("pitch", Vector3D.MINUS_J, 0.00));
- losBuilder.addTransform(new FixedZHomothety("factor", 1.0));
+ losBuilder.addTransform(new FixedRotation(sensorName+"_roll", Vector3D.MINUS_I, 0.00));
+ losBuilder.addTransform(new FixedRotation(sensorName+"_pitch", Vector3D.MINUS_J, 0.00));
+ losBuilder.addTransform(new FixedZHomothety(sensorName+"_factor", 1.0));
return losBuilder.build();
}
diff --git a/src/tutorials/java/AffinagePleiades/SensorToSensorMeasureGenerator.java b/src/tutorials/java/AffinagePleiades/SensorToSensorMeasureGenerator.java
index 57d80ebb..767c21b4 100644
--- a/src/tutorials/java/AffinagePleiades/SensorToSensorMeasureGenerator.java
+++ b/src/tutorials/java/AffinagePleiades/SensorToSensorMeasureGenerator.java
@@ -23,6 +23,8 @@ import org.orekit.rugged.linesensor.LineSensor;
import org.orekit.rugged.linesensor.SensorPixel;
import org.orekit.rugged.errors.RuggedException;
+import org.orekit.rugged.errors.RuggedExceptionWrapper;
+import org.orekit.rugged.errors.RuggedMessages;
import org.orekit.time.AbsoluteDate;
import org.hipparchus.analysis.differentiation.DerivativeStructure;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
@@ -78,6 +80,10 @@ public class SensorToSensorMeasureGenerator {
// generate reference mapping
sensorNameA = viewingModelA.getSensorName();
sensorNameB = viewingModelB.getSensorName();
+ // check that sensors's name is different
+ if (sensorNameA.contains(sensorNameB)) {
+ throw new RuggedExceptionWrapper(new RuggedException(RuggedMessages.DUPLICATED_PARAMETER_NAME, sensorNameA));
+ }
mapping = new SensorToSensorMapping(sensorNameA, sensorNameB);
this.ruggedA = ruggedA;
--
GitLab