InterSensorsOptimizationProblemBuilder.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.Collection;
import java.util.HashMap;
import java.util.Iterator;
import java.util.LinkedHashMap;
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.Pair;
import org.orekit.rugged.adjustment.measurements.Observables;
import org.orekit.rugged.adjustment.measurements.SensorToSensorMapping;
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.rugged.utils.SpacecraftToObservedBody;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.ParameterDriver;
/** Constructs the optimization problem for a list of tie points.
* @author Guylaine Prat
* @author Lucie Labat Allee
* @author Jonathan Guinet
* @author Luc Maisonobe
* @since 2.0
*/
public class InterSensorsOptimizationProblemBuilder extends OptimizationProblemBuilder {
/** Key for target. */
private static final String TARGET = "Target";
/** Key for weight. */
private static final String WEIGHT = "Weight";
/** List of rugged instance to refine.*/
private Map<String, Rugged> ruggedMap;
/** Sensor to ground mapping to generate target tab for optimization.*/
private List<SensorToSensorMapping> sensorToSensorMappings;
/** Targets and weights of optimization problem. */
private HashMap<String, double[] > targetAndWeight;
/** Constructor.
* @param sensors list of sensors to refine
* @param measurements set of observables
* @param ruggedList names of rugged to refine
*/
public InterSensorsOptimizationProblemBuilder(final List<LineSensor> sensors,
final Observables measurements, final Collection<Rugged> ruggedList) {
super(sensors, measurements);
this.ruggedMap = new LinkedHashMap<String, Rugged>();
for (final Rugged rugged : ruggedList) {
this.ruggedMap.put(rugged.getName(), rugged);
}
this.initMapping();
}
/** {@inheritDoc} */
@Override
protected void initMapping() {
this.sensorToSensorMappings = new ArrayList<SensorToSensorMapping>();
for (final String ruggedNameA : this.ruggedMap.keySet()) {
for (final String ruggedNameB : this.ruggedMap.keySet()) {
for (final LineSensor sensorA : this.getSensors()) {
for (final LineSensor sensorB : this.getSensors()) {
final String sensorNameA = sensorA.getName();
final String sensorNameB = sensorB.getName();
final SensorToSensorMapping mapping = this.getMeasurements().getInterMapping(ruggedNameA, sensorNameA, ruggedNameB, sensorNameB);
if (mapping != null) {
this.sensorToSensorMappings.add(mapping);
}
}
}
}
}
}
/** {@inheritDoc} */
@Override
protected void createTargetAndWeight() {
int n = 0;
for (final SensorToSensorMapping reference : this.sensorToSensorMappings) {
n += reference.getMapping().size();
}
if (n == 0) {
throw new RuggedException(RuggedMessages.NO_REFERENCE_MAPPINGS);
}
n = 2 * n;
final double[] target = new double[n];
final double[] weight = new double[n];
int k = 0;
for (final SensorToSensorMapping reference : this.sensorToSensorMappings) {
// Get central body constraint weight
final double bodyConstraintWeight = reference.getBodyConstraintWeight();
int i = 0;
for (Iterator<Map.Entry<SensorPixel, SensorPixel>> gtIt = reference.getMapping().iterator(); gtIt.hasNext(); i++) {
if (i == reference.getMapping().size()) break;
// Get LOS distance
final Double losDistance = reference.getLosDistance(i);
weight[k] = 1.0 - bodyConstraintWeight;
target[k++] = losDistance.doubleValue();
// Get central body distance (constraint)
final Double bodyDistance = reference.getBodyDistance(i);
weight[k] = bodyConstraintWeight;
target[k++] = bodyDistance.doubleValue();
}
}
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 distance 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 SensorToSensorMapping reference : this.sensorToSensorMappings) {
final String ruggedNameA = reference.getRuggedNameA();
final String ruggedNameB = reference.getRuggedNameB();
final Rugged ruggedA = this.ruggedMap.get(ruggedNameA);
if (ruggedA == null) {
throw new RuggedException(RuggedMessages.INVALID_RUGGED_NAME);
}
final Rugged ruggedB = this.ruggedMap.get(ruggedNameB);
if (ruggedB == null) {
throw new RuggedException(RuggedMessages.INVALID_RUGGED_NAME);
}
for (final Map.Entry<SensorPixel, SensorPixel> mapping : reference.getMapping()) {
final SensorPixel spA = mapping.getKey();
final SensorPixel spB = mapping.getValue();
final LineSensor lineSensorB = ruggedB.getLineSensor(reference.getSensorNameB());
final LineSensor lineSensorA = ruggedA.getLineSensor(reference.getSensorNameA());
final AbsoluteDate dateA = lineSensorA.getDate(spA.getLineNumber());
final AbsoluteDate dateB = lineSensorB.getDate(spB.getLineNumber());
final double pixelA = spA.getPixelNumber();
final double pixelB = spB.getPixelNumber();
final SpacecraftToObservedBody scToBodyA = ruggedA.getScToBody();
final DerivativeStructure[] ilResult =
ruggedB.distanceBetweenLOSderivatives(lineSensorA, dateA, pixelA, scToBodyA,
lineSensorB, dateB, pixelB, this.getGenerator());
// 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; // pass to the next evaluation
}
}
// distance 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();
}
}