GroundOptimizationProblemBuilder.java
/* Copyright 2013-2019 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.adjustment;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import org.hipparchus.analysis.differentiation.DerivativeStructure;
import org.hipparchus.linear.Array2DRowRealMatrix;
import org.hipparchus.linear.ArrayRealVector;
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.LeastSquaresProblem;
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.rugged.adjustment.measurements.Observables;
import org.orekit.rugged.adjustment.measurements.SensorToGroundMapping;
import org.orekit.rugged.api.Rugged;
import org.orekit.rugged.errors.RuggedException;
import org.orekit.rugged.errors.RuggedMessages;
import org.orekit.rugged.linesensor.LineSensor;
import org.orekit.rugged.linesensor.SensorPixel;
import org.orekit.utils.ParameterDriver;
/** Ground optimization problem builder.
* builds the optimization problem relying on ground measurements.
* @author Guylaine Prat
* @author Lucie Labat Allee
* @author Jonathan Guinet
* @author Luc Maisonobe
* @since 2.0
*/
public class GroundOptimizationProblemBuilder extends OptimizationProblemBuilder {
/** Key for target. */
private static final String TARGET = "Target";
/** Key for weight. */
private static final String WEIGHT = "Weight";
/** Rugged instance to refine.*/
private final Rugged rugged;
/** Sensor to ground mapping to generate target tab for optimization.*/
private List<SensorToGroundMapping> sensorToGroundMappings;
/** Minimum line for inverse location estimation.*/
private int minLine;
/** Maximum line for inverse location estimation.*/
private int maxLine;
/** Target and weight (the solution of the optimization problem).*/
private HashMap<String, double[] > targetAndWeight;
/** Build a new instance of the optimization problem.
* @param sensors list of sensors to refine
* @param measurements set of observables
* @param rugged name of rugged to refine
*/
public GroundOptimizationProblemBuilder(final List<LineSensor> sensors,
final Observables measurements, final Rugged rugged) {
super(sensors, measurements);
this.rugged = rugged;
this.initMapping();
}
/** {@inheritDoc} */
@Override
protected void initMapping() {
final String ruggedName = rugged.getName();
this.sensorToGroundMappings = new ArrayList<SensorToGroundMapping>();
for (final LineSensor lineSensor : this.getSensors()) {
final SensorToGroundMapping mapping = this.getMeasurements().getGroundMapping(ruggedName, lineSensor.getName());
if (mapping != null) {
this.sensorToGroundMappings.add(mapping);
}
}
}
/** {@inheritDoc} */
@Override
protected void createTargetAndWeight() {
int n = 0;
for (final SensorToGroundMapping reference : this.sensorToGroundMappings) {
n += reference.getMapping().size();
}
if (n == 0) {
throw new RuggedException(RuggedMessages.NO_REFERENCE_MAPPINGS);
}
final double[] target = new double[2 * n];
final double[] weight = new double[2 * n];
double min = Double.POSITIVE_INFINITY;
double max = Double.NEGATIVE_INFINITY;
int k = 0;
for (final SensorToGroundMapping reference : this.sensorToGroundMappings) {
for (final Map.Entry<SensorPixel, GeodeticPoint> mapping : reference.getMapping()) {
final SensorPixel sp = mapping.getKey();
weight[k] = 1.0;
target[k++] = sp.getLineNumber();
weight[k] = 1.0;
target[k++] = sp.getPixelNumber();
min = FastMath.min(min, sp.getLineNumber());
max = FastMath.max(max, sp.getLineNumber());
}
}
this.minLine = (int) FastMath.floor(min - ESTIMATION_LINE_RANGE_MARGIN);
this.maxLine = (int) FastMath.ceil(max - ESTIMATION_LINE_RANGE_MARGIN);
this.targetAndWeight = new HashMap<String, double[]>();
this.targetAndWeight.put(TARGET, target);
this.targetAndWeight.put(WEIGHT, weight);
}
/** {@inheritDoc} */
@Override
protected MultivariateJacobianFunction createFunction() {
// model function
final MultivariateJacobianFunction model = point -> {
// set the current parameters values
int i = 0;
for (final ParameterDriver driver : this.getDrivers()) {
driver.setNormalizedValue(point.getEntry(i++));
}
final double[] target = this.targetAndWeight.get(TARGET);
// compute inverse loc and its partial derivatives
final RealVector value = new ArrayRealVector(target.length);
final RealMatrix jacobian = new Array2DRowRealMatrix(target.length, this.getNbParams());
int l = 0;
for (final SensorToGroundMapping reference : this.sensorToGroundMappings) {
for (final Map.Entry<SensorPixel, GeodeticPoint> mapping : reference.getMapping()) {
final GeodeticPoint gp = mapping.getValue();
final DerivativeStructure[] ilResult = this.rugged.inverseLocationDerivatives(reference.getSensorName(), gp, minLine, maxLine, this.getGenerator());
if (ilResult == null) {
value.setEntry(l, minLine - 100.0); // arbitrary
// line far
// away
value.setEntry(l + 1, -100.0); // arbitrary
// pixel far away
} else {
// extract the value
value.setEntry(l, ilResult[0].getValue());
value.setEntry(l + 1, ilResult[1].getValue());
// extract the Jacobian
final int[] orders = new int[this.getNbParams()];
int m = 0;
for (final ParameterDriver driver : this.getDrivers()) {
final double scale = driver.getScale();
orders[m] = 1;
jacobian.setEntry(l, m,
ilResult[0]
.getPartialDerivative(orders) *
scale);
jacobian.setEntry(l + 1, m,
ilResult[1]
.getPartialDerivative(orders) *
scale);
orders[m] = 0;
m++;
}
}
l += 2;
}
}
// inverse loc result with Jacobian for all reference points
return new Pair<RealVector, RealMatrix>(value, jacobian);
};
return model;
}
/** Least square problem builder.
* @param maxEvaluations maxIterations and evaluations
* @param convergenceThreshold parameter convergence threshold
* @return the least square problem
*/
@Override
public final LeastSquaresProblem build(final int maxEvaluations, final double convergenceThreshold) {
this.createTargetAndWeight();
final double[] target = this.targetAndWeight.get(TARGET);
final double[] start = this.createStartTab();
final ParameterValidator validator = this.createParameterValidator();
final ConvergenceChecker<LeastSquaresProblem.Evaluation> checker = this.createChecker(convergenceThreshold);
final MultivariateJacobianFunction model = this.createFunction();
return new LeastSquaresBuilder()
.lazyEvaluation(false).maxIterations(maxEvaluations)
.maxEvaluations(maxEvaluations).weight(null).start(start)
.target(target).parameterValidator(validator).checker(checker)
.model(model).build();
}
}