Skip to content
Snippets Groups Projects
Commit 9e205334 authored by Damon Kohler's avatar Damon Kohler
Browse files

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.
parent b668ee95
No related branches found
No related tags found
No related merge requests found
...@@ -16,10 +16,12 @@ ...@@ -16,10 +16,12 @@
package org.ros.rosjava_geometry; package org.ros.rosjava_geometry;
import org.ros.message.Time;
import org.ros.namespace.GraphName; 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) * @author damonkohler@google.com (Damon Kohler)
*/ */
...@@ -32,8 +34,8 @@ public class FrameTransform { ...@@ -32,8 +34,8 @@ public class FrameTransform {
public static FrameTransform public static FrameTransform
fromTransformStamped(geometry_msgs.TransformStamped transformStamped) { fromTransformStamped(geometry_msgs.TransformStamped transformStamped) {
Transform transform = Transform.newFromTransformMessage(transformStamped.getTransform()); Transform transform = Transform.newFromTransformMessage(transformStamped.getTransform());
String source = transformStamped.getHeader().getFrameId(); String target = transformStamped.getHeader().getFrameId();
String target = transformStamped.getChildFrameId(); String source = transformStamped.getChildFrameId();
return new FrameTransform(transform, GraphName.of(source), GraphName.of(target)); return new FrameTransform(transform, GraphName.of(source), GraphName.of(target));
} }
...@@ -55,8 +57,54 @@ public class FrameTransform { ...@@ -55,8 +57,54 @@ public class FrameTransform {
return target; 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 @Override
public String toString() { public String toString() {
return String.format("FrameTransform<Source: %s, Target: %s, %s>", source, target, transform); 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;
}
} }
...@@ -27,20 +27,19 @@ import java.util.Map; ...@@ -27,20 +27,19 @@ import java.util.Map;
/** /**
* A tree of {@link FrameTransform}s. * A tree of {@link FrameTransform}s.
*
* <p> * <p>
* {@link FrameTransformTree} does not currently support time travel. Lookups * {@link FrameTransformTree} does not currently support time travel. Lookups
* always use the newest {@link TransformStamped}. * always use the newest {@link TransformStamped}.
* *
* @author damonkohler@google.com (Damon Kohler)
* @author moesenle@google.com (Lorenz Moesenlechner) * @author moesenle@google.com (Lorenz Moesenlechner)
*
*/ */
public class FrameTransformTree { public class FrameTransformTree {
private final NameResolver nameResolver; 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. * frame.
*/ */
private final Map<GraphName, LazyFrameTransform> transforms; private final Map<GraphName, LazyFrameTransform> transforms;
...@@ -51,14 +50,19 @@ public class FrameTransformTree { ...@@ -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 * @param transformStamped
* the transform to update * the {@link geometry_msgs.TransformStamped} message to update with
*/ */
public void updateTransform(geometry_msgs.TransformStamped transformStamped) { public void updateTransform(geometry_msgs.TransformStamped transformStamped) {
GraphName source = nameResolver.resolve(transformStamped.getChildFrameId()); GraphName target = nameResolver.resolve(transformStamped.getChildFrameId());
transforms.put(source, new LazyFrameTransform(transformStamped)); transforms.put(target, new LazyFrameTransform(transformStamped));
} }
private FrameTransform getLatestTransform(GraphName frame) { private FrameTransform getLatestTransform(GraphName frame) {
...@@ -111,12 +115,13 @@ public class FrameTransformTree { ...@@ -111,12 +115,13 @@ public class FrameTransformTree {
FrameTransform result = FrameTransform result =
new FrameTransform(Transform.newIdentityTransform(), sourceFrame, sourceFrame); new FrameTransform(Transform.newIdentityTransform(), sourceFrame, sourceFrame);
while (true) { while (true) {
FrameTransform parent = getLatestTransform(result.getTargetFrame()); FrameTransform resultToParent = getLatestTransform(result.getTargetFrame());
if (parent == null) { if (resultToParent == null) {
return result; return result;
} }
Transform transform = result.getTransform().multiply(parent.getTransform()); // Now resultToParent.getSourceFrame() == result.getTargetFrame()
GraphName targetFrame = nameResolver.resolve(parent.getSourceFrame()); Transform transform = resultToParent.getTransform().multiply(result.getTransform());
GraphName targetFrame = nameResolver.resolve(resultToParent.getTargetFrame());
result = new FrameTransform(transform, sourceFrame, targetFrame); result = new FrameTransform(transform, sourceFrame, targetFrame);
} }
} }
......
...@@ -68,18 +68,6 @@ public class Quaternion { ...@@ -68,18 +68,6 @@ public class Quaternion {
this.w = w; 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() { public Quaternion invert() {
return new Quaternion(-x, -y, -z, w); return new Quaternion(-x, -y, -z, w);
} }
...@@ -136,6 +124,11 @@ public class Quaternion { ...@@ -136,6 +124,11 @@ public class Quaternion {
this.w = w; this.w = w;
} }
@Override
public String toString() {
return String.format("Quaternion<x: %.4f, y: %.4f, z: %.4f, w: %.4f>", x, y, z, w);
}
@Override @Override
public int hashCode() { public int hashCode() {
final int prime = 31; final int prime = 31;
...@@ -171,9 +164,4 @@ public class Quaternion { ...@@ -171,9 +164,4 @@ public class Quaternion {
return false; return false;
return true; return true;
} }
@Override
public String toString() {
return String.format("Quaternion<x: %.4f, y: %.4f, z: %.4f, w: %.4f>", x, y, z, w);
}
} }
...@@ -35,8 +35,7 @@ public class Transform { ...@@ -35,8 +35,7 @@ public class Transform {
} }
public Transform multiply(Transform other) { public Transform multiply(Transform other) {
return new Transform(transformVector(other.getTranslation()), return new Transform(translate(other.getTranslation()), rotate(other.getRotation()));
transformQuaternion(other.getRotation()));
} }
public Transform invert() { public Transform invert() {
...@@ -44,29 +43,33 @@ public class Transform { ...@@ -44,29 +43,33 @@ public class Transform {
return new Transform(inverseRotation.rotateVector(translation.invert()), inverseRotation); return new Transform(inverseRotation.rotateVector(translation.invert()), inverseRotation);
} }
public Vector3 transformVector(Vector3 vector) { public Vector3 translate(Vector3 vector) {
return translation.add(rotation.rotateVector(vector)); return translation.add(rotation.rotateVector(vector));
} }
public Quaternion transformQuaternion(Quaternion quaternion) { public Quaternion rotate(Quaternion quaternion) {
return rotation.multiply(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) { public geometry_msgs.Transform toTransformMessage(geometry_msgs.Transform result) {
result.setTranslation(translation.toVector3Message(result.getTranslation())); result.setTranslation(translation.toVector3Message(result.getTranslation()));
result.setRotation(rotation.toQuaternionMessage(result.getRotation())); result.setRotation(rotation.toQuaternionMessage(result.getRotation()));
return result; 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) { public geometry_msgs.Pose toPoseMessage(geometry_msgs.Pose result) {
result.setPosition(translation.toPointMessage(result.getPosition())); result.setPosition(translation.toPointMessage(result.getPosition()));
result.setOrientation(rotation.toQuaternionMessage(result.getOrientation())); result.setOrientation(rotation.toQuaternionMessage(result.getOrientation()));
...@@ -108,11 +111,42 @@ public class Transform { ...@@ -108,11 +111,42 @@ public class Transform {
} }
public static Transform newIdentityTransform() { public static Transform newIdentityTransform() {
return new Transform(Vector3.newIdentityVector3(), Quaternion.newIdentityQuaternion()); return new Transform(Vector3.newZeroVector(), Quaternion.newIdentityQuaternion());
} }
@Override @Override
public String toString() { public String toString() {
return String.format("Transform<%s, %s>", translation, rotation); 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;
}
} }
...@@ -103,7 +103,7 @@ public class Vector3 { ...@@ -103,7 +103,7 @@ public class Vector3 {
return new Vector3(message.getX(), message.getY(), message.getZ()); return new Vector3(message.getX(), message.getY(), message.getZ());
} }
public static Vector3 newIdentityVector3() { public static Vector3 newZeroVector() {
return new Vector3(0, 0, 0); return new Vector3(0, 0, 0);
} }
......
/*
* 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());
}
}
...@@ -25,56 +25,6 @@ import org.junit.Test; ...@@ -25,56 +25,6 @@ import org.junit.Test;
*/ */
public class QuaternionTest { 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 @Test
public void testAxisAngleToQuaternion() { public void testAxisAngleToQuaternion() {
Quaternion quaternion; Quaternion quaternion;
...@@ -136,7 +86,7 @@ public class QuaternionTest { ...@@ -136,7 +86,7 @@ public class QuaternionTest {
Quaternion quaternion = Quaternion.newFromAxisAngle(new Vector3(0, 0, 1), Math.PI / 2); Quaternion quaternion = Quaternion.newFromAxisAngle(new Vector3(0, 0, 1), Math.PI / 2);
Quaternion inverse = quaternion.invert(); Quaternion inverse = quaternion.invert();
Quaternion rotated = quaternion.multiply(inverse); Quaternion rotated = quaternion.multiply(inverse);
assertEquals(0, rotated.getAngle(), 1e-9); assertEquals(1, rotated.getW(), 1e-9);
} }
@Test @Test
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment