两个坐标系之间的转换(tf)

Question

• 我有一个 tracepen，一个 3D 跟踪设备。我可以在 ROS 中以 tracepenPose() 的形式获取它的数据，但是这个 Pose 是 w.r.t.一些未知的 tracepenOrig，但我想要它 w.r.t.世界框架。
• 所以我想找到一个校准过程，让我在世界和 tracepen_Orig tf(world,tracepenOrig) 之间转换。
• 我有一个校准插座，我知道它相对于世界坐标系的姿势，所以我知道 tf(world,socket)。
• 当我将 tracepen 插入校准插座时，tf(world,tracepenPlugged) 必须与 tf(world,socket) 相同。

解决方案尝试 这看起来好多了。

• 我把描笔插入插座
• 我将 tracepen 的 Pose 发布为 tf(tracepenOrig,tracepenPlugged)
• 我将世界到套接字的转换发布为 tf(world, socket)
• 在插入状态下，我知道 tf(world,tracepenPlugged) 必须与 tf(world,socket) 相同
• 或者作为齐次变换：matrix_world_tracepenPlugged = matrix_world_socket
• 我得到从 world 到 tracepenOrigin 的完全同质转换 as
• matrix_world_tracepenOrigin = matrix_world_tracepenPlugged@matrix_tracepenPlugged_tracepenOrigin
• 由此我可以发布 tf(world,tracepenOrigin)
• 由此我可以将 tracepenPose() 发布为 tf(tracepenOrigin, tracepen)

它几乎可以工作。 发布了一个 tracepenOrigin 框架，其中 tracepen 作为子框架。 插入触控笔时，触控笔框架与插座框架重合。

但仍然： 移动描笔已关闭。

校准后一切看起来都很好

移动插座 x+0.2（tracepen 仍然插入）后，tracepen 向另一个方向移动。它应该仍在插座中。 两个框架的旋转也不相同。

代码

  def calibration(self):
    # I have a tracepen, a 3D tracked device. I can get its Data in ROS as tracepenPose(), but this Pose is w.r.t. some unknown_tracepenOrigin.
    # So i want to find a calibration which transforms the data from this unknown frame to my world frame.

    # Therefore i am looking for the tf(world,tracepenOrigin).
    
    
    # The tracepenPose is w.r.t. some unknown tracepenOrigin. The tracepenPose is the transformation from the tracepenOriginin of this frame to the tracepen. 
    # As the tracepen is plugged, it is called tracepenPlugged.
    rawPluggedPose = self.tracepenPose()
    tf_tracepenOrigin_tracepenPlugged = self.calculate_tf("tracepenOrigin", "tracepenPlugged", rawPluggedPose)
    self.tf_broadcaster.sendTransform(tf_tracepenOrigin_tracepenPlugged)
    rospy.sleep(0.2)
    
    # Look up the inverse trafo from tracepenPlugged to tracepenOrigin.
    tf_tracepenPlugged_tracepenOrigin = self.tf_buffer.lookup_transform("tracepenPlugged", "tracepenOrigin",  rospy.Time())
    # Calculate the matrix from this tf.
    matrix_tracepenPlugged_tracepenOrigin = self.matrix_from_tf(tf_tracepenPlugged_tracepenOrigin)
    
    
    # I have a calibration socket which Pose i know relative to the world frame, so i know the tf(world,socket).
    tf_world_socket = self.tf_buffer.lookup_transform("world", "socket",  rospy.Time())
    matrix_world_socket = self.matrix_from_tf(tf_world_socket)
    # while plugged, i know that tf(world,tracepenPlugged) must be the same as tf(world,socket).
    # so matrix_world_tracepenPlugged = matrix_world_socket
    matrix_world_tracepenPlugged = matrix_world_socket
    
    self.matrix_world_tracepenOrigin = matrix_world_tracepenPlugged @ matrix_tracepenPlugged_tracepenOrigin
    

  def run(self):
      rate = rospy.Rate(10)
      while not rospy.is_shutdown():

        # Get data from OpenVr
        tracepenPose = self.tracepenPose()
        
        # Get the tracepen_raw_tf and broadcast it
        #tf_tracepen_raw = self.calculate_tf("world", "tracepen_raw", tracepenPose)
        #self.tf_broadcaster.sendTransform(tf_tracepen_raw)
        #self.tf_broadcaster.sendTransform(self.tf_calib)
        
        #self.tf_broadcaster.sendTransform(self.tf_world_tracepenOrigin)
        self.tf_world_tracepenOrigin = self.tf_from_matrix(self.matrix_world_tracepenOrigin, "world", "tracepenOrigin")
        self.tf_broadcaster.sendTransform(self.tf_world_tracepenOrigin)
        
        
        #Tracepen from TF calculations
        tf_tracepenOrigin_tracepen = self.calculate_tf("tracepenOrigin", "tracepen", tracepenPose)
        self.tf_broadcaster.sendTransform(tf_tracepenOrigin_tracepen)
        
        rate.sleep()