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)
[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
