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 + 1.5])
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())
print TRANS_MAT
else:
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)
def calcTransMatrix():
global TRANS_MAT, kinect_frame_pts
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_matrix.transpose()
TRANS_MAT = getTransformationMat(Kinect_frame_matrix.transpose())
print TRANS_MAT
def run(self):
points = [[-10,25,8.7], [0,21.22,12.86], [0,36,8.7], [10,21,15], [-15,31,20],
[10,31,20], [10,29,15], [15,29,15], [-15,20,15], [-10,20,15],
[3,18,5], [5,20,25], [-5,25,25], [-5,30,6], [15,20,6]]
DELAY = 10
kinect_frame_pts = []
cntFrames = 0
indx = 0
capture = cv2.VideoCapture(cv2.CAP_OPENNI)
x, y = 0, 0
while True:
capture.grab()
ok, rgb = capture.retrieve(0, cv2.CAP_OPENNI_BGR_IMAGE)
ok, real = capture.retrieve(0, cv2.CAP_OPENNI_POINT_CLOUD_MAP)
rgb[:, :, 0] = rgb[:, :, 0] * (real[:, :, 2] < 1.5)
rgb[:, :, 1] = rgb[:, :, 1] * (real[:, :, 2] < 1.5)
rgb[:, :, 2] = rgb[:, :, 2] * (real[:, :, 2] < 1.5)
rgb[:, :, 0] = rgb[:, :, 0] * (real[:, :, 2] > 0.2)
rgb[:, :, 1] = rgb[:, :, 1] * (real[:, :, 2] > 0.2)
rgb[:, :, 2] = rgb[:, :, 2] * (real[:, :, 2] > 0.2)
cp = rgb.copy()
if (indx > 1):
cv2.circle(rgb, (y, x), 10, (0, 255, 0), 2)
if (rgb.shape[2] == 3):
rgb = cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB)
height, width = rgb.shape[:2]
self.frameReady.emit(QImage(rgb, width, height, QImage.Format_RGB888))
if (indx < 16 and cntFrames % DELAY == 0):
y, x = getGripperCenter(cp)
xw = 100 * real[x][y][0]
yw = 100 * real[x][y][1]
zw = 100 * real[x][y][2]
if (indx > 0):
print "Point ", indx, ": ", xw, yw, zw
kinect_frame_pts.append([xw, yw, zw])
#if (indx < 15):
#GoToPos(points[indx][0], points[indx][1], points[indx][2], 'close')
indx += 1
if (indx == 16):
print "Calculating transformation matrix......"
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],])
TRANS_MAT = getTransformationMat(Kinect_frame_matrix.transpose())
self.done.emit(TRANS_MAT)
break
cntFrames += 1
capture.release()