def getErrorList(fileList, euler_hx_gt, t_hx_gt, euler_hy_gt, t_hy_gt):
euler_hx_error_mutlti = np.array([])
t_hx_error_mutlti = np.array([])
euler_hy_error_mutlti = np.array([])
t_hy_error_mutlti = np.array([])
for file in fileList:
result = utils.json_load(file)
if not result[0]["simu"] == 0.002:
continue
euler_hx_error = np.array([])
t_hx_error = np.array([])
euler_hy_error = np.array([])
t_hy_error = np.array([])
for i in range(5, 26):
for dict in result:
if dict["image_number"] == i:
Hx_temp = dict[Hx]
Hy_temp = dict[Hy]
euler_hx_temp = transforms3d.euler.mat2euler(Hx_temp[:3, :3])
t_hx_temp = Hx_temp[:3, 3]
euler_hx_error_rx = min(
abs(euler_hx_temp[0] - euler_hx_gt[0]),
abs(euler_hx_temp[0] - euler_hx_gt[0] + 2 * 3.1415926),
abs(euler_hx_temp[0] - euler_hx_gt[0] - 2 * 3.1415926))
euler_hx_error_ry = min(
abs(euler_hx_temp[1] - euler_hx_gt[1]),
abs(euler_hx_temp[1] - euler_hx_gt[1] + 2 * 3.1415926),
abs(euler_hx_temp[1] - euler_hx_gt[1] - 2 * 3.1415926))
euler_hx_error_rz = min(
abs(euler_hx_temp[2] - euler_hx_gt[2]),
abs(euler_hx_temp[2] - euler_hx_gt[2] + 2 * 3.1415926),
abs(euler_hx_temp[2] - euler_hx_gt[2] - 2 * 3.1415926))
if max(euler_hx_error_rx, euler_hx_error_ry,
euler_hx_error_rz) > 0.08:
print(file)
euler_hx_error = np.append(
euler_hx_error,
np.mean(
np.array([
euler_hx_error_rx, euler_hx_error_ry, euler_hx_error_rz
])))
t_hx_error = np.append(t_hx_error,
np.mean(np.abs(t_hx_temp - t_hx_gt)))
euler_hy_temp = transforms3d.euler.mat2euler(Hy_temp[:3, :3])
# if np.linalg.norm(euler_hy_temp - euler_hy_gt) > np.linalg.norm(euler_hy_temp + euler_hy_gt):
# euler_hy_temp = -euler_hy_temp
t_hy_temp = Hy_temp[:3, 3]
#euler_hy_error = np.append(euler_hy_error, np.abs(euler_hy_temp - euler_hy_gt))
euler_hy_error_rx = min(
abs(euler_hy_temp[0] - euler_hy_gt[0]),
abs(euler_hy_temp[0] - euler_hy_gt[0] + 2 * 3.1415926),
abs(euler_hy_temp[0] - euler_hy_gt[0] - 2 * 3.1415926))
euler_hy_error_ry = min(
abs(euler_hy_temp[1] - euler_hy_gt[1]),
abs(euler_hy_temp[1] - euler_hy_gt[1] + 2 * 3.1415926),
abs(euler_hy_temp[1] - euler_hy_gt[1] - 2 * 3.1415926))
euler_hy_error_rz = min(
abs(euler_hy_temp[2] - euler_hy_gt[2]),
abs(euler_hy_temp[2] - euler_hy_gt[2] + 2 * 3.1415926),
abs(euler_hy_temp[2] - euler_hy_gt[2] - 2 * 3.1415926))
euler_hy_error = np.append(
euler_hy_error,
np.mean(
np.array([
euler_hy_error_rx, euler_hy_error_ry, euler_hy_error_rz
])))
t_hy_error = np.append(t_hy_error,
np.mean(np.abs(t_hy_temp - t_hy_gt)))
euler_hx_error_mutlti = np.append(euler_hx_error_mutlti,
euler_hx_error)
t_hx_error_mutlti = np.append(t_hx_error_mutlti, t_hx_error)
euler_hy_error_mutlti = np.append(euler_hy_error_mutlti,
euler_hy_error)
t_hy_error_mutlti = np.append(t_hy_error_mutlti, t_hy_error)
euler_hx_error_mutlti = euler_hx_error_mutlti.reshape(
[-1, np.size(x_range)])
t_hx_error_mutlti = t_hx_error_mutlti.reshape([-1, np.size(x_range)])
euler_hy_error_mutlti = euler_hy_error_mutlti.reshape(
[-1, np.size(x_range)])
t_hy_error_mutlti = t_hy_error_mutlti.reshape([-1, np.size(x_range)])
return euler_hx_error_mutlti, t_hx_error_mutlti, euler_hy_error_mutlti, t_hy_error_mutlti
def getErrorList(rootdir,fileList,euler_hx_gt,t_hx_gt,euler_hy_gt,t_hy_gt,method_list,name_list):
# hx_df_euler = pd.DataFrame(columns=('image number', 'euler_error', 'method'))
# hy_df_euler = pd.DataFrame(columns=('image number', 'euler_error', 'method'))
# hx_df_t = pd.DataFrame(columns=('image number', 't_error', 'method'))
# hy_df_t = pd.DataFrame(columns=('image number', 't_error', 'method'))
image_number = []
hx_euler = []
hy_euler = []
hx_t= []
hy_t = []
method = []
for file in fileList:
name=None
for i in range(len(method_list)):
if file.startswith(method_list[i]):
name = name_list[i]
if name is None:
continue
result = utils.json_load(os.path.join(rootdir,file))
if not result[0]["simu"]==0.002:
continue
for dict in result:
number = dict["image_number"]
if number<5:
continue
Hx_temp = dict[Hx]
Hy_temp = dict[Hy]
euler_hx_temp = transforms3d.euler.mat2euler(Hx_temp[:3, :3])
t_hx_temp = Hx_temp[:3, 3]
euler_hx_error_rx = min(abs(euler_hx_temp[0] - euler_hx_gt[0]),
abs(euler_hx_temp[0] - euler_hx_gt[0] + 2 * 3.1415926),
abs(euler_hx_temp[0] - euler_hx_gt[0] - 2 * 3.1415926))
euler_hx_error_ry = min(abs(euler_hx_temp[1] - euler_hx_gt[1]),
abs(euler_hx_temp[1] - euler_hx_gt[1] + 2 * 3.1415926),
abs(euler_hx_temp[1] - euler_hx_gt[1] - 2 * 3.1415926))
euler_hx_error_rz = min(abs(euler_hx_temp[2] - euler_hx_gt[2]),
abs(euler_hx_temp[2] - euler_hx_gt[2] + 2 * 3.1415926),
abs(euler_hx_temp[2] - euler_hx_gt[2] - 2 * 3.1415926))
if max(euler_hx_error_rx,euler_hx_error_ry,euler_hx_error_rz)>0.08 :
print(file)
continue
hx_euler_error = np.mean(np.array([euler_hx_error_rx, euler_hx_error_ry, euler_hx_error_rz]))
hx_t_error = np.mean(np.abs(t_hx_temp - t_hx_gt))
image_number.append(number)
hx_euler.append(hx_euler_error)
hx_t.append(hx_t_error)
method.append(name)
# hx_df_euler = hx_df_euler.append({'image number':number,"euler_error":hx_euler_error,"method":name},ignore_index=True)
# hx_df_t = hx_df_t.append({'image number':number, 't_error':hx_t_error, 'method':name},ignore_index=True)
euler_hy_temp = transforms3d.euler.mat2euler(Hy_temp[:3, :3])
t_hy_temp = Hy_temp[:3, 3]
euler_hy_error_rx = min(abs(euler_hy_temp[0] - euler_hy_gt[0]),
abs(euler_hy_temp[0] - euler_hy_gt[0] + 2 * 3.1415926),
abs(euler_hy_temp[0] - euler_hy_gt[0] - 2 * 3.1415926))
euler_hy_error_ry = min(abs(euler_hy_temp[1] - euler_hy_gt[1]),
abs(euler_hy_temp[1] - euler_hy_gt[1] + 2 * 3.1415926),
abs(euler_hy_temp[1] - euler_hy_gt[1] - 2 * 3.1415926))
euler_hy_error_rz = min(abs(euler_hy_temp[2] - euler_hy_gt[2]),
abs(euler_hy_temp[2] - euler_hy_gt[2] + 2 * 3.1415926),
abs(euler_hy_temp[2] - euler_hy_gt[2] - 2 * 3.1415926))
hy_euler_error = np.mean(np.array([euler_hy_error_rx, euler_hy_error_ry, euler_hy_error_rz]))
hy_t_error = np.mean(np.abs(t_hy_temp - t_hy_gt))
#
hy_euler.append(hy_euler_error)
hy_t.append(hy_t_error)
hx_df_euler = pd.DataFrame({"image number":image_number,"error":hx_euler,"method":method})
hx_df_t = pd.DataFrame({"image number":image_number,"error":hx_t,"method":method})
hy_df_euler = pd.DataFrame({"image number":image_number,"error":hy_euler,"method":method})
hy_df_t = pd.DataFrame({"image number":image_number,"error":hy_t,"method":method})
return hx_df_euler,hy_df_euler,hx_df_t,hy_df_t
def getErrorList(fileList, euler_camera2end_gt, t_camera2end_gt,
euler_obj2base_gt, t_obj2base_gt):
euler_camera2end_error_mutlti = np.array([])
t_camera2end_error_mutlti = np.array([])
euler_obj2base_error_mutlti = np.array([])
t_obj2base_error_mutlti = np.array([])
for file in fileList:
result = utils.json_load(file)
if not result[0]["simu"] == 0.002:
continue
euler_camera2end_error = np.array([])
t_camera2end_error = np.array([])
euler_obj2base_error = np.array([])
t_obj2base_error = np.array([])
for i in range(5, 31):
for dict in result:
if dict["image_number"] == i:
Hcamera2end_temp = dict["Hcamera2end"]
Hobj2base_temp = dict["Hobj2base"]
euler_camera2end_temp = transforms3d.euler.mat2euler(
Hcamera2end_temp[:3, :3])
t_camera2end_temp = Hcamera2end_temp[:3, 3]
euler_camera2end_error_rx = min(
abs(euler_camera2end_temp[0] - euler_camera2end_gt[0]),
abs(euler_camera2end_temp[0] - euler_camera2end_gt[0] +
2 * 3.1415926),
abs(euler_camera2end_temp[0] - euler_camera2end_gt[0] -
2 * 3.1415926))
euler_camera2end_error_ry = min(
abs(euler_camera2end_temp[1] - euler_camera2end_gt[1]),
abs(euler_camera2end_temp[1] - euler_camera2end_gt[1] +
2 * 3.1415926),
abs(euler_camera2end_temp[1] - euler_camera2end_gt[1] -
2 * 3.1415926))
euler_camera2end_error_rz = min(
abs(euler_camera2end_temp[2] - euler_camera2end_gt[2]),
abs(euler_camera2end_temp[2] - euler_camera2end_gt[2] +
2 * 3.1415926),
abs(euler_camera2end_temp[2] - euler_camera2end_gt[2] -
2 * 3.1415926))
euler_camera2end_error = np.append(
euler_camera2end_error,
np.array([
euler_camera2end_error_rx, euler_camera2end_error_ry,
euler_camera2end_error_rz
]))
t_camera2end_error = np.append(
t_camera2end_error,
np.abs(t_camera2end_temp - t_camera2end_gt))
euler_obj2base_temp = transforms3d.euler.mat2euler(
Hobj2base_temp[:3, :3])
# if np.linalg.norm(euler_obj2base_temp - euler_obj2base_gt) > np.linalg.norm(euler_obj2base_temp + euler_obj2base_gt):
# euler_obj2base_temp = -euler_obj2base_temp
t_obj2base_temp = Hobj2base_temp[:3, 3]
#euler_obj2base_error = np.append(euler_obj2base_error, np.abs(euler_obj2base_temp - euler_obj2base_gt))
euler_obj2base_error_rx = min(
abs(euler_obj2base_temp[0] - euler_obj2base_gt[0]),
abs(euler_obj2base_temp[0] - euler_obj2base_gt[0] +
2 * 3.1415926),
abs(euler_obj2base_temp[0] - euler_obj2base_gt[0] -
2 * 3.1415926))
euler_obj2base_error_ry = min(
abs(euler_obj2base_temp[1] - euler_obj2base_gt[1]),
abs(euler_obj2base_temp[1] - euler_obj2base_gt[1] +
2 * 3.1415926),
abs(euler_obj2base_temp[1] - euler_obj2base_gt[1] -
2 * 3.1415926))
euler_obj2base_error_rz = min(
abs(euler_obj2base_temp[2] - euler_obj2base_gt[2]),
abs(euler_obj2base_temp[2] - euler_obj2base_gt[2] +
2 * 3.1415926),
abs(euler_obj2base_temp[2] - euler_obj2base_gt[2] -
2 * 3.1415926))
euler_obj2base_error = np.append(
euler_obj2base_error,
np.array([
euler_obj2base_error_rx, euler_obj2base_error_ry,
euler_obj2base_error_rz
]))
t_obj2base_error = np.append(
t_obj2base_error, np.abs(t_obj2base_temp - t_obj2base_gt))
euler_camera2end_error = euler_camera2end_error.reshape([-1, 3])
t_camera2end_error = t_camera2end_error.reshape([-1, 3])
euler_obj2base_error = euler_obj2base_error.reshape([-1, 3])
t_obj2base_error = t_obj2base_error.reshape([-1, 3])
euler_camera2end_error_mutlti = np.append(
euler_camera2end_error_mutlti, euler_camera2end_error)
t_camera2end_error_mutlti = np.append(t_camera2end_error_mutlti,
t_camera2end_error)
euler_obj2base_error_mutlti = np.append(euler_obj2base_error_mutlti,
euler_obj2base_error)
t_obj2base_error_mutlti = np.append(t_obj2base_error_mutlti,
t_obj2base_error)
euler_camera2end_error_mutlti = euler_camera2end_error_mutlti.reshape(
[-1, 26, 3])
t_camera2end_error_mutlti = t_camera2end_error_mutlti.reshape([-1, 26, 3])
euler_obj2base_error_mutlti = euler_obj2base_error_mutlti.reshape(
[-1, 26, 3])
t_obj2base_error_mutlti = t_obj2base_error_mutlti.reshape([-1, 26, 3])
euler_camera2end_error_mean = np.mean(euler_camera2end_error_mutlti,
axis=0)
t_camera2end_error_mean = np.mean(t_camera2end_error_mutlti, axis=0)
euler_obj2base_error_mean = np.mean(euler_obj2base_error_mutlti, axis=0)
t_obj2base_error_mean = np.mean(t_obj2base_error_mutlti, axis=0)
return euler_camera2end_error_mean, t_camera2end_error_mean, euler_obj2base_error_mean, t_obj2base_error_mean
from utils import depthUtils
from board import apriltagboard
import numpy as np
def get_robot_pose(Hcam2end, Hpencil2cam, cur_Hend2base, cur_Hobj2camera,
point):
Hobj2pencil = np.identity(4)
Hobj2pencil[:3, 3] = -point[:]
Hrobot = np.dot(
np.dot(cur_Hend2base, np.dot(Hcam2end, cur_Hobj2camera)),
np.linalg.inv(np.dot(Hcam2end, np.dot(Hpencil2cam, Hobj2pencil))))
return Hrobot
result = utils.json_load("../real_data/2020_12_17_17_59/result.json")
#result = utils.json_load("../real_data/2020_12_16_12_17/result.json")
Hcam2end = result[-1]["Hcamera2end"]
board = apriltagboard.AprilTagBoard("../config/apriltag_paper.yml",
"../config/tagId.csv")
camera = kinect.Kinect(1026, "../config/intrinsic_1217.yml")
robot = aubo.robot(port=1025)
Hobj2base = result[-1]["Hobj2base"]
border_center = np.array([(board.tag_size + board.markerSeparation) * 3,
(board.tag_size + board.markerSeparation) * 2, 0,
1]).reshape([4, 1])
border_cneter_ = np.dot(Hobj2base, border_center)
width = (board.tag_size + board.markerSeparation) * 7
height = (board.tag_size + board.markerSeparation) * 5
q = transforms3d.quaternions.mat2quat(Hobj2base[:3, :3])
robot.build_board(border_cneter_[0, 0], border_cneter_[1, 0],
else:
import cv2
from camera import kinect
from robot import aubo
import numpy as np
from utils import depthUtils
from auto import utils
'''
get Hpencil2cam
'''
board = apriltagboard.AprilTagBoard("../config/apriltag_real.yml",
"../config/tagId.csv")
# camera = kinect.Kinect(1026,"../config/intrinsic_rgb.yml")
# camera = kinect.Kinect(1026,"../real_data/intrinisc_data/intrinsic.yml")
# camera = kinect.Kinect(1026,"../config/intrinsic_1217.yml")
result = utils.json_load("../real_data/2020_12_19_15_30/result.json")
#result = utils.json_load("../real_data/2020_12_16_12_17/result.json")
Hcam2end = result[-1]["Hcamera2end"]
Hobj2base = result[-1]["Hobj2base"]
robot = aubo.robot(1025)
#camerapose1 = board.extrinsic_opt(camera.intrinsic, camera.dist,extrinsic_depth,imgpoint,objpoint)
falg, robot_pose = robot.get_pose()
pencil_end = np.array([-board.tag_size / 2, -board.tag_size / 2, 0])
Hobj2pencil = np.identity(4)
Hobj2pencil[:3, 3] = -pencil_end[:]
Hpencil2end = np.dot(np.dot(np.linalg.inv(robot_pose), Hobj2base),
np.linalg.inv(Hobj2pencil))
print(Hpencil2end)
fp = cv2.FileStorage("../config/Hpencil2end.yml", cv2.FileStorage_WRITE)
def getErrorList(fileList,euler_camera2end_gt,t_camera2end_gt,euler_obj2base_gt,t_obj2base_gt):
euler_camera2end_error_mutlti = np.array([])
t_camera2end_error_mutlti = np.array([])
euler_obj2base_error_mutlti = np.array([])
t_obj2base_error_mutlti = np.array([])
noise_data = []
for j in range(6):
noise_data.append([])
for file in fileList:
result = utils.json_load(file)
for dict in result:
if not dict["image_number"] ==20:
continue
index = int(dict["simu"]/0.002)
noise_data[index].append(dict)
for group in noise_data:
euler_camera2end_error = np.array([])
t_camera2end_error = np.array([])
euler_obj2base_error = np.array([])
t_obj2base_error = np.array([])
for data in group:
Hcamera2end_temp = data["Hcamera2end"]
Hobj2base_temp = data["Hobj2base"]
euler_camera2end_temp = transforms3d.euler.mat2euler(Hcamera2end_temp[:3, :3])
t_camera2end_temp = Hcamera2end_temp[:3, 3]
euler_camera2end_error_rx = min(abs(euler_camera2end_temp[0] - euler_camera2end_gt[0]),
abs(euler_camera2end_temp[0] - euler_camera2end_gt[0] + 2 * 3.1415926),
abs(euler_camera2end_temp[0] - euler_camera2end_gt[0] - 2 * 3.1415926))
euler_camera2end_error_ry = min(abs(euler_camera2end_temp[1] - euler_camera2end_gt[1]),
abs(euler_camera2end_temp[1] - euler_camera2end_gt[1] + 2 * 3.1415926),
abs(euler_camera2end_temp[1] - euler_camera2end_gt[1] - 2 * 3.1415926))
euler_camera2end_error_rz = min(abs(euler_camera2end_temp[2] - euler_camera2end_gt[2]),
abs(euler_camera2end_temp[2] - euler_camera2end_gt[2] + 2 * 3.1415926),
abs(euler_camera2end_temp[2] - euler_camera2end_gt[2] - 2 * 3.1415926))
euler_camera2end_error = np.append(euler_camera2end_error, np.array(
[euler_camera2end_error_rx, euler_camera2end_error_ry, euler_camera2end_error_rz]))
t_camera2end_error = np.append(t_camera2end_error, np.abs(t_camera2end_temp - t_camera2end_gt))
euler_obj2base_temp = transforms3d.euler.mat2euler(Hobj2base_temp[:3, :3])
# if np.linalg.norm(euler_obj2base_temp - euler_obj2base_gt) > np.linalg.norm(euler_obj2base_temp + euler_obj2base_gt):
# euler_obj2base_temp = -euler_obj2base_temp
t_obj2base_temp = Hobj2base_temp[:3, 3]
# euler_obj2base_error = np.append(euler_obj2base_error, np.abs(euler_obj2base_temp - euler_obj2base_gt))
euler_obj2base_error_rx = min(abs(euler_obj2base_temp[0] - euler_obj2base_gt[0]),
abs(euler_obj2base_temp[0] - euler_obj2base_gt[0] + 2 * 3.1415926),
abs(euler_obj2base_temp[0] - euler_obj2base_gt[0] - 2 * 3.1415926))
euler_obj2base_error_ry = min(abs(euler_obj2base_temp[1] - euler_obj2base_gt[1]),
abs(euler_obj2base_temp[1] - euler_obj2base_gt[1] + 2 * 3.1415926),
abs(euler_obj2base_temp[1] - euler_obj2base_gt[1] - 2 * 3.1415926))
euler_obj2base_error_rz = min(abs(euler_obj2base_temp[2] - euler_obj2base_gt[2]),
abs(euler_obj2base_temp[2] - euler_obj2base_gt[2] + 2 * 3.1415926),
abs(euler_obj2base_temp[2] - euler_obj2base_gt[2] - 2 * 3.1415926))
euler_obj2base_error = np.append(euler_obj2base_error, np.array(
[euler_obj2base_error_rx, euler_obj2base_error_ry, euler_obj2base_error_rz]))
t_obj2base_error = np.append(t_obj2base_error, np.abs(t_obj2base_temp - t_obj2base_gt))
euler_camera2end_error = euler_camera2end_error.reshape([-1, 3])
t_camera2end_error = t_camera2end_error.reshape([-1, 3])
euler_obj2base_error = euler_obj2base_error.reshape([-1, 3])
t_obj2base_error = t_obj2base_error.reshape([-1, 3])
euler_camera2end_error_mutlti = np.append(euler_camera2end_error_mutlti, np.mean(euler_camera2end_error,axis=0))
t_camera2end_error_mutlti = np.append(t_camera2end_error_mutlti, np.mean(t_camera2end_error,axis=0))
euler_obj2base_error_mutlti = np.append(euler_obj2base_error_mutlti, np.mean(euler_obj2base_error,axis=0))
t_obj2base_error_mutlti = np.append(t_obj2base_error_mutlti, np.mean(t_obj2base_error,axis=0))
euler_camera2end_error_mutlti = euler_camera2end_error_mutlti.reshape([ 6, 3])
t_camera2end_error_mutlti = t_camera2end_error_mutlti.reshape([6, 3])
euler_obj2base_error_mutlti = euler_obj2base_error_mutlti.reshape([ 6, 3])
t_obj2base_error_mutlti = t_obj2base_error_mutlti.reshape([ 6, 3])
return euler_camera2end_error_mutlti, t_camera2end_error_mutlti, euler_obj2base_error_mutlti, t_obj2base_error_mutlti
method={
"no_Lock":0,
"std":1,
"no_lock_std":3,
#"rme":2,
#"no_lock_rme":4,
"random":5,
"ias":6
}
board = apriltagboard.AprilTagBoard("../config/apriltag.yml", "../config/tagId.csv")
cliend = vrep_connect.getVrep_connect()
camera = Kinect_test.Camera(cliend,"../config/intrinsic_gt.yml")
robot = LBR4p.Robot(cliend)
move_lock = lock(robot,camera)
threads = []
init_robot_pose_list = utils.json_load("../config/init_robot_pose.json")
for j in range(6):
for i in range(30):
simu = j*0.002
robot.set_simu(simu)
print("start to cal {} iters data".format(i))
start = time.time()
# 中间写上代码块
auto_handeye = auto_handeye_calibration_thread.auto_handeye_calibration(board, robot, camera,
"../config/auto_set.yml", move_lock)
auto_handeye.set_init_robot_pose(random.choice(init_robot_pose_list))
auto_handeye.init_handeye()
auto_handeye.handeye_cali()
Hx = auto_handeye.Hx
Hy = auto_handeye.Hy