GroundOptimizationProblemBuilder.java

  1. /* Copyright 2013-2022 CS GROUP
  2.  * Licensed to CS GROUP (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;

  19. import java.util.ArrayList;
  20. import java.util.HashMap;
  21. import java.util.List;
  22. import java.util.Map;

  24. import org.hipparchus.analysis.differentiation.Gradient;
  25. import org.hipparchus.linear.Array2DRowRealMatrix;
  26. import org.hipparchus.linear.ArrayRealVector;
  27. import org.hipparchus.linear.RealMatrix;
  28. import org.hipparchus.linear.RealVector;
  29. import org.hipparchus.optim.ConvergenceChecker;
  30. import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresBuilder;
  31. import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem;
  32. import org.hipparchus.optim.nonlinear.vector.leastsquares.MultivariateJacobianFunction;
  33. import org.hipparchus.optim.nonlinear.vector.leastsquares.ParameterValidator;
  34. import org.hipparchus.util.FastMath;
  35. import org.hipparchus.util.Pair;
  36. import org.orekit.bodies.GeodeticPoint;
  37. import org.orekit.rugged.adjustment.measurements.Observables;
  38. import org.orekit.rugged.adjustment.measurements.SensorToGroundMapping;
  39. import org.orekit.rugged.api.Rugged;
  40. import org.orekit.rugged.errors.RuggedException;
  41. import org.orekit.rugged.errors.RuggedMessages;
  42. import org.orekit.rugged.linesensor.LineSensor;
  43. import org.orekit.rugged.linesensor.SensorPixel;
  44. import org.orekit.utils.ParameterDriver;

  46. /** Ground optimization problem builder.
  47.  * builds the optimization problem relying on ground measurements.
  48.  * @author Guylaine Prat
  49.  * @author Lucie Labat Allee
  50.  * @author Jonathan Guinet
  51.  * @author Luc Maisonobe
  52.  * @since 2.0
  53.  */
  54. public class GroundOptimizationProblemBuilder extends OptimizationProblemBuilder {

  56.     /** Key for target. */
  57.     private static final String TARGET = "Target";

  59.     /** Key for weight. */
  60.     private static final String WEIGHT = "Weight";

  62.     /** Rugged instance to refine.*/
  63.     private final Rugged rugged;

  65.     /** Sensor to ground mapping to generate target tab for optimization.*/
  66.     private List<SensorToGroundMapping> sensorToGroundMappings;

  68.     /** Minimum line for inverse location estimation.*/
  69.     private int minLine;

  71.     /** Maximum line for inverse location estimation.*/
  72.     private int maxLine;

  74.     /** Target and weight (the solution of the optimization problem).*/
  75.     private HashMap<String, double[] > targetAndWeight;


  78.     /** Build a new instance of the optimization problem.
  79.      * @param sensors list of sensors to refine
  80.      * @param measurements set of observables
  81.      * @param rugged name of rugged to refine
  82.      */
  83.     public GroundOptimizationProblemBuilder(final List<LineSensor> sensors,
  84.                                             final Observables measurements, final Rugged rugged) {

  86.         super(sensors, measurements);
  87.         this.rugged = rugged;
  88.         this.initMapping();
  89.     }

  91.     /** {@inheritDoc} */
  92.     @Override
  93.     protected void initMapping() {

  95.         final String ruggedName = rugged.getName();
  96.         this.sensorToGroundMappings = new ArrayList<>();
  97.         for (final LineSensor lineSensor : this.getSensors()) {
  98.             final SensorToGroundMapping mapping = this.getMeasurements().getGroundMapping(ruggedName, lineSensor.getName());
  99.             if (mapping != null) {
  100.                 this.sensorToGroundMappings.add(mapping);
  101.             }
  102.         }
  103.     }

  105.     /** {@inheritDoc} */
  106.     @Override
  107.     protected void createTargetAndWeight() {

  109.         int n = 0;
  110.         for (final SensorToGroundMapping reference : this.sensorToGroundMappings) {
  111.             n += reference.getMapping().size();
  112.         }

  114.         if (n == 0) {
  115.             throw new RuggedException(RuggedMessages.NO_REFERENCE_MAPPINGS);
  116.         }
  117.         final double[] target = new double[2 * n];
  118.         final double[] weight = new double[2 * n];

  120.         double min = Double.POSITIVE_INFINITY;
  121.         double max = Double.NEGATIVE_INFINITY;
  122.         int k = 0;

  124.         for (final SensorToGroundMapping reference : this.sensorToGroundMappings) {
  125.             for (final Map.Entry<SensorPixel, GeodeticPoint> mapping : reference.getMapping()) {
  126.                 final SensorPixel sp = mapping.getKey();
  127.                 weight[k] = 1.0;
  128.                 target[k++] = sp.getLineNumber();
  129.                 weight[k] = 1.0;
  130.                 target[k++] = sp.getPixelNumber();
  131.                 min = FastMath.min(min, sp.getLineNumber());
  132.                 max = FastMath.max(max, sp.getLineNumber());
  133.             }
  134.         }

  136.         this.minLine = (int) FastMath.floor(min - ESTIMATION_LINE_RANGE_MARGIN);
  137.         this.maxLine = (int) FastMath.ceil(max - ESTIMATION_LINE_RANGE_MARGIN);
  138.         this.targetAndWeight = new HashMap<String, double[]>();
  139.         this.targetAndWeight.put(TARGET, target);
  140.         this.targetAndWeight.put(WEIGHT, weight);
  141.     }

  143.     /** {@inheritDoc} */
  144.     @Override
  145.     protected MultivariateJacobianFunction createFunction() {

  147.         // model function
  148.         final MultivariateJacobianFunction model = point -> {

  150.             // set the current parameters values
  151.             int i = 0;
  152.             for (final ParameterDriver driver : this.getDrivers()) {
  153.                 driver.setNormalizedValue(point.getEntry(i++));
  154.             }

  156.             final double[] target = this.targetAndWeight.get(TARGET);

  158.             // compute inverse loc and its partial derivatives
  159.             final RealVector value = new ArrayRealVector(target.length);
  160.             final RealMatrix jacobian = new Array2DRowRealMatrix(target.length, this.getNbParams());
  161.             int l = 0;
  162.             for (final SensorToGroundMapping reference : this.sensorToGroundMappings) {
  163.                 for (final Map.Entry<SensorPixel, GeodeticPoint> mapping : reference.getMapping()) {
  164.                     final GeodeticPoint gp = mapping.getValue();
  165.                     final Gradient[] ilResult = this.rugged.inverseLocationDerivatives(reference.getSensorName(), gp, minLine, maxLine, this.getGenerator());

  167.                     if (ilResult == null) {
  168.                         value.setEntry(l, minLine - 100.0); // arbitrary
  169.                         // line far
  170.                         // away
  171.                         value.setEntry(l + 1, -100.0); // arbitrary
  172.                         // pixel far away
  173.                     } else {
  174.                         // extract the value
  175.                         value.setEntry(l, ilResult[0].getValue());
  176.                         value.setEntry(l + 1, ilResult[1].getValue());

  178.                         // extract the Jacobian
  179.                         final int[] orders = new int[this.getNbParams()];
  180.                         int m = 0;
  181.                         for (final ParameterDriver driver : this.getDrivers()) {
  182.                             final double scale = driver.getScale();
  183.                             orders[m] = 1;
  184.                             jacobian.setEntry(l, m,
  185.                                     ilResult[0]
  186.                                             .getPartialDerivative(orders) *
  187.                                             scale);
  188.                             jacobian.setEntry(l + 1, m,
  189.                                     ilResult[1]
  190.                                             .getPartialDerivative(orders) *
  191.                                             scale);
  192.                             orders[m] = 0;
  193.                             m++;
  194.                         }
  195.                     }

  197.                     l += 2;
  198.                 }
  199.             }

  201.             // inverse loc result with Jacobian for all reference points
  202.             return new Pair<RealVector, RealMatrix>(value, jacobian);
  203.         };

  205.         return model;
  206.     }


  209.     /** Least square problem builder.
  210.      * @param maxEvaluations maxIterations and evaluations
  211.      * @param convergenceThreshold parameter convergence threshold
  212.      * @return the least square problem
  213.      */
  214.     @Override
  215.     public final LeastSquaresProblem build(final int maxEvaluations, final double convergenceThreshold) {

  217.         this.createTargetAndWeight();
  218.         final double[] target = this.targetAndWeight.get(TARGET);
  219.         final double[] start = this.createStartTab();
  220.         final ParameterValidator validator = this.createParameterValidator();
  221.         final ConvergenceChecker<LeastSquaresProblem.Evaluation> checker = this.createChecker(convergenceThreshold);
  222.         final MultivariateJacobianFunction model = this.createFunction();
  223.         return new LeastSquaresBuilder()
  224.                         .lazyEvaluation(false).maxIterations(maxEvaluations)
  225.                         .maxEvaluations(maxEvaluations).weight(null).start(start)
  226.                         .target(target).parameterValidator(validator).checker(checker)
  227.                         .model(model).build();
  228.     }
  229. }
Created with JaCoCo 0.8.7.202105040129