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()); }