def test_register_scan(self):
"""Test the generation of global hypotheses."""
self.tracker.register_scan(
lmb.Scan(lmb.sensors.EyeOfMordor(), [
lmb.GaussianReport(np.array([8.0, 8.0]), np.eye(2)),
lmb.GaussianReport(np.array([2.0, 2.0]), np.eye(2))
]))
def draw():
"""Create plot."""
params = lmb.Parameters()
params.N_max = 100
params.kappa = lmb.models.UniformClutter(0.0001)
params.init_target = lmb.DefaultTargetInit(0.1, 1, 1)
params.r_lim = 0.00256 # 0.002564102564102564
params.nstd = 20
params.rBmax = 0.8
params.w_lim = 1e-4
params.maxhyp = 1e2
tracker = lmb.LMB(params)
sensor = lmb.sensors.EyeOfMordor(width=1545, height=1080)
sensor.lambdaB = 0.1
names, detections = read_mot()
for frame in range(min(names.keys()), max(names.keys())):
start = time.time()
if frame > 1:
tracker.predict(1)
reports = {lmb.GaussianReport(
# np.random.multivariate_normal(t[0:2], np.diag([0.01] * 2)), # noqa
(obs[:2] + obs[2:] / 2.0),
np.eye(2) * 5,
lmb.models.position_measurement,
i)
for i, obs in enumerate(detections[frame])}
this_scan = lmb.Scan(sensor, reports)
tracker.register_scan(this_scan)
fps = time.time() - start
# Display tracking results
targets = tracker.query_targets()
print('frame:', frame, datetime.now().strftime("%H:%M:%S"))
img = cv2.imread(path.join('../MOT17-02/img1', names[frame]))
print('enof_targets %s, nof_targets %s, detection(len) %s' % (tracker.enof_targets(),
tracker.nof_targets(), len(detections[frame])))
img = cv2.putText(img, 'Frame {}'.format(frame) + ', FPS:{}'.format(round(1 / fps, 2)),
org=(img.shape[1] - 400, 30), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=1,
color=(255, 255, 255), thickness=2)
for t in targets:
ty, r, x, P = t.history[-1]
img = cv2.circle(img, (int(x[0]), int(x[1])), radius=8, color=(255, 255, 255), thickness=-1)
img = cv2.putText(img, str(t.id),
org=(int(x[0]) + 5, int(x[1]) + 5), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.65,
color=(0, 255, 255), thickness=2)
scale_percent = 60 # percent of original size
width = int(img.shape[1] * scale_percent / 100)
height = int(img.shape[0] * scale_percent / 100)
dim = (width, height)
# resize image
resized = cv2.resize(img, dim, interpolation=cv2.INTER_AREA)
cv2.imshow('Image', resized)
cv2.imwrite('../MOT17-02/output/' + str(frame) + '.jpg', img)
cv2.waitKey(1)
){kind=link}
async def scans(self):
"""Get scans."""
reports = [
lmb.GaussianReport(self.sensor,
lmb.cf.ne2ll(np.random.rand(2, 1) * 99, self.origin),
np.eye(2) * 40)
for _ in range(10)]
lamB = self.lambdaB(reports, self.sensor.fov)
if inspect.isawaitable(lamB):
lamB = await lamB
self.sensor.lambdaB = lamB
yield 0, self.sensor, reports
def test_correct(self):
"""Correction step."""
z = np.array([2.0] * 2)
R = np.eye(2)
params = MagicMock()
params.kappa = MagicMock(return_value=1)
r = lmb.GaussianReport(z, R)
r.sensor = lmb.sensors.EyeOfMordor()
self.pf.correct(params, r)
x = self.pf.mean()
self.assertAlmostEqual(x[0], 0.5, 1)
self.assertAlmostEqual(x[1], 0.5, 1)
self.assertAlmostEqual(x[2], 0.75, 1)
self.assertAlmostEqual(x[3], 0.75, 1)
def look_for_ice(planes, icebergs, k):
"""Retrieve, for each plane, the visible ice."""
for p in planes:
if p.active(k):
fov = Polygon(p.fov)
p.findings = [
i for i in range(icebergs.positions.shape[1]) if fov.contains(
Point(icebergs.positions[0, i], icebergs.positions[1, i]))
]
p.scan = lmb.Scan(
lmb.sensors.SquareSensor(p.fov, p.p_detect, p.lambdaB), [
lmb.GaussianReport(
np.random.multivariate_normal(icebergs.positions[:, i],
Pz_true), p.Pz, i)
for i in p.findings
])
def test_track(self):
"""Test repeated updates from moving targets."""
targets = [
np.array([0.0, 0.0, 1.0, 1.0]),
np.array([0.0, 10.0, 1.0, -1.0])
]
for t in range(3):
if t > 0:
self.tracker.predict(1)
for t in targets:
t[0:2] += t[2:]
reports = {
lmb.GaussianReport(
np.random.multivariate_normal(t[0:2],
np.diag([0.1, 0.1])), # noqa
# t[0:2],
np.eye(2) * 0.1,
lmb.models.position_measurement,
i)
for i, t in enumerate(targets)
}
self.tracker.register_scan(
lmb.Scan(lmb.sensors.EyeOfMordor(), reports))
async def run(self):
"""Run application."""
_LOGGER.debug("Starting application.")
plt.figure(figsize=(30, 30))
model = lmb.CV(0.9, 1.5)
sensor = lmb.PositionSensor()
sensor.lambdaB = 2
sensor.pD = 0.8
targets = [
np.array([0.0, 0.0, 1, 0.5]),
np.array([0.0, 10.0, 1, -0.5]),
]
ntargets_true = []
ntargets_verified = []
ntargets = []
plt.subplot(3, 1, 1)
history = []
origin = np.array([58.3887657, 15.6965082])
sensor.fov.from_gaussian(origin, 20 * np.eye(2))
pre_enof_targets = 0
ospa, gospa = [], []
last_time = 0
gridsize = np.array([-5, 5])
phdpoints = np.empty((2, 40 * 40 / abs(gridsize[0] * gridsize[1])))
i = 0
for x in range(20, -20, gridsize[0]):
for y in range(-20, 20, gridsize[1]):
phdpoints[:, i] = cf.ne2ll(np.array([[x], [y]]), origin)
i = i + 1
for k in range(12):
print()
print("k:", k)
time = k
if k > 0:
await self.predict(model, k, last_time)
tracker_targets = await self.get_targets()
# print("Predicted: ", tracker_targets)
for target in targets:
target[0:2] += target[2:]
reports = [
lmb.GaussianReport(
# np.random.multivariate_normal(t[0:2], np.diag([0.01] * 2)), # noqa
cf.ne2ll(t[0:2, np.newaxis], origin),
np.eye(2) * 0.1,
0.01) for i, t in enumerate(targets)
]
print("Reports: ", reports)
if k > 0:
sensor.lambdaB = max(0.2 * (len(reports) - pre_enof_targets),
0.01 * len(reports))
await self.correct(sensor, reports, k)
tracker_targets = await self.get_targets()
history.append({"time": time, "targets": tracker_targets})
pre_enof_targets = await self.enof_targets()
ntargets.append(pre_enof_targets)
ntargets_verified.append(await self.nof_targets(0.7))
ntargets_true.append(len(targets))
ll_targets = [
np.concatenate((cf.ne2ll(t[0:2], origin), t[2:]))
for t in targets
]
ospa.append(self.tracker.ospa(ll_targets, 1, 2))
gospa.append(self.tracker.gospa(ll_targets, 1, 1))
phd = self.tracker.pos_phd(phdpoints)
print("points size: ", phdpoints.shape)
print("phd size: ", phd.shape)
with open(f"phdfiles/crosstrack_phd_{k:05}.json", 'w') as phdfile:
json.dump(
{
"points": phdpoints.tolist(),
"gridsize": gridsize.tolist(),
"phd": phd.tolist()
}, phdfile)
plot.plot_scan(reports, origin)
plt.plot([t[0] for t in targets], [t[1] for t in targets],
marker='D',
color='y',
alpha=.5,
linestyle='None')
print("Nof clusters: ", self.tracker.nof_clusters)
plot.plot_history(history, origin, covellipse=False, min_r=0.7)
plt.xlim([-1, k + 3])
plt.ylabel('Tracks')
plt.subplot(3, 1, 2)
plt.plot(ntargets, label='Estimate')
plt.plot(ntargets_true, label='True')
plt.plot(ntargets_verified, label='Verified', marker='*')
plt.ylabel('# Targets')
plt.legend(fancybox=True, framealpha=0.5, loc=4, prop={'size': 10})
plt.xlim([-1, k + 3])
plt.subplot(3, 1, 3)
plt.plot(ospa, label="OSPA")
plt.plot(gospa, label="GOSPA")
plt.legend()
plt.xlim([-1, k + 3])
async def run(self):
"""Run application."""
_LOGGER.debug("Starting application.")
plt.figure(figsize=(30, 30))
model = lmb.CV(0.9, 1.5)
sensor = lmb.PositionSensor()
sensor.lambdaB = 2
sensor.pD = 0.8
targets = [
np.array([0.0, 0.0, 1, 0.5]),
np.array([0.0, 10.0, 1, -0.5]),
]
ntargets_true = []
ntargets_verified = []
ntargets = []
plt.subplot(3, 1, 1)
history = []
origin = np.array([58.3887657, 15.6965082])
pre_enof_targets = 0
ospa, gospa = [], []
last_time = 0
for k in range(30):
# print()
print("k:", k)
if k > 0:
sensor.lambdaB = 0.2
await self.predict(model, k, last_time)
tracker_targets = await self.get_targets()
# print("Predicted: ", tracker_targets)
for target in targets:
target[0:2] += target[2:]
# if k == 5:
# targets.append(np.array([5.0, 5.0, 1.0, 0.0]))
if k % 7 == 0:
targets.append(np.random.multivariate_normal(
np.array([k, 7.0, 0.0, 0.0]),
np.diag([1, 0.5, 1, 1])))
if k % 9 == 1:
del targets[-1]
if k == 10:
targets.append(np.array([k, -30.0, 1.0, -0.5]))
# if k == 20:
# targets.append(np.array([k, 0.0, 1.0, 4.0]))
reports = [lmb.GaussianReport(
# np.random.multivariate_normal(t[0:2], np.diag([0.01] * 2)), # noqa
cf.ne2ll(t[0:2, np.newaxis], origin),
np.eye(2) * 0.1, 0.01)
for i, t in enumerate(targets)]
sensor.lambdaB = max(0.2 * (len(reports) - pre_enof_targets), 0.01 * len(reports))
await self.correct(sensor, reports, k)
tracker_targets = await self.get_targets()
history.append((k, tracker_targets))
pre_enof_targets = await self.enof_targets()
ntargets.append(pre_enof_targets)
ntargets_verified.append(await self.nof_targets(0.7))
ntargets_true.append(len(targets))
ll_targets = [np.concatenate((cf.ne2ll(t[0:2], origin), t[2:])) for t in targets]
ospa.append(self.tracker.ospa(ll_targets, 1, 2))
gospa.append(self.tracker.gospa(ll_targets, 1, 1))
plot.plot_scan(reports, origin)
plt.plot([t[0] for t in targets],
[t[1] for t in targets],
marker='D', color='y', alpha=.5, linestyle='None')
print("Nof clusters: ", self.tracker.nof_clusters)
plot.plot_history(history, origin, covellipse=False, min_r=0.7)
plt.xlim([-1, k + 3])
plt.ylabel('Tracks')
plt.subplot(3, 1, 2)
plt.plot(ntargets, label='Estimate')
plt.plot(ntargets_true, label='True')
plt.plot(ntargets_verified, label='Verified', marker='*')
plt.ylabel('# Targets')
plt.legend(fancybox=True, framealpha=0.5, loc=4, prop={'size': 10})
plt.xlim([-1, k + 3])
plt.subplot(3, 1, 3)
plt.plot(ospa, label="OSPA")
plt.plot(gospa, label="GOSPA")
plt.legend()
plt.xlim([-1, k + 3])