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

Adds additional tests and methods to facilitate testing.

Cleans up the FrameTransformTree.transform() algorithm.
parent 12719133
No related branches found
No related tags found
No related merge requests found
......@@ -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
......
......@@ -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) {
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;
......
......@@ -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);
......
......@@ -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)
......
......@@ -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() {
{
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.translation(0, 1, 0);
frameTransformTree.update(newTransformStampedMessage(transform, "bar", "foo", new Time()));
}
{
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
......
......@@ -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));
}
}
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment