How to get the pose-transform between poses

[beetleskin]

Hi, I'm looking into euclid and like it - it seems there are not that many robotics-centric spatial-math libs out there for java!

For my use-case, I often have to transform a point from one frame/pose to another frame/pose. For this, I'm used to functions like
poseA.between(poseB) giving me the transform between two poses and pose.inverse() giving me the inverse of the pose's transform - where pose would be in 2D or 3D. How would I do that in euclid?

For some context, I'm coming from the GTSAM way of pose calculus: https://github.com/devbharat/gtsam/blob/master/gtsam/geometry/Pose3.h

Thanks!

[Sylvain]

Hi @beetleskin, thanks for the nice comment.
So we typically use the ReferenceFrame framework to help managing the calculation and maintenance of the transforms between frames. I invite you to take a look at the Frame-objects that facilitate the transformation of geometries from one frame to another such as FramePoint3D. Here's a simple example:

package example;

import us.ihmc.euclid.referenceFrame.FramePoint3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.transform.RigidBodyTransform;

public class ReferenceFrameExample
{
   public static void main(String[] args)
   {
      ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();

      ReferenceFrame frameA = new ReferenceFrame("frameA", worldFrame)
      {
         @Override
         protected void updateTransformToParent(RigidBodyTransform transformToParent)
         {
            transformToParent.getTranslation().set(0.0, 0.1, 0.2);
            transformToParent.getRotation().setToPitchOrientation(0.2);
         }
      };

      ReferenceFrame frameB = new ReferenceFrame("frameB", frameA)
      {
         @Override
         protected void updateTransformToParent(RigidBodyTransform transformToParent)
         {
            transformToParent.getTranslation().set(0.5, 0.6, 0.7);
            transformToParent.getRotation().setToRollOrientation(0.2);
         }
      };

      ReferenceFrame frameC = new ReferenceFrame("frameC", frameA)
      {
         @Override
         protected void updateTransformToParent(RigidBodyTransform transformToParent)
         {
            transformToParent.getTranslation().set(1.0, 2.0, 3.0);
            transformToParent.getRotation().setToYawOrientation(0.2);
         }
      };

      frameA.update(); // Invokes the overridden method updateTransformToParent to process any potential changes.
      frameB.update(); // Invokes the overridden method updateTransformToParent to process any potential changes.
      frameC.update(); // Invokes the overridden method updateTransformToParent to process any potential changes.

      System.out.println("Transform from frameC to frameB");
      System.out.println(frameC.getTransformToDesiredFrame(frameB));

      FramePoint3D point = new FramePoint3D(frameC, 0.1, 0.7, 0.2); // a 3D point expressed in frameC
      System.out.println(point);
      point.changeFrame(frameB); // the same point but now expressed in frameB
      System.out.println(point);
   }
}

I hope that's helpful!

[beetleskin]

It is! But I got a list of thousand poses that define some frames. I don't want to define a full transform-chain via parent-child relations and the ReferenceFrame. I rather have Poses that define some transforms that I want to apply to points (or lines). It seems there is no interface to do that directly (Pose3D::(inverse)transformPoint(somePoint3D)) - I have to convert a Pose3D to a RigidTransform first? Or did I miss a shortcut?

In detail: I got a list of points in global frame, each is rigidly attached to a pose-frame (to a trajectory-point). Now I update the trajectory, and need to update those points as well, so I'd like to do:

• resolve global point in local trajectory pose/frame
• resolve local point in global again via updated trajectory pose/frame

[Sylvain]

Yeah indeed, the Pose3D as it is currently implemented is more about considering it as a geometry object and not as a transform. I can't be used at the moment to transform other geometries.
RigidBodyTransform will be a necessary intermediate :/ can you use the RigidBodyTransform directly?

[beetleskin]

That would also work I guess. I just found it nicer to describe Poses as .. Poses in the first place. Any thoughts about a zero-copy interface between the two?

I'll close, no issue here and questions answered. Thanks! :) And again thanks for closing a huge gap in Java. And maybe, sorry for advertising again, get some inspiration from the GTSAM spatial APIs, its really great for robotics applications of any kind.

[Sylvain]

So could actually just instantiate a RigidBodyTransformBasics directly linking to the rotation and position of the pose. That would essentially be a way to do a zero-copy interface

[Sylvain]

I had heard of GTSAM in the past, I'll take a look at it. Thanks for the link