def test_video(color=True, stereo=False, **kwargs):
for l,r in test_dataset(**kwargs).iter_stereo_frames():
l = to_color(l) if color else to_gray(l)
if not stereo:
yield l
else:
r = to_color(r) if color else to_gray(r)
yield l,r
def test_image(color=True, scale=1.0, stereo=False):
for l,r in test_dataset().iter_stereo_frames():
l = to_color(l) if color else to_gray(l)
if not stereo:
return l
else:
r = to_color(r) if color else to_gray(r)
return l,r
def colorize_stereo_disparity(disp, im=None, max_disparity=256):
# Display colored disparity
disp_color = colormap(disp.astype(np.float32) / max_disparity)
if im is None:
return disp_color
else:
return np.vstack([to_color(im), disp_color])
def draw_lines(im, pts1, pts2, colors=None, thickness=1):
out = to_color(im)
cols = get_color(len(pts1), colors=colors)
for col, pt1, pt2 in zip(cols, pts1, pts2):
cv2.line(out, (pt1[0], pt1[1]), (pt2[0], pt2[1]), tuple(col),
thickness)
return out
def visualize(self, im, ids, pts, colored=False):
vis = to_color(im)
dt_vis = self.dt_.visualize(vis, pts)
OpenCVKLT.draw_tracks(self, vis, colored=colored, max_track_length=10)
out = np.vstack([vis, dt_vis])
imshow_cv('dt_vis', out, wait=1)
return out
def draw_features(im, pts, colors=None, size=2):
out = to_color(im)
cols = get_color(len(pts), colors=colors)
for col, pt in zip(cols, pts):
tl = np.int32(pt - size)
br = np.int32(pt + size)
cv2.rectangle(out, (tl[0], tl[1]), (br[0], br[1]), tuple(map(int,
col)), -1)
# cv2.circle(out, tuple(map(int, pt)), size, tuple(col), -1, lineType=cv2.CV_AA)
return out
def draw_lines(im, pts1, pts2, colors=None, thickness=1):
out = to_color(im)
if colors is not None:
cols = colors.astype(np.int64)
else:
cols = np.tile([0, 255, 0], (len(pts1), 1)).astype(np.int64)
for col, pt1, pt2 in zip(cols, pts1, pts2):
cv2.line(out, (pt1[0], pt1[1]), (pt2[0], pt2[1]), tuple(col),
thickness)
return out
def draw_features(im, pts, colors=None, size=2):
out = to_color(im)
if colors is not None:
cols = colors.astype(np.int64)
else:
cols = np.tile([0, 255, 0], (len(pts), 1)).astype(np.int64)
for col, pt in zip(cols, pts):
tl = np.int32(pt - size)
br = np.int32(pt + size)
cv2.rectangle(out, (tl[0], tl[1]), (br[0], br[1]), tuple(col), -1)
# cv2.circle(out, tuple(map(int, pt)), size, tuple(col), -1, lineType=cv2.CV_AA)
return out
def plot_epipolar_line(im_1, F_10, x_0, im_0=None):
"""
Plot the epipole and epipolar line F * x = 0.
l[0] * x + l[1] * y + l[2] = 0
@ x=0: y = -l[2] / l[1]
@ x=W: y = (-l[2] -l[0]*W) / l[1]
"""
H, W = im_1.shape[:2]
lines_1 = epipolar_line(F_10, x_0)
vis_1 = to_color(im_1)
vis_0 = to_color(im_0) if im_0 is not None else None
N = 20
cols = get_color_by_label(np.arange(len(x_0)) % N) * 255
# for tid, pt in zip(ids, pts):
# cv2.circle(vis, tuple(map(int, pt)), 2,
# tuple(map(int, cols[tid % N])) if colored else (0,240,0),
# -1, lineType=cv2.CV_AA)
# col = (0,255,0)
for col, l1 in zip(cols, lines_1):
try:
x0, y0 = list(map(int, [0, -l1[2] / l1[1]]))
x1, y1 = list(map(int, [W, -(l1[2] + l1[0] * W) / l1[1]]))
cv2.line(vis_1, (x0, y0), (x1, y1), col, 1)
except:
pass
# raise RuntimeWarning('Failed to estimate epipolar line {:s}'.format(l1))
if vis_0 is not None:
for col, x in zip(cols, x_0):
cv2.circle(vis_0, tuple(x), 3, col, -1)
return np.vstack([vis_0, vis_1])
return vis_1
def test_uw_rgbd_object():
from pybot.vision.image_utils import to_color
from pybot.vision.imshow_utils import imshow_cv
object_directory = '~/data/rgbd_datasets/udub/rgbd-object-crop/rgbd-dataset'
rgbd_data_uw = UWRGBDObjectDataset(directory=object_directory)
for f in rgbd_data_uw.iteritems(every_k_frames=5):
bbox = f.bbox
imshow_cv(
'frame',
np.hstack([f.img, np.bitwise_and(f.img, to_color(f.mask))]),
text='Image + Mask [Category: [%i] %s, Instance: %i]' %
(bbox['category'], rgbd_data_uw.get_category_name(
bbox['category']), bbox['instance']))
imshow_cv('depth', (f.depth / 16).astype(np.uint8), text='Depth')
def on_image_cb(t, im):
global rgb
vis = to_color(im)
rgb = to_color(im)[::4, ::4]
vis = print_status(vis, text='Pose: {}'.format(odom_str))
imshow_cv('im', vis)