diff --git a/core/src/main/java/org/orekit/rugged/api/LineSensor.java b/core/src/main/java/org/orekit/rugged/api/LineSensor.java index 2d7a6f7326ebe88e2da7ed0fce39190eef5c8bc0..f0bfd9a8cb919a6d7117a330ae4e90b95a658e99 100644 --- a/core/src/main/java/org/orekit/rugged/api/LineSensor.java +++ b/core/src/main/java/org/orekit/rugged/api/LineSensor.java @@ -19,9 +19,6 @@ package org.orekit.rugged.api; 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.time.AbsoluteDate; /** Line sensor model. @@ -44,9 +41,6 @@ public class LineSensor { /** Pixels lines-of-sight. */ private final Vector3D[] x; - /** Mean plane normal. */ - private final Vector3D normal; - /** Simple constructor. * @param name name of the sensor * @param position sensor position @@ -66,38 +60,6 @@ public class LineSensor { x[i] = los.get(i).normalize(); } - // we consider the viewing directions as a point cloud - // and want to find the plane containing origin that best fits it - - // build a centered data matrix - // (for each viewing direction, we add both the direction and its - // opposite, thus ensuring the plane will contain origin) - final RealMatrix matrix = MatrixUtils.createRealMatrix(3, 2 * los.size()); - for (int i = 0; i < x.length; ++i) { - final Vector3D l = x[i]; - matrix.setEntry(0, 2 * i, l.getX()); - matrix.setEntry(1, 2 * i, l.getY()); - matrix.setEntry(2, 2 * i, l.getZ()); - matrix.setEntry(0, 2 * i + 1, -l.getX()); - matrix.setEntry(1, 2 * i + 1, -l.getY()); - matrix.setEntry(2, 2 * i + 1, -l.getZ()); - } - - // 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) - final Vector3D singularVector = new Vector3D(svd.getU().getColumn(2)).normalize(); - - // check rotation order - if (Vector3D.dotProduct(singularVector, Vector3D.crossProduct(los.get(0), los.get(los.size() - 1))) >= 0) { - normal = singularVector; - } else { - normal = singularVector.negate(); - } - } /** Get the name of the sensor. @@ -146,18 +108,6 @@ public class LineSensor { return datationModel.getRate(lineNumber); } - /** Get the mean plane normal. - * <p> - * The normal is oriented such traversing pixels in increasing indices - * order corresponds is consistent with trigonometric order (i.e. - * counterclockwise). - * </p> - * @return mean plane normal - */ - public Vector3D getMeanPlaneNormal() { - return normal; - } - /** Get the sensor position. * @return position */ diff --git a/core/src/main/java/org/orekit/rugged/api/Rugged.java b/core/src/main/java/org/orekit/rugged/api/Rugged.java index 674b0dd30d887193e4349c72bbed68e960bd8a1d..d37d8bd482e7db93b2b3a7f0e99eb9ba22caee3c 100644 --- a/core/src/main/java/org/orekit/rugged/api/Rugged.java +++ b/core/src/main/java/org/orekit/rugged/api/Rugged.java @@ -1002,7 +1002,8 @@ public class Rugged { // find approximately the pixel along this sensor line final SensorPixelCrossing pixelCrossing = - new SensorPixelCrossing(sensor, crossingResult.getTargetDirection().toVector3D(), + new SensorPixelCrossing(sensor, planeCrossing.getMeanPlaneNormal(), + crossingResult.getTargetDirection().toVector3D(), MAX_EVAL, COARSE_INVERSE_LOCALIZATION_ACCURACY); final double coarsePixel = pixelCrossing.locatePixel(); if (Double.isNaN(coarsePixel)) { diff --git a/core/src/main/java/org/orekit/rugged/api/SensorMeanPlaneCrossing.java b/core/src/main/java/org/orekit/rugged/api/SensorMeanPlaneCrossing.java index 915fea065fb516dbcf9441f4690fdc21d6aebd6c..21a92acddcfaa885911a78d6261a208cba6c812e 100644 --- a/core/src/main/java/org/orekit/rugged/api/SensorMeanPlaneCrossing.java +++ b/core/src/main/java/org/orekit/rugged/api/SensorMeanPlaneCrossing.java @@ -19,6 +19,9 @@ package org.orekit.rugged.api; import org.apache.commons.math3.analysis.differentiation.DerivativeStructure; import org.apache.commons.math3.geometry.euclidean.threed.FieldVector3D; 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.apache.commons.math3.util.FastMath; import org.orekit.frames.Transform; import org.orekit.rugged.utils.SpacecraftToObservedBody; @@ -61,6 +64,9 @@ class SensorMeanPlaneCrossing { /** Line sensor. */ private final LineSensor sensor; + /** Sensor mean plane normal. */ + private final Vector3D meanPlaneNormal; + /** Maximum number of evaluations. */ private final int maxEval; @@ -100,6 +106,51 @@ class SensorMeanPlaneCrossing { this.midBodyToInert = scToBody.getBodyToInertial(midDate); this.midScToInert = scToBody.getScToInertial(midDate); + this.meanPlaneNormal = computeMeanPlaneNormal(); + + } + + /** Compute the plane containing origin that best fits viewing directions point cloud. + * <p> + * The normal is oriented such traversing pixels in increasing indices + * order corresponds is consistent with trigonometric order (i.e. + * counterclockwise). + * </p> + * @return normal of the mean plane + */ + private Vector3D computeMeanPlaneNormal() { + + // build a centered data matrix + // (for each viewing direction, we add both the direction and its + // opposite, thus ensuring the plane will contain origin) + final RealMatrix matrix = MatrixUtils.createRealMatrix(3, 2 * sensor.getNbPixels()); + for (int i = 0; i < sensor.getNbPixels(); ++i) { + final Vector3D l = sensor.getLos(i); + matrix.setEntry(0, 2 * i, l.getX()); + matrix.setEntry(1, 2 * i, l.getY()); + matrix.setEntry(2, 2 * i, l.getZ()); + matrix.setEntry(0, 2 * i + 1, -l.getX()); + matrix.setEntry(1, 2 * i + 1, -l.getY()); + matrix.setEntry(2, 2 * i + 1, -l.getZ()); + } + + // 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) + final Vector3D singularVector = new Vector3D(svd.getU().getColumn(2)).normalize(); + + // check rotation order + final Vector3D first = sensor.getLos(0); + final Vector3D last = sensor.getLos(sensor.getNbPixels() - 1); + if (Vector3D.dotProduct(singularVector, Vector3D.crossProduct(first, last)) >= 0) { + return singularVector; + } else { + return singularVector.negate(); + } + } /** Get the minimum line number in the search interval. @@ -116,6 +167,18 @@ class SensorMeanPlaneCrossing { return maxLine; } + /** Get the mean plane normal. + * <p> + * The normal is oriented such traversing pixels in increasing indices + * order corresponds is consistent with trigonometric order (i.e. + * counterclockwise). + * </p> + * @return mean plane normal + */ + public Vector3D getMeanPlaneNormal() { + return meanPlaneNormal; + } + /** Container for mean plane crossing result. */ public static class CrossingResult { @@ -178,7 +241,7 @@ class SensorMeanPlaneCrossing { final FieldVector3D<DerivativeStructure> targetDirection = evaluateLine(crossingLine, targetPV, bodyToInert, scToInert); - final DerivativeStructure beta = FieldVector3D.angle(targetDirection, sensor.getMeanPlaneNormal()); + final DerivativeStructure beta = FieldVector3D.angle(targetDirection, meanPlaneNormal); final double deltaL = (0.5 * FastMath.PI - beta.getValue()) / beta.getPartialDerivative(1); if (FastMath.abs(deltaL) <= accuracy) { diff --git a/core/src/main/java/org/orekit/rugged/api/SensorPixelCrossing.java b/core/src/main/java/org/orekit/rugged/api/SensorPixelCrossing.java index 13df8699a02b11af11b4714e25b2f3e913532e00..6cd31e53edb56dc3e90b3bba6b0b7cda3eedd76e 100644 --- a/core/src/main/java/org/orekit/rugged/api/SensorPixelCrossing.java +++ b/core/src/main/java/org/orekit/rugged/api/SensorPixelCrossing.java @@ -50,14 +50,16 @@ class SensorPixelCrossing { /** Simple constructor. * @param sensor sensor to consider + * @param meanNormal mean plane normal of the line sensor * @param targetDirection target direction in spacecraft frame * @param maxEval maximum number of evaluations * @param accuracy accuracy to use for finding crossing line number */ - public SensorPixelCrossing(final LineSensor sensor, final Vector3D targetDirection, + public SensorPixelCrossing(final LineSensor sensor, final Vector3D meanNormal, + final Vector3D targetDirection, final int maxEval, final double accuracy) { this.sensor = sensor; - this.cross = Vector3D.crossProduct(sensor.getMeanPlaneNormal(), targetDirection).normalize(); + this.cross = Vector3D.crossProduct(meanNormal, targetDirection).normalize(); this.maxEval = maxEval; this.accuracy = accuracy; } diff --git a/core/src/test/java/org/orekit/rugged/api/LineSensorTest.java b/core/src/test/java/org/orekit/rugged/api/LineSensorTest.java deleted file mode 100644 index a86aa2c616f75a426c3598116ce25bc553deb42c..0000000000000000000000000000000000000000 --- a/core/src/test/java/org/orekit/rugged/api/LineSensorTest.java +++ /dev/null @@ -1,92 +0,0 @@ -/* 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.api; - -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.rugged.api.LineSensor; -import org.orekit.time.AbsoluteDate; - -public class LineSensorTest { - - @Test - public void testPerfectLine() { - - final Vector3D position = 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> los = new ArrayList<Vector3D>(); - for (int i = -1000; i <= 1000; ++i) { - final double alpha = i * 0.17 / 1000; - los.add(new Vector3D(FastMath.cos(alpha), fovCenter, FastMath.sin(alpha), cross)); - } - - final LineSensor sensor = new LineSensor("perfect line", - new LinearLineDatation(AbsoluteDate.J2000_EPOCH, 0.0, 1.0 / 1.5e-3), - position, los); - - Assert.assertEquals("perfect line", sensor.getName()); - Assert.assertEquals(AbsoluteDate.J2000_EPOCH, sensor.getDate(0.0)); - Assert.assertEquals(0.0, Vector3D.distance(position, sensor.getPosition()), 1.0e-15); - Assert.assertEquals(0.0, Vector3D.angle(normal, sensor.getMeanPlaneNormal()), 1.0e-15); - - } - - @Test - public void testNoisyLine() { - - final RandomGenerator random = new Well19937a(0xf3ddb33785e12bdal); - final Vector3D position = 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> los = new ArrayList<Vector3D>(); - for (int i = -1000; i <= 1000; ++i) { - 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 LineSensor sensor = new LineSensor("noisy line", - new LinearLineDatation(AbsoluteDate.J2000_EPOCH, 0.0, 1.0 / 1.5e-3), - position, los); - - Assert.assertEquals("noisy line", sensor.getName()); - Assert.assertEquals(AbsoluteDate.J2000_EPOCH, sensor.getDate(0.0)); - Assert.assertEquals(0.0, Vector3D.distance(position, sensor.getPosition()), 1.0e-5); - Assert.assertEquals(0.0, Vector3D.angle(normal, sensor.getMeanPlaneNormal()), 8.0e-7); - - } - -} diff --git a/core/src/test/java/org/orekit/rugged/api/SensorMeanPlaneCrossingTest.java b/core/src/test/java/org/orekit/rugged/api/SensorMeanPlaneCrossingTest.java new file mode 100644 index 0000000000000000000000000000000000000000..4c799351ebfccda5e694cbe14ef5dc65afa33126 --- /dev/null +++ b/core/src/test/java/org/orekit/rugged/api/SensorMeanPlaneCrossingTest.java @@ -0,0 +1,195 @@ +/* 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.api; + +import java.io.File; +import java.net.URISyntaxException; +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.Before; +import org.junit.Test; +import org.orekit.attitudes.NadirPointing; +import org.orekit.attitudes.YawCompensation; +import org.orekit.bodies.BodyShape; +import org.orekit.bodies.OneAxisEllipsoid; +import org.orekit.data.DataProvidersManager; +import org.orekit.data.DirectoryCrawler; +import org.orekit.errors.OrekitException; +import org.orekit.errors.PropagationException; +import org.orekit.frames.FramesFactory; +import org.orekit.orbits.CircularOrbit; +import org.orekit.orbits.Orbit; +import org.orekit.orbits.PositionAngle; +import org.orekit.propagation.Propagator; +import org.orekit.propagation.SpacecraftState; +import org.orekit.propagation.analytical.KeplerianPropagator; +import org.orekit.propagation.sampling.OrekitFixedStepHandler; +import org.orekit.rugged.api.LinearLineDatation; +import org.orekit.rugged.api.LineSensor; +import org.orekit.rugged.utils.SpacecraftToObservedBody; +import org.orekit.time.AbsoluteDate; +import org.orekit.utils.AngularDerivativesFilter; +import org.orekit.utils.CartesianDerivativesFilter; +import org.orekit.utils.Constants; +import org.orekit.utils.IERSConventions; +import org.orekit.utils.TimeStampedAngularCoordinates; +import org.orekit.utils.TimeStampedPVCoordinates; + +public class SensorMeanPlaneCrossingTest { + + @Test + public void testPerfectLine() throws RuggedException, OrekitException { + + final Vector3D position = 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> los = new ArrayList<Vector3D>(); + for (int i = -1000; i <= 1000; ++i) { + final double alpha = i * 0.17 / 1000; + los.add(new Vector3D(FastMath.cos(alpha), fovCenter, FastMath.sin(alpha), cross)); + } + + final LineSensor sensor = new LineSensor("perfect line", + new LinearLineDatation(AbsoluteDate.J2000_EPOCH, 0.0, 1.0 / 1.5e-3), + position, los); + + Assert.assertEquals("perfect line", sensor.getName()); + Assert.assertEquals(AbsoluteDate.J2000_EPOCH, sensor.getDate(0.0)); + Assert.assertEquals(0.0, Vector3D.distance(position, sensor.getPosition()), 1.0e-15); + + SensorMeanPlaneCrossing mean = new SensorMeanPlaneCrossing(sensor, createInterpolator(sensor), + 0, 2000, true, true, 50, 0.01); + Assert.assertEquals(0.0, Vector3D.angle(normal, mean.getMeanPlaneNormal()), 1.0e-15); + + } + + @Test + public void testNoisyLine() throws RuggedException, OrekitException { + + final RandomGenerator random = new Well19937a(0xf3ddb33785e12bdal); + final Vector3D position = 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> los = new ArrayList<Vector3D>(); + for (int i = -1000; i <= 1000; ++i) { + 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 LineSensor sensor = new LineSensor("noisy line", + new LinearLineDatation(AbsoluteDate.J2000_EPOCH, 0.0, 1.0 / 1.5e-3), + position, los); + + Assert.assertEquals("noisy line", sensor.getName()); + Assert.assertEquals(AbsoluteDate.J2000_EPOCH, sensor.getDate(0.0)); + Assert.assertEquals(0.0, Vector3D.distance(position, sensor.getPosition()), 1.0e-5); + + SensorMeanPlaneCrossing mean = new SensorMeanPlaneCrossing(sensor, createInterpolator(sensor), + 0, 2000, true, true, 50, 0.01); + Assert.assertEquals(0.0, Vector3D.angle(normal, mean.getMeanPlaneNormal()), 8.0e-7); + + } + + private SpacecraftToObservedBody createInterpolator(LineSensor sensor) + throws RuggedException, OrekitException { + Orbit orbit = new CircularOrbit(7173352.811913891, + -4.029194321683225E-4, 0.0013530362644647786, + FastMath.toRadians(98.63218182243709), + FastMath.toRadians(77.55565567747836), + FastMath.PI, PositionAngle.TRUE, + FramesFactory.getEME2000(), sensor.getDate(1000), + Constants.EIGEN5C_EARTH_MU); + BodyShape earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS, + Constants.WGS84_EARTH_FLATTENING, + FramesFactory.getITRF(IERSConventions.IERS_2010, true)); + AbsoluteDate minDate = sensor.getDate(0); + AbsoluteDate maxDate = sensor.getDate(2000); + return new SpacecraftToObservedBody(orbit.getFrame(), earth.getBodyFrame(), + minDate, maxDate, 5.0, + orbitToPV(orbit, earth, minDate, maxDate, 0.25), + 2, CartesianDerivativesFilter.USE_P, + orbitToQ(orbit, earth, minDate, maxDate, 0.25), + 2, AngularDerivativesFilter.USE_R, + 0.01); + } + + private List<TimeStampedPVCoordinates> orbitToPV(Orbit orbit, BodyShape earth, + AbsoluteDate minDate, AbsoluteDate maxDate, + double step) + throws PropagationException { + Propagator propagator = new KeplerianPropagator(orbit); + propagator.setAttitudeProvider(new YawCompensation(new NadirPointing(earth))); + propagator.propagate(minDate); + final List<TimeStampedPVCoordinates> list = new ArrayList<TimeStampedPVCoordinates>(); + propagator.setMasterMode(step, new OrekitFixedStepHandler() { + public void init(SpacecraftState s0, AbsoluteDate t) { + } + public void handleStep(SpacecraftState currentState, boolean isLast) { + list.add(new TimeStampedPVCoordinates(currentState.getDate(), + currentState.getPVCoordinates().getPosition(), + currentState.getPVCoordinates().getVelocity())); + } + }); + propagator.propagate(maxDate); + return list; + } + + private List<TimeStampedAngularCoordinates> orbitToQ(Orbit orbit, BodyShape earth, + AbsoluteDate minDate, AbsoluteDate maxDate, + double step) + throws PropagationException { + Propagator propagator = new KeplerianPropagator(orbit); + propagator.setAttitudeProvider(new YawCompensation(new NadirPointing(earth))); + propagator.propagate(minDate); + final List<TimeStampedAngularCoordinates> list = new ArrayList<TimeStampedAngularCoordinates>(); + propagator.setMasterMode(step, new OrekitFixedStepHandler() { + public void init(SpacecraftState s0, AbsoluteDate t) { + } + public void handleStep(SpacecraftState currentState, boolean isLast) { + list.add(new TimeStampedAngularCoordinates(currentState.getDate(), + currentState.getAttitude().getRotation(), + Vector3D.ZERO)); + } + }); + propagator.propagate(maxDate); + return list; + } + + @Before + public void setUp() throws OrekitException, URISyntaxException { + String path = getClass().getClassLoader().getResource("orekit-data").toURI().getPath(); + DataProvidersManager.getInstance().addProvider(new DirectoryCrawler(new File(path))); + } + +}