def test_disparity_with_bm(drive=11, frame=0):
import cv2
xyd = load_disparity_points(drive, frame, color=True)
disp = np.zeros(image_shape, dtype=np.uint8)
for x, y, d in np.round(xyd):
disp[y, x] = d
# compare with block matching
left, right = load_stereo_frame(drive, frame, color=False)
ndisp = 128
bm = cv2.createStereoBM(numDisparities=ndisp, blockSize=9)
bm.setPreFilterSize(41)
bm.setPreFilterCap(31)
bm.setTextureThreshold(20)
bm.setUniquenessRatio(10)
bm.setSpeckleWindowSize(100)
bm.setSpeckleRange(32)
bm_disp = bm.compute(left, right) / 16
# compute difference
diff = np.zeros(image_shape)
for x, y, d in np.round(xyd):
if bm_disp[y, x] >= 0:
diff[y, x] = bm_disp[y, x] - d
# downsample for visualization purposes
diff2 = np.zeros((image_shape[0] / 2, image_shape[1] / 2))
for i in range(diff2.shape[0]):
for j in range(diff2.shape[1]):
r = diff[2 * i:2 * i + 2, 2 * j:2 * j + 2].flatten()
ind = np.argmax(np.abs(r))
diff2[i, j] = r[ind]
# custom colormap
from matplotlib.colors import LinearSegmentedColormap
g = 1.0
cdict1 = {
'red': ((0.0, 0.0, 0.0), (0.5, 0.0, 0.0), (1.0, 1.0, 1.0)),
'green': ((0.0, g, g), (0.5, 0.0, 0.0), (1.0, g, g)),
'blue': ((0.0, 1.0, 1.0), (0.5, 0.0, 0.0), (1.0, 0.0, 0.0)),
}
cmap = LinearSegmentedColormap('BlueRed', cdict1)
plt.figure(1)
plt.clf()
plt.subplot(311)
plt.imshow(disp)
plt.colorbar()
plt.subplot(312)
plt.imshow(bm_disp)
plt.colorbar()
plt.subplot(313)
plt.imshow(diff2, cmap=cmap, interpolation='nearest')
plt.colorbar()
plt.show()
def test_load_disparity_points(drive=11, frame=0):
xyd = load_disparity_points(drive, frame, color=True)
disp = np.zeros(image_shape, dtype=np.uint8)
for x, y, d in np.round(xyd):
disp[y, x] = d
plt.figure(1)
plt.clf()
plt.imshow(disp)
plt.show()
def test_load_disparity_points(drive=11, frame=0):
xyd = load_disparity_points(drive, frame, color=True)
disp = np.zeros(image_shape, dtype=np.uint8)
for x, y, d in np.round(xyd):
disp[y, x] = d
plt.figure(1)
plt.clf()
plt.imshow(disp)
plt.show()
def estimate_coarse_error(down_factor, iters, video, n_disp, drive, n_frames=20, **params):
# TODO: use fine BP as ground truth?
coarse_error = []
for i in range(n_frames):
print(i)
frame = video[i]
tic('coarse')
coarse_disp = coarse_bp(frame, down_factor=down_factor, iters=iters, **params)
toc()
xyd = load_disparity_points(drive, i)
coarse_error.append(error_on_points(xyd, coarse_disp, n_disp, kind='close'))
return np.mean(coarse_error)
def test_interp(drive=11, frame=0):
xyd = load_disparity_points(drive, frame, color=False)
lin_disp = lin_interp(image_shape, xyd)
lstsq_disp = lstsq_interp(image_shape, xyd, lamb=0.5)
# lstsq_disp = lstsq_interp(image_shape, points, disps, maxiter=100)
plt.figure()
plt.clf()
plt.subplot(211)
plt.imshow(lin_disp)
plt.subplot(212)
plt.imshow(lstsq_disp)
plt.show()
def test_interp(drive=11, frame=0):
xyd = load_disparity_points(drive, frame, color=False)
lin_disp = lin_interp(image_shape, xyd)
lstsq_disp = lstsq_interp(image_shape, xyd, lamb=0.5)
# lstsq_disp = lstsq_interp(image_shape, points, disps, maxiter=100)
plt.figure()
plt.clf()
plt.subplot(211)
plt.imshow(lin_disp)
plt.subplot(212)
plt.imshow(lstsq_disp)
plt.show(block=False)
def test_load_disparity_points(drive=11, frame=0):
xyd = load_disparity_points(drive, frame, color=True)
disp = np.zeros(image_shape, dtype=np.uint8)
for x, y, d in np.round(xyd):
disp[y, x] = d
stf = load_stereo_frame(drive, 0)
plt.figure(1)
plt.clf()
plt.subplot(2,1,1)
plt.imshow(disp)
plt.subplot(2,1,2)
plt.imshow(stf[0],cmap=plt.cm.gray)
plt.show()
def test_clr(drive=11, frame=0):
xyd = load_disparity_points(drive, frame, color=False)
disp = np.zeros(image_shape)
for j, i, d in np.round(xyd):
disp[i, j] = d
pair = load_stereo_frame(drive, frame, color=False)
bp_disp2 = bp_stereo_interp(pair[0], pair[1], xyd)
cpair = load_stereo_frame(drive, frame, color=True)
plt.figure()
plt.clf()
plt.subplot(411)
plt.imshow(cpair[0])
plt.subplot(412)
plt.imshow(cpair[1])
plt.subplot(413)
plt.imshow(disp)
plt.subplot(414)
plt.imshow(bp_disp2)
plt.show(block=False)
def test_bp_interp(drive=11, frame=0):
xyd = load_disparity_points(drive, frame, color=False)
disp = np.zeros(image_shape)
for j, i, d in np.round(xyd):
disp[i, j] = d
bp_disp = bp_interp(image_shape, xyd)
pair = load_stereo_frame(drive, frame)
bp_disp2 = bp_stereo_interp(pair[0], pair[1], xyd)
plt.figure()
plt.clf()
plt.subplot(411)
plt.imshow(pair[0], cmap='gray')
plt.subplot(412)
plt.imshow(disp)
plt.subplot(413)
plt.imshow(bp_disp)
plt.subplot(414)
plt.imshow(bp_disp2)
plt.show()
def test_bp_interp(drive=11, frame=0):
xyd = load_disparity_points(drive, frame, color=False)
disp = np.zeros(image_shape)
for j, i, d in np.round(xyd):
disp[i, j] = d
bp_disp = bp_interp(image_shape, xyd)
pair = load_stereo_frame(drive, frame)
bp_disp2 = bp_stereo_interp(pair[0], pair[1], xyd)
plt.figure()
plt.clf()
plt.subplot(411)
plt.imshow(pair[0], cmap='gray')
plt.subplot(412)
plt.imshow(disp)
plt.subplot(413)
plt.imshow(bp_disp)
plt.subplot(414)
plt.imshow(bp_disp2)
plt.show()
def test_disparity_with_bm(drive=11, frame=0):
# cv2.createStereoBM seems to be absent from current version of opencv
import cv2
xyd = load_disparity_points(drive, frame, color=True)
disp = np.zeros(image_shape, dtype=np.uint8)
for x, y, d in np.round(xyd):
disp[y, x] = d
# compare with block matching
left, right = load_stereo_frame(drive, frame, color=False)
ndisp = 128
bm = cv2.createStereoBM(numDisparities=ndisp, blockSize=9)
bm.setPreFilterSize(41)
bm.setPreFilterCap(31)
bm.setTextureThreshold(20)
bm.setUniquenessRatio(10)
bm.setSpeckleWindowSize(100)
bm.setSpeckleRange(32)
bm_disp = bm.compute(left, right) / 16
# compute difference
diff = np.zeros(image_shape)
for x, y, d in np.round(xyd):
if bm_disp[y, x] >= 0:
diff[y, x] = bm_disp[y, x] - d
# downsample for visualization purposes
diff2 = np.zeros((image_shape[0] / 2, image_shape[1] / 2))
for i in range(diff2.shape[0]):
for j in range(diff2.shape[1]):
r = diff[2*i:2*i+2, 2*j:2*j+2].flatten()
ind = np.argmax(np.abs(r))
diff2[i, j] = r[ind]
# custom colormap
from matplotlib.colors import LinearSegmentedColormap
g = 1.0
cdict1 = {
'red': ((0.0, 0.0, 0.0),
(0.5, 0.0, 0.0),
(1.0, 1.0, 1.0)),
'green': ((0.0, g, g),
(0.5, 0.0, 0.0),
(1.0, g, g)),
'blue': ((0.0, 1.0, 1.0),
(0.5, 0.0, 0.0),
(1.0, 0.0, 0.0)),
}
cmap = LinearSegmentedColormap('BlueRed', cdict1)
plt.figure(1)
plt.clf()
plt.subplot(311)
plt.imshow(disp)
plt.colorbar()
plt.subplot(312)
plt.imshow(bm_disp)
plt.colorbar()
plt.subplot(313)
plt.imshow(diff2, cmap=cmap, interpolation='nearest')
plt.colorbar()
plt.show()
import cv2
import numpy as np
import matplotlib.pyplot as plt
from kitti.raw import load_stereo_frame
from kitti.velodyne import load_disparity_points
from bp_wrapper import downsample, coarse_bp, foveal_bp, error_on_points, plot_fovea
from hunse_tools.timing import tic, toc
idrive = 51
iframe = 50
frame = load_stereo_frame(idrive, iframe)
points = load_disparity_points(idrive, iframe)
full_values = 128
frame_down_factor = 1
values = full_values / 2**frame_down_factor
iters = 3
params = {
'data_weight': 0.16145115747533928,
'disc_max': 294.1504935618425,
'data_max': 32.024780646200725,
'ksize': 3
}
frame = (downsample(frame[0],
frame_down_factor), downsample(frame[1],
frame_down_factor))
# --- setup
calib = Calib()
disp2imu = calib.get_disp2imu()
imu2disp = calib.get_imu2disp()
drive = 51
video = load_stereo_video(drive)
positions = load_video_odometry(drive)
full_shape = video[0][0].shape
ivid = 0
# nvid = 50
nvid = len(video)
video = video[ivid:ivid + nvid]
positions = positions[ivid:ivid + nvid]
points = [load_disparity_points(drive, i) for i in range(ivid, ivid + nvid)]
n_disp = 128
low0 = coarse_bp(video[0])
coarse_shape = low0.shape
def objective(args):
print(args)
# --- run for x frames
fovea_shape = (180, 270)
fovea_margin = (10, 10)
fovea_ij = 100, 600
def estimate_fovea_error(coarse_down_factor, coarse_iters, fovea_down_factor, fovea_iters, fovea_shape, video, positions, drive, n_frames=20):
fovea_ij = 200, 600 #note: ground truth only available in lower part of frame
#fovea_ij = 40, 100
low0 = coarse_bp(video[0], down_factor=coarse_down_factor, iters=coarse_iters)
high0 = coarse_bp(video[0], down_factor=fovea_down_factor, iters=fovea_iters)
high1 = fovea_bp(video[0], fovea_ij, fovea_shape, low0, down_factor=fovea_down_factor, iters=fovea_iters)
# high1 = np.zeros(full_shape, dtype=int)
# high1 = fine_bp(initial)
coarse_shape = np.asarray(low0.shape)
fine_shape = np.asarray(high0.shape)
# fine_shape = full_shape
fine_full_ratio = fine_shape / np.asarray(full_shape, dtype=float)
filt = BryanFilter(coarse_shape, fine_shape, fovea_shape)
# print(filt.coarse_shape)
# print(filt.fine_shape)
# print(filt.fovea_shape)
# filt.fovea_margin = fovea_margin
print('estimating error with ' + str(fovea_down_factor) + ' ' + str(fovea_iters) + ' ' + str(fovea_shape))
fig = plt.figure(1)
fig.clf()
ax_disp = plt.gca()
#TODO: resolution set here
plot_disp = ax_disp.imshow(high0, vmin=0, vmax=n_disp)
filt_error = []
for i in range(n_frames):
frame = video[i]
coarse_disp = coarse_bp(frame, down_factor=coarse_down_factor, iters=coarse_iters, **coarse_params)
#TODO: extract method
ratio = np.round(float(frame.shape[1]) / float(coarse_disp.shape[0]))
ij0 = np.round(np.asarray(fovea_ij) / ratio)
coarse_shape = np.round(np.asarray(fovea_shape) / ratio)
ij1 = ij0 + coarse_shape
coarse_subwindow = coarse_disp[ij0[0]:ij1[0], ij0[1]:ij1[1]]
seed = cv2.resize(coarse_subwindow, fovea_shape[::-1])
band_size = 5
seed[band_size:-band_size, band_size:-band_size] = 0
fovea_disp = fovea_bp(frame, fovea_ij, fovea_shape, seed, down_factor=fovea_down_factor, iters=fovea_iters, **fovea_params)
# fovea_disp[:] = 0
filt.compute(positions[i], coarse_disp, fovea_disp, fovea_ij, disp2imu, imu2disp)
# --- update fovea position
if 1:
fm, fn = filt.fovea_shape
icost = cv2.integral(filt.cost)
fcost = -np.inf * np.ones_like(icost)
fcostr = fcost[:-fm, :-fn]
fcostr[:] = icost[fm:, fn:]
fcostr -= icost[:-fm, fn:]
fcostr -= icost[fm:, :-fn]
fcostr += icost[:-fm, :-fn]
fcostr[:, :n_disp] = -np.inf # need space left of fovea for disparity
#TODO: set fovea_ij here (currently trying to figure out why errors don't depend on params without setting it)
fovea_ij = np.unravel_index(np.argmax(fcost), fcost.shape)
# translate from fine_shape into full_shape coordinates
fovea_ij = np.round(np.asarray(fovea_ij) / fine_full_ratio)
xyd = load_disparity_points(drive, i)
err = error_on_points(xyd, filt.disp, n_disp, kind='close')
filt_error.append(err)
# show results
img = cv2.cvtColor(frame[0], cv2.COLOR_GRAY2RGB)
# Point = lambda ij: (int(ij[1]), int(ij[0]))
# draw_rect = lambda img, ij, shape, color: cv2.rectangle(
# img, Point(ij), Point((ij[0] + shape[0], ij[1] + shape[1])), color, thickness=2)
# draw_rect(img, fovea_ij, fovea_shape, (255, 0, 0))
# draw_rect(img, new_fovea_ij, fovea_shape, (0, 255, 0))
plot_disp.set_data(filt.disp)
fig.canvas.draw()
######
print('error ' + str(filt_error))
return np.mean(filt_error)
tic('Fovea position')
fm, fn = filt.fovea_shape
icost = cv2.integral(filt.cost)
fcost = -np.inf * np.ones_like(icost)
fcostr = fcost[:-fm, :-fn]
fcostr[:] = icost[fm:, fn:]
fcostr -= icost[:-fm, fn:]
fcostr -= icost[fm:, :-fn]
fcostr += icost[:-fm, :-fn]
fcostr[:, :n_disp] = -np.inf # need space left of fovea for disparity
new_fovea_ij = np.unravel_index(np.argmax(fcost), fcost.shape)
toc()
# compute error
xyd = load_disparity_points(drive, iframe)
coarse_error = error_on_points(xyd, coarse_disp, n_disp, kind='close')
filt_error = error_on_points(xyd, filt.disp, n_disp, kind='close')
# filt_nofovea_error = error_on_points(xyd, filt_nofovea.disp)
print("Errors %d: coarse = %0.3f, filt fovea = %0.3f\n" %
(iframe, coarse_error, filt_error))
# show results
img = cv2.cvtColor(frame[0], cv2.COLOR_GRAY2RGB)
Point = lambda ij: (int(ij[1]), int(ij[0]))
draw_rect = lambda img, ij, shape, color: cv2.rectangle(
img, Point(ij), Point((ij[0] + shape[0], ij[1] + shape[1])), color, thickness=2)
draw_rect(img, fovea_ij, fovea_shape, (255, 0, 0))
draw_rect(img, new_fovea_ij, fovea_shape, (0, 255, 0))
plot_disp.set_data(filt.disp)
def __getitem__(_, i):
return load_disparity_points(self.drive, i)
import cv2
import numpy as np
import matplotlib.pyplot as plt
from kitti.raw import load_stereo_frame
from kitti.velodyne import load_disparity_points
from bp_wrapper import downsample, coarse_bp, foveal_bp, error_on_points, plot_fovea
from hunse_tools.timing import tic, toc
idrive = 51
iframe = 50
frame = load_stereo_frame(idrive, iframe)
points = load_disparity_points(idrive, iframe)
full_values = 128
frame_down_factor = 1
values = full_values / 2**frame_down_factor
iters = 3
params = {
'data_weight': 0.16145115747533928, 'disc_max': 294.1504935618425,
'data_max': 32.024780646200725, 'ksize': 3}
frame = (downsample(frame[0], frame_down_factor),
downsample(frame[1], frame_down_factor))
tic()
coarse_disp = coarse_bp(frame, values=values, down_factor=1, iters=iters, **params)
coarse_disp *= 2**frame_down_factor
def __getitem__(_, i):
return load_disparity_points(self.drive, i)
