private void add(GraphName source, LazyFrameTransform lazyFrameTransform) {
// This adds support for tf2 while maintaining backward compatibility with tf.
GraphName relativeSource = source.toRelative();
if (!transforms.containsKey(relativeSource)) {
transforms.put(
relativeSource, new CircularBlockingDeque<LazyFrameTransform>(TRANSFORM_QUEUE_CAPACITY));
}
synchronized (mutex) {
transforms.get(relativeSource).addFirst(lazyFrameTransform);
}
}
/**
* @return the {@link FrameTransform} from source the frame to the target frame, or {@code null}
* if no {@link FrameTransform} could be found
*/
public FrameTransform transform(GraphName source, GraphName target) {
Preconditions.checkNotNull(source);
Preconditions.checkNotNull(target);
// This adds support for tf2 while maintaining backward compatibility with tf.
GraphName relativeSource = source.toRelative();
GraphName relativeTarget = target.toRelative();
if (relativeSource.equals(relativeTarget)) {
return new FrameTransform(Transform.identity(), relativeSource, relativeTarget, null);
}
FrameTransform sourceToRoot = transformToRoot(relativeSource);
FrameTransform targetToRoot = transformToRoot(relativeTarget);
if (sourceToRoot == null && targetToRoot == null) {
return null;
}
if (sourceToRoot == null) {
if (targetToRoot.getTargetFrame().equals(relativeSource)) {
// relativeSource is root.
return targetToRoot.invert();
} else {
return null;
}
}
if (targetToRoot == null) {
if (sourceToRoot.getTargetFrame().equals(relativeTarget)) {
// relativeTarget is root.
return sourceToRoot;
} else {
return null;
}
}
if (sourceToRoot.getTargetFrame().equals(targetToRoot.getTargetFrame())) {
// Neither relativeSource nor relativeTarget is root and both share the
// same root.
Transform transform =
targetToRoot.getTransform().invert().multiply(sourceToRoot.getTransform());
return new FrameTransform(transform, relativeSource, relativeTarget, sourceToRoot.getTime());
}
// No known transform.
return null;
}
/**
* Returns the most recent {@link FrameTransform} for target {@code source}.
*
* @param source the frame to look up
* @return the most recent {@link FrameTransform} for {@code source} or {@code null} if no
* transform for {@code source} is available
*/
public FrameTransform lookUp(GraphName source) {
Preconditions.checkNotNull(source);
// This adds support for tf2 while maintaining backward compatibility with tf.
return getLatest(source.toRelative());
}