Frank Dellaert

ICRA Conference 2024 Conference Paper

A Group Theoretic Metric for Robot State Estimation Leveraging Chebyshev Interpolation

• Varun Agrawal
• Frank Dellaert

We propose a new metric for robot state estimation based on the recently introduced SE 2 (3) Lie group definition. Our metric is related to prior metrics for SLAM but explicitly takes into account the linear velocity of the state estimate, improving over current pose-based trajectory analysis. This has the benefit of providing a single, quantitative metric to evaluate state estimation algorithms against, while being compatible with existing tools and libraries. Since ground truth data generally consists of pose data from motion capture systems, we also propose an approach to compute the ground truth linear velocity based on polynomial interpolation. Using Chebyshev interpolation and a pseudospectral parameterization, we can accurately estimate the ground truth linear velocity of the trajectory in an optimal fashion with best approximation error. We demonstrate how this approach performs on multiple robotic platforms where accurate state estimation is vital, and compare it to alternative approaches such as finite differences. The pseudospectral parameterization also provides a means of trajectory data compression as an additional benefit. Experimental results show our method provides a valid and accurate means of comparing state estimation systems, which is also easy to interpret and report.

IROS Conference 2024 Conference Paper

Architectural-Scale Artistic Brush Painting with a Hybrid Cable Robot

• Gerry Chen
• Tristan Al-Haddad
• Frank Dellaert
• Seth Hutchinson 0001

Robot art presents an opportunity to both showcase and advance state-of-the-art robotics through the challenging task of creating art. Creating large-scale artworks in particular engages the public in a way that small-scale works cannot, and the distinct qualities of brush strokes contribute to an organic and human-like quality. Combining the large scale of murals with the strokes of the brush medium presents an especially impactful result, but also introduces unique challenges in maintaining precise, dextrous motion control of the brush across such a large workspace. In this work, we present the first robot to our knowledge that can paint architectural-scale murals with a brush. We create a hybrid robot consisting of a cable-driven parallel robot and 4 degree of freedom (DoF) serial manipulator to paint a 27m by 3. 7m mural on windows spanning 2-stories of a building. We discuss our approach to achieving both the scale and accuracy required for brush-painting a mural through a combination of novel mechanical design elements, coordinated planning and control, and on-site calibration algorithms with experimental validations.

ICRA Conference 2024 Conference Paper

Generalizing Trajectory Retiming to Quadratic Objective Functions

• Gerry Chen
• Frank Dellaert
• Seth Hutchinson 0001

Trajectory retiming is the task of computing a feasible time parameterization to traverse a path. It is commonly used in the decoupled approach to trajectory optimization whereby a path is first found, then a retiming algorithm computes a speed profile that satisfies kino-dynamic and other constraints. While trajectory retiming is most often formulated with the minimum-time objective (i. e. traverse the path as fast as possible), it is not always the most desirable objective, particularly when we seek to balance multiple objectives or when bang-bang control is unsuitable. In this paper, we present a novel algorithm based on factor graph variable elimination that can solve for the global optimum of the retiming problem with quadratic objectives as well (e. g. minimize control effort or match a nominal speed by minimizing squared error), which may extend to arbitrary objectives with iteration. Our work extends prior works, which find only solutions on the boundary of the feasible region, while maintaining the same linear time complexity from a single forward-backward pass. We experimentally demonstrate that (1) we achieve better real-world robot performance by using quadratic objectives in place of the minimum-time objective, and (2) our implementation is comparable or faster than state-of-the-art retiming algorithms.

ICRA Conference 2023 Conference Paper

A Hybrid Cable-Driven Robot for Non-Destructive Leafy Plant Monitoring and Mass Estimation using Structure from Motion

• Gerry Chen
• Harsh Muriki
• Andrew Sharkey
• Cédric Pradalier
• Yongsheng Chen
• Frank Dellaert

We propose a novel hybrid cable-based robot with manipulator and camera for high-accuracy, medium-throughput plant monitoring in a vertical hydroponic farm and, as an example application, demonstrate non-destructive plant mass estimation. Plant monitoring with high temporal and spatial resolution is important to both farmers and researchers to detect anomalies and develop predictive models for plant growth. The availability of high-quality, off-the-shelf structure-from-motion (SfM) and photogrammetry packages has enabled a vibrant community of roboticists to apply computer vision for non-destructive plant monitoring. While existing approaches tend to focus on either high-throughput (e. g. satellite, unmanned aerial vehicle (UAV), vehicle-mounted, conveyor-belt imagery) or high-accuracy/robustness to occlusions (e. g. turn-table scanner or robot arm), we propose a middle-ground that achieves high accuracy with a medium-throughput, highly automated robot. Our design pairs the workspace scalability of a cable-driven parallel robot (CDPR) with the dexterity of a 4 degree-of-freedom (DoF) robot arm to autonomously image many plants from a variety of viewpoints. We describe our robot design and demonstrate it experimentally by collecting daily photographs of 54 plants from 64 viewpoints each. We show that our approach can produce scientifically useful measurements, operate fully autonomously after initial calibration, and produce better reconstructions and plant property estimates than those of over-canopy methods (e. g. UAV). As example applications, we show that our system can successfully estimate plant mass with a Mean Absolute Error (MAE) of 0. 586g and, when used to perform hypothesis testing on the relationship between mass and age, produces p-values comparable to ground-truth data (p=0. 0020 and p=0. 0016, respectively).

ICRA Conference 2023 Conference Paper

Constraint Manifolds for Robotic Inference and Planning

• Yetong Zhang
• Fan Jiang
• Gerry Chen
• Varun Agrawal
• Adam Rutkowski
• Frank Dellaert

We propose a manifold optimization approach for solving constrained inference and planning problems. The approach employs a framework that transforms an arbitrary nonlinear equality constrained optimization problem into an unconstrained manifold optimization problem. The core of the transformation process is the formulation of constraint manifolds that represent sets of variables subject to equality constraints. We propose various approaches to define the tan-gent spaces and retraction operations of constraint manifolds, which are crucial for manifold optimization. We evaluate our constraint manifold optimization approach on multiple constrained inference and planning problems, and show that it generates strictly feasible results with increased efficiency as compared to state-of-the-art constrained optimization methods.

IROS Conference 2022 Conference Paper

Efficient Range-Constraint Manifold Optimization with Application to Cooperative Navigation

• Yetong Zhang
• Gerry Chen
• Adam Rutkowski
• Frank Dellaert

We present a manifold optimization approach to solve inference and planning problems with range constraints. The core of our approach is the definition of a manifold that represents points or poses with range constraints. We discover that the manifold of range-constrained points is homogeneous under the rigid transformation group action, and utilize the group action to derive the tangent space, retraction and topology of the manifold. We evaluate the performance of manifold optimization approach on solving range-constrained inference problems over state-of-the-art constrained optimization methods. The results show that manifold optimization with the range-constraint manifold achieves both faster speed and better constraint satisfaction. We further study the conditions of inference problems that we can treat range measurements as constraints in practice.

ICRA Conference 2022 Conference Paper

GTGraffiti: Spray Painting Graffiti Art from Human Painting Motions with a Cable Driven Parallel Robot

• Gerry Chen
• Sereym Baek
• Juan-Diego Florez
• Wanli Qian
• Sang-won Leigh
• Seth Hutchinson 0001
• Frank Dellaert

We present GTGraffiti, a graffiti painting system from Georgia Tech that tackles challenges in art, hardware, and human-robot collaboration. The problem of painting graffiti in a human style is particularly challenging and requires a system-level approach because the robotics and art must be designed around each other. The robot must be highly dynamic over a large workspace while the artist must work within the robot's limitations. Our approach consists of three stages: artwork capture, robot hardware, and planning & control. We use motion capture to capture collaborator painting motions which are then composed and processed into a time-varying linear feedback controller for a cable-driven parallel robot (CDPR) to execute. In this work, we will describe the capturing process, the design and construction of a purpose-built CDPR, and the software for turning an artist's vision into control commands. Our work represents an important step towards faithfully recreating human graffiti artwork by demonstrating that we can reproduce artist motions up to 2m/s and 20m/s 2 within 9. 3mm RMSE to paint artworks.

IROS Conference 2022 Conference Paper

InCOpt: Incremental Constrained Optimization using the Bayes Tree

• Mohamad Qadri
• Paloma Sodhi
• Joshua G. Mangelson
• Frank Dellaert
• Michael Kaess

In this work, we investigate the problem of incre-mentally solving constrained non-linear optimization problems formulated as factor graphs. Prior incremental solvers were either restricted to the unconstrained case or required periodic batch relinearizations of the objective and constraints which are expensive and detract from the online nature of the algorithm. We present InCOpt, an Augmented Lagrangian-based incremental constrained optimizer that views matrix operations as message passing over the Bayes tree. We first show how the linear system, resulting from linearizing the constrained objective, can be represented as a Bayes tree. We then propose an algorithm that views forward and back substitutions, which naturally arise from solving the Lagrangian, as upward and downward passes on the tree. Using this formulation, In-COpt can exploit properties such as fluid/online relinearization leading to increased accuracy without a sacrifice in runtime. We evaluate our solver on different applications (navigation and manipulation) and provide an extensive evaluation against existing constrained and unconstrained solvers.

IROS Conference 2022 Conference Paper

Locally Optimal Estimation and Control of Cable Driven Parallel Robots using Time Varying Linear Quadratic Gaussian Control

• Gerry Chen
• Seth Hutchinson 0001
• Frank Dellaert

We present a locally optimal tracking controller for Cable Driven Parallel Robot (CDPR) control based on a time-varying Linear Quadratic Gaussian (TV-LQG) controller. In contrast to many methods which use fixed feedback gains, our time-varying controller computes the optimal gains depending on the location in the workspace and the future trajectory. Meanwhile, we rely heavily on offline computation to reduce the burden of online implementation and feasibility checking. Following the growing popularity of probabilistic graphical models for optimal control, we use factor graphs as a tool to formulate our controller for their efficiency, intuitiveness, and modularity. The topology of a factor graph encodes the relevant structural properties of equations in a way that facilitates insight and efficient computation using sparse linear algebra solvers. We first use factor graph optimization to compute a nominal trajectory, then linearize the graph and apply variable elimination to compute the locally optimal, time varying linear feedback gains. Next, we leverage the factor graph formulation to compute the locally optimal, time-varying Kalman Filter gains, and finally combine the locally optimal linear control and estimation laws to form a TV-LQG controller. We compare the tracking accuracy of our TV-LQG controller to a state-of-the-art dual-space feed-forward controller on a 2. 9m x 2. 3m, 4-cable planar robot and demonstrate improved tracking accuracies of 0. 8° and 11. 6 mm root mean square error in rotation and translation respectively.

ICRA Conference 2022 Conference Paper

Simultaneous Control and Trajectory Estimation for Collision Avoidance of Autonomous Robotic Spacecraft Systems

• Matthew King-Smith
• Panagiotis Tsiotras
• Frank Dellaert

We propose factor graph optimization for simultaneous planning, control, and trajectory estimation for collision-free navigation of autonomous systems in environments with moving objects. The proposed online probabilistic motion planning and trajectory estimation navigation technique generates optimal collision-free state and control trajectories for autonomous vehicles when the obstacle motion model is both unknown and known. We evaluate the utility of the algorithm to support future autonomous robotic space missions.

ICRA Conference 2021 Conference Paper

Continuous-time State & Dynamics Estimation using a Pseudo-Spectral Parameterization

• Varun Agrawal
• Frank Dellaert

We present a novel continuous time trajectory representation based on a Chebyshev polynomial basis, which when governed by known dynamics models, allows for full trajectory and robot dynamics estimation, particularly useful for high-performance robotics applications such as unmanned aerial vehicles. We show that we can gracefully incorporate model dynamics to our trajectory representation, within a factor-graph based framework, and leverage ideas from pseudo- spectral optimal control to parameterize the state and the control trajectories as interpolating polynomials. This allows us to perform efficient optimization at specifically chosen points derived from the theory, while recovering full trajectory estimates. Through simulated experiments we demonstrate the applicability of our representation for accurate flight dynamics estimation for multirotor aerial vehicles. The representation framework is general and can thus be applied to a multitude of high-performance applications beyond multirotor platforms.

ICRA Conference 2021 Conference Paper

Equality Constrained Linear Optimal Control With Factor Graphs

• Shuo Yang
• Gerry Chen
• Yetong Zhang
• Howie Choset
• Frank Dellaert

This paper presents a novel factor graph-based approach to solve the discrete-time finite-horizon Linear Quadratic Regulator problem subject to auxiliary linear equality constraints within and across time steps. We represent such optimal control problems using constrained factor graphs and optimize the factor graphs to obtain the optimal trajectory and the feedback control policies using the variable elimination algorithm with a modified Gram-Schmidt process. We prove that our approach has the same order of computational complexity as the state-of-the-art dynamic programming approach. Furthermore, current dynamic programming approaches can only handle equality constraints between variables at the same time step, but ours can handle equality constraints among any combination of variables at any time step while maintaining linear complexity with respect to trajectory length. Our approach can be used to efficiently generate trajectories and feedback control policies to achieve periodic motion or repetitive manipulation.

ICRA Conference 2021 Conference Paper

Factor Graph-Based Trajectory Optimization for a Pneumatically-Actuated Jumping Robot

• Lucas O. Tiziani
• Yetong Zhang
• Frank Dellaert
• Frank L. Hammond

Roboticists have increasingly sought to incorporate mechanical compliance into legged robots to realize a range of potential benefits, from improved agility to resilience in complex environments. A promising approach for building compliance into robot legs is to utilize the pneumatic artificial muscle, a pneumatic actuator with inherent compliance due to the compressibility of air. While previous work has explored the capabilities of pneumatic-muscle driven robots in highly dynamic tasks like jumping, there is a lack of trajectory planning strategies for such robots. In this paper, we detail our approach to planning vertical jumping trajectories for a planar two-legged robot driven by four pneumatic artificial muscles using on/off "burst inflation" control. The trajectory optimization problem is represented as a factor graph and solved with the GTSAM optimizer. A hybrid dynamics approach is used to handle foot-ground contacts. The average jump height error between simulation and experiment across multiple jumping trajectories of varying heights was 9. 5 cm; the average RMS error between all four joints was 5. 6 deg. This work provides a basis to plan more complex jumping and leaping trajectories for pneumatic muscle-driven robots.

IROS Conference 2021 Conference Paper

MR-iSAM2: Incremental Smoothing and Mapping with Multi-Root Bayes Tree for Multi-Robot SLAM

• Yetong Zhang
• Ming Hsiao
• Jing Dong 0002
• Jakob J. Engel
• Frank Dellaert

We present multi-robot iSAM2 (MR-iSAM2), an efficient incremental smoothing and mapping (iSAM) algorithm to solve multi-robot simultaneous localization and mapping (SLAM) inference problems. MR-iSAM2 is based on a novel data structure multi-root Bayes tree (MRBT), which packs multiple Bayes trees with the same undirected clique structure. In multi-robot scenarios, the MRBT enables new measurements from different robots to be updated in different root branches, while all updates are performed around the single root of the Bayes tree in the original iSAM2 algorithm. As a result, the MRBT better reveals the underlying sparsity and information flow in multi-robot SLAM inference problems than the Bayes tree. Based on this insight, we further develop MR-iSAM2 to incrementally update and maintain the sparsity structure of the MRBT and enable efficient information propagation among the roots for inter-robot inference. We analyze the properties of the MR-iSAM2 algorithm, and show with both synthetic and real world datasets that it significantly outperforms iSAM2 in efficiency when solving multi-robot SLAM problems.

IROS Conference 2020 Conference Paper

Robot Calligraphy using Pseudospectral Optimal Control in Conjunction with a Novel Dynamic Brush Model

• Sen Wang
• Jiaqi Chen
• Xuanliang Deng
• Seth Hutchinson 0001
• Frank Dellaert

Chinese calligraphy is a unique art form with great artistic value but difficult to master. In this paper, we formulate the calligraphy writing problem as a trajectory optimization problem, and propose an improved virtual brush model for simulating the real writing process. Our approach is inspired by pseudospectral optimal control in that we parameterize the actuator trajectory for each stroke as a Chebyshev polynomial. The proposed dynamic virtual brush model plays a key role in formulating the objective function to be optimized. Our approach shows excellent performance in drawing aesthetically pleasing characters, and does so much more efficiently than previous work, opening up the possibility to achieve real-time closed-loop control.

ICRA Conference 2018 Conference Paper

Sparse Gaussian Processes on Matrix Lie Groups: A Unified Framework for Optimizing Continuous-Time Trajectories

• Jing Dong 0002
• Mustafa Mukadam
• Byron Boots
• Frank Dellaert

Continuous-time trajectories are useful for reasoning about robot motion in a wide range of tasks. Sparse Gaussian processes (GPs) can be used as a non-parametric representation for trajectory distributions that enables fast trajectory optimization by sparse GP regression. However, most previous approaches that utilize sparse GPs for trajectory optimization are limited by the fact that the robot state is represented in vector space. In this paper, we first extend previous works to consider the state on general matrix Lie groups by applying a constant-velocity prior and defining locally linear GPs. Next, we discuss how sparse GPs on Lie groups provide a unified continuous-time framework for trajectory optimization for solving a number of robotics problems including state estimation and motion planning. Finally, we demonstrate and evaluate our approach on several different estimation and motion planning tasks with both synthetic and real-world experiments.

ICRA Conference 2017 Conference Paper

4D crop monitoring: Spatio-temporal reconstruction for agriculture

• Jing Dong 0002
• John Gary Burnham
• Byron Boots
• Glen C. Rains
• Frank Dellaert

Autonomous crop monitoring at high spatial and temporal resolution is a critical problem in precision agriculture. While Structure from Motion and Multi-View Stereo algorithms can finely reconstruct the 3D structure of a field with low-cost image sensors, these algorithms fail to capture the dynamic nature of continuously growing crops. In this paper we propose a 4D reconstruction approach to crop monitoring, which employs a spatio-temporal model of dynamic scenes that is useful for precision agriculture applications. Additionally, we provide a robust data association algorithm to address the problem of large appearance changes due to scenes being viewed from different angles at different points in time, which is critical to achieving 4D reconstruction. Finally, we collected a high-quality dataset with ground-truth statistics to evaluate the performance of our method. We demonstrate that our 4D reconstruction approach provides models that are qualitatively correct with respect to visual appearance and quantitatively accurate when measured against the ground truth geometric properties of the monitored crops.

ICRA Conference 2016 Conference Paper

Distributed trajectory estimation with privacy and communication constraints: A two-stage distributed Gauss-Seidel approach

• Siddharth Choudhary
• Luca Carlone
• Carlos Nieto-Granda
• John G. Rogers
• Henrik I. Christensen
• Frank Dellaert

We propose a distributed algorithm to estimate the 3D trajectories of multiple cooperative robots from relative pose measurements. Our approach leverages recent results [1] which show that the maximum likelihood trajectory is well approximated by a sequence of two quadratic subproblems. The main contribution of the present work is to show that these subproblems can be solved in a distributed manner, using the distributed Gauss-Seidel (DGS) algorithm. Our approach has several advantages. It requires minimal information exchange, which is beneficial in presence of communication and privacy constraints. It has an anytime flavor: after few iterations the trajectory estimates are already accurate, and they asymptotically convergence to the centralized estimate. The DGS approach scales well to large teams, and it has a straightforward implementation. We test the approach in simulations and field tests, demonstrating its advantages over related techniques.

ICRA Conference 2015 Conference Paper

Differential dynamic programming for optimal estimation

• Marin Kobilarov
• Duy-Nguyen Ta
• Frank Dellaert

This paper studies an optimization-based approach for solving optimal estimation and optimal control problems through a unified computational formulation. The goal is to perform trajectory estimation over extended past horizons and model-predictive control over future horizons by enforcing the same dynamics, control, and sensing constraints in both problems, and thus solving both problems with identical computational tools. Through such systematic estimation-control formulation we aim to improve the performance of autonomous systems such as agile robotic vehicles. This work focuses on sequential sweep trajectory optimization methods, and more specifically extends the method known as differential dynamic programming to the parameter-dependent setting in order to enable the solutions to general estimation and control problems.

ICRA Conference 2015 Conference Paper

Distributed real-time cooperative localization and mapping using an uncertainty-aware expectation maximization approach

• Jing Dong 0002
• Erik Nelson
• Vadim Indelman
• Nathan Michael
• Frank Dellaert

We demonstrate distributed, online, and real-time cooperative localization and mapping between multiple robots operating throughout an unknown environment using indirect measurements. We present a novel Expectation Maximization (EM) based approach to efficiently identify inlier multi-robot loop closures by incorporating robot pose uncertainty, which significantly improves the trajectory accuracy over long-term navigation. An EM and hypothesis based method is used to determine a common reference frame. We detail a 2D laser scan correspondence method to form robust correspondences between laser scans shared amongst robots. The implementation is experimentally validated using teams of aerial vehicles, and analyzed to determine its accuracy, computational efficiency, scalability to many robots, and robustness to varying environments. We demonstrate through multiple experiments that our method can efficiently build maps of large indoor and outdoor environments in a distributed, online, and real-time setting.

ICRA Conference 2015 Conference Paper

Duality-based verification techniques for 2D SLAM

• Luca Carlone
• Frank Dellaert

While iterative optimization techniques for Simultaneous Localization and Mapping (SLAM) are now very efficient and widely used, none of them can guarantee global convergence to the maximum likelihood estimate. Local convergence usually implies artifacts in map reconstruction and large localization errors, hence it is very undesirable for applications in which accuracy and safety are of paramount importance. We provide a technique to verify if a given 2D SLAM solution is globally optimal. The insight is that, while computing the optimal solution is hard in general, duality theory provides tools to compute tight bounds on the optimal cost, via convex programming. These bounds can be used to evaluate the quality of a SLAM solution, hence providing a “sanity check” for state-of-the-art incremental and batch solvers. Experimental results show that our technique successfully identifies wrong estimates (i. e. , local minima) in large-scale SLAM scenarios. This work, together with [1], represents a step towards the objective of having SLAM techniques with guaranteed performance, that can be used in safety-critical applications.

IROS Conference 2015 Conference Paper

Exactly sparse memory efficient SLAM using the multi-block alternating direction method of multipliers

• Siddharth Choudhary
• Luca Carlone
• Henrik I. Christensen
• Frank Dellaert

Large-scale SLAM demands for scalable techniques in which the computational burden and the memory consumption is shared among many processing units. While recent literature offers competitive approaches for scalable mapping, these usually involve approximations to preserve sparsity of the resulting subproblems. We present an approach to scalable SLAM that is exactly sparse. The main insight is that rather than eliminating variables (which induces dense cliques), we split the separators connecting subgraphs. Then, we enforce consistency of the separators in different subgraphs using hard constraints. The resulting constrained optimization problem can be solved in a decentralized manner using the multi-block Alternating Direction Method of Multipliers (ADMM). Our framework is appealing since (i) it preserves the sparsity structure of the original problem, (ii) it has a straightforward implementation, (iii) it allows to easily trade-off between computation time and accuracy. While our approach is currently slower than competitors, it is more accurate than other memory efficient alternatives. Moreover, we believe that the proposed framework can be of interest on its own as it draws connections with recent literature on decentralized optimization.

ICRA Conference 2015 Conference Paper

Information-based reduced landmark SLAM

• Siddharth Choudhary
• Vadim Indelman
• Henrik I. Christensen
• Frank Dellaert

In this paper, we present an information-based approach to select a reduced number of landmarks and poses for a robot to localize itself and simultaneously build an accurate map. We develop an information theoretic algorithm to efficiently reduce the number of landmarks and poses in a SLAM estimate without compromising the accuracy of the estimated trajectory. We also propose an incremental version of the reduction algorithm which can be used in SLAM framework resulting in information based reduced landmark SLAM. The results of reduced landmark based SLAM algorithm are shown on Victoria park dataset and a Synthetic dataset and are compared with standard graph SLAM (SAM [6]) algorithm. We demonstrate a reduction of 40-50% in the number of landmarks and around 55% in the number of poses with minimal estimation error as compared to standard SLAM algorithm.

ICRA Conference 2015 Conference Paper

Initialization techniques for 3D SLAM: A survey on rotation estimation and its use in pose graph optimization

• Luca Carlone
• Roberto Tron
• Kostas Daniilidis
• Frank Dellaert

Pose graph optimization is the non-convex optimization problem underlying pose-based Simultaneous Localization and Mapping (SLAM). If robot orientations were known, pose graph optimization would be a linear least-squares problem, whose solution can be computed efficiently and reliably. Since rotations are the actual reason why SLAM is a difficult problem, in this work we survey techniques for 3D rotation estimation. Rotation estimation has a rich history in three scientific communities: robotics, computer vision, and control theory. We review relevant contributions across these communities, assess their practical use in the SLAM domain, and benchmark their performance on representative SLAM problems (Fig. 1). We show that the use of rotation estimation to bootstrap iterative pose graph solvers entails significant boost in convergence speed and robustness.

IROS Conference 2015 Conference Paper

Lagrangian duality in 3D SLAM: Verification techniques and optimal solutions

• Luca Carlone
• David M. Rosen
• Giuseppe Carlo Calafiore
• John J. Leonard
• Frank Dellaert

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.

ICRA Conference 2015 Conference Paper

Monocular image space tracking on a computationally limited MAV

• Kyel Ok
• Dinesh Gamage
• Tom Drummond
• Frank Dellaert
• Nicholas Roy

We propose a method of monocular camera-inertial based navigation for computationally limited micro air vehicles (MAVs). Our approach is derived from the recent development of parallel tracking and mapping algorithms, but unlike previous results, we show how the tracking and mapping processes operate using different representations. The separation of representations allows us not only to move the computational load of full map inference to a ground station, but to further reduce the computational cost of on-board tracking for pose estimation. Our primary contribution is to show how the cost of tracking the vehicle pose on-board can be substantially reduced by estimating the camera motion directly in the image frame, rather than in the world co-ordinate frame. We demonstrate our method on an Ascending Technologies Pelican quad-rotor, and show that we can track the vehicle pose with reduced on-board computation but without compromised navigation accuracy.

ICRA Conference 2014 Conference Paper

A hierarchical wavelet decomposition for continuous-time SLAM

• Sean Anderson
• Frank Dellaert
• Tim D. Barfoot

This paper proposes using hierarchical wavelets as a basis in parametric continuous-time batch estimation. The need for a continuous-time robot pose in the simultaneous localization and mapping (SLAM) problem has arisen as state-of-the-art batch SLAM algorithms attempt to handle more challenging hardware; specifically, the continuous-time framework is particularly beneficial when using high-rate sensors, multiple unsynchronized sensors, or scanning sensors, such as lidar and rolling-shutter cameras, during motion. Although the traditional discrete-time SLAM formulation can be adapted by using temporal pose interpolation, approaches using the continuous-time framework are able to generate smooth robot trajectories with less state variables. In this paper, we focus on the parametric approach using temporal basis functions to develop a finite-element representation of the continuous-time robot trajectory. While the majority of current implementations have utilized a uniformly spaced B-spline basis, we note that trajectory richness is often quite variable; in this paper, we show how a hierarchical system of wavelet basis functions can be used to increase the resolution of the solution only in the temporally local regions of the trajectory that require additional detail. We validate our approach by contrasting uniform B-splines and wavelets in a six-dimensional pose-graph SLAM experiment, using both simulated and real data.

ICRA Conference 2014 Conference Paper

Constrained optimal selection for multi-sensor robot navigation using plug-and-play factor graphs

• Han-Pang Chiu
• Xun S. Zhou
• Luca Carlone
• Frank Dellaert
• Supun Samarasekera
• Rakesh Kumar 0001

This paper proposes a real-time navigation approach that is able to integrate many sensor types while fulfilling performance needs and system constraints. Our approach uses a plug-and-play factor graph framework, which extends factor graph formulation to encode sensor measurements with different frequencies, latencies, and noise distributions. It provides a flexible foundation for plug-and-play sensing, and can incorporate new evolving sensors. A novel constrained optimal selection mechanism is presented to identify the optimal subset of active sensors to use, during initialization and when any sensor condition changes. This mechanism constructs candidate subsets of sensors based on heuristic rules and a ternary tree expansion algorithm. It quickly decides the optimal subset among candidates by maximizing observability coverage on state variables, while satisfying resource constraints and accuracy demands. Experimental results demonstrate that our approach selects subsets of sensors to provide satisfactory navigation solutions under various conditions, on large-scale real data sets using many sensors.

IROS Conference 2014 Conference Paper

Direct superpixel labeling for mobile robot navigation using learned general optical flow templates

• Richard Roberts 0001
• Frank Dellaert

Towards the goal of autonomous obstacle avoidance for mobile robots, we present a method for superpixel labeling using optical flow templates. Optical flow provides a rich source of information that complements image appearance and point clouds in determining traversability. While much past work uses optical flow towards traversability in a heuristic manner, the method we present here instead classifies flow according to several optical flow templates that are specific to the typical environment shape. Our first contribution over prior work in superpixel labeling using optical flow templates is large improvements in accuracy and efficiency by inference directly from spatiotemporal gradients instead of from independently-computed optical flow, and from improved optical flow modeling for obstacles. Our second contribution over the same is extending superpixel labeling methods to arbitrary camera optics without the need to calibrate the camera, by developing and demonstrating a method for learning optical flow templates from unlabeled video. Our experiments demonstrate successful obstacle detection in an outdoor mobile robot dataset.

ICRA Conference 2014 Conference Paper

Eliminating conditionally independent sets in factor graphs: A unifying perspective based on smart factors

• Luca Carlone
• Zsolt Kira
• Chris Beall
• Vadim Indelman
• Frank Dellaert

Factor graphs are a general estimation framework that has been widely used in computer vision and robotics. In several classes of problems a natural partition arises among variables involved in the estimation. A subset of the variables are actually of interest for the user: we call those target variables. The remaining variables are essential for the formulation of the optimization problem underlying maximum a posteriori (MAP) estimation; however these variables, that we call support variables, are not strictly required as output of the estimation problem. In this paper, we propose a systematic way to abstract support variables, defining optimization problems that are only defined over the set of target variables. This abstraction naturally leads to the definition of smart factors, which correspond to constraints among target variables. We show that this perspective unifies the treatment of heterogeneous problems, ranging from structureless bundle adjustment to robust estimation in SLAM. Moreover, it enables to exploit the underlying structure of the optimization problem and the treatment of degenerate instances, enhancing both computational efficiency and robustness.

IROS Conference 2014 Conference Paper

iSPCG: Incremental subgraph-preconditioned conjugate gradient method for online SLAM with many loop-closures

• Yong-Dian Jian
• Frank Dellaert

We propose a novel method to solve online SLAM problems with many loop-closures on the basis of two state-of-the-art SLAM methods, iSAM and SPCG. We first use iSAM to solve a sparse sub-problem to obtain an approximate solution. When the error grows larger than a threshold or the optimal solution is requested, we use subgraph-preconditioned conjugate gradient method to solve the original problem where the subgraph preconditioner and initial estimate are provided by iSAM. Finally we use the optimal solution from SPCG to regularize iSAM in the next steps. The proposed method is consistent, efficient and can find the optimal solution. We apply this method to solve large simulated and real SLAM problems, and obtain promising results.

IROS Conference 2014 Conference Paper

Linear-time estimation with tree assumed density filtering and low-rank approximation

• Duy-Nguyen Ta
• Frank Dellaert

We present two fast and memory-efficient approximate estimation methods, targeting obstacle avoidance applications on small robot platforms. Our methods avoid a main bottleneck of traditional filtering techniques, which creates densely correlated cliques of landmarks, leading to expensive time and space complexity. We introduce a novel technique to avoid the dense cliques by sparsifying them into a tree structure and maintain that tree structure efficiently over time. Unlike other edge removal graph sparsification methods, our methods sparsify the landmark cliques by introducing new variables to de-correlate them. The first method projects the current density onto a tree rooted at the same variable at each step. The second method improves upon the first one by carefully choosing a new low-dimensional root variable at each step to replace such that the independence and conditional densities of the landmarks given the trajectory are optimally preserved. Our experiments show a significant improvement in time and space complexity of the methods compared to other standard filtering techniques in worst-case scenarios, with small trade-offs in accuracy due to low-rank approximation errors.

ICRA Conference 2014 Conference Paper

Modern MAP inference methods for accurate and fast occupancy grid mapping on higher order factor graphs

• Vikas Dhiman
• Abhijit Kundu
• Frank Dellaert
• Jason J. Corso

Using the inverse sensor model has been popular in occupancy grid mapping. However, it is widely known that applying the inverse sensor model to mapping requires certain assumptions that are not necessarily true. Even the works that use forward sensor models have relied on methods like expectation maximization or Gibbs sampling which have been succeeded by more effective methods of maximum a posteriori (MAP) inference over graphical models. In this paper, we propose the use of modern MAP inference methods along with the forward sensor model. Our implementation and experimental results demonstrate that these modern inference methods deliver more accurate maps more efficiently than previously used methods.

ICRA Conference 2014 Conference Paper

Multi-robot pose graph localization and data association from unknown initial relative poses via expectation maximization

• Vadim Indelman
• Erik Nelson
• Nathan Michael
• Frank Dellaert

This paper presents a novel approach for multirobot pose graph localization and data association without requiring prior knowledge about the initial relative poses of the robots. Without a common reference frame, the robots can only share observations of interesting parts of the environment, and trying to match between observations from different robots will result in many outlier correspondences. Our approach is based on the following key observation: while each multi-robot correspondence can be used in conjunction with the local robot estimated trajectories, to calculate the transformation between the robot reference frames, only the inlier correspondences will be similar to each other. Using this concept, we develop an expectation-maximization (EM) approach to efficiently infer the robot initial relative poses and solve the multi-robot data association problem. Once this transformation between the robot reference frames is estimated with sufficient measure of confidence, we show that a similar EM formulation can be used to solve also the full multi-robot pose graph problem with unknown multi-robot data association. We evaluate the performance of the developed approach both in a statistical synthetic-environment study and in a real-data experiment, demonstrating its robustness to high percentage of outliers.

ICRA Conference 2014 Conference Paper

Planning under uncertainty in the continuous domain: A generalized belief space approach

• Vadim Indelman
• Luca Carlone
• Frank Dellaert

This work investigates the problem of planning under uncertainty, with application to mobile robotics. We propose a probabilistic framework in which the robot bases its decisions on the generalized belief, which is a probabilistic description of its own state and of external variables of interest. The approach naturally leads to a dual-layer architecture: an inner estimation layer, which performs inference to predict the outcome of possible decisions, and an outer decisional layer which is in charge of deciding the best action to undertake. The approach does not discretize the state or control space, and allows planning in continuous domain. Moreover, it allows to relax the assumption of maximum likelihood observations: predicted measurements are treated as random variables and are not considered as given. Experimental results show that our planning approach produces smooth trajectories while maintaining uncertainty within reasonable bounds.

IROS Conference 2014 Conference Paper

Selecting good measurements via ℓ1 relaxation: A convex approach for robust estimation over graphs

• Luca Carlone
• Andrea Censi
• Frank Dellaert

Pose graph optimization is an elegant and efficient formulation for robot localization and mapping. Experimental evidence suggests that, in real problems, the set of measurements used to estimate robot poses is prone to contain outliers, due to perceptual aliasing and incorrect data association. While several related works deal with the rejection of outliers during pose estimation, the goal of this paper is to propose a grounded strategy for measurements selection, i. e. , the output of our approach is a set of “reliable” measurements, rather than pose estimates. Because the classification in inliers/outliers is not observable in general, we pose the problem as finding the maximal subset of the measurements that is internally coherent. In the linear case, we show that the selection of the maximal coherent set can be (conservatively) relaxed to obtain a linear programming problem with ℓ 1 objective. We show that this approach can be extended to (nonlinear) planar pose graph optimization using similar ideas as our previous work on linear approaches to pose graph optimization. We evaluate our method on standard datasets, and we show that it is robust to a large number of outliers and different outlier generation models, while entailing the advantages of linear programming (fast computation, scalability).

IROS Conference 2014 Conference Paper

SLAM with object discovery, modeling and mapping

• Siddharth Choudhary
• Alexander J. B. Trevor
• Henrik I. Christensen
• Frank Dellaert

Object discovery and modeling have been widely studied in the computer vision and robotics communities. SLAM approaches that make use of objects and higher level features have also recently been proposed. Using higher level features provides several benefits: these can be more discriminative, which helps data association, and can serve to inform service robotic tasks that require higher level information, such as object models and poses. We propose an approach for online object discovery and object modeling, and extend a SLAM system to utilize these discovered and modeled objects as landmarks to help localize the robot in an online manner. Such landmarks are particularly useful for detecting loop closures in larger maps. In addition to the map, our system outputs a database of detected object models for use in future SLAM or service robotic tasks. Experimental results are presented to demonstrate the approach's ability to detect and model objects, as well as to improve SLAM results by detecting loop closures.

ICRA Conference 2013 Conference Paper

DDF-SAM 2. 0: Consistent distributed smoothing and mapping

• Alexander Cunningham
• Vadim Indelman
• Frank Dellaert

This paper presents an consistent decentralized data fusion approach for robust multi-robot SLAM in dangerous, unknown environments. The DDF-SAM 2. 0 approach extends our previous work by combining local and neighborhood information in a single, consistent augmented local map, without the overly conservative approach to avoiding information double-counting in the previous DDF-SAM algorithm. We introduce the anti-factor as a means to subtract information in graphical SLAM systems, and illustrate its use to both replace information in an incremental solver and to cancel out neighborhood information from shared summarized maps. This paper presents and compares three summarization techniques, with two exact approaches and an approximation. We evaluated the proposed system in a synthetic example and show the augmented local system and the associated summarization technique do not double-count information, while keeping performance tractable.

IROS Conference 2013 Conference Paper

Incremental light bundle adjustment for robotics navigation

• Vadim Indelman
• Andrew Melim
• Frank Dellaert

This paper presents a new computationally-efficient method for vision-aided navigation (VAN) in autonomous robotic applications. While many VAN approaches are capable of processing incoming visual observations, incorporating loop-closure measurements typically requires performing a bundle adjustment (BA) optimization, that involves both all the past navigation states and the observed 3D points. Our approach extends the incremental light bundle adjustment (LBA) method, recently developed for structure from motion [10], to information fusion in robotics navigation and in particular for including loop-closure information. Since in many robotic applications the prime focus is on navigation rather then mapping, and as opposed to traditional BA, we algebraically eliminate the observed 3D points and do not explicitly estimate them. Computational complexity is further improved by applying incremental inference. To maintain highrate performance over time, consecutive IMU measurements are summarized using a recently-developed technique and navigation states are added to the optimization only at camera rate. If required, the observed 3D points can be reconstructed at any time based on the optimized robot's poses. The proposed method is compared to BA both in terms of accuracy and computational complexity in a statistical simulation study.

ICRA Conference 2013 Conference Paper

Path planning with uncertainty: Voronoi Uncertainty Fields

• Kyel Ok
• Sameer Ansari
• Billy Gallagher
• William Sica
• Frank Dellaert
• Mike Stilman

In this paper, a two-level path planning algorithm that deals with map uncertainty is proposed. The higher level planner uses modified generalized Voronoi diagrams to guarantee finding a connected path from the start to the goal if a collision-free path exists. The lower level planner considers uncertainty of the observed obstacles in the environment and assigns repulsive forces based on their distance to the robot and their positional uncertainty. The attractive forces from the Voronoi nodes and the repulsive forces from the uncertainty-biased potential fields form a hybrid planner we call Voronoi Uncertainty Fields (VUF). The proposed planner has two strong properties: (1) bias against uncertain obstacles, and (2) completeness. We analytically prove the properties and run simulations to validate our method in a forest-like environment.

ICRA Conference 2013 Conference Paper

Robust vision-aided navigation using Sliding-Window Factor graphs

• Han-Pang Chiu
• Stephen Williams
• Frank Dellaert
• Supun Samarasekera
• Rakesh Kumar 0001

This paper proposes a navigation algorithm that provides a low-latency solution while estimating the full nonlinear navigation state. Our approach uses Sliding-Window Factor Graphs, which extend existing incremental smoothing methods to operate on the subset of measurements and states that exist inside a sliding time window. We split the estimation into a fast short-term smoother, a slower but fully global smoother, and a shared map of 3D landmarks. A novel three-stage visual feature model is presented that takes advantage of both smoothers to optimize the 3D landmark map, while minimizing the computation required for processing tracked features in the short-term smoother. This three-stage model is formulated based on the maturity of the estimation of the 3D location of the underlying landmark in the map. Long-range associations are used as global measurements from matured landmarks in the short-term smoother and loop closure constraints in the long-term smoother. Experimental results demonstrate our approach provides highly-accurate solutions on large-scale real data sets using multiple sensors in GPS-denied settings.

IROS Conference 2013 Conference Paper

Support-theoretic subgraph preconditioners for large-scale SLAM

• Yong-Dian Jian
• Doru Balcan
• Ioannis Panageas
• Prasad Tetali
• Frank Dellaert

Efficiently solving large-scale sparse linear systems is important for robot mapping and navigation. Recently, the subgraph-preconditioned conjugate gradient method has been proposed to combine the advantages of two reigning paradigms, direct and iterative methods, to improve the efficiency of the solver. Yet the question of how to pick a good subgraph is still an open problem. In this paper, we propose a new metric to measure the quality of a spanning tree preconditioner based on support theory. We use this metric to develop an algorithm to find good subgraph preconditioners and apply them to solve the SLAM problem. The results show that although the proposed algorithm is not fast enough, the new metric is effective and resulting subgraph preconditioners significantly improve the efficiency of the state-of-the-art solver.

IROS Conference 2012 Conference Paper

Accurate on-line 3D occupancy grids using Manhattan world constraints

• Brian Peasley
• Stan Birchfield
• Alexander Cunningham
• Frank Dellaert

In this paper we present an algorithm for constructing nearly drift-free 3D occupancy grids of large indoor environments in an online manner. Our approach combines data from an odometry sensor with output from a visual registration algorithm, and it enforces a Manhattan world constraint by utilizing factor graphs to produce an accurate online estimate of the trajectory of a mobile robotic platform. We also examine the advantages and limitations of the octree data structure representation of a 3D environment. Through several experiments in environments with varying sizes and construction we show that our method reduces rotational and translational drift significantly without performing any loop closing techniques.

ICRA Conference 2012 Conference Paper

Fully distributed scalable smoothing and mapping with robust multi-robot data association

• Alexander Cunningham
• Kai M. Wurm
• Wolfram Burgard
• Frank Dellaert

In this paper we focus on the multi-robot perception problem, and present an experimentally validated end-to-end multi-robot mapping framework, enabling individual robots in a team to see beyond their individual sensor horizons. The inference part of our system is the DDF-SAM algorithm [1], which provides a decentralized communication and inference scheme, but did not address the crucial issue of data association. One key contribution is a novel, RANSAC-based, approach for performing the between-robot data associations and initialization of relative frames of reference. We demonstrate this system with both data collected from real robot experiments, as well as in a large scale simulated experiment demonstrating the scalability of the proposed approach.

IROS Conference 2011 Conference Paper

Collaborative stereo

• Markus W. Achtelik
• Stephan Weiss 0002
• Margarita Chli
• Frank Dellaert
• Roland Siegwart

In this paper, we propose a method to recover the relative pose of two robots in absolute scale and in real-time using one monocular camera on each robot. We achieve this by fusing measurements from the onboard inertial sensors on each platform with information obtained from feature correspondences between the two cameras using an Extended Kalman Filter (EKF). This forms a flexible stereo rig, providing the ability to treat the two robots as one single dynamic sensor, which can adapt to the environment and thus improve environmental mapping, obstacle avoidance and navigation. We demonstrate the power of this approach on both simulation and real datasets, employing two micro aerial vehicles (MAVs) to illustrate successful operation over general 3D motion.

ICRA Conference 2011 Conference Paper

iSAM2: Incremental smoothing and mapping with fluid relinearization and incremental variable reordering

• Michael Kaess
• Hordur Johannsson
• Richard Roberts 0001
• Viorela Ila
• John J. Leonard
• Frank Dellaert

We present iSAM2, a fully incremental, graph-based version of incremental smoothing and mapping (iSAM). iSAM2 is based on a novel graphical model-based interpretation of incremental sparse matrix factorization methods, afforded by the recently introduced Bayes tree data structure. The original iSAM algorithm incrementally maintains the square root information matrix by applying matrix factorization updates. We analyze the matrix updates as simple editing operations on the Bayes tree and the conditional densities represented by its cliques. Based on that insight, we present a new method to incrementally change the variable ordering which has a large effect on efficiency. The efficiency and accuracy of the new method is based on fluid relinearization, the concept of selectively relinearizing variables as needed. This allows us to obtain a fully incremental algorithm without any need for periodic batch steps. We analyze the properties of the resulting algorithm in detail, and show on various real and simulated datasets that the iSAM2 algorithm compares favorably with other recent mapping algorithms in both quality and efficiency.

ICRA Conference 2011 Conference Paper

Visibility learning in large-scale urban environment

• Pablo F. Alcantarilla
• Kai Ni 0001
• Luis Miguel Bergasa
• Frank Dellaert

A crucial step in many vision based applications, such as localization and structure from motion, is the data association between a large map of known 3D points and 2D features perceived by a new camera. In this paper, we propose a novel approach to predict the visibility of known 3D points with respect to a query camera in large-scale environments. In our approach, we model the visibility of each 3D point with respect to a camera pose using a memory-based learning algorithm, in which a distance metric between cameras is learned in an entirely non-parametric way. We show that by fully exploiting the geometric relationships between the 3D map and the camera poses, as well as the related appearance information, the resulting prediction is much more robust and efficient than conventional approaches. We demonstrate the performance of our algorithm on a large urban 3D model in terms of both speed and accuracy.

IROS Conference 2010 Conference Paper

3D reconstruction of underwater structures

• Chris Beall
• Brian Lawrence
• Viorela Ila
• Frank Dellaert

Environmental change is a growing international concern, calling for the regular monitoring, studying and preserving of detailed information about the evolution of underwater ecosystems. For example, fragile coral reefs are exposed to various sources of hazards and potential destruction, and need close observation. Computer vision offers promising technologies to build 3D models of an environment from two-dimensional images. The state of the art techniques have enabled high-quality digital reconstruction of large-scale structures, e. g. , buildings and urban environments, but only sparse representations or dense reconstruction of small objects have been obtained from underwater video and still imagery. The application of standard 3D reconstruction methods to challenging underwater environments typically produces unsatisfactory results. Accurate, full camera trajectories are needed to serve as the basis for dense 3D reconstruction. A highly accurate sparse 3D reconstruction is the ideal foundation on which to base subsequent dense reconstruction algorithms. In our application the models are constructed from synchronized high definition videos collected using a wide baseline stereo rig. The rig can be hand-held, attached to a boat, or even to an autonomous underwater vehicle. We solve this problem by employing a smoothing and mapping toolkit developed in our lab specifically for this type of application. The result of our technique is a highly accurate sparse 3D reconstruction of underwater structures such as corals.

IROS Conference 2010 Conference Paper

DDF-SAM: Fully distributed SLAM using Constrained Factor Graphs

• Alexander Cunningham
• Balamanohar Paluri
• Frank Dellaert

We address the problem of multi-robot distributed SLAM with an extended Smoothing and Mapping (SAM) approach to implement Decentralized Data Fusion (DDF). We present DDF-SAM, a novel method for efficiently and robustly distributing map information across a team of robots, to achieve scalability in computational cost and in communication bandwidth and robustness to node failure and to changes in network topology. DDF-SAM consists of three modules: (1) a local optimization module to execute single-robot SAM and condense the local graph; (2) a communication module to collect and propagate condensed local graphs to other robots, and (3) a neighborhood graph optimizer module to combine local graphs into maps describing the neighborhood of a robot. We demonstrate scalability and robustness through a simulated example, in which inference is consistently faster than a comparable naive approach.

ICRA Conference 2010 Conference Paper

Learning visibility of landmarks for vision-based localization

• Pablo F. Alcantarilla
• Sang Min Oh
• Gian Luca Mariottini
• Luis Miguel Bergasa
• Frank Dellaert

We aim to perform robust and fast vision-based localization using a pre-existing large map of the scene. A key step in localization is associating the features extracted from the image with the map elements at the current location. Although the problem of data association has greatly benefited from recent advances in appearance-based matching methods, less attention has been paid to the effective use of the geometric relations between the 3D map and the camera in the matching process. In this paper we propose to exploit the geometric relationship between the 3D map and the camera pose to determine the visibility of the features. In our approach, we model the visibility of every map feature with respect to the camera pose using a non-parametric distribution model. We learn these non-parametric distributions during the 3D reconstruction process, and develop efficient algorithms to predict the visibility of features during localization. With this approach, the matching process only uses those map features with the highest visibility score, yielding a much faster algorithm and superior localization results. We demonstrate an integrated system based on the proposed idea and highlight its potential benefits for the localization in large and cluttered environments

IROS Conference 2010 Conference Paper

Multi-level submap based SLAM using nested dissection

• Kai Ni 0001
• Frank Dellaert

We propose a novel batch algorithm for SLAM problems that distributes the workload in a hierarchical way. We show that the original SLAM graph can be recursively partitioned into multiple-level submaps using the nested dissection algorithm, which leads to the cluster tree, a powerful graph representation. By employing the nested dissection algorithm, our algorithm greatly minimizes the dependencies between two subtrees, and the optimization of the original SLAM graph can be done using a bottom-up inference along the corresponding cluster tree. To speed up the computation, we also introduce a base node for each submap and use it to represent the rigid transformation of the submap in the global coordinate frame. As a result, the optimization moves the base nodes rather than the actual submap variables. We demonstrate that our algorithm is not only exact but also much faster than alternative approaches in both simulations and real-world experiments.

IROS Conference 2010 Conference Paper

Subgraph-preconditioned conjugate gradients for large scale SLAM

• Frank Dellaert
• Justin Carlson
• Viorela Ila
• Kai Ni 0001
• Charles E. Thorpe

In this paper we propose an efficient preconditioned conjugate gradients (PCG) approach to solving large-scale SLAM problems. While direct methods, popular in the literature, exhibit quadratic convergence and can be quite efficient for sparse problems, they typically require a lot of storage and efficient elimination orderings to be found. In contrast, iterative optimization methods only require access to the gradient and have a small memory footprint, but can suffer from poor convergence. Our new method, subgraph preconditioning, is obtained by re-interpreting the method of conjugate gradients in terms of the graphical model representation of the SLAM problem. The main idea is to combine the advantages of direct and iterative methods, by identifying a sub-problem that can be easily solved using direct methods, and solving for the remaining part using PCG. The easy sub-problems correspond to a spanning tree, a planar subgraph, or any other substructure that can be efficiently solved. As such, our approach provides new insights into the performance of state of the art iterative SLAM methods based on re-parameterized stochastic gradient descent. The efficiency of our new algorithm is illustrated on large datasets, both simulated and real.

ICRA Conference 2010 Conference Paper

Visual odometry priors for robust EKF-SLAM

• Pablo F. Alcantarilla
• Luis Miguel Bergasa
• Frank Dellaert

One of the main drawbacks of standard visual EKF-SLAM techniques is the assumption of a general camera motion model. Usually this motion model has been implemented in the literature as a constant linear and angular velocity model. Because of this, most approaches cannot deal with sudden camera movements, causing them to lose accurate camera pose and leading to a corrupted 3D scene map. In this work we propose increasing the robustness of EKF-SLAM techniques by replacing this general motion model with a visual odometry prior, which provides a real-time relative pose prior by tracking many hundreds of features from frame to frame. We perform fast pose estimation using the two-stage RANSAC-based approach from [1]: a two-point algorithm for rotation followed by a one-point algorithm for translation. Then we integrate the estimated relative pose into the prediction step of the EKF. In the measurement update step, we only incorporate a much smaller number of landmarks into the 3D map to maintain real-time operation. Incorporating the visual odometry prior in the EKF process yields better and more robust localization and mapping results when compared to the constant linear and angular velocity model case. Our experimental results, using a handheld stereo camera as the only sensor, clearly show the benets of our method against the standard constant velocity model.

ICRA Conference 2009 Conference Paper

Bayesian surprise and landmark detection

• Ananth Ranganathan
• Frank Dellaert

Automatic detection of landmarks, usually special places in the environment such as gateways, for topological mapping has proven to be a difficult task. We present the use of Bayesian surprise, introduced in computer vision, for landmark detection. Further, we provide a novel hierarchical, graphical model for the appearance of a place and use this model to perform surprise-based landmark detection. Our scheme is agnostic to the sensor type, and we demonstrate this by implementing a simple laser model for computing surprise. We evaluate our landmark detector using appearance and laser measurements in the context of a topological mapping algorithm, thus demonstrating the practical applicability of the detector.

ICRA Conference 2009 Conference Paper

Flow separation for fast and robust stereo odometry

• Michael Kaess
• Kai Ni 0001
• Frank Dellaert

Separating sparse flow provides fast and robust stereo visual odometry that deals with nearly degenerate situations that often arise in practical applications. We make use of the fact that in outdoor situations different constraints are provided by close and far structure, where the notion of close depends on the vehicle speed. The motion of distant features determines the rotational component that we recover with a robust two-point algorithm. Once the rotation is known, we recover the translational component from close features using a robust one-point algorithm. The overall algorithm is faster than estimating the motion in one step by a standard RANSAC-based three-point algorithm. And in contrast to other visual odometry work, we avoid the problem of nearly degenerate data, under which RANSAC is known to return inconsistent results. We confirm our claims on data from an outdoor robot equipped with a stereo rig.

ICRA Conference 2008 Conference Paper

Place recognition-based fixed-lag smoothing for environments with unreliable GPS

• Roozbeh Mottaghi
• Michael Kaess
• Ananth Ranganathan
• Richard Roberts 0001
• Frank Dellaert

Pose estimation of outdoor robots presents some distinct challenges due to the various uncertainties in the robot sensing and action. In particular, global positioning sensors of outdoor robots do not always work perfectly, causing large drift in the location estimate of the robot. To overcome this common problem, we propose a new approach for global localization using place recognition. First, we learn the location of some arbitrary key places using odometry measurements and GPS measurements only at the start and the end of the robot trajectory. In subsequent runs, when the robot perceives a key place, our fixed-lag smoother fuses odometry measurements with the relative location to the key place to improve its pose estimate. Outdoor mobile robot experiments show that place recognition measurements significantly improve the estimate of the smoother in the absence of GPS measurements.

IJCAI Conference 2007 Conference Paper

• Michael Kaess
• Ananth Ranganathan
• Frank Dellaert

We propose a novel approach to the problem of simultaneous localization and mapping (SLAM) based on incremental smoothing, that is suitable for real-time applications in large-scale environments. The main advantages over filter-based algorithms are that we solve the full SLAM problem without the need for any approximations, and that we do not suffer from linearization errors. We achieve efficiency by updating the square-root information matrix, a factored version of the naturally sparse smoothing information matrix. We can efficiently recover the exact trajectory and map at any given time by back-substitution. Furthermore, our approach allows access to the exact covariances, as it does not suffer from under-estimation of uncertainties, which is another problem inherent to filters. We present simulation-based results for the linear case, showing constant time updates for exploration tasks. We further evaluate the behavior in the presence of loops, and discuss how our approach extends to the non-linear case. Finally, we evaluate the overall non-linear algorithm on the standard Victoria Park data set.

IJCAI Conference 2007 Conference Paper

• Ananth Ranganathan
• Michael Kaess
• Frank Dellaert

Smoothing approaches to the Simultaneous Localization and Mapping (SLAM) problem in robotics are superior to the more common filtering approaches in being exact, better equipped to deal with non-linearities, and computing the entire robot trajectory. However, while filtering algorithms that perform map updates in constant time exist, no analogous smoothing method is available. We aim to rectify this situation by presenting a smoothing-based solution to SLAM using Loopy Belief Propagation (LBP) that can perform the trajectory and map updates in constant time except when a loop is closed in the environment. The SLAM problem is represented as a Gaussian Markov Random Field (GMRF) over which LBP is performed. We prove that LBP, in this case, is equivalent to Gauss-Seidel relaxation of a linear system. The inability to compute marginal covariances efficiently in a smoothing algorithm has previously been a stumbling block to their widespread use. LBP enables the efficient recovery of the marginal covariances, albeit approximately, of landmarks and poses. While the final covariances are overconfident, the ones obtained from a spanning tree of the GMRF are conservative, making them useful for data association. Experiments in simulation and using real data are presented.

IROS Conference 2007 Conference Paper

Fast 3D pose estimation with out-of-sequence measurements

• Ananth Ranganathan
• Michael Kaess
• Frank Dellaert

We present an algorithm for pose estimation using fixed-lag smoothing. We show that fixed-lag smoothing enables inclusion of measurements from multiple asynchronous measurement sources in an optimal manner. Since robots usually have a plurality of uncoordinated sensors, our algorithm has an advantage over filtering-based estimation algorithms, which cannot incorporate delayed measurements optimally. We provide an implementation of the general fixed-lag smoothing algorithm using square root smoothing, a technique that has become prominent. Square root smoothing uses fast sparse matrix factorization and enables our fixed-lag pose estimation algorithm to run at upwards of 20 Hz. Our algorithm has been extensively tested over hundreds of hours of operation on a robot operating in outdoor environments. We present results based on these tests that verify our claims using wheel encoders, visual odometry, and GPS as sensors.

ICRA Conference 2007 Conference Paper

iSAM: Fast Incremental Smoothing and Mapping with Efficient Data Association

• Michael Kaess
• Ananth Ranganathan
• Frank Dellaert

We introduce incremental smoothing and mapping (iSAM), a novel approach to the problem of simultaneous localization and mapping (SLAM) that addresses the data association problem and allows real-time application in large-scale environments. We employ smoothing to obtain the complete trajectory and map without the need for any approximations, exploiting the natural sparsity of the smoothing information matrix. A QR-factorization of this information matrix is at the heart of our approach. It provides efficient access to the exact covariances as well as to conservative estimates that are used for online data association. It also allows recovery of the exact trajectory and map at any given time by back-substitution. Instead of refactoring in each step, we update the QR-factorization whenever a new measurement arrives. We analyze the effect of loops, and show how our approach extends to the non-linear case. Finally, we provide experimental validation of the overall non-linear algorithm based on the standard Victoria Park data set with unknown correspondences.

ICRA Conference 2007 Conference Paper

Tectonic SAM: Exact, Out-of-Core, Submap-Based SLAM

• Kai Ni 0001
• Drew Steedly
• Frank Dellaert

Simultaneous localization and mapping (SLAM) is a method that robots use to explore, navigate, and map an unknown environment. However, this method poses inherent problems with regard to cost and time. To lower computation costs, smoothing and mapping (SAM) approaches have shown some promise, and they also provide more accurate solutions than filtering approaches in realistic scenarios. However, in SAM approaches, updating the linearization is still the most time-consuming step. To mitigate this problem, we propose a submap-based approach, tectonic SAM, in which the original optimization problem is solved by using a divide-and-conquer scheme. Submaps are optimized independently and parameterized relative to a local coordinate frame. During the optimization, the global position of the submap may change dramatically, but the positions of the nodes in the submap relative to the local coordinate frame do not change very much. The key contribution of this paper is to show that the linearization of the submaps can be cached and reused when they are combined into a global map. According to the results of both simulation and real experiments, Tectonic SAM drastically speeds up SAM in very large environments while still maintaining its global accuracy.

ICRA Conference 2006 Conference Paper

A Rao-Blackwellized Particle Filter for Topological Mapping

• Ananth Ranganathan
• Frank Dellaert

We present a particle filtering algorithm to construct topological maps of an uninstrument environment. The algorithm presented here constructs the posterior on the space of all possible topologies given measurements, and is based on our previous work on a Bayesian inference framework for topological maps (A. Ranganathan and F. Dellaert, 2004). Constructing the posterior solves the perceptual aliasing problem in a general, robust manner. The use of a Rao-Blackwellized particle filter (RBPF) for this purpose makes the inference in the space of topologies incremental and run in real-time. The RBPF maintains the joint posterior on topological maps and locations of landmarks. We demonstrate that, using the landmark locations thus obtained, the global metric map can be obtained from the topological map generated by our algorithm through a simple post-processing step. A data-driven proposal is provided to overcome the degeneracy problem inherent in particle filters. The use of a Dirichlet process prior on landmark labels is also a novel aspect of this work. We use laser range scan and odometry measurements to present experimental results on a robot

ICRA Conference 2005 Conference Paper

A Markov Chain Monte Carlo Approach to Closing the Loop in SLAM

• Michael Kaess
• Frank Dellaert

The problem of simultaneous localization and mapping has received much attention over the last years. Especially large scale environments, where the robot trajectory loops back on itself, are a challenge. In this paper we introduce a new solution to this problem of closing the loop. Our algorithm is EM-based, but differs from previous work. The key is a probability distribution over partitions of feature tracks that is determined in the E-step, based on the current estimate of the motion. This virtual structure is then used in the M-step to obtain a better estimate for the motion. We demonstrate the success of our algorithm in experiments on real laser data.

AAAI Conference 2005 Conference Paper

A Multifrontal QR Factorization Approach to Distributed Inference Applied to Multirobot Localization and Mapping

• Frank Dellaert

QR factorization is most often used as a “black box” algorithm, but is in fact an elegant computation on a factor graph. By computing a rooted clique tree on this graph, the computation can be parallelized across subtrees, which forms the basis of so-called multifrontal QR methods. By judiciously choosing the order in which variables are eliminated in the clique tree computation, we show that one straightforwardly obtains a method for performing inference in distributed sensor networks. One obvious application is distributed localization and mapping with a team of robots. We phrase the problem as inference on a large-scale Gaussian Markov Random Field induced by the measurement factor graph, and show how multifrontal QR on this graph solves for the global map and all the robot poses in a distributed fashion. The method is illustrated using both small and large-scale simulations, and validated in practice through actual robot experiments.

ICRA Conference 2005 Conference Paper

Using Hierarchical EM to Extract Planes from 3D Range Scans

• Rudolph Triebel
• Wolfram Burgard
• Frank Dellaert

Recently, the acquisition of three-dimensional maps has become more and more popular. This is motivated by the fact that robots act in the three-dimensional world and several tasks such as path planning or localizing objects can be carried out more reliable using three-dimensional representations. In this paper we consider the problem of extracting planes from three-dimensional range data. In contrast to previous approaches our algorithm uses a hierarchical variant of the popular Expectation Maximization (EM) algorithm [1] to simultaneously learn the main directions of the planar structures. These main directions are then used to correct the position and orientation of planes. In practical experiments carried out with real data and in simulations we demonstrate that our algorithm can accurately extract planes and their orientation from range data.

ICRA Conference 2005 Conference Paper

What Are the Ants Doing? Vision-Based Tracking and Reconstruction of Control Programs

• Magnus Egerstedt
• Tucker R. Balch
• Frank Dellaert
• Florent Delmotte
• Zia Khan

In this paper, we study the problem of going from a real-world, multi-agent system to the generation of control programs in an automatic fashion. In particular, a computer vision system is presented, capable of simultaneously tracking multiple agents, such as social insects. Moreover, the data obtained from this system is fed into a mode-reconstruction module that generates low-complexity control programs, i. e. strings of symbolic descriptions of control-interrupt pairs, consistent with the empirical data. The result is a mechanism for going from the real system to an executable implementation that can be used for controlling multiple mobile robots.

IROS Conference 2004 Conference Paper

Inference in the space of topological maps: an MCMC-based approach

• Ananth Ranganathan
• Frank Dellaert

While probabilistic techniques have been considered extensively in the context of metric maps, no general purpose probabilistic methods exist for topological maps. We present the concept of probabilistic topological maps (PTMs), a sample-based representation that approximates the posterior distribution over topologies given the available sensor measurements. The PTM is obtained through the use of MCMC-based Bayesian inference over the space of all possible topologies. It is shown that the space of all topologies is equivalent to the space of set partitions of all available measurements. While the space of possible topologies is intractably large, our use of Markov chain Monte Carlo sampling to infer the approximate histograms overcomes the combinatorial nature of this space and provides a general solution to the correspondence problem in the context of topological mapping. We present experimental results that validate our technique and generate good maps even when using only odometry as the sensor measurements.

IROS Conference 2004 Conference Paper

Map-based priors for localization

• Sang Min Oh
• Sarah Tariq
• Bruce N. Walker
• Frank Dellaert

Localization from sensor measurements is a fundamental task for navigation. Particle filters are among the most promising candidates to provide a robust and real-time solution to the localization problem. They instantiate the localization problem as a Bayesian altering problem and approximate the posterior density over location by a weighted sample set. In this paper, we introduce map-based priors for localization, using the semantic information available in maps to bias the motion model toward areas of higher probability. We, show that such priors, under a particular assumption, can easily be incorporated in the particle filter by means of a pseudo likelihood. The resulting filter is more reliable and more accurate. We show experimental results on a GPS based outdoor people tracker that illustrate the approach and highlight its potential.

IROS Conference 2003 Conference Paper

Efficient particle filter-based tracking of multiple interacting targets using an MRF-based motion model

• Zia Khan
• Tucker R. Balch
• Frank Dellaert

We describe a multiple hypothesis particle filter for tracking targets that are influenced by the proximity and/or behavior of other targets. Our contribution is to show how a Markov random field motion prior, built on the fly at each time step, can model these interactions to enable more accurate tracking. We present results for a social insect tracking application, where we model the domain knowledge that two targets cannot occupy the same space, and targets actively avoid collisions. We show that using this model improves track quality and efficiency. Unfortunately, the joint particle tracker we propose suffers from exponential complexity in the number of tracked targets. An approximation to the joint filter, however, consisting of multiple nearly independent particle filters can provide similar track quality at substantially lower computational cost.

ICRA Conference 2003 Conference Paper

Inkrinsic localization and mapping with 2 applications: diffusion mapping and marco polo localization

• Frank Dellaert
• Fernando Alegre
• Eric Beowulf Martinson

We investigate intrinsic localization and mapping (ILM) for teams of mobile robots, a multi-robot variant of SLAM where the robots themselves are used as landmarks. We develop what is essentially a straightforward application of Bayesian estimation to the problem, and present two complimentary views on the associated optimization problem that provide insight into the problem and allows one to devise initialization strategies, indispensable in practice. We also provide a discussion of the degrees of freedom and ambiguities in the solution. Finally, we introduce two applications of ILM that bring out its potential: Diffusion Mapping and Marco Polo localization.

ICRA Conference 2003 Conference Paper

Marco polo localization

• Eric Beowulf Martinson
• Frank Dellaert

We introduce the Marco Polo localization approach, where we apply sound as a tool for gathering range measurements between robots, and use those to solve a range-only simultaneous localization and mapping problem. Range is calculated by correlating two recordings of the same sound, recorded on a pair of robots, after which the resulting time delay estimate is converted to a range measurement. The algorithmic approach we use is a straightforward application of the Bayesian estimation framework. We also present two complementary views on the associated optimization problem that provide insight into the problem and allows one to devise initialization strategies, indispensable in a range-only scenario. We illustrate the approach with both simulated and experimental results.

ICRA Conference 2002 Conference Paper

Linear 2D Localization and Mapping for Single and Multiple Robot Scenarios

• Frank Dellaert
• Ashley W. Stroupe

We show how to recover 2D structure and motion linearly in order to initialize simultaneous mapping and localization (SLAM) for bearings-only measurements and planar motion. The method supplies a good initial estimate of the geometry, even without odometry or in multiple robot scenarios. Hence, it substantially enlarges the scope in which non-linear batch-type SLAM algorithms can be applied. The method is applicable when at least seven landmarks are seen from three different vantage points, whether by one robot that moves over time or by multiple robots that observe a set of common landmarks.

AIJ Journal 2001 Journal Article

Robust Monte Carlo localization for mobile robots

• Sebastian Thrun
• Dieter Fox
• Wolfram Burgard
• Frank Dellaert

Mobile robot localization is the problem of determining a robot's pose from sensor data. This article presents a family of probabilistic localization algorithms known as Monte Carlo Localization (MCL). MCL algorithms represent a robot's belief by a set of weighted hypotheses (samples), which approximate the posterior under a common Bayesian formulation of the localization problem. Building on the basic MCL algorithm, this article develops a more robust algorithm called Mixture-MCL, which integrates two complimentary ways of generating samples in the estimation. To apply this algorithm to mobile robots equipped with range finders, a kernel density tree is learned that permits fast sampling. Systematic empirical results illustrate the robustness and computational efficiency of the approach.

NeurIPS Conference 2000 Conference Paper

Feature Correspondence: A Markov Chain Monte Carlo Approach

• Frank Dellaert
• Steven Seitz
• Sebastian Thrun
• Charles Thorpe

When trying to recover 3D structure from a set of images, the most difficult problem is establishing the correspondence between the measurements. Most existing approaches assume that features can be tracked across frames, whereas methods that exploit rigidity constraints to facilitate matching do so only under restricted cam(cid: 173) era motion. In this paper we propose a Bayesian approach that avoids the brittleness associated with singling out one "best" cor(cid: 173) respondence, and instead consider the distribution over all possible correspondences. We treat both a fully Bayesian approach that yields a posterior distribution, and a MAP approach that makes use of EM to maximize this posterior. We show how Markov chain Monte Carlo methods can be used to implement these techniques in practice, and present experimental results on real data.

ICRA Conference 1999 Conference Paper

MINERVA: A Second-Generation Museum Tour-Guide Robot

• Sebastian Thrun
• Maren Bennewitz
• Wolfram Burgard
• Armin B. Cremers
• Frank Dellaert
• Dieter Fox
• Dirk Hähnel
• Charles R. Rosenberg

This paper describes an interactive tour-guide robot, which was successfully exhibited in a Smithsonian museum. During its two weeks of operation, the robot interacted with thousands of people, traversing more than 44 km at speeds of up to 163 cm/sec. Our approach specifically addresses issues such as safe navigation in unmodified and dynamic environments, and short-term human-robot interaction. It uses learning pervasively at all levels of the software architecture.

ICRA Conference 1999 Conference Paper

Monte Carlo Localization for Mobile Robots

• Frank Dellaert
• Dieter Fox
• Wolfram Burgard
• Sebastian Thrun

To navigate reliably in indoor environments, a mobile robot must know where it is. Thus, reliable position estimation is a key problem in mobile robotics. We believe that probabilistic approaches are among the most promising candidates to providing a comprehensive and real-time solution to the robot localization problem. However, current methods still face considerable hurdles. In particular the problems encountered are closely related to the type of representation used to represent probability densities over the robot's state space. Earlier work on Bayesian filtering with particle-based density representations opened up a new approach for mobile robot localization based on these principles. We introduce the Monte Carlo localization method, where we represent the probability density involved by maintaining a set of samples that are randomly drawn from it. By using a sampling-based representation we obtain a localization method that can represent arbitrary distributions. We show experimentally that the resulting method is able to efficiently localize a mobile robot without knowledge of its starting location. It is faster, more accurate and less memory-intensive than earlier grid-based methods, .

ICRA Conference 1998 Conference Paper

Model-Based Car Tracking Integrated with a Road-Follower

• Frank Dellaert
• Dean Pomerleau
• Charles E. Thorpe

This paper discusses how we integrated our 3D car tracking approach with the lane following module RALPH on the Navlab autonomous vehicles, obtaining a hybrid vision system that tracks both the road and cars better than those two systems in isolation. The tracking system brings precise and crisp measurements of the car in the image, and performs image stabilization. However, because it does not know, about the yaw or lateral offset of the ego-vehicle, its curvature estimate can be misguided. RALPH takes a more global image processing approach and can provide this missing information, as well as a good estimate of curvature, so that the combined curvature estimate is superior to both taken in isolation. The additional information provided by RALPH also improves tracking performance, and allows us to estimate properties of the tracked car that were previously unobservable, in particular its in-lane displacement. Better car tracking, and a better idea of where the road is, gives us a substantial foundation on which to base other capabilities needed to realize fully autonomous vehicles.

IROS Conference 1998 Conference Paper

Super-resolved texture tracking of planar surface patches

• Frank Dellaert
• Charles E. Thorpe
• Sebastian Thrun

We present an approach to tracking planar surface patches over time. In addition to tracking a patch with full six degrees of freedom, the algorithm also produces a super-resolved estimate of the texture present on the patch. This texture estimate is kept as an explicit model texture image which is refined over time. We then use it to infer the 3D motion of the patch from the image sequence. The main idea behind the approach is to use a technique from computer graphics, known as texture mapping, as the measurement model in an extended Kalman filter. We also calculate the partial derivative of this image formation process with respect to the 3D pose of the patch, which functions as the measurement Jacobian. The super-resolved estimate of the texture is obtained using the standard extended Kalman filter measurement update, with one essential approximation that makes this computationally feasible. The resulting equations are remarkably simple, yet lead to estimates that are properly super-resolved. In addition to developing the theory behind the approach, we also demonstrate both the tracking and the super-resolution aspect of the algorithm on real image sequences.