def compute_hand_eye_calibration(dq_B_H_vec_inliers, dq_W_E_vec_inliers,
                                 scalar_part_tolerance=1e-2,
                                 enforce_same_non_dual_scalar_sign=True):
  """
  Do the actual hand eye-calibration as described in the referenced paper.
  Assumes the outliers have already been removed and the scalar parts of
  each pair are a match.
  """
  n_quaternions = len(dq_B_H_vec_inliers)

  # Verify that the first pose is at the origin.
  assert np.allclose(dq_B_H_vec_inliers[0].dq,
                     [0., 0., 0., 1.0, 0., 0., 0., 0.],
                     atol=1.e-8), dq_B_H_vec_inliers[0]
  assert np.allclose(dq_W_E_vec_inliers[0].dq,
                     [0., 0., 0., 1.0, 0., 0., 0., 0.],
                     atol=1.e-8), dq_W_E_vec_inliers[0]

  if enforce_same_non_dual_scalar_sign:
    for i in range(n_quaternions):
      dq_W_E = dq_W_E_vec_inliers[i]
      dq_B_H = dq_B_H_vec_inliers[i]
      if ((dq_W_E.q_rot.w < 0. and dq_B_H.q_rot.w > 0.) or
              (dq_W_E.q_rot.w > 0. and dq_B_H.q_rot.w < 0.)):
        dq_W_E_vec_inliers[i].dq = -dq_W_E_vec_inliers[i].dq.copy()

  # 0. Stop alignment if there are still pairs that do not have matching
  # scalar parts.
  for j in range(n_quaternions):
    dq_B_H = dq_W_E_vec_inliers[j]
    dq_W_E = dq_B_H_vec_inliers[j]

    scalar_parts_B_H = dq_B_H.scalar()
    scalar_parts_W_E = dq_W_E.scalar()

    assert np.allclose(scalar_parts_B_H.dq, scalar_parts_W_E.dq,
                       atol=scalar_part_tolerance), (
        "Mismatch of scalar parts of dual quaternion at idx {}:"
        " dq_B_H: {} dq_W_E: {}".format(j, dq_B_H, dq_W_E))

  # 1.
  # Construct 6n x 8 matrix T
  t_matrix = setup_t_matrix(dq_B_H_vec_inliers, dq_W_E_vec_inliers)

  # 2.
  # Compute SVD of T and check if only two singular values are almost equal to
  # zero. Take the corresponding right-singular vectors (v_7 and v_8)
  U, s, V = np.linalg.svd(t_matrix)

  # Check if only the last two singular values are almost zero.
  bad_singular_values = False
  for i, singular_value in enumerate(s):
    if i < 6:
      if singular_value < 5e-1:
        bad_singular_values = True
    else:
      if singular_value > 5e-1:
        bad_singular_values = True
  v_7 = V[6, :].copy()
  v_8 = V[7, :].copy()
  # print("v_7: {}".format(v_7))
  # print("v_8: {}".format(v_8))

  # 3.
  # Compute the coefficients of (35) and solve it, finding two solutions for s.
  u_1 = v_7[0:4].copy()
  u_2 = v_8[0:4].copy()
  v_1 = v_7[4:8].copy()
  v_2 = v_8[4:8].copy()
  # print("u_1: {}, \nu_2: {}, \nv_1: {}, \nv_2: {}".format(u_1, u_2, v_1, v_2))

  a = np.dot(u_1.T, v_1)
  assert a != 0.0, "This would involve division by zero."
  b = np.dot(u_1.T, v_2) + np.dot(u_2.T, v_1)
  c = np.dot(u_2.T, v_2)
  # print("a: {}, b: {}, c: {}".format(a, b, c))
  square_root_term = b * b - 4.0 * a * c

  if square_root_term < -1e-2:
    assert False, "square_root_term is too negative: {}".format(
        square_root_term)
  if square_root_term < 0.0:
    square_root_term = 0.0
  s_1 = (-b + np.sqrt(square_root_term)) / (2.0 * a)
  s_2 = (-b - np.sqrt(square_root_term)) / (2.0 * a)
  # print("s_1: {}, s_2: {}".format(s_1, s_2))

  # 4.
  # For these two s values, compute s^2*u_1^T*u_1 + 2*s*u_1^T*u_2 + u_2^T*u_2
  # From these choose the largest to compute lambda_2 and then lambda_1
  solution_1 = s_1 * s_1 * np.dot(u_1.T, u_1) + 2.0 * \
      s_1 * np.dot(u_1.T, u_2) + np.dot(u_2.T, u_2)
  solution_2 = s_2 * s_2 * np.dot(u_1.T, u_1) + 2.0 * \
      s_2 * np.dot(u_1.T, u_2) + np.dot(u_2.T, u_2)

  if solution_1 > solution_2:
    assert solution_1 > 0.0, solution_1
    lambda_2 = np.sqrt(1.0 / solution_1)
    lambda_1 = s_1 * lambda_2
  else:
    assert solution_2 > 0.0, solution_2
    lambda_2 = np.sqrt(1.0 / solution_2)
    lambda_1 = s_2 * lambda_2
  # print("lambda_1: {}, lambda_2: {}".format(lambda_1, lambda_2))

  # 5.
  # The result is lambda_1*v_7 + lambda_2*v_8
  dq_H_E = DualQuaternion.from_vector(lambda_1 * v_7 + lambda_2 * v_8)
  # Normalize the output, to get rid of numerical errors.
  dq_H_E.normalize()

  if (dq_H_E.q_rot.w < 0.):
    dq_H_E.dq = -dq_H_E.dq.copy()
  return (dq_H_E, s, bad_singular_values)

  Do the actual hand eye-calibration as described in the referenced paper.
  Assumes the outliers have already been removed and the scalar parts of
  each pair are a match.
  
hand_eye_calibration/python/hand_eye_calibration/dual_quaternion_hand_eye_calibration.py