Fixes bugs in transform logic and add tests.

Removes unused/dangerous methods for converting quaternions to axis-angle.
Adds new method for getting the matrix form of a quaternion.

rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransform.java

@@ -16,10 +16,12 @@
 
 package org.ros.rosjava_geometry;
 
+import org.ros.message.Time;
 import org.ros.namespace.GraphName;
 
 /**
- * Describes a {@link Transform} from a source frame to a target frame.
+ * Describes a {@link Transform} from data in the source frame to data in the
+ * target frame.
  * 
  * @author damonkohler@google.com (Damon Kohler)
  */
@@ -32,8 +34,8 @@ public class FrameTransform {
   public static FrameTransform
       fromTransformStamped(geometry_msgs.TransformStamped transformStamped) {
     Transform transform = Transform.newFromTransformMessage(transformStamped.getTransform());
-    String source = transformStamped.getHeader().getFrameId();
-    String target = transformStamped.getChildFrameId();
+    String target = transformStamped.getHeader().getFrameId();
+    String source = transformStamped.getChildFrameId();
     return new FrameTransform(transform, GraphName.of(source), GraphName.of(target));
   }
 
@@ -55,8 +57,54 @@ public class FrameTransform {
     return target;
   }
 
+  public geometry_msgs.TransformStamped toTransformStampedMessage(Time stamp,
+      geometry_msgs.TransformStamped result) {
+    result.getHeader().setFrameId(target.toString());
+    result.getHeader().setStamp(stamp);
+    result.setChildFrameId(source.toString());
+    transform.toTransformMessage(result.getTransform());
+    return result;
+  }
+
   @Override
   public String toString() {
     return String.format("FrameTransform<Source: %s, Target: %s, %s>", source, target, transform);
   }
+
+  @Override
+  public int hashCode() {
+    final int prime = 31;
+    int result = 1;
+    result = prime * result + ((source == null) ? 0 : source.hashCode());
+    result = prime * result + ((target == null) ? 0 : target.hashCode());
+    result = prime * result + ((transform == null) ? 0 : transform.hashCode());
+    return result;
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (this == obj)
+      return true;
+    if (obj == null)
+      return false;
+    if (getClass() != obj.getClass())
+      return false;
+    FrameTransform other = (FrameTransform) obj;
+    if (source == null) {
+      if (other.source != null)
+        return false;
+    } else if (!source.equals(other.source))
+      return false;
+    if (target == null) {
+      if (other.target != null)
+        return false;
+    } else if (!target.equals(other.target))
+      return false;
+    if (transform == null) {
+      if (other.transform != null)
+        return false;
+    } else if (!transform.equals(other.transform))
+      return false;
+    return true;
+  }
 }

rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransformTree.java

@@ -27,20 +27,19 @@ import java.util.Map;
 
 /**
  * A tree of {@link FrameTransform}s.
- * 
  * <p>
  * {@link FrameTransformTree} does not currently support time travel. Lookups
  * always use the newest {@link TransformStamped}.
  * 
+ * @author damonkohler@google.com (Damon Kohler)
  * @author moesenle@google.com (Lorenz Moesenlechner)
- * 
  */
 public class FrameTransformTree {
 
   private final NameResolver nameResolver;
 
   /**
-   * A {@link Map} of the most recent {@link LazyFrameTransform} by source
+   * A {@link Map} of the most recent {@link LazyFrameTransform} by target
    * frame.
    */
   private final Map<GraphName, LazyFrameTransform> transforms;
@@ -51,14 +50,19 @@ public class FrameTransformTree {
   }
 
   /**
-   * Updates the transform tree with the provided transform.
+   * Updates the tree with the provided {@link geometry_msgs.TransformStamped}
+   * message.
+   * <p>
+   * Note that the tree is updated lazily. Modifications to the provided
+   * {@link geometry_msgs.TransformStamped} message may cause unpredictable
+   * results.
    * 
    * @param transformStamped
-   *          the transform to update
+   *          the {@link geometry_msgs.TransformStamped} message to update with
    */
   public void updateTransform(geometry_msgs.TransformStamped transformStamped) {
-    GraphName source = nameResolver.resolve(transformStamped.getChildFrameId());
-    transforms.put(source, new LazyFrameTransform(transformStamped));
+    GraphName target = nameResolver.resolve(transformStamped.getChildFrameId());
+    transforms.put(target, new LazyFrameTransform(transformStamped));
   }
 
   private FrameTransform getLatestTransform(GraphName frame) {
@@ -111,12 +115,13 @@ public class FrameTransformTree {
     FrameTransform result =
         new FrameTransform(Transform.newIdentityTransform(), sourceFrame, sourceFrame);
     while (true) {
-      FrameTransform parent = getLatestTransform(result.getTargetFrame());
-      if (parent == null) {
+      FrameTransform resultToParent = getLatestTransform(result.getTargetFrame());
+      if (resultToParent == null) {
         return result;
       }
-      Transform transform = result.getTransform().multiply(parent.getTransform());
-      GraphName targetFrame = nameResolver.resolve(parent.getSourceFrame());
+      // Now resultToParent.getSourceFrame() == result.getTargetFrame()
+      Transform transform = resultToParent.getTransform().multiply(result.getTransform());
+      GraphName targetFrame = nameResolver.resolve(resultToParent.getTargetFrame());
       result = new FrameTransform(transform, sourceFrame, targetFrame);
     }
   }

rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Quaternion.java

@@ -68,18 +68,6 @@ public class Quaternion {
     this.w = w;
   }
 
-  public double getAngle() {
-    return 2 * Math.acos(w);
-  }
-
-  public Vector3 getAxis() {
-    double length = Math.sqrt(1 - w * w);
-    if (length > 1e-9) {
-      return new Vector3(x / length, y / length, z / length);
-    }
-    return new Vector3(0, 0, 0);
-  }
-
   public Quaternion invert() {
     return new Quaternion(-x, -y, -z, w);
   }
@@ -136,6 +124,11 @@ public class Quaternion {
     this.w = w;
   }
 
+  @Override
+  public String toString() {
+    return String.format("Quaternion<x: %.4f, y: %.4f, z: %.4f, w: %.4f>", x, y, z, w);
+  }
+
   @Override
   public int hashCode() {
     final int prime = 31;
@@ -171,9 +164,4 @@ public class Quaternion {
       return false;
     return true;
   }
-
-  @Override
-  public String toString() {
-    return String.format("Quaternion<x: %.4f, y: %.4f, z: %.4f, w: %.4f>", x, y, z, w);
-  }
 }

rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Transform.java

@@ -35,8 +35,7 @@ public class Transform {
   }
 
   public Transform multiply(Transform other) {
-    return new Transform(transformVector(other.getTranslation()),
-        transformQuaternion(other.getRotation()));
+    return new Transform(translate(other.getTranslation()), rotate(other.getRotation()));
   }
 
   public Transform invert() {
@@ -44,29 +43,33 @@ public class Transform {
     return new Transform(inverseRotation.rotateVector(translation.invert()), inverseRotation);
   }
 
-  public Vector3 transformVector(Vector3 vector) {
+  public Vector3 translate(Vector3 vector) {
     return translation.add(rotation.rotateVector(vector));
   }
 
-  public Quaternion transformQuaternion(Quaternion quaternion) {
+  public Quaternion rotate(Quaternion quaternion) {
     return rotation.multiply(quaternion);
   }
 
+  public double[] toMatrix() {
+    double x = getRotation().getX();
+    double y = getRotation().getY();
+    double z = getRotation().getZ();
+    double w = getRotation().getW();
+    return new double[] {
+        1 - 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, 1 - 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, 1 - 2 * x * x - 2 * y * y, 0,
+        getTranslation().getX(), getTranslation().getY(), getTranslation().getZ(), 1
+        };
+  }
+
   public geometry_msgs.Transform toTransformMessage(geometry_msgs.Transform result) {
     result.setTranslation(translation.toVector3Message(result.getTranslation()));
     result.setRotation(rotation.toQuaternionMessage(result.getRotation()));
     return result;
   }
 
-  public geometry_msgs.TransformStamped toTransformStampedMessage(GraphName frame,
-      GraphName childFrame, Time stamp, geometry_msgs.TransformStamped result) {
-    result.getHeader().setFrameId(frame.toString());
-    result.getHeader().setStamp(stamp);
-    result.setChildFrameId(childFrame.toString());
-    result.setTransform(toTransformMessage(result.getTransform()));
-    return result;
-  }
-
   public geometry_msgs.Pose toPoseMessage(geometry_msgs.Pose result) {
     result.setPosition(translation.toPointMessage(result.getPosition()));
     result.setOrientation(rotation.toQuaternionMessage(result.getOrientation()));
@@ -108,11 +111,42 @@ public class Transform {
   }
 
   public static Transform newIdentityTransform() {
-    return new Transform(Vector3.newIdentityVector3(), Quaternion.newIdentityQuaternion());
+    return new Transform(Vector3.newZeroVector(), Quaternion.newIdentityQuaternion());
   }
 
   @Override
   public String toString() {
     return String.format("Transform<%s, %s>", translation, rotation);
   }
+
+  @Override
+  public int hashCode() {
+    final int prime = 31;
+    int result = 1;
+    result = prime * result + ((rotation == null) ? 0 : rotation.hashCode());
+    result = prime * result + ((translation == null) ? 0 : translation.hashCode());
+    return result;
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (this == obj)
+      return true;
+    if (obj == null)
+      return false;
+    if (getClass() != obj.getClass())
+      return false;
+    Transform other = (Transform) obj;
+    if (rotation == null) {
+      if (other.rotation != null)
+        return false;
+    } else if (!rotation.equals(other.rotation))
+      return false;
+    if (translation == null) {
+      if (other.translation != null)
+        return false;
+    } else if (!translation.equals(other.translation))
+      return false;
+    return true;
+  }
 }

rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Vector3.java

@@ -103,7 +103,7 @@ public class Vector3 {
     return new Vector3(message.getX(), message.getY(), message.getZ());
   }
 
-  public static Vector3 newIdentityVector3() {
+  public static Vector3 newZeroVector() {
     return new Vector3(0, 0, 0);
   }
 

rosjava_geometry/src/test/java/org/ros/rosjava_geometry/FrameTransformTreeTest.java

+/*
+ * Copyright (C) 2011 Google Inc.
+ *
+ * Licensed 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.ros.rosjava_geometry;
+
+import static org.junit.Assert.assertEquals;
+
+import org.junit.Test;
+import org.ros.internal.message.DefaultMessageFactory;
+import org.ros.internal.message.definition.MessageDefinitionReflectionProvider;
+import org.ros.message.MessageDefinitionProvider;
+import org.ros.message.MessageFactory;
+import org.ros.message.Time;
+import org.ros.namespace.GraphName;
+import org.ros.namespace.NameResolver;
+
+/**
+ * @author damonkohler@google.com (Damon Kohler)
+ */
+public class FrameTransformTreeTest {
+
+  @Test
+  public void testIdentityTransforms() {
+    MessageDefinitionProvider messageDefinitionProvider = new MessageDefinitionReflectionProvider();
+    MessageFactory messageFactory = new DefaultMessageFactory(messageDefinitionProvider);
+    NameResolver nameResolver = NameResolver.newRoot();
+    FrameTransformTree frameTransformTree = new FrameTransformTree(nameResolver);
+
+    {
+      geometry_msgs.TransformStamped message =
+          messageFactory.newFromType(geometry_msgs.TransformStamped._TYPE);
+      Transform transform = Transform.newIdentityTransform();
+      FrameTransform frameTransform =
+          new FrameTransform(transform, GraphName.of("baz"), GraphName.of("bar"));
+      frameTransform.toTransformStampedMessage(new Time(), message);
+      frameTransformTree.updateTransform(message);
+    }
+
+    {
+      geometry_msgs.TransformStamped message =
+          messageFactory.newFromType(geometry_msgs.TransformStamped._TYPE);
+      Transform transform = Transform.newIdentityTransform();
+      FrameTransform frameTransform =
+          new FrameTransform(transform, GraphName.of("bar"), GraphName.of("foo"));
+      frameTransform.toTransformStampedMessage(new Time(), message);
+      frameTransformTree.updateTransform(message);
+    }
+
+    FrameTransform frameTransform =
+        frameTransformTree.newFrameTransform(GraphName.of("baz"), GraphName.of("foo"));
+    assertEquals(nameResolver.resolve("baz"), frameTransform.getSourceFrame());
+    assertEquals(nameResolver.resolve("foo"), frameTransform.getTargetFrame());
+    assertEquals(Transform.newIdentityTransform(), frameTransform.getTransform());
+  }
+
+  @Test
+  public void testTransformToRoot() {
+    MessageDefinitionProvider messageDefinitionProvider = new MessageDefinitionReflectionProvider();
+    MessageFactory messageFactory = new DefaultMessageFactory(messageDefinitionProvider);
+    NameResolver nameResolver = NameResolver.newRoot();
+    FrameTransformTree frameTransformTree = new FrameTransformTree(nameResolver);
+
+    {
+      geometry_msgs.TransformStamped message =
+          messageFactory.newFromType(geometry_msgs.TransformStamped._TYPE);
+      Vector3 vector = Vector3.newZeroVector();
+      Quaternion quaternion = new Quaternion(Math.sqrt(0.5), 0, 0, Math.sqrt(0.5));
+      Transform transform = new Transform(vector, quaternion);
+      GraphName source = GraphName.of("baz");
+      GraphName target = GraphName.of("bar");
+      FrameTransform frameTransform = new FrameTransform(transform, source, target);
+      frameTransform.toTransformStampedMessage(new Time(), message);
+      frameTransformTree.updateTransform(message);
+    }
+
+    {
+      geometry_msgs.TransformStamped message =
+          messageFactory.newFromType(geometry_msgs.TransformStamped._TYPE);
+      Vector3 vector = new Vector3(0, 1, 0);
+      Quaternion quaternion = Quaternion.newIdentityQuaternion();
+      Transform transform = new Transform(vector, quaternion);
+      GraphName source = GraphName.of("bar");
+      GraphName target = GraphName.of("foo");
+      FrameTransform frameTransform = new FrameTransform(transform, source, target);
+      frameTransform.toTransformStampedMessage(new Time(), message);
+      frameTransformTree.updateTransform(message);
+    }
+
+    FrameTransform frameTransform =
+        frameTransformTree.newFrameTransform(GraphName.of("baz"), GraphName.of("foo"));
+    // 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);
+    assertEquals(nameResolver.resolve("baz"), frameTransform.getSourceFrame());
+    assertEquals(nameResolver.resolve("foo"), frameTransform.getTargetFrame());
+    assertEquals(transform, frameTransform.getTransform());
+  }
+}

rosjava_geometry/src/test/java/org/ros/rosjava_geometry/QuaternionTest.java

@@ -25,56 +25,6 @@ import org.junit.Test;
  */
 public class QuaternionTest {
 
-  @Test
-  public void testCalculateRotationAngleAxis() {
-    Quaternion quaternion;
-    Vector3 axis;
-
-    quaternion = new Quaternion(0, 0, 0, 1);
-    assertEquals(0.0, quaternion.getAngle(), 1e-9);
-    axis = quaternion.getAxis();
-    assertEquals(0, axis.getX(), 1e-9);
-    assertEquals(0, axis.getY(), 1e-9);
-    assertEquals(0, axis.getZ(), 1e-9);
-
-    quaternion = new Quaternion(0, 0, 1, 0);
-    assertEquals(Math.PI, quaternion.getAngle(), 1e-9);
-    axis = quaternion.getAxis();
-    assertEquals(0, axis.getX(), 1e-9);
-    assertEquals(0, axis.getY(), 1e-9);
-    assertEquals(1, axis.getZ(), 1e-9);
-
-    quaternion = new Quaternion(0, 0, -0.7071067811865475, 0.7071067811865475);
-    // The actual angle is -Math.PI / 2 but this is represented by a flipped
-    // rotation axis in the quaternion.
-    assertEquals(Math.PI / 2, quaternion.getAngle(), 1e-9);
-    axis = quaternion.getAxis();
-    assertEquals(0, axis.getX(), 1e-9);
-    assertEquals(0, axis.getY(), 1e-9);
-    assertEquals(-1, axis.getZ(), 1e-9);
-
-    quaternion = new Quaternion(0, 0, 0.9238795325112867, 0.38268343236508984);
-    assertEquals(0.75 * Math.PI, quaternion.getAngle(), 1e-9);
-    axis = quaternion.getAxis();
-    assertEquals(0, axis.getX(), 1e-9);
-    assertEquals(0, axis.getY(), 1e-9);
-    assertEquals(1, axis.getZ(), 1e-9);
-
-    quaternion = new Quaternion(0, 0, -0.9238795325112867, 0.38268343236508984);
-    assertEquals(0.75 * Math.PI, quaternion.getAngle(), 1e-9);
-    axis = quaternion.getAxis();
-    assertEquals(0, axis.getX(), 1e-9);
-    assertEquals(0, axis.getY(), 1e-9);
-    assertEquals(-1, axis.getZ(), 1e-9);
-
-    quaternion = new Quaternion(0, 0, 0.7071067811865475, -0.7071067811865475);
-    assertEquals(1.5 * Math.PI, quaternion.getAngle(), 1e-9);
-    axis = quaternion.getAxis();
-    assertEquals(0, axis.getX(), 1e-9);
-    assertEquals(0, axis.getY(), 1e-9);
-    assertEquals(1, axis.getZ(), 1e-9);
-  }
-
   @Test
   public void testAxisAngleToQuaternion() {
     Quaternion quaternion;
@@ -136,7 +86,7 @@ public class QuaternionTest {
     Quaternion quaternion = Quaternion.newFromAxisAngle(new Vector3(0, 0, 1), Math.PI / 2);
     Quaternion inverse = quaternion.invert();
     Quaternion rotated = quaternion.multiply(inverse);
-    assertEquals(0, rotated.getAngle(), 1e-9);
+    assertEquals(1, rotated.getW(), 1e-9);
   }
 
   @Test