Started work on viewing models parameters estimation.

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

src/main/java/org/orekit/rugged/api/SensorToGroundMapping.java

+/* 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());
+    }
+
+}

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
 

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>

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
+

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>

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}
+

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>
+

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>
+

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>

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>

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