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