Skip to content
Snippets Groups Projects
Commit 878e8e95 authored by Jonathan Guinet's avatar Jonathan Guinet
Browse files

localisation metrics rework

parent 18628e9b
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/tutorials/java/AffinagePleiades/DistanceTools.java 0 → 100644
+ 92
− 0
View file @ 878e8e95
package AffinagePleiades;
import org.hipparchus.util.FastMath;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
public class DistanceTools {
/**
* Earth radius in cms
*/
public static final double EARTH_RADIUS = 637100000d;
public DistanceTools() {
}
private static boolean checkDiff(double lon1, double lat1, double lon2, double lat2)
{
if(FastMath.abs(lon1 - lon2) < 1e-15 && FastMath.abs(lat1 - lat2) < 1e-15 )
return false;
return true;
}
/**
* Convert the given degrees value in meters
*
* @param distanceDeg
* @return the given degrees value in meters
*/
public static double computeDistanceDeg(double lonDeg, double latDeg, double distanceDeg) {
double lonDeg1 = lonDeg;
double latDeg1 = latDeg;
double lonDeg2 = lonDeg + distanceDeg;
double latDeg2 = latDeg;
return computeDistance(lonDeg1, latDeg1, lonDeg2, latDeg2);
}
/**
* Compute distance between point (lonDeg1, latDeg1) and point (lonDeg2,
* latDeg2) in meters
*
* @param lonDeg1
* @param latDeg1
* @param lonDeg2
* @param latDeg2
* @return distance between point (lonDeg1, latDeg1) and point (lonDeg2,
* latDeg2)
*/
public static double computeDistance(double lonDeg1, double latDeg1, double lonDeg2, double latDeg2) {
double returned;
if(!checkDiff(lonDeg1,latDeg1,lonDeg2,latDeg2) )
return 0.0;
//System.out.format("******** %n");
//System.out.format("%8.14f %8.14f %n",lonDeg1,latDeg1);
//System.out.format("%3.14e %n",lonDeg2-lonDeg1);
//System.out.format("%8.14f %8.14f %n",lonDeg2,latDeg2);
//System.out.format("%3.14e %n",latDeg2-latDeg1);
double xRad1 = FastMath.toRadians(lonDeg1);
double xRad2 = FastMath.toRadians(lonDeg2);
double yRad1 = FastMath.toRadians(latDeg1);
double yRad2 = FastMath.toRadians(latDeg2);
returned = computeDistanceRad(xRad1, yRad1, xRad2, yRad2);
return returned;
}
/**
* distance in meters between point (xRad1, yRad1) and point (xRad2, yRad2)
*
* @param xRad1
* @param xRad2
* @param yRad1
* @param yRad2
* @return
*/
public static double computeDistanceRad(double xRad1, double yRad1, double xRad2, double yRad2) {
// get vectors on unit sphere from angular coordinates
Vector3D p1 = new Vector3D(yRad1, xRad1); //
Vector3D p2 = new Vector3D(yRad2, xRad2);
return EARTH_RADIUS /100* Vector3D.angle(p1, p2);
}
}
src/tutorials/java/AffinagePleiades/LocalisationMetrics.java
+ 108
− 160
View file @ 878e8e95
......@@ -16,8 +16,6 @@
*/
package AffinagePleiades;
import org.hipparchus.linear.ArrayRealVector;
import org.hipparchus.linear.RealVector;
......@@ -27,7 +25,6 @@ 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.s2geolib.utils.S2GeolibConstants;
import java.util.Collections;
import java.util.Collection;
......@@ -39,166 +36,117 @@ import java.util.Locale;
import org.hipparchus.util.FastMath;
import org.orekit.bodies.GeodeticPoint;
/** class for measure generation
/**
* class for measure generation
*
* @author Jonathan Guinet
*/
public class LocalisationMetrics {
/** mapping */
private Set<Map.Entry<SensorPixel, GeodeticPoint>> groundTruthMappings;
/** 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;
private final double EARTH_RADIUS = 637100000d;
private boolean computeInDeg;
/* max residual distance */
private double resMax;
/* mean residual distance */
private double resMean;
/** Simple constructor.
* <p>
*
* </p>
*/
public LocalisationMetrics(SensorToGroundMapping groundTruthMapping, Rugged rugged, boolean computeInDeg) throws RuggedException
{
groundTruthMappings = groundTruthMapping.getMappings();
this.rugged = rugged;
this.sensor = rugged.getLineSensor(groundTruthMapping.getSensorName());
this.computeInDeg = computeInDeg;
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;
}
/**
* Compute distance between point (lonDeg1, latDeg1) and point (lonDeg2, latDeg2) in meters
* @param lonDeg1
* @param latDeg1
* @param lonDeg2
* @param latDeg2
* @return distance between point (lonDeg1, latDeg1) and point (lonDeg2, latDeg2)
*/
private double computeDistance(double lonDeg1, double latDeg1, double lonDeg2, double latDeg2) {
double returned;
double xRad1 = FastMath.toRadians(lonDeg1);
double xRad2 = FastMath.toRadians(lonDeg2);
double yRad1 = FastMath.toRadians(latDeg1);
double yRad2 = FastMath.toRadians(latDeg2);
returned = computeDistanceRad(xRad1, yRad1, xRad2, yRad2);
return returned;
}
/**
* distance in meters between point (xRad1, yRad1) and point (xRad2, yRad2)
* @param xRad1
* @param xRad2
* @param yRad1
* @param yRad2
* @return
*/
private double computeDistanceRad(double xRad1, double yRad1, double xRad2, double yRad2) {
double returned;
double sinValue = FastMath.sin(xRad1) * FastMath.sin(xRad2);
double deltaLambda = FastMath.abs(yRad1 - yRad2);
double cosValue = FastMath.cos(xRad1) * FastMath.cos(xRad2) * FastMath.cos(deltaLambda);
double deltaPhy = FastMath.acos(sinValue + cosValue);
if (Double.isNaN(deltaPhy)) {
deltaPhy = 0d;
}
returned = EARTH_RADIUS / 100 * deltaPhy;
return returned;
}
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 distance = 0;
if(this.computeInDeg == true)
{
double lonDiff = esGP.getLongitude() - gtGP.getLongitude();
double latDiff = esGP.getLatitude() - gtGP.getLatitude();
double altDiff = esGP.getAltitude() - gtGP.getAltitude();
distance = Math.sqrt( lonDiff * lonDiff + latDiff * latDiff + altDiff * altDiff );
}
else
distance = computeDistance(esGP.getLongitude(), esGP.getLatitude(),gtGP.getLongitude() , gtGP.getLatitude());
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);
}
}
private Set<Map.Entry<SensorPixel, GeodeticPoint>> estimationMappings;
private Rugged rugged;
private LineSensor sensor;
private PleiadesViewingModel viewingModel;
private int measureCount;
private boolean computeInDeg;
/* max residual distance */
private double resMax;
/* mean residual distance */
private double resMean;
/**
* Simple constructor.
* <p>
*
* </p>
*/
public LocalisationMetrics(SensorToGroundMapping groundTruthMapping, Rugged rugged, boolean computeInDeg)
throws RuggedException {
groundTruthMappings = groundTruthMapping.getMappings();
this.rugged = rugged;
this.sensor = rugged.getLineSensor(groundTruthMapping.getSensorName());
this.computeInDeg = computeInDeg;
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;
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 distance = 0;
if (this.computeInDeg == true) {
double lonDiff = esGP.getLongitude() - gtGP.getLongitude();
double latDiff = esGP.getLatitude() - gtGP.getLatitude();
double altDiff = esGP.getAltitude() - gtGP.getAltitude();
distance = Math.sqrt(lonDiff * lonDiff + latDiff * latDiff);
} else {
distance = DistanceTools.computeDistanceRad(esGP.getLongitude(), esGP.getLatitude(),
gtGP.getLongitude(), gtGP.getLatitude());
}
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);
}
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment