#print('s3 mean is', mean_s3 * 1000, 'mm')
#print('s3 std is', std_s3 * 1000, 'mm')
#print('s3 count is', count_s3)
#print('s1 is', s1b_2)
#print('s2 is', s2b_2)
#print('s3 is', s3b_2)
# solve common base transformation
rot, t = solve_commonbase(s1b, s2b, s3b)
print('rot is', rot)
print('t is', t)
#print('old ball position is', ball)
#ball_tem = np.insert(ball, 3, 1.0)
#ball_new = np.matmul(convert_homo_transform(rot, t), ball_tem)
#print('new ball position is', ball_new)
#s1_tem = np.insert(s1b_2, 3, 1.0)
#print('old s1 is', s1b_2)
#print('new s1 is', np.matmul(convert_homo_transform(rot, t), s1_tem))
kdl_final = convert_Yomi_convention(kdl_tem, rot, t)
kdl_result = base + "\\tracker_kdl.csv"
print('kdl_final is', kdl_final)
Yomiwrite.write_csv_array(kdl_result, kdl_final)
exit()
#result_file = base + folder + "\\Yomisettings.csv"
result_file = base + "\\Yomisettings_debug.csv"
Yomiwrite.write_csv_array(result_file, kdl1, fmt="%.8f")
label='rigid registration mismatch')
plt.scatter(range(len(affine_mismatch_error)),
affine_mismatch_error,
color='g',
label='affine registration mismatch')
plt.scatter(range(len(init_mismatch_error)),
init_mismatch_error,
color='b',
label='initial registration mismatch')
plt.scatter(range(len(curvilinear_mismatch_error)),
curvilinear_mismatch_error,
color='y',
label='curvilinear correction mismatch')
mismatch_rigid_file = 'G:\\My Drive\\Project\\IntraOral Scanner Registration\\rigid_mismatch.csv'
Yomiwrite.write_csv_array(mismatch_rigid_file, rigid_mismatch_error)
mismatch_affine_file = 'G:\\My Drive\\Project\\IntraOral Scanner Registration\\affine_mismatch.csv'
Yomiwrite.write_csv_array(mismatch_affine_file, affine_mismatch_error)
mismatch_init_file = 'G:\\My Drive\\Project\\IntraOral Scanner Registration\\initial_mismatch.csv'
Yomiwrite.write_csv_array(mismatch_init_file, init_mismatch_error)
mismatch_curvilinear_file = 'G:\\My Drive\\Project\\IntraOral Scanner Registration\\curvilinear_mismatch.csv'
Yomiwrite.write_csv_array(mismatch_curvilinear_file,
curvilinear_mismatch_error)
# Plotting for checking principal axes - Start
# fig6 = plt.figure(6)
# plt.scatter(range(len(x1_angle_mismatch_rigid)), x1_angle_mismatch_rigid, color = 'r', label='x1 rigid registration error')
# plt.scatter(range(len(x1_angle_mismatch_affine)), x1_angle_mismatch_affine, color='g', label='x1 affine registration error')
# plt.scatter(range(len(x1_angle_mismatch_init)), x1_angle_mismatch_init, color='b', label='x1 initial registration error')
#
# fig7 = plt.figure(7)
i = 5
cylinder_base, cylinder_axis = check_cylinder_base_full(
accuracy_raw, 1, 7, reference='cylinder')
faro_base, faro_axis = check_cylinder_base_full(faro_raw,
1,
7,
axis='y',
reference='cylinder')
pos_error, height_error, angular_error = estimate_full_relative(
cylinder_base, cylinder_axis, faro_base)
pos_error2, height_error2, angular_error2 = estimate_full_absolute(
cylinder_base, cylinder_axis, faro_base, faro_axis, i)
Yomiwrite.write_csv_array(
base_path + '\\results\\' + 'correction_pos_error' + np.str(i) +
'.txt', pos_error2)
Yomiwrite.write_csv_array(
base_path + '\\results\\' + 'correction_height_error' + np.str(i) +
'.txt', height_error2)
Yomiwrite.write_csv_array(
base_path + '\\results\\' + 'correction_angular_error' + np.str(i) +
'.txt', angular_error2)
plt.show()
exit()
# the code below is working for accuracy, don't delete
i = 0
for item in os.listdir(base_path + '\\stl_measurements'):
i += 1
s1 = Yomiread.read_calibration_measurements(base +
'\\divot_s1_jointangles.m')[:,
6:13]
s2 = Yomiread.read_calibration_measurements(base +
'\\divot_s2_jointangles.m')[:,
6:13]
s3 = Yomiread.read_calibration_measurements(base +
'\\divot_s3_jointangles.m')[:,
6:13]
#s3 = Yomiread.read_calibration_measurements(base + folder + '\\divot_s3_jointangles.m' )[:, 6:13]
# Add 3 degree off on J3
sb_offset = sb
mb_offset = mb
lb_offset = lb
offset = 3.1459 * np.pi / 180
for i in range(len(sb)):
sb_offset[i, :] = sb[i, :] + [0, 0, offset, 0, 0, 0, 0]
for j in range(len(mb)):
mb_offset[i, :] = mb[i, :] + [0, 0, offset, 0, 0, 0, 0]
for j in range(len(lb)):
lb_offset[i, :] = lb[i, :] + [0, 0, offset, 0, 0, 0, 0]
kdl1 = opt_full(sb_offset, sb_bb, mb_offset, mb_bb, lb_offset, lb_bb, s1, s2,
s3)
exit()
#result_file = base + folder + "\\Yomisettings.csv"
result_file = base + "\\Yomisettings_debug.csv"
Yomiwrite.write_csv_array(result_file, kdl1, fmt="%.8f")