def setup_s_matrix(dq_1, dq_2):
  """This sets up the [6x8] S matrix, see Eq. (31) of the referenced paper.

  S = (skew(I(qr1)+I(qr2)) I(qr1)-I(qr2) 0_{3x3}             0_{3x1}      )
      (skew(I(qt1)+I(qt2)) I(qt1)-I(qt2) skew(I(qr1)+I(qr2)) I(qr1)-I(qr2))
  I(q) denotes the vector of the imaginary components of a quaternion.
  Note: The order of the blocks switched as we are using q = [x y z w]^T
  instead of q = [w x y z].T.
  """
  scalar_parts_1 = dq_1.scalar()
  scalar_parts_2 = dq_2.scalar()

  assert np.allclose(
      scalar_parts_1.dq, scalar_parts_2.dq,
      atol=5e-2), (
      "\ndq1:\n{},\nscalar_parts_1:\n{},\ndq2:\n{},\nscalar_parts_2:\n{}\n"
      "Scalar parts should always be equal.".format(dq_1, scalar_parts_1, dq_2,
                                                    scalar_parts_2))

  s_matrix = np.zeros([6, 8])
  s_matrix[0:3, 0:3] = skew_from_vector(dq_1.q_rot.q[0:-1] + dq_2.q_rot.q[0:-1])
  s_matrix[0:3, 3] = dq_1.q_rot.q[0:-1] - dq_2.q_rot.q[0:-1]
  s_matrix[3:6, 0:3] = skew_from_vector(dq_1.q_dual.q[0:-1] +
                                        dq_2.q_dual.q[0:-1])
  s_matrix[3:6, 3] = dq_1.q_dual.q[0:-1] - dq_2.q_dual.q[0:-1]
  s_matrix[3:6, 4:7] = skew_from_vector(dq_1.q_rot.q[0:-1] + dq_2.q_rot.q[0:-1])
  s_matrix[3:6, 7] = dq_1.q_rot.q[0:-1] - dq_2.q_rot.q[0:-1]
  # print("S: \n{}".format(s_matrix))

  rank_s_matrix = np.linalg.matrix_rank(s_matrix)
  assert rank_s_matrix <= 6, s_matrix
  return s_matrix.copy()
This sets up the [6x8] S matrix, see Eq. (31) of the referenced paper.

  S = (skew(I(qr1)+I(qr2)) I(qr1)-I(qr2) 0_{3x3}             0_{3x1}      )
      (skew(I(qt1)+I(qt2)) I(qt1)-I(qt2) skew(I(qr1)+I(qr2)) I(qr1)-I(qr2))
  I(q) denotes the vector of the imaginary components of a quaternion.
  Note: The order of the blocks switched as we are using q = [x y z w]^T
  instead of q = [w x y z].T.
  
hand_eye_calibration/python/hand_eye_calibration/dual_quaternion_hand_eye_calibration.py