def opt_error_fun(est_input, *args):
"""
:param est_input: {numpy.array} -- the initial guess of the transformation of Toct and Tneedle
:param args: {tuple} -- the set of tranfermation of Tlink and ICP transformation matrix
:return: error: {float} -- the error between the true transformation and estimated transformation
"""
Roct = so3.from_rpy(est_input[0:3])
toct = est_input[3:6]
Rn = so3.from_rpy(est_input[6:9])
tn = est_input[9:12]
Tlink_set = args[0]
Tneedle2oct_icp = args[1]
fun_list = np.array([])
for i in range(len(Tlink_set)):
fun_list = np.append(
fun_list,
np.multiply(
so3.error(so3.inv(Tneedle2oct_icp[i][0]),
so3.mul(so3.mul(so3.inv(Roct), Tlink_set[i][0]),
Rn)), 1))
fun_list = np.append(
fun_list,
np.multiply(
vectorops.sub(
vectorops.sub([0., 0., 0.],
so3.apply(so3.inv(Tneedle2oct_icp[i][0]),
Tneedle2oct_icp[i][1])),
so3.apply(
so3.inv(Roct),
vectorops.add(vectorops.sub(Tlink_set[i][1], toct),
so3.apply(Tlink_set[i][0], tn)))), 1000))
return fun_list
def taskDifference(self,a,b):
if self.taskType == 'po':
return se3.error(a,b)
elif self.taskType == 'position':
return vectorops.sub(a,b)
elif self.taskType == 'orientation':
return so3.error(a,b)
else:
raise ValueError("Invalid taskType "+self.taskType)
def taskDifference(self, a, b):
if self.taskType == 'po':
return se3.error(a, b)
elif self.taskType == 'position':
return vectorops.sub(a, b)
elif self.taskType == 'orientation':
return so3.error(a, b)
else:
raise ValueError("Invalid taskType " + self.taskType)
def error_analysis(self):
"""
:return: None
"""
Toct = (so3.from_rpy(self.min_res.x[0:3]), self.min_res.x[3:6])
Tneedle = (so3.from_rpy(self.min_res.x[6:9]), self.min_res.x[9:12])
trans_error_list = []
trans_error_mat = []
rot_error_list = []
rot_error_mat = []
redraw_list = []
for i in range(len(self.Tlink_set)):
Toct_needle_est = se3.mul(
se3.mul(se3.inv(Toct), self.Tlink_set[i]), Tneedle)
trans_error = vectorops.sub(
se3.inv(self.trans_list[i])[1], Toct_needle_est[1])
rot_error = so3.error(
se3.inv(self.trans_list[i])[0], Toct_needle_est[0])
trans_error_list.append(vectorops.normSquared(trans_error))
trans_error_mat.append(np.absolute(trans_error))
rot_error_list.append(vectorops.normSquared(rot_error))
rot_error_mat.append(np.absolute(rot_error))
redraw_list.append(
redraw_pcd(self.pcd_list[i], Toct_needle_est)[0])
redraw_list.append(
redraw_pcd(self.pcd_list[i], Toct_needle_est)[1])
redraw_list.append(
create_mesh_coordinate_frame(size=0.0015, origin=[0, 0, 0]))
draw_geometries(redraw_list)
trans_error_list = np.array(trans_error_list)
trans_error_mat = np.array(trans_error_mat)
rot_error_list = np.array(rot_error_list)
rot_error_mat = np.array(rot_error_mat)
rmse_trans = np.sqrt(np.mean(trans_error_list))
rmse_rot = np.sqrt(np.mean(rot_error_list))
print("The squared translation error list is:\n ",
np.sqrt(trans_error_list), "\nAnd the its RMSE is ", rmse_trans)
print("The mean error in XYZ directions is: ",
np.mean(trans_error_mat, axis=0))
print("The squared rotation error list is:\n ",
np.sqrt(rot_error_list), "\nAnd the its RMSE is ", rmse_rot)
print("The mean error in three rotation vectors is: ",
np.mean(rot_error_mat, axis=0))
# np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num +
# "/calibration_error_rmse.npy", np.array([rmse_trans, rmse_rot]), allow_pickle=True)
# np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num +
# "/calibration_error_list.npy", np.array([np.sqrt(trans_error_list),
# np.sqrt(rot_error_list)]), allow_pickle=True)
# np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num +
# "/calibration_error_mat.npy", np.array([np.mean(trans_error_mat, axis=0),
# np.mean(rot_error_mat, axis=0)]), allow_pickle=True)
trans_all2needle(self.Tlink_set, Toct, Tneedle, self.pcd_list)
def compute(self, x, grad_level=0):
self.robot.setConfig(x)
R, _ = self.link.getTransform()
moment = so3.error(R, self.target_R)
# for point x
val = np.array(moment)
if grad_level == 1:
rel_R = so3.from_moment(moment)
J1 = self.robot.orientationJacobian(self.linkid)
m_omega = np.array([
MomentDerivative(moment, rel_R, [1, 0, 0]),
MomentDerivative(moment, rel_R, [0, 1, 0]),
MomentDerivative(moment, rel_R, [0, 0, 1])
]).T
jac = m_omega.dot(J1)
return (val, jac)
return (val, )
def compute(self, x, grad_level=0):
self.robot.setConfig(x)
# p0 = self.link.getWorldPosition([0, 0, 0])
R, p0 = self.link.getTransform()
moment = so3.error(R, self.target_R)
# for point x
val = np.concatenate((np.array(p0) - self.p0, moment))
if grad_level == 1:
rel_R = so3.from_moment(moment)
J0 = self.robot.positionJacobian(self.linkid, lcl_pos=[0, 0, 0])
J1 = self.robot.orientationJacobian(self.linkid)
m_omega = np.array([
MomentDerivative(moment, rel_R, [1, 0, 0]),
MomentDerivative(moment, rel_R, [0, 1, 0]),
MomentDerivative(moment, rel_R, [0, 0, 1])
]).T
jac = np.r_[J0, m_omega.dot(J1)]
return (val, jac)
return (val, )