/** Transforms a stamped pose to the given target frame, and returns the result in stampedOut. */ public void transformPose( String targetFrameID, Stamped<Matrix4d> stampedIn, Stamped<Matrix4d> stampedOut) { StampedTransform transform = lookupTransform(targetFrameID, stampedIn.frameID, stampedIn.timeStamp); transform.transformPose(stampedIn.getData(), stampedOut.getData()); stampedOut.frameID = targetFrameID; stampedOut.timeStamp = stampedIn.timeStamp; }
/** * Transforms a stamped point to the given target frame and time, based on a given fixed frame, * and returns the result in stampedOut. */ public void transformPoint( String targetFrameID, Time targetTime, Stamped<Point3d> stampedIn, String fixedFrameID, Stamped<Point3d> stampedOut) { StampedTransform transform = lookupTransform( targetFrameID, targetTime, stampedIn.frameID, stampedIn.timeStamp, fixedFrameID); transform.transformPoint(stampedIn.getData(), stampedOut.getData()); stampedOut.frameID = targetFrameID; stampedOut.timeStamp = stampedIn.timeStamp; }