From f938334f638808560c2fb10274ccc366a75ef39a Mon Sep 17 00:00:00 2001
From: Jonathan Guinet <jonathan.guinet@c-s.fr>
Date: Fri, 17 Feb 2017 13:10:07 -0500
Subject: [PATCH] add adjustment package
---
.../rugged/adjustment/AdjustmentContext.java | 172 +++++++------
.../GroundOptimizationProblemBuilder.java | 197 +++++++++++++++
.../adjustment/LeastSquareAdjuster.java | 58 ++++-
.../OptimizationProblemBuilder.java | 226 +++++++++++++++---
4 files changed, 546 insertions(+), 107 deletions(-)
create mode 100644 src/main/java/org/orekit/rugged/adjustment/GroundOptimizationProblemBuilder.java
diff --git a/src/main/java/org/orekit/rugged/adjustment/AdjustmentContext.java b/src/main/java/org/orekit/rugged/adjustment/AdjustmentContext.java
index 6e0fbe43..74b6d088 100644
--- a/src/main/java/org/orekit/rugged/adjustment/AdjustmentContext.java
+++ b/src/main/java/org/orekit/rugged/adjustment/AdjustmentContext.java
@@ -16,29 +16,23 @@
*/
package org.orekit.rugged.adjustment;
-import org.orekit.rugged.refining.measures.Observables;
-import org.orekit.rugged.api.Rugged;
-import org.orekit.rugged.errors.RuggedException;
-
-import org.hipparchus.optim.ConvergenceChecker;
-import org.hipparchus.optim.nonlinear.vector.leastsquares.GaussNewtonOptimizer;
-import org.hipparchus.optim.nonlinear.vector.leastsquares.LevenbergMarquardtOptimizer;
-import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresBuilder;
-import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem;
-import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer;
-import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer.Optimum;
-import org.hipparchus.optim.nonlinear.vector.leastsquares.MultivariateJacobianFunction;
-import org.hipparchus.optim.nonlinear.vector.leastsquares.ParameterValidator;
-import org.orekit.rugged.adjustment.OptimizerId;
-
-import org.hipparchus.linear.DiagonalMatrix;
-import org.hipparchus.optim.ConvergenceChecker;
-
+import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
+import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer.Optimum;
+import org.orekit.errors.OrekitException;
+import org.orekit.errors.OrekitExceptionWrapper;
+import org.orekit.rugged.api.Rugged;
+import org.orekit.rugged.errors.RuggedException;
+import org.orekit.rugged.errors.RuggedExceptionWrapper;
+import org.orekit.rugged.errors.RuggedMessages;
+import org.orekit.rugged.linesensor.LineSensor;
+import org.orekit.rugged.refining.measures.Observables;
+import org.orekit.utils.ParameterDriver;
+
/**
* Create adjustment context for viewing model refining refining
*
@@ -53,6 +47,10 @@ public class AdjustmentContext {
/** set of measures. */
private final Observables measures;
+ /** least square optimizer choice.*/
+ private OptimizerId optimizerID;
+
+
/** Build a new instance.
* @param viewingModel viewingModel
* @param measures control and tie points
@@ -64,62 +62,102 @@ public class AdjustmentContext {
this.viewingModel.put("Rugged", r);
}
this.measures = measures;
+ this.optimizerID = OptimizerId.GAUSS_NEWTON_QR;
}
-
- public Optimum estimateFreeParameters(String RuggedName) {
- Optimum optimum;
- return optimum;
- }
-
- public Optimum estimateFreeParameters2Viewing(String RuggedNameA, String RuggedNameB) {
- Optimum optimum;
- return optimum;
- }
-
- /** Create the LeastSquareProblem.
- * @param maxEvaluations maximum iterations and evaluations in optimization problem
- * @param weight Measures weight matrix
- * @param start
- * @param target
- * @param validator
- * @param checker
- * @param model
- * @return the least square problem
+
+ /** setter for optimizer algorithm.
+ * @param optimizerId the algorithm
*/
- public LeastSquaresProblem LeastSquaresProblemBuilder(final int maxEvaluations, final double[] weight,
- final double[] start, final double[] target,
- final ParameterValidator validator,
- final ConvergenceChecker<LeastSquaresProblem.Evaluation> checker,
- final MultivariateJacobianFunction model) {
- final LeastSquaresProblem problem = new LeastSquaresBuilder()
- .lazyEvaluation(false).maxIterations(maxEvaluations)
- .maxEvaluations(maxEvaluations).weight(new DiagonalMatrix(weight)).start(start)
- .target(target).parameterValidator(validator).checker(checker)
- .model(model).build();
- return problem;
+ public void setOptimizer(final OptimizerId optimizerId)
+ {
+ this.optimizerID = optimizerId;
}
- /** Create the optimizer.
- * @param optimizerID reference optimizer identifier
- * @return optimizer
+ /**
+ * 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 ParameterDriver viewing model
+ * parameters} retrieved by calling the
+ * {@link LineSensor#getParametersDrivers() getParametersDrivers()} method
+ * on the desired sensors must be configured. The parameters that should be
+ * estimated must have their {@link ParameterDriver#setSelected(boolean)
+ * selection status} set to {@link true} whereas the parameters that should
+ * retain their current value must have their
+ * {@link ParameterDriver#setSelected(boolean) selection status} set to
+ * {@link false}. If needed, the {@link ParameterDriver#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#getParametersDrivers()
+ * getParametersDrivers()} 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 users want 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)
+ * @return optimum of the least squares problem
+ * @exception RuggedException if several parameters with the same name
+ * exist, if no parameters have been selected for estimation, or
+ * if parameters cannot be estimated (too few measurements,
+ * ill-conditioned problem ...)
*/
- private LeastSquaresOptimizer selectOptimizer(final OptimizerId optimizerID) {
- // set up the optimizer
- switch (optimizerID) {
- case LEVENBERG_MARQUADT:
- return new LevenbergMarquardtOptimizer();
- case GAUSS_NEWTON_LU :
- return new GaussNewtonOptimizer()
- .withDecomposition(GaussNewtonOptimizer.Decomposition.LU);
- case GAUSS_NEWTON_QR :
- return new GaussNewtonOptimizer()
- .withDecomposition(GaussNewtonOptimizer.Decomposition.QR);
- default :
- // this should never happen
- throw RuggedException.createInternalError(null);
- }
+ public Optimum estimateFreeParameters(final String RuggedName, final int maxEvaluations, final double parametersConvergenceThreshold)
+ throws RuggedException {
+ try {
+
+ final Rugged rugged = this.viewingModel.get(RuggedName);
+ if (rugged == null) {
+ throw new RuggedException(RuggedMessages.INVALID_RUGGED_NAME);
+ }
+
+
+ final String sensorName = measures.getGroundMapping().getSensorName();
+
+ final List<LineSensor> selectedSensors = new ArrayList<>();
+ //TODO loop over all sensors
+ //for (final SensorToGroundMapping reference : references) {
+ selectedSensors.add(rugged.getLineSensor(sensorName));
+ //}
+ /** builder */
+ final GroundOptimizationProblemBuilder optimizationProblem = new GroundOptimizationProblemBuilder(selectedSensors, measures, rugged);
+
+
+ final LeastSquareAdjuster adjuster = new LeastSquareAdjuster(this.optimizerID);
+
+ return adjuster.optimize(optimizationProblem.build(maxEvaluations, parametersConvergenceThreshold));
+
+
+ } catch (RuggedExceptionWrapper rew) {
+ throw rew.getException();
+ } catch (OrekitExceptionWrapper oew) {
+ final OrekitException oe = oew.getException();
+ throw new RuggedException(oe, oe.getSpecifier(), oe.getParts());
+ }
}
+ /*public Optimum estimateFreeParameters(final String RuggedNameA, final String RuggedNameB, final int maxEvaluations, final float parametersConvergenceThreshold)*/
+
}
diff --git a/src/main/java/org/orekit/rugged/adjustment/GroundOptimizationProblemBuilder.java b/src/main/java/org/orekit/rugged/adjustment/GroundOptimizationProblemBuilder.java
new file mode 100644
index 00000000..54371084
--- /dev/null
+++ b/src/main/java/org/orekit/rugged/adjustment/GroundOptimizationProblemBuilder.java
@@ -0,0 +1,197 @@
+package org.orekit.rugged.adjustment;
+
+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.errors.OrekitException;
+import org.orekit.errors.OrekitExceptionWrapper;
+import org.orekit.rugged.api.Rugged;
+import org.orekit.rugged.errors.RuggedException;
+import org.orekit.rugged.errors.RuggedExceptionWrapper;
+import org.orekit.rugged.errors.RuggedMessages;
+import org.orekit.rugged.linesensor.LineSensor;
+import org.orekit.rugged.linesensor.SensorPixel;
+import org.orekit.rugged.refining.measures.Observables;
+import org.orekit.rugged.refining.measures.SensorToGroundMapping;
+import org.orekit.utils.ParameterDriver;
+
+
+
+public class GroundOptimizationProblemBuilder
+extends OptimizationProblemBuilder {
+
+ /** rugged instance to refine.*/
+ private final Rugged rugged;
+
+ /** sensorToGround mapping to generate target tab for optimization.*/
+ private final SensorToGroundMapping sensorToGroundMapping;
+
+ /** minimum line for inverse location estimation.*/
+ private int minLine;
+
+ /** maximum line for inverse location estimation.*/
+ private int maxLine;
+
+ private HashMap<String, double[] > targetAndWeight;
+
+ /**
+ * @param sensors list of sensors to refine
+ * @param measures set of observables
+ * @param rugged name of rugged to refine
+ * @throws RuggedException an exception is generated if no parameters has been selected for refining
+ */
+ public GroundOptimizationProblemBuilder(final List<LineSensor> sensors,
+ final Observables measures, final Rugged rugged)
+ throws RuggedException {
+ super(sensors, measures);
+ this.rugged = rugged;
+ this.sensorToGroundMapping = this.measures.getGroundMapping();
+ }
+
+
+ @Override
+ protected void createTargetAndWeight()
+ throws RuggedException {
+ try {
+ final int n = this.sensorToGroundMapping.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 Map.Entry<SensorPixel, GeodeticPoint> mapping : this.sensorToGroundMapping.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);
+
+ } catch (RuggedExceptionWrapper rew) {
+ throw rew.getException();
+ }
+ }
+
+ @Override
+ protected MultivariateJacobianFunction createFunction()
+ {
+ // model function
+ final MultivariateJacobianFunction model = point -> {
+ try {
+
+ // set the current parameters values
+ int i = 0;
+ for (final ParameterDriver driver : this.drivers.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.nbParams);
+ int l = 0;
+ for (final Map.Entry<SensorPixel, GeodeticPoint> mapping : this.sensorToGroundMapping.getMapping()) {
+ final GeodeticPoint gp = mapping.getValue();
+ final DerivativeStructure[] ilResult = this.rugged.inverseLocationDerivatives(this.sensorToGroundMapping.getSensorName(), gp, minLine, maxLine, generator);
+
+ 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.nbParams];
+ int m = 0;
+ for (final ParameterDriver driver : this.drivers.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);
+
+ } catch (RuggedException re) {
+ throw new RuggedExceptionWrapper(re);
+ } catch (OrekitException oe) {
+ throw new OrekitExceptionWrapper(oe);
+ }
+ };
+ return model;
+ }
+
+
+ /** leastsquare problem builder.
+ * @param maxEvaluations maxIterations and evaluations
+ * @param convergenceThreshold parameter convergence treshold
+ * @throws RuggedException if sensor is not found
+ * @return
+ */
+ @Override
+ public final LeastSquaresProblem build(final int maxEvaluations, final double convergenceThreshold) throws RuggedException {
+
+ 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();
+ }
+
+}
diff --git a/src/main/java/org/orekit/rugged/adjustment/LeastSquareAdjuster.java b/src/main/java/org/orekit/rugged/adjustment/LeastSquareAdjuster.java
index 8a344468..10b31a04 100644
--- a/src/main/java/org/orekit/rugged/adjustment/LeastSquareAdjuster.java
+++ b/src/main/java/org/orekit/rugged/adjustment/LeastSquareAdjuster.java
@@ -17,8 +17,64 @@
package org.orekit.rugged.adjustment;
-
+import org.hipparchus.optim.nonlinear.vector.leastsquares.GaussNewtonOptimizer;
+import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer;
+import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer.Optimum;
+import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem;
+import org.hipparchus.optim.nonlinear.vector.leastsquares.LevenbergMarquardtOptimizer;
+import org.orekit.rugged.errors.RuggedException;
public class LeastSquareAdjuster {
+ /** least square optimzaer.*/
+ private final LeastSquaresOptimizer adjuster;
+
+ /** least square optimizer choice.*/
+ private final OptimizerId optimizerID;
+
+ /** constructor.
+ * @param optimizerID optimizer choice
+ */
+ LeastSquareAdjuster(final OptimizerId optimizerID)
+ {
+ this.optimizerID = optimizerID;
+ this.adjuster = this.selectOptimizer();
+ }
+
+ /** default constructor assuming Gauss Newton QR algorithm.*/
+ LeastSquareAdjuster()
+ {
+ this.optimizerID = OptimizerId.GAUSS_NEWTON_QR;
+ this.adjuster = this.selectOptimizer();
+ }
+
+ /** solve the least square problem.
+ * @param problem the least square problem
+ * @return the solution
+ */
+ public Optimum optimize(final LeastSquaresProblem problem) {
+ return this.adjuster.optimize(problem);
+ }
+
+ /** Create the optimizer.
+ * @return optimizer
+ */
+ private LeastSquaresOptimizer selectOptimizer() {
+ // set up the optimizer
+ switch (this.optimizerID) {
+ case LEVENBERG_MARQUADT:
+ return new LevenbergMarquardtOptimizer();
+ case GAUSS_NEWTON_LU :
+ return new GaussNewtonOptimizer()
+ .withDecomposition(GaussNewtonOptimizer.Decomposition.LU);
+ case GAUSS_NEWTON_QR :
+ return new GaussNewtonOptimizer()
+ .withDecomposition(GaussNewtonOptimizer.Decomposition.QR);
+ default :
+ // this should never happen
+ throw RuggedException.createInternalError(null);
+ }
+
+ }
+
}
diff --git a/src/main/java/org/orekit/rugged/adjustment/OptimizationProblemBuilder.java b/src/main/java/org/orekit/rugged/adjustment/OptimizationProblemBuilder.java
index 7dc0b50c..a1cfa78d 100644
--- a/src/main/java/org/orekit/rugged/adjustment/OptimizationProblemBuilder.java
+++ b/src/main/java/org/orekit/rugged/adjustment/OptimizationProblemBuilder.java
@@ -1,58 +1,138 @@
/**
- *
+ *
*/
package org.orekit.rugged.adjustment;
-
-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.orekit.rugged.refining.measures.SensorToGroundMapping;
-import org.orekit.rugged.refining.measures.SensorToSensorMapping;
-
-import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem;
-import java.lang.Object;
-import org.orekit.utils.ParameterDriver;
-import org.orekit.utils.ParameterDriversList;
+import org.hipparchus.analysis.differentiation.DerivativeStructure;
import org.hipparchus.optim.ConvergenceChecker;
+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.orekit.errors.OrekitException;
import org.orekit.errors.OrekitExceptionWrapper;
+import org.orekit.rugged.errors.RuggedException;
+import org.orekit.rugged.errors.RuggedExceptionWrapper;
+import org.orekit.rugged.errors.RuggedMessages;
+import org.orekit.rugged.linesensor.LineSensor;
+import org.orekit.rugged.refining.measures.Observables;
+import org.orekit.rugged.utils.DSGenerator;
+import org.orekit.utils.ParameterDriver;
+import org.orekit.utils.ParameterDriversList;
-/** Builder for optimization problem
+/**
+ * Builder for optimization problem
* <p>
-* </p>
+ * </p>
+ *
* @author Jonathan Guinet
*/
-public class OptimizationProblemBuilder {
-
- final ParameterDriversList drivers;
+abstract class OptimizationProblemBuilder {
+
+ /**
+ * Margin used in parameters estimation for the inverse location lines
+ * range.
+ */
+ protected static final int ESTIMATION_LINE_RANGE_MARGIN = 100;
+
+ /**
+ *
+ */
+ final DSGenerator generator;
+
+ /**
+ *
+ */
+ protected final ParameterDriversList drivers;
+
+ /** number of params to refine. */
+ protected final int nbParams;
- public OptimizationProblemBuilder(final ParameterDriversList drivers) {
- this.drivers = drivers;
-
+
+ /** measures .*/
+ protected Observables measures;
+
+
+ /** OptimizationProblemBuilder constructor
+ * @param sensors
+ * @param measures
+ * @throws RuggedException
+ */
+ OptimizationProblemBuilder(final List<LineSensor> sensors,
+ Observables measures)
+ throws RuggedException {
+ try {
+ this.generator = this.createGenerator(sensors);
+ this.drivers = this.generator.getSelected();
+ this.nbParams = this.drivers.getNbParams();
+ if (this.nbParams == 0) {
+ throw new RuggedException(RuggedMessages.NO_PARAMETERS_SELECTED);
+ }
+ } catch (RuggedExceptionWrapper rew) {
+ throw rew.getException();
+ }
+
+ this.measures = measures;
+ }
+
+ /** nbParams getter.
+ * @return returns the number of variable parameters
+ */
+ public final int getNbParams()
+ {
+ return this.getNbParams();
+ }
+
+ /**
+ * parameters drivers list getter.
+ *
+ * @return selected parameters driver list
+ */
+ public final ParameterDriversList getSelectedParametersDriver() {
+ return this.drivers;
}
- /** Create the convergence check
+
+
+ /** leastsquare problem builder.
+ * @param maxEvaluations maxIterations and evaluations
+ * @param convergenceThreshold parameter convergence threshold
+ * @throws RuggedException if sensor is not found
+ * @return the problem
+ */
+
+ public abstract LeastSquaresProblem build(final int maxEvaluations, final float convergenceThreshold) throws RuggedException;
+
+
+ /**
+ * Create the convergence check.
* <p>
- * check Linf distance of parameters variation between previous and current iteration
+ * check Linf distance of parameters variation between previous and current
+ * iteration
* </p>
+ *
* @param parametersConvergenceThreshold convergence threshold
* @return the checker
*/
final ConvergenceChecker<LeastSquaresProblem.Evaluation>
- createChecker(final float parametersConvergenceThreshold) {
+ createChecker(final float parametersConvergenceThreshold) {
final ConvergenceChecker<LeastSquaresProblem.Evaluation> checker = (iteration,
- previous,
- current) -> current
- .getPoint()
- .getLInfDistance(previous
- .getPoint()) <= parametersConvergenceThreshold;
- return checker;
+ previous,
+ current) -> current
+ .getPoint()
+ .getLInfDistance(previous
+ .getPoint()) <= parametersConvergenceThreshold;
+ return checker;
}
- /** create start point for optimization algorithm.
+ /**
+ * create start point for optimization algorithm.
+ *
* @return start parameters values
*/
final double[] createStartTab() {
@@ -66,16 +146,16 @@ public class OptimizationProblemBuilder {
}
- final Set<double[]> createTargetAndWeight(SensorToSensorMapping references) {
- return Set<>;
- }
-
- final Set<double[]> createTargetAndWeight(SensorToGroundMapping references) {
- return Set<>;
- }
-
-
- /** create parameter validator.
+ protected abstract void createTargetAndWeight() throws RuggedException;
+
+
+ protected abstract MultivariateJacobianFunction createFunction();
+
+
+
+ /**
+ * create parameter validator.
+ *
* @return parameter validator
*/
final ParameterValidator createParameterValidator() {
@@ -95,4 +175,72 @@ public class OptimizationProblemBuilder {
};
return validator;
}
+
+ /**
+ * Create the generator for {@link DerivativeStructure} instances.
+ *
+ * @param selectedSensors sensors referencing the parameters drivers
+ * @return a new generator
+ */
+ private DSGenerator
+ createGenerator(final List<LineSensor> selectedSensors) {
+
+ final Set<String> names = new HashSet<>();
+ for (final LineSensor sensor : selectedSensors) {
+ sensor.getParametersDrivers().forEach(driver -> {
+ if (names.contains(driver.getName()) == false) {
+ names.add(driver.getName());
+ }
+ });
+ }
+
+ // set up generator list and map
+ final ParameterDriversList selected = new ParameterDriversList();
+ final Map<String, Integer> map = new HashMap<>();
+ for (final LineSensor sensor : selectedSensors) {
+ sensor.getParametersDrivers().filter(driver -> driver.isSelected())
+ .forEach(driver -> {
+ if (map.get(driver.getName()) == null) {
+ map.put(driver.getName(), map.size());
+ }
+
+ try {
+ selected.add(driver);
+ } catch (OrekitException e) {
+ e.printStackTrace();
+ }
+
+ });
+ }
+
+ return new DSGenerator() {
+
+ /** {@inheritDoc} */
+ @Override
+ public ParameterDriversList getSelected() {
+ return selected;
+ }
+
+ /** {@inheritDoc} */
+ @Override
+ public DerivativeStructure constant(final double value) {
+ return new DerivativeStructure(map.size(), 1, value);
+ }
+
+ /** {@inheritDoc} */
+ @Override
+ public DerivativeStructure variable(final ParameterDriver driver) {
+ final Integer index = map.get(driver.getName());
+ if (index == null) {
+ return constant(driver.getValue());
+ } else {
+ return new DerivativeStructure(map.size(), 1,
+ index.intValue(),
+ driver.getValue());
+ }
+ }
+
+ };
+ }
+
}
--
GitLab