def get_kinect_frame_pt(): capture.grab() ok, frame = capture.retrieve(0, cv2.CAP_OPENNI_BGR_IMAGE) if not ok: print "HERE" gripper_centerXY = getGripperCenter(frame) return get_position(gripper_centerXY[0],gripper_centerXY[1])
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()
ok, rgb = capture.retrieve(0, cv2.CAP_OPENNI_BGR_IMAGE) if (indx > 1): cv2.circle(rgb, (y, x), 10, (0, 255, 0), 2) cv2.imshow('RGB image', rgb) ok, depth = capture.retrieve(0, cv2.CAP_OPENNI_DISPARITY_MAP) cv2.imshow('Depth map', depth) framesList.append(rgb) if(len(framesList) > 8): framesList.pop(0) if (indx < 16 and cntFrames % DELAY == 0): ok, real = capture.retrieve(0, cv2.CAP_OPENNI_POINT_CLOUD_MAP) x, y = getGripperCenter(framesList) 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......" calcTransMatrix() cv2.setMouseCallback('RGB image', mouseClick) indx += 1
def get_kinect_frame_pt(): frame = get_video() gripper_centerXY = getGripperCenter(frame) gripper_center = get_position(gripper_centerXY[0], gripper_centerXY[1]) return gripper_center