Add noisy measures

src/tutorials/java/AffinagePleiades/AffinageRugged.java

@@ -171,7 +171,8 @@ public class AffinageRugged {
             MeasureGenerator measure = new MeasureGenerator(pleiadesViewingModel,rugged);
             int lineSampling = 1000;
             int pixelSampling = 1000;		
-            measure.CreateMeasure(lineSampling, pixelSampling);
+            //measure.CreateMeasure(lineSampling, pixelSampling); 
+            measure.CreateNoisyMeasure(lineSampling, pixelSampling); // Test with noisy measures
             System.out.format("nb TiePoints %d %n", measure.getMeasureCount());
 
             System.out.format(" **** Reset Roll/Pitch/Factor **** %n");

src/tutorials/java/AffinagePleiades/MeasureGenerator.java

@@ -23,6 +23,11 @@ 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.geometry.euclidean.threed.Vector3D;
+import org.hipparchus.random.UncorrelatedRandomVectorGenerator;
+import org.hipparchus.random.UniformRandomGenerator;
+import org.hipparchus.random.Well19937a;
+import org.hipparchus.util.FastMath;
 import org.orekit.bodies.GeodeticPoint;
 
 /** class for measure generation
@@ -72,19 +77,73 @@ public class MeasureGenerator {
     
     public void CreateMeasure(final int lineSampling,final int pixelSampling)  throws RuggedException
     {
-    
-    for (double line = 0; line < viewingModel.dimension; line += lineSampling) {
+    	for (double line = 0; line < viewingModel.dimension; line += lineSampling) {
+        	
+        	AbsoluteDate date = sensor.getDate(line);
+        	for (int pixel = 0; pixel < sensor.getNbPixels(); pixel += pixelSampling) {
+
+        		GeodeticPoint gp2 = rugged.directLocation(date, sensor.getPosition(),
+                                                      sensor.getLOS(date, pixel));
+            
+        		mapping.addMapping(new SensorPixel(line, pixel), gp2);
+        		measureCount++;
+        	}
+        }
+    }
+    public void CreateNoisyMeasure(final int lineSampling,final int pixelSampling)  throws RuggedException
+    {
+    	Vector3D latLongError = estimateLatLongError();
     	
-        AbsoluteDate date = sensor.getDate(line);
-        for (int pixel = 0; pixel < sensor.getNbPixels(); pixel += pixelSampling) {
-            GeodeticPoint gp2 = rugged.directLocation(date, sensor.getPosition(),
+    	double latError = FastMath.toRadians(latLongError.getX()); // in line: -0.000002 deg
+    	double lonError = FastMath.toRadians(latLongError.getY()); // in line: 0.000012 deg
+    	System.out.format("Corresponding error estimation on ground: {lat: %1.10f deg, lon: %1.10f deg} %n", latLongError.getX(), latLongError.getY());
+    	
+    	// Gaussian random generator
+    	// Build a null mean random uncorrelated vector generator with standard deviation corresponding to the estimated error on ground
+    	double mean[] = {0.0,0.0,0.0};
+    	double std[] = {latError,lonError,0.0};
+    	UniformRandomGenerator rng = new UniformRandomGenerator(new Well19937a(0xefac03d9be4d24b9l));
+    	UncorrelatedRandomVectorGenerator rvg = new UncorrelatedRandomVectorGenerator(mean, std, rng);
+        
+    	System.out.format("Add a gaussian noise to measures without biais (null mean) and standard deviation corresponding to the estimated error on ground.%n");
+    	for (double line = 0; line < viewingModel.dimension; line += lineSampling) {
+        	
+        	AbsoluteDate date = sensor.getDate(line);
+        	for (int pixel = 0; pixel < sensor.getNbPixels(); pixel += pixelSampling) {
+
+        		// Components of generated vector follow (independent) Gaussian distribution
+            	Vector3D vecRandom = new Vector3D(rvg.nextVector());
+            	
+        		GeodeticPoint gp2 = rugged.directLocation(date, sensor.getPosition(),
                                                       sensor.getLOS(date, pixel));
-            mapping.addMapping(new SensorPixel(line, pixel), gp2);
-            measureCount++;
+            
+        		GeodeticPoint gpNoisy = new GeodeticPoint(gp2.getLatitude()+vecRandom.getX(), 
+                        gp2.getLongitude()+vecRandom.getY(),
+                        gp2.getAltitude()); // no altitude error introducing
+
+        		/*if(line == 0) {
+        			System.out.format("Init  gp: (%f,%d): %s %n",line,pixel,gp2.toString());
+        			System.out.format("Random:   (%f,%d): %s %n",line,pixel,vecRandom.toString());
+        			System.out.format("Final gp: (%f,%d): %s %n",line,pixel,gpNoisy.toString());
+        		}*/
+   
+        		mapping.addMapping(new SensorPixel(line, pixel), gpNoisy);
+        		measureCount++;
+        	}
         }
     }
+    private Vector3D estimateLatLongError() throws RuggedException {
+    
+    	System.out.format("Uncertainty in pixel (in line) for a real geometric refining: 1 pixel (assumption)%n");
+    	double line=0;
+    	AbsoluteDate date = sensor.getDate(line);
+    	GeodeticPoint gp_pix0 = rugged.directLocation(date, sensor.getPosition(), sensor.getLOS(date, 0));
+    	GeodeticPoint gp_pix1 = rugged.directLocation(date, sensor.getPosition(), sensor.getLOS(date, 1));
+    	double latErr=FastMath.toDegrees(gp_pix0.getLatitude()-gp_pix1.getLatitude());
+		double lonErr=FastMath.toDegrees(gp_pix0.getLongitude()-gp_pix1.getLongitude());
+    	
+    	return new Vector3D(latErr,lonErr,0.0);
     }
-	
 
     
 }