From 9b50644f2a4115a1ab0417c98c1cc69dde1ff050 Mon Sep 17 00:00:00 2001
From: Damon Kohler <damonkohler@google.com>
Date: Tue, 7 Aug 2012 10:13:52 +0200
Subject: [PATCH] Adds additional tests and methods to facilitate testing.
 Cleans up the FrameTransformTree.transform() algorithm.

---
 .../ros/rosjava_geometry/FrameTransform.java  |  4 +
 .../rosjava_geometry/FrameTransformTree.java  | 32 ++++---
 .../org/ros/rosjava_geometry/Quaternion.java  |  4 +
 .../org/ros/rosjava_geometry/Transform.java   | 80 +++++++++++-----
 .../FrameTransformTreeTest.java               | 96 ++++++++++++++++---
 .../ros/rosjava_geometry/TransformTest.java   | 69 +++++++------
 6 files changed, 204 insertions(+), 81 deletions(-)

diff --git a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransform.java b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransform.java
index 1d0e99d4..be961618 100644
--- a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransform.java
+++ b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransform.java
@@ -79,6 +79,10 @@ public class FrameTransform {
     return target;
   }
 
+  public FrameTransform invert() {
+    return new FrameTransform(transform.invert(), target, source, time);
+  }
+
   /**
    * @return the time associated with the {@link FrameTransform} or {@code null}
    *         if there is no associated time
diff --git a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransformTree.java b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransformTree.java
index e4e9571d..4f250959 100644
--- a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransformTree.java
+++ b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransformTree.java
@@ -196,27 +196,34 @@ public class FrameTransformTree {
       return new FrameTransform(Transform.identity(), resolvedSource, resolvedTarget, null);
     }
     FrameTransform sourceToRoot = transformToRoot(resolvedSource);
-    if (sourceToRoot != null && sourceToRoot.getTargetFrame().equals(resolvedTarget)) {
-      return sourceToRoot;
-    }
     FrameTransform targetToRoot = transformToRoot(resolvedTarget);
-    if (targetToRoot != null) {
-      if (targetToRoot.getTargetFrame().equals(resolvedTarget)) {
-        return targetToRoot;
-      }
+    if (sourceToRoot == null && targetToRoot == null) {
+      return null;
+    }
+    if (sourceToRoot == null) {
       if (targetToRoot.getTargetFrame().equals(resolvedSource)) {
-        Transform transform = targetToRoot.getTransform().invert();
-        return new FrameTransform(transform, resolvedSource, resolvedTarget, targetToRoot.getTime());
+        // resolvedSource is root.
+        return targetToRoot.invert();
+      } else {
+        return null;
       }
     }
-    if (sourceToRoot == null || targetToRoot == null) {
-      return null;
+    if (targetToRoot == null) {
+      if (sourceToRoot.getTargetFrame().equals(resolvedTarget)) {
+        // resolvedTarget is root.
+        return sourceToRoot;
+      } else {
+        return null;
+      }
     }
     if (sourceToRoot.getTargetFrame().equals(targetToRoot.getTargetFrame())) {
+      // Neither resolvedSource nor resolvedTarget is root and both share the
+      // same root.
       Transform transform =
           targetToRoot.getTransform().invert().multiply(sourceToRoot.getTransform());
       return new FrameTransform(transform, resolvedSource, resolvedTarget, sourceToRoot.getTime());
     }
+    // No known transform.
     return null;
   }
 
@@ -234,7 +241,8 @@ public class FrameTransformTree {
    *          the resolved source frame
    * @return the {@link Transform} from {@code source} to root
    */
-  private FrameTransform transformToRoot(GraphName resolvedSource) {
+  @VisibleForTesting
+  FrameTransform transformToRoot(GraphName resolvedSource) {
     FrameTransform result = getLatest(resolvedSource);
     if (result == null) {
       return null;
diff --git a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Quaternion.java b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Quaternion.java
index f0d399ce..55d33b60 100644
--- a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Quaternion.java
+++ b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Quaternion.java
@@ -124,6 +124,10 @@ public class Quaternion {
     return Math.sqrt(getMagnitudeSquared());
   }
 
+  public boolean isAlmostNeutral(double epsilon) {
+    return Math.abs(1 - x * x - y * y - z * z - w * w) < epsilon;
+  }
+
   public geometry_msgs.Quaternion toQuaternionMessage(geometry_msgs.Quaternion result) {
     result.setX(x);
     result.setY(y);
diff --git a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Transform.java b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Transform.java
index ba2439cc..6d23eb79 100644
--- a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Transform.java
+++ b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Transform.java
@@ -16,18 +16,24 @@
 
 package org.ros.rosjava_geometry;
 
+import com.google.common.annotations.VisibleForTesting;
+import com.google.common.collect.Lists;
+
 import org.ros.message.Time;
 import org.ros.namespace.GraphName;
 
+import java.util.List;
+
 /**
- * A transformation in terms of translation and rotation.
+ * A transformation in terms of translation, rotation, and scale.
  * 
+ * @author damonkohler@google.com (Damon Kohler)
  * @author moesenle@google.com (Lorenz Moesenlechner)
  */
 public class Transform {
 
   private Vector3 translation;
-  private Quaternion rotation;
+  private Quaternion rotationAndScale;
 
   public static Transform fromTransformMessage(geometry_msgs.Transform message) {
     return new Transform(Vector3.fromVector3Message(message.getTranslation()),
@@ -65,7 +71,7 @@ public class Transform {
 
   public Transform(Vector3 translation, Quaternion rotation) {
     this.translation = translation;
-    this.rotation = rotation;
+    this.rotationAndScale = rotation;
   }
 
   /**
@@ -76,24 +82,29 @@ public class Transform {
    * @return the resulting {@link Transform}
    */
   public Transform multiply(Transform other) {
-    return new Transform(apply(other.getTranslation()), apply(other.getRotation()));
+    return new Transform(apply(other.translation), apply(other.rotationAndScale));
   }
 
   public Transform invert() {
-    Quaternion inverseRotation = rotation.invert();
-    return new Transform(inverseRotation.rotateVector(translation.invert()), inverseRotation);
+    Quaternion inverseRotationAndScale = rotationAndScale.invert();
+    return new Transform(inverseRotationAndScale.rotateVector(translation.invert()),
+        inverseRotationAndScale);
   }
 
   public Vector3 apply(Vector3 vector) {
-    return rotation.rotateVector(vector).add(translation);
+    return rotationAndScale.rotateVector(vector).add(translation);
   }
 
   public Quaternion apply(Quaternion quaternion) {
-    return rotation.multiply(quaternion);
+    return rotationAndScale.multiply(quaternion);
   }
 
   public Transform scale(double factor) {
-    return new Transform(translation, rotation.scale(factor));
+    return new Transform(translation, rotationAndScale.scale(Math.sqrt(factor)));
+  }
+
+  public double getScale() {
+    return rotationAndScale.getMagnitudeSquared();
   }
 
   /**
@@ -102,28 +113,28 @@ public class Transform {
    *      rotation matrix</a>
    */
   public double[] toMatrix() {
-    double x = getRotation().getX();
-    double y = getRotation().getY();
-    double z = getRotation().getZ();
-    double w = getRotation().getW();
-    double mm = getRotation().getMagnitudeSquared();
+    double x = rotationAndScale.getX();
+    double y = rotationAndScale.getY();
+    double z = rotationAndScale.getZ();
+    double w = rotationAndScale.getW();
+    double mm = rotationAndScale.getMagnitudeSquared();
     return new double[] {
         mm - 2 * y * y - 2 * z * z, 2 * x * y + 2 * z * w, 2 * x * z - 2 * y * w, 0,
         2 * x * y - 2 * z * w, mm - 2 * x * x - 2 * z * z, 2 * y * z + 2 * x * w, 0,
         2 * x * z + 2 * y * w, 2 * y * z - 2 * x * w, mm - 2 * x * x - 2 * y * y, 0,
-        getTranslation().getX(), getTranslation().getY(), getTranslation().getZ(), 1
+        translation.getX(), translation.getY(), translation.getZ(), 1
         };
   }
 
   public geometry_msgs.Transform toTransformMessage(geometry_msgs.Transform result) {
     result.setTranslation(translation.toVector3Message(result.getTranslation()));
-    result.setRotation(rotation.toQuaternionMessage(result.getRotation()));
+    result.setRotation(rotationAndScale.toQuaternionMessage(result.getRotation()));
     return result;
   }
 
   public geometry_msgs.Pose toPoseMessage(geometry_msgs.Pose result) {
     result.setPosition(translation.toPointMessage(result.getPosition()));
-    result.setOrientation(rotation.toQuaternionMessage(result.getOrientation()));
+    result.setOrientation(rotationAndScale.toQuaternionMessage(result.getOrientation()));
     return result;
   }
 
@@ -135,24 +146,43 @@ public class Transform {
     return result;
   }
 
-  public Vector3 getTranslation() {
+  public boolean almostEquals(Transform other, double epsilon) {
+    List<Double> epsilons = Lists.newArrayList();
+    epsilons.add(translation.getX() - other.getTranslation().getX());
+    epsilons.add(translation.getY() - other.getTranslation().getY());
+    epsilons.add(translation.getZ() - other.getTranslation().getZ());
+    epsilons.add(rotationAndScale.getX() - other.getRotationAndScale().getX());
+    epsilons.add(rotationAndScale.getY() - other.getRotationAndScale().getY());
+    epsilons.add(rotationAndScale.getZ() - other.getRotationAndScale().getZ());
+    epsilons.add(rotationAndScale.getW() - other.getRotationAndScale().getW());
+    for (double e : epsilons) {
+      if (Math.abs(e) > epsilon) {
+        return false;
+      }
+    }
+    return true;
+  }
+
+  @VisibleForTesting
+  Vector3 getTranslation() {
     return translation;
   }
 
-  public Quaternion getRotation() {
-    return rotation;
+  @VisibleForTesting
+  Quaternion getRotationAndScale() {
+    return rotationAndScale;
   }
 
   @Override
   public String toString() {
-    return String.format("Transform<%s, %s>", translation, rotation);
+    return String.format("Transform<%s, %s>", translation, rotationAndScale);
   }
 
   @Override
   public int hashCode() {
     final int prime = 31;
     int result = 1;
-    result = prime * result + ((rotation == null) ? 0 : rotation.hashCode());
+    result = prime * result + ((rotationAndScale == null) ? 0 : rotationAndScale.hashCode());
     result = prime * result + ((translation == null) ? 0 : translation.hashCode());
     return result;
   }
@@ -166,10 +196,10 @@ public class Transform {
     if (getClass() != obj.getClass())
       return false;
     Transform other = (Transform) obj;
-    if (rotation == null) {
-      if (other.rotation != null)
+    if (rotationAndScale == null) {
+      if (other.rotationAndScale != null)
         return false;
-    } else if (!rotation.equals(other.rotation))
+    } else if (!rotationAndScale.equals(other.rotationAndScale))
       return false;
     if (translation == null) {
       if (other.translation != null)
diff --git a/rosjava_geometry/src/test/java/org/ros/rosjava_geometry/FrameTransformTreeTest.java b/rosjava_geometry/src/test/java/org/ros/rosjava_geometry/FrameTransformTreeTest.java
index 13fcd367..b41da59b 100644
--- a/rosjava_geometry/src/test/java/org/ros/rosjava_geometry/FrameTransformTreeTest.java
+++ b/rosjava_geometry/src/test/java/org/ros/rosjava_geometry/FrameTransformTreeTest.java
@@ -129,31 +129,97 @@ public class FrameTransformTreeTest {
     }
   }
 
-  @Test
-  public void testTransformToRoot() {
+  /**
+   * Fills the {@link FrameTransformTree} with the following frame topography:
+   * 
+   * <pre>
+   *       foo
+   *    bar   bop
+   * baz         fuz
+   * </pre>
+   */
+  private void updateFrameTransformTree() {
+    {
+      Transform transform = Transform.translation(0, 1, 0);
+      frameTransformTree.update(newTransformStampedMessage(transform, "bar", "foo", new Time()));
+    }
     {
-      Vector3 vector = Vector3.zero();
-      Quaternion quaternion = new Quaternion(Math.sqrt(0.5), 0, 0, Math.sqrt(0.5));
-      Transform transform = new Transform(vector, quaternion);
+      Transform transform = Transform.xRotation(Math.PI / 2);
       frameTransformTree.update(newTransformStampedMessage(transform, "baz", "bar", new Time()));
     }
-
     {
-      Vector3 vector = new Vector3(0, 1, 0);
-      Quaternion quaternion = Quaternion.identity();
-      Transform transform = new Transform(vector, quaternion);
-      frameTransformTree.update(newTransformStampedMessage(transform, "bar", "foo", new Time()));
+      Transform transform = Transform.translation(1, 0, 0);
+      frameTransformTree.update(newTransformStampedMessage(transform, "bop", "foo", new Time()));
+    }
+    {
+      Transform transform = Transform.yRotation(Math.PI / 2);
+      frameTransformTree.update(newTransformStampedMessage(transform, "fuz", "bop", new Time()));
     }
+  }
 
-    FrameTransform frameTransform = frameTransformTree.transform("baz", "foo");
+  private void checkBazToFooTransform(FrameTransform frameTransform) {
     // If we were to reverse the order of the transforms in our implementation,
     // we would expect the translation vector to be <0, 0, 1> instead.
-    Vector3 vector = new Vector3(0, 1, 0);
-    Quaternion quaternion = new Quaternion(Math.sqrt(0.5), 0, 0, Math.sqrt(0.5));
-    Transform transform = new Transform(vector, quaternion);
+    Transform transform = Transform.translation(0, 1, 0).multiply(Transform.xRotation(Math.PI / 2));
+    Quaternion rotationAndScale = transform.getRotationAndScale();
+    assertTrue(String.format("%s is not neutral.", rotationAndScale),
+        rotationAndScale.isAlmostNeutral(1e-9));
     assertEquals(nameResolver.resolve("baz"), frameTransform.getSourceFrame());
     assertEquals(nameResolver.resolve("foo"), frameTransform.getTargetFrame());
-    assertEquals(transform, frameTransform.getTransform());
+    assertTrue(transform.almostEquals(frameTransform.getTransform(), 1e-9));
+  }
+
+  @Test
+  public void testTransformBazToRoot() {
+    updateFrameTransformTree();
+    checkBazToFooTransform(frameTransformTree.transformToRoot(nameResolver.resolve("baz")));
+  }
+
+  @Test
+  public void testTransformBazToFoo() {
+    updateFrameTransformTree();
+    checkBazToFooTransform(frameTransformTree.transform("baz", "foo"));
+    checkBazToFooTransform(frameTransformTree.transform("foo", "baz").invert());
+  }
+
+  private void checkFuzToFooTransform(FrameTransform frameTransform) {
+    // If we were to reverse the order of the transforms in our implementation,
+    // we would expect the translation vector to be <0, 0, 1> instead.
+    Transform transform = Transform.translation(1, 0, 0).multiply(Transform.yRotation(Math.PI / 2));
+    Quaternion rotationAndScale = transform.getRotationAndScale();
+    assertTrue(String.format("%s is not neutral.", rotationAndScale),
+        rotationAndScale.isAlmostNeutral(1e-9));
+    assertEquals(nameResolver.resolve("fuz"), frameTransform.getSourceFrame());
+    assertEquals(nameResolver.resolve("foo"), frameTransform.getTargetFrame());
+    assertTrue(String.format("Expected %s != %s", transform, frameTransform.getTransform()),
+        transform.almostEquals(frameTransform.getTransform(), 1e-9));
+  }
+
+  @Test
+  public void testTransformFuzToRoot() {
+    updateFrameTransformTree();
+    checkFuzToFooTransform(frameTransformTree.transformToRoot(nameResolver.resolve("fuz")));
+  }
+
+  @Test
+  public void testTransformFuzToFoo() {
+    updateFrameTransformTree();
+    checkFuzToFooTransform(frameTransformTree.transform("fuz", "foo"));
+    checkFuzToFooTransform(frameTransformTree.transform("foo", "fuz").invert());
+  }
+
+  @Test
+  public void testTransformBazToFuz() {
+    updateFrameTransformTree();
+    FrameTransform frameTransform = frameTransformTree.transform("baz", "fuz");
+    Transform transform =
+        Transform.yRotation(Math.PI / 2).invert().multiply(Transform.translation(1, 0, 0).invert())
+            .multiply(Transform.translation(0, 1, 0)).multiply(Transform.xRotation(Math.PI / 2));
+    assertTrue(transform.getRotationAndScale().isAlmostNeutral(1e-9));
+    assertEquals(nameResolver.resolve("baz"), frameTransform.getSourceFrame());
+    assertEquals(nameResolver.resolve("fuz"), frameTransform.getTargetFrame());
+    assertTrue(String.format("Expected %s != %s", transform, frameTransform.getTransform()),
+        transform.almostEquals(frameTransform.getTransform(), 1e-9));
   }
 
   @Test
diff --git a/rosjava_geometry/src/test/java/org/ros/rosjava_geometry/TransformTest.java b/rosjava_geometry/src/test/java/org/ros/rosjava_geometry/TransformTest.java
index f5bbd4b0..8373829c 100644
--- a/rosjava_geometry/src/test/java/org/ros/rosjava_geometry/TransformTest.java
+++ b/rosjava_geometry/src/test/java/org/ros/rosjava_geometry/TransformTest.java
@@ -17,9 +17,12 @@
 package org.ros.rosjava_geometry;
 
 import static org.junit.Assert.assertEquals;
+import static org.junit.Assert.assertTrue;
 
 import org.junit.Test;
 
+import java.util.Random;
+
 /**
  * @author damonkohler@google.com (Damon Kohler)
  */
@@ -27,52 +30,60 @@ public class TransformTest {
 
   @Test
   public void testMultiply() {
-    Transform transform1 = new Transform(new Vector3(1, 0, 0), new Quaternion(0, 0, 0, 1));
+    Transform transform1 = new Transform(Vector3.xAxis(), Quaternion.identity());
     Transform transform2 =
-        new Transform(new Vector3(0, 1, 0), Quaternion.fromAxisAngle(new Vector3(0, 0, 1),
-            Math.PI / 2));
+        new Transform(Vector3.yAxis(), Quaternion.fromAxisAngle(Vector3.zAxis(), Math.PI / 2));
 
     Transform result1 = transform1.multiply(transform2);
     assertEquals(1.0, result1.getTranslation().getX(), 1e-9);
     assertEquals(1.0, result1.getTranslation().getY(), 1e-9);
     assertEquals(0.0, result1.getTranslation().getZ(), 1e-9);
-    assertEquals(0.0, result1.getRotation().getX(), 1e-9);
-    assertEquals(0.0, result1.getRotation().getY(), 1e-9);
-    assertEquals(0.7071067811865475, result1.getRotation().getZ(), 1e-9);
-    assertEquals(0.7071067811865475, result1.getRotation().getW(), 1e-9);
+    assertEquals(0.0, result1.getRotationAndScale().getX(), 1e-9);
+    assertEquals(0.0, result1.getRotationAndScale().getY(), 1e-9);
+    assertEquals(0.7071067811865475, result1.getRotationAndScale().getZ(), 1e-9);
+    assertEquals(0.7071067811865475, result1.getRotationAndScale().getW(), 1e-9);
 
     Transform result2 = transform2.multiply(transform1);
     assertEquals(0.0, result2.getTranslation().getX(), 1e-9);
     assertEquals(2.0, result2.getTranslation().getY(), 1e-9);
     assertEquals(0.0, result2.getTranslation().getZ(), 1e-9);
-    assertEquals(0.0, result2.getRotation().getX(), 1e-9);
-    assertEquals(0.0, result2.getRotation().getY(), 1e-9);
-    assertEquals(0.7071067811865475, result2.getRotation().getZ(), 1e-9);
-    assertEquals(0.7071067811865475, result2.getRotation().getW(), 1e-9);
+    assertEquals(0.0, result2.getRotationAndScale().getX(), 1e-9);
+    assertEquals(0.0, result2.getRotationAndScale().getY(), 1e-9);
+    assertEquals(0.7071067811865475, result2.getRotationAndScale().getZ(), 1e-9);
+    assertEquals(0.7071067811865475, result2.getRotationAndScale().getW(), 1e-9);
   }
 
   @Test
   public void testInvert() {
     Transform transform =
-        new Transform(new Vector3(0, 1, 0), Quaternion.fromAxisAngle(new Vector3(0, 0, 1),
-            Math.PI / 2));
-    Transform transformInverse = transform.invert();
+        new Transform(Vector3.yAxis(), Quaternion.fromAxisAngle(Vector3.zAxis(), Math.PI / 2));
+    Transform inverse = transform.invert();
+
+    assertEquals(-1.0, inverse.getTranslation().getX(), 1e-9);
+    assertEquals(0.0, inverse.getTranslation().getY(), 1e-9);
+    assertEquals(0.0, inverse.getTranslation().getZ(), 1e-9);
+    assertEquals(0.0, inverse.getRotationAndScale().getX(), 1e-9);
+    assertEquals(0.0, inverse.getRotationAndScale().getY(), 1e-9);
+    assertEquals(-0.7071067811865475, inverse.getRotationAndScale().getZ(), 1e-9);
+    assertEquals(0.7071067811865475, inverse.getRotationAndScale().getW(), 1e-9);
 
-    assertEquals(-1.0, transformInverse.getTranslation().getX(), 1e-9);
-    assertEquals(0.0, transformInverse.getTranslation().getY(), 1e-9);
-    assertEquals(0.0, transformInverse.getTranslation().getZ(), 1e-9);
-    assertEquals(0.0, transformInverse.getRotation().getX(), 1e-9);
-    assertEquals(0.0, transformInverse.getRotation().getY(), 1e-9);
-    assertEquals(-0.7071067811865475, transformInverse.getRotation().getZ(), 1e-9);
-    assertEquals(0.7071067811865475, transformInverse.getRotation().getW(), 1e-9);
+    Transform neutral = transform.multiply(inverse);
+    assertTrue(neutral.almostEquals(Transform.identity(), 1e-9));
+  }
 
-    Transform neutral = transform.multiply(transformInverse);
-    assertEquals(0.0, neutral.getTranslation().getX(), 1e-9);
-    assertEquals(0.0, neutral.getTranslation().getY(), 1e-9);
-    assertEquals(0.0, neutral.getTranslation().getZ(), 1e-9);
-    assertEquals(0.0, neutral.getRotation().getX(), 1e-9);
-    assertEquals(0.0, neutral.getRotation().getY(), 1e-9);
-    assertEquals(0.0, neutral.getRotation().getZ(), 1e-9);
-    assertEquals(1.0, neutral.getRotation().getW(), 1e-9);
+  @Test
+  public void testInvertRandom() {
+    Random random = new Random();
+    random.setSeed(42);
+    for (int i = 0; i < 10000; i++) {
+      Vector3 vector = new Vector3(random.nextDouble(), random.nextDouble(), random.nextDouble());
+      Quaternion quaternion =
+          new Quaternion(random.nextDouble(), random.nextDouble(), random.nextDouble(),
+              random.nextDouble());
+      Transform transform = new Transform(vector, quaternion);
+      Transform inverse = transform.invert();
+      Transform neutral = transform.multiply(inverse);
+      assertTrue(neutral.almostEquals(Transform.identity(), 1e-9));
+    }
   }
 }
-- 
GitLab