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_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_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_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()
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 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))
def test_disp2rect():
drive = 11
frame = 0
color = True
img0, img1 = load_stereo_frame(drive, frame, color=color)
calib = Calib(color=color) # get calibration
# get points
vpts = load_velodyne_points(drive, frame)
# remove invalid points
# m = (vpts[:, 0] >= 5)
# m = (vpts[:, 0] >= 5) & (np.abs(vpts[:, 1]) < 5)
m = (vpts[:, 0] >= 5) & (vpts[:, 2] >= -3)
vpts = vpts[m, :]
rpts = calib.velo2rect(vpts)
# get disparities
xyd = calib.rect2disp(rpts)
xyd, valid_rpts = calib.filter_disps(xyd, return_mask=True)
if 1:
# plot disparities
disp = np.zeros(image_shape, dtype=np.uint8)
for x, y, d in np.round(xyd):
disp[y, x] = d
plt.figure(101)
plt.clf()
plt.subplot(211)
plt.imshow(img0, cmap='gray')
plt.subplot(212)
plt.imshow(disp)
plt.show()
# assert False
# convert back to rect
rpts2 = calib.disp2rect(xyd)
assert np.allclose(rpts[valid_rpts], rpts2)
# plotting
if 0:
plt.figure(101)
plt.clf()
img0, img1 = load_stereo_frame(drive, frame, color=color)
plt.imshow(img0, cmap='gray')
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure(1)
fig.clf()
ax = fig.add_subplot(111, projection='3d')
ax.plot3D(rpts[:, 0], rpts[:, 1], rpts[:, 2], '.')
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
plt.show()
def test_disp2rect():
drive = 11
frame = 0
color = True
img0, img1 = load_stereo_frame(drive, frame, color=color)
calib = Calib(color=color) # get calibration
# get points
vpts = load_velodyne_points(drive, frame)
# remove invalid points
# m = (vpts[:, 0] >= 5)
# m = (vpts[:, 0] >= 5) & (np.abs(vpts[:, 1]) < 5)
m = (vpts[:, 0] >= 5) & (vpts[:, 2] >= -3)
vpts = vpts[m, :]
rpts = calib.velo2rect(vpts)
# get disparities
xyd = calib.rect2disp(rpts)
xyd, valid_rpts = calib.filter_disps(xyd, return_mask=True)
if 1:
# plot disparities
disp = np.zeros(image_shape, dtype=np.uint8)
for x, y, d in np.round(xyd):
disp[y, x] = d
plt.figure(101)
plt.clf()
plt.subplot(211)
plt.imshow(img0, cmap='gray')
plt.subplot(212)
plt.imshow(disp)
plt.show()
# assert False
# convert back to rect
rpts2 = calib.disp2rect(xyd)
assert np.allclose(rpts[valid_rpts], rpts2)
# plotting
if 0:
plt.figure(101)
plt.clf()
img0, img1 = load_stereo_frame(drive, frame, color=color)
plt.imshow(img0, cmap='gray')
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure(1)
fig.clf()
ax = fig.add_subplot(111, projection='3d')
ax.plot3D(rpts[:, 0], rpts[:, 1], rpts[:, 2], '.')
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
plt.show()
