diff --git a/src/main/java/org/orekit/rugged/core/Sensor.java b/src/main/java/org/orekit/rugged/core/Sensor.java
index e1364c1570a8da3737a1c59f74de85d03a0532fc..72f7f122dc60cdcfe76a178375a70313c03006bf 100644
--- a/src/main/java/org/orekit/rugged/core/Sensor.java
+++ b/src/main/java/org/orekit/rugged/core/Sensor.java
@@ -19,9 +19,14 @@ package org.orekit.rugged.core;
import java.util.List;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
+import org.apache.commons.math3.linear.MatrixUtils;
+import org.apache.commons.math3.linear.RealMatrix;
+import org.apache.commons.math3.linear.SingularValueDecomposition;
import org.orekit.rugged.api.LineDatation;
import org.orekit.time.AbsoluteDate;
+import com.sun.org.apache.bcel.internal.generic.GETSTATIC;
+
/** Container for sensor data.
* <p>
* Instances of this class are guaranteed to be immutable.
@@ -45,6 +50,12 @@ public class Sensor {
/** Datation model. */
private final LineDatation datationModel;
+ /** Mean plane normal. */
+ private final Vector3D normal;
+
+ /** Mean plane reference point. */
+ private final Vector3D referencePoint;
+
/** Simple constructor.
* @param name name of the sensor
* @param referenceDate reference date
@@ -55,11 +66,51 @@ public class Sensor {
public Sensor(final String name,
final AbsoluteDate referenceDate, final LineDatation datationModel,
final List<Vector3D> positions, final List<Vector3D> los) {
+
this.name = name;
this.referenceDate = referenceDate;
this.positions = positions;
this.los = los;
this.datationModel = datationModel;
+
+ // we consider the viewing directions as a point cloud
+ // and want to find the plane that best fits it
+
+ // start by finding the centroid
+ // (which will also be our plane reference point)
+ double centroidX = 0;
+ double centroidY = 0;
+ double centroidZ = 0;
+ for (int i = 0; i < los.size(); ++i) {
+ final Vector3D p = positions.get(i);
+ final Vector3D l = los.get(i);
+ centroidX += p.getX() + l.getX();
+ centroidY += p.getY() + l.getY();
+ centroidZ += p.getZ() + l.getZ();
+ }
+ centroidX /= los.size();
+ centroidY /= los.size();
+ centroidZ /= los.size();
+ referencePoint = new Vector3D(centroidX, centroidY, centroidZ);
+
+ // build a centered data matrix
+ RealMatrix matrix = MatrixUtils.createRealMatrix(3, los.size());
+ for (int i = 0; i < los.size(); ++i) {
+ final Vector3D p = positions.get(i);
+ final Vector3D l = los.get(i);
+ matrix.setEntry(0, i, p.getX() + l.getX() - centroidX);
+ matrix.setEntry(1, i, p.getY() + l.getY() - centroidY);
+ matrix.setEntry(2, i, p.getZ() + l.getZ() - centroidZ);
+ }
+
+ // compute Singular Value Decomposition
+ final SingularValueDecomposition svd = new SingularValueDecomposition(matrix);
+
+ // extract the left singular vector corresponding to least singular value
+ // (i.e. last vector since Apache Commons Math returns the values
+ // in non-increasing order)
+ normal = new Vector3D(svd.getU().getColumn(2)).normalize();
+
}
/** Get the name of the sensor.
@@ -108,4 +159,18 @@ public class Sensor {
return datationModel.getLine(date.durationFrom(referenceDate));
}
+ /** Get the mean plane normal.
+ * @return mean plane normal
+ */
+ public Vector3D getMeanPlaneNormal() {
+ return normal;
+ }
+
+ /** Get the mean plane reference point.
+ * @return mean plane reference point
+ */
+ public Vector3D getMeanPlaneReferencePoint() {
+ return referencePoint;
+ }
+
}
diff --git a/src/test/java/org/orekit/rugged/core/SensorTest.java b/src/test/java/org/orekit/rugged/core/SensorTest.java
new file mode 100644
index 0000000000000000000000000000000000000000..9a5c3eae4072a921cfb88e71898102a8870439f3
--- /dev/null
+++ b/src/test/java/org/orekit/rugged/core/SensorTest.java
@@ -0,0 +1,93 @@
+/* Copyright 2013-2014 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 org.orekit.rugged.core;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
+import org.apache.commons.math3.random.RandomGenerator;
+import org.apache.commons.math3.random.Well19937a;
+import org.apache.commons.math3.util.FastMath;
+import org.junit.Assert;
+import org.junit.Test;
+import org.orekit.rugged.api.LinearLineDatation;
+import org.orekit.time.AbsoluteDate;
+
+public class SensorTest {
+
+ @Test
+ public void testPerfectLine() {
+
+ final Vector3D center = new Vector3D(1.5, Vector3D.PLUS_I);
+ final Vector3D normal = Vector3D.PLUS_I;
+ final Vector3D fovCenter = Vector3D.PLUS_K;
+ final Vector3D cross = Vector3D.crossProduct(normal, fovCenter);
+
+ // build lists of pixels regularly spread on a perfect plane
+ final List<Vector3D> positions = new ArrayList<Vector3D>();
+ final List<Vector3D> los = new ArrayList<Vector3D>();
+ for (int i = -1000; i <= 1000; ++i) {
+ positions.add(center);
+ final double alpha = i * 0.17 / 1000;
+ los.add(new Vector3D(FastMath.cos(alpha), fovCenter, FastMath.sin(alpha), cross));
+ }
+
+ final Sensor sensor = new Sensor("perfect line", AbsoluteDate.J2000_EPOCH,
+ new LinearLineDatation(0.0, 1.0 / 1.5e-3), positions, los);
+
+ Assert.assertEquals("perfect line", sensor.getName());
+ Assert.assertEquals(AbsoluteDate.J2000_EPOCH, sensor.getDate(0.0));
+ Assert.assertEquals(0.0, Vector3D.dotProduct(normal, center.subtract(sensor.getMeanPlaneReferencePoint())), 1.0e-15);
+ Assert.assertEquals(0.0, FastMath.sin(Vector3D.angle(normal, sensor.getMeanPlaneNormal())), 1.0e-15);
+
+ }
+
+ @Test
+ public void testNoisyLine() {
+
+ final RandomGenerator random = new Well19937a(0xf3ddb33785e12bdal);
+ final Vector3D center = new Vector3D(1.5, Vector3D.PLUS_I);
+ final Vector3D normal = Vector3D.PLUS_I;
+ final Vector3D fovCenter = Vector3D.PLUS_K;
+ final Vector3D cross = Vector3D.crossProduct(normal, fovCenter);
+
+ // build lists of pixels regularly spread on a perfect plane
+ final List<Vector3D> positions = new ArrayList<Vector3D>();
+ final List<Vector3D> los = new ArrayList<Vector3D>();
+ for (int i = -1000; i <= 1000; ++i) {
+ positions.add(center);
+ final double alpha = i * 0.17 / 10 + 1.0e-5 * random.nextDouble();
+ final double delta = 1.0e-5 * random.nextDouble();
+ final double cA = FastMath.cos(alpha);
+ final double sA = FastMath.sin(alpha);
+ final double cD = FastMath.cos(delta);
+ final double sD = FastMath.sin(delta);
+ los.add(new Vector3D(cA * cD, fovCenter, sA * cD, cross, sD, normal));
+ }
+
+ final Sensor sensor = new Sensor("noisy line", AbsoluteDate.J2000_EPOCH,
+ new LinearLineDatation(0.0, 1.0 / 1.5e-3), positions, los);
+
+ Assert.assertEquals("noisy line", sensor.getName());
+ Assert.assertEquals(AbsoluteDate.J2000_EPOCH, sensor.getDate(0.0));
+ Assert.assertEquals(0.0, Vector3D.dotProduct(normal, center.subtract(sensor.getMeanPlaneReferencePoint())), 1.0e-5);
+ Assert.assertEquals(0.0, FastMath.sin(Vector3D.angle(normal, sensor.getMeanPlaneNormal())), 3.0e-7);
+
+ }
+
+}