296 research outputs found
On-Manifold Preintegration for Real-Time Visual-Inertial Odometry
Current approaches for visual-inertial odometry (VIO) are able to attain
highly accurate state estimation via nonlinear optimization. However, real-time
optimization quickly becomes infeasible as the trajectory grows over time, this
problem is further emphasized by the fact that inertial measurements come at
high rate, hence leading to fast growth of the number of variables in the
optimization. In this paper, we address this issue by preintegrating inertial
measurements between selected keyframes into single relative motion
constraints. Our first contribution is a \emph{preintegration theory} that
properly addresses the manifold structure of the rotation group. We formally
discuss the generative measurement model as well as the nature of the rotation
noise and derive the expression for the \emph{maximum a posteriori} state
estimator. Our theoretical development enables the computation of all necessary
Jacobians for the optimization and a-posteriori bias correction in analytic
form. The second contribution is to show that the preintegrated IMU model can
be seamlessly integrated into a visual-inertial pipeline under the unifying
framework of factor graphs. This enables the application of
incremental-smoothing algorithms and the use of a \emph{structureless} model
for visual measurements, which avoids optimizing over the 3D points, further
accelerating the computation. We perform an extensive evaluation of our
monocular \VIO pipeline on real and simulated datasets. The results confirm
that our modelling effort leads to accurate state estimation in real-time,
outperforming state-of-the-art approaches.Comment: 20 pages, 24 figures, accepted for publication in IEEE Transactions
on Robotics (TRO) 201
Lagrangian Duality in 3D SLAM: Verification Techniques and Optimal Solutions
State-of-the-art techniques for simultaneous localization and mapping (SLAM)
employ iterative nonlinear optimization methods to compute an estimate for
robot poses. While these techniques often work well in practice, they do not
provide guarantees on the quality of the estimate. This paper shows that
Lagrangian duality is a powerful tool to assess the quality of a given
candidate solution. Our contribution is threefold. First, we discuss a revised
formulation of the SLAM inference problem. We show that this formulation is
probabilistically grounded and has the advantage of leading to an optimization
problem with quadratic objective. The second contribution is the derivation of
the corresponding Lagrangian dual problem. The SLAM dual problem is a (convex)
semidefinite program, which can be solved reliably and globally by
off-the-shelf solvers. The third contribution is to discuss the relation
between the original SLAM problem and its dual. We show that from the dual
problem, one can evaluate the quality (i.e., the suboptimality gap) of a
candidate SLAM solution, and ultimately provide a certificate of optimality.
Moreover, when the duality gap is zero, one can compute a guaranteed optimal
SLAM solution from the dual problem, circumventing non-convex optimization. We
present extensive (real and simulated) experiments supporting our claims and
discuss practical relevance and open problems.Comment: 10 pages, 4 figure
- …