From e2f8078865671db297c43992891983a6722dc701 Mon Sep 17 00:00:00 2001
From: LabatAllee Lucie <lucie.labat-allee@c-s.fr>
Date: Wed, 9 Nov 2016 16:21:28 +0100
Subject: [PATCH] add distanceBetweenLOS(), distanceBetweenLOSDerivatives() and
estimateFreeParams2Models() functions, SensorToSensorMapping.java class
---
.../java/org/orekit/rugged/api/Rugged.java | 273 ++++++++++++++++++
.../rugged/api/SensorToSensorMapping.java | 92 ++++++
.../java/AffinagePleiades/AffinageRugged.java | 3 +-
3 files changed, 366 insertions(+), 2 deletions(-)
create mode 100644 src/main/java/org/orekit/rugged/api/SensorToSensorMapping.java
diff --git a/src/main/java/org/orekit/rugged/api/Rugged.java b/src/main/java/org/orekit/rugged/api/Rugged.java
index 3609513c..2dbc7696 100644
--- a/src/main/java/org/orekit/rugged/api/Rugged.java
+++ b/src/main/java/org/orekit/rugged/api/Rugged.java
@@ -66,6 +66,7 @@ import org.orekit.utils.ParameterDriver;
/** Main class of Rugged library API.
* @see RuggedBuilder
* @author Luc Maisonobe
+ * @author Lucie LabatAllee
*/
public class Rugged {
@@ -584,7 +585,279 @@ public class Rugged {
return result;
}
+ /** Compute distance between two line sensors.
+ * @param sensorNameA name of the line sensor A
+ * @param dateA current date for sensor A
+ * @param pixelA pixel index for sensor A
+ * @param sensorNameB name of the line sensor B
+ * @param dateB current date for sensor B
+ * @param pixelB pixel index for sensor B
+ * @return distance computing
+ * @exception RuggedException if sensor is unknown
+ */
+ public double distanceBetweenLOS(final String sensorNameA,
+ final AbsoluteDate dateA, final int pixelA,
+ final String sensorNameB,
+ final AbsoluteDate dateB, final int pixelB)
+ throws RuggedException {
+
+ final LineSensor sensorA = getLineSensor(sensorNameA);
+ final LineSensor sensorB = getLineSensor(sensorNameB);
+
+ // Get sensors's position and LOS
+ final Vector3D vA = sensorA.getLOS(dateA, pixelA); // V_a : line of sight's vectorA
+ final Vector3D vB = sensorB.getLOS(dateB, pixelB); // V_b
+ final Vector3D sA = sensorA.getPosition(); // S_a : sensorA 's position
+ final Vector3D sB = sensorB.getPosition(); // S_b
+
+ final Vector3D vBase= sB.subtract(sA); // S_b - S_a
+ final double svA = Vector3D.dotProduct(vBase, vA); // SV_a = (S_b - S_a).V_a
+ final double svB = Vector3D.dotProduct(vBase, vB); // SV_b = (S_b - S_a).V_b
+
+ final double vAvB = Vector3D.dotProduct(vA, vB); // V_a.V_b
+
+ // Compute lambda_b = (SV_a * V_a.V_b - SV_b) / (1 + (V_a.V_b)²)
+ final double lambdaB = (svA * vAvB - svB) / (1 + FastMath.pow(vAvB,2));
+
+ // Compute lambda_a = SV_a + lambdaB * V_a.V_b
+ final double lambdaA = svA + lambdaB * vAvB;
+
+ final Vector3D mA = sA.add(vA.scalarMultiply(lambdaA)); // M_a = S_a + lambda_a * V_a
+ final Vector3D mB = sB.add(vB.scalarMultiply(lambdaB)); // M_b = S_b + lambda_b * V_b
+
+ final Vector3D vDistance = mB.subtract(mA); // M_b - M_a
+
+ // Get square of the euclidean norm
+ final double d = vDistance.getNormSq();
+ return d;
+ }
+ /** Compute distance between two line sensors with derivatives.
+ * @param sensorNameA name of the line sensor A
+ * @param dateA current date for sensor A
+ * @param pixelA pixel index for sensor A
+ * @param sensorNameB name of the line sensor B
+ * @param dateB current date for sensor B
+ * @param pixelB pixel index for sensor B
+ * @param generator generator to use for building {@link DerivativeStructure} instances
+ * @return distance computing with derivatives
+ * @exception RuggedException if sensor is unknown
+ * @see #distanceBetweenLOS(String, AbsoluteDate, int, String, AbsoluteDate, int)
+ */
+ private DerivativeStructure[] distanceBetweenLOSDerivatives(final String sensorNameA,
+ final AbsoluteDate dateA, final int pixelA,
+ final String sensorNameB,
+ final AbsoluteDate dateB, final int pixelB,
+ final DSGenerator generator)
+ throws RuggedException {
+
+ final LineSensor sensorA = getLineSensor(sensorNameA);
+ final LineSensor sensorB = getLineSensor(sensorNameB);
+
+ // Get sensors's LOS
+ final FieldVector3D<DerivativeStructure> vA = sensorA.getLOSDerivatives(dateA, pixelA, generator); // V_a : line of sight's vectorA
+ final FieldVector3D<DerivativeStructure> vB = sensorB.getLOSDerivatives(dateB, pixelB, generator); // V_b
+
+ // Get sensors's position. TODO: check if we have to implement getPositionDerivatives() method & CO
+ final Vector3D sAtmp = sensorA.getPosition(); // S_a : sensorA 's position
+ final Vector3D sBtmp = sensorB.getPosition(); // S_b
+ final DerivativeStructure scaleFactor = FieldVector3D.dotProduct(vA.normalize(), vA.normalize()); // V_a.V_a=1
+ // Build a vector from position vector and a scale factor (equals to 1).
+ // The vector built will be scaleFactor * sA for example.
+ final FieldVector3D<DerivativeStructure> sA = new FieldVector3D<DerivativeStructure>(scaleFactor, sAtmp);
+ final FieldVector3D<DerivativeStructure> sB = new FieldVector3D<DerivativeStructure>(scaleFactor, sBtmp);
+ // final FieldVector3D<DerivativeStructure> sA = sensorA.getPositionDerivatives(); // S_a : sensorA 's position
+ // final FieldVector3D<DerivativeStructure> sB = sensorB.getPositionDerivatives(); // S_b
+
+ final FieldVector3D<DerivativeStructure> vBase= sB.subtract(sA); // S_b - S_a
+ final DerivativeStructure svA = FieldVector3D.dotProduct(vBase, vA); // SV_a = (S_b - S_a).V_a
+ final DerivativeStructure svB = FieldVector3D.dotProduct(vBase, vB); // SV_b = (S_b - S_a).V_b
+
+ final DerivativeStructure vAvB = FieldVector3D.dotProduct(vA, vB); // V_a.V_b
+
+ // Compute lambda_b = (SV_a * V_a.V_b - SV_b) / (1 + (V_a.V_b)²)
+ final DerivativeStructure lambdaB = (svA.multiply(vAvB).subtract(svB)).divide(vAvB.pow(2).add(1));
+
+ // Compute lambda_a = SV_a + lambdaB * V_a.V_b
+ final DerivativeStructure lambdaA = vAvB.multiply(lambdaB).add(svA);
+
+ final FieldVector3D<DerivativeStructure> mA = sA.add(vA.scalarMultiply(lambdaA)); // M_a = S_a + lambda_a * V_a
+ final FieldVector3D<DerivativeStructure> mB = sB.add(vB.scalarMultiply(lambdaB)); // M_b = S_b + lambda_b * V_b
+
+ final FieldVector3D<DerivativeStructure> vDistance = mB.subtract(mA); // M_b - M_a
+
+ // Get square of the euclidean norm
+ final DerivativeStructure d = vDistance.getNormSq();
+ return new DerivativeStructure[] {d};
+ }
+ /** Estimate the free parameters from two viewing models (A and B)
+ * @param references reference mappings between two sensors pixels from two models
+ * and the corresponding computed distance between LOS 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)
+ * @param ruggedA rugged instance from viewing model A
+ * @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 ...)
+ */
+ public Optimum estimateFreeParams2Models(final Collection<SensorToSensorMapping> references,
+ final int maxEvaluations,
+ final double parametersConvergenceThreshold,
+ Rugged ruggedA)
+ throws RuggedException {
+ try {
+
+ // TODO BEGIN-----------------------
+ // Verify that createGenerator's construction is ok with the use of two Rugged instance
+ final List<LineSensor> selectedSensors = new ArrayList<>();
+ for (final SensorToSensorMapping reference : references) {
+ selectedSensors.add(getLineSensor(reference.getSensorNameA())); // from ruggedA instance
+ selectedSensors.add(getLineSensor(reference.getSensorNameB())); // from current ruggedB instance
+
+ }
+ final DSGenerator generator = createGenerator(selectedSensors);
+ final List<ParameterDriver> selected = generator.getSelected();
+ if (selected.isEmpty()) {
+ throw new RuggedException(RuggedMessages.NO_PARAMETERS_SELECTED);
+ }
+ // TODO END--------------------------
+
+ // get start point (as a normalized value)
+ final double[] start = new double[selected.size()];
+ for (int i = 0; i < start.length; ++i) {
+ start[i] = selected.get(i).getNormalizedValue();
+ }
+
+ // set up target : distance between two LOS from both viewing models (A and B)
+ int n = 0;
+ for (final SensorToSensorMapping reference : references) {
+ n += reference.getMappings().size();
+ }
+ if (n == 0) {
+ throw new RuggedException(RuggedMessages.NO_REFERENCE_MAPPINGS);
+ }
+ final double[] target = new double[n];
+ int k = 0;
+ for (final SensorToSensorMapping reference : references) {
+ for (final Double distMeas : reference.getMapDistance()) {
+ target[k++] = distMeas.doubleValue(); // distances measurements
+ }
+ }
+
+ // prevent parameters to exceed their prescribed bounds
+ final ParameterValidator validator = params -> {
+ try {
+ int i = 0;
+ for (final ParameterDriver driver : selected) {
+ // let the parameter handle min/max clipping
+ driver.setNormalizedValue(params.getEntry(i));
+ params.setEntry(i++, driver.getNormalizedValue());
+ }
+ return params;
+ } catch (OrekitException oe) {
+ throw new OrekitExceptionWrapper(oe);
+ }
+ };
+
+ // convergence checker
+ final ConvergenceChecker<LeastSquaresProblem.Evaluation> checker =
+ (iteration, previous, current) ->
+ current.getPoint().getLInfDistance(previous.getPoint()) <= parametersConvergenceThreshold;
+ // model function
+ final MultivariateJacobianFunction model = point -> {
+ try {
+
+ // set the current parameters values
+ int i = 0;
+ for (final ParameterDriver driver : selected) {
+ driver.setNormalizedValue(point.getEntry(i++));
+ // TODO: to be confirmed with the remark done above.
+ }
+
+ // compute distance and its partial derivatives
+ final RealVector value = new ArrayRealVector(target.length);
+ final RealMatrix jacobian = new Array2DRowRealMatrix(target.length, selected.size());
+ int l = 0;
+ for (final SensorToSensorMapping reference : references) {
+ for (final Map.Entry<SensorPixel, SensorPixel> mapping : reference.getMappings()) {
+ final SensorPixel spA = mapping.getKey();
+ final SensorPixel spB = mapping.getValue();
+ LineSensor lineSensorB = this.getLineSensor(reference.getSensorNameB());
+ LineSensor lineSensorA = ruggedA.getLineSensor(reference.getSensorNameA());
+ final AbsoluteDate dateA = lineSensorA.getDate(spA.getLineNumber());
+ final AbsoluteDate dateB = lineSensorB.getDate(spB.getLineNumber());
+ final int pixelA = (int)spA.getPixelNumber(); // Note: Rugged don't deal with half-pixel
+ final int pixelB = (int)spB.getPixelNumber();
+
+ final DerivativeStructure[] ilResult =
+ distanceBetweenLOSDerivatives(reference.getSensorNameA(),
+ dateA,
+ pixelA,
+ reference.getSensorNameB(),
+ dateB,
+ pixelB,
+ generator);
+
+ if (ilResult == null) {
+ // TODO
+ } else {
+ // extract the value
+ value.setEntry(l, ilResult[0].getValue());
+
+ // extract the Jacobian
+ final int[] orders = new int[selected.size()];
+ for (int m = 0; m < selected.size(); ++m) {
+ final double scale = selected.get(m).getScale();
+ orders[m] = 1;
+ jacobian.setEntry(l, m, ilResult[0].getPartialDerivative(orders) * scale);
+ orders[m] = 0;
+ }
+ }
+
+ l += 1; // pass to the next evaluation
+
+ }
+ }
+
+ // distance 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);
+ }
+ };
+
+ // set up the least squares problem
+ final LeastSquaresProblem problem = new LeastSquaresBuilder().
+ lazyEvaluation(false).
+ maxIterations(maxEvaluations).
+ maxEvaluations(maxEvaluations).
+ weight(null).
+ start(start).
+ target(target).
+ parameterValidator(validator).
+ checker(checker).
+ model(model).
+ build();
+
+ // set up the optimizer
+ final LeastSquaresOptimizer optimizer = new LevenbergMarquardtOptimizer();
+
+ // solve the least squares problem
+ return optimizer.optimize(problem);
+
+ } catch (RuggedExceptionWrapper rew) {
+ throw rew.getException();
+ } catch (OrekitExceptionWrapper oew) {
+ final OrekitException oe = oew.getException();
+ throw new RuggedException(oe, oe.getSpecifier(), oe.getParts());
+ }
+ }
/** Get the mean plane crossing finder for a sensor.
* @param sensorName name of the line sensor
* @param minLine minimum line number
diff --git a/src/main/java/org/orekit/rugged/api/SensorToSensorMapping.java b/src/main/java/org/orekit/rugged/api/SensorToSensorMapping.java
new file mode 100644
index 00000000..ed784f8a
--- /dev/null
+++ b/src/main/java/org/orekit/rugged/api/SensorToSensorMapping.java
@@ -0,0 +1,92 @@
+/* Copyright 2013-2016 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.api;
+
+import java.util.ArrayList;
+import java.util.Collection;
+import java.util.Collections;
+import java.util.IdentityHashMap;
+import java.util.Map;
+import java.util.Set;
+import org.orekit.rugged.linesensor.SensorPixel;
+
+/** Container for mapping between sensor A pixels and sensor B pixels
+ * and storing distance between the two LOS computed with @link {@link Rugged#distanceBetweenLOS(
+ * String, org.orekit.time.AbsoluteDate, int, String, org.orekit.time.AbsoluteDate, int)}
+ * @author Lucie LabatAllee
+ * @since 2.0
+ */
+public class SensorToSensorMapping {
+
+ /** Name of the sensors to which mapping applies. */
+ private final String sensorNameA;
+ private final String sensorNameB;
+
+ /** Mapping from sensor A to sensor B. */
+ private final Map<SensorPixel, SensorPixel> sensorToSensor;
+
+ /** Distance between two LOS */
+ private final Collection<Double> mapDistance;
+
+
+ /** Build a new instance.
+ * @param sensorNameA name of the sensor A to which mapping applies
+ * @param sensorNameB name of the sensor B to which mapping applies
+ */
+ public SensorToSensorMapping(final String sensorNameA, final String sensorNameB) {
+ this.sensorNameA = sensorNameA;
+ this.sensorNameB = sensorNameB;
+ this.sensorToSensor = new IdentityHashMap<>();
+ this.mapDistance = new ArrayList<Double>();
+ }
+
+ /** Get the name of the sensor A to which mapping applies.
+ * @return name of the sensor A to which mapping applies
+ */
+ public String getSensorNameA() {
+ return sensorNameA;
+ }
+
+ /** Get the name of the sensor B to which mapping applies.
+ * @return name of the sensor B to which mapping applies
+ */
+ public String getSensorNameB() {
+ return sensorNameB;
+ }
+
+ /** Add a mapping between one sensor A pixel to one sensor B pixel and computed distance between both LOS
+ * @param pixelA sensor A pixel
+ * @param pixelB sensor B pixel corresponding to the sensor A pixel
+ */
+ public void addMapping(final SensorPixel pixelA, final SensorPixel pixelB, final double distance) {
+ sensorToSensor.put(pixelA, pixelB);
+ mapDistance.add(distance);
+ }
+
+ /** Get all the mapping entries.
+ * @return an unmodifiable view of all mapping entries
+ */
+ public Set<Map.Entry<SensorPixel, SensorPixel>> getMappings() {
+ return Collections.unmodifiableSet(sensorToSensor.entrySet());
+ }
+ /**
+ * @return the mapDistance
+ */
+ public Collection<Double> getMapDistance() {
+ return mapDistance;
+ }
+}
diff --git a/src/tutorials/java/AffinagePleiades/AffinageRugged.java b/src/tutorials/java/AffinagePleiades/AffinageRugged.java
index f1cb304f..ad6f1164 100644
--- a/src/tutorials/java/AffinagePleiades/AffinageRugged.java
+++ b/src/tutorials/java/AffinagePleiades/AffinageRugged.java
@@ -55,8 +55,7 @@ import org.orekit.utils.PVCoordinates;
/**
* Parameter refining context
* @author Jonathan Guinet
- * @author Lucie Labatallee
- *
+ * @author Lucie LabatAllee
*/
public class AffinageRugged {
--
GitLab