  Applications from manipulation to autonomous vehicles rely on robust and
general object tracking to safely perform tasks in dynamic environments. We
propose the first certifiably optimal category-level approach for simultaneous
shape estimation and pose tracking of an object of known category (e.g. a car).
Our approach uses 3D semantic keypoint measurements extracted from an RGB-D
image sequence, and phrases the estimation as a fixed-lag smoothing problem.
Temporal constraints enforce the object's rigidity (fixed shape) and smooth
motion according to a constant-twist motion model. The solutions to this
problem are the estimates of the object's state (poses, velocities) and shape
(paramaterized according to the active shape model) over the smoothing horizon.
Our key contribution is to show that despite the non-convexity of the fixed-lag
smoothing problem, we can solve it to certifiable optimality using a small-size
semidefinite relaxation. We also present a fast outlier rejection scheme that
filters out incorrect keypoint detections with shape and time compatibility
tests, and wrap our certifiable solver in a graduated non-convexity scheme. We
evaluate the proposed approach on synthetic and real data, showcasing its
performance in a table-top manipulation scenario and a drone-based vehicle
tracking application.
