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: