add residual computation

src/tutorials/java/AffinagePleiades/AffinageRugged.java

@@ -19,6 +19,7 @@ package AffinagePleiades;
 import org.hipparchus.geometry.euclidean.threed.Vector3D;
 import org.hipparchus.util.FastMath;
 import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer.Optimum;
+import org.hipparchus.linear.RealVector;
 
 import java.io.File;
 import java.util.Locale;
@@ -49,6 +50,8 @@ import org.orekit.utils.AngularDerivativesFilter;
 import org.orekit.utils.CartesianDerivativesFilter;
 import org.orekit.utils.PVCoordinates;
 
+
+
 /**
  * Parameter estimation context 
  * @author Jonathan Guinet
@@ -160,8 +163,11 @@ public class AffinageRugged {
             System.out.format(" **** Generate Measures **** %n");
             
             MeasureGenerator measure = new MeasureGenerator(pleiadesViewingModel,rugged);
-            measure.CreateMeasure(1000, 1000);
+            int lineSampling = 1000;
+            int pixelSampling = 1000;		
+            measure.CreateMeasure(lineSampling, pixelSampling);
 
+            System.out.format("nb TiePoints %d %n", measure.getMeasureCount());
             System.out.format(" **** Reset Roll/Pitch **** %n");
             rugged.
             getLineSensor(pleiadesViewingModel.getSensorName()).
@@ -177,11 +183,19 @@ public class AffinageRugged {
             });
             
             
+            System.out.format(" **** Initial Residuals  **** %n");
+
+
+            
+            LocalisationMetrics initlialResiduals = new LocalisationMetrics(measure.getMapping(),rugged);
+            System.out.format("residuals max :  %3.6e mean %3.6e  %n",initlialResiduals.getMaxResidual(),initlialResiduals.getMeanResidual());
+             
+           
             
             System.out.format(" **** Start optimization  **** %n");
             // perform parameters estimation
             int maxIterations = 15;
-            double convergenceThreshold =  1.0e-9;
+            double convergenceThreshold =  1e-12;
             
             System.out.format("iterations max %d convergence threshold  %3.6e \n",maxIterations, convergenceThreshold);
 
@@ -189,7 +203,7 @@ public class AffinageRugged {
             Optimum optimum = rugged.estimateFreeParameters(Collections.singletonList(measure.getMapping()), maxIterations,convergenceThreshold);
             
    
-            System.out.format(" Optimization performed in %d interation \n",optimum.getEvaluations());
+            System.out.format(" Optimization performed in %d iterations \n",optimum.getEvaluations());
 
             // check estimated values
             double estimatedRoll = rugged.getLineSensor(pleiadesViewingModel.getSensorName()).
@@ -206,6 +220,19 @@ public class AffinageRugged {
             double pitchError = (estimatedPitch - pitchValue);
             System.out.format("Estimated pitch %3.5f pitch  error %3.6e  %n ", estimatedPitch, pitchError);
             
+            System.out.format(" **** Estimated Measures **** %n");
+            
+            MeasureGenerator estimatedMeasure = new MeasureGenerator(pleiadesViewingModel,rugged);
+            estimatedMeasure.CreateMeasure(lineSampling, pixelSampling);
+
+            System.out.format(" **** Compute Statistics **** %n");
+
+            LocalisationMetrics localisationResiduals = new LocalisationMetrics(measure.getMapping(),rugged);
+            System.out.format("residuals max :  %3.6e mean %3.6e  %n",localisationResiduals.getMaxResidual(),localisationResiduals.getMeanResidual());
+            //RealVector residuals =  optimum.getResiduals();
+
+
+            
             
 
         } catch (OrekitException oe) {

src/tutorials/java/AffinagePleiades/LocalisationMetrics.java

+/* 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.linear.ArrayRealVector;
+import org.hipparchus.linear.RealVector;
+
+import org.orekit.rugged.api.SensorToGroundMapping;
+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 java.util.Collections;
+import java.util.Collection;
+import java.util.Map;
+import java.util.Set;
+import java.util.Iterator;
+import java.util.Locale;
+
+import org.hipparchus.util.FastMath;
+import org.orekit.bodies.GeodeticPoint;
+
+/** class for measure generation
+ * @author Jonathan Guinet
+ */
+public class LocalisationMetrics {
+
+
+    /** mapping */
+    private Set <Map.Entry<SensorPixel, GeodeticPoint>> groundTruthMappings;
+
+    private Set <Map.Entry<SensorPixel, GeodeticPoint>> estimationMappings; 
+    
+    private Rugged rugged;
+    
+    private LineSensor sensor;
+
+    private PleiadesViewingModel viewingModel;
+    
+    private int measureCount;
+    
+    
+    /* max residual distance */
+    private double resMax;
+    
+    
+    /* mean residual distance */
+    private double resMean;
+    
+    
+    /** Simple constructor.
+     * <p>
+     *
+     * </p>
+     */
+    public LocalisationMetrics(SensorToGroundMapping groundTruthMapping, Rugged rugged) throws RuggedException
+    {
+	
+
+    	groundTruthMappings =  groundTruthMapping.getMappings();
+    	this.rugged = rugged;
+    	this.sensor = rugged.getLineSensor(groundTruthMapping.getSensorName());
+    	this.computeMetrics();
+    	
+    }
+    
+    
+    /** Get the maximum residual;
+     * @return max residual
+     */
+    public double getMaxResidual() {
+        return resMax;
+    }
+    
+
+    
+    /** Get the mean residual;
+     * @return mean residual
+     */
+    public double getMeanResidual() {
+        return resMean;
+    }
+    
+    
+    
+    public void computeMetrics()  throws RuggedException {
+    	    	    
+            //final RealVector longDiffVector;
+            //final RealVector latDiffVector;
+            //final RealVector altDiffVector;
+            RealVector distanceVector = new ArrayRealVector();
+            double count=0;
+            resMax = 0; 
+    	  	int k = groundTruthMappings.size();
+    	  	
+    	  	Iterator<Map.Entry<SensorPixel, GeodeticPoint>> gtIt = groundTruthMappings.iterator();
+    	  	
+    	  	
+    	  	while (gtIt.hasNext()) {
+    	  		Map.Entry<SensorPixel, GeodeticPoint> gtMapping =gtIt.next();
+    	  		
+    	  		
+                final SensorPixel gtSP = gtMapping.getKey();
+                final GeodeticPoint gtGP = gtMapping.getValue();
+                
+                AbsoluteDate date = sensor.getDate(gtSP.getLineNumber());
+                
+                GeodeticPoint esGP = rugged.directLocation(date, sensor.getPosition(),
+                        sensor.getLOS(date, (int) gtSP.getPixelNumber()));
+                   
+                
+                double lonDiff = esGP.getLongitude() - gtGP.getLongitude();
+                double latDiff = esGP.getLatitude() - gtGP.getLatitude();		
+                double altDiff = esGP.getAltitude() - gtGP.getAltitude();
+                //longDiffVector.append(lonDiff);
+                //latDiffVector.append(latDiff);
+                //altDiffVector.append(altDiff);
+    	  	    double distance = Math.sqrt( lonDiff * lonDiff + latDiff * latDiff + altDiff * altDiff );
+    	  	    count += distance;
+    	  	    if (distance > resMax)
+    	  	    {
+    	  	    		resMax = distance;
+    	  	    }
+                //distanceVector.append(distance);
+    	  		
+    	  	}
+    	  	
+            //resMax =  distanceVector.getMaxValue();
+            //System.out.format(Locale.US, "max: %3.6e %n ",distanceVector.getMaxValue() );
+          
+            resMean = count / k ;
+            //System.out.format("number of points %d  %n", k);
+        }
+    	
+  	
+
+    
+}
+