Added automatic mean plane computation for line sensors.

This will allow implementing inverse localization.

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;
+    }
+
 }

src/test/java/org/orekit/rugged/core/SensorTest.java

+/* 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);
+
+    }
+
+}