dual_quaternion_hand_eye_calibration.py 文件源码

def compute_pose_error(pose_A, pose_B):
  """
  Compute the error norm of position and orientation.
  """
  error_position = np.linalg.norm(pose_A[0:3] - pose_B[0:3], ord=2)

  # Construct quaternions to compare.
  quaternion_A = Quaternion(q=pose_A[3:7])
  quaternion_A.normalize()
  if quaternion_A.w < 0:
    quaternion_A.q = -quaternion_A.q
  quaternion_B = Quaternion(q=pose_B[3:7])
  quaternion_B.normalize()
  if quaternion_B.w < 0:
    quaternion_B.q = -quaternion_B.q

  # Sum up the square of the orientation angle error.
  error_angle_rad = angle_between_quaternions(
      quaternion_A, quaternion_B)
  error_angle_degrees = math.degrees(error_angle_rad)
  if error_angle_degrees > 180.0:
    error_angle_degrees = math.fabs(360.0 - error_angle_degrees)

  return (error_position, error_angle_degrees)