1

Detection and Tracking of General
Movable Objects in Large 3D Maps

arXiv:1712.08409v2 [cs.RO] 30 Jan 2018

Nils Bore, Johan Ekekrantz, Patric Jensfelt and John Folkesson
Robotics, Perception and Learning Lab
Royal Institute of Technology (KTH)
Stockholm, SE-100 44, Sweden
Email: {nbore, ekz, patric, johnf}@kth.se
Abstract—This paper studies the problem of detection and tracking of
general objects with long-term dynamics, observed by a mobile robot
moving in a large environment. A key problem is that due to the
environment scale, it can only observe a subset of the objects at any
given time. Since some time passes between observations of objects in
different places, the objects might be moved when the robot is not
there. We propose a model for this movement in which the objects
typically only move locally, but with some small probability they jump
longer distances, through what we call global motion. For filtering, we
decompose the posterior over local and global movements into two linked
processes. The posterior over the global movements and measurement
associations is sampled, while we track the local movement analytically
using Kalman filters. This novel filter is evaluated on point cloud data
gathered autonomously by a mobile robot over an extended period of time.
We show that tracking jumping objects is feasible, and that the proposed
probabilistic treatment outperforms previous methods when applied to
real world data. The key to efficient probabilistic tracking in this scenario
is focused sampling of the object posteriors.
Keywords—Mobile robot, multi-target tracking, movable objects, mapping.

I.

I NTRODUCTION

Mobile robots often operate in large environments that can not
be fully observed without moving. In fact, the floor plan of most
buildings is divided into rooms, which the robot can only visit one at
a time. This setup creates an interesting problem if the robot is to keep
track of specific objects. For example, imagine a care robot that helps
elderly keep track of their belongings, as suggested in [1]. Since the
robot cannot be in all places simultaneously, it will not observe most
objects as they move, as is required by classical tracking techniques.
We might instead rely fully on appearance to identify the objects, but
this also fails if the objects are visually similar. To enable tracking in
this scenario, we need to take into account that the objects might have
moved while we were not observing them. Imagine we are looking
for a patient’s cell phone, and that we last saw it in her room. In
general, we should expect to find the object in the same place, or
somewhere nearby. Further, if the robot looks, and the phone is not
there, the next hypothesis is that it should be somewhere within the
closed environment of the patient’s department or the care home. Any
further guesses need to be based on knowledge of who might have
taken the phone and to where. These common sense intuitions should
form the basis for any tracking algorithm that works even when the
robot does not continuously observe the targets.
The need for tracking specific instances of objects arises in several
robotic applications, for example when mapping in highly dynamic
[2] and long-term scenarios [3], obstacle avoidance [4][5] and object
search [6]. Of particular interest to us is surveillance of objects with
long-term dynamics moving in large environments. The application
we have in mind is a robot tasked with monitoring a number of mostly
static objects that typically do not leave some closed environment,
such as in the care scenario described above. Another application
is security, where a robot should monitor the presence for example
of important items in an office. In these surveillance applications, a
probabilistic model for the objects’ positions is essential for the robot,
since it aids it in knowing where to look. In addition to helping us
distinguish similar objects, a tracking framework with a probabilistic

motion model provides us with such distributions, as illustrated in
Figure 1. When the robot covers more locations, the model should
refine the distributions to aid the search.
As mentioned in [7], tracking of instances is also necessary when
we would like to learn explicit movement models for objects. This is
true both for instance level models and more general object categories.
For example, to estimate a movement model for the mug category, it
does not suffice to study the collective movement of mugs. This can
be illustrated by a collection of mugs standing in a kitchen drying
rack. If the method can only estimate the number of mugs in the rack,
it will fail to detect that people are leaving washed mugs, in order to
pick them up later. Therefore, a reasonable algorithmic model would
be that of a queue, rather than a set. More generally, a cumulative
model will fail to capture any such interactions of objects.
Since a mobile robot cannot observe all objects in a large environment simultaneously, some of them will be moved when the robot is
not looking. Most often, objects only move locally, as humans might
use them for some task and then put them down. However, objects
sometimes move in unpredictable ways, to entirely different parts of
the robot environment. This might happen if the task is completed
somewhere else than the original object position, as when drinking
from a cup brought from the kitchen. A key insight is that these
latter movements happen much more seldom than small adjustments
of object positions. We therefore propose decomposing the modeling
of object movement into two parts: local movements describing the
small adjustments, and rarer global movements for longer jumps.
In this scheme, we can explain most object detections with local
movements from the previous positions. One concrete advantage of
this can be highlighted by an example where we have observed two
visually similar objects. With new observations, the first object is well
explained by local motion from the previous position. If the second
is not but a similar object is observed elsewhere, we can conclude
that the second is more likely to have jumped there than the first.
This matches well with our human intuition.
To track objects within the maps, we need to generate object
detections from the scene observations. In previous work on similar
topics [7][3], it has been popular to use scene differencing for
detection. This is natural, as we are only interested in tracking
objects that move; static objects are trivial to track. But even movable
objects often remain static for long periods of time, necessitating
a mechanism for detection when they do not move. Methods have
been proposed for extending 3D scene differencing temporally, to
segment one movable object across a sequence of scene observations
[8]. In this work, we propose restricting such an extension to only the
unambiguous parts. In practice, this means the we identify two parts
in different scenes as the same only if they are static between the
observations. As we will show, such a scheme can still segment the
movable objects in all frames. Importantly, it defers movable object
clustering to the tracker, which incorporates a probabilistic motion
model and can therefore handle uncertainty.
In our scenario, the task is assumed to be to track and monitor
a fixed set of objects. The robot patrols one floor of a building at
regular intervals, visits all relevant locations and builds local 3D
maps. From each map observation, we can extract a set of object
detections and corresponding visual features. In the experiments
we use CNN features, which proved to discriminate well between
dissimilar objects. Although it only takes a few minutes to travel
between the different locations and construct the maps, we expect
the robot to visit the same location again on the time scale of a few
hours. Since it can only survey one location at a time, the majority
of the objects we are interested in will not be observed for hours
at a time. Between observations the objects move locally within the
locations and will occasionally jump to different locations. The sparse

Submitted to IEEE Transactions on Robotics, October 2017. c 2017 IEEE. Personal use of this material is permitted. Permission from IEEE must be obtained
for all other uses, in any current or future media, including reprinting/republishing this material for advertising or promotional purposes, creating new collective
works, for resale or redistribution to servers or lists, or reuse of any copyrighted component of this work in other works.

2

(a) Position probabilities of chair 1.

(b) Position probabilities of chair 2.

(c) Position probabilities of chair 3.

Fig. 1: Example of our system tracking objects: the current detections of objects (red) and location probabilities (blue-violet) for three different
chairs. The chairs are visually similar, making long-term tracking just from visual or shape features difficult.

observations and the two movement modes complicate inferring the
object positions and demand careful modeling of probabilities.
The main idea presented in the paper brings together modeling of
discrete object jumps in between locations and smaller local variation
in a probabilistic multi-target tracking framework. We survey previous
work of relevance and present a key set of assumptions which make
the problem tractable. Since the method is developed for tracking in
the context of a mobile robot patrolling an environment, we have
developed a complete system to verify the viability of the approach.
In summary, we present the following contributions:
1) A multi-object tracking model for continuous movement paired
with larger discontinuous movement that happens when the
robot is somewhere else
2) A practical, efficient inference scheme that fully captures the
proposed underlying model
3) Experimental validation on a robot and in simulation
II.

R ELATED W ORK

This paper sits atop and draws upon a variety of different disciplines, including mapping, computer vision and Bayesian inference.
As such, we focus on a few key areas where we find the most
relevant work. Most studies that address detection and tracking of
movable objects in a mobile robotic framework are found within
the area of robot mapping. In this field, considerable effort has
been spent to handle dynamics in order to better model noise in
the measurements and to improve localization [9][10]. Others aim
to infer object positions with respect to the map, as described
in the sections below. Our work is a variant of the multi-target
tracking problem; another area of relevance which we discuss in the
subsequent paragraphs. Finally, we survey related work on 3D object
detection and segmentation.
Within the fields of robot mapping and localization, there have
been substantial contributions to solving detection and tracking of
multiple objects, referred to as the DATMO problem [11]. The area
can be divided into approaches dealing with short-term dynamics
such as cars and humans [2][12], and long-term dynamics, such
as furniture [13][7][3][14]. In a robotic setting, short-term dynamic
objects are typically continuously observed while long-term, or semistatic, objects are only observed every once in a while, during
which the objects might have moved. In the following, we will
focus on methods that deal with the latter problem, corresponding
to our problem formulation. Biswas et al. [3] studies the problem of
associating dynamic objects between measurements at different times.
Besides the static map, they also keep track of a set of objects, each
associated with at set of measurements. While [3] do not incorporate
a movement model, thus allowing arbitrary movement, they use
the mutual exclusion constraint, which disallows association of two

detections made at the same time to the same object. A more recent
system dealing with objects that are moved was presented in [15]. The
authors specifically address the problem of modeling how long objects
can be expected to stay at their last observed position. In addition, they
present a scheme for learning the model from observations. While we
do not employ the proposed probabilistic model for this time interval,
it can be integrated into our system by modifying our prior model.
The GATMO system [7] presented by Gallagher et al. is the method
that comes closest to realizing the vision we present in this paper. In
fact, that system addresses the problem of tracking short-term, as
well as long-term dynamics. The method detects movable objects by
means of scene differencing on 2D laser data The discovered objects
are kept in a database of categories, with an object moving to an
”Absent” category if it is no longer in a similar position as when
last observed. It may move back to the ”Movable” category if there
is a new detection somewhere else with a similar laser signature.
Also worth noting here is earlier work of Schulz et al. [13] and
Wolf et al. [16], who present similar systems, but only for tracking
moveable objects locally within a map. Notably, [13] also incorporates
the localization uncertainty of the robot when estimating the object
positions. GATMO [7] and to some extent [13][16] address the same
general problem that we do. We improve on [13][16] by also tracking
larger scale motion, necessitating joint modeling of the objects. Our
probabilistic model is an important improvement on the GATMO
[7] system. As demonstrated by our experiments, it enables robust
performance also in the presence of noise. Lastly, unlike these earlier
systems, the proposed system operates on top of 3D maps.
At the basic level, our problem is a multi-target tracking (MTT)
problem, . Many techniques have been proposed to address MTT but
few address the problem in a setting similar to ours. Our system
must track the discrete process of jumping between a finite number
of locations as well as the continuous 2D position of the objects.
Typically, this also includes inferring the association of measurements
to targets. However, explicitly representing associations between
targets, locations and measurements requires a high-dimensional state
space and is not feasible. Instead, we focus on sampling based particle
filters. To improve performance in high-dimensional state spaces one
can sometimes use analytic representations of the continuous part,
resulting in a so-called Rao-Blackwellized particle filter (RBPF).
RBPFs have been proposed for multi-target tracking in a number of
papers, e.g. [17][18][19]. Our approach is inspired by that of Särkkä
et al. [17], which integrates one measurement at a time and samples
correspondences to the tracked targets. Given the associations, the
continuous positions can be tracked using classical Kalman filters.
However, our sampling scheme is more similar to the method of Vu
et al. [20], which uses MCMC techniques to estimate the associations
of several measurements jointly. Another approach to MTT is Proba-

3

bility Hypothesis Density (PHD) filters [21][22], which maintain the
combined target intensity function rather than the posteriors. As we
are interested in explicit object posteriors, e.g. for object search, these
methods are not directly relevant to this paper.
In the broader field of 3D object detection and segmentation,
there are several works relevant to ours, particularly in regards to
segmentation. In [8], Herbst et al. present a system which jointly
segments scenes observed at different times. The method uses an
elegant multi-scene MRF formulation and graph cuts to segment
objects that move in between observations. This has the advantage of
providing consistent segmentation boundaries by comparing several
static scenes to each other at once, thus filtering out noise. While the
output of this segmentation is the same as ours, it is unlikely that
method would scale from the tens of scenes presented in the paper to
the hundreds that might be collected by a robot. Instead, our method
can run in an online manner, which is desirable on a robot platform.
In [23], Finman et al. present a complete system for object discovery.
Their method detects objects from pair-wise scene differencing and
associates new detections with previously observed object models.
Similar to us, they update the visual object models online using a
tracker, but do not explicitly model specific object instances or their
positions. In [24] we introduced a scene differencing method that
was also used in [25]. The method finds objects by comparing to a
background model of the environment, called a meta-room. In the
current work, we do not rely on a background model. Instead, we
compare precisely registered depth frames of subsequent observations,
allowing us to reliably detect smaller objects than [24], such as
mugs. Another advantage to [24] is the option of joint segmentation,
allowing us to segment static objects in the past if they move in
subsequent observations.
In a subsequent work [25], we studied the related problem of
offline clustering of object instances that have been observed within
one room at different times. One of the main conclusions was that
lighting variation or inability to always observe objects from similar
angles present problems for object recognition in this application. To
alleviate this problem, we proposed to group the initial clusters of
visual features by spatio-temporal coherency. Importantly, the second
step makes sure no objects observed at the same time are assigned to
the same cluster. In the current work, we approach the problem from
a different angle, by studying objects that may also move between
locations but never completely leave or enter the robot environment.
Further, while the offline clustering algorithm in [25] is greedy, we
propose an online probabilistic framework where we perform joint
data association.
To summarize, while [25][16][13] track local movement using
features and [3][14] track only using features, ignoring position,
none of them formulate a full motion model that can track general
objects. Only [7] studies the full problem of tracking general longterm dynamic objects within large environments, but without a
specified motion model or a noise model. We improve on their work
by incorporating motion and noise priors in a probabilistic model
working on 3D data. This enables reliable estimates even in the noisy
environments where many robots find themselves. Detections come
from a simple temporally consistent segmentation logic that combines
the precision of [8] with the fast sequential updates of [24].
III.

M ETHOD

A robot moves between a finite number of locations l ∈ L
in an environment. Its task is to monitor a number of semi-static
objects j ∈ 1 . . . N at each time step k. For this, the robot
needs to reason about the current locations lj,k of the objects, and
their exact positions x̂j,k within the locations. At each time k, it
observes one of the locations lky , giving it a sequence of Mk point

Fig. 2: The change detection result in one local map. There are
different sources of detections, coming from forward change detection
(green), backward change detection (red) and propagated detections
(yellow). The hole in the center of the map is due to the robot standing
in that position when collecting the local map.

measurements Yk = {y1,k , . . . , yMk ,k }. Each point comprises a
f
s
, together with a visual feature vector ŷm,k
, each
2D position ŷm,k
with some noise. While some of the measurements correspond to
one of the N objects it is monitoring, others originate from other
objects or spurious noise. To know which objects to monitor, the
initial positions x̂1,0 , . . . , x̂N,0 are given. Our tracking formulation
is based on the closed world assumption, meaning no object j enters
or fully leaves the environment. This is justified, since the tracking
system can still maintain a distribution over possible positions in the
environment, while concluding it does not know where an object is
if the uncertainty grows too high.
In the following section, we briefly describe how our method
generates the point measurements Yk from RGBD scans of the
current location lky . Then in subsequent Sections III-B1 and III-B2,
we detail the local and global process models, respectively. Finally, in
Section III-B3 and onwards, we describe how to combine the models
into a joint posterior and then propose a framework for efficient
inference.
A. Map Differencing and Consistent Segmentation
As our robot patrols the environment, it performs 360◦ 3D sweeps
using its RGBD sensor at the pre-specified locations L. The RGDB
frames are registered into local maps, see [25] for details. Our aim
is to track objects moving within our environment. It is therefore
natural to use change detection techniques [24][23][26] to detect
image segments corresponding to moving objects.
Change detection only detects objects that move in between two
time points k and k + 1. However, if at time k + 2 any of the objects
remained in the same place as in k + 1, we would like to still detect
them, as this is vital information for our object tracking. That is,
we need to distinguish between not having moved and not being
there any more. Therefore, we add a new component to the detection
system wherein we propagate detections of objects into previous
and subsequent observations. This allows us to in theory segment
all instances of objects that have moved at any point in the robot’s
observations. When formulating the principles for propagating the
detected objects, there are several scenarios to take into account. First,
an object can appear as well as disappear, allowing it to be detected by
change detection. We distinguish between detection of appearing and
disappearing objects, by referring to the processes as forward and

4

Moves

Before

Static

Static

Moves

After

...

Forward propagated detection

Forward detection

Backward detection

Present
Absent
t1

t2

...

t3

tN −2

tN −1

tN

Fig. 3: A real world example where a chair moves and then moves again. It is detected through forward change detection and the detection is
propagated into subsequent frames. The second movement is detected via the backward change detection pass.

Backward prop.

Absent

t1

Backward prop.

Local movement

Present
t2

t3

t4

t5

Forward prop.

Occluded
t6

t7

t8

t9

t10

t11

t12

t13

Fig. 4: A more complex example where the object moves several times and with occlusion. Note that small local movements will be registered
as a backward and a forward detection at the same time step. When occluded, we do not register any measurements.

backward change detection, respectively. If an object has appeared
in frame k + 1, it could be present in the subsequent frames while
backward detections might have been present in previous frames. We
thus compare all the registered depth image frames in observation
k+1 with those in k+2 to see if the pixels corresponding to the object
have similar values, see Figure 3. The object detections are propagated
in either direction until the depth values do no longer correspond or
we detected an object change in the opposite direction, indicating that
the object disappeared, see Figure 4. Finally, a complicating factor is
the objects might be occluded. While one could use the observations
before and after an occlusion to model the probability of the object
being there, we do not to attempt any such reasoning in this work.
To summarize, several passes determine the final object detections.
First, our algorithm computes forward- and backward change detection passes by comparing each subsequent pair of observations. Then,
a forward pass propagates all the detected objects to the subsequent
static observations of the same object. No propagated detection is
added when it has already been detected by another pass. Finally,
a backward pass propagates the detections from backward change
detection. The output of the segmentation of one local map is shown
in Figure 2. Note that the method can be modified to run online
simply by skipping the backward detection and propagation steps.
B. Object Posteriors
Now we turn to the problem of associating the detections from
every time step with objects in order to track them. The modeling
of object movement can be described in terms of a combination of
two different processes. On the one hand, an object often moves
locally in one place, as illustrated in Figure 5a. Picture, for example,
a chair in front of a desk; it frequently moves a bit when the user
is getting up or adjusting the seating position. More rarely, objects
might also move somewhere completely different, as illustrated in
Figure 5b. To be able to track these rare movements, we need to

reason situations when a tracked object’s visual descriptor does not
match anything nearby its previous position. If its descriptor is also
close to that of a new, unexplained object, we might be confident the
object has ”jumped”. Since the robot can only observe objects within
one location l ∈ L at each time step, estimating lj,k for each of the
objects will prove essential to our treatment of the problem. In the
following, we describe our modeling of the two processes separately
and then how to combine them.
1) Local Movement: Individual object, or target, states xj,k and
measurements ym,k at time k are composed of continuous object 2D
position ˆ·s and descriptor ˆ·f vectors, as well as a discrete location
l ∈ L,




 
s
x̂sj,k
ŷm,k
 , lj,k  , ym,k = 
 , lky  .
xj,k = 
f
x̂fj,k
ŷm,k
The continuous states x̂ can be observed directly, with some noise
and therefore have the same dimension as the measurements ŷ. Note
s
that for the measurements, the continuous location ŷm,k
also uniquely
y
defines the discrete location lk . We denote the set of target states at
time step k, Xk = {x1,k , . . . , xN,k } and the measurements Yk . If
the objects move only locally, and if we assume Gaussian noise for
the process, and for the position and descriptor measurements, we
can describe the system using linear dynamics. Given assignments
cj,k = m that map each target j to its corresponding measurement
m, we may use the standard update equations for the Kalman filter
to track each target separately. With the prior that the object stays
locally, the state will simply propagate as the previous state plus
some normally distributed noise. The continuous states can further
be directly observed, giving us
x̂j,k = x̂j,k−1 + qj,k−1
ŷk,cj,k = x̂j,k + rj,k

,

(1)

5

(a) Local, small movements of the objects within the locations.

(b) Global movements, jumping within or between different locations.

Fig. 5: Illustration of the possible movements of three objects. Arrows indicate possible movements. The objects can move locally, within a
certain area. Additionally, they may also jump to new locations in completely different areas of the environment.

with qj,k−1 and rj,k denoting process noise and measurement noise,
respectively. The feature part has no process noise but we do assume
normally distributed measurement noise. In our application this is
a reasonable assumption, as discussed in Section IV-A. Note that
our model can incorporate other distributions. In this local model,
the location lj,k never changes but it is implicitly given by the last
measurement associated with the target.
2) Global Movement & Associations: In addition to the continuous
pose, our objects can also jump between several discrete locations
l ∈ L. These often correspond to rooms such as offices or different
parts of a hallway. At each time point, we assume an object might
take the ”action” uj,k ∈ {jump, no jump} of jumping to a random l
or staying,

 pjump , if uj,k = jump
.
(2)
p(uj,k ) =
 1 − pjump , if u = no jump
j,k

If object j did not jump, we know it stayed in the same location as
before, uj,k = no jump ⇒ lj,k = lj,k−1 . In the event of a jump, it
may uniformly jump to any of the Nl = |L| locations in the robot
environment. There is therefore the probability N1l of jumping to
l −1
the location lky we are currently observing and NN
of jumping to
l
any of the other locations. If the number Nl is large, we gain little
information about an object’s location from knowing it is missing
from one of the locations. Instead, for simplicity we introduce a new
location lunknown , indicating that we believe the object jumped to a new
location, but we do not know which. Therefore, if lj,k = lunknown , we
already know j jumped in subsequent time steps, and the location
stays lunknown until it is associated with a measurement, lj,k−1 =
lunknown ⇒ uj,k = jump. As we will see, this simplification helps
improve our inference. Thus, the location priors are

1


, if lj,k = lky

 Nl
Nl −1
p(lj,k |uj,k = jump, lky ) =
, if lj,k = lunknown .
Nl



 0, for other l ∈ L
Intrinsically, multi-target tracking is a problem of estimating the
measurement associations cj,k . Now, cj,k can take on measurement
indices m ∈ 1, . . . , Mk as well as , which indicates the target did
not give rise to any measurement at this time. If, for example, the last
target estimate lj,k−1 is not at the current observation location lky , and
the target did not jump, the probability of not getting a measurement
of the target is 1, since we can only observe one location at a time.
Even if the object is at the estimated location, it is detected with

some large probability pmeas , allowing for some errors in the detector.
Together, this gives us the measurement prior model,

 1, if lj,k 6= ly
k
y
,
p(cj,k = |lj,k , lk ) =
 1 − pmeas , if l = ly
j,k

k

and correspondingly, if each of the measurements in the location are
a-priori equally likely to originate from the object,
p(cj,k = m|lj,k , lky ) =

1
(1 − p(cj,k = |lj,k , lky )) .
Mk

(3)

With the conditional priors in hand, we can compute
the individual target association priors p(cj,k |lky , lj,k−1 ) =
p(cj,k |lky , lj,k )p(lj,k |lky , lj,k−1 , uj,k )p(uj,k ), see Table I. Since
the associations encode the locations lj,k except for when cj,k = ,
we simplify notation by taking ck to mean all associations and
locations at time k. Note that the individual association priors are
conditioned only on cj,k−1 , as opposed to the full ck−1 . Further,
there is no closed form expression to combine them into a joint
prior p(ck |c1:k−1 , Y1:k−1 ), since we need to disallow assignments
of targets to the same measurement. While we never sample from
this specific distribution, it is relevant to talk about how one might
do so here; we will use the same techniques later on for the proposal
distribution.
One option to sample from the joint association prior is to use
MCMC methods. In our case, one might use blocked Gibbs sampling
[27] of two random target assignments at a time, conditioned on the
other assignments. It is important to sample the assignments in blocks
of several targets since this allows the targets to switch measurement
assignments. The Gibbs sampling procedure allows us to keep the
probability of assigning a measurement in the current room fixed,
even when most of the observations are already assigned to another
target. In effect, this is analogous to adaptively changing the set of
measurements Yk depending on which are unassigned at that iteration of the sampling procedure, and sample from p(cj,k |lky , lj,k−1 )
computed over this modified set of measurements.
While we have found that Gibbs sampling of the joint prior is
indeed feasible, it might be unnecessarily slow. We can also approximate the distribution using the assumption that the target locations
lj,k are independent. However, there is still the constraint over ck that
no two targets can be associated with the same measurement, giving

 0, if ∃j 6= j 0 : cj,k = c 0 6= 
j ,k
p̃(ck |c1:k−1 , Y1:k−1 ) ∝
. (4)
∼ Q
y
p(c |l , l
) otherwise
j

j,k k

j,k−1

6

p(uj,k , lj,k , cj,k |lj,k−1 )

uj,k = no jump,

uj,k = no jump,

uj,k = jump,

uj,k = jump,

uj,k = jump,

uj,k = jump,

lj,k = lj,k−1 ,

lj,k = lj,k−1 ,

y
lj,k = lk
,

y
lj,k = lk
,

lj,k = lunknown ,

lj,k = lunknown ,
cj,k = 
Nl −1
Nl pjump
Nl −1
Nl pjump
Nl −1
Nl

cj,k = m

cj,k = 

(1 − pjump )pmeas

(1 − pjump )(1 − pmeas )

y
lj,k−1 6= lk

0

(1 − pjump )

lj,k−1 = lunknown

0

0

y
lj,k−1 = lk

1
Mk

cj,k = m
1
Mk N l
1
Mk N l

pjump pmeas
pjump pmeas

1
Mk Nl

pmeas

1
Nl
1
Nl

cj,k = 

cj,k = m

pjump (1 − pmeas )

0

pjump (1 − pmeas )

0

1
Nl

(1 − pmeas )

0

TABLE I: The prior probabilities given previous location. Note that we can get the probability for row two from row one by taking into account
that p(cj,k = m|lj,k−1 6= lky , uj,k = no jump) = 0. Similarly, we get the third row from p(uj,k = jump) = 1.

In particular, this approximation becomes exact when Mk  N , since
a target can then be assigned to lky without any noticeable effect on
the other priors. This distribution is easier to work with as we can
use rejection sampling to sample from the individual priors and reject
any sample set with overlapping assignments.
3) Combined Process Model: Now, we would like to combine
the continuous local processes and the model for the locations and
associations c1:k in such a way that we can estimate the full posterior
p(X1:k , c1:k |Y1:k ) jointly. Fortunately, given ck and measurements
Yk , the states xj are independent. This allows us to decompose the
posterior:
p(X1:k , c1:k |Y1:k ) = p(X1:k |c1:k , Y1:k )p(c1:k |Y1:k )
Y
= p(c1:k |Y1:k )
p(xj,1:k |c1:k , Y1:k ).

(5)

j

x̂sj,k = x̂sj,k−1 + qsj,k−1 ,

if uj,k = no jump

x̂sj,k
x̂fj,k

if uj,k = jump

∼ U (X ),
= x̂fj,k−1 + qfj,k−1

ŷm,k = x̂j,k + rj,k ,

,

(6)

if cj,k = m

where the random parts are distributed according to
ck ∼ p(ck |c1:k−1 , Y1:k−1 )
qj,k−1 ∼ N (0, Qk )

p(c1:k |Y1:k ) ∝ p(Yk |c1:k , Y1:k−1 )p(c1:k |Y1:k−1 )
= p(Yk |c1:k , Y1:k−1 )p(ck |c1:k−1 , Y1:k−1 )p(c1:k−1 |Y1:k−1 ).

.

(7)

rj,k ∼ N (0, Rk )
4) Likelihoods: The idea behind our Rao-Blackwellized particle
filter
 i is to sample locations and associations ck and then update
x̂j using linear Gaussian dynamics. The idea comes from [17],
where the system incorporates one new measurement for every time
step. Since our system has distinct time steps where it gets several
coupled measurements at once, we incorporate these measurements
in the same update step, unlike [17][19]. In the following, we will
look at how to extract weighted samples of the posterior p(c1:k |Y1:k )

(8)

The first term, p(Yk |c1:k , Y1:k−1 ) = L(Yk |ck ), defines our
measurement likelihood. Given the full set of associations ck , it
decomposes into a product of point likelihoods. Since we know which
data points originate from the tracked objects, we know the others do
not. We will refer to the latter as clutter measurements, giving
Y
Y
L(Yk |ck ) =
p(ym,k |cj,k = m, uj,k )
p(ym,k |m clutter).
j:cj,k =m

This is the basic principle underlying the use of Rao-Blackwellized
particle filters for multiple target tracking, see for example [17][19]. It
allows us to sample a ci1:k for every particle i while also maintaining
an analytic filter for each state x̂ij . This decomposition reduces
sampling variance, allowing the use of far fewer particles than if
we were to track the full state (X1:k , c1:k ) using particle sampling.
We will outline our modeling of the combined process dynamics
before we delve further into the details of inference. In the following,
we assume the process has propagated according to the discrete
dynamics in the previous section. If an object j does not jump, it
adheres to the dynamics in Equation 1. If it does jump, the movement
model a-priori distributes the spatial part uniformly over the target
spatial domain, x̂sj,k ∼ U (X s ). If at the same time, it is associated
with a measurement ŷm,k , our new estimate of the target position
s
, Rsk ), where Rsk is the spatial measurement
will therefore be N (ŷm,k
noise. The complete continuous dynamics are
s

recursively using a particle filter. This can then be combined with the
linear part of the state to form the full posterior, see Equation 5. Using
Bayes’ rule two times, we can decompose the association posterior
to give us a recursive expression:

m:m is clutter

(9)
The point likelihood of local movements p(ym,k |cj,k = m, uj,k =
no jump) is simply the Kalman marginal likelihoods of position and
features [17]. If we instead consider jumps uj,k = jump, we know
from the previous section that x̂sj,k is a-priori uniformly distributed
over the spatial domain. Further, taking the association cj,k = m
into account, it must be somewhere within the measurement location
lky . The likelihood is therefore given by the Kalman feature marginal
likelihood times a uniform density over the area. With Ak being the
area of that location, and µfj,k , Σfj,k the feature estimate, we get
p(ym,k |cj,k = m, uj,k = j.) =

1
f
N (ŷm,k
; µfj,k , Σfj,k + Rfk ).
Ak

As of yet, we have not defined any prior probability of getting
clutter measurements, only of targets not giving rise to a measurement. However, as we will see, clutter is implicitly sampled within the
Gibbs sampling procedure. The likelihood of the clutter measurements
is given by a uniform density over the spatial and feature domains,
p(ym,k |m clutter) = A 1S f . Note that the support of the feature
k
density, S f , needs to be estimated from data.
5) Sampling from the Proposal: A density q(ck )
∝
p(Yk |c1:k , Y1:k−1 )p(ck |c1:k−1 , Y1:k−1 ) proportional to the
update of the posterior in Equation 8 is called the optimal
importance distribution. Sampling particle updates from the proposal
q(ck ) would minimize variance among our particle weights but
is in general difficult and typically requires some approximation
[30]. Using approximate MCMC methods to sample from the
proposal is therefore a well-established idea [30] and has been
used to estimate data associations for multi-target tracking in
[18]. In our case, recall that p(ck |c1:k−1 , Y1:k−1 ) describes our
transition model, which is in turn given by the individual association
priors as outlined in Section III-B1. The individual priors can
be combined with the likelihoods to compute the individual
proposals qj (cj,k ) ∝ p(Yk |cj,k )p(cj,k |cj:k−1 , lj,k−1 ). Using
these distributions, the importance distribution q(ck ) is sampled
analogously to p(ck |c1:k−1 , Y1:k−1 ); either with Gibbs sampling or

7
20

100
10

50

0

− 10

− 20

− 30

0

Object
Object
Object
Object
Object
Object
Object
− 40

0
1
2
3
4
5
6

Object
Object
Object
Object
Object
Object
Noise

7
8
9
10
11
12

− 20

− 50

Object
Object
Object
Object

− 100

0

20

40

(a) Features from experiment 1 reduced to two dimensions for visualization. They discriminate well between most of the objects. The only
exceptions are the monitor and chairs to the far right: the features
of objects 10 and 12 changed drastically after the objects jumped,
interleaving them with the noise measurements and those of object 4.

− 150

0
1
2
3

Object 4
Object 5
Object 6
Noise
− 100

− 50

0

50

100

(b) Features from experiment 2. Since the tracked objects are visually
similar chairs, the features do not discriminate well. However, we see
that the noisy detections are well separated from the chairs, indicating
that they help us focus on the objects of interest.

Fig. 6: CNN Features from the Google Inception v3 network [28]. They consist of the tensor responses at the final bottleneck layer
dimension reduced using t-SNE [29]. Additional images and labels for training covariances were extracted from the KTH Longterm Dataset
Labels (https://strands.pdc.kth.se/public/KTH_longterm_dataset_labels/readme.html) data set. Note that each
class corresponds to a specific instance. Semantically similar instances are mostly grouped together. For example, the four classes to the left
in Figure 6a represent all of the food containers, bottles and mugs in that dataset.

approximated with an independence assumption. In the following,
we explore the two methods in more detail.
In Gibbs sampling, the idea is to sample one of the variables
cj,k at a time conditioned on all the others, denoted c−j
=
k
{cj 0 ,k }j 0 \ cj,k . This gives us a modified individual proposal in
the form of a conditional distribution over cj,k , qj (cj,k |c−j
k ) ∝
−j
p(Yk |cj,k , c−j
)p(c
|c
,
c
).
Note
that
since
we
know
all of
j,k
j,k−1
k
k
the assignments, p(Yk |cj,k , c−j
k ) can be uniquely identified with
either a target or a clutter likelihood in Equation 9. Again, due to
the hard constraints in the prior, it is important to sample several
assignments block-wise. We do 100 iterations of burn-in, sampling
two random target assignments from a joint version of qj (cj,k |c−j
k )
at each iteration, and pick the final assignments as our sample. The
algorithm is initialized with assignments from approximate rejection
sampling as described below.
We also investigate a faster sampling scheme using the prior of
Equation 4, paired with approximate independent likelihoods. The
data points are already independent given assignments, ym,k ⊥
⊥
yn,k |cj,k = m. But given no assignment cj,k = , without the other
assignments we do not know if it implies that a measurement will
be rejected as clutter. Instead, we approximate it with an independent
likelihood p(ym,k |cj,k = ) that should be the same for all data
points, see Section III-D for a derivation of the approximation used
here. Given the likelihood,
Q a joint independent distribution over
ck is given by q̃(ck ) =
j p(ym,k |cj,k )p(cj,k |cj:k−1 , lj,k−1 ). To
sample from the approximate posterior, we generate samples from
q̃(ck ), which we reject if any two targets are assigned to the same
measurement. In the experiments, we report results both from this
approximate rejection sampling and from Gibbs sampling.

C. Calculating the Weights
Since the likelihood p(Yk |c1:k , Y1:k−1 ) is not actually a probability mass function with respect to ck , i.e. it does not sum to

one, the importance distribution is defined by the product modulo
a normalization constant Zki ,
q(ck ) =

1
L(Yk |ck )p(ck |ci1:k−1 , Y1:k−1 ),
Zki

and Zki is given by the sum of the product over all assignments,
X
Zki =
L(Yk |ck )p(ck |ci1:k−1 , Y1:k−1 ).
(10)
ck

Since Zki varies between the particles, we need to update the weights
proportional to their values of Zki , as it is not reflected in the
sampling. Intuitively, this is similar to a particle filter that uses the
likelihood to update the weights of the particles. As we can not
compute the Zki directly, it needs to be approximated. Components
of the sum in Equation 10 typically take on their largest values in
a few places where the likelihood is large. The idea is therefore to
produce estimates of Zki by sampling from the proposal distribution
q(ck ) ∝ p(ck |ci1:k−1
P , Y1:k−1 )L(Yk |ck ). In the following, we mainly
use the fact that ck p(ck |ci1:k−1 , Y1:k−1 ) = 1:
Zki = P

ck

Zki
p(ck |ci1:k−1 , Y1:k−1 )

Zki
i
ck p(ck |c1:k−1 , Y1:k−1 )L(Yk |ck )/L(Yk |ck )

= P

(11)

1
1
=
.
q(c
)/L(Y
|c
)
(1/L(Y
E
k
k
k
k |ck ))
q
ck

= P

If we look at the last line, we can then estimate the expectation
Eq (1/L(Yk |ck )) by sampling many ck ∼ q(ck ) and compute
1/L(Yk |ck ) for each sample. The inverse of the estimated expectation gives us our estimate of the sum Zk . Importantly, we already
perform Gibbs sampling from q(ck ) to sample our particle proposals.
By simply running a few more iterations of the same MCMC chain,
we can use this procedure to estimate Zk . Being able to produce

8

both of these properties at the same time caters to the efficiency and
simplicity of the algorithm.
In addition to this scheme, we also evaluate a faster version
where we compute Zk using the approximated independent posterior q̃(ck ). Inserting the corresponding product into the sum of
Equation
product over the individual sums, Zki ≈
Q
P 10 results in
Qa P
j
cj,k qj (cj,k ) =
j
cj,k p(ym,k |cj,k )p(cj,k |cj:k−1 , lj,k−1 ).
1) Rao-Blackwellized Multi-Target Tracking: In summary, this
gives us the sequential importance sampling updates:
1) Sample new locations and associations, either using Gibbs
sampling or approximate rejection sampling:
cik ∼ q(ck |Y1:k , ci1:k−1 )
2) Update the position and feature estimates µij,k , Σij,k
3) Update the weights Q
using
PGibbs from previous section or with
approximate Zki = j cj,k qj (cj,k ):
wki

=

i
wk−1
Zki ,

wki

wi
= P ki
wk

At each step of the filtering, we can then construct a posterior over
the joint feature and spatial distributions of the objects. The feature
part can be marginalized out simply by removing the corresponding
dimensions from the posterior:
X
p(x̂j |Y1:k ) =
wi N (x̂j ; µij,k , Σij,k ).
(12)
i

The resulting density is illustrated in the filtering results, see for
example Figure 1 and Table III.
D. Implementation
While each particle could potentially need its whole history of
associations and measurements to define the probabilities, in reality
we can get away with much less. Each particle is parametrized
i
}j=1:N of sets for the targets j.
by a list Pki = {µij,k , Σij,k , lj,k
i
fully specify the probability
In particular, the target locations lj,k
p(ck |ci1:k−1 , Y1:k−1 ) of the proposal distribution, as seen in Table I.
µij,k and Σij,k parametrize the marginal likelihoods.
For the version with approximate rejection sampling using q̃(ck ),
we need to estimate the independent likelihoods p(ym,k |cj,k = )
conditioned on no observation of j. These roughly describe the data
distribution. One approach would be to try to directly approximate the
unconditional likelihood of the data points, which is hard in itself.
However, we have found empirically that another approach works
better in our case. We proceed using the intuition that these values
should remain fairly constant with respect to the current target j. If the
current data is assigned low values by the Kalman likelihoods, those
values should start approaching the data density. This allows sampling
for example of jumps. With this intuition in mind, we approximate
the data likelihood by the expectation of the data likelihood with
respect to the estimated continuous distribution of target j, Ŷ ∼
N (µj,k , Σj,k + Rk ). In the derivation, we use the fact that there
should be negligible overlap
P between the different target distributions
of one particle and that ck :cj,k =m p(ck ) ≈ N1 :
i
h
i
h
i X h
E p(Ŷ) =
E p(Ŷ|ck ) p(ck ) ≈ E p(Ŷ|cj,k = m) ×
Ŷ

ck Ŷ

X
ck :cj,k =m

p(ck ) ≈

Ŷ

D
i
(4π)− 2
1 h
E p(Ŷ|cj,k = m) = p
N Ŷ
N |Σj,k + Rk |

While this is an approximation, we have seen empirically that it
performs better than trying to estimate the data likelihood. Again, we
reason this is due to that the marginal p(cj,k = |Y1:k ) is determined

mainly by the ratio to the point likelihoods of the measurement
associations.
IV.

E XPERIMENTS

51st: 7

39th: 2
15th: 5

4

37th: 6
0
45th: 5

8th: 2
4

29th: 8

60th: 10
30th: 5
39th: 11

46th: 3

17th: 1

22nd: 9

20th: 6

28th: 1

2

0

67th: 10
10th: 9

3

(a) Experiment 1.

(b) Experiment 2.

Fig. 7: The jumps of the objects in the two experiments. Arrows
indicate jumping objects while dots show the position of static objects
with only local movement. Jumps are annotated with time step and
object id. In the first experiment there are 3 jumps between 2 rooms
(i.e. locations). The second experiment contains 13 objects from 3
rooms jumping a total of 14 times.

We perform several experiments, both on real data collected
autonomously by a robot and on simulated data. In the following,
we describe the exact details of the method used as well as the
experimental setups.
A. Detections and Features
For basic change detection, we rely on the Statistical Inlier Estimation (SIE) method of Ekekrantz et al. [31]. The method compares two
subsequent local maps from the same location to find the differences
and through them the moving objects. While the method can also
extract parts that are highly dynamic (i.e. moving while the sweeps are
collected), we choose to ignore these parts, as they mostly consist of
moving humans. SIE explicitly models the noise of the range sensors
as part of its optimization, enabling us to reliably extract smaller
objects close by but also larger objects that are further away. The
resulting detections are fed into the system of Section III-A to add
detections of the objects in the observations where they are static.
In recent years, convolutional neural networks have come to play
a vital role in computer vision. Today, they represent the most
successful technique e.g. for classification of images. For applications,
a popular method for adopting a network to a particular domain
is finetuning. In [32] and [33], the authors showed finetuning of
networks often lead to slightly better performance, but can be difficult
to get right. Further, in [33] as well as [34], features extracted from the
tensor responses of one of the layers in a pre-trained network showed
remarkable performance on a wide range of computer vision tasks.
Importantly for our application, [33] showed these features could
achieve state-of-the art performance in both instance recognition
benchmarks and retrieval scenarios. Whenever we can distinguish the

9

Fig. 8: The objects in experiment 2. Objects 0-12 from left to right: Trash can, bag, fire extinguisher, bottle, chair, mug, trash can, bag,
container, trash can, chair, mug, monitor. The tracker features are initialized from these images, which were also extracted using the method
in Section III-A. The colors along the bottom correspond to object colors in Figures 7a and 6a.

objects by visual appearance we would like to take advantage of
the discriminative power of these features. Therefore, we use CNN
features reduced to three dimensions using t-SNE [29], see Figure 6.
In our experiments, we found that three dimensions provide a good
trade-off between computational complexity of the filter, accurate
estimation of covariances and discrimination. In addition, we have
seen that after t-SNE reduction, the variation within each class is
well approximated by a multivariate Gaussian. The two-dimensional
illustration in Figure 6 gives some indication of this.
B. Parameters
Parameter

pjump

pmeas

σq

σr

Ak

Particles

Rfk , S f

Value

0.03

0.98

0.35

0.15

20m2

300

estimated

Most of the parameters have some meaning and can to an extent be
selected by hand. In these experiments, we have pjump = 0.03, pmeas =
0.98, Qsk = σq2 I, Rsk = σr2 I, with σq = 0.35, σr = 0.15. Note that
the spatial dimensions are independent. We deliberately set pjump to a
slightly lower value to avoid sampling too many jumps, which might
lead to particle depletion with many objects. For simplicity, the spatial
process noise Qsk is constant, but could also increase as some function
of the time between observation k−1 and k. Qfk = 0 since we do not
expect the descriptors to change over time. We fit the measurement
covariance Rfk and support S f from the data set referenced in Figure
6. While the areas of the locations, Ak , can be estimated from the
3D map observations, the rooms in our data sets are all around 20m2
and we use this value in all experiments.
C. Evaluation metrics
To compare the performance of different methods, we use the
CLEAR Multiple Object Tracking metrics from [35]. The metrics
are defined using the association of targets to observations based on
estimated positions. These associations can be used to compute the
mean errors, number of mismatching associations, number of false
positives and the number of missed observations. The MOTA metric
[35] combines several of these measures into a global score, showing
the performance of the trackers. We also compute the MOTP metric,
which is simply the mean distance error of the associations [35].
To find the best association for the MOTA calculation, we use the
Hungarian algorithm to minimize the combined distance between
filter estimates and observations, similar to [35]. We only consider
associations closer than 0.5m. Once we have computed the associations, we compute the false positives as the number of estimates
associated with a measurement with no label. Missed observations are
similarly labels with no estimate. Mismatches are observations which
the estimate assigns a label different from the annotated label. In [35],
the authors count mismatches only in the observation sequence where
the initial error is made; for example when two target paths cross.
However, for our application it is important we maintain tracking of

the correct targets over the whole sequence. We therefore count any
observation with the wrong label estimate as a mismatch.
To establish an independent baseline to compare against, we also
used our detections and features in a system similar to GATMO
[7]. Since all short-term dynamics and static structures have already
been filtered, we only need to keep track of the long-term dynamics.
Therefore, the baseline tracker keeps track of two sets of objects,
movable and absent. We use the information from Section III-A to
know if a detected object was propagated, i.e. if it has not moved.
Given the previous sets of movable and absent objects, a movable
object is explained and kept in the movable set if it is static as
compared to the previous observation. If it is not, or the object
was in the absent set, it can be matched to one of the unexplained
observations. Then, a match is made if the visual feature distance is
smaller than some threshold. We find the threshold by running several
experiments with different thresholds, and picking the one that gives
the best results on both annotated data sets. If it is matched in this
way, it is placed in the movable set, otherwise among the absent
objects. The estimated position of a given object is simply taken to
be that of its last matched observation.
D. Experimental setup
We perform several experiments using the STRANDS robot platform [36]. In each case, the inputs to the system are RGBD frames
a robot collects autonomously while moving between a few different
locations. Typically, after observing one location, the robot visits the
other ones before returning to the same location again. Since we
want to validate the system in the presence of lots of movement, and
especially movement of the objects between different locations, we
manually move the some of the objects when the robot is away. This
gives us ground truth knowledge of which specific objects moved.
As the motion of single objects in the experiments was exaggerated
as compared to most natural environments, we argue that it provides
a thorough validation of the robustness of the tracker. The collected
frames are fused into maps, mostly consisting of an entire room, and
segmented using the system in Section III-A. Features are extracted
from the images associated with the point cloud segments. The object
position observations are simply computed as the mean of the point
clouds. To initialize the tracking system, we mark the segments
corresponding to the objects we wish to track in the first observations.
The first experiment should verify the tracking ability under
frequent movement, but with dissimilar objects, to avoid ambiguity.
There are a couple of instances of most types, but in particular the fire
extinguisher and monitor types only appear once, see Figure 8. In the
sequence of 77 observations, the 13 objects jump a total of 14 times,
see Figure 7a. While the first experiment should validate the ability
to track in favorable circumstances, the second experiment tests the
ability to handle ambiguity and noise in the features. To that end, all
of the objects are chairs, and most of them are visually similar. The
seven chairs in the sequence are all in either of two locations. In the
55 observations of the locations, there are three jumps taking place,
as well as considerable local movement of all chairs, see Figure 7b.

10

The second experiment should be challenging for methods that do
not explicitly handle ambiguous evidence, such as the baseline.
To verify how much information either modality contributes to
the tracker, we benchmark the full system as well as the local
and global tracking components individually. The local tracker is
simply a variant of the full system with pjump = 0, disallowing any
jumping within or between locations. A global tracker based purely
on features is obtained by replacing the spatial Kalman measurement
likelihood with a uniform distribution. The spatial position thus has
no contribution to the final estimate. To attain position estimates, we
instead set the positions of each particle’s targets to those of their last
estimated measurement correspondences cj,k .
Finally, we perform experiments in simulation to investigate how
tracking performance is affected by varying the number of tracked
objects and the rate of jumps. Given a number of pre-defined areas,
jumps are sampled between the areas as well as Gaussian process
noise around the current positions. Additionally, with each observation round we sample a number of noise detections not corresponding
to any of the targets. Since we sample a full simulation sequence
for every value of the parameters, the sampling will introduce some
additional variability in the performance measures. To decrease the
effects of sampling, we sample three simulations per value, and
evaluate the tracker ten times on every sampled simulation.
V.

R ESULTS

Since the filtering method is sampling based and the results vary
slightly, we do 50 experiments with each method and present both
qualitative and quantitative results from these runs. The experiments
are all with 300 particles, which proved to be sufficient to track
the posteriors (see Section V-A). Qualitative results are reported on
the version using approximate rejection sampling of the proposal,
with comparisons to the results with other variations of our method
and to the baseline. We evaluate sampling the proposals using Gibbs
sampling by itself and then also with Gibbs weight calculation.
A. Experiment 1: Various Objects

MOTA (± st dev)

0.75

0.70

0.65

0.60

100

200

300

400
500
Num ber part icles

600

700

800

Fig. 9: The MOTA score as a function of the number of particles, with
statistics from 10 runs on experiment 2. After about 300, increasing
the number of particles has little effect.

In Figure 9, we see that the MOTA score on this experiment
increases with the number of particles. After about 300 particles,
the trend stabilizes at around 0.7, demonstrating that 300 is enough
to approximate our defined posterior accurately, and that other errors
are due to modeling imperfections or system noise. The posteriors of
four different objects from experiment 1 are visualized in Table III.
Whenever an object disappears from a location, it affects the posterior
of the corresponding object. The local uncertainty increases, at the
same time as new associations are sampled around other measurement
locations. In the example posteriors, we see that the posteriors are well
concentrated when the objects are static.

Features of the tracked objects in experiment 1 are shown in
Figure 6a. The different object classes result in features that are well
separated. Out of the ten experiments, 10 out of the 14 jumps are
tracked correctly in the majority of cases. These include successful
tracking of diverse objects such as mugs, bottles, canisters, trash cans
and bags. In fact, the failures were all with larger objects such as the
jumping chair and the monitor. In these cases, the features seem to
have dramatically changed before or during the jumps of the objects.
In the case of the office chair, a rotation led to the feature of another
chair to become more similar, causing confusion in the inference.
The other failure occurred at time steps 37 and 39 when the fire
extinguisher and a trash can respectively switched places, see Table
III and Figure 7a. This typically lead the tracker to estimate that the
objects stayed in the same places and adapt to the new appearance.
Besides the ten successfully tracked jumps, the two static objects were
inferred correctly.
System

MOTP[35]

Miss rate

False pos.

Mism.

Simplified

0.11

0.17

0.07

0.08

0.67

Gibbs sampl.

0.12

0.17

0.06

0.07

0.70

MOTA[35]

Gibbs weights

0.12

0.17

0.04

0.06

0.73

Gibbs weights*

0.11

0.13

0.01

0.00

0.86

Features**

0.18

0.40

0.07

0.06

0.46

Local***

0.13

0.21

0.04

0.27

0.48

Baseline

0.11

0.08

0.01

0.00

0.91

TABLE II: Results from experiment 1. The main method is run with
the simplified independent proposal, Gibbs sampling of proposals and
Gibbs estimation of weights. The rates are averaged over 50 runs.
* optimized

for experiment (0.3 × Rfk )

** no

spatial tracking

*** p

jump

=0

Quantitative results are presented in Table II. Importantly, we
observed that in the experiments where the results were qualitatively
worse, this correlated with a significantly lower MOTA rate. As
expected, with almost no overlap between the feature distributions,
the baseline achieves good results. Compared to the baseline, the
proposed tracker takes several time steps to converge on the new
target location whenever a target jumps. This is due to the uncertainty
embedded in the tracker, and leads to a significantly lower MOTA
score in this case. If we decrease the feature uncertainty to about
one third of the estimated covariance Rfk , as shown in Table II,
the proposed tracker becomes more certain and collapses on new
locations more quickly, leading to a score almost in line with the
baseline. In general, the proposed tracker infers the same jumps as
the baseline, but estimates are lagging due to uncertainty.
From the results, we see that the local tracker performed significantly worse than the full system on this dataset. Due to the large
number of jumps, the model is too limited in this scenario. The high
mismatch rate of the local tracker is due to its inability to jump,
instead settling for the neighboring observations which might be
visually dissimilar. On the other hand, we see that the feature-based
tracker was almost on par with the local model. However, the high
estimated uncertainty of the features keeps the feature-based tracker
from achieving a result in line with that of the baseline. The result
tells us approximately how much of the performance of the combined
model is purely due to the features. We can see that, with the feature
uncertainty, the combined model performs significantly better than the
individual models. This is mirrored in the qualitative results, where
the full inference framework clearly outperformed both methods.
In this experiment, each Gibbs sampling method contributes to
a slight performance improvement over rejection sampling. Qualitatively, the Gibbs strategies were able to more often correctly infer

11

1

Object 8 - Container

Object 6 - Trash can

Object 5 - Mug

Object 3 - Bottle

Initialization

2

3

4

Jump

Jump

Jump

Jump

Jump

TABLE III: Posteriors (red-blue gradients) of four different objects from experiment 1 at different time steps together with the image of the
closest measurement. If there is no image, no measurement was associated with the estimate at that time step. The trash can in column 3
shows one of the failures. Since that and the fire extinguisher swap places around step 2, the estimates are confused, causing a mismatch. The
other objects are correctly tracked, since the images of ”Intialization” and ”4” coincide. Note that all objects jump at least once.

1.0

1.0

0.9

0.9

0.8

0.8
MOTA (± st dev)

MOTA (± st dev)

12

0.7
0.6
0.5

0.7
0.6
0.5

0.4

0.4

0.3

0.3

0.2

5

10

15
Num ber t arget s

20

0.2

25

0.02

0.04

0.06

0.08

0.10

0.12

pjum p

Fig. 10: The MOTA score as a function of the number of targets in
simulation. Note that the score stays constant when adding targets. The
initial variability is due to how the simulation is set up; if only one
target is at a location and it jumps somewhere else, the old location is
unlikely to be observed again, leading the tracker to believe that the
target is still there.

Fig. 11: MOTA score as a function of the rate of jump in simulation,
with statistics from 30 runs. Note that the score is decreasing with
the jump rate. Since three simulations are sampled for each rate, some
additional variability can be seen.

that the garbage can and fire extinguisher switched places and, more
generally, collapse on the correct locations faster. These results and
those from experiment 2 give us confidence that while the MCMC
schemes are slightly more accurate, the approximate version is a
reasonable approximation. The trade-off becomes apparent in the
run-time of the method, which is roughly doubled when doing 100
iterations of burn in for Gibbs sampling.

relies on the uncertain features to estimate associations. Qualitative
results reflected the low score, with the estimated positions frequently
jumping between different positions.
Since there are few jumps in this experiment, we see that the local
version of the tracker performs well also in comparison to the full
system. In particular, since the jumps do not occur until halfway
through the sequence, this version typically gives perfect results up
until that point. However, the larger jumps cannot be tracked, and it
errs at this point. Interestingly, the small jump within one room at the
end of the sequence is typically inferred correctly. The feature-based
tracker qualitatively performed badly for this experiment, which can
also be seen from the MOTA. Since different particle hypotheses were
typically associated with several of the chairs in the two rooms, the
estimate ended up somewhere in the middle. This can be seen by the
high miss rate, which is due to the position annotations being too far
away from any estimates. In conclusion, we see the combined tracker
performed the best in this scenario, with the local tracker providing
most information for the joint inference.

B. Experiment 2: Chairs
As can be seen in Figure 6b, the features do not discriminate well
between the chairs and the inference therefore has to rely more on
the measurement positions to estimate the target positions. Of the
three jumps, the filter tracks two of them correctly, while the last
one happens too late in the observation sequence for the estimate to
consolidate on the new position. In general, the filter requires two to
three observations before a jump is inferred. At the first jump, the
chair jumps close to another identical chair, leading the tracker to
confuse the two chairs in the majority of the ten runs. Such errors
are to be expected as there is no way to distinguish these visually
identical chairs. Chair number 4 (see Figure 7b) moves around within
a roughly three meter diameter but the tracker accurately follows it
in most of the runs. The rest of the objects all move around locally
and are tracked correctly in the majority of experiments.
System

MOTP[35]

Miss rate

False pos.

Mism.

Simplified

0.18

0.18

0.03

0.11

0.68

Gibbs sampl.

0.18

0.15

0.02

0.12

0.70

Gibbs weights

0.18

0.19

0.04

0.09

0.68

Features*

0.25

0.84

0.01

0.04

0.11

Local**

0.18

0.17

0.04

0.12

0.67

Baseline

0.16

0.20

0.02

0.28

0.49

MOTA[35]

TABLE IV: Results from experiment 2 with chairs. The rates are an
average over 50 runs. * no spatial tracking ** pjump = 0

Quantitative results are presented in Table IV. Both Gibbs proposal
sampling and with weight calculation have about the same MOTA
score as approximate rejection sampling, with pure Gibbs proposal
sampling being slightly higher. Correspondingly, qualitative results
for all three methods are similar. The baseline method performs
significantly worse than these methods on this experiment. It suffers
since it only tracks static objects spatially, without a soft local
movement prior. Whenever there is any movement, it therefore

C. Experiment 3: Simulations
In the simulated environment, we generated results for various
number of targets and rate of jumps, with three simulations sampled
for every parameter value. Our hypothesis is that tracking performance should decrease with the rate of jump, rather than with the
number of targets. This is due to how our proposal distribution is
constructed; when one target is well explained by a measurement,
most of the particles are likely to sample that association. Hence,
particle depletion happens more rapidly when there are several
uncertain associations, as happens when targets jump. If we look at
the graph in Figure 11, we can indeed see that the MOTA develops
negatively with an increasing jump rate. Correspondingly, Figure 10
shows the performance stays more or less constant when increasing
the number of targets. From these graphs, we can deduce that the
complexity of the proposed algorithm scales with the jump rate rather
than with the number of targets. This is important in real world
environments, where the number of objects might be very large, but
jumps happen relatively seldom.
D. Data from robot deployment
In addition to the experiments, we have also evaluated the method
on data gathered by a mobile robot in a real world deployment.
The robot patrolled an office environment during a period of two
months, gathering observations of a few places with one or two days’
interval. In our analysis, we have focused on three places with highly

13

System

MOTP[35]

Miss rate

False pos.

Mism.

MOTA[35]

Proposed

0.14

0.15

0.10

0.04

0.71

Experiment 1

Experiment 2

0.02

0.02

0.04

0.04

0.75
0.72

0.06

0.08

0.66
0.63

0.06

0.60

MOTA

pjump

TABLE V: Statistics from robot deployment, with 9 objects in 34
observations. Values are comparable to other experiments.

pjump

0.69

0.57

0.08

0.54
0.99

movable objects: one reception area and two kitchens. In these scenes,
we marked three objects in each location, most of them moving
around within the area. Since no motion was observed in between
the locations, the experiment should mostly be seen as a way to test
the stability of the methods in a real world scenario. In Table V, we
see that the numbers reflect those in other experiments. Indeed, the
method manages to track the objects through most of the sequences,
confirming the validity of the approach as applied to natural data.
VI.

D ISCUSSION

We see that the joint tracker is able to bring in information from
both the visual and spatial modalities to produce a reliable estimate
of the object positions. In the case where we have different object
classes, the CNN features proved to be highly discriminative, allowing
us to track the jumps even of comparatively small cups. In fact,
the small objects seem to consistently provide good features, even
in the presence of motion blur. This, together with the preliminary
deployment results in Section V-D, gives us confidence that the
proposed system will be able to work in a real world scenario, where
change detection is likely to detect the movement of a multitude
of small objects. Further, the method works well even when we
increase the number of objects to track since the accuracy of the
algorithm depends more on the number of jumps. To maintain
accurate posteriors in the event of many jumping objects, one could
employ an adaptive particle set scheme such as [37].
The baseline method is based on the previous GATMO [7], as
applicable within our system. Since this method does not reason
probabilistically, it suffers when there is noise and ambiguity in the
measurements, as can be seen in the second experiment. Since there is
overlap between the distributions of the different features, objects with
overlap are sometimes confused. In a real-world scenario, this kind
of situation is to be expected, as there are often many similar-looking
objects. Moreover, we expect more noise in real-world scenarios,
when there is more lighting variation, and the objects may be observed
from more angles. We conclude that it is important to incorporate
uncertainty as part of the tracking, especially in environments that
contain multiple visually similar objects.
While not discussed in the results, the tracking is enabled partly
by the performance of the change detection framework discussed in
Section III-A. Even when the objects did not move, and were detected
through the propagation approach, all objects were detected in all the
observations. This is remarkable, since in total, we have 522 annotated
objects in the datasets. It shows that this simple method is sufficient, at
least given precise registration. However, the segmentation sometimes
either over- or undersegments the objects. Particularly for the chairs,
oversegmentation is an issue that should be dealt with in a more
principled fashion, potentially by training a supervised segmentation
method on similar data. In most cases, the tracker was nonetheless
able to handle these deficiencies.
In Figure 12, we see the effect of varying the pjump and pmeas
parameters. In the experiments, we have chosen to use the same
values of the parameters for consistency. However, from the figure,
we observe that the method performs better on experiment 1 when the
value is higher than the one used. The difference in the optimal value
of pjump between the experiments is due to that the actual rate of jumps

0.98

0.97

pmeas

0.96

0.95

0.99

0.98

0.97

0.96

0.95

0.51

pmeas

Fig. 12: Effect of the interaction of pjump and pmeas on the MOTA
score in the two experiments. In experiment 2, the method performs
well roughly within the quadrant pjump ≤ 0.04, pmeas ≥ 0.975. The
optimal value of pjump seems to be higher in the first experiment, at
or above 0.08.

is higher in experiment 1, with about 0.18 jumps per observation
versus 0.05 in experiment 2. The relation in the jump frequencies to
the optimal pjump values leads us to believe that it might be beneficial
to estimate the number of jumps using the algorithm and adjust the
pjump parameter iteratively in an EM fashion. Similarly, we saw in
experiment 1 that estimating the feature covariance Rf for the data
at hand can significantly improve results.
VII.

C ONCLUSION & F UTURE W ORK

We presented a system which can track multiple similar objects
even in the presence of noise and when only a subset of the objects
can be observed at any given time. There are three major reasons
that together enable the system to track objects of sizes that range
from mugs to chairs accurately. First, the probabilistic method of
[31] together with our temporal segmentation allows us to identify
even small changes in the 3D maps consistently. Secondly, recent
CNN architectures such as the one used [28] allow us to represent
the discovered objects using discriminative features. For the smaller
objects, it is key to use the image data as the depth data is often
too noisy to produce reliable features. Third, our Rao-Blackwellized
object tracker accurately models the dynamics of the scenario and
allows us to maintain realistic posteriors of object positions. Even
when the objects are visually similar, the tracker can rely on the
spatial measurements for reasoning.
As mentioned, one pitfall of the current system is that it relies
on a closed world assumption. Without this, tracking the jumps of
objects would likely be intractable. An open question is how to resolve
this conflict. We would like to investigate if we could model how
probable different object types are to stay within the confines of
the environment. For example, a chair is very likely to stay while
a cardboard box will likely be thrown away. It would therefore be
appealing to learn these probabilities and track jumps only when the
object is likely to stay in the environment. For temporary objects, the
system should instead track the births and deaths, see e.g. [18][17].
With the ability to integrate new objects as they are observed comes
also the possibility of applying the movement models to improve a
full-blown SLAM system as suggested in [38].
More generally, we would like to learn more properties of the
objects given the visual features. In addition to estimating the jump
probability of the object type, there should also be specific movement
model variances for different targets. In this case, a monitor serves as
a good example of something that stays in the exact same place for a
long time, while a chair will always be expected to move around a bit.
The chair should thus have a higher uncertainty Qsk associated with
its movement model than the monitor. In turn, one specific kitchen
chair might be more probable to move than an office chair.

14

VIII.

ACKNOWLEDGEMENTS

The authors would like to thank Erik Ward for many rewarding
conversations on the problems of multi-target tracking. The work presented in this paper has been funded by the European Union Seventh
Framework Programme (FP7/2007-2013) under grant agreement No
600623 (“STRANDS”).
R EFERENCES
[1]

[2]

[3]

[4]

[5]

[6]

[7]

[8]

[9]

[10]

[11]

[12]

[13]

[14]

[15]

[16]

[17]

J. Koch, J. Wettach, E. Bloch, and K. Berns, “Indoor localisation of
humans, objects, and mobile robots with rfid infrastructure,” in Hybrid
Intelligent Systems, 2007. HIS 2007. 7th International Conference on,
pp. 271–276, IEEE, 2007.
C.-C. Wang, C. Thorpe, S. Thrun, M. Hebert, and H. Durrant-Whyte,
“Simultaneous localization, mapping and moving object tracking,” The
International Journal of Robotics Research, vol. 26, no. 9, pp. 889–916,
2007.
R. Biswas, B. Limketkai, S. Sanner, and S. Thrun, “Towards object
mapping in non-stationary environments with mobile robots,” in Intelligent Robots and Systems, 2002. IEEE/RSJ International Conference
on, vol. 1, pp. 1014–1019, IEEE, 2002.
N. E. Du Toit and J. W. Burdick, “Robot motion planning in dynamic,
uncertain environments,” IEEE Transactions on Robotics, vol. 28, no. 1,
pp. 101–115, 2012.
L. Montesano, J. Minguez, and L. Montano, “Modeling the static and
the dynamic parts of the environment to improve sensor-based navigation,” in Robotics and Automation, 2005. ICRA 2005. Proceedings
of the 2005 IEEE International Conference on, pp. 4556–4562, IEEE,
2005.
K. Shubina and J. K. Tsotsos, “Visual search for an object in a
3d environment using a mobile robot,” Computer Vision and Image
Understanding, vol. 114, no. 5, pp. 535–547, 2010.
G. Gallagher, S. S. Srinivasa, J. A. Bagnell, and D. Ferguson, “Gatmo:
A generalized approach to tracking movable objects,” in Robotics
and Automation, 2009. ICRA’09. IEEE International Conference on,
pp. 2043–2048, IEEE, 2009.
E. Herbst, X. Ren, and D. Fox, “RGBD object discovery via multi-scene
analysis,” in Intelligent Robots and Systems (IROS), 2011 IEEE/RSJ
International Conference on, pp. 4850–4856, IEEE, 2011.
D. F. Wolf and G. S. Sukhatme, “Mobile robot simultaneous localization
and mapping in dynamic environments,” Autonomous Robots, vol. 19,
no. 1, pp. 53–65, 2005.
P. Biber, T. Duckett, et al., “Dynamic maps for long-term operation of
mobile service robots,” in Robotics: science and systems, pp. 17–24,
2005.
C.-C. Wang and C. Thorpe, “Simultaneous localization and mapping
with detection and tracking of moving objects,” in Robotics and Automation, 2002. Proceedings. ICRA’02. IEEE International Conference
on, vol. 3, pp. 2918–2924, IEEE, 2002.
M. Montemerlo, S. Thrun, and W. Whittaker, “Conditional particle
filters for simultaneous mobile robot localization and people-tracking,”
in Robotics and Automation, 2002. Proceedings. ICRA’02. IEEE International Conference on, vol. 1, pp. 695–701, IEEE, 2002.
D. Schulz and W. Burgard, “Probabilistic state estimation of dynamic
objects with a moving mobile robot,” Robotics and Autonomous Systems, vol. 34, no. 2, pp. 107–115, 2001.
D. Anguelov, R. Biswas, D. Koller, B. Limketkai, and S. Thrun,
“Learning hierarchical object maps of non-stationary environments
with mobile robots,” in Proceedings of the Eighteenth conference on
Uncertainty in artificial intelligence, pp. 10–17, Morgan Kaufmann
Publishers Inc., 2002.
R. Toris and S. Chernova, “Temporal persistence modeling for object
search,” in Robotics and Automation (ICRA), 2017 IEEE International
Conference on, pp. 3215–3222, IEEE, 2017.
D. F. Wolf and G. S. Sukhatme, “Towards mapping dynamic environments,” in In Proceedings of the International Conference on Advanced
Robotics (ICAR), pp. 594–600, 2003.
S. Särkkä, A. Vehtari, and J. Lampinen, “Rao-blackwellized particle
filter for multiple target tracking,” Information Fusion, vol. 8, no. 1,
pp. 2–15, 2007.

[18]

[19]

[20]

[21]
[22]

[23]

[24]

[25]

[26]

[27]

[28]

[29]
[30]

[31]

[32]

[33]

[34]

[35]

[36]

[37]

[38]

S. Oh, S. Russell, and S. Sastry, “Markov chain monte carlo data
association for multi-target tracking,” IEEE Transactions on Automatic
Control, vol. 54, no. 3, pp. 481–497, 2009.
I. Miller and M. Campbell, “Rao-blackwellized particle filtering for
mapping dynamic environments,” in Robotics and Automation, 2007
IEEE International Conference on, pp. 3862–3869, IEEE, 2007.
T. Vu, B.-N. Vo, and R. Evans, “A particle marginal metropolis-hastings
multi-target tracker,” IEEE Transactions on Signal Processing, vol. 62,
no. 15, pp. 3953–3964, 2014.
R. Mahler and T. Zajic, “Multitarget filtering using a multitarget firstorder moment statistic,” in Proc. SPIE, vol. 4380, pp. 184–195, 2001.
B.-N. Vo and W.-K. Ma, “The gaussian mixture probability hypothesis
density filter,” IEEE Transactions on signal processing, vol. 54, no. 11,
pp. 4091–4104, 2006.
R. Finman, T. Whelan, M. Kaess, and J. J. Leonard, “Toward lifelong
object segmentation from change detection in dense RGBD maps,” in
Mobile Robots (ECMR), 2013 European Conference on, pp. 178–185,
IEEE, 2013.
R. Ambrus, N. Bore, J. Folkesson, and P. Jensfelt, “Meta-rooms:
Building and maintaining long term spatial models in a dynamic world,”
in Intelligent Robots and Systems (IROS), 2014 IEEE/RSJ International
Conference on, pp. 1854–1861, IEEE, 2014.
R. Ambrus, J. Ekekrantz, J. Folkesson, and P. Jensfelt, “Unsupervised
learning of spatial-temporal models of objects in a long-term autonomy
scenario,” in Intelligent Robots and Systems (IROS), 2015 IEEE/RSJ
International Conference on, pp. 5678–5685, IEEE, 2015.
E. Herbst, P. Henry, and D. Fox, “Toward online 3-d object segmentation and mapping,” in Robotics and Automation (ICRA), 2014 IEEE
International Conference on, pp. 3193–3200, IEEE, 2014.
S. Geman and D. Geman, “Stochastic relaxation, gibbs distributions,
and the bayesian restoration of images,” IEEE Transactions on pattern
analysis and machine intelligence, no. 6, pp. 721–741, 1984.
C. Szegedy, V. Vanhoucke, S. Ioffe, J. Shlens, and Z. Wojna, “Rethinking the inception architecture for computer vision,” in Proceedings
of the IEEE Conference on Computer Vision and Pattern Recognition,
pp. 2818–2826, 2016.
L. v. d. Maaten and G. Hinton, “Visualizing data using t-sne,” Journal
of Machine Learning Research, vol. 9, no. Nov, pp. 2579–2605, 2008.
A. Doucet, S. Godsill, and C. Andrieu, “On sequential monte carlo
sampling methods for bayesian filtering,” Statistics and computing,
vol. 10, no. 3, pp. 197–208, 2000.
J. Ekekrantz, N. Bore, R. Ambrus, J. Folkesson, and P. Jensfelt,
“Unsupervised object discovery and segmentation of RGBD images,”
arXiv preprint arXiv:1710.06929, 2017.
K. Chatfield, K. Simonyan, A. Vedaldi, and A. Zisserman, “Return of
the devil in the details: Delving deep into convolutional nets,” arXiv
preprint arXiv:1405.3531, 2014.
A. Sharif Razavian, H. Azizpour, J. Sullivan, and S. Carlsson, “Cnn
features off-the-shelf: an astounding baseline for recognition,” in Proceedings of the IEEE Conference on Computer Vision and Pattern
Recognition Workshops, pp. 806–813, 2014.
J. Donahue, Y. Jia, O. Vinyals, J. Hoffman, N. Zhang, E. Tzeng, and
T. Darrell, “Decaf: A deep convolutional activation feature for generic
visual recognition.,” in Icml, vol. 32, pp. 647–655, 2014.
K. Bernardin and R. Stiefelhagen, “Evaluating multiple object tracking
performance: the clear mot metrics,” EURASIP Journal on Image and
Video Processing, vol. 2008, no. 1, pp. 1–10, 2008.
N. Hawes, C. Burbridge, F. Jovan, L. Kunze, B. Lacerda, L. Mudrová, J. Young, J. Wyatt, D. Hebesberger, T. Kortner, et al., “The
strands project: Long-term autonomy in everyday environments,” IEEE
Robotics & Automation Magazine, vol. 24, no. 3, pp. 146–156, 2017.
D. Fox, “Adapting the sample size in particle filters through kldsampling,” The international Journal of robotics research, vol. 22,
no. 12, pp. 985–1003, 2003.
S. L. Bowman, N. Atanasov, K. Daniilidis, and G. J. Pappas, “Probabilistic data association for semantic slam,” in Robotics and Automation
(ICRA), 2017 IEEE International Conference on, IEEE, 2017.

Submitted to IEEE Transactions on Robotics, October 2017. c 2017 IEEE. Personal use of this material is permitted. Permission from IEEE must be obtained
for all other uses, in any current or future media, including reprinting/republishing this material for advertising or promotional purposes, creating new collective
works, for resale or redistribution to servers or lists, or reuse of any copyrighted component of this work in other works.

