cx = int(moment['m10'] / moment['m00']) cy = int(moment['m01'] / moment['m00']) # Preferred Options: #mm_diameter = (2 * math.tan((equi_diameter / 2.0 / w) * FOVX) * distance_to_object) #(equi_diameter / w) * (2.0 * distance_to_object * math.tan(/2.0)) # ~FOV # Unconfirmed: this distance tries to get the unmodified distance # Reminder: For this, it's Height x Width actualDistmm = depth_frame.asarray() actualDistmm = cv2.medianBlur(actualDistmm, 5) cv2.flip(actualDistmm, 1) dist_to_centroid = mean_val #actualDistmm[cy][cx] if dist_to_centroid < HIGH_DISTANCE_BOUND: coords = registration.getPointXYZ(undistorted, max_loc[1], max_loc[0]) mm_diameter = (equi_diameter) * (1.0 / focal_x) * coords[2] publish_obstacle_position(coords[0], coords[1], coords[2], mm_diameter) color = cv2.ellipse(color, ellipse, (0, 255, 0), 2) cv2.drawContours(color, [box], 0, (0, 0, 255), 1) font = cv2.FONT_HERSHEY_SIMPLEX cv2.putText(color, "x" + str(coords[0]), (cx, cy + 30), font, 0.4, (0, 0, 255), 1, cv2.LINE_AA) cv2.putText(color, "y" + str(coords[1]), (cx, cy + 45), font, 0.4, (0, 255, 0), 1, cv2.LINE_AA) cv2.putText(color, "z" + str(coords[2]), (cx, cy + 60),
def test_sync_multi_frame(): fn = Freenect2() num_devices = fn.enumerateDevices() assert num_devices > 0 serial = fn.getDefaultDeviceSerialNumber() assert serial == fn.getDeviceSerialNumber(0) device = fn.openDevice(serial) assert fn.getDefaultDeviceSerialNumber() == device.getSerialNumber() device.getFirmwareVersion() listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir | FrameType.Depth) # Register listeners device.setColorFrameListener(listener) device.setIrAndDepthFrameListener(listener) device.start() # Registration registration = Registration(device.getIrCameraParams(), device.getColorCameraParams()) undistorted = Frame(512, 424, 4) registered = Frame(512, 424, 4) # optional parameters for registration bigdepth = Frame(1920, 1082, 4) color_depth_map = np.zeros((424, 512), np.int32) # test if we can get two frames at least frames = listener.waitForNewFrame() listener.release(frames) # frames as a first argment also should work frames = FrameMap() listener.waitForNewFrame(frames) color = frames[FrameType.Color] ir = frames[FrameType.Ir] depth = frames[FrameType.Depth] for frame in [ir, depth]: assert frame.exposure == 0 assert frame.gain == 0 assert frame.gamma == 0 for frame in [color]: assert frame.exposure > 0 assert frame.gain > 0 assert frame.gamma > 0 registration.apply(color, depth, undistorted, registered) # with optinal parameters registration.apply(color, depth, undistorted, registered, bigdepth=bigdepth, color_depth_map=color_depth_map.ravel()) assert color.width == 1920 assert color.height == 1080 assert color.bytes_per_pixel == 4 assert ir.width == 512 assert ir.height == 424 assert ir.bytes_per_pixel == 4 assert depth.width == 512 assert depth.height == 424 assert depth.bytes_per_pixel == 4 assert color.asarray().shape == (color.height, color.width, 4) assert ir.asarray().shape == (ir.height, ir.width) assert depth.asarray(np.float32).shape == (depth.height, depth.width) listener.release(frames) return color def __test_cannot_determine_type_of_frame(frame): frame.asarray() for frame in [registered, undistorted]: yield raises(ValueError)(__test_cannot_determine_type_of_frame), frame # getPointXYZ x, y, z = registration.getPointXYZ(undistorted, 512 // 2, 424 // 2) if not np.isnan([x, y, z]).any(): assert z > 0 # getPointXYZRGB x, y, z, b, g, r = registration.getPointXYZRGB(undistorted, registered, 512 // 2, 424 // 2) if not np.isnan([x, y, z]).any(): assert z > 0 assert np.isfinite([b, g, r]).all() for pix in [b, g, r]: assert pix >= 0 and pix <= 255 device.stop() device.close()
def test_sync_multi_frame(): fn = Freenect2() num_devices = fn.enumerateDevices() assert num_devices > 0 serial = fn.getDefaultDeviceSerialNumber() assert serial == fn.getDeviceSerialNumber(0) device = fn.openDevice(serial) assert fn.getDefaultDeviceSerialNumber() == device.getSerialNumber() device.getFirmwareVersion() listener = SyncMultiFrameListener( FrameType.Color | FrameType.Ir | FrameType.Depth) # Register listeners device.setColorFrameListener(listener) device.setIrAndDepthFrameListener(listener) device.start() # Registration registration = Registration(device.getIrCameraParams(), device.getColorCameraParams()) undistorted = Frame(512, 424, 4) registered = Frame(512, 424, 4) # optional parameters for registration bigdepth = Frame(1920, 1082, 4) color_depth_map = np.zeros((424, 512), np.int32) # test if we can get two frames at least frames = listener.waitForNewFrame() listener.release(frames) # frames as a first argment also should work frames = FrameMap() listener.waitForNewFrame(frames) color = frames[FrameType.Color] ir = frames[FrameType.Ir] depth = frames[FrameType.Depth] for frame in [ir, depth]: assert frame.exposure == 0 assert frame.gain == 0 assert frame.gamma == 0 for frame in [color]: assert frame.exposure > 0 assert frame.gain > 0 assert frame.gamma > 0 registration.apply(color, depth, undistorted, registered) # with optinal parameters registration.apply(color, depth, undistorted, registered, bigdepth=bigdepth, color_depth_map=color_depth_map.ravel()) registration.undistortDepth(depth, undistorted) assert color.width == 1920 assert color.height == 1080 assert color.bytes_per_pixel == 4 assert ir.width == 512 assert ir.height == 424 assert ir.bytes_per_pixel == 4 assert depth.width == 512 assert depth.height == 424 assert depth.bytes_per_pixel == 4 assert color.asarray().shape == (color.height, color.width, 4) assert ir.asarray().shape == (ir.height, ir.width) assert depth.asarray(np.float32).shape == (depth.height, depth.width) listener.release(frames) def __test_cannot_determine_type_of_frame(frame): frame.asarray() for frame in [registered, undistorted]: yield raises(ValueError)(__test_cannot_determine_type_of_frame), frame # getPointXYZ x, y, z = registration.getPointXYZ(undistorted, 512 // 2, 424 // 2) if not np.isnan([x, y, z]).any(): assert z > 0 # getPointXYZRGB x, y, z, b, g, r = registration.getPointXYZRGB(undistorted, registered, 512 // 2, 424 // 2) if not np.isnan([x, y, z]).any(): assert z > 0 assert np.isfinite([b, g, r]).all() for pix in [b, g, r]: assert pix >= 0 and pix <= 255 device.stop() device.close()
#mm_diameter = (2 * math.tan((equi_diameter / 2.0 / w) * FOVX) * distance_to_object) #(equi_diameter / w) * (2.0 * distance_to_object * math.tan(/2.0)) # ~FOV # Unconfirmed: this distance tries to get the unmodified distance # Reminder: For this, it's Height x Width actualDistmm = depth_frame.asarray() actualDistmm = cv2.medianBlur(actualDistmm, 5) cv2.flip(actualDistmm, 1) dist_to_centroid = actualDistmm[cy][cx] if dist_to_centroid < HIGH_DISTANCE_BOUND: # Sets mm_diameter to the object's diameter in pixels wide, for calibration if desired mm_diameter = equi_diameter * (1.0 / focal_x) * dist_to_centroid coords = registration.getPointXYZ(depth_frame, cy, cx) ellipse = cv2.fitEllipse(cntr) img = cv2.ellipse(img, ellipse, (0, 255, 0), 2) rect = cv2.minAreaRect(cntr) box = cv2.boxPoints(rect) box = np.int0(box) cv2.drawContours(img, [box], 0, (0, 0, 255), 1) font = cv2.FONT_HERSHEY_SIMPLEX cv2.putText(img, "x" + str(coords[0]), (cx, cy + 30), font, 0.4, (0, 0, 255), 1, cv2.LINE_AA) cv2.putText(img, "y" + str(coords[1]), (cx, cy + 45), font, 0.4, (0, 255, 0), 1, cv2.LINE_AA) cv2.putText(img, "z" + str(coords[2]), (cx, cy + 60), font,
class Tracker: # X_POSITION = -0.11387496 # Z_POSITION = 1.3 X_POSITION = -0.185 Z_POSITION = 1.5 greenLower = (29, 86, 6) greenUpper = (64, 255, 255) greenLower = (27, 100, 50) greenUpper = (60, 255, 255) def __init__(self, use_kinect=False, basket=False, gui=True): self.connected = False self.max_radius = 100000 self.running = False self.killed = False self.gui = gui self.basket = basket self.use_kinect = use_kinect # self.fig, self.ax = plt.subplots() if self.use_kinect: self.px = [] self.py = [] logger = createConsoleLogger(LoggerLevel.Error) setGlobalLogger(logger) while not self.connected: # Bad, but needed if self.use_kinect: self.connect_kinect() else: self.connect_camera() self.thread = threading.Thread(target=self.video_thread) self.thread.start() def connect_camera(self): # Bad, but needed self.camera = cv2.VideoCapture(1) if not self.camera.isOpened(): print("Unable to connect to the camera, check again") time.sleep(5) else: self.connected = True def connect_kinect(self): # Bad, but needed self.fn = Freenect2() num_devices = self.fn.enumerateDevices() if num_devices == 0: print("Kinect not found, check again") self.connected = False return try: self.pipeline = OpenGLPacketPipeline() except: self.pipeline = CpuPacketPipeline() serial = self.fn.getDeviceSerialNumber(0) self.device = self.fn.openDevice(serial, pipeline=self.pipeline) self.listener = SyncMultiFrameListener( FrameType.Color | FrameType.Ir | FrameType.Depth) # Register listeners self.device.setColorFrameListener(self.listener) self.device.setIrAndDepthFrameListener(self.listener) self.device.start() print("started") # NOTE: must be called after device.start() self.registration = Registration(self.device.getIrCameraParams(), self.device.getColorCameraParams()) print("registration") self.undistorted = Frame(512, 424, 4) self.registered = Frame(512, 424, 4) self.connected = True def video_thread(self): print('Camera thread started') need_bigdepth = False need_color_depth_map = False bigdepth = Frame(1920, 1082, 4) if need_bigdepth else None color_depth_map = np.zeros((424, 512), np.int32).ravel() \ if need_color_depth_map else None while not self.killed: while self.running: # (grabbed, frame) = self.camera.read() frames = self.listener.waitForNewFrame() frame = frames["color"] ir = frames["ir"] depth = frames["depth"] self.registration.apply(frame, depth, self.undistorted, self.registered, bigdepth=bigdepth, color_depth_map=color_depth_map) # print(frame.width, frame.height, frame.bytes_per_pixel) # print(ir.width, ir.height, ir.bytes_per_pixel) # print(depth.width, depth.height, depth.bytes_per_pixel) # print(frame.asarray().shape, ir.asarray().shape, depth.asarray().shape) # color_image_array = frame.asarray() color_image_array = self.registered.asarray(np.uint8) hsv = cv2.cvtColor(color_image_array, cv2.COLOR_BGR2HSV) mask = cv2.inRange(hsv, self.greenLower, self.greenUpper) mask = cv2.erode(mask, None, iterations=2) mask = cv2.dilate(mask, None, iterations=2) cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2] center = None if len(cnts) > 0: c = max(cnts, key=cv2.contourArea) ((x, y), radius) = cv2.minEnclosingCircle(c) M = cv2.moments(c) center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"])) if radius > 10: cv2.circle(color_image_array, (int(x), int(y)), int(radius), (0, 255, 255), 2) cv2.circle(color_image_array, center, 5, (0, 0, 255), -1) x, y, z = self.registration.getPointXYZ(self.undistorted, y, x) # print(x, y, z, end='\r') if not math.isnan(y) and not math.isnan(z): self.px.append(y) self.py.append(z) row = tuple(c[c[:, :, 1].argmin()][0])[1] self.max_radius = min(self.max_radius, row) if self.gui: cv2.imshow("Frame", color_image_array) cv2.imshow("Masked", mask) self.listener.release(frames) cv2.waitKey(1) & 0xFF # self.ax.plot(self.px) # self.ax.plot(self.py) # plt.pause(0.01) cv2.destroyAllWindows() print("exit looping") def start(self): self.max_radius = 100000 if self.use_kinect: self.px = [] self.py = [] self.running = True def stop(self): self.running = False def kill(self): self.killed = True if self.use_kinect: self.device.stop() self.device.close() def get_reward(self): if self.use_kinect: if not self.basket: try: peaks = peakutils.indexes(self.px) except Exception as e: print(e) peaks = np.array([]) if peaks.any(): # for xx in peaks: # t.ax.plot(xx, self.px[xx], 'ro') return self.py[peaks[0]] * 100 else: return 0 else: print(np.mean(self.px), np.mean(self.py)) dist = distance(np.array([self.px, self.py]).T, self.X_POSITION, self.Z_POSITION) print("distance", dist, len(self.px)) return dist * 100 else: return self.max_radius
pts.appendleft(center) # loop over the set of tracked points for i in range(1, len(pts)): # if either of the tracked points are None, ignore # them if pts[i - 1] is None or pts[i] is None: continue # otherwise, compute the thickness of the line and # draw the connecting lines thickness = int(np.sqrt(64 / float(i + 1)) * 2.5) cv2.line(color, pts[i - 1], pts[i], (0, 0, 255), thickness) if center: temp = registration.getPointXYZ(undistorted, center[0], center[1]) print(temp) # NOTE for visualization: # cv2.imshow without OpenGL backend seems to be quite slow to draw all # things below. Try commenting out some imshow if you don't have a fast # visualization backend. #cv2.imshow("ir", ir.asarray() / 65535.) #cv2.imshow("depth", depth.asarray() / 4500.) #cv2.imshow("color", cv2.resize(color.asarray(),(int(1920 / 3), int(1080 / 3)))) cv2.imshow("color", color) #cv2.imshow("registered", registered.asarray(np.uint8)) if need_bigdepth: