From 950e0db29611f188843daf2fa11a0c844bca858c Mon Sep 17 00:00:00 2001
From: Jonathan Guinet <jonathan.guinet@c-s.fr>
Date: Thu, 10 Nov 2016 11:21:34 -0500
Subject: [PATCH] AffinageRuggedLiaison
---
.../AffinageRuggedLiaison.java | 291 ++++++++++++++++++
.../SensorToSensorMeasureGenerator.java | 156 ++++++++++
2 files changed, 447 insertions(+)
create mode 100644 src/tutorials/java/AffinagePleiades/AffinageRuggedLiaison.java
create mode 100644 src/tutorials/java/AffinagePleiades/SensorToSensorMeasureGenerator.java
diff --git a/src/tutorials/java/AffinagePleiades/AffinageRuggedLiaison.java b/src/tutorials/java/AffinagePleiades/AffinageRuggedLiaison.java
new file mode 100644
index 00000000..2e889963
--- /dev/null
+++ b/src/tutorials/java/AffinagePleiades/AffinageRuggedLiaison.java
@@ -0,0 +1,291 @@
+/* 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 AffinagePleiades;
+
+import org.hipparchus.geometry.euclidean.threed.Vector3D;
+import org.hipparchus.util.FastMath;
+import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer.Optimum;
+import java.io.File;
+import java.util.Locale;
+import java.util.Collections;
+import java.io.FileWriter;
+import java.io.StringWriter;
+import java.io.PrintWriter;
+import java.io.IOException;
+
+import org.orekit.bodies.BodyShape;
+import org.orekit.bodies.GeodeticPoint;
+import org.orekit.data.DataProvidersManager;
+import org.orekit.data.DirectoryCrawler;
+import org.orekit.errors.OrekitException;
+import org.orekit.errors.OrekitExceptionWrapper;
+import org.orekit.forces.gravity.potential.NormalizedSphericalHarmonicsProvider;
+
+import org.orekit.orbits.Orbit;
+import org.orekit.rugged.api.AlgorithmId;
+import org.orekit.rugged.api.BodyRotatingFrameId;
+import org.orekit.rugged.api.EllipsoidId;
+import org.orekit.rugged.api.InertialFrameId;
+import org.orekit.rugged.api.Rugged;
+import org.orekit.rugged.api.RuggedBuilder;
+import org.orekit.rugged.errors.RuggedException;
+import org.orekit.rugged.linesensor.LineSensor;
+import org.orekit.rugged.linesensor.SensorPixel;
+import org.orekit.time.AbsoluteDate;
+import org.orekit.utils.AngularDerivativesFilter;
+import org.orekit.utils.CartesianDerivativesFilter;
+import org.orekit.utils.PVCoordinates;
+
+
+
+/**
+ * Parameter refining context
+ * @author Jonathan Guinet
+ * @author Lucie LabatAllee
+ */
+public class AffinageRuggedLiaison {
+
+ /**
+ * @param args
+ */
+ public static void main(String[] args) {
+
+ try {
+
+ // Initialize Orekit, assuming an orekit-data folder is in user home directory
+ File home = new File(System.getProperty("user.home"));
+ File orekitData = new File(home, "workspace/data/orekit-data");
+ //File orekitData = new File(home, "COTS/orekit-data");
+ DataProvidersManager.getInstance().addProvider(new DirectoryCrawler(orekitData));
+
+
+
+ //create 2 Pleiades Viewing Model
+ final String SensorNameA = "SensorA";
+ final double incidenceAngleA = -0.0;
+ final String dateA = "2016-01-01T11:59:50.0";
+ PleiadesViewingModel pleiadesViewingModelA = new PleiadesViewingModel(SensorNameA,incidenceAngleA,dateA);
+ final AbsoluteDate minDateA = pleiadesViewingModelA.getMinDate();
+ final AbsoluteDate maxDateA = pleiadesViewingModelA.getMaxDate();
+ final AbsoluteDate refDateA = pleiadesViewingModelA.getDatationReference();
+
+ //CreateOrbitA
+ OrbitModel orbitmodelA = new OrbitModel();
+ BodyShape earthA = orbitmodelA.createEarth();
+ NormalizedSphericalHarmonicsProvider gravityFieldA = orbitmodelA.createGravityField();
+ Orbit orbitA = orbitmodelA.createOrbit(gravityFieldA.getMu(), refDateA);
+
+ final double [] rollPoly = {0.0,0.0,0.0};
+ double[] pitchPoly = {0.0,0.0};
+ double[] yawPoly = {0.0,0.0,0.0};
+ orbitmodelA.setLOFTransform(rollPoly, pitchPoly, yawPoly, minDateA);
+
+ PVCoordinates PVA = orbitA.getPVCoordinates(earthA.getBodyFrame());
+ GeodeticPoint gpA = earthA.transform(PVA.getPosition(), earthA.getBodyFrame(), orbitA.getDate());
+
+ System.out.format(Locale.US, "Geodetic Point at date %s : φ = %8.10f °, λ = %8.10f %n",orbitA.getDate().toString(),
+ FastMath.toDegrees(gpA.getLatitude()),
+ FastMath.toDegrees(gpA.getLongitude()));
+
+
+ //create 2 Pleiades Viewing Model
+ final String SensorNameB = "SensorB";
+ final double incidenceAngleB = +0.0;
+ final String dateB = "2016-01-01T12:00:10.0";
+ PleiadesViewingModel pleiadesViewingModelB = new PleiadesViewingModel(SensorNameB,incidenceAngleB,dateB);
+ final AbsoluteDate minDateB = pleiadesViewingModelB.getMinDate();
+ final AbsoluteDate maxDateB = pleiadesViewingModelB.getMaxDate();
+ final AbsoluteDate refDateB = pleiadesViewingModelB.getDatationReference();
+
+ //CreateOrbitA
+ OrbitModel orbitmodelB = new OrbitModel();
+ BodyShape earthB = orbitmodelB.createEarth();
+ NormalizedSphericalHarmonicsProvider gravityFieldB = orbitmodelB.createGravityField();
+ Orbit orbitB = orbitmodelB.createOrbit(gravityFieldB.getMu(), refDateB);
+
+
+ orbitmodelB.setLOFTransform(rollPoly, pitchPoly, yawPoly, minDateB);
+
+ PVCoordinates PVB = orbitB.getPVCoordinates(earthB.getBodyFrame());
+ GeodeticPoint gpB = earthB.transform(PVB.getPosition(), earthB.getBodyFrame(), orbitB.getDate());
+
+ System.out.format(Locale.US, "Geodetic Point at date %s : φ = %8.10f °, λ = %8.10f %n",orbitA.getDate().toString(),
+ FastMath.toDegrees(gpB.getLatitude()),
+ FastMath.toDegrees(gpB.getLongitude()));
+
+ // Build Rugged A and B
+ RuggedBuilder ruggedBuilderA = new RuggedBuilder();
+
+ LineSensor lineSensorA = pleiadesViewingModelA.getLineSensor();
+ ruggedBuilderA.addLineSensor(lineSensorA);
+
+
+ ruggedBuilderA.setAlgorithm(AlgorithmId.IGNORE_DEM_USE_ELLIPSOID);
+ ruggedBuilderA.setEllipsoid(EllipsoidId.WGS84, BodyRotatingFrameId.ITRF);
+ ruggedBuilderA.setTimeSpan(minDateA,maxDateA, 0.001, 5.0).
+ setTrajectory(InertialFrameId.EME2000,
+ orbitmodelA.orbitToPV(orbitA, earthA, minDateA.shiftedBy(-0.0), maxDateA.shiftedBy(+0.0), 0.25),
+ 8, CartesianDerivativesFilter.USE_PV,
+ orbitmodelA.orbitToQ(orbitA, earthA, minDateA.shiftedBy(-0.0), maxDateA.shiftedBy(+0.0), 0.25),
+ 2, AngularDerivativesFilter.USE_R);
+ Rugged ruggedA = ruggedBuilderA.build();
+
+ RuggedBuilder ruggedBuilderB = new RuggedBuilder();
+
+ LineSensor lineSensorB = pleiadesViewingModelB.getLineSensor();
+ ruggedBuilderB.addLineSensor(lineSensorB);
+
+
+ ruggedBuilderB.setAlgorithm(AlgorithmId.IGNORE_DEM_USE_ELLIPSOID);
+ ruggedBuilderB.setEllipsoid(EllipsoidId.WGS84, BodyRotatingFrameId.ITRF);
+ ruggedBuilderB.setTimeSpan(minDateB,maxDateB, 0.001, 5.0).
+ setTrajectory(InertialFrameId.EME2000,
+ orbitmodelB.orbitToPV(orbitB, earthB, minDateB.shiftedBy(-0.0), maxDateB.shiftedBy(+0.0), 0.25),
+ 8, CartesianDerivativesFilter.USE_PV,
+ orbitmodelB.orbitToQ(orbitB, earthB, minDateB.shiftedBy(-0.0), maxDateB.shiftedBy(+0.0), 0.25),
+ 2, AngularDerivativesFilter.USE_R);
+ Rugged ruggedB = ruggedBuilderB.build();
+
+
+
+
+
+
+
+
+
+ Vector3D positionA = lineSensorA.getPosition(); // This returns a zero vector since we set the relative position of the sensor w.r.T the satellite to 0.
+
+ AbsoluteDate lineDateA = lineSensorA.getDate(pleiadesViewingModelA.dimension/2);
+ Vector3D losA = lineSensorA.getLOS(lineDateA,pleiadesViewingModelA.dimension/2);
+ GeodeticPoint centerPointA = ruggedA.directLocation(lineDateA, positionA, losA);
+ System.out.format(Locale.US, "center geodetic position : φ = %8.10f °, λ = %8.10f °, h = %8.3f m%n",
+ FastMath.toDegrees(centerPointA.getLatitude()),
+ FastMath.toDegrees(centerPointA.getLongitude()),centerPointA.getAltitude());
+
+
+ Vector3D positionB = lineSensorB.getPosition(); // This returns a zero vector since we set the relative position of the sensor w.r.T the satellite to 0.
+
+ AbsoluteDate lineDateB = lineSensorB.getDate(pleiadesViewingModelB.dimension/2);
+ Vector3D losB = lineSensorB.getLOS(lineDateB,pleiadesViewingModelB.dimension/2);
+ GeodeticPoint centerPointB = ruggedB.directLocation(lineDateB, positionB, losB);
+ System.out.format(Locale.US, "center geodetic position : φ = %8.10f °, λ = %8.10f °, h = %8.3f m%n",
+ FastMath.toDegrees(centerPointB.getLatitude()),
+ FastMath.toDegrees(centerPointB.getLongitude()),centerPointB.getAltitude());
+
+
+ lineDateB = lineSensorB.getDate(0);
+ losB = lineSensorB.getLOS(lineDateB,0);
+ GeodeticPoint firstPointB = ruggedB.directLocation(lineDateB, positionB, losB);
+ System.out.format(Locale.US, "first geodetic position : φ = %8.10f °, λ = %8.10f °, h = %8.3f m%n",
+ FastMath.toDegrees(firstPointB.getLatitude()),
+ FastMath.toDegrees(firstPointB.getLongitude()),firstPointB.getAltitude());
+
+ lineDateB = lineSensorB.getDate(pleiadesViewingModelB.dimension-1);
+ losB = lineSensorB.getLOS(lineDateB,pleiadesViewingModelB.dimension-1);
+ GeodeticPoint lastPointB = ruggedB.directLocation(lineDateB, positionB, losB);
+ System.out.format(Locale.US, "last geodetic position : φ = %8.10f °, λ = %8.10f °, h = %8.3f m%n",
+ FastMath.toDegrees(lastPointB.getLatitude()),
+ FastMath.toDegrees(lastPointB.getLongitude()),lastPointB.getAltitude());
+
+
+
+
+
+
+
+
+
+ double distance = DistanceTools.computeDistanceRad(centerPointA.getLongitude(), centerPointA.getLatitude(),
+ centerPointB.getLongitude(), centerPointB.getLatitude());
+
+ System.out.format("distance %f meters %n",distance);
+
+
+ double rollValueA = FastMath.toRadians(0.6);
+ double pitchValueA = FastMath.toRadians(0.0);
+ double factorValue = 1.00;
+
+ System.out.format("roll : %3.5f pitch : %3.5f factor : %3.5f \n",rollValueA,pitchValueA,factorValue);
+
+ ruggedA.
+ getLineSensor(SensorNameA).
+ getParametersDrivers().
+ filter(driver -> driver.getName().equals("roll")).
+ findFirst().get().setValue(rollValueA);
+
+ ruggedA.
+ getLineSensor(SensorNameA).
+ getParametersDrivers().
+ filter(driver -> driver.getName().equals("pitch")).
+ findFirst().get().setValue(pitchValueA);
+
+ ruggedA.
+ getLineSensor(SensorNameA).
+ getParametersDrivers().
+ filter(driver -> driver.getName().equals("factor")).
+ findFirst().get().setValue(factorValue);
+
+
+ lineDateA = lineSensorA.getDate(pleiadesViewingModelA.dimension/2);
+ losA = lineSensorA.getLOS(lineDateA,pleiadesViewingModelA.dimension/2);
+ centerPointA = ruggedA.directLocation(lineDateA, positionA, losA);
+ System.out.format(Locale.US, "center geodetic position : φ = %8.10f °, λ = %8.10f °, h = %8.3f m%n",
+ FastMath.toDegrees(centerPointA.getLatitude()),
+ FastMath.toDegrees(centerPointA.getLongitude()),centerPointA.getAltitude());
+
+
+ distance = DistanceTools.computeDistanceRad(centerPointA.getLongitude(), centerPointA.getLatitude(),
+ centerPointB.getLongitude(), centerPointB.getLatitude());
+
+ System.out.format("distance %f meters %n",distance);
+
+ // Search the sensor pixel seeing point
+ int minLine = 0;
+ int maxLine = pleiadesViewingModelB.dimension-1;
+ SensorPixel sensorPixelB = ruggedB.inverseLocation(SensorNameB, centerPointA, minLine, maxLine);
+ // we need to test if the sensor pixel is found in the prescribed lines otherwise the sensor pixel is null
+ if (sensorPixelB != null){
+ System.out.format(Locale.US, "Sensor Pixel found : line = %5.3f, pixel = %5.3f %n", sensorPixelB.getLineNumber(), sensorPixelB.getPixelNumber());
+ } else {
+ System.out.println("Sensor Pixel is null: point cannot be seen between the prescribed line numbers\n");
+ }
+
+
+ //add mapping
+
+ SensorToSensorMeasureGenerator measure = new SensorToSensorMeasureGenerator(pleiadesViewingModelA,ruggedA,pleiadesViewingModelB,ruggedB);
+ int lineSampling = 1000;
+ int pixelSampling = 1000;
+ measure.CreateMeasure(lineSampling, pixelSampling);
+ System.out.format("nb TiePoints %d %n", measure.getMeasureCount());
+
+
+
+ } catch (OrekitException oe) {
+ System.err.println(oe.getLocalizedMessage());
+ System.exit(1);
+ } catch (RuggedException re) {
+ System.err.println(re.getLocalizedMessage());
+ System.exit(1);
+ }
+
+ }
+
+
+
+}
diff --git a/src/tutorials/java/AffinagePleiades/SensorToSensorMeasureGenerator.java b/src/tutorials/java/AffinagePleiades/SensorToSensorMeasureGenerator.java
new file mode 100644
index 00000000..53c16350
--- /dev/null
+++ b/src/tutorials/java/AffinagePleiades/SensorToSensorMeasureGenerator.java
@@ -0,0 +1,156 @@
+/* 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 AffinagePleiades;
+
+
+import org.orekit.rugged.api.SensorToSensorMapping;
+import org.orekit.rugged.api.Rugged;
+import org.orekit.rugged.linesensor.LineSensor;
+import org.orekit.rugged.linesensor.SensorPixel;
+
+import org.orekit.rugged.errors.RuggedException;
+import org.orekit.time.AbsoluteDate;
+import org.hipparchus.analysis.differentiation.DerivativeStructure;
+import org.hipparchus.geometry.euclidean.threed.Vector3D;
+import org.hipparchus.random.UncorrelatedRandomVectorGenerator;
+import org.hipparchus.random.GaussianRandomGenerator;
+import org.hipparchus.random.Well19937a;
+import org.hipparchus.util.FastMath;
+import org.orekit.bodies.GeodeticPoint;
+import org.orekit.rugged.utils.DSGenerator;
+
+import java.util.Locale;
+
+/** class for measure generation
+ * @author Jonathan Guinet
+ */
+public class SensorToSensorMeasureGenerator {
+
+
+ /** mapping */
+ private SensorToSensorMapping mapping;
+
+ private Rugged ruggedA;
+
+ private Rugged ruggedB;
+
+ private LineSensor sensorA;
+
+ private LineSensor sensorB;
+
+ private PleiadesViewingModel viewingModelA;
+
+ private PleiadesViewingModel viewingModelB;
+
+ private int measureCount;
+
+ private String sensorNameA;
+ private String sensorNameB;
+
+
+
+
+ /** SensorToSensorMeasureGenerator
+ * @param viewingModelA
+ * @param ruggedA
+ * @param viewingModelB
+ * @param ruggedB
+ * @throws RuggedException
+ */
+ public SensorToSensorMeasureGenerator(PleiadesViewingModel viewingModelA, Rugged ruggedA,PleiadesViewingModel viewingModelB, Rugged ruggedB) throws RuggedException
+ {
+
+ // generate reference mapping
+ sensorNameA = viewingModelA.getSensorName();
+ sensorNameB = viewingModelB.getSensorName();
+
+ mapping = new SensorToSensorMapping(sensorNameA, sensorNameB);
+ this.ruggedA = ruggedA;
+ this.ruggedB = ruggedB;
+
+ this.viewingModelA = viewingModelA;
+ this.viewingModelB = viewingModelB;
+
+ sensorA = ruggedA.getLineSensor(mapping.getSensorNameA());
+ sensorB = ruggedB.getLineSensor(mapping.getSensorNameB());
+ measureCount = 0;
+
+ }
+
+ public SensorToSensorMapping getMapping() {
+ return mapping;
+ }
+
+ public int getMeasureCount() {
+ return measureCount;
+ }
+
+ /** tie point creation from directLocation with RuugedA and inverse location with RuggedB
+ * @param lineSampling sampling along lines
+ * @param pixelSampling sampling along columns
+ * @throws RuggedException
+ */
+ public void CreateMeasure(final int lineSampling,final int pixelSampling) throws RuggedException
+ {
+ // Search the sensor pixel seeing point
+ final int minLine = 0;
+ final int maxLine = viewingModelB.dimension - 1;
+
+ for (double line = 0; line < viewingModelA.dimension; line += lineSampling) {
+
+ AbsoluteDate dateA = sensorA.getDate(line);
+ for (int pixelA = 0; pixelA < sensorA
+ .getNbPixels(); pixelA += pixelSampling) {
+
+ GeodeticPoint gpA = ruggedA
+ .directLocation(dateA, sensorA.getPosition(),
+ sensorA.getLOS(dateA, pixelA));
+
+ SensorPixel sensorPixelB = ruggedB
+ .inverseLocation(sensorNameB, gpA, minLine, maxLine);
+ // we need to test if the sensor pixel is found in the
+ // prescribed lines otherwise the sensor pixel is null
+ if (sensorPixelB != null) {
+ System.out.format(Locale.US,
+ "Sensor Pixel found : line = %5.3f, pixel = %5.3f %n",
+ sensorPixelB.getLineNumber(),
+ sensorPixelB.getPixelNumber());
+
+ // TODO test if distance is 0.0 with crossing LOS
+ final AbsoluteDate dateB = sensorB.getDate(sensorPixelB.getLineNumber());
+ double pixelB = sensorPixelB.getPixelNumber();
+ //TODO work with double pixel values
+ double distance = ruggedA.distanceBetweenLOS(sensorA,dateA, pixelA, sensorB ,dateB,(int) pixelB);
+
+ System.out.format(Locale.US,"distance %f %n",distance);
+ mapping.addMapping(new SensorPixel(line, pixelA),
+ sensorPixelB, 0.0);
+ measureCount++;
+ } else {
+ System.out
+ .println("Sensor Pixel is null: point cannot be seen between the prescribed line numbers\n");
+
+ }
+
+ }
+ }
+ }
+
+
+
+}
+
--
GitLab