code: def pose_inv(pose):
      
  
      # Note, the inverse of a pose matrix is the following
      # [R t; 0 1]^-1 = [R.T -R.T*t; 0 1]
  
      # Intuitively, this makes sense.
      # The original pose matrix translates by t, then rotates by R.
      # We just invert the rotation by applying R-1 = R.T, and also translate back.
      # Since we apply translation first before rotation, we need to translate by
      # -t in the original frame, which is -R-1*t in the new frame, and then rotate back by
      # R-1 to align the axis again.
  
      pose_inv = np.zeros((4, 4))
      pose_inv[:3, :3] = pose[:3, :3].T
      pose_inv[:3, 3] = -pose_inv[:3, :3].dot(pose[:3, 3])
      pose_inv[3, 3] = 1.0
      return pose_inv
docs: Computes the inverse of a homogenous matrix corresponding to the pose of some
      frame B in frame A. The inverse is the pose of frame A in frame B.
  
      Args:
          pose: numpy array of shape (4,4) for the pose to inverse
  
      Returns:
          numpy array of shape (4,4) for the inverse pose