From 25c543a2ed39555185bfa0b58bfaf4965d8291da Mon Sep 17 00:00:00 2001
From: Luc Maisonobe <luc@orekit.org>
Date: Thu, 18 Aug 2016 21:04:44 +0200
Subject: [PATCH] Started work on viewing models parameters estimation.
---
.../java/org/orekit/rugged/api/Rugged.java | 186 +++++++++++++++++-
.../rugged/api/SensorToGroundMapping.java | 97 +++++++++
.../orekit/rugged/errors/RuggedMessages.java | 3 +-
.../org/orekit/rugged/RuggedMessages_de.utf8 | 3 +
.../org/orekit/rugged/RuggedMessages_en.utf8 | 3 +
.../org/orekit/rugged/RuggedMessages_es.utf8 | 3 +
.../org/orekit/rugged/RuggedMessages_fr.utf8 | 3 +
.../org/orekit/rugged/RuggedMessages_gl.utf8 | 3 +
.../org/orekit/rugged/RuggedMessages_it.utf8 | 3 +
.../org/orekit/rugged/RuggedMessages_no.utf8 | 3 +
.../org/orekit/rugged/RuggedMessages_ro.utf8 | 3 +
.../rugged/errors/RuggedMessagesTest.java | 2 +-
12 files changed, 306 insertions(+), 6 deletions(-)
create mode 100644 src/main/java/org/orekit/rugged/api/SensorToGroundMapping.java
diff --git a/src/main/java/org/orekit/rugged/api/Rugged.java b/src/main/java/org/orekit/rugged/api/Rugged.java
index 59950a68..b785eebf 100644
--- a/src/main/java/org/orekit/rugged/api/Rugged.java
+++ b/src/main/java/org/orekit/rugged/api/Rugged.java
@@ -16,18 +16,35 @@
*/
package org.orekit.rugged.api;
-import org.hipparchus.analysis.differentiation.DerivativeStructure;
-import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
-import org.hipparchus.geometry.euclidean.threed.Vector3D;
-import org.hipparchus.util.FastMath;
+import java.util.ArrayList;
import java.util.Collection;
import java.util.HashMap;
+import java.util.HashSet;
+import java.util.List;
import java.util.Map;
+import java.util.Set;
+import org.hipparchus.analysis.differentiation.DerivativeStructure;
+import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
+import org.hipparchus.geometry.euclidean.threed.Vector3D;
+import org.hipparchus.linear.RealMatrix;
+import org.hipparchus.linear.RealVector;
+import org.hipparchus.optim.ConvergenceChecker;
+import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresBuilder;
+import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer;
+import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem;
+import org.hipparchus.optim.nonlinear.vector.leastsquares.LevenbergMarquardtOptimizer;
+import org.hipparchus.optim.nonlinear.vector.leastsquares.MultivariateJacobianFunction;
+import org.hipparchus.optim.nonlinear.vector.leastsquares.ParameterValidator;
+import org.hipparchus.util.FastMath;
+import org.hipparchus.util.Pair;
import org.orekit.bodies.GeodeticPoint;
+import org.orekit.errors.OrekitException;
+import org.orekit.errors.OrekitExceptionWrapper;
import org.orekit.frames.Transform;
import org.orekit.rugged.errors.DumpManager;
import org.orekit.rugged.errors.RuggedException;
+import org.orekit.rugged.errors.RuggedExceptionWrapper;
import org.orekit.rugged.errors.RuggedMessages;
import org.orekit.rugged.intersection.IntersectionAlgorithm;
import org.orekit.rugged.linesensor.LineSensor;
@@ -35,6 +52,7 @@ import org.orekit.rugged.linesensor.SensorMeanPlaneCrossing;
import org.orekit.rugged.linesensor.SensorPixel;
import org.orekit.rugged.linesensor.SensorPixelCrossing;
import org.orekit.rugged.utils.ExtendedEllipsoid;
+import org.orekit.rugged.utils.ExtendedParameterDriver;
import org.orekit.rugged.utils.NormalizedGeodeticPoint;
import org.orekit.rugged.utils.SpacecraftToObservedBody;
import org.orekit.time.AbsoluteDate;
@@ -598,6 +616,166 @@ public class Rugged {
finders.put(planeCrossing.getSensor().getName(), planeCrossing);
}
+ /** Estimate the free parameters in viewing model to match specified sensor
+ * to ground mappings.
+ * <p>
+ * This method is typically used for calibration of on-board sensor parameters,
+ * like rotation angles polynomial coefficients.
+ * </p>
+ * <p>
+ * Before using this method, the {@link ExtendedParameterDriver viewing model
+ * parameters} retrieved by calling the {@link
+ * LineSensor#getExtendedParametersDrivers() getExtendedParametersDrivers()}
+ * method on the desired sensors must be configured. The parameters that should
+ * be estimated must have their {@link ExtendedParameterDriver#setSelected(boolean)
+ * selection status} set to {@link true} whereas the parameters that should retain
+ * their current value must have their {@link ExtendedParameterDriver#setSelected(boolean)
+ * selection status} set to {@link false}. If needed, the {@link
+ * ExtendedParameterDriver#setValue(double) value} of the estimated/selected parameters
+ * can also be changed before calling the method, as this value will serve as the
+ * initial value in the estimation process.
+ * </p>
+ * <p>
+ * The method solves a least-squares problem to minimize the residuals between test
+ * locations and the reference mappings by adjusting the selected viewing models
+ * parameters.
+ * </p>
+ * <p>
+ * The estimated parameters can be retrieved after the method completes by calling
+ * again the {@link LineSensor#getExtendedParametersDrivers() getExtendedParametersDrivers()}
+ * method on the desired sensors and checking the updated values of the parameters.
+ * In fact, as the values of the parameters are already updated by this method, if
+ * wants to use the updated values immediately to perform new direct/inverse
+ * locations they can do so without looking at the parameters: the viewing models
+ * are already aware of the updated parameters.
+ * </p>
+ * @param references reference mappings between sensors pixels and ground point that
+ * should ultimately be reached by adjusting selected viewing models parameters
+ * @param maxEvaluations maximum number of evaluations
+ * @param parametersConvergenceThreshold convergence threshold on
+ * normalized parameters (dimensionless, related to parameters scales)
+ * @exception RuggedException if several parameters with the same name exist,
+ * or if parameters cannot be estimated (too few measurements, ill-conditioned problem ...)
+ */
+ public void estimateFreeParameters(final Collection<SensorToGroundMapping> references,
+ final int maxEvaluations,
+ final double parametersConvergenceThreshold)
+ throws RuggedException {
+ try {
+
+ // we are more stringent than Orekit orbit determination:
+ // we do not allow different parameters with the same name
+ final Set<String> names = new HashSet<>();
+ for (final SensorToGroundMapping reference : references) {
+ reference.getSensor().getExtendedParametersDrivers().forEach(driver -> {
+ if (names.contains(driver.getName())) {
+ throw new RuggedExceptionWrapper(new RuggedException(RuggedMessages.DUPLICATED_PARAMETER_NAME,
+ driver.getName()));
+ }
+ });
+ }
+
+ // gather free parameters from all reference mappings
+ final List<ExtendedParameterDriver> freeParameters = new ArrayList<>();
+ for (final SensorToGroundMapping reference : references) {
+ reference.
+ getSensor().
+ getExtendedParametersDrivers().
+ filter(driver -> driver.isSelected()).
+ forEach(driver -> freeParameters.add(driver));
+ }
+
+ // set up the indices and number of estimated parameters,
+ // so DerivativeStructure instances with the proper characteristics can be built
+ int index = 0;
+ for (final ExtendedParameterDriver driver : freeParameters) {
+ driver.setNbEstimated(freeParameters.size());
+ driver.setIndex(index++);
+ }
+
+ // get start point (as a normalized value)
+ final double[] start = new double[freeParameters.size()];
+ for (int i = 0; i < start.length; ++i) {
+ start[i] = freeParameters.get(i).getNormalizedValue();
+ }
+
+ // set up target in sensor domain
+ int n = 0;
+ for (final SensorToGroundMapping reference : references) {
+ n += reference.getMappings().size();
+ }
+ final double[] target = new double[2 * n];
+ int k = 0;
+ for (final SensorToGroundMapping reference : references) {
+ for (final Map.Entry<SensorPixel, GeodeticPoint> mapping : reference.getMappings()) {
+ target[k++] = mapping.getKey().getLineNumber();
+ target[k++] = mapping.getKey().getPixelNumber();
+ }
+ }
+
+ // prevent parameters to exceed their prescribed bounds
+ final ParameterValidator validator = params -> {
+ try {
+ int i = 0;
+ for (final ExtendedParameterDriver driver : freeParameters) {
+ // let the parameter handle min/max clipping
+ driver.setNormalizedValue(params.getEntry(i));
+ params.setEntry(i++, driver.getNormalizedValue());
+ }
+ return params;
+ } catch (OrekitException oe) {
+ throw new OrekitExceptionWrapper(oe);
+ }
+ };
+
+ // convergence checker
+ final ConvergenceChecker<LeastSquaresProblem.Evaluation> checker =
+ (iteration, previous, current) ->
+ current.getPoint().getLInfDistance(previous.getPoint()) <= parametersConvergenceThreshold;
+
+ // model function
+ final MultivariateJacobianFunction model = point -> {
+ try {
+
+ // set the current parameters values
+ int i = 0;
+ for (final ExtendedParameterDriver driver : freeParameters) {
+ driver.setNormalizedValue(point.getEntry(i++));
+ }
+
+ // TODO: compute inverse loc and its partial derivatives
+ return new Pair<RealVector, RealMatrix>(null, null);
+
+ } catch (OrekitException oe) {
+ throw new OrekitExceptionWrapper(oe);
+ }
+ };
+
+ // set up the least squares problem
+ final LeastSquaresProblem problem = new LeastSquaresBuilder().
+ lazyEvaluation(false).
+ weight(null).
+ start(start).
+ target(target).
+ parameterValidator(validator).
+ checker(checker).
+ model(model).
+ build();
+
+ // set up the optimizer
+ final LeastSquaresOptimizer optimizer = new LevenbergMarquardtOptimizer();
+
+ // solve the least squares problem
+ optimizer.optimize(problem);
+
+ } catch (RuggedExceptionWrapper rew) {
+ throw rew.getException();
+ } catch (OrekitExceptionWrapper oew) {
+ final OrekitException oe = oew.getException();
+ throw new RuggedException(oe, oe.getSpecifier(), oe.getParts());
+ }
+ }
+
/** Get transform from spacecraft to inertial frame.
* @param date date of the transform
* @return transform from spacecraft to inertial frame
diff --git a/src/main/java/org/orekit/rugged/api/SensorToGroundMapping.java b/src/main/java/org/orekit/rugged/api/SensorToGroundMapping.java
new file mode 100644
index 00000000..3de6f880
--- /dev/null
+++ b/src/main/java/org/orekit/rugged/api/SensorToGroundMapping.java
@@ -0,0 +1,97 @@
+/* Copyright 2013-2016 CS Systèmes d'Information
+ * Licensed to CS Systèmes d'Information (CS) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.
+ * CS licenses this file to You under the Apache License, Version 2.0
+ * (the "License"); you may not use this file except in compliance with
+ * the License. You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+package org.orekit.rugged.api;
+
+import java.util.Collections;
+import java.util.IdentityHashMap;
+import java.util.Map;
+import java.util.Set;
+
+import org.orekit.bodies.GeodeticPoint;
+import org.orekit.rugged.linesensor.LineSensor;
+import org.orekit.rugged.linesensor.SensorPixel;
+
+/** Container for mapping between sensor pixels and ground points.
+ * @author Luc Maisonobe
+ * @since 2.0
+ */
+public class SensorToGroundMapping {
+
+ /** Sensor to which mapping applies. */
+ private final LineSensor sensor;
+
+ /** Mapping from sensor to ground. */
+ private final Map<SensorPixel, GeodeticPoint> sensorToGround;
+
+ /** Mapping from ground to sensor. */
+ private final Map<GeodeticPoint, SensorPixel> groundToSensor;
+
+ /** Build a new instance.
+ * @param sensor sensor to which mapping applies
+ */
+ public SensorToGroundMapping(final LineSensor sensor) {
+ this.sensor = sensor;
+ this.sensorToGround = new IdentityHashMap<>();
+ this.groundToSensor = new IdentityHashMap<>();
+ }
+
+ /** Get the sensor to which mapping applies.
+ * @return sensor to which mapping applies
+ */
+ public LineSensor getSensor() {
+ return sensor;
+ }
+
+ /** Add a mapping between one sensor pixel and one ground point.
+ * @param pixel sensor pixel
+ * @param groundPoint ground point corresponding to the sensor pixel
+ */
+ public void addMapping(final SensorPixel pixel, final GeodeticPoint groundPoint) {
+ sensorToGround.put(pixel, groundPoint);
+ groundToSensor.put(groundPoint, pixel);
+ }
+
+ /** Get the ground point corresponding to a pixel.
+ * @param pixel sensor pixel (it must one of the instances
+ * passed to {@link #addMapping(SensorPixel, GeodeticPoint)},
+ * not a pixel at the same location)
+ * @return corresponding ground point, or null if the pixel was
+ * not passed to {@link #addMapping(SensorPixel, GeodeticPoint)}
+ */
+ public GeodeticPoint getGroundPoint(final SensorPixel pixel) {
+ return sensorToGround.get(pixel);
+ }
+
+ /** Get the sensor pixel corresponding to a ground point.
+ * @param groundPoint ground point (it must one of the instances
+ * passed to {@link #addMapping(SensorPixel, GeodeticPoint)},
+ * not a ground point at the same location)
+ * @return corresponding sensor pixel, or null if the ground point
+ * was not passed to {@link #addMapping(SensorPixel, GeodeticPoint)}
+ */
+ public SensorPixel getPixel(final GeodeticPoint groundPoint) {
+ return groundToSensor.get(groundPoint);
+ }
+
+ /** Get all the mapping entries.
+ * @return an unmodifiable view of all mapping entries
+ */
+ public Set<Map.Entry<SensorPixel, GeodeticPoint>> getMappings() {
+ return Collections.unmodifiableSet(sensorToGround.entrySet());
+ }
+
+}
diff --git a/src/main/java/org/orekit/rugged/errors/RuggedMessages.java b/src/main/java/org/orekit/rugged/errors/RuggedMessages.java
index 7c5f5f92..a0c453b1 100644
--- a/src/main/java/org/orekit/rugged/errors/RuggedMessages.java
+++ b/src/main/java/org/orekit/rugged/errors/RuggedMessages.java
@@ -74,7 +74,8 @@ public enum RuggedMessages implements Localizable {
LIGHT_TIME_CORRECTION_REDEFINED("light time correction redefined, line {0}, file {1}: {2}"),
ABERRATION_OF_LIGHT_CORRECTION_REDEFINED("aberration of light correction redefined, line {0}, file {1}: {2}"),
TILE_ALREADY_DEFINED("tile {0} already defined, line {1}, file {2}: {3}"),
- UNKNOWN_TILE("unknown tile {0}, line {1}, file {2}: {3}");
+ UNKNOWN_TILE("unknown tile {0}, line {1}, file {2}: {3}"),
+ DUPLICATED_PARAMETER_NAME("a different parameter with name {0} already exists");
// CHECKSTYLE: resume JavadocVariable check
diff --git a/src/main/resources/assets/org/orekit/rugged/RuggedMessages_de.utf8 b/src/main/resources/assets/org/orekit/rugged/RuggedMessages_de.utf8
index 5cffbc91..9938abc6 100644
--- a/src/main/resources/assets/org/orekit/rugged/RuggedMessages_de.utf8
+++ b/src/main/resources/assets/org/orekit/rugged/RuggedMessages_de.utf8
@@ -72,3 +72,6 @@ TILE_ALREADY_DEFINED = <MISSING TRANSLATION>
# unknown tile {0}, line {1}, file {2}: {3}
UNKNOWN_TILE = <MISSING TRANSLATION>
+
+# a different parameter with name {0} already exists
+DUPLICATED_PARAMETER_NAME = <MISSING TRANSLATION>
diff --git a/src/main/resources/assets/org/orekit/rugged/RuggedMessages_en.utf8 b/src/main/resources/assets/org/orekit/rugged/RuggedMessages_en.utf8
index fab5d639..b3604412 100644
--- a/src/main/resources/assets/org/orekit/rugged/RuggedMessages_en.utf8
+++ b/src/main/resources/assets/org/orekit/rugged/RuggedMessages_en.utf8
@@ -73,3 +73,6 @@ TILE_ALREADY_DEFINED = tile {0} already defined, line {1}, file {2}: {3}
# unknown tile {0}, line {1}, file {2}: {3}
UNKNOWN_TILE = unknown tile {0}, line {1}, file {2}: {3}
+# a different parameter with name {0} already exists
+DUPLICATED_PARAMETER_NAME = a different parameter with name {0} already exists
+
diff --git a/src/main/resources/assets/org/orekit/rugged/RuggedMessages_es.utf8 b/src/main/resources/assets/org/orekit/rugged/RuggedMessages_es.utf8
index a69adf75..25642997 100644
--- a/src/main/resources/assets/org/orekit/rugged/RuggedMessages_es.utf8
+++ b/src/main/resources/assets/org/orekit/rugged/RuggedMessages_es.utf8
@@ -72,3 +72,6 @@ TILE_ALREADY_DEFINED = <MISSING TRANSLATION>
# unknown tile {0}, line {1}, file {2}: {3}
UNKNOWN_TILE = <MISSING TRANSLATION>
+
+# a different parameter with name {0} already exists
+DUPLICATED_PARAMETER_NAME = <MISSING TRANSLATION>
diff --git a/src/main/resources/assets/org/orekit/rugged/RuggedMessages_fr.utf8 b/src/main/resources/assets/org/orekit/rugged/RuggedMessages_fr.utf8
index dd035767..969170f1 100644
--- a/src/main/resources/assets/org/orekit/rugged/RuggedMessages_fr.utf8
+++ b/src/main/resources/assets/org/orekit/rugged/RuggedMessages_fr.utf8
@@ -73,3 +73,6 @@ TILE_ALREADY_DEFINED = tuile {0} déjà définie ligne {1} du fichier {2}: {3}
# unknown tile {0}, line {1}, file {2}: {3}
UNKNOWN_TILE = tuile {0} inconnue ligne {1} du fichier {2}: {3}
+# a different parameter with name {0} already exists
+DUPLICATED_PARAMETER_NAME = il existe déjà un autre paramètre nommé {0}
+
diff --git a/src/main/resources/assets/org/orekit/rugged/RuggedMessages_gl.utf8 b/src/main/resources/assets/org/orekit/rugged/RuggedMessages_gl.utf8
index ffb09788..27bf9ece 100644
--- a/src/main/resources/assets/org/orekit/rugged/RuggedMessages_gl.utf8
+++ b/src/main/resources/assets/org/orekit/rugged/RuggedMessages_gl.utf8
@@ -73,3 +73,6 @@ TILE_ALREADY_DEFINED = <MISSING TRANSLATION>
# unknown tile {0}, line {1}, file {2}: {3}
UNKNOWN_TILE = <MISSING TRANSLATION>
+# a different parameter with name {0} already exists
+DUPLICATED_PARAMETER_NAME = <MISSING TRANSLATION>
+
diff --git a/src/main/resources/assets/org/orekit/rugged/RuggedMessages_it.utf8 b/src/main/resources/assets/org/orekit/rugged/RuggedMessages_it.utf8
index 21b54586..569c8374 100644
--- a/src/main/resources/assets/org/orekit/rugged/RuggedMessages_it.utf8
+++ b/src/main/resources/assets/org/orekit/rugged/RuggedMessages_it.utf8
@@ -73,3 +73,6 @@ TILE_ALREADY_DEFINED = <MISSING TRANSLATION>
# unknown tile {0}, line {1}, file {2}: {3}
UNKNOWN_TILE = <MISSING TRANSLATION>
+# a different parameter with name {0} already exists
+DUPLICATED_PARAMETER_NAME = <MISSING TRANSLATION>
+
diff --git a/src/main/resources/assets/org/orekit/rugged/RuggedMessages_no.utf8 b/src/main/resources/assets/org/orekit/rugged/RuggedMessages_no.utf8
index 05765b86..f94c208b 100644
--- a/src/main/resources/assets/org/orekit/rugged/RuggedMessages_no.utf8
+++ b/src/main/resources/assets/org/orekit/rugged/RuggedMessages_no.utf8
@@ -72,3 +72,6 @@ TILE_ALREADY_DEFINED = <MISSING TRANSLATION>
# unknown tile {0}, line {1}, file {2}: {3}
UNKNOWN_TILE = <MISSING TRANSLATION>
+
+# a different parameter with name {0} already exists
+DUPLICATED_PARAMETER_NAME = <MISSING TRANSLATION>
diff --git a/src/main/resources/assets/org/orekit/rugged/RuggedMessages_ro.utf8 b/src/main/resources/assets/org/orekit/rugged/RuggedMessages_ro.utf8
index 68d195bb..72263acf 100644
--- a/src/main/resources/assets/org/orekit/rugged/RuggedMessages_ro.utf8
+++ b/src/main/resources/assets/org/orekit/rugged/RuggedMessages_ro.utf8
@@ -72,3 +72,6 @@ TILE_ALREADY_DEFINED = <MISSING TRANSLATION>
# unknown tile {0}, line {1}, file {2}: {3}
UNKNOWN_TILE = <MISSING TRANSLATION>
+
+# a different parameter with name {0} already exists
+DUPLICATED_PARAMETER_NAME = <MISSING TRANSLATION>
diff --git a/src/test/java/org/orekit/rugged/errors/RuggedMessagesTest.java b/src/test/java/org/orekit/rugged/errors/RuggedMessagesTest.java
index bceb8253..62e411c5 100644
--- a/src/test/java/org/orekit/rugged/errors/RuggedMessagesTest.java
+++ b/src/test/java/org/orekit/rugged/errors/RuggedMessagesTest.java
@@ -30,7 +30,7 @@ public class RuggedMessagesTest {
@Test
public void testMessageNumber() {
- Assert.assertEquals(25, RuggedMessages.values().length);
+ Assert.assertEquals(26, RuggedMessages.values().length);
}
@Test
--
GitLab