def main():
board = apriltagboard.AprilTagBoard("../config/apriltag_real.yml", "../config/tagId.csv")
root_dir = "../real_data/12_16_20_55"
img_list = getImgList(root_dir)
# if len(img_list)<10:
# print("number of img not enough")
# return 0
image_size = tuple([1920, 1080])
image_list = []
obj_list = []
print(board.GetBoardAllPoints())
for i in range(len(img_list)):
image = cv2.imread(img_list[i])
flag,imagepoints, objpoints = board.getObjImgPointList(image)
if not flag:
continue
image_list.append(imagepoints)
obj_list.append(objpoints)
rme, intrinsic, discoff = board.intrinsic(image_list, obj_list, image_size)
print(intrinsic, rme)
fs = cv2.FileStorage(root_dir + "/intrinsic_rgb.yml", cv2.FileStorage_WRITE)
fs.write("cameraMatrix", intrinsic)
fs.write("discoff", discoff)
fs.release()
return 0
def main():
board = apriltagboard.AprilTagBoard("../config/apriltag_real.yml",
"../config/tagId.csv")
#root_dir = "../real_data/12_16_20_55"
root_dir = "../real_data/intrinisc_data"
img_list, depth_image_list = getImgList(root_dir)
if len(img_list) != len(depth_image_list):
print("numer of img and depth not same")
return 0
img_list.sort()
depth_image_list.sort()
print(img_list)
print(depth_image_list)
# if len(img_list)<10:
# print("number of img not enough")
# return 0
depth_list = []
image_size = tuple([1920, 1080])
image_list = []
obj_list = []
for i in range(len(img_list)):
image = cv2.imread(img_list[i])
flag, objpoints, imagepoints = board.getObjImgPointList(image)
if not flag:
continue
depth_points, rejectids = depthUtils.get_depth(imagepoints,
depth_image_list[i])
if len(rejectids) == np.size(len(depth_points)):
continue
image_list.append(np.delete(imagepoints, rejectids, axis=0))
depth_list.append(np.delete(depth_points, rejectids, axis=0))
obj_list.append(np.delete(objpoints, rejectids, axis=0))
rme, intrinsic, discoff = board.intrinsic(image_list, obj_list, image_size)
print(intrinsic, rme)
intrinsic, discoff = board.intrinsic_depth_opt4(obj_list, image_list,
depth_list, intrinsic,
discoff)
print(intrinsic, rme)
for i in range(len(obj_list)):
extrinsic = board.extrinsic(image_list[i], obj_list[i], intrinsic,
discoff)
extrinsic_opt = board.extrinsic_opt(intrinsic, discoff, extrinsic,
image_list[i], obj_list[i])
flag, extrinsic_depth = board.extrisic_depth(obj_list[i],
image_list[i],
depth_list[i], intrinsic,
discoff)
print("extrinsic", extrinsic)
print("extrinsic_opt", extrinsic_opt)
print("extrinsic_depth", extrinsic_depth)
fs = cv2.FileStorage(root_dir + "/intrinsic.yml", cv2.FileStorage_WRITE)
fs.write("cameraMatrix", intrinsic)
print(intrinsic)
fs.write("discoff", discoff)
fs.release()
return 0
from board import apriltagboard
ros_cv2_path = '/opt/ros/kinetic/lib/python2.7/dist-packages'
if ros_cv2_path in sys.path:
sys.path.remove(ros_cv2_path)
import cv2
sys.path.append(ros_cv2_path)
else:
import cv2
from camera import kinect
from robot import aubo
import numpy as np
from utils import depthUtils
camera = kinect.Kinect(1026, "../config/intrinsic_1216.yml")
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")
flag, img, depth = camera.get_rgb_depth_image()
flag, objpoint, imgpoint = board.getObjImgPointList(img, verbose=0)
camerapose = board.extrinsic(imgpoint, objpoint, camera.intrinsic, camera.dist)
camerapose1 = board.extrinsic_opt(camera.intrinsic, camera.dist, camerapose,
imgpoint, objpoint)
depth_points, rejectids = depthUtils.get_depth2(imgpoint, depth)
imgpoint = np.delete(imgpoint, rejectids, axis=0)
depth_points = np.delete(depth_points, rejectids, axis=0)
objpoint = np.delete(objpoint, rejectids, axis=0)
flag, extrinsic = board.extrisic_depth(objpoint, imgpoint, depth_points,
camera.intrinsic, camera.dist)
def main():
board = apriltagboard.AprilTagBoard("../config/apriltag.yml", "../config/tagId.csv")
verbose = 0
root_dir ="../offline_data/handeye2"
image_list,depth_list = getImgList(root_dir)
if len(image_list)!= len(depth_list):
print("numer of img and depth not same")
image_list.sort()
depth_list.sort()
number = len(image_list)
fs = cv2.FileStorage(root_dir+"/intrinsic.yml", cv2.FILE_STORAGE_READ)
camera_matrix = fs.getNode("cameraMatrix").mat()
discoff = fs.getNode("discoff").mat()
imag_points_list = []
obj_points_list = []
depth_points_list = []
extrinsic_list = []
for i in range(number):
image = cv2.imread(image_list[i])
flag,imagepoints, objpoints = board.getObjImgPointList(image)
# depth_points, rejectids = depthUtils.get_depth(imagepoints, depth_list[i])
# imagepoints = np.delete(imagepoints, rejectids, axis=0)
# depth_points = np.delete(depth_points, rejectids, axis=0)
# objpoints = np.delete(objpoints, rejectids, axis=0)
extrinsic = board.extrinsic(objpoints,imagepoints,camera_matrix,discoff)
extrinsic = board.extrinsic_opt(camera_matrix,discoff,extrinsic,imagepoints,objpoints)
extrinsic_list.append(extrinsic)
imag_points_list.append(imagepoints)
obj_points_list.append(objpoints)
# depth_points_list.append(depth_points)
robot_pose = getPose2(os.path.join(root_dir,"pose.txt"))
while(True):
A, B = motion.motion_axxb(robot_pose, extrinsic_list)
Tsai_handeye = tsai.calibration(A, B)
dual_handeye = dual.calibration(A, B)
rx_handeye = rx.refine(dual_handeye, robot_pose, extrinsic_list, board.GetBoardAllPoints())
rx_error = rx.proj_error(rx_handeye, robot_pose, extrinsic_list, board.GetBoardAllPoints())
q = transforms3d.quaternions.mat2quat(rx_handeye[:3, :3])
handeye_q = np.array([q[1],q[2],q[3],q[0]])
handeye_result = np.append(handeye_q,rx_handeye[:3, 3])
print("camera_matrix:", camera_matrix)
print("dicoff:", discoff)
print("rx", q, rx_handeye[:3, 3])
A, B = motion.motion_axyb(robot_pose, extrinsic_list)
li_x, li_y = li.calibration(A, B)
rz_x, rz_y = rz.refine(li_x, li_y, robot_pose, extrinsic_list, board.GetBoardAllPoints())
q = transforms3d.quaternions.mat2quat(rz_x[:3, :3])
print("rz", q, rz_x[:3, 3])
rz_error = rz.proj_error_each_point(rz_x, rz_y, robot_pose, extrinsic_list, board.GetBoardAllPoints())
x,y = np.where(rz_error.reshape([1,-1])>0.005)
if y.shape[0]==0:
break
x,y = np.where(rz_error.reshape([1,-1])==np.max(rz_error))
del robot_pose[y[0]]
del extrinsic_list[y[0]]
rx_error = rx.proj_error(rx_handeye, robot_pose, extrinsic_list, board.GetBoardAllPoints())
q = transforms3d.quaternions.mat2quat(rx_handeye[:3, :3])
print("rme",np.mean(np.abs(rx_error)))
handeye_q = np.array([q[1], q[2], q[3], q[0]])
handeye_result = np.append(handeye_q, rx_handeye[:3, 3])
fs = cv2.FileStorage(root_dir + "/handeye.yml", cv2.FileStorage_WRITE)
fs.write("cameraMatrix", camera_matrix)
fs.write("discoff", discoff)
fs.write("handeye", rx_handeye)
fs.release()
return
