Example #1
0
def main_calculateCameraPose(WD, pose_amount, polar_ang, CAMERA_POSE_PATH):
    # ax = frame3D.make_3D_fig(axSize=200)
    World = frame3D.Frame(np.matlib.identity(4))
    Camera = frame3D.Frame(World.pose)
    orientation = frame3D.Orientation()

    thetas = np.linspace(0, 360, pose_amount,
                         endpoint=False)  # azimuthal angle
    X0 = (np.radians(45), np.radians(45), np.radians(45))  # initial guess
    camOrientations = np.zeros((pose_amount, 6))

    if polar_ang == 0:
        for ind, theta in enumerate(thetas):
            t = theta + 60
            camOrientations[ind, :] = (0.0, 0.0, WD, np.pi, 0.0, np.radians(t))
    else:
        for ind, theta in enumerate(thetas):
            orientation_2 = get_photo_orientation_by_given_shoot_angle(X0,\
                        World.pose, WD, np.radians(polar_ang), np.radians(theta))
            X0 = orientation_2[3:6]
            cmd = orientation.asItself(orientation_2).cmd()
            Camera.transform(cmd, refFrame=World.pose)
            camOrientations[ind, :] = Camera.get_orientation_from_pose(
                Camera.pose, refFrame=World.pose)

    return camOrientations
Example #2
0
def main(DEBUG):
    CONFIG = 'config.yaml'
    with open(CONFIG) as f:
        path = yaml.load(f)

    BASE = path['pegInHole'] if DEBUG else path['ROOT']
    XZ_BASE = path['solveXZ'] if DEBUG else path['ROOT']
    HS_BASE = path['holeSearching'] if DEBUG else path['ROOT']
    TCP_BASE = path['TCPCalibration'] if DEBUG else path['ROOT']

    PIH_goal_PATH = BASE + 'goal/pih_goal.yaml'
    Y_PATH = TCP_BASE + 'goal/Y.yaml'
    X_PATH = XZ_BASE + 'goal/HX.yaml'
    W_PATH = HS_BASE + 'goal/HW.yaml'

    with open(W_PATH) as f:
        W = np.matrix(yaml.load(f))
    with open(Y_PATH) as f:
        Y = np.matrix(yaml.load(f))
    with open(X_PATH) as f:
        X = np.matrix(yaml.load(f))

    # holeID from most precision 1 to 9
    Whole = convertToHole(W, holeID=6)

    WDs = [0.04, 0.02, 0.01, 0.0, -0.01, -0.015, -0.02]
    wayposes = np.zeros((len(WDs), 7))
    for i, WD in enumerate(WDs):
        C = np.matrix([[0.0, 1.0, 0.0, 0.0], [1.0, 0.0, 0.0, 0.0],
                       [0.0, 0.0, -1.0, WD], [0.0, 0.0, 0.0, 1.0]])
        B = Whole * np.linalg.inv(C) * np.linalg.inv(Y)
        pih_goal = as_ROSgoal(B)
        wayposes[i] = pih_goal

    with open(PIH_goal_PATH, 'w') as f:
        yaml.dump(wayposes.tolist(), f, default_flow_style=False)

    # Data visualization
    World = frame3D.Frame(np.matlib.identity(4))
    Base = frame3D.Frame(World.pose)
    Workpiece = frame3D.Frame(Base.pose)
    Flange = frame3D.Frame(Base.pose)
    Tool = frame3D.Frame(Base.pose)
    Camera = frame3D.Frame(Base.pose)
    orientation = frame3D.Orientation()
    ax = frame3D.make_3D_fig(axSize=0.45)

    Flange.transform_by_rotation_mat(B, refFrame=Base.pose)
    Tool.transform_by_rotation_mat(Y, refFrame=Flange.pose)
    Camera.transform_by_rotation_mat(X, refFrame=Flange.pose)
    Workpiece.transform_by_rotation_mat(Whole, refFrame=Base.pose)

    Base.plot_frame(ax, 'Base')
    Flange.plot_frame(ax, 'Flange')
    Tool.plot_frame(ax, 'Tool')
    Camera.plot_frame(ax, 'Camera')
    Workpiece.plot_frame(ax, 'Workpiece ')
Example #3
0
def main(DEBUG):
    CONFIG = 'config.yaml'
    with open(CONFIG) as f:
        path = yaml.load(f)

    BASE = path['TCPCalibration'] if DEBUG else path['ROOT']
    HS_BASE = path['holeSearching'] if DEBUG else path['ROOT']
    XZ_BASE = path['solveXZ'] if DEBUG else path['ROOT']
    Y_PATH = BASE + 'goal/Y.yaml'

    Y = TCPCalibration()
    with open(Y_PATH, 'w') as f:
        yaml.dump(Y.tolist(), f, default_flow_style=False)

    # Data visualization
    X_PATH = XZ_BASE + 'goal/HX.yaml'
    Bc_PATH = HS_BASE + 'goal/Bc.yaml'
    WHole_PATH = HS_BASE + 'goal/Whole.yaml'
    with open(Bc_PATH) as f:
        Bc = np.array(yaml.load(f))
    with open(WHole_PATH) as f:
        Wh = np.array(yaml.load(f))
    with open(X_PATH) as f:
        X = np.array(yaml.load(f))

    World = frame3D.Frame(np.matlib.identity(4))
    Base = frame3D.Frame(World.pose)
    Workpiece = frame3D.Frame(Base.pose)
    Flange = frame3D.Frame(Base.pose)
    Tool = frame3D.Frame(Base.pose)
    Camera = frame3D.Frame(Base.pose)
    orientation = frame3D.Orientation()
    ax = frame3D.make_3D_fig(axSize=0.45)

    Flange.transform_by_rotation_mat(Bc, refFrame=Base.pose)
    Tool.transform_by_rotation_mat(Y, refFrame=Flange.pose)
    Camera.transform_by_rotation_mat(X, refFrame=Flange.pose)
    Workpiece.transform_by_rotation_mat(Wh, refFrame=Base.pose)

    Base.plot_frame(ax, 'Base')
    Flange.plot_frame(ax, 'Flange')
    Tool.plot_frame(ax, 'Tool')
    Camera.plot_frame(ax, 'Camera')
    Workpiece.plot_frame(ax, 'Workpiece ')

    plt.show()
Example #4
0
def main_denso(DEBUG):
    # PATH SETTING
    CONFIG = 'config.yaml'
    with open(CONFIG) as f:
        path = yaml.load(f)

    BASE = path['APose'] if DEBUG else path['ROOT']
    AF_BASE = path['AFocus'] if DEBUG else path['ROOT']
    BF_GOAL = AF_BASE + 'goal/bf_goal.yaml'
    CAMERA_POSE_PATH = BASE + 'goal/camera_pose.yaml'
    ROS_GOAL_PATH = BASE + 'goal/ap_goal.yaml'
    Bs_PATH = BASE + 'goal/Bs.yaml'
    X_PATH = BASE + 'goal/X.yaml'
    Z_PATH = BASE + 'goal/Z.yaml'
    Z2_PATH = BASE + 'goal/Z2.yaml'

    with open(BF_GOAL) as f:
        bf_goal = np.array(yaml.load(f))

    cam_pose = [-0.040, 0, 0.040, 0, 0,
                -90]  # pose wrt Flange in meter: (x,y,z,Rx,Ry,Rz)
    WD = bf_goal[2] - cam_pose[2]  # work distence from camera to pattern(m)

    # First layer of poses
    phi = 0  # polarangle 6
    camOrien1 = main_calculateCameraPose(WD, 6, phi, CAMERA_POSE_PATH)

    # Second layer of poses
    phi = 10  # polarangle
    camOrien2 = main_calculateCameraPose(WD, 12, phi, CAMERA_POSE_PATH)

    # Third layer of poses
    phi = 15  # polarangle
    camOrien3 = main_calculateCameraPose(WD, 6, phi, CAMERA_POSE_PATH)

    camOrientations = np.concatenate((camOrien1, camOrien2, camOrien3), axis=0)

    # data visualization
    World = frame3D.Frame(np.matlib.identity(4))
    Pattern = frame3D.Frame(World.pose)
    Image = frame3D.Frame(Pattern.pose)
    Camera = frame3D.Frame(Pattern.pose)
    Flange = frame3D.Frame(Pattern.pose)
    Base = frame3D.Frame(Pattern.pose)
    Flange_test = frame3D.Frame(Pattern.pose)
    orientation = frame3D.Orientation()
    ax = frame3D.make_3D_fig(axSize=0.45)

    ##################
    # Determine Z
    ##################
    # Base as World coordinate
    Base.plot_frame(ax, 'Base')
    # Base to Flange
    B0 = as_MatrixGoal(bf_goal)
    Flange.transform_by_rotation_mat(B0, refFrame=Base.pose)
    Flange.plot_frame(ax, 'initFlange')
    # Flange to Camera
    cmd_X = orientation.asRad(cam_pose).cmd()
    X = Camera.transform(cmd_X, refFrame=World.pose)
    Camera.transform(cmd_X, refFrame=Flange.pose)
    Camera.plot_frame(ax, 'initCamera')
    # Camera to Pattern

    cmd_A0 = orientation.asRad((0, 0, WD, 0, 180, 0)).cmd()
    A0 = Pattern.transform(cmd_A0, refFrame=World.pose)
    Pattern.transform(cmd_A0, refFrame=Camera.pose)
    Pattern.plot_frame(ax, 'Pattern')
    # Base to Pattern
    Z = B0 * X * A0

    ##################
    # Pose calculation
    ##################
    pose_amount = len(camOrientations)
    iAs = np.zeros((pose_amount, 4, 4))
    Bs = np.zeros((pose_amount, 4, 4))
    iX = np.linalg.inv(X)
    for i, camOrientation in enumerate(camOrientations):
        cmd_iA = orientation.asItself(camOrientation).cmd()
        iAs[i] = Camera.transform(cmd_iA, refFrame=World.pose)
        Camera.transform(cmd_iA, refFrame=Pattern.pose)
        Camera.plot_frame(ax, '')

        Flange.transform_by_rotation_mat(iX, refFrame=Camera.pose)
        # Flange.plot_frame(ax, '')
        # {Test Flange calculate from Z*A*X}
        Bs[i] = Z * iAs[i] * iX
    plt.show()
    # plt.show(block=False)
    # plt.pause(0.5)
    # plt.close()

    with open(Bs_PATH, 'w') as f:
        yaml.dump(Bs.tolist(), f, default_flow_style=False)
        print('Save the Bs to yaml data file.')

    with open(X_PATH, 'w') as f:
        yaml.dump(X.tolist(), f, default_flow_style=False)
        print('Save the X to yaml data file.')

    with open(Z_PATH, 'w') as f:
        yaml.dump(Z.tolist(), f, default_flow_style=False)
        print('Save the Z to yaml data file.')

    #################
    # Save as moveit command
    #################
    goals = as_ROSgoal(Bs)
    with open(ROS_GOAL_PATH, 'w') as f:
        yaml.dump(goals.tolist(), f, default_flow_style=False)
        print('Save the ROS goal to yaml data file.')
def main(DEBUG):
    # # PATH SETTING
    # CONFIG = 'config.yaml'
    # with open(CONFIG) as f:
    #     path = yaml.load(f)

    # BASE = path['APose'] if DEBUG else path['ROOT']
    # CAMERA_POSE_PATH = BASE + 'goal/camera_pose.yaml'
    # X_PATH = BASE + 'goal/X.yaml'
    # Z_PATH = BASE + 'goal/Z.yaml'
    # Z2_PATH = BASE + 'goal/Z2.yaml'



    # PE_BASE = path['PoseEstimation'] if DEBUG else path['ROOT']

    # IMAGE_PATH = BASE + 'img/hs*.bmp'
    # CAMERA_MAT = PE_BASE + 'goal/camera_mat.yaml'
    # D_MAT = BASE + 'goal/D.yaml'
    # Bc_PATH = BASE + 'goal/Bc.yaml'
    # cam_pose = [-0.040, 0, 0.040, 0, 0, -90] # pose wrt Flange in meter: (x,y,z,Rx,Ry,Rz)
# ----------------------------
    CONFIG = 'config.yaml'
    with open(CONFIG) as f:
        path = yaml.load(f)

    BASE = path['holeSearching'] if DEBUG else path['ROOT']
    AF_BASE = path['AFocus'] if DEBUG else path['ROOT']
    XZ_BASE = path['solveXZ'] if DEBUG else path['ROOT']
    BF_GOAL = AF_BASE + 'goal/bf_goal.yaml'
    HX_PATH = XZ_BASE + 'goal/HX.yaml'
    Init_Hole_GOAL = BASE + 'goal/init_hole.yaml'
    HW_PATH = BASE + 'goal/HW.yaml'
    # ROS_GOAL_PATH = BASE + 'goal/hs_goal.yaml'

    with open(BF_GOAL) as f:
        bf_goal = np.array(yaml.load(f))

    with open(HX_PATH) as f:
        X = np.matrix(yaml.load(f))

    with open(HW_PATH) as f:
        W = np.matrix(yaml.load(f))

    with open(Init_Hole_GOAL) as f:
        ihs = np.array(yaml.load(f)) # ROS goal: (x, y, z, qx, qy,qz, qw)
        Bs = np.zeros((len(ihs), 4, 4))
        for i, ih in enumerate(ihs):
            q = np.quaternion(ih[6], ih[3], ih[4], ih[5])
            
            B = np.identity(4)
            B[0:3, 0:3] = quaternion.as_rotation_matrix(q)
            B[0:3, 3] = ih[0:3]
            Bs[i] = B

    WD = bf_goal[2] - X[2, 3]# work distence from camera to pattern(m)
    phi = 5 # polarangle 
    camOrientations = main_calculateCameraPose(WD, 3, phi) 


    # data visualization
    World = frame3D.Frame(np.matlib.identity(4))
    Workpiece = frame3D.Frame(World.pose)
    Camera = frame3D.Frame(Workpiece.pose)
    Flange = frame3D.Frame(Workpiece.pose)
    Base = frame3D.Frame(Workpiece.pose)
    orientation = frame3D.Orientation()
    ax = frame3D.make_3D_fig(axSize=0.45)

    ##################
    # Pose calculation
    ##################
    pose_amount = len(camOrientations)
    iDs = np.zeros((pose_amount,4,4))
    Bs = np.zeros((pose_amount,4,4))
    iX = np.linalg.inv(X)

    Workpiece.transform_by_rotation_mat(W, refFrame=Base.pose)
    Workpiece.plot_frame(ax, 'Workpiece')
    Base.plot_frame(ax, 'Base')
    for i, camOrientation in enumerate(camOrientations):
        cmd_iA = orientation.asItself(camOrientation).cmd()
        iDs[i] = Camera.transform(cmd_iA, refFrame=World.pose)
        Camera.transform(cmd_iA, refFrame=Workpiece.pose)
        Camera.plot_frame(ax, '')    

        Flange.transform_by_rotation_mat(iX, refFrame=Camera.pose)
        # Flange.plot_frame(ax, '')
        # {Test Flange calculate from Z*A*X}
        Bs[i] = W*iDs[i]*iX
    plt.show()
    # plt.show(block=False)
    # plt.pause(0.5)
    # plt.close() 

    #################
    # Save as moveit command
    #################
    goals = as_ROSgoal(Bs)
    with open(Init_Hole_GOAL, 'w') as f:
        yaml.dump(goals.tolist(), f, default_flow_style=False)
def main(DEBUG):
    CONFIG = 'config.yaml'
    with open(CONFIG) as f:
        path = yaml.load(f)

    BASE = path['holeSearching'] if DEBUG else path['ROOT']
    PE_BASE = path['PoseEstimation'] if DEBUG else path['ROOT']
    XZ_BASE = path['solveXZ'] if DEBUG else path['ROOT']

    Init_Hole_GOAL = BASE + 'goal/init_hole.yaml'
    IMAGE_PATH = BASE + 'img/hs0*.bmp'
    CAMERA_MAT = PE_BASE + 'goal/camera_mat.yaml'
    HX_PATH = XZ_BASE + 'goal/HX.yaml'
    D_MAT = BASE + 'goal/D.yaml'
    HW_MAT = BASE + 'goal/HW.yaml'
    Bc_PATH = BASE + 'goal/Bc.yaml'
    HS_GOAL_PATH = BASE + 'goal/hs_goal.yaml'
    HS_RESULT = BASE + 'goal/hs_result.yaml'
    
    squareLength = 0.01   # Here, our measurement unit is m.
    markerLength = 0.25/40.0   # Here, our measurement unit is m.
    aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_250 )
    arucoParams = cv2.aruco.DetectorParameters_create()
    with open(CAMERA_MAT) as f:
        cameraMatrix = np.array(yaml.load(f))
        distCoeffs = np.array([[0.0, 0.0, 0.0, 0.0, 0.0]]) # Assume to zero

    rvecs = []
    tvecs = []

    images = sorted(glob.glob(IMAGE_PATH))
    for ind, fname in enumerate(images):
        frame = cv2.imread(fname)
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(gray, aruco_dict, parameters=arucoParams)

        if len(corners)>0:
            diamondCorners, diamondIds = cv2.aruco.detectCharucoDiamond(gray, corners, ids, squareLength/markerLength)

            if len(diamondCorners) >= 1: 
                img_with_diamond = cv2.aruco.drawDetectedDiamonds(frame, diamondCorners, diamondIds, (0,255,0))
                rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(diamondCorners, squareLength, cameraMatrix, distCoeffs)
                img_with_diamond = cv2.aruco.drawAxis(img_with_diamond, cameraMatrix, distCoeffs, rvec, tvec, 1)    # axis length 100 can be changed according to your requirement
                
                rvecs.append(rvec)
                tvecs.append(tvec)

        else:
            img_with_diamond = gray

    #     cv2.namedWindow('diamond', cv2.WINDOW_NORMAL)
    #     cv2.resizeWindow('diamond', 1200, 800)
    #     cv2.imshow("diamond", img_with_diamond)   # display
    #     cv2.waitKey(30)
    # cv2.destroyAllWindows()

    Ds = as_homogeneous_mats(rvecs, tvecs) # from Camera to Object
    with open(D_MAT, 'w') as f:
        yaml.dump(Ds.tolist(), f, default_flow_style=False)

    #################
    # Calculate workpiece 
    #################
    with open(HX_PATH) as f:
        X = np.matrix(yaml.load(f))

    with open(Init_Hole_GOAL) as f:
        ihs = np.array(yaml.load(f)) # ROS goal: (x, y, z, qx, qy,qz, qw)
        Bs = np.zeros((len(ihs), 4, 4))
        for i, ih in enumerate(ihs):
            q = np.quaternion(ih[6], ih[3], ih[4], ih[5])
            
            B = np.identity(4)
            B[0:3, 0:3] = quaternion.as_rotation_matrix(q)
            B[0:3, 3] = ih[0:3]
            Bs[i] = B
    Ws = getWbyMatrixProduct(Bs, X, Ds)
    rf, W = solveWbylstsq(Bs, X, Ds)


    (x, y) = (-0.015, 0.015)
    # (x, y) = (0, 0)
    # (x, y) = (-0.0149+0.0725, 0.0153-0.050)
    Worigin = convertToOrigin(W, x, y)
 
    with open(HW_MAT, 'w') as f:
        yaml.dump(Worigin.tolist(), f, default_flow_style=False)
    
    World = frame3D.Frame(np.matlib.identity(4))
    Base = frame3D.Frame(World.pose)
    Flange = frame3D.Frame(Base.pose)
    Camera = frame3D.Frame(Base.pose)
    Workpiece = frame3D.Frame(Base.pose)
    orientation = frame3D.Orientation()
    ax = frame3D.make_3D_fig(axSize=0.45)
    
    # {Base as World}
    poseID = 0
    Base.plot_frame(ax, 'Base')
    Flange.transform_by_rotation_mat(Bs[poseID], refFrame=Base.pose)
    Camera.transform_by_rotation_mat(X, refFrame=Flange.pose)
    Workpiece.transform_by_rotation_mat(Ds[poseID], refFrame=Camera.pose)
    Base.plot_frame(ax, 'Base')
    Camera.plot_frame(ax, 'Camera') 
    Flange.plot_frame(ax, 'Flange') 
    Workpiece.plot_frame(ax, 'Workpiece')

    SIM = False
    if SIM:
        W_ideal = np.array([[-1.0, 0.0, 0.0, -0.06],
                            [0.0, -1.0, 0.0, -0.30],
                            [0.0, 0.0, 1.0, 0.01],
                            [0.0, 0.0, 0.0, 1.0]])
        pError, oError = calPositionAndOrientationError(W, W_ideal)
        print(pError*1000, oError)
        for w in Ws:
            pError, oError = calPositionAndOrientationError(w, W_ideal)
            print(pError*1000, oError)
    else:
        W_assumed = np.identity(4)
        _, oError = calPositionAndOrientationError(W, W_assumed)
        print(oError)
        for w in Ws:
            _, oError = calPositionAndOrientationError(w, W_assumed)
            print(oError)
    hs_res = np.array([np.copy(W)])
    hs_res[0,3,3] = rf

    with open(HS_RESULT, 'a') as f:
        yaml.safe_dump(hs_res.tolist(), f, default_flow_style=False)
        # yaml.safe_dump()

    # Centering command validation
    WD = 0.32692938211698336
    cmd_Dc = orientation.asRad((0.0, 0.0, WD, 0.0, 180, 0.0)).cmd()
    Dc = Workpiece.transform(cmd_Dc, refFrame=World.pose)
    Bc = Worigin*np.linalg.inv(Dc)*np.linalg.inv(X)
    
    goal = as_ROSgoal(Bc)
    with open(HS_GOAL_PATH, 'w') as f:
        yaml.dump(goal.tolist(), f, default_flow_style=False)
Example #7
0
def main(DEBUG):
    CONFIG = 'config.yaml'
    with open(CONFIG) as f:
        path = yaml.load(f)

    BASE = path['holeSearching'] if DEBUG else path['ROOT']
    PE_BASE = path['PoseEstimation'] if DEBUG else path['ROOT']
    XZ_BASE = path['solveXZ'] if DEBUG else path['ROOT']

    Init_Hole_GOAL = BASE + 'goal/init_hole.yaml'
    IMAGE_PATH = BASE + 'img/hs*.bmp'
    CAMERA_MAT = PE_BASE + 'goal/camera_mat.yaml'
    HX_PATH = XZ_BASE + 'goal/HX.yaml'
    D_MAT = BASE + 'goal/D.yaml'
    HW_MAT = BASE + 'goal/HW.yaml'
    Bc_PATH = BASE + 'goal/Bc.yaml'

    squareLength = 0.01  # Here, our measurement unit is m.
    markerLength = 0.25 / 40.0  # Here, our measurement unit is m.
    aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_250)
    arucoParams = cv2.aruco.DetectorParameters_create()
    with open(CAMERA_MAT) as f:
        cameraMatrix = np.array(yaml.load(f))
        distCoeffs = np.array([[0.0, 0.0, 0.0, 0.0, 0.0]])  # Assume to zero

    rvecs = []
    tvecs = []

    images = sorted(glob.glob(IMAGE_PATH))
    for ind, fname in enumerate(images):
        frame = cv2.imread(fname)
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(
            gray, aruco_dict, parameters=arucoParams)

        if len(corners) > 0:
            diamondCorners, diamondIds = cv2.aruco.detectCharucoDiamond(
                gray, corners, ids, squareLength / markerLength)

            if len(diamondCorners) >= 1:
                img_with_diamond = cv2.aruco.drawDetectedDiamonds(
                    frame, diamondCorners, diamondIds, (0, 255, 0))
                rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(
                    diamondCorners, squareLength, cameraMatrix, distCoeffs)
                img_with_diamond = cv2.aruco.drawAxis(
                    img_with_diamond, cameraMatrix, distCoeffs, rvec, tvec, 1
                )  # axis length 100 can be changed according to your requirement

                rvecs.append(rvec)
                tvecs.append(tvec)

        else:
            img_with_diamond = gray

        cv2.namedWindow('diamond', cv2.WINDOW_NORMAL)
        cv2.resizeWindow('diamond', 1200, 800)
        cv2.imshow("diamond", img_with_diamond)  # display
        cv2.waitKey(10)
    cv2.destroyAllWindows()

    Ds = as_homogeneous_mats(rvecs, tvecs)  # from Camera to Object
    with open(D_MAT, 'w') as f:
        yaml.dump(Ds.tolist(), f, default_flow_style=False)

    #################
    # Calculate workpiece
    #################
    with open(HX_PATH) as f:
        X = np.matrix(yaml.load(f))

    with open(Init_Hole_GOAL) as f:
        ihs = np.array(yaml.load(f))  # ROS goal: (x, y, z, qx, qy,qz, qw)
        Bs = np.zeros((len(ihs), 4, 4))
        for i, ih in enumerate(ihs):
            q = np.quaternion(ih[6], ih[3], ih[4], ih[5])

            B = np.identity(4)
            B[0:3, 0:3] = quaternion.as_rotation_matrix(q)
            B[0:3, 3] = ih[0:3]
            Bs[i] = B
    Ws = getWbyMatrixProduct(Bs, X, Ds)
    W = solveWbylstsq(Bs, X, Ds)

    (x, y) = (-0.0149, 0.0153)
    # (x, y) = (0, 0)
    Worigin = convertToOrigin(W, x, y)
    print('Worigin')
    print(Worigin)
    with open(HW_MAT, 'w') as f:
        yaml.dump(Worigin.tolist(), f, default_flow_style=False)

    World = frame3D.Frame(np.matlib.identity(4))
    Base = frame3D.Frame(World.pose)
    Flange = frame3D.Frame(Base.pose)
    Camera = frame3D.Frame(Base.pose)
    Workpiece = frame3D.Frame(Base.pose)
    Workorigin = frame3D.Frame(Base.pose)
    orientation = frame3D.Orientation()
    ax = frame3D.make_3D_fig(axSize=0.45)
Example #8
0
def main(DEBUG, output_pattern_img=True):
    # PATH SETTING
    CONFIG = 'config.yaml'
    with open(CONFIG) as f:
        path = yaml.load(f)

    BASE = path['PoseEstimation'] if DEBUG else path['ROOT']
    AC_BASE = path['ACenter'] if DEBUG else path['ROOT']
    AP_BASE = path['APose'] if DEBUG else path['ROOT']
    IMAGE_PATH = AP_BASE + 'img/ap*.bmp'
    INTMAT_GUESS = AC_BASE + 'goal/camera_mat.yaml'
    INTMAT = BASE + 'goal/camera_mat.yaml'
    EXTMAT = BASE + 'goal/As.yaml'
    IDEAL_A = AP_BASE + 'goal/A_desired.yaml'

    dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_100)
    board = cv2.aruco.CharucoBoard_create(11, 9, 0.010, 0.005, dictionary)

    #Dump the calibration board to a file
    if output_pattern_img:
        img = board.draw((100 * 11, 100 * 9))
        cv2.imwrite(BASE + 'img/charuco.png', img)

    with open(INTMAT_GUESS) as f:
        cameraMatrix_guess = np.array(yaml.load(f))
    distCoeffs = np.array([0.0, 0.0, 0.0, 0.0, 0.0])
    allCHCors = []
    allCHIds = []

    images = sorted(glob.glob(IMAGE_PATH))
    for ind, fname in enumerate(images):
        frame = cv2.imread(fname)
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        ARcors, ARids, _ = cv2.aruco.detectMarkers(gray, dictionary)

        if len(ARcors) > 0:
            ret, CHcors, CHids = cv2.aruco.interpolateCornersCharuco(
                ARcors, ARids, gray, board)
            if CHcors is not None and CHids is not None and len(CHcors) > 3:
                allCHCors.append(CHcors)
                allCHIds.append(CHids)

            cv2.aruco.drawDetectedMarkers(frame, ARcors, ARids)

        # cv2.namedWindow('frame', cv2.WINDOW_NORMAL)
        # cv2.resizeWindow('frame', 1200, 800)
        # cv2.imshow('frame', frame)
        # cv2.waitKey(30)

    cv2.destroyAllWindows()
    imsize = gray.shape
    retval, cameraMatrix, distCoeffs, rvecs, tvecs = cv2.aruco.calibrateCameraCharuco(
        allCHCors, allCHIds, board, imsize, None, None)

    tot_error = 0
    tot_points = 0
    for i, iCHIds in enumerate(allCHIds):
        objpoints = board.chessboardCorners[iCHIds.flatten(), :]
        imgpoints2, _ = cv2.projectPoints(objpoints, rvecs[i], tvecs[i],
                                          cameraMatrix, distCoeffs)
        # tot_error += cv2.norm(allCHCors[i],imgpoints2, cv2.NORM_L2)/len(imgpoints2)
        tot_error += np.sum(np.abs(allCHCors[i] - imgpoints2)**2)
        tot_points += len(allCHCors[i])
        if i > 18:
            plt.scatter(allCHCors[i][:, 0, 0],
                        allCHCors[i][:, 0, 1],
                        color='red')
            plt.scatter(imgpoints2[:, 0, 0], imgpoints2[:, 0, 1], color='blue')
            # plt.draw()
            # plt.pause(0.3)
            # plt.clf()
            # plt.show()

    mean_error = np.sqrt(tot_error / tot_points)
    print("Mean reprojection error: {0}".format(mean_error))
    # print("Final reprojection error opencv: {0}".format(retval))

    # ext_mat: Extrinsic matrix from Pattern to Camera
    As = as_homogeneous_mat(rvecs, tvecs)

    with open(EXTMAT, 'w') as f:
        yaml.dump(As.tolist(), f, default_flow_style=False)

    with open(INTMAT, 'w') as f:
        yaml.dump(cameraMatrix.tolist(), f, default_flow_style=False)

    # SIM = True
    # if SIM:
    #     with open(IDEAL_A) as f:
    #         As_ideal = np.array(yaml.load(f))
    #     for (A, A_ideal) in zip(As, As_ideal):
    #         pError, oError = calPositionAndOrientationError(A, A_ideal)
    #         print(pError)

    # Data visualization
    X_PATH = AP_BASE + 'goal/X.yaml'
    B_PATH = AP_BASE + 'goal/Bs.yaml'
    Z_PATH = AP_BASE + 'goal/Z.yaml'
    with open(X_PATH) as f:
        X = np.array(yaml.load(f))
    with open(B_PATH) as f:
        Bs = np.array(yaml.load(f))
    with open(Z_PATH) as f:
        Z = np.array(yaml.load(f))

    World = frame3D.Frame(np.matlib.identity(4))
    Pattern = frame3D.Frame(World.pose)
    Image = frame3D.Frame(Pattern.pose)
    Camera = frame3D.Frame(Pattern.pose)
    Flange = frame3D.Frame(Pattern.pose)
    Base = frame3D.Frame(Pattern.pose)
    Flange_test = frame3D.Frame(Pattern.pose)
    orientation = frame3D.Orientation()
    ax = frame3D.make_3D_fig(axSize=0.45)

    ##################
    # Determine Z
    ##################
    Base.plot_frame(ax, 'Base')
    # cmd_Z1 = orientation.asRad((0.045, 0.055, 0, 0, 0, 0)).cmd()
    # cmd_Z2 = orientation.asRad((0.31, 0.05, 0.001, 0.0, 0.0, -60)).cmd()
    # cmd_iZ1 = orientation.asRad((-0.045, -0.055, 0, 0, 0, 0)).cmd()
    # cmd_Z3 = orientation.asRad((0.2398686, 0.06147114, 0.001, 0.0, 0.0, -60)).cmd()
    cmd_Z2 = orientation.asRad((0.3, 0.0, 0.001, 0.0, 0.0, -90.0)).cmd()
    cmd_Z3 = orientation.asRad((0.255, 0.055, 0.002, 0.0, 0.0, -90.0)).cmd()
    Pattern.transform(cmd_Z2, refFrame=Base.pose)
    # Image.transform(cmd_iZ1, refFrame=Pattern.pose)
    Image.transform(cmd_Z3, refFrame=Base.pose)

    for i, (A, B) in enumerate(zip(As, Bs)):
        Flange.transform_by_rotation_mat(B, refFrame=Base.pose)
        Camera.transform_by_rotation_mat(X, refFrame=Flange.pose)
        # Image.transform_by_rotation_mat(A, refFrame=Camera.pose)
        # Pattern.transform(cmd_Z1, refFrame=Image.pose)

        Flange.plot_frame(ax, '')
        Camera.plot_frame(ax, '')
        Image.plot_frame(ax, 'Image')
        Pattern.plot_frame(ax, 'Pattern')
Example #9
0
def main(DEBUG, output_pattern_img=True):
    # PATH SETTING
    CONFIG = 'config.yaml'
    with open(CONFIG) as f:
        path = yaml.load(f)

    BASE = path['PoseEstimation'] if DEBUG else path['ROOT']
    AC_BASE = path['ACenter'] if DEBUG else path['ROOT']
    AP_BASE = path['APose'] if DEBUG else path['ROOT']
    IMAGE_PATH = AP_BASE + 'img/ap*.bmp'
    INTMAT = AC_BASE + 'goal/camera_mat.yaml'
    INTMAT_CP = BASE + 'goal/camera_mat.yaml'
    EXTMAT = BASE + 'goal/As.yaml'

    dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_100)
    board = cv2.aruco.CharucoBoard_create(11, 9, 0.010, 0.005, dictionary)

    #Dump the calibration board to a file
    if output_pattern_img:
        img = board.draw((100 * 11, 100 * 9))
        cv2.imwrite(BASE + 'charuco.png', img)

    # Camera parameter
    with open(INTMAT) as f:
        cameraMatrix = np.array(yaml.load(f))
    distCoeffs = np.array([0.0, 0.0, 0.0, 0.0, 0.0])
    rvecs = []
    tvecs = []
    allCHCors = []
    allCHIds = []
    # CHimgpoints = [] # 2d points in image plane.
    # ARimgpoints = [] # 2d points in image plane.

    images = sorted(glob.glob(IMAGE_PATH))
    for ind, fname in enumerate(images):
        frame = cv2.imread(fname)
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        ARcors, ARids, _ = cv2.aruco.detectMarkers(gray, dictionary)

        if len(ARcors) > 0:
            ret, CHcors, CHids = cv2.aruco.interpolateCornersCharuco(
                ARcors, ARids, gray, board)
            retval, rvec, tvec = cv2.aruco.estimatePoseCharucoBoard(
                CHcors, CHids, board, cameraMatrix, distCoeffs)

            if retval and CHcors is not None and CHids is not None and len(
                    CHcors) > 3:
                rvecs.append(rvec)
                tvecs.append(tvec)
                allCHCors.append(CHcors)
                allCHIds.append(CHids)

            cv2.aruco.drawDetectedMarkers(frame, ARcors, ARids)

        cv2.namedWindow('frame', cv2.WINDOW_NORMAL)
        cv2.resizeWindow('frame', 1200, 800)
        cv2.imshow('frame', frame)
        cv2.waitKey(10)

    tot_error = 0
    tot_points = 0
    for i, iCHIds in enumerate(allCHIds):
        objpoints = board.chessboardCorners[iCHIds.flatten(), :]
        imgpoints2, _ = cv2.projectPoints(objpoints, rvecs[i], tvecs[i],
                                          cameraMatrix, distCoeffs)
        tot_error += np.sum(np.abs(allCHCors[i] - imgpoints2)**2)
        tot_points += len(allCHCors[i])
        # plt.scatter(allCHCors[i][:, 0, 0], allCHCors[i][:, 0, 1], color='red')
        # plt.scatter(imgpoints2[:, 0, 0], imgpoints2[:, 0, 1], color='blue')
        # plt.show()
        # plt.show(block=False)
        # plt.pause(0.5)
        # plt.close()

    mean_error = np.sqrt(tot_error / tot_points)
    print("Mean reprojection error: {0}".format(mean_error))

    cv2.destroyAllWindows()
    imsize = gray.shape
    # ext_mat: Extrinsic matrix from Pattern to Camera
    As = as_homogeneous_mat(rvecs, tvecs)

    with open(EXTMAT, 'w') as f:
        yaml.dump(As.tolist(), f, default_flow_style=False)
        print('Save the Extrinsic matrix to yaml data file.')

    with open(INTMAT_CP, 'w') as f:
        yaml.dump(cameraMatrix.tolist(), f, default_flow_style=False)
        print('Save the Intrinsic matrix to yaml data file.')

    # Data visualization
    X_PATH = AP_BASE + 'goal/X.yaml'
    B_PATH = AP_BASE + 'goal/Bs.yaml'
    Z_PATH = AP_BASE + 'goal/Z.yaml'
    with open(X_PATH) as f:
        X = np.array(yaml.load(f))
    with open(B_PATH) as f:
        Bs = np.array(yaml.load(f))
    with open(Z_PATH) as f:
        Z = np.array(yaml.load(f))

    World = frame3D.Frame(np.matlib.identity(4))
    Pattern = frame3D.Frame(World.pose)
    Image = frame3D.Frame(Pattern.pose)
    Camera = frame3D.Frame(Pattern.pose)
    Flange = frame3D.Frame(Pattern.pose)
    Base = frame3D.Frame(Pattern.pose)
    Flange_test = frame3D.Frame(Pattern.pose)
    orientation = frame3D.Orientation()
    ax = frame3D.make_3D_fig(axSize=0.45)

    ##################
    # Determine Z
    ##################
    Base.plot_frame(ax, 'Base')
    cmd_Z1 = orientation.asRad((0.045, 0.055, 0, 0, 0, 0)).cmd()

    for i, (A, B) in enumerate(zip(As, Bs)):
        Flange.transform_by_rotation_mat(B, refFrame=Base.pose)
        Camera.transform_by_rotation_mat(X, refFrame=Flange.pose)
        Image.transform_by_rotation_mat(A, refFrame=Camera.pose)
        Pattern.transform(cmd_Z1, refFrame=Image.pose)

        if i < 12:
            # Flange.plot_frame(ax, '')
            Camera.plot_frame(ax, '')
            Image.plot_frame(ax, 'Image')
            Pattern.plot_frame(ax, 'Pattern')