OptimizationProblemBuilder.java

  1. /* Copyright 2013-2019 CS Systèmes d'Information
  2.  * Licensed to CS Systèmes d'Information (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * CS licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *   http://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */

  17. package org.orekit.rugged.adjustment;

  18. import java.util.ArrayList;
  19. import java.util.HashMap;
  20. import java.util.HashSet;
  21. import java.util.List;
  22. import java.util.Map;
  23. import java.util.Set;

  24. import org.hipparchus.analysis.differentiation.DSFactory;
  25. import org.hipparchus.analysis.differentiation.DerivativeStructure;
  26. import org.hipparchus.optim.ConvergenceChecker;
  27. import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem;
  28. import org.hipparchus.optim.nonlinear.vector.leastsquares.MultivariateJacobianFunction;
  29. import org.hipparchus.optim.nonlinear.vector.leastsquares.ParameterValidator;
  30. import org.orekit.rugged.adjustment.measurements.Observables;
  31. import org.orekit.rugged.errors.RuggedException;
  32. import org.orekit.rugged.errors.RuggedMessages;
  33. import org.orekit.rugged.linesensor.LineSensor;
  34. import org.orekit.rugged.utils.DSGenerator;
  35. import org.orekit.utils.ParameterDriver;

  36. /**
  37.  * Builder for optimization problem.
  38.  * <p>
  39.  * Constructs the optimization problem defined by a set of measurement and sensors.
  40.  * </p>
  41.  * @author Jonathan Guinet
  42.  * @author Guylaine Prat
  43.  * @since 2.0
  44.  */
  45. abstract class OptimizationProblemBuilder {

  46.     /** Margin used in parameters estimation for the inverse location lines range. */
  47.     protected static final int ESTIMATION_LINE_RANGE_MARGIN = 100;

  48.     /** Derivative structure generator.*/
  49.     private final DSGenerator generator;

  50.     /** Parameter drivers list. */
  51.     private final List<ParameterDriver> drivers;

  52.     /** Number of parameters to refine. */
  53.     private final int nbParams;

  54.     /** Measurements. */
  55.     private Observables measurements;

  56.     /** Sensors list. */
  57.     private final List<LineSensor> sensors;

  58.     /** Constructor.
  59.      * @param sensors list of sensors to refine
  60.      * @param measurements set of observables
  61.      */
  62.     OptimizationProblemBuilder(final List<LineSensor> sensors, final Observables measurements) {

  63.         this.generator = this.createGenerator(sensors);
  64.         this.drivers = this.generator.getSelected();
  65.         this.nbParams = this.drivers.size();
  66.         if (this.nbParams == 0) {
  67.             throw new RuggedException(RuggedMessages.NO_PARAMETERS_SELECTED);
  68.         }
  69.         this.measurements = measurements;
  70.         this.sensors = sensors;
  71.     }

  72.     /** Least squares problem builder.
  73.      * @param maxEvaluations maximum number of evaluations
  74.      * @param convergenceThreshold convergence threshold
  75.      * @return the least squares problem
  76.      */

  77.     public abstract LeastSquaresProblem build(int maxEvaluations, double convergenceThreshold);

  78.     /** Create the convergence check.
  79.      * <p>
  80.      * check LInf distance of parameters variation between previous and current iteration
  81.      * </p>
  82.      * @param parametersConvergenceThreshold convergence threshold
  83.      * @return the checker
  84.      */
  85.     final ConvergenceChecker<LeastSquaresProblem.Evaluation>
  86.                             createChecker(final double parametersConvergenceThreshold) {

  87.         final ConvergenceChecker<LeastSquaresProblem.Evaluation> checker = (iteration, previous, current)
  88.             -> current.getPoint().getLInfDistance(previous.getPoint()) <= parametersConvergenceThreshold;

  89.         return checker;
  90.     }

  91.     /** Create start points for optimization algorithm.
  92.      * @return start parameters values (normalized)
  93.      */
  94.     final double[] createStartTab() {

  95.         // Get start points (as a normalized value)
  96.         final double[] start = new double[this.nbParams];
  97.         int iStart = 0;
  98.         for (final ParameterDriver driver : this.drivers) {
  99.             start[iStart++] = driver.getNormalizedValue();
  100.         }
  101.         return start;
  102.     }

  103.     /** Create targets and weights of optimization problem. */
  104.     protected abstract void createTargetAndWeight();

  105.     /** Create the model function value and its Jacobian.
  106.      * @return the model function value and its Jacobian
  107.      */
  108.     protected abstract MultivariateJacobianFunction createFunction();

  109.     /** Parse the observables to select mapping .*/
  110.     protected abstract void initMapping();

  111.     /** Create parameter validator.
  112.      * @return parameter validator
  113.      */
  114.     final ParameterValidator createParameterValidator() {

  115.         // Prevent parameters to exceed their prescribed bounds
  116.         final ParameterValidator validator = params -> {
  117.             int i = 0;
  118.             for (final ParameterDriver driver : this.drivers) {

  119.                 // let the parameter handle min/max clipping
  120.                 driver.setNormalizedValue(params.getEntry(i));
  121.                 params.setEntry(i++, driver.getNormalizedValue());
  122.             }
  123.             return params;
  124.         };

  125.         return validator;
  126.     }

  127.     /** Create the generator for {@link DerivativeStructure} instances.
  128.      * @param selectedSensors list of sensors referencing the parameters drivers
  129.      * @return a new generator
  130.      */
  131.     private DSGenerator createGenerator(final List<LineSensor> selectedSensors) {

  132.         // Initialize set of drivers name
  133.         final Set<String> names = new HashSet<>();

  134.         // Get the drivers name
  135.         for (final LineSensor sensor : selectedSensors) {

  136.             // Get the drivers name for the sensor
  137.             sensor.getParametersDrivers().forEach(driver -> {

  138.                 // Add the name of the driver to the set of drivers name
  139.                 if (names.contains(driver.getName()) == false) {
  140.                     names.add(driver.getName());
  141.                 }
  142.             });
  143.         }

  144.         // Set up generator list and map
  145.         final List<ParameterDriver> selected = new ArrayList<>();
  146.         final Map<String, Integer> map = new HashMap<>();

  147.         // Get the list of selected drivers
  148.         for (final LineSensor sensor : selectedSensors) {

  149.             sensor.getParametersDrivers().filter(driver -> driver.isSelected()).forEach(driver -> {
  150.                 if (map.get(driver.getName()) == null) {
  151.                     map.put(driver.getName(), map.size());
  152.                     selected.add(driver);
  153.                 }
  154.             });
  155.         }

  156.         final DSFactory factory = new DSFactory(map.size(), 1);

  157.         // Derivative Structure Generator
  158.         return new DSGenerator() {

  159.             /** {@inheritDoc} */
  160.             @Override
  161.             public List<ParameterDriver> getSelected() {
  162.                 return selected;
  163.             }

  164.             /** {@inheritDoc} */
  165.             @Override
  166.             public DerivativeStructure constant(final double value) {
  167.                 return factory.constant(value);
  168.             }

  169.             /** {@inheritDoc} */
  170.             @Override
  171.             public DerivativeStructure variable(final ParameterDriver driver) {

  172.                 final Integer index = map.get(driver.getName());
  173.                 if (index == null) {
  174.                     return constant(driver.getValue());
  175.                 } else {
  176.                     return factory.variable(index.intValue(), driver.getValue());
  177.                 }
  178.             }
  179.         };
  180.     }

  181.     /** Get the sensors list.
  182.      * @return the sensors list
  183.      */
  184.     protected List<LineSensor> getSensors() {
  185.         return sensors;
  186.     }

  187.     /** Get the number of parameters to refine.
  188.      * @return the number of parameters to refine
  189.      */
  190.     protected final int getNbParams() {
  191.         return this.nbParams;
  192.     }

  193.     /**
  194.      * Get the parameters drivers list.
  195.      * @return the selected list of parameters driver
  196.      */
  197.     protected final List<ParameterDriver> getDrivers() {
  198.         return this.drivers;
  199.     }

  200.     /**
  201.      * Get the derivative structure generator.
  202.      * @return the derivative structure generator.
  203.      */
  204.     protected final DSGenerator getGenerator() {
  205.         return this.generator;
  206.     }

  207.     /** Get the measurements.
  208.      * @return the measurements
  209.      */
  210.     protected Observables getMeasurements() {
  211.         return measurements;
  212.     }
  213. }