1   package org.orekit.rugged.adjustment.util;
2   
3   import java.io.File;
4   import java.lang.reflect.InvocationTargetException;
5   import java.net.URISyntaxException;
6   import java.util.ArrayList;
7   import java.util.Arrays;
8   import java.util.List;
9   
10  import org.hipparchus.analysis.differentiation.DerivativeStructure;
11  import org.hipparchus.analysis.differentiation.Gradient;
12  import org.hipparchus.geometry.euclidean.threed.Vector3D;
13  import org.hipparchus.random.GaussianRandomGenerator;
14  import org.hipparchus.random.UncorrelatedRandomVectorGenerator;
15  import org.hipparchus.random.Well19937a;
16  import org.hipparchus.util.FastMath;
17  import org.junit.jupiter.api.AfterEach;
18  import org.junit.jupiter.api.Assertions;
19  import org.orekit.bodies.BodyShape;
20  import org.orekit.bodies.GeodeticPoint;
21  import org.orekit.data.DataContext;
22  import org.orekit.data.DirectoryCrawler;
23  import org.orekit.errors.OrekitException;
24  import org.orekit.orbits.Orbit;
25  import org.orekit.rugged.TestUtils;
26  import org.orekit.rugged.adjustment.InterSensorsOptimizationProblemBuilder;
27  import org.orekit.rugged.adjustment.measurements.Observables;
28  import org.orekit.rugged.adjustment.measurements.SensorToSensorMapping;
29  import org.orekit.rugged.api.AlgorithmId;
30  import org.orekit.rugged.api.BodyRotatingFrameId;
31  import org.orekit.rugged.api.EllipsoidId;
32  import org.orekit.rugged.api.InertialFrameId;
33  import org.orekit.rugged.api.Rugged;
34  import org.orekit.rugged.api.RuggedBuilder;
35  import org.orekit.rugged.linesensor.LineSensor;
36  import org.orekit.rugged.linesensor.SensorPixel;
37  import org.orekit.rugged.utils.DerivativeGenerator;
38  import org.orekit.rugged.utils.SpacecraftToObservedBody;
39  import org.orekit.time.AbsoluteDate;
40  import org.orekit.utils.AngularDerivativesFilter;
41  import org.orekit.utils.CartesianDerivativesFilter;
42  import org.orekit.utils.Constants;
43  import org.orekit.utils.TimeStampedAngularCoordinates;
44  import org.orekit.utils.TimeStampedPVCoordinates;
45  
46  
47  /** Initialization for inter sensor refining Junit tests.
48   * @author Guylaine Prat
49   */
50  public class InitInterRefiningTest {
51  
52      /** Pleiades viewing model A */
53      private PleiadesViewingModel pleiadesViewingModelA;
54  
55      /** Pleiades viewing model B */
56      private PleiadesViewingModel pleiadesViewingModelB;
57      
58      /** Line sensor A */
59      private LineSensor lineSensorA;
60      
61      /** Line sensor B */
62      private LineSensor lineSensorB;
63  
64      /** RuggedA's instance */
65      private Rugged ruggedA;
66  
67      /** RuggedB's instance */
68      private Rugged ruggedB;
69      
70      /** Number of parameters to adjust */
71      private int parameterToAdjust;
72      
73      // Part of the name of parameter drivers
74      static final String rollSuffix = "_roll";
75      static final String pitchSuffix = "_pitch";
76      static final String factorName = "factor";
77  
78      // Default values for disruption to apply to roll (deg), pitch (deg) and factor 
79      static final double defaultRollDisruptionA =  0.004;
80      static final double defaultPitchDisruptionA = 0.0008;
81      static final double defaultFactorDisruptionA = 1.000000001;
82      static final double defaultRollDisruptionB =  -0.004;
83      static final double defaultPitchDisruptionB = 0.0008;
84      
85      /**
86       * Initialize refining tests with default values for disruptions on sensors characteristics
87       */
88      public void initRefiningTest() {
89          
90          initRefiningTest(defaultRollDisruptionA, defaultPitchDisruptionA, defaultFactorDisruptionA, 
91                           defaultRollDisruptionB, defaultPitchDisruptionB);
92      }
93  
94      /** Initialize refining tests with disruption on sensors characteristics
95       * @param rollDisruptionA disruption to apply to roll angle for sensor A (deg)
96       * @param pitchDisruptionA disruption to apply to pitch angle for sensor A (deg)
97       * @param factorDisruptionA disruption to apply to homothety factor for sensor A
98       * @param rollDisruptionB disruption to apply to roll angle for sensor B (deg)
99       * @param pitchDisruptionB disruption to apply to pitch angle for sensor B (deg)
100      */
101     public void initRefiningTest(double rollDisruptionA, double pitchDisruptionA, double factorDisruptionA, 
102                                  double rollDisruptionB, double pitchDisruptionB) {
103         try {
104             
105             String path = getClass().getClassLoader().getResource("orekit-data").toURI().getPath();
106             DataContext.getDefault().getDataProvidersManager().addProvider(new DirectoryCrawler(new File(path)));
107             
108             // Initialize refining context
109             // ---------------------------
110             final String sensorNameA = "SensorA";
111             final double rollAngleA = -5.0;
112             final String dateA = "2016-01-01T11:59:50.0";
113             this.pleiadesViewingModelA = new PleiadesViewingModel(sensorNameA, rollAngleA, dateA);
114 
115             final String sensorNameB = "SensorB";
116             final double rollAngleB = 0.0;
117             final String dateB = "2016-01-01T12:02:50.0";
118             this.pleiadesViewingModelB = new PleiadesViewingModel(sensorNameB, rollAngleB, dateB);
119 
120             PleiadesOrbitModel orbitmodelA =  new PleiadesOrbitModel();
121             PleiadesOrbitModel orbitmodelB =  new PleiadesOrbitModel();
122 
123             // Sensors's definition: creation of 2 Pleiades viewing models
124             // -----------------------------------------------------------
125             
126             // 1/- Create First Pleiades Viewing Model A
127             final AbsoluteDate minDateA =  pleiadesViewingModelA.getMinDate();
128             final AbsoluteDate maxDateA =  pleiadesViewingModelA.getMaxDate();
129             final AbsoluteDate refDateA = pleiadesViewingModelA.getDatationReference();
130             this.lineSensorA =  pleiadesViewingModelA.getLineSensor();
131 
132             // ----Satellite position, velocity and attitude: create orbit model A
133             BodyShape earthA = TestUtils.createEarth();
134             Orbit orbitA  = orbitmodelA.createOrbit(Constants.EIGEN5C_EARTH_MU, refDateA);
135 
136             // ----If no LOF Transform Attitude Provider is Nadir Pointing Yaw Compensation
137             final double [] rollPoly = {0.0,0.0,0.0};
138             final double[] pitchPoly = {0.025,0.0};
139             final double[] yawPoly = {0.0,0.0,0.0};
140             orbitmodelA.setLOFTransform(rollPoly, pitchPoly, yawPoly, minDateA);
141 
142             // ----Satellite attitude
143             List<TimeStampedAngularCoordinates> satelliteQListA = orbitmodelA.orbitToQ(orbitA, earthA, minDateA.shiftedBy(-0.0), maxDateA.shiftedBy(+0.0), 0.25);
144             final int nbQPoints = 2;
145 
146             // ----Position and velocities
147             List<TimeStampedPVCoordinates> satellitePVListA = orbitmodelA.orbitToPV(orbitA, earthA, minDateA.shiftedBy(-0.0), maxDateA.shiftedBy(+0.0), 0.25);
148             final int nbPVPoints = 8;
149 
150             // Rugged A initialization
151             // ---------------------
152             RuggedBuilder ruggedBuilderA = new RuggedBuilder();
153 
154             ruggedBuilderA.addLineSensor(lineSensorA);
155             ruggedBuilderA.setAlgorithm(AlgorithmId.IGNORE_DEM_USE_ELLIPSOID);
156             ruggedBuilderA.setEllipsoid(EllipsoidId.WGS84, BodyRotatingFrameId.ITRF);
157             ruggedBuilderA.setTimeSpan(minDateA,maxDateA, 0.001, 5.0);
158             ruggedBuilderA.setTrajectory(InertialFrameId.EME2000, satellitePVListA, nbPVPoints,
159                                          CartesianDerivativesFilter.USE_PV, satelliteQListA,
160                                          nbQPoints, AngularDerivativesFilter.USE_R);
161             ruggedBuilderA.setLightTimeCorrection(false);
162             ruggedBuilderA.setAberrationOfLightCorrection(false);
163 
164             ruggedBuilderA.setName("RuggedA");
165 
166             this.ruggedA = ruggedBuilderA.build();
167 
168 
169             // 2/- Create Second Pleiades Viewing Model
170             final AbsoluteDate minDateB =  pleiadesViewingModelB.getMinDate();
171             final AbsoluteDate maxDateB =  pleiadesViewingModelB.getMaxDate();
172             final AbsoluteDate refDateB = pleiadesViewingModelB.getDatationReference();
173             this.lineSensorB =  pleiadesViewingModelB.getLineSensor();
174 
175             // ----Satellite position, velocity and attitude: create orbit model B
176             BodyShape earthB = TestUtils.createEarth();
177             Orbit orbitB = orbitmodelB.createOrbit(Constants.EIGEN5C_EARTH_MU, refDateB);
178 
179             // ----Satellite attitude
180             List<TimeStampedAngularCoordinates> satelliteQListB = orbitmodelB.orbitToQ(orbitB, earthB, minDateB.shiftedBy(-0.0), maxDateB.shiftedBy(+0.0), 0.25);
181 
182             // ----Position and velocities
183             List<TimeStampedPVCoordinates> satellitePVListB = orbitmodelB.orbitToPV(orbitB, earthB, minDateB.shiftedBy(-0.0), maxDateB.shiftedBy(+0.0), 0.25);
184 
185             // Rugged B initialization
186             // ---------------------
187             RuggedBuilder ruggedBuilderB = new RuggedBuilder();
188 
189             ruggedBuilderB.addLineSensor(lineSensorB);
190             ruggedBuilderB.setAlgorithm(AlgorithmId.IGNORE_DEM_USE_ELLIPSOID);
191             ruggedBuilderB.setEllipsoid(EllipsoidId.WGS84, BodyRotatingFrameId.ITRF);
192             ruggedBuilderB.setTimeSpan(minDateB,maxDateB, 0.001, 5.0);
193             ruggedBuilderB.setTrajectory(InertialFrameId.EME2000, satellitePVListB, nbPVPoints,
194                                          CartesianDerivativesFilter.USE_PV, satelliteQListB,
195                                          nbQPoints, AngularDerivativesFilter.USE_R);
196             ruggedBuilderB.setLightTimeCorrection(false);
197             ruggedBuilderB.setAberrationOfLightCorrection(false);
198 
199             ruggedBuilderB.setName("RuggedB");
200 
201             this.ruggedB = ruggedBuilderB.build();
202 
203             
204             // Select parameters to adjust
205             // ---------------------------
206             RefiningParametersDriver.setSelectedRoll(ruggedA, sensorNameA);
207             RefiningParametersDriver.setSelectedPitch(ruggedA, sensorNameA);
208 
209             RefiningParametersDriver.setSelectedRoll(ruggedB, sensorNameB);
210             RefiningParametersDriver.setSelectedPitch(ruggedB, sensorNameB);
211             
212             this.parameterToAdjust = 4;
213 
214             // Initialize disruptions:
215             // -----------------------
216             // introduce rotations around instrument axes (roll and pitch angles, scale factor)
217 
218             // apply disruptions on physical model (acquisition A)
219             RefiningParametersDriver.applyDisruptionsRoll(ruggedA, sensorNameA, FastMath.toRadians(rollDisruptionA));
220             RefiningParametersDriver.applyDisruptionsPitch(ruggedA, sensorNameA, FastMath.toRadians(pitchDisruptionA));
221             RefiningParametersDriver.applyDisruptionsFactor(ruggedA, sensorNameA, factorDisruptionA);
222             
223             // apply disruptions on physical model (acquisition B)
224             RefiningParametersDriver.applyDisruptionsRoll(ruggedB, sensorNameB, FastMath.toRadians(rollDisruptionB));
225             RefiningParametersDriver.applyDisruptionsPitch(ruggedB, sensorNameB, FastMath.toRadians(pitchDisruptionB));
226             
227             
228         } catch (OrekitException oe) {
229             Assertions.fail(oe.getLocalizedMessage());
230         } catch (URISyntaxException use) {
231             Assertions.fail(use.getLocalizedMessage());
232         }
233     }
234     
235     /**
236      * Get the list of Rugged instances
237      * @return rugged instances as list
238      */
239     public List<Rugged> getRuggedList(){
240         
241         List<Rugged> ruggedList = Arrays.asList(ruggedA, ruggedB);
242         return ruggedList;
243     }
244     
245 
246     /** Compute the distances between LOS of two real pixels (one from sensor A and one from sensor B)
247      * @param realPixelA real pixel from sensor A
248      * @param realPixelB real pixel from sensor B
249      * @return the distances of two real pixels computed between LOS and to the ground
250      */
251     public double[] computeDistancesBetweenLOS(final SensorPixel realPixelA, final SensorPixel realPixelB) {
252         
253         final SpacecraftToObservedBody scToBodyA = ruggedA.getScToBody();
254 
255         final AbsoluteDate realDateA = lineSensorA.getDate(realPixelA.getLineNumber());
256         final AbsoluteDate realDateB = lineSensorB.getDate(realPixelB.getLineNumber());
257         
258         final double[] distanceLOSB = ruggedB.distanceBetweenLOS(
259                                            lineSensorA, realDateA, realPixelA.getPixelNumber(), 
260                                            scToBodyA,
261                                            lineSensorB, realDateB, realPixelB.getPixelNumber());
262         
263         return distanceLOSB;
264     }
265     
266     /** Compute the distances with derivatives between LOS of two real pixels (one from sensor A and one from sensor B)
267      * @param realPixelA real pixel from sensor A
268      * @param realPixelB real pixel from sensor B
269      * @return the distances of two real pixels computed between LOS and to the ground
270      * @deprecated as of 2.2, replaced by {@link #computeDistancesBetweenLOSGradient(SensorPixel, SensorPixel, double, double)}
271      */
272     public DerivativeStructure[] computeDistancesBetweenLOSDerivatives(final SensorPixel realPixelA, final SensorPixel realPixelB,
273                                                                        final double losDistance, final double earthDistance) 
274         throws NoSuchMethodException, SecurityException, IllegalAccessException, IllegalArgumentException, InvocationTargetException {
275         final Gradient[] gradient = computeDistancesBetweenLOSGradient(realPixelA, realPixelB, losDistance, earthDistance);
276         final DerivativeStructure[] ds = new DerivativeStructure[gradient.length];
277         for (int i = 0; i < gradient.length; ++i) {
278             ds[i] = gradient[i].toDerivativeStructure();
279         }
280         return ds;
281     }
282 
283     /** Compute the distances with derivatives between LOS of two real pixels (one from sensor A and one from sensor B)
284      * @param realPixelA real pixel from sensor A
285      * @param realPixelB real pixel from sensor B
286      * @return the distances of two real pixels computed between LOS and to the ground
287      * @since 2.2
288      */
289     public Gradient[] computeDistancesBetweenLOSGradient(final SensorPixel realPixelA, final SensorPixel realPixelB,
290                                                          final double losDistance, final double earthDistance) 
291         throws NoSuchMethodException, SecurityException, IllegalAccessException, IllegalArgumentException, InvocationTargetException {
292             
293         final SpacecraftToObservedBody scToBodyA = ruggedA.getScToBody();
294 
295         final AbsoluteDate realDateA = lineSensorA.getDate(realPixelA.getLineNumber());
296         final AbsoluteDate realDateB = lineSensorB.getDate(realPixelB.getLineNumber());
297         
298         final List<LineSensor> sensors = new ArrayList<LineSensor>();
299         sensors.addAll(ruggedA.getLineSensors());
300         sensors.addAll(ruggedB.getLineSensors());
301         
302         final List<Rugged> ruggedList = new ArrayList<Rugged>();
303         ruggedList.add(ruggedA);
304         ruggedList.add(ruggedB);
305         
306         // prepare generator
307         final Observables measurements = new Observables(2);
308         SensorToSensorMapping interMapping = new SensorToSensorMapping(lineSensorA.getName(), ruggedA.getName(), lineSensorB.getName(), ruggedB.getName());
309         interMapping.addMapping(realPixelA, realPixelB, losDistance, earthDistance);
310         measurements.addInterMapping(interMapping);
311         
312         
313         InterSensorsOptimizationProblemBuilder optimizationPbBuilder = new InterSensorsOptimizationProblemBuilder(sensors, measurements, ruggedList);
314         java.lang.reflect.Method createGenerator = InterSensorsOptimizationProblemBuilder.class.getSuperclass().getDeclaredMethod("createGenerator", List.class);
315         createGenerator.setAccessible(true);
316         
317         List<LineSensor> listLineSensor = new ArrayList<LineSensor>();
318         listLineSensor.addAll(ruggedA.getLineSensors());
319         listLineSensor.addAll(ruggedB.getLineSensors());
320 
321         @SuppressWarnings("unchecked")
322         DerivativeGenerator<Gradient> generator = (DerivativeGenerator<Gradient>) createGenerator.invoke(optimizationPbBuilder, listLineSensor);
323 
324         return ruggedB.distanceBetweenLOSderivatives(lineSensorA, realDateA, realPixelA.getPixelNumber(), 
325                                                      scToBodyA,
326                                                      lineSensorB, realDateB, realPixelB.getPixelNumber(),
327                                                      generator);
328         
329     }
330     
331     /** Generate noisy measurements (sensor to sensor mapping)
332      * @param lineSampling line sampling
333      * @param pixelSampling pixel sampling
334      * @param earthConstraintWeight the earth constrint weight
335      * @param earthConstraintPostponed flag to tell if the earth constraint weight is set at construction (false) or after (true) - For JUnit coverage purpose
336      */
337     public Observables generateNoisyPoints(final int lineSampling, final int pixelSampling, final double earthConstraintWeight, final boolean earthConstraintPostponed) {
338 
339         // Outliers control
340         final double outlierValue = 1.e+2;
341         
342         // Generate measurements with constraints on Earth distance and outliers control
343         
344         // Generate reference mapping, with Earth distance constraints.
345         // Weighting will be applied as follow :
346         //     (1-earthConstraintWeight) for losDistance weighting
347         //     earthConstraintWeight for earthDistance weighting
348         SensorToSensorMapping interMapping;
349         if (! earthConstraintPostponed) {
350             interMapping = new SensorToSensorMapping(lineSensorA.getName(), ruggedA.getName(), 
351                     lineSensorB.getName(), ruggedB.getName(), 
352                     earthConstraintWeight);
353         } else { // used for JUnit coverage purpose
354             interMapping = new SensorToSensorMapping(lineSensorA.getName(), ruggedA.getName(), 
355                     lineSensorB.getName(), ruggedB.getName());
356             // set the earthConstraintWeight after construction
357             interMapping.setBodyConstraintWeight(earthConstraintWeight);
358         }
359 
360         // Observables which contains sensor to sensor mapping
361         Observables observables = new Observables(2);
362         
363         // Generation noisy measurements
364         // distribution: gaussian(0), vector dimension: 2
365         final double meanA[] = { 5.0, 5.0 };
366         final double stdA[]  = { 0.1, 0.1 };
367         final double meanB[] = { 0.0, 0.0 };
368         final double stdB[]  = { 0.1, 0.1 };
369 
370         // Seed has been fixed for tests purpose
371         final GaussianRandomGenerator rngA = new GaussianRandomGenerator(new Well19937a(0xefac03d9be4d24b9l));
372         final UncorrelatedRandomVectorGenerator rvgA = new UncorrelatedRandomVectorGenerator(meanA, stdA, rngA);
373 
374         // Seed has been fixed for tests purpose
375         final GaussianRandomGenerator rngB = new GaussianRandomGenerator(new Well19937a(0xdf1c03d9be0b34b9l));
376         final UncorrelatedRandomVectorGenerator rvgB = new UncorrelatedRandomVectorGenerator(meanB, stdB, rngB);
377 
378         
379         // Search the sensor pixel seeing point
380         final int minLine = 0;
381         final int maxLine = pleiadesViewingModelB.getDimension() - 1;
382         
383         final String sensorNameB = lineSensorB.getName();
384 
385         for (double line = 0; line < pleiadesViewingModelA.getDimension(); line += lineSampling) {
386 
387             final AbsoluteDate dateA = lineSensorA.getDate(line);
388             
389             for (double pixelA = 0; pixelA < lineSensorA.getNbPixels(); pixelA += pixelSampling) {
390                 
391                 final GeodeticPoint gpA = ruggedA.directLocation(dateA, lineSensorA.getPosition(),
392                                                                  lineSensorA.getLOS(dateA, pixelA));
393                 final SensorPixel sensorPixelB = ruggedB.inverseLocation(sensorNameB, gpA, minLine, maxLine);
394 
395                 // We need to test if the sensor pixel is found in the prescribed lines
396                 // otherwise the sensor pixel is null
397                 if (sensorPixelB != null) {
398                     
399                     final AbsoluteDate dateB = lineSensorB.getDate(sensorPixelB.getLineNumber());
400                     final double pixelB = sensorPixelB.getPixelNumber();
401 
402                     // Get spacecraft to body transform of Rugged instance A
403                     final SpacecraftToObservedBody scToBodyA = ruggedA.getScToBody();
404 
405                     final GeodeticPoint gpB = ruggedB.directLocation(dateB, lineSensorB.getPosition(),
406                                                                      lineSensorB.getLOS(dateB, pixelB));
407                     final double geoDistance = computeDistanceInMeter(gpA, gpB);
408                     
409                     // Create the inter mapping if distance is below outlier value
410                     if (geoDistance < outlierValue) {
411 
412                         final double[] vecRandomA = rvgA.nextVector();
413                         final double[] vecRandomB = rvgB.nextVector();
414 
415                         final SensorPixel realPixelA = new SensorPixel(line + vecRandomA[0], pixelA + vecRandomA[1]);
416                         final SensorPixel realPixelB = new SensorPixel(sensorPixelB.getLineNumber() + vecRandomB[0],
417                                                                        sensorPixelB.getPixelNumber() + vecRandomB[1]);
418                         
419                         final AbsoluteDate realDateA = lineSensorA.getDate(realPixelA.getLineNumber());
420                         final AbsoluteDate realDateB = lineSensorB.getDate(realPixelB.getLineNumber());
421                         
422                         final double[] distanceLOSB = ruggedB.distanceBetweenLOS(lineSensorA, realDateA, realPixelA.getPixelNumber(), scToBodyA,
423                                                                                  lineSensorB, realDateB, realPixelB.getPixelNumber());
424                         final Double losDistance = 0.0;
425                         final Double earthDistance = distanceLOSB[1];
426 
427                         interMapping.addMapping(realPixelA, realPixelB, losDistance, earthDistance);
428 
429                     } // end test if geoDistance < outlierValue
430                 } // end test if sensorPixelB != null
431                 
432             } // end loop on pixel of sensorA
433         } // end loop on line of sensorA
434 
435         observables.addInterMapping(interMapping);
436         return observables;
437     }
438     
439     
440     /** Generate simple noisy measurements (sensor to sensor mapping)
441      * @param lineSampling line sampling
442      * @param pixelSampling pixel sampling
443      * @param earthConstraintWeight the earth constrint weight
444      * @param earthConstraintPostponed flag to tell if the earth constraint weight is set at construction (false) or after (true) - For JUnit coverage purpose
445      */
446     public Observables generateSimpleInterMapping(final int lineSampling, final int pixelSampling, final double earthConstraintWeight, final boolean earthConstraintPostponed) {
447 
448         // Outliers control
449         final double outlierValue = 1.e+2;
450         
451         // Generate measurements with constraints on Earth distance and outliers control
452         
453         // Generate reference mapping, with Earth distance constraints.
454         // Weighting will be applied as follow :
455         //     (1-earthConstraintWeight) for losDistance weighting
456         //     earthConstraintWeight for earthDistance weighting
457         SensorToSensorMapping interMapping;
458         if (! earthConstraintPostponed) {
459             interMapping = new SensorToSensorMapping(lineSensorA.getName(), lineSensorB.getName(), earthConstraintWeight);
460         } else { // used for JUnit coverage purpose
461             interMapping = new SensorToSensorMapping(lineSensorA.getName(),  lineSensorB.getName());
462             // set the earthConstraintWeight after construction
463             interMapping.setBodyConstraintWeight(earthConstraintWeight);
464         }
465 
466         // Observables which contains sensor to sensor mapping
467         Observables observables = new Observables(2);
468         
469         // Generation noisy measurements
470         // distribution: gaussian(0), vector dimension: 2
471         final double meanA[] = { 5.0, 5.0 };
472         final double stdA[]  = { 0.1, 0.1 };
473         final double meanB[] = { 0.0, 0.0 };
474         final double stdB[]  = { 0.1, 0.1 };
475 
476         // Seed has been fixed for tests purpose
477         final GaussianRandomGenerator rngA = new GaussianRandomGenerator(new Well19937a(0xefac03d9be4d24b9l));
478         final UncorrelatedRandomVectorGenerator rvgA = new UncorrelatedRandomVectorGenerator(meanA, stdA, rngA);
479 
480         // Seed has been fixed for tests purpose
481         final GaussianRandomGenerator rngB = new GaussianRandomGenerator(new Well19937a(0xdf1c03d9be0b34b9l));
482         final UncorrelatedRandomVectorGenerator rvgB = new UncorrelatedRandomVectorGenerator(meanB, stdB, rngB);
483 
484         
485         // Search the sensor pixel seeing point
486         final int minLine = 0;
487         final int maxLine = pleiadesViewingModelB.getDimension() - 1;
488         
489         final String sensorNameB = lineSensorB.getName();
490 
491         for (double line = 0; line < pleiadesViewingModelA.getDimension(); line += lineSampling) {
492 
493             final AbsoluteDate dateA = lineSensorA.getDate(line);
494             
495             for (double pixelA = 0; pixelA < lineSensorA.getNbPixels(); pixelA += pixelSampling) {
496                 
497                 final GeodeticPoint gpA = ruggedA.directLocation(dateA, lineSensorA.getPosition(),
498                                                                  lineSensorA.getLOS(dateA, pixelA));
499                 final SensorPixel sensorPixelB = ruggedB.inverseLocation(sensorNameB, gpA, minLine, maxLine);
500 
501                 // We need to test if the sensor pixel is found in the prescribed lines
502                 // otherwise the sensor pixel is null
503                 if (sensorPixelB != null) {
504                     
505                     final AbsoluteDate dateB = lineSensorB.getDate(sensorPixelB.getLineNumber());
506                     final double pixelB = sensorPixelB.getPixelNumber();
507 
508                     final GeodeticPoint gpB = ruggedB.directLocation(dateB, lineSensorB.getPosition(),
509                                                                      lineSensorB.getLOS(dateB, pixelB));
510                     final double geoDistance = computeDistanceInMeter(gpA, gpB);
511                     
512                     // Create the inter mapping if distance is below outlier value
513                     if (geoDistance < outlierValue) {
514 
515                         final double[] vecRandomA = rvgA.nextVector();
516                         final double[] vecRandomB = rvgB.nextVector();
517 
518                         final SensorPixel realPixelA = new SensorPixel(line + vecRandomA[0], pixelA + vecRandomA[1]);
519                         final SensorPixel realPixelB = new SensorPixel(sensorPixelB.getLineNumber() + vecRandomB[0],
520                                                                        sensorPixelB.getPixelNumber() + vecRandomB[1]);
521                         // dummy value for JUnit test purpose
522                         final Double losDistance = FastMath.abs(vecRandomA[0]);
523 
524                         interMapping.addMapping(realPixelA, realPixelB, losDistance);
525 
526                     } // end test if geoDistance < outlierValue
527                 } // end test if sensorPixelB != null
528                 
529             } // end loop on pixel of sensorA
530         } // end loop on line of sensorA
531 
532         observables.addInterMapping(interMapping);
533         return observables;
534     }
535     
536     /** Compute a geodetic distance in meters between two geodetic points.
537      * @param geoPoint1 first geodetic point (rad)
538      * @param geoPoint2 second geodetic point (rad)
539      * @return distance in meters
540      */
541     private static double computeDistanceInMeter(final GeodeticPoint geoPoint1, final GeodeticPoint geoPoint2) {
542 
543         // get vectors on unit sphere from angular coordinates
544         final Vector3D p1 = new Vector3D(geoPoint1.getLatitude(), geoPoint1.getLongitude());
545         final Vector3D p2 = new Vector3D(geoPoint2.getLatitude(), geoPoint2.getLongitude());
546         return Constants.WGS84_EARTH_EQUATORIAL_RADIUS * Vector3D.angle(p1, p2);
547     }
548 
549     /** Get the number of parameters to adjust
550      * @return number of parameters to adjust
551      */
552     public int getParameterToAdjust() {
553         return parameterToAdjust;
554     }
555     
556     @AfterEach
557     public void tearDown() {
558     }
559 }