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