Beispiel #1
0
def go_to_position_mouse(event, y, x, flags, param):
    global TRANS_MAT, cnt, done
    if event == cv2.EVENT_LBUTTONDOWN:
        print y, x
        capture.grab()
        ok, real = capture.retrieve(0, cv2.CAP_OPENNI_POINT_CLOUD_MAP)
        xw = 100 * real[x][y][0]
        yw = 100 * real[x][y][1]
        zw = 100 * real[x][y][2]
        print xw, yw, zw
        if cnt < 16:
            if cnt < 15:
                GoToPos(points[cnt][0], points[cnt][1], points[cnt][2], 'close')
                #time.sleep(5)
            kinect_frame_pts.append([xw, yw, zw])
            cnt += 1
        else:
            if not done: 
                done = True
                Kinect_frame_matrix = np.matrix([[kinect_frame_pts[0][0],kinect_frame_pts[0][1],kinect_frame_pts[0][2],1],
                                            [kinect_frame_pts[1][0],kinect_frame_pts[1][1],kinect_frame_pts[1][2],1],
                                            [kinect_frame_pts[2][0],kinect_frame_pts[2][1],kinect_frame_pts[2][2],1],
                                            [kinect_frame_pts[3][0],kinect_frame_pts[3][1],kinect_frame_pts[3][2],1],
                                            [kinect_frame_pts[4][0],kinect_frame_pts[4][1],kinect_frame_pts[4][2],1],
                                            [kinect_frame_pts[5][0],kinect_frame_pts[5][1],kinect_frame_pts[5][2],1],
                                            [kinect_frame_pts[6][0],kinect_frame_pts[6][1],kinect_frame_pts[6][2],1],
                                            [kinect_frame_pts[7][0],kinect_frame_pts[7][1],kinect_frame_pts[7][2],1],
                                            [kinect_frame_pts[8][0],kinect_frame_pts[8][1],kinect_frame_pts[8][2],1],
                                            [kinect_frame_pts[9][0],kinect_frame_pts[9][1],kinect_frame_pts[9][2],1],
                                            [kinect_frame_pts[10][0],kinect_frame_pts[10][1],kinect_frame_pts[10][2],1],
                                            [kinect_frame_pts[11][0],kinect_frame_pts[11][1],kinect_frame_pts[11][2],1],
                                            [kinect_frame_pts[12][0],kinect_frame_pts[12][1],kinect_frame_pts[12][2],1],
                                            [kinect_frame_pts[13][0],kinect_frame_pts[13][1],kinect_frame_pts[13][2],1],
                                            [kinect_frame_pts[14][0],kinect_frame_pts[14][1],kinect_frame_pts[14][2],1],])

                print kinect_frame_pts
                #print Kinect_frame_matrix.transpose()
                TRANS_MAT = getTransformationMat(Kinect_frame_matrix.transpose())
            else:
                TRANS_MAT = [[1.38884742e-01,   7.14152672e-02,  -9.97560137e-01,   9.08830124e+01],
			     [1.20949309e+00,  -9.34430346e-02,  -1.09917642e-01,   2.06001291e+01],
			     [-6.30752771e-02,   1.29666723e+00,   8.56265774e-02,   8.16176389e+00],
 			     [0.00000000e+00,   8.67361738e-18,  -1.11022302e-16,   1.00000000e+00]]

                point = TRANS_MAT * np.matrix([[xw],[yw],[zw],[1]])
                print point[0],point[1],point[2]
		moveObj(point[0],point[1],point[2],0,21.22,12.86)
Beispiel #2
0
            [kinect_frame_pts[3][0], kinect_frame_pts[3][1], kinect_frame_pts[3][2], 1],
            [kinect_frame_pts[4][0], kinect_frame_pts[4][1], kinect_frame_pts[4][2], 1],
            [kinect_frame_pts[5][0], kinect_frame_pts[5][1], kinect_frame_pts[5][2], 1],
            [kinect_frame_pts[6][0], kinect_frame_pts[6][1], kinect_frame_pts[6][2], 1],
            [kinect_frame_pts[7][0], kinect_frame_pts[7][1], kinect_frame_pts[7][2], 1],
            [kinect_frame_pts[8][0], kinect_frame_pts[8][1], kinect_frame_pts[8][2], 1],
            [kinect_frame_pts[9][0], kinect_frame_pts[9][1], kinect_frame_pts[9][2], 1],
            [kinect_frame_pts[10][0], kinect_frame_pts[10][1], kinect_frame_pts[10][2], 1],
            [kinect_frame_pts[11][0], kinect_frame_pts[11][1], kinect_frame_pts[11][2], 1],
            [kinect_frame_pts[12][0], kinect_frame_pts[12][1], kinect_frame_pts[12][2], 1],
            [kinect_frame_pts[13][0], kinect_frame_pts[13][1], kinect_frame_pts[13][2], 1],
            [kinect_frame_pts[14][0], kinect_frame_pts[14][1], kinect_frame_pts[14][2], 1],
        ]
    )
    print Kinect_frame_matrix.transpose()
    TRANS_MAT = getTransformationMat(Kinect_frame_matrix.transpose())

    print TRANS_MAT

    cv2.setMouseCallback("RGB image", go_to_position_mouse)

    while 1:
        frame = get_video()
        # get a frame from depth sensor
        get_depth()
        # display RGB image
        cv2.imshow("RGB image", frame)
        # display depth image
        cv2.imshow("Depth image", depth.astype(np.uint8))

        # quit program when 'esc' key is pressed