def analyze_imu(path):
r""" Plots both angular velocities and linear accelarations over time from this sequence"""
f, ax = plt.subplots(2, sharex=True)
ax[0].set_title('Acceleration')
X = np.full([3], np.nan)
ACC = np.full([3], np.nan)
GYR = np.full([3], np.nan)
names = ['X Y Z'.split(), 'pitch roll yaw'.split()]
# Create a dataset
sequence = Sequence(path)
for imu in sequence.imu:
X = np.vstack((X, 3 * [imu.stamp]))
ACC = np.vstack((ACC, imu.acceleration))
GYR = np.vstack((GYR, imu.angular_velocity))
for i in range(3):
ax[0].plot(X[:, i], ACC[:, i], label=names[0][i])
ax[1].plot(X[:, i], GYR[:, i], label=names[1][i])
ax[0].set_title('Acceleration [m/s^2]')
ax[0].legend()
ax[0].grid(True)
ax[1].set_title('Angular Velocity [rad/s]')
ax[1].legend()
ax[1].grid(True)
plt.xlabel('Time [s]')
plt.show()
def analyze_frames(path, shutter, speed=1.):
# Create a dataset
sequence = Sequence(path)
# Iterate over all stereo images for the given shutter method
for stereo in sequence.cameras(shutter):
# Create a combined stereo image
display = np.concatenate((stereo.L.load(), stereo.R.load()), axis=1)
# Write some information on it
text = 'Frame %s | Stamp %s | L' % (stereo.ID, stereo.stamp)
cv2.putText(display,
text,
org=(50, 50),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=1,
color=(0, 0, 0))
text = 'Frame %s | Stamp %s | R' % (stereo.ID, stereo.stamp)
cv2.putText(display,
text,
org=(display.shape[1] // 2, 50),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=1,
color=(0, 0, 0))
# Resize the display to fit the screen
display = cv2.resize(display, (0, 0), fx=.5, fy=.5)
cv2.imshow('ShutTUM', display)
# Do the spinning
if speed == 0: cv2.waitKey(0)
else: cv2.waitKey(int(stereo.dt(ifunknown=.1) * 1000 / args.speed))
cv2.waitKey(0) # wait until user terminates
from ShutTUM.sequence import Sequence
# Intialize
ros.init_node('playback')
ros.set_param('/use_sim_time', True)
loop = ros.get_param('~loop', False)
start = ros.get_param('~start', None)
end = ros.get_param('~end', None)
speed = ros.get_param('~speed', 1)
img_topic = ros.get_param("~image_topic", "image_raw")
is_calib = ros.get_param('~calibration', False)
distmodel = ros.get_param('~distortion_model', 'fov')
bridge = CvBridge()
sequence = ros.get_param('~sequence')
dataset = Sequence(sequence, calibration=is_calib)
msg = 'Playback started [%s]' % sequence
if loop: msg += ' [LOOP]'
if start: msg += ' [START: %s s]' % start
else: start = None
if end: msg += ' [END: %s s]' % end
else: end = None
if speed != 1: msg += ' [%.2fx]' % speed
if speed == 0:
ros.logwarning('Speed is zero, results might not be as expected')
ros.loginfo(msg)
# Create Publishers
clock = ros.Publisher('/clock', Clock, queue_size=10)
tffer = tf.TransformBroadcaster()
def setUp(self):
d = p.dirname(p.realpath(__file__))
self.path = p.join(d, 'framedrop')
self.sequence = Sequence(self.path)
def setUp(self):
d = p.dirname(p.realpath(__file__))
self.sequence = Sequence(p.join(d, 'valid'))
def analyse_frame_drops(path, include_log=False):
r""" Returns an 2D array with the frames recorded by the cams
The four columns correspond to cam1 .. cam4.
The N rows correspond to the N triggered frames. The value of each
cell shows, if the camera has captured this frame (by putting its caputured ID
into the cell) or if it dropped this frame (by leaving the cell at zero).
When you use ``include_log`` a tuple will be returned, with the array as first part
and a string as formatted table as second part.
"""
sequence = Sequence(args.sequence, stereosync=False)
shutters = dict(
map(lambda (k, v): (k, v[0].upper()), sequence.shutter_types.items()))
symbol = lambda (x): '%05d' % x if x > 0 else '-----'
def limit(msg, n):
if len(msg) < n: return msg.ljust(n)
return '%s...' % msg[0:n - 3]
frames = np.zeros(
(sequence.raw.frames.shape[0] + 1, 4)) # skip zeroth frame
gids = [0, 1] if shutters['cam1'] == 'G' else [3, 2]
rids = [0, 1] if shutters['cam1'] == 'R' else [3, 2]
for img in sequence.cameras('global'):
if img.L is not None: frames[img.ID, gids[0]] = img.ID
if img.R is not None: frames[img.ID, gids[1]] = img.ID
for img in sequence.cameras('rolling'):
if img.L is not None: frames[img.ID, rids[0]] = img.ID
if img.R is not None: frames[img.ID, rids[1]] = img.ID
if not include_log:
return frames
else:
msg = ""
msg += '=====================================================================\n'
msg += '| ShutTUM Sequence Sequence: %s |\n' % limit(args.sequence, 37)
msg += '+===================================================================+\n'
msg += '| Frames (%s triggered), dropped frames: |\n' % symbol(
frames.shape[0] - 1)
msg += '+----------------+----------------+----------------+----------------+\n'
msg += '| cam1 (%s) | cam2 (%s) | cam3 (%s) | cam4 (%s) |\n' % (
shutters['cam1'], shutters['cam2'], shutters['cam3'],
shutters['cam4'])
msg += '+----------------+----------------+----------------+----------------+\n'
for idx, line in enumerate(frames):
if idx == 0: continue
if np.all(line): continue
msg += '|'
for id in line:
msg += ' %s |' % symbol(id)
if not np.any(line): msg += ' (frame %05d)\n' % idx
else: msg += '\n'
drops = (frames > 0).sum(axis=0)
msg += '+----------------+----------------+----------------+----------------+\n'
msg += '|'
for drop in drops:
msg += ' %05s (%02.1f%%) |' % (drop, 100. * drop /
(frames.shape[0] - 1))
msg += '\n+----------------+----------------+----------------+----------------+\n'
return frames, msg
def play(sequence_path, shutter, side, debug=False, options=[], dso_prefix=''):
r""" Let's DSO run for a specific camera on a sequence from the ShutTUM dataset
:param sequence_path: (str) the path to the ShutTUM sequence
:param shutter: {'global', 'rolling'} which shutter type of the sequence shall be used
:param side: {'L', 'R'} to work with the left or right camera
:param debug: (bool) prints the executing command & does not remove the .temp directory
:param options: (list(str)) additional arguments for dso in the format [name]=[value] (e.g. quiet, nogui, nolog...).
:param dso_prefix: (str) a path prefix to where the 'dso_dataset' executable lies (default '')
Caution with nogui=0 this function will not terminate until you close Pangolin
:return: a 2D numpy array (shape Nx8 with N=number of images) with the columns [Time X Y Z W X Y Z]
"""
sequence = Sequence(sequence_path)
temp = p.join(sequence_path, '.temp')
if p.exists(temp): shutil.rmtree(temp)
os.mkdir(temp)
cams = sequence.cameras(shutter)
# Create the camera.txt file for DSO
calib = p.join(temp, 'camera.txt')
sequence.stereosync = True
firstframe = next(iter(cams))
firstframe = firstframe.L if side == 'L' else firstframe.R
with open(calib, 'w') as file:
file.write('%.6f\t%.6f\t%.6f\t%.6f\t%.6f' %
(firstframe.focal.x / sequence.resolution.width,
firstframe.focal.y / sequence.resolution.height,
firstframe.principle.x / sequence.resolution.width,
firstframe.principle.y / sequence.resolution.height,
firstframe.distortion('fov')))
file.write(os.linesep)
file.write('%d\t%d' % sequence.resolution)
file.write(os.linesep)
file.write(
'0.4\t0.53\t0.5\t0.5\t0'
) # post-rectification equivilant pinhole model [fx fy px py omega]
file.write(os.linesep)
file.write('640\t480')
with open(p.join(sequence_path, 'frames', 'times.txt'), 'w') as file:
i = 0
sequence.stereosync = False
for image in sequence.cameras(shutter):
i += 1
cam = image.L if side == 'L' else image.R
if cam is None:
if debug: print(" -> Frame drop %d, skipping..." % image.ID)
continue
file.write('%d\t%.6f\t%.3f' % (cam.ID, cam.stamp, cam.exposure))
file.write(os.linesep)
images = p.join(sequence_path, 'frames', firstframe.reference)
gamma = p.join(sequence_path, 'params', firstframe.reference, 'gamma.txt')
vignette = p.join(sequence_path, 'params', firstframe.reference,
'vignette.png')
try:
if not filter(lambda o: o.startswith('nogui='), options):
options.append('nogui=1')
cmd = [
p.join(dso_prefix, 'dso_dataset'),
'files=%s' % images,
'calib=%s' % calib,
'gamma=%s' % gamma,
'vignette=%s' % vignette, ' '.join(options)
]
if debug: print(' '.join(cmd))
DEVNULL = open(os.devnull, 'w')
check_call(cmd, stdout=DEVNULL, stderr=STDOUT)
except CalledProcessError:
pass # CTRL-C
else:
if not p.exists('result.txt'): return None
results = np.genfromtxt('result.txt')
results[:, [4, 5, 6, 7]] = results[:, [7, 4, 5,
6]] # Quaternion xyzw -> wxyz
return results
finally:
if not debug: shutil.rmtree(temp)
def setUp(self):
d = p.dirname(p.realpath(__file__))
self.sequence = Sequence(p.join(d, 'valid'))
self.raw = self.sequence.raw.groundtruth[0, :]
self.gt = GroundTruth(self.sequence, self.raw)
def setUp(self):
d = p.dirname(p.realpath(__file__))
self.path = p.join(d, 'valid')
self.sequence = Sequence(self.path)
self.data = self.sequence.raw.frames[0, :]
self.stereo = StereoImage(self.sequence, self.data, 'rolling')