def setup_zed(scale=1.0):
"""
Run calibration:
Scale (0.5):
python calibrate_stereo.py --scale 0.5 --rows 8 --columns 6 --square-size 9.3 zed/data/ zed/calib
Scale (1.0):
python calibrate_stereo.py --scale 1.0 --rows 8 --columns 6 --square-size 9.3 zed/data/ zed/calib
Print calibration:
python bot_vision/calib/print_calib.py zed/calib_1.0
"""
# Saved Calibration
# calib_path = '/home/spillai/perceptual-learning/software/python/bot_vision/calib/zed/calib'
# calibration = StereoCalibration(input_folder=calib_path)
# Scale
# Determine scale from image width (720p)
# scale = float(width) / 720.0
# Setup one-time calibration
# fx, fy, cx, cy = 702.429138, 702.429138, 652.789368, 360.765472
# @ 1080: fx, fy, cx, cy = 1396.555664 * s, 1396.555664 * s, 972.651123 * s, 540.047119 * s
# @ 360p
# D0: [-0.0254902 , -0.00319033, 0. , 0. , 0.03270019]
# D1: [-0.03288394, 0.0149428 , 0. , 0. , 0.01202393]
# K0: [ 337.10210476, 0. , 329.15867687],
# [ 0. , 337.10210476, 178.30881956],
# [ 0. , 0. , 1. ]
# @ 720p
# D0: [-0.01704945, -0.01655319, 0. , 0. , 0.04144856]
# D1: [-0.02413693, 0.00169603, 0. , 0. , 0.023676 ]
# K0: [ 677.57005977, 0. , 658.49378727],
# [ 0. , 677.57005977, 358.58253284],
# [ 0. , 0. , 1. ]
# @ 360p
image_width = 360
fx, fy, cx, cy = 337.10210476, 337.10210476, 329.15867687, 178.30881956
# @ 720p
# image_width = 720
# fx, fy, cx, cy = 677.57005977463643, 677.57005977463643, 658.49378727401586, 358.58253283725276
print 'fx, fy, cx, cy', fx, fy, cx, cy, scale
calib_params = StereoCamera.from_calib_params(fx * scale,
fy * scale,
cx * scale,
cy * scale,
baseline=0.12)
# calib_params = get_calib_params() # baseline_px=baseline_px * scale)
# calib_params.D0 = np.array([0, 0, 0, 0, 0], np.float64)
# calib_params.D0 = np.array([-0.16, 0, 0, 0, 0], np.float64)
# calib_params.D1 = calib_params.D0
# calib_params.image_width = image_width * scale
return calib_params
def calib_read(fn, scale):
db = AttrDict.load_yaml(fn)
P0 = np.float32(db[calib_left].split(' '))
P1 = np.float32(db[calib_right].split(' '))
fx, cx, cy = P0[0], P0[2], P0[6]
baseline_px = np.fabs(P1[3])
return StereoCamera.from_calib_params(fx, fx, cx, cy, baseline_px=baseline_px)
def setup_bb(scale=1.0):
# Setup one-time calibration
calib_path = '/home/spillai/perceptual-learning/software/python/bot_vision/calib/bb/calib'
calibration = StereoCalibration(input_folder=calib_path)
calib_params = AttrDict(
get_stereo_calibration_params(input_folder=calib_path))
return StereoCamera.from_calib_params(calib_params.fx,
calib_params.fy,
calib_params.cx,
calib_params.cy,
baseline=calib_params.baseline)
def __init__(self, stereo, stereo_calib, rectify=None):
self.stereo_ = stereo
self.rectify_ = rectify
# Set fake calibration parameters if None
if stereo_calib is None:
stereo_calib = StereoCamera.from_calib_params(1000,
1000,
W / 2 - 0.5,
H / 2 - 0.5,
baseline=0.12)
self.stereo_.set_calibration(stereo_calib.left.K, stereo_calib.right.K,
stereo_calib.left.D, stereo_calib.right.D,
stereo_calib.left.R, stereo_calib.right.R,
stereo_calib.left.P, stereo_calib.right.P,
stereo_calib.Q, stereo_calib.right.t)
class TsukubaStereo2012Reader(object):
"""
TsukubaStereo2012Reader: StereoDatasetReader + Calib
The resolution of the images is 640x480 pixels, the baseline of the stereo
camera is 10cm and the focal length of the camera is 615 pixels.
https://github.com/pablospe/tsukuba_db/blob/master/README.Tsukuba
"""
calib = StereoCamera.from_calib_params(615, 615, 319.5, 239.5,
baseline=0.10, shape=np.int32([480, 640]))
def __init__(self, directory='NewTsukubaStereoDataset/',
left_template='illumination/daylight/left/tsukuba_daylight_L_%05i.png',
right_template='illumination/daylight/right/tsukuba_daylight_R_%05i.png',
start_idx=1, max_files=50000, scale=1.0, grayscale=False):
# Minor dataset related check
if start_idx < 1:
raise RuntimeError('TsukubaStereo2012Reader has to start at index 1')
# Set args
self.scale = scale
# Get calib
self.calib = TsukubaStereo2012Reader.calib.scaled(scale)
# Read poses
try:
pose_fn = os.path.join(os.path.expanduser(directory), 'groundtruth/camera_track.txt')
self.poses = FileReader(pose_fn, start_idx=start_idx, process_cb=tsukuba_load_poses)
except Exception as e:
self.poses = repeat(None)
raise RuntimeError('Failed to load poses properly, cannot proceed {:}'.format(e))
# Read stereo images
self.stereo = StereoDatasetReader(directory=directory,
left_template=left_template,
right_template=right_template,
start_idx=start_idx, max_files=max_files, scale=scale, grayscale=grayscale)
print 'Initialized stereo dataset reader with %f scale' % scale
gt_fn = os.path.join(os.path.expanduser(directory),
'groundtruth/disparity_maps/left/tsukuba_disparity_L_%05i.png')
self.gt = DatasetReader(process_cb=TsukubaStereo2012Reader.dispread_process_cb(scale=scale), template=gt_fn,
start_idx=start_idx, max_files=max_files)
@staticmethod
def dispread_process_cb(scale=1.0):
"""Scale disparity values for depth images"""
return lambda fn: im_resize(cv2.imread(fn, cv2.IMREAD_UNCHANGED), scale=scale) * scale
def iterframes(self, *args, **kwargs):
for (left, right), pose in izip(self.iter_stereo_frames(*args, **kwargs),
self.poses.iteritems(*args, **kwargs)):
yield AttrDict(left=left, right=right, pose=pose)
def iter_gt_frames(self, *args, **kwargs):
for (left, right), pose, depth in izip(self.iter_stereo_frames(*args, **kwargs),
self.poses.iteritems(*args, **kwargs),
self.gt.iteritems(*args, **kwargs)):
yield AttrDict(left=left, right=right, pose=pose, depth=depth)
def iter_stereo_frames(self, *args, **kwargs):
return self.stereo.iteritems(*args, **kwargs)
@property
def stereo_frames(self):
return self.iter_stereo_frames()
def viz_gt_poses(self):
draw_utils.publish_pose_list('POSES', self.poses.items, frame_id='camera')
class KITTIDatasetReader(object):
"""
KITTIDatasetReader: ImageDatasetReader + VelodyneDatasetReader + Calib
http://www.cvlibs.net/datasets/kitti/setup.php
"""
kitti_00_02 = StereoCamera.from_calib_params(
718.8560, 718.8560, 607.1928, 185.2157,
baseline_px=386.1448, shape=np.int32([376, 1241]))
kitti_03 = StereoCamera.from_calib_params(
721.5377, 721.5377, 609.5593, 172.854,
baseline_px=387.5744, shape=np.int32([376, 1241]))
kitti_04_12 = StereoCamera.from_calib_params(
707.0912, 707.0912, 601.8873, 183.1104,
baseline_px=379.8145, shape=np.int32([370, 1226]))
wheel_baseline = 1.6
baseline = 0.5371 # baseline_px / fx
velo2cam = 0.27 # Velodyne is 27 cm behind cam_0 (x-forward, y-left, z-up)
velo_height = 1.73
cam_height = 1.65
# TODO (sudeep.pillai): Check validity
p_bc = rpyxyz(-np.pi/2, 0, -np.pi/2, 0, 0, 0, axes='sxyz').inverse()
p_bv = rpyxyz(0, 0, 0, -0.27, 0, 0, axes='sxyz')
p_cv = (p_bc.inverse() * p_bv).inverse()
velodyne2body = p_bv
camera2body = p_bc
def __init__(self, directory='',
sequence='',
left_template='image_0/%06i.png',
right_template='image_1/%06i.png',
velodyne_template='velodyne/%06i.bin',
start_idx=0, max_files=100000, scale=1.0):
# Set args
self.sequence = sequence
self.scale = scale
# Get calib
try:
self.calib = kitti_stereo_calib(sequence, scale=scale)
except Exception as e:
self.calib = None
# Read stereo images
seq_directory = os.path.join(os.path.expanduser(directory),
'sequences', sequence)
self.stereo = StereoDatasetReader(
directory=seq_directory,
left_template=os.path.join(seq_directory,left_template),
right_template=os.path.join(seq_directory,right_template),
start_idx=start_idx, max_files=max_files, scale=scale)
print('Initialized stereo dataset reader with %f scale' % scale)
# Read poses
try:
pose_fn = os.path.join(
os.path.expanduser(directory),
'poses', ''.join([sequence, '.txt']))
self.poses_ = FileReader(pose_fn, process_cb=kitti_load_poses)
except Exception as e:
print('Failed to read pose data: {}'.format(e))
self.poses_ = NoneReader()
# Read velodyne
if velodyne_template is None:
self.velodyne = NoneReader()
print('Skipping velodyne data')
else:
try:
self.velodyne = VelodyneDatasetReader(
template=os.path.join(seq_directory,velodyne_template),
start_idx=start_idx, max_files=max_files
)
except Exception as e:
print('Failed to read velodyne data: {}'.format(e))
self.velodyne = NoneReader()
@property
def poses(self):
return self.poses_.items
def iteritems(self, *args, **kwargs):
return self.stereo.left.iteritems(*args, **kwargs)
def iter_stereo_frames(self, *args, **kwargs):
return self.stereo.iteritems(*args, **kwargs)
def iter_velodyne_frames(self, *args, **kwargs):
"""
for pc in dataset.iter_velodyne_frames():
X = pc[:,:3]
"""
return self.velodyne.iteritems(*args, **kwargs)
def iter_stereo_velodyne_frames(self, *args, **kwargs):
return izip(self.left.iteritems(*args, **kwargs),
self.right.iteritems(*args, **kwargs),
self.velodyne.iteritems(*args, **kwargs))
def iterframes(self, *args, **kwargs):
for (left, right), pose, velodyne in izip(
self.iter_stereo_frames(*args, **kwargs),
self.poses_.iteritems(*args, **kwargs),
self.velodyne.iteritems(*args, **kwargs)):
yield AttrDict(left=left, right=right, velodyne=velodyne, pose=pose)
def iter_gt_frames(self, *args, **kwargs):
for (left, right), pose in izip(
self.iter_stereo_frames(*args, **kwargs),
self.poses_.iteritems(*args, **kwargs)):
yield AttrDict(left=left, right=right, velodyne=None, pose=pose)
@property
def stereo_frames(self):
return self.iter_stereo_frames()
@property
def velodyne_frames(self):
return self.iter_velodyne_frames()
@classmethod
def iterscenes(cls, sequences, directory='',
left_template='image_0/%06i.png',
right_template='image_1/%06i.png',
velodyne_template='velodyne/%06i.bin',
start_idx=0, max_files=100000,
scale=1.0, verbose=False):
for seq in progressbar(sequences,
size=len(sequences), verbose=verbose):
yield seq, cls(directory=directory, sequence=seq,
left_template=left_template,
right_template=right_template,
velodyne_template=velodyne_template,
start_idx=start_idx, max_files=max_files)
class KITTIDatasetReader(object):
"""
KITTIDatasetReader: ImageDatasetReader + VelodyneDatasetReader + Calib
http://www.cvlibs.net/datasets/kitti/setup.php
"""
kitti_00_02 = StereoCamera.from_calib_params(718.86, 718.86, 607.19, 185.22,
baseline_px=386.1448, shape=np.int32([376, 1241]))
kitti_03 = StereoCamera.from_calib_params(721.5377, 721.5377, 609.5593, 172.854,
baseline_px=387.5744, shape=np.int32([376, 1241]))
kitti_04_12 = StereoCamera.from_calib_params(707.0912, 707.0912, 601.8873, 183.1104,
baseline_px=379.8145, shape=np.int32([376, 1241]))
baseline = 0.5371 # baseline_px / fx
velo2cam = 0.27 # Velodyne is 27 cm behind cam_0 (x-forward, y-left, z-up)
def __init__(self, directory='',
sequence='',
left_template='image_0/%06i.png',
right_template='image_1/%06i.png',
velodyne_template='velodyne/%06i.bin',
start_idx=0, max_files=50000, scale=1.0):
# Set args
self.sequence = sequence
self.scale = scale
# Get calib
self.calib = kitti_stereo_calib(sequence, scale=scale)
# Read stereo images
seq_directory = os.path.join(os.path.expanduser(directory), 'sequences', sequence)
self.stereo = StereoDatasetReader(directory=seq_directory,
left_template=os.path.join(seq_directory,left_template),
right_template=os.path.join(seq_directory,right_template),
start_idx=start_idx, max_files=max_files, scale=scale)
# Read poses
try:
pose_fn = os.path.join(os.path.expanduser(directory), 'poses', ''.join([sequence, '.txt']))
self.poses = FileReader(pose_fn, process_cb=kitti_load_poses)
except Exception as e:
self.poses = repeat(None)
try:
# Read velodyne
self.velodyne = VelodyneDatasetReader(
template=os.path.join(seq_directory,velodyne_template),
start_idx=start_idx, max_files=max_files
)
except Exception as e:
self.velodyne = repeat(None)
print 'Initialized stereo dataset reader with %f scale' % scale
def iteritems(self, *args, **kwargs):
return self.stereo.left.iteritems(*args, **kwargs)
def iter_stereo_frames(self, *args, **kwargs):
return self.stereo.iteritems(*args, **kwargs)
def iter_velodyne_frames(self, *args, **kwargs):
"""
for pc in dataset.iter_velodyne_frames():
X = pc[:,:3]
"""
return self.velodyne.iteritems(*args, **kwargs)
def iter_stereo_velodyne_frames(self, *args, **kwargs):
return izip(self.left.iteritems(*args, **kwargs),
self.right.iteritems(*args, **kwargs),
self.velodyne.iteritems(*args, **kwargs))
def iterframes(self, *args, **kwargs):
for (left, right), pose in izip(self.iter_stereo_frames(*args, **kwargs), self.poses.iteritems(*args, **kwargs)):
yield AttrDict(left=left, right=right, velodyne=None, pose=pose)
def iter_gt_frames(self, *args, **kwargs):
for (left, right), pose in izip(self.iter_stereo_frames(*args, **kwargs), self.poses.iteritems(*args, **kwargs)):
yield AttrDict(left=left, right=right, velodyne=None, pose=pose)
@property
def stereo_frames(self):
return self.iter_stereo_frames()
@property
def velodyne_frames(self):
return self.iter_velodyne_frames()
# @classmethod
# def stereo_test_dataset(cls, directory, subdir, scale=1.0):
# """
# Ground truth dataset iterator
# """
# left_directory = os.path.join(os.path.expanduser(directory), '%s_0' % subdir)
# right_directory = os.path.join(os.path.expanduser(directory), '%s_1' % subdir)
# noc_directory = os.path.join(os.path.expanduser(directory), 'disp_noc')
# occ_directory = os.path.join(os.path.expanduser(directory), 'disp_occ')
# c = cls(sequence='00')
# c.scale = scale
# c.calib = kitti_stereo_calib(1, scale=scale)
# # Stereo is only evaluated on the _10.png images
# c.stereo = StereoDatasetReader.from_directory(left_directory, right_directory, pattern='*_10.png')
# c.noc = ImageDatasetReader.from_directory(noc_directory)
# c.occ = ImageDatasetReader.from_directory(occ_directory)
# c.poses = [None] * c.stereo.length
# return c
@classmethod
def iterscenes(cls, sequences, directory='',
left_template='image_0/%06i.png', right_template='image_1/%06i.png',
velodyne_template='velodyne/%06i.bin', start_idx=0, max_files=50000,
scale=1.0, verbose=False):
for seq in progressbar(sequences, size=len(sequences), verbose=verbose):
yield seq, cls(
directory=directory, sequence=seq, left_template=left_template,
right_template=right_template, velodyne_template=velodyne_template,
start_idx=start_idx, max_files=max_files)