コード例 #1
0
 /**
  * @param source the source frame
  * @return the {@link Transform} from {@code source} to root
  */
 @VisibleForTesting
 FrameTransform transformToRoot(GraphName source) {
   FrameTransform result = getLatest(source);
   if (result == null) {
     return null;
   }
   while (true) {
     FrameTransform resultToParent = lookUp(result.getTargetFrame(), result.getTime());
     if (resultToParent == null) {
       return result;
     }
     // Now resultToParent.getSourceFrame() == result.getTargetFrame()
     Transform transform = resultToParent.getTransform().multiply(result.getTransform());
     GraphName target = resultToParent.getTargetFrame();
     result = new FrameTransform(transform, source, target, result.getTime());
   }
 }
コード例 #2
0
 /**
  * @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;
 }