Michael Kaess

YNICL Journal 2026 Journal Article

Grey matter volume associations with personality functioning in a clinical cohort of female youths

• Madelyn Thomson
• Marialuisa Cavelti
• Ines Mürner-Lavanchy
• Silvano Sele
• Niklas Bürgi
• Nora Seiffert
• Franz Moggi
• Roland Wiest

Borderline Personality Disorder (BPD) is a severe mental illness, although its neurobiological underpinnings remain largely unknown. Structural Magnetic Resonance Imaging (sMRI) in adolescents can offer insights into potential biomarkers to help advance early detection and targeted intervention. However, previous findings have been mixed, possibly due to clinical heterogeneity that may be better captured using a dimensional approach to personality functioning (PF). The current study explored grey matter volume (GMV) in youth with varying degrees of BPD pathology, and associations with dimensional PF. N = 93 females (14-21 years) comprising three groups (full-threshold BPD, sub-threshold BPD, and healthy controls) underwent sMRI and were assessed with the Semi-Structured Interview for Personality Functioning DSM-5 (STiP-5.1). Groups were combined to reflect dimensional personality pathology. Multiple linear regression analyses were conducted to determine associations between the STiP-5.1 total score, and each of its four elements with: (i) total GMV, (ii) GMV in individual brain regions defined by the Desikan-Killiany-Tourville atlas, (iii) selected regions of interest (ROIs). All analyses were statistically non-significant: STiP-5.1 total and total GMV (p = 0.61); STiP-5.1 total and individual brain regions (all corrected p values ≥0.82); STiP-5.1 total and ROIs (all corrected p values ≥0.91). Results were non-significant for each element, and a validity check using BPD criteria confirmed STiP-5.1 findings. We found no evidence of an association of dimensionally assessed PF with GMV in young females. The pursuit of clinical research efforts on other potential biomarkers using dimensional conceptualisations of PF may represent worthy endeavours.

IROS Conference 2025 Conference Paper

A Comprehensive Evaluation of LiDAR Odometry Techniques

• Easton R. Potokar
• Michael Kaess

Light Detection and Ranging (LiDAR) sensors have become the sensor of choice for many robotic state estimation tasks. Because of this, in recent years there has been significant work done to find the most accurate method to perform state estimation using these sensors. In each of these prior works, an explosion of possible technique combinations has occurred, with each work comparing LiDAR Odometry (LO) "pipelines" to prior "pipelines". Unfortunately, little work up to this point has performed the significant amount of ablation studies comparing the various building-blocks of a LO pipeline. In this work, we summarize the various techniques that go into defining a LO pipeline and empirically evaluate these LO components on an expansive number of datasets across environments, LiDAR types, and vehicle motions. Finally, we make empirically-backed recommendations for the design of future LO pipelines to provide the most accurate and reliable performance.

IROS Conference 2025 Conference Paper

Acoustic Neural 3D Reconstruction Under Pose Drift

• Tianxiang Lin
• Mohamad Qadri
• Kevin Zhang 0003
• Adithya Pediredla
• Christopher A. Metzler
• Michael Kaess

We consider the problem of optimizing neural implicit surfaces for 3D reconstruction using acoustic images collected with drifting sensor poses. The accuracy of current state-of-the-art 3D acoustic modeling algorithms is highly dependent on accurate pose estimation; small errors in sensor pose can lead to severe reconstruction artifacts. In this paper, we propose an algorithm that jointly optimizes the neural scene representation and sonar poses. Our algorithm does so by parameterizing the 6DoF poses as learnable parameters and backpropagating gradients through the neural renderer and implicit representation. We validated our algorithm on both real and simulated datasets. It produces high-fidelity 3D reconstructions even under significant pose drift.

RLJ Journal 2025 Journal Article

Your Learned Constraint is Secretly a Backward Reachable Tube

• Mohamad Qadri
• Gokul Swamy
• Jonathan Francis
• Michael Kaess
• Andrea Bajcsy

Inverse Constraint Learning (ICL) is the problem of inferring constraints from safe (i.e., constraint-satisfying) demonstrations. The hope is that these inferred constraints can then be used downstream to search for safe policies for new tasks and, potentially, under different dynamics. Our paper explores the question of what mathematical entity ICL recovers. Somewhat surprisingly, we show that both in theory and in practice, ICL recovers the set of states where failure is inevitable, rather than the set of states where failure has already happened. In the language of safe control, this means we recover a backwards reachable tube (BRT) rather than a failure set. In contrast to the failure set, the BRT depends on the dynamics of the data collection system. We discuss the implications of the dynamics-conditionedness of the recovered constraint on both the sample-efficiency of policy search and the transferability of learned constraints.

RLC Conference 2025 Conference Paper

Your Learned Constraint is Secretly a Backward Reachable Tube

• Mohamad Qadri
• Gokul Swamy
• Jonathan Francis
• Michael Kaess
• Andrea Bajcsy

Inverse Constraint Learning (ICL) is the problem of inferring constraints from safe (i. e. , constraint-satisfying) demonstrations. The hope is that these inferred constraints can then be used downstream to search for safe policies for new tasks and, potentially, under different dynamics. Our paper explores the question of what mathematical entity ICL recovers. Somewhat surprisingly, we show that both in theory and in practice, ICL recovers the set of states where failure is inevitable, rather than the set of states where failure has already happened. In the language of safe control, this means we recover a backwards reachable tube (BRT) rather than a failure set. In contrast to the failure set, the BRT depends on the dynamics of the data collection system. We discuss the implications of the dynamics-conditionedness of the recovered constraint on both the sample-efficiency of policy search and the transferability of learned constraints.

IROS Conference 2024 Conference Paper

A Slices Perspective for Incremental Nonparametric Inference in High Dimensional State Spaces

• Moshe Shienman
• Ohad Levy-Or
• Michael Kaess
• Vadim Indelman

We introduce an innovative method for incremental nonparametric probabilistic inference in high-dimensional state spaces. Our approach leverages slices from highdimensional surfaces to efficiently approximate posterior distributions of any shape. Unlike many existing graph-based methods, our slices perspective eliminates the need for additional intermediate reconstructions, maintaining a more accurate representation of posterior distributions. Additionally, we propose a novel heuristic to balance between accuracy and efficiency, enabling real-time operation in nonparametric scenarios. In empirical evaluations on synthetic and real-world datasets, our slices approach consistently outperforms other state-of-the-art methods. It demonstrates superior accuracy and achieves a significant reduction in computational complexity, often by an order of magnitude.

ICRA Conference 2024 Conference Paper

Asynchronous Distributed Smoothing and Mapping via On-Manifold Consensus ADMM

• Daniel McGann
• Kyle Lassak
• Michael Kaess

In this paper we present a fully distributed, asynchronous, and general purpose optimization algorithm for Consensus Simultaneous Localization and Mapping (CSLAM). Multi-robot teams require that agents have timely and accurate solutions to their state as well as the states of the other robots in the team. To optimize this solution we develop a CSLAM back-end based on Consensus ADMM called MESA (Manifold, Edge-based, Separable ADMM). MESA is fully distributed to tolerate failures of individual robots, asynchronous to tolerate communication delays and outages, and general purpose to handle any CSLAM problem formulation. We demonstrate that MESA exhibits superior convergence rates and accuracy compare to existing state-of-the art CSLAM back-end optimizers.

IROS Conference 2024 Conference Paper

BEVLoc: Cross-View Localization and Matching via Birds-Eye-View Synthesis

• Christopher Klammer
• Michael Kaess

Ground to aerial matching is a crucial and challenging task in outdoor robotics, particularly when GPS is absent or unreliable. Structures like buildings or large dense forests create interference, requiring GNSS replacements for global positioning estimates. The true difficulty lies in reconciling the perspective difference between the ground and air images for acceptable localization. Taking inspiration from the autonomous driving community, we propose a novel framework for synthesizing a birds-eye-view (BEV) scene representation to match and localize against an aerial map in off-road environments. We leverage contrastive learning with domain specific hard negative mining to train a network to learn similar representations between the synthesized BEV and the aerial map. During inference, BEVLoc guides the identification of the most probable locations within the aerial map through a coarse-to-fine matching strategy. Our results demonstrate promising initial outcomes in extremely difficult forest environments with limited semantic diversity. We analyze our model’s performance for coarse and fine matching, assessing both the raw matching capability of our model and its performance as a GNSS replacement. Our work delves into off-road map localization while establishing a foundational baseline for future developments in localization. Our code is available at: https://github.com/rpl-cmu/bevloc

IROS Conference 2024 Conference Paper

BEVRender: Vision-based Cross-view Vehicle Registration in Off-road GNSS-denied Environment

• Lihong Jin
• Wei Dong
• Wenshan Wang
• Michael Kaess

We introduce BEVRender, a novel learning-based approach for the localization of ground vehicles in Global Navigation Satellite System (GNSS)-denied off-road scenarios. These environments are typically challenging for conventional vision-based state estimation due to the lack of distinct visual landmarks and the instability of vehicle poses. To address this, BEVRender generates high-quality local bird’s-eye-view (BEV) images of the local terrain. Subsequently, these images are aligned with a georeferenced aerial map through template matching to achieve accurate cross-view registration. Our approach overcomes the inherent limitations of visual inertial odometry systems and the substantial storage requirements of image-retrieval localization strategies, which are susceptible to drift and scalability issues, respectively. Extensive experimentation validates BEVRender’s advancement over existing GNSS-denied visual localization methods, demonstrating notable enhancements in both localization accuracy and update frequency.

ICRA Conference 2024 Conference Paper

Learning Covariances for Estimation with Constrained Bilevel Optimization

• Mohamad Qadri
• Zachary Manchester
• Michael Kaess

We consider the problem of learning error covariance matrices for robotic state estimation. The convergence of a state estimator to the correct belief over the robot state is dependent on the proper tuning of noise models. During inference, these models are used to weigh different blocks of the Jacobian and error vector resulting from linearization and hence, additionally affect the stability and convergence of the non-linear system. We propose a gradient-based method to estimate well-conditioned covariance matrices by formulating the learning process as a constrained bilevel optimization problem over factor graphs. We evaluate our method against baselines across a range of simulated and real-world tasks and demonstrate that our technique converges to model estimates that lead to better solutions as evidenced by the improved tracking accuracy on unseen test trajectories.

ICRA Conference 2024 Conference Paper

Multi-Radar Inertial Odometry for 3D State Estimation using mmWave Imaging Radar

• Jui-Te Huang
• Ruoyang Xu
• Akshay Hinduja
• Michael Kaess

State estimation is a crucial component for the successful implementation of robotic systems, relying on sensors such as cameras, LiDAR, and IMUs. However, in real-world scenarios, the performance of these sensors is degraded by challenging environments, e. g. adverse weather conditions and low-light scenarios. The emerging 4D imaging radar technology is capable of providing robust perception in adverse conditions. Despite its potential, challenges remain for indoor settings where noisy radar data does not present clear geometric features. Moreover, disparities in radar data resolution and field of view (FOV) can lead to inaccurate measurements. While prior research has explored radar-inertial odometry based on Doppler velocity information, challenges remain for the estimation of 3D motion because of the discrepancy in the FOV and resolution of the radar sensor. In this paper, we address Doppler velocity measurement uncertainties. We present a method to optimize body frame velocity while managing Doppler velocity uncertainty. Based on our observations, we propose a dual imaging radar configuration to mitigate the challenge of discrepancy in radar data. To attain high-precision 3D state estimation, we introduce a strategy that seamlessly integrates radar data with a consumer-grade IMU sensor using fixed-lag smoothing optimization. Finally, we evaluate our approach using real-world 3D motion data.

ICRA Conference 2024 Conference Paper

SONIC: Sonar Image Correspondence using Pose Supervised Learning for Imaging Sonars

• Samiran Gode
• Akshay Hinduja
• Michael Kaess

In this paper, we address the challenging problem of data association for underwater SLAM through a novel method for sonar image correspondence using learned features. We introduce SONIC (SONar Image Correspondence), a pose-supervised network designed to yield robust feature correspondence capable of withstanding viewpoint variations. The inherent complexity of the underwater environment stems from the dynamic and frequently limited visibility conditions, restricting vision to a few meters of often featureless expanses. This makes camera-based systems suboptimal in most open water application scenarios. Consequently, multibeam imaging sonars emerge as the preferred choice for perception sensors. However, they too are not without their limitations. While imaging sonars offer superior long-range visibility compared to cameras, their measurements can appear different from varying viewpoints. This inherent variability presents formidable challenges in data association, particularly for feature-based methods. Our method demonstrates significantly better performance in generating correspondences for sonar images which will pave the way for more accurate loop closure constraints and sonar-based place recognition. Code as well as simulated and real-world datasets are made public on https://github.com/rpl-cmu/sonic to facilitate further development in the field.

ICRA Conference 2023 Conference Paper

Conditional GANs for Sonar Image Filtering with Applications to Underwater Occupancy Mapping

• Tianxiang Lin
• Akshay Hinduja
• Mohamad Qadri
• Michael Kaess

Underwater robots typically rely on acoustic sensors like sonar to perceive their surroundings. However, these sensors are often inundated with multiple sources and types of noise, which makes using raw data for any meaningful inference with features, objects, or boundary returns very difficult. While several conventional methods of dealing with noise exist, their success rates are unsatisfactory. This paper presents a novel application of conditional Generative Adversarial Networks (cGANs) to train a model to produce noise-free sonar images, outperforming several conventional filtering methods. Estimating free space is crucial for autonomous robots performing active exploration and mapping. Thus, we apply our approach to the task of underwater occupancy mapping and show superior free and occupied space inference when compared to conventional methods.

ICRA Conference 2023 Conference Paper

Efficient Bundle Adjustment for Coplanar Points and Lines

• Lipu Zhou
• Jiacheng Liu 0008
• Fengguang Zhai
• Pan Ai
• Kefei Ren
• Yinian Mao
• Guoquan Huang 0003
• Ziyang Meng 0001

Bundle adjustment (BA) is a well-studied fundamental problem in the robotics and vision community. In man-made environments, coplanar points and lines are ubiquitous. However, the number of works on bundle adjustment with coplanar points and lines is relatively small. This paper focuses on this special BA problem, referred to as $\pi-\mathbf{BA}$. For a point or a line on a plane, we derive a new constraint to describe the relationship among two poses and the plane, called $\pi$ -constraint. We distribute $\pi$ -constraints into different groups. Each group is called a $\pi$ -factor. We prove that, with some simple preprocessing, the computational complexity associated with a $\pi$ -factor in the Levenberg-Marquardt (LM) algorithm is $O(1)$, independent of the number of $\pi$ -constraints packed into the $\pi$ -factor. In $\pi-\mathbf{BA}, \pi$ -factors replace original reprojection errors. One problem is how to divide $\pi$ -constraints into $\pi$ -factors. Different strategies may result in different numbers of $\pi$ -factors, which in turn affects the efficiency. It is difficult to get the optimal division. We present a greedy algorithm to overcome this problem. Experimental results verify that our algorithm can significantly accelerate the computation.

ICRA Conference 2023 Conference Paper

Neural Implicit Surface Reconstruction using Imaging Sonar

• Mohamad Qadri
• Michael Kaess
• Ioannis Gkioulekas

We present a technique for dense 3D reconstruction of objects using an imaging sonar, also known as forward-looking sonar (FLS). Compared to previous methods that model the scene geometry as point clouds or volumetric grids, we represent the geometry as a neural implicit function. Additionally, given such a representation, we use a differentiable volumetric renderer that models the propagation of acoustic waves to synthesize imaging sonar measurements. We perform experiments on real and synthetic datasets and show that our algorithm reconstructs high-fidelity surface geometry from multi-view FLS images at much higher quality than was possible with previous techniques and without suffering from their associated memory overhead.

ICRA Conference 2023 Conference Paper

Robust Incremental Smoothing and Mapping (riSAM)

• Daniel McGann
• John G. Rogers
• Michael Kaess

This paper presents a method for robust optimization for online incremental Simultaneous Localization and Mapping (SLAM). Due to the NP-Hardness of data association in the presence of perceptual aliasing, tractable (approximate) approaches to data association will produce erroneous measurements. We require SLAM back-ends that can converge to accurate solutions in the presence of outlier measurements while meeting online efficiency constraints. Existing robust SLAM methods either remain sensitive to outliers, become increasingly sensitive to initialization, or fail to provide online efficiency. We present the robust incremental Smoothing and Mapping (riSAM) algorithm, a robust back-end optimizer for incremental SLAM based on Graduated Non-Convexity. We demonstrate on benchmarking datasets that our algorithm achieves online efficiency, outperforms existing online approaches, and matches or improves the performance of existing offline methods.

IROS Conference 2022 Conference Paper

Acoustic Localization and Communication Using a MEMS Microphone for Low-cost and Low-power Bio-inspired Underwater Robots

• Akshay Hinduja
• Yunsik Ohm
• Jiahe Liao
• Carmel Majidi
• Michael Kaess

Having accurate localization capabilities is one of the fundamental requirements of autonomous robots. For underwater vehicles, the choices for effective localization are limited due to limitations of GPS use in water and poor environ-mental visibility that makes camera-based methods ineffective. Popular inertial navigation methods for underwater localization using Doppler-velocity log sensors, sonar, high-end inertial navigation systems, or acoustic positioning systems require bulky expensive hardware which are incompatible with low-cost, bio-inspired underwater robots. In this paper, we introduce an approach for underwater robot localization inspired by GPS methods known as acoustic pseudoranging. Our method allows us to potentially localize multiple bio-inspired robots equipped with commonly available micro electro-mechanical systems microphones. This is achieved through estimating the time difference of arrival of acoustic signals sent simultaneously through four speakers with a known constellation geometry. We also leverage the same acoustic framework to perform one-way communication with the robot to execute some primitive motions. To our knowledge, this is the first application of the approach for the on-board localization of small bio-inspired robots in water. Hardware schematics and the accompanying code are released to aid further development in the field 3 3 https://github.com/rpl-cmu/underwater-acoustic-pseudoranging.

ICRA Conference 2022 Conference Paper

EDPLVO: Efficient Direct Point-Line Visual Odometry

• Lipu Zhou
• Guoquan Huang 0003
• Yinian Mao
• Shengze Wang 0002
• Michael Kaess

This paper introduces an efficient direct visual odometry (VO) algorithm using points and lines. Pixels on lines are generally adopted in direct methods. However, the original photometric error is only defined for points. It seems difficult to extend it to lines. In previous works, the collinear constraints for points on lines are either ignored [1] or introduce heavy computational load into the resulting optimization system [2]. This paper extends the photometric error for lines. We prove that the 3D points of the points on a 2D line are determined by the inverse depths of the endpoints of the 2D line, and derive a closed-form solution for this problem. This property can significantly reduce the number of variables to speed up the optimization, and can make the collinear constraint exactly satisfied. Furthermore, we introduce a two-step method to further accelerate the optimization, and prove the convergence of this method. The experimental results show that our algorithm outperforms the state-of-the-art direct VO algorithms.

ICRA Conference 2022 Conference Paper

GPS-Denied Global Visual-Inertial Ground Vehicle State Estimation via Image Registration

• Yehonathan Litman
• Daniel McGann
• Eric Dexheimer
• Michael Kaess

Robotic systems such as unmanned ground vehicles (UGVs) often depend on GPS for navigation in outdoor environments. In GPS-denied environments, one approach to maintain a global state estimate is localizing based on preexisting georeferenced aerial or satellite imagery. However, this is inherently challenged by the significantly differing perspectives between the UGV and reference images. In this paper, we introduce a system for global localization of UGVs in remote, natural environments. We use multi-stereo visual inertial odometry (MSVIO) to provide local tracking. To overcome the challenge of differing viewpoints we use a probabilistic occupancy model to generate synthetic orthographic images from color images taken by the UGV. We then derive global information by scan matching local images to existing reference imagery and then use a pose graph to fuse the measurements to provide uninterrupted global positioning after loss of GPS signal. We show that our system generates visually accurate orthographic images of the environment, provides reliable global measurements, and maintains an accurate global state estimate in GPS-denied conditions.

IROS Conference 2022 Conference Paper

Group-k Consistent Measurement Set Maximization for Robust Outlier Detection

• Brendon Forsgren
• Ram Vasudevan
• Michael Kaess
• Timothy W. McLain
• Joshua G. Mangelson

This paper presents a method for the robust selection of measurements in a simultaneous localization and mapping (SLAM) framework. Existing methods check consistency or compatibility on a pairwise basis, however many measurement types are not sufficiently constrained in a pairwise scenario to determine if either measurement is inconsistent with the other. This paper presents group- $k$ consistency maximization ( $\mathrm{G}k\text{CM}$ ) that estimates the largest set of measurements that is internally group- $k$ consistent. Solving for the largest set of group- $k$ consistent measurements can be formulated as an instance of the maximum clique problem on generalized graphs and can be solved by adapting current methods. This paper evaluates the performance of $\mathrm{G}k\text{CM}$ using simulated data and compares it to pairwise consistency maximization (PCM) presented in previous work.

ICRA Conference 2022 Conference Paper

HoloOcean: An Underwater Robotics Simulator

• Easton R. Potokar
• Spencer Ashford
• Michael Kaess
• Joshua G. Mangelson

Due to the difficulty and expense of underwater field trials, a high fidelity underwater simulator is a necessity for testing and developing algorithms. To fill this need, we present HoloOcean, an open source underwater simulator, built upon Unreal Engine 4 (UE4). HoloOcean comes equipped with multi-agent support, various sensor implementations of common underwater sensors, and simulated communications support. We also implement a novel sonar sensor model that leverages an octree representation of the environment for efficient and realistic sonar imagery generation. Due to being built upon UE4, new environments are straightforward to add, enabling easy extensions to be built. Finally, HoloOcean is controlled via a simple python interface, allowing simple installation via pip, and requiring few lines of code to execute simulations.

IROS Conference 2022 Conference Paper

HoloOcean: Realistic Sonar Simulation

• Easton R. Potokar
• Kalliyan Lay
• Kalin Norman
• Derek Benham
• Tracianne B. Neilsen
• Michael Kaess
• Joshua G. Mangelson

Sonar sensors play an integral part in underwater robotic perception by providing imagery at long distances where standard optical cameras cannot. They have proven to be an important part in various robotic algorithms including localization, mapping, and structure from motion. Unfortunately, generating realistic sonar imagery for algorithm development is difficult due to the high cost of field trials and lack of simulation methods. To remove these obstacles, we present various upgrades to the sonar simulation method in HoloOcean, our open-source marine robotics simulator. In particular, we improve the noise modeling using a novel cluster-based multipath ray-tracing algorithm, various probabilistic noise models, and material dependence. We also develop and integrate simulated models for side-scan, single-beam, and multibeam profiling sonars.

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

Learned Depth Estimation of 3D Imaging Radar for Indoor Mapping

• Ruoyang Xu
• Wei Dong
• Akash Sharma
• Michael Kaess

3D imaging radar offers robust perception capability through visually demanding environments due to the unique penetrative and reflective properties of millimeter waves (mmWave). Current approaches for 3D perception with imaging radar require knowledge of environment geometry, accumulation of data from multiple frames for perception, or access to between-frame motion. Imaging radar presents an additional difficulty due to the complexity of its data representation. To address these issues, and make imaging radar easier to use for downstream robotics tasks, we propose a learning-based method that regresses radar measurements into cylindrical depth maps using LiDAR supervision. Due to the limitation of the regression formulation, directions where the radar beam could not reach will still generate a valid depth. To address this issue, our method additionally learns a 3D filter to remove those pixels. Experiments show that our system generates visually accurate depth estimation. Furthermore, we confirm the overall ability to generalize in the indoor scene using the estimated depth for probabilistic occupancy mapping with ground truth trajectory. The code and model will be released 1 1 https://github.com/rpl-cmu/learned-depth-imaging-radar.

ICRA Conference 2022 Conference Paper

PatchGraph: In-hand tactile tracking with learned surface normals

• Paloma Sodhi
• Michael Kaess
• Mustafa Mukadam
• Stuart Anderson

We address the problem of tracking 3D object poses from touch during in-hand manipulations. Specifically, we look at tracking small objects using vision-based tactile sensors that provide high-dimensional tactile image measurements at the point of contact. While prior work has relied on a-priori information about the object being localized, we remove this requirement. Our key insight is that an object is composed of several local surface patches, each informative enough to achieve reliable object tracking. Moreover, we can recover the geometry of this local patch online by extracting local surface normal information embedded in each tactile image. We propose a novel two-stage approach. First, we learn a mapping from tactile images to surface normals using an image translation network. Second, we use these surface normals within a factor graph to both reconstruct a local patch map and use it to infer 3D object poses. We demonstrate reliable object tracking for over 100 contact sequences across unique shapes with four objects in simulation and two objects in the real-world.

ICRA Conference 2022 Conference Paper

ShapeMap 3-D: Efficient shape mapping through dense touch and vision

• Sudharshan Suresh
• Zilin Si
• Joshua G. Mangelson
• Wenzhen Yuan 0001
• Michael Kaess

Knowledge of 3-D object shape is of great importance to robot manipulation tasks, but may not be readily available in unstructured environments. While vision is often occluded during robot-object interaction, high-resolution tactile sensors can give a dense local perspective of the object. However, tactile sensors have limited sensing area and the shape representation must faithfully approximate non-contact areas. In addition, a key challenge is efficiently incorporating these dense tactile measurements into a 3-D mapping framework. In this work, we propose an incremental shape mapping method using a GelSight tactile sensor and a depth camera. Local shape is recovered from tactile images via a learned model trained in simulation. Through efficient inference on a spatial factor graph informed by a Gaussian process, we build an implicit surface representation of the object. We demonstrate visuo-tactile mapping in both simulated and real-world experiments, to incrementally build 3-D reconstructions of household objects.

YNICL Journal 2022 Journal Article

The relationship between adolescents' externalizing and internalizing symptoms and brain development over a period of three years

• Irina Jarvers
• Stephanie Kandsperger
• Daniel Schleicher
• Ayaka Ando
• Franz Resch
• Julian Koenig
• Michael Kaess
• Romuald Brunner

BACKGROUND: Adolescence is a crucial period for both brain maturation and the emergence of mental health disorders. Associations between brain morphology and internalizing/externalizing symptomatology have been identified in clinical or at-risk samples, but age-related developmental differences were rarely considered. The current study investigated the longitudinal relationship between internalizing/externalizing symptoms and brain development in the absence of psychiatric disorders during early and late adolescence. METHODS: 98 healthy adolescents within two cohorts (younger: 9 years, older: 12 years) participated in annual assessments for three years; a clinical assessment measuring their externalizing and internalizing symptoms (SDQ) and an MRI assessment measuring their brain volume and white matter microstructure, including fractional anisotropy (FA), mean diffusivity (MD) and average path length. RESULTS: Linear mixed effect models and cross-lagged panel models showed that larger subcortical gray matter volume predicted more externalizing symptoms in older adolescents whereas decreases of subcortical gray matter volume predicted more externalizing symptoms for younger adolescents. Additionally, longer average white matter path length predicted more externalizing symptoms for older adolescents, while decreases in cerebral white matter volume were predictive of more externalizing symptoms for younger adolescents. There were no predictive effects for internalizing symptoms, FA or MD. CONCLUSIONS: Delays in subcortical brain maturation, in both early and late adolescence, are associated with increases in externalizing behavior which indicates a higher risk for psychopathology and warrants further investigations.

ICRA Conference 2021 Conference Paper

A Graph-Based Method for Joint Instance Segmentation of Point Clouds and Image Sequences

• Montiel Abello
• Joshua G. Mangelson
• Michael Kaess

We address the problem of class agnostic, joint instance segmentation of scene data. While learning-based semantic instance segmentation methods have achieved impressive progress, their use is limited in robotics applications due to reliance on expensive training data annotations and assumptions of single sensor modality or known object classes. We propose a novel graph-based instance segmentation approach that combines information from a 2D image sequence and a 3D point cloud capturing the scene. Our approach propagates information with a general graph representation to produce a segmentation taking into account both geometric and photometric information. This allows us to leverage information from complementary sensor modalities without requiring training data. Our method shows improved object recall and boundary identification over state-of-the-art RGB-D segmentation methods. We demonstrate generality by evaluating on both RGB-D data and a LiDAR+image sensor data.

ICRA Conference 2021 Conference Paper

Compositional and Scalable Object SLAM

• Akash Sharma
• Wei Dong
• Michael Kaess

We present a fast, scalable, and accurate Simultaneous Localization and Mapping (SLAM) system that represents indoor scenes as a graph of objects. Leveraging the observation that artificial environments are structured and occupied by recognizable objects, we show that a compositional and scalable object mapping formulation is amenable to a robust SLAM solution for drift-free large-scale indoor reconstruction. To achieve this, we propose a novel semantically assisted data association strategy that results in unambiguous persistent object landmarks and a 2. 5D compositional rendering method that enables reliable frame-to-model RGB-D tracking. Consequently, we deliver an optimized online implementation that can run at near frame rate with a single graphics card, and provide a comprehensive evaluation against state-of-the-art baselines. An open-source implementation will be provided at https://github.com/rpl-cmu/object-slam.

IROS Conference 2021 Conference Paper

Ground Encoding: Learned Factor Graph-based Models for Localizing Ground Penetrating Radar

• Alexander Baikovitz
• Paloma Sodhi
• Michael Dille
• Michael Kaess

We address the problem of robot localization using ground penetrating radar (GPR) sensors. Current approaches for localization with GPR sensors require a priori maps of the system’s environment as well as access to approximate global positioning (GPS) during operation. In this paper, we propose a novel, real-time GPR-based localization system for unknown and GPS-denied environments. We model the localization problem as an inference over a factor graph. Our approach combines 1D single-channel GPR measurements to form 2D image submaps. To use these GPR images in the graph, we need sensor models that can map noisy, high-dimensional image measurements into the state space. These are challenging to obtain a priori since image generation has a complex dependency on subsurface composition and radar physics, which itself varies with sensors and variations in subsurface electromagnetic properties. Our key idea is to instead learn relative sensor models directly from GPR data that map non-sequential GPR image pairs to relative robot motion. These models are incorporated as factors within the factor graph with relative motion predictions correcting for accumulated drift in the position estimates. We demonstrate our approach over datasets collected across multiple locations using a custom designed experimental rig. We show reliable, real-time localization using only GPR and odometry measurements for varying trajectories in three distinct GPS-denied environments.

ICRA Conference 2021 Conference Paper

HyperMap: Compressed 3D Map for Monocular Camera Registration

• Ming-Fang Chang
• Joshua G. Mangelson
• Michael Kaess
• Simon Lucey

We address the problem of image registration to a compressed 3D map. While this is most often performed by comparing LiDAR scans to the point cloud based map, it depends on an expensive LiDAR sensor at run time and the large point cloud based map creates overhead in data storage and transmission. Recently, efforts have been underway to replace the expensive LiDAR sensor with cheaper cameras and perform 2D-3D localization. In contrast to the previous work that learns relative pose by comparing projected depth and camera images, we propose HyperMap, a paradigm shift from online depth map feature extraction to offline 3D map feature computation for the 2D-3D camera registration task through end-to-end training. In the proposed pipeline, we first perform offline 3D sparse convolution to extract and compress the voxelwise hypercolumn features for the whole map. Then at run-time, we project and decode the compressed map features to the rough initial camera pose to form a virtual feature image. A Convolutional Neural Network (CNN) is then used to predict the relative pose between the camera image and the virtual feature image. In addition, we propose an efficient occlusion handling layer, specifically designed for large point clouds, to remove occluded points in projection. Our experiments on synthetic and real datasets show that, by moving the feature computation load offline and compressing, we reduced map size by 87−94% while maintaining comparable or better accuracy.

ICRA Conference 2021 Conference Paper

Learning Tactile Models for Factor Graph-based Estimation

• Paloma Sodhi
• Michael Kaess
• Mustafa Mukadam
• Stuart Anderson

We’re interested in the problem of estimating object states from touch during manipulation under occlusions. In this work, we address the problem of estimating object poses from touch during planar pushing. Vision-based tactile sensors provide rich, local image measurements at the point of contact. A single such measurement, however, contains limited information and multiple measurements are needed to infer latent object state. We solve this inference problem using a factor graph. In order to incorporate tactile measurements in the graph, we need local observation models that can map highdimensional tactile images onto a low-dimensional state space. Prior work has used low-dimensional force measurements or engineered functions to interpret tactile measurements. These methods, however, can be brittle and difficult to scale across objects and sensors. Our key insight is to directly learn tactile observation models that predict the relative pose of the sensor given a pair of tactile images. These relative poses can then be incorporated as factors within a factor graph. We propose a two-stage approach: first we learn local tactile observation models supervised with ground truth data, and then integrate these models along with physics and geometric factors within a factor graph optimizer. We demonstrate reliable object tracking using only tactile feedback for ~150 real-world planar pushing sequences with varying trajectories across three object shapes.

IROS Conference 2021 Conference Paper

Map Compressibility Assessment for LiDAR Registration

• Ming-Fang Chang
• Wei Dong
• Joshua G. Mangelson
• Michael Kaess
• Simon Lucey

We aim to assess the performance of LiDAR-to-map registration on compressive maps. Modern autonomous vehicles utilize pre-built HD (High-Definition) maps to perform sensor-to-map registration, which recovers pose estimation failures and reduces drift in a large-scale environment. However, sensor-to-map registration is usually realized by registering the sensor to a dense 3D model, which occupies massive storage space in the HD map and requires much data processing overhead. Although smaller 3D models are preferable, the optimal compressive map format for preservation of the best registration performance remains unclear. In this paper, we propose a novel and challenging benchmark to evaluate existing LiDAR-to-map registration methods from three perspectives: map compressibility, robustness, and precision. We compared various map formats, including raw points, hierarchical GMMs, and feature points, and show their performance trade-offs between compressibility and robustness on real-world LiDAR datasets: KITTI Odometry Dataset and Argoverse Tracking Dataset. Our benchmark reveals that state-of-the-art deep feature point based methods outperform traditional methods significantly when the map size budget is high. However, when map size budget is low, deep methods are outperformed by the methods using simpler models in Argoverse Tracking Dataset due to poor spatial coverage. In addition, we observe that the recently published TEASER++ significantly outperforms RANSAC for the feature point methods. Our analysis provides a valuable reference for the community to design budgeted real-world systems and find potential research opportunities. We will release the benchmark for public use.

YNICL Journal 2021 Journal Article

Resting state prefrontal cortex oxygenation in adolescent non-suicidal self-injury – A near-infrared spectroscopy study

• Julian Koenig
• Saskia Höper
• Patrice van der Venne
• Ines Mürner-Lavanchy
• Franz Resch
• Michael Kaess

INTRODUCTION: Neural alterations in limbic and prefrontal circuits in association with self-injurious behavior have been studied primarily in adult borderline personality disorder (BPD). In adolescent patients, research is still sparse. Here, we used resting functional near-infrared spectroscopy (NIRS) to examine oxygenation of the prefrontal cortex (PFC) and its association with symptom severity in adolescents engaging in non-suicidal self-injury (NSSI) and matched healthy controls (HC). METHODS: Adolescents (12-17 years) with recurrent episodes of NSSI (n = 170) and healthy controls (n = 43) performed a low-demanding resting-state vanilla baseline task. Mean oxygenation of the PFC and functional connectivity within the PFC, were measured using an 8-channel functional NIRS system (Octamon, Artinis, The Netherlands). Various clinical variables derived from diagnostic interviews and self-reports were included in statistical analyses to explore potential associations with PFC oxygenation and connectivity. RESULTS: Adolescents with NSSI showed significantly decreased PFC oxygenation compared to HC, as indexed by oxygenated hemoglobin. Lower PFC oxygenation was associated with greater adverse childhood experiences and less health-related quality of life (HRQoL). While there was no evidence for alterations in PFC connectivity in adolescents engaging in NSSI compared to HC, increased PFC connectivity in the full sample was associated with greater adverse childhood experience, greater BPD pathology, greater depression severity and psychological burden in general, as well as lower HRQoL. CONCLUSION: This study is the first to examine PFC oxygenation using NIRS technology in adolescents engaging in NSSI. Overall, results indicate small effects not specific to NSSI. Clinical implications of these findings and recommendations for further research are discussed.

ICRA Conference 2021 Conference Paper

Tactile SLAM: Real-time inference of shape and pose from planar pushing

• Sudharshan Suresh
• Maria Bauzá 0001
• Kuan-Ting Yu
• Joshua G. Mangelson
• Alberto Rodriguez 0003
• Michael Kaess

Tactile perception is central to robot manipulation in unstructured environments. However, it requires contact, and a mature implementation must infer object models while also accounting for the motion induced by the interaction. In this work, we present a method to estimate both object shape and pose in real-time from a stream of tactile measurements. This is applied towards tactile exploration of an unknown object by planar pushing. We consider this as an online SLAM problem with a nonparametric shape representation. Our formulation of tactile inference alternates between Gaussian process implicit surface regression and pose estimation on a factor graph. Through a combination of local Gaussian processes and fixed-lag smoothing, we infer object shape and pose in real-time. We evaluate our system across different objects in both simulated and real-world planar pushing tasks.

ICRA Conference 2021 Conference Paper

π-LSAM: LiDAR Smoothing and Mapping With Planes

• Lipu Zhou
• Shengze Wang 0002
• Michael Kaess

This paper introduces a real-time dense planar LiDAR SLAM system, named π-LSAM, for the indoor environment. The widely used LiDAR odometry and mapping (LOAM) framework [1] does not include bundle adjustment (BA) and generates a low fidelity tracking pose. This paper seeks to overcome these drawbacks for the indoor environment. Specifically, we use the plane as the landmark, and introduce plane adjustment (PA) as our back-end to jointly optimize planes and keyframe poses. We present the π-factor to significantly reduce the computational complexity of PA. In addition, we introduce an efficient loop detection algorithm based on the RANSAC framework using planes. In the front-end, our algorithm performs global registration in real time. To achieve this performance, we maintain the local-to-global point-to-plane correspondences scan by scan, so that we only need a small local KD-tree to establish the data association between a LiDAR scan and the global planes, rather than a large global KD-tree used in previous works. With this local-to-global data association, our algorithm directly identifies planes in a LiDAR scan, and yields an accurate and globally consistent pose. Experimental results show that our algorithm significantly outperforms the state-of-the-art LOAM variant, LeGO-LOAM [2], and our algorithm achieves real time.

ICRA Conference 2020 Conference Paper

A Fast and Accurate Solution for Pose Estimation from 3D Correspondences

• Lipu Zhou
• Shengze Wang 0002
• Michael Kaess

Estimating pose from given 3D correspondences, including point-to-point, point-to-line and point-to-plane correspondences, is a fundamental task in computer vision with many applications. We present a fast and accurate solution for the least-squares problem of this task. Previous works mainly focus on studying the way to find the global minimizer of the least-squares problem. However, existing works that show the ability to achieve the global minimizer are still unsuitable for real-time applications. Furthermore, as one of contributions of this paper, we prove that there exist ambiguous configurations for any number of lines and planes. These configurations have several solutions in theory, which makes the correct solution may come from a local minimizer when the data are with noise. Previous works based on convex optimization which is unable to find local minimizers do not work in the ambiguous configuration. Our algorithm is efficient and able to reveal local minimizers. We employ the Cayley-Gibbs-Rodriguez (CGR) parameterization of the rotation to derive a general rational cost for the three cases of 3D correspondences. The main contribution of this paper is to solve the first-order optimality conditions of the least-squares problem, which are of a complicated rational form. The central idea of our algorithm is to introduce some intermediate unknowns to simplify the problem. Extensive experimental results show that our algorithm is more stable than previous algorithms when the number N of correspondences is small. Besides, when N is large, our algorithm achieves the same accuracy as the state-of-the-art algorithm [1], but our algorithm is about 7 times faster than [1] in real applications.

IROS Conference 2020 Conference Paper

A Robust Multi-Stereo Visual-Inertial Odometry Pipeline

• Joshua Jaekel
• Joshua G. Mangelson
• Sebastian A. Scherer
• Michael Kaess

In this paper we present a novel multi-stereo visual-inertial odometry (VIO) framework which aims to improve the robustness of a robot's state estimate during aggressive motion and in visually challenging environments. Our system uses a fixed-lag smoother which jointly optimizes for poses and landmarks across all stereo pairs. We propose a 1-point RANdom SAmple Consensus (RANSAC) algorithm which is able to perform outlier rejection across features from all stereo pairs. To handle the problem of noisy extrinsics, we account for uncertainty in the calibration of each stereo pair and model it in both our front-end and back-end. The result is a VIO system which is able to maintain an accurate state estimate under conditions that have typically proven to be challenging for traditional state-of-the-art VIO systems. We demonstrate the benefits of our proposed multi-stereo algorithm by evaluating it with both simulated and real world data. We show that our proposed algorithm is able to maintain a state estimate in scenarios where traditional VIO algorithms fail.

IROS Conference 2020 Conference Paper

A Theory of Fermat Paths for 3D Imaging Sonar Reconstruction

• Eric Westman
• Ioannis Gkioulekas
• Michael Kaess

In this work, we present a novel method for reconstructing particular 3D surface points using an imaging sonar sensor. We derive the two-dimensional Fermat flow equation, which may be applied to the planes defined by each discrete azimuth angle in the sonar image. We show that the Fermat flow equation applies to boundary points and surface points which correspond to specular reflections within the 2D plane defined by their azimuth angle measurement. The Fermat flow equation can be used to resolve the 2D location of these surface points within the plane, and therefore also their full 3D location. This is achieved by translating the sensor to estimate the spatial gradient of the range measurement. This method does not rely on the precise image intensity values or the reflectivity of the imaged surface to solve for the surface point locations. We demonstrate the effectiveness of our proposed method by reconstructing 3D object points on both simulated and real-world datasets.

ICRA Conference 2020 Conference Paper

A Volumetric Albedo Framework for 3D Imaging Sonar Reconstruction

• Eric Westman
• Ioannis Gkioulekas
• Michael Kaess

We present a novel framework for object-level 3D underwater reconstruction using imaging sonar sensors. We demonstrate that imaging sonar reconstruction is analogous to the problem of confocal non-line-of-sight (NLOS) reconstruction. Drawing upon this connection, we formulate the problem as one of solving for volumetric albedo, where the scene of interest is modeled as a directionless albedo field. After discretization, reconstruction reduces to a convex linear optimization problem, which we can augment with a variety of priors and regularization terms. We show how to solve the resulting regularized problems using the alternating direction method of multipliers (ADMM) algorithm. We demonstrate the effectiveness of the proposed approach in simulation and on real-world datasets collected in a controlled, test tank environment with several different sonar elevation apertures.

ICRA Conference 2020 Conference Paper

Active SLAM using 3D Submap Saliency for Underwater Volumetric Exploration

• Sudharshan Suresh
• Paloma Sodhi
• Joshua G. Mangelson
• David Wettergreen
• Michael Kaess

In this paper, we present an active SLAM framework for volumetric exploration of 3D underwater environments with multibeam sonar. Recent work in integrated SLAM and planning performs localization while maintaining volumetric free-space information. However, an absence of informative loop closures can lead to imperfect maps, and therefore unsafe behavior. To solve this, we propose a navigation policy that reduces vehicle pose uncertainty by balancing between volumetric exploration and revisitation. To identify locations to revisit, we build a 3D visual dictionary from real-world sonar data and compute a metric of submap saliency. Revisit actions are chosen based on propagated pose uncertainty and sensor information gain. Loop closures are integrated as constraints in our pose-graph SLAM formulation and these deform the global occupancy grid map. We evaluate our performance in simulation and real-world experiments, and highlight the advantages over an uncertainty-agnostic framework.

IROS Conference 2020 Conference Paper

ARAS: Ambiguity-aware Robust Active SLAM based on Multi-hypothesis State and Map Estimations

• Ming Hsiao
• Joshua G. Mangelson
• Sudharshan Suresh
• Christian Debrunner
• Michael Kaess

In this paper, we introduce an ambiguity-aware robust active SLAM (ARAS) framework that makes use of multi-hypothesis state and map estimations to achieve better robustness. Ambiguous measurements can result in multiple probable solutions in a multi-hypothesis SLAM (MH-SLAM) system if they are temporarily unsolvable (due to insufficient information), our ARAS aims at taking all these probable estimations into account explicitly for decision making and planning, which, to the best of our knowledge, has not yet been covered by any previous active SLAM approach (which mostly consider a single hypothesis at a time). This novel ARAS framework 1) adopts local contours for efficient multi-hypothesis exploration, 2) incorporates an active loop closing module that revisits mapped areas to acquire information for hypotheses pruning to maintain the overall computational efficiency, and 3) demonstrates how to use the output target pose for path planning under the multi-hypothesis estimations. Through extensive simulations and a real-world experiment, we demonstrate that the proposed ARAS algorithm can actively map general indoor environments more robustly than a similar single-hypothesis approach in the presence of ambiguities.

IROS Conference 2020 Conference Paper

Efficient Multiresolution Scrolling Grid for Stereo Vision-based MAV Obstacle Avoidance

• Eric Dexheimer
• Joshua G. Mangelson
• Sebastian A. Scherer
• Michael Kaess

Fast, aerial navigation in cluttered environments requires a suitable map representation for path planning. In this paper, we propose the use of an efficient, structured multiresolution representation that expands the sensor range of dense local grids for memory-constrained platforms. While similar data structures have been proposed, we avoid processing redundant occupancy information and use the organization of the grid to improve efficiency. By layering 3D circular buffers that double in resolution at each level, obstacles near the robot are represented at finer resolutions while coarse spatial information is maintained at greater distances. We also introduce a novel method for efficiently calculating the Euclidean distance transform on the multiresolution grid by leveraging its structure. Lastly, we utilize our proposed framework to demonstrate improved stereo camera-based MAV obstacle avoidance with an optimization-based planner in simulation.

IROS Conference 2020 Conference Paper

Efficient Trajectory Library Filtering for Quadrotor Flight in Unknown Environments

• Vaibhav K. Viswanathan
• Eric Dexheimer
• Guanrui Li
• Giuseppe Loianno
• Michael Kaess
• Sebastian A. Scherer

Quadrotor flight in cluttered, unknown environments is challenging due to the limited range of perception sensors, challenging obstacles, and limited onboard computation. In this work, we directly address these challenges by proposing an efficient, reactive planning approach. We introduce the Bitwise Trajectory Elimination (BiTE) algorithm for efficiently filtering out in-collision trajectories from a trajectory library by using bitwise operations. Then, we outline a full receding-horizon planning approach for quadrotor flight in unknown environments demonstrated at up to 50 Hz on an onboard computer. This approach is evaluated extensively in simulation and shown to collision check up to 4896 trajectories in under 20μs, which is the fastest collision checking time for a MAV planner, to the best of the authors' knowledge. Finally, we validate our planner in over 120 minutes of flights in forest-like and urban subterranean environments.

ICRA Conference 2020 Conference Paper

ICS: Incremental Constrained Smoothing for State Estimation

• Paloma Sodhi
• Sanjiban Choudhury
• Joshua G. Mangelson
• Michael Kaess

A robot operating in the world constantly receives information about its environment in the form of new measurements at every time step. Smoothing-based estimation methods seek to optimize for the most likely robot state estimate using all measurements up till the current time step. Existing methods solve for this smoothing objective efficiently by framing the problem as that of incremental unconstrained optimization. However, in many cases observed measurements and knowledge of the environment is better modeled as hard constraints derived from real-world physics or dynamics. A key challenge is that the new optimality conditions introduced by the hard constraints break the matrix structure needed for incremental factorization in these incremental optimization methods. Our key insight is that if we leverage primal-dual methods, we can recover a matrix structure amenable to incremental factorization. We propose a framework ICS that combines a primal-dual method like the Augmented Lagrangian with an incremental Gauss Newton approach that reuses previously computed matrix factorizations. We evaluate ICS on a set of simulated and real-world problems involving equality constraints like object contact and inequality constraints like collision avoidance.

AAAI Conference 2019 Conference Paper

A Robust and Efficient Algorithm for the PnL Problem Using Algebraic Distance to Approximate the Reprojection Distance

• Lipu Zhou
• Yi Yang
• Montiel Abello
• Michael Kaess

This paper proposes a novel algorithm to solve the pose estimation problem from 2D/3D line correspondences, known as the Perspective-n-Line (PnL) problem. It is widely known that minimizing the geometric distance generally results in more accurate results than minimizing an algebraic distance. However, the rational form of the reprojection distance of the line yields a complicated cost function, which makes solving the first-order optimality conditions infeasible. Furthermore, iterative algorithms based on the reprojection distance are time-consuming for a large-scale problem. In contrast to previous works which minimize a cost function based on an algebraic distance that may not approximate the reprojection distance of the line, we design two simple algebraic distances to gradually approximate the reprojection distance. This speeds up the computation, and maintains the robustness of the geometric distance. The two algebraic distances result in two polynomial cost functions, which can be efficiently solved. We directly solve the first-order optimality conditions of the first problem with a novel hidden variable method. This algorithm makes use of the specific structure of the resulting polynomial system, therefore it is more stable than the general Gröbner basis polynomial solver. Then, we minimize the second polynomial cost function by the damped Newton iteration, starting from the solution of the first cost function. Experimental results show that the first step of our algorithm is already superior to the state-of-the-art algorithms in terms of accuracy and applicability, and faster than the algorithms based on Gröbner basis polynomial solver. The second step yields comparable results to the results from minimizing the reprojection distance, but is much more efficient. For speed, our algorithm is applicable to real-time applications.

IROS Conference 2019 Conference Paper

An Efficient and Accurate Algorithm for the Perspecitve-n-Point Problem

• Lipu Zhou
• Michael Kaess

In this paper, we address the problem of pose estimation from N 2D/3D point correspondences, known as the Perspective-n-Point (PnP) problem. Although many solutions have been proposed, it is hard to optimize both computational complexity and accuracy at the same time. In this paper, we propose an accurate and simultaneously efficient solution to the PnP problem. Previous PnP algorithms generally involve two sets of unknowns including the depth of each pixel and the pose of the camera. Our formulation does not involve the depth of each pixel. By introducing some intermediate variables, this formulation leads to a fourth degree polynomial cost function with 3 unknowns that only involves the rotation. In contrast to previous works, we do not address this minimization problem by solving the first-order optimality conditions using the off-the-shelf Gröbner basis method, as the Gröbner basis method may encounter numeric problems. Instead, we present a method based on linear system null space analysis to provide a robust initial estimation for a Newton iteration. Experimental results demonstrate that our algorithm is comparable to the start-of-the-art algorithms in terms of accuracy, and the speed of our algorithm is among the fastest algorithms.

IROS Conference 2019 Conference Paper

Degeneracy-Aware Factors with Applications to Underwater SLAM

• Akshay Hinduja
• Bing-Jui Ho
• Michael Kaess

Simultaneous Localization and Mapping (SLAM) is commonly formulated as an optimization over a graph. A popular approach is the pose graph, which seeks to solve for robots poses that are constrained by pose-to-pose measurements, such as odometry measurements or loop closures. For range sensors, these pose-to-pose constraints can be achieved by performing scan matching techniques, such as Iterative Closest Point (ICP). However, in environments with insufficient or degenerate geometric features, the ICP solution can be unreliable and lead to significant drift in the trajectory of the graph optimization solution. In this paper, we propose a degeneracy-aware approach which has two stages: (1) a degeneracy-aware ICP algorithm and (2) a partially constrained loop closure factor to incorporate the results from (1) into the SLAM pose graph optimization. Our approach performs updates and optimizes both ICP and the pose graph in only the well constrained directions of the state space. These directions are selected on the basis of a dynamic threshold, which updates at each iteration. We apply the proposed algorithm to autonomous underwater mapping with sonar. To evaluate the performance of this algorithm, we conduct experiments in both simulation and real world scenarios, and show the method’s robustness to navigational drift and ability to reject poor loop closures in degenerate environments, which would otherwise degrade the accuracy of the trajectory and the quality of the resulting map.

ICRA Conference 2019 Conference Paper

Dense Surface Reconstruction from Monocular Vision and LiDAR

• Zimo Li
• Prakruti C. Gogia
• Michael Kaess

In this work, we develop a new surface reconstruction pipeline that combines monocular camera images and LiDAR measurements from a moving sensor rig to reconstruct dense 3D mesh models of indoor scenes. For surface reconstruction, the 3D LiDAR and camera are widely deployed for gathering geometric information from environments. Current state-of-the-art multi-view stereo or LiDAR-only reconstruction methods cannot reconstruct indoor environments accurately due to shortcomings of each sensor type. In our approach, LiDAR measurements are integrated into a multi-view stereo pipeline for point cloud densification and tetrahedralization. In addition to that, a graph cut algorithm is utilized to generate a watertight surface mesh. Because our proposed method leverages the complementary nature of these two sensors, the accuracy and completeness of the output model are improved. The experimental results on real world data show that our method significantly outperforms both the state-of-the-art camera-only and LiDAR-only reconstruction methods in accuracy and completeness.

IROS Conference 2019 Conference Paper

Dense, Sonar-based Reconstruction of Underwater Scenes

• Pedro Vaz Teixeira
• Dehann Fourie
• Michael Kaess
• John J. Leonard

Typically, the reconstruction problem is addressed in three independent steps: first, sensor processing techniques are used to filter and segment sensor data as required by the front end. Second, the front end builds the factor graph for the problem to obtain an accurate estimate of the robot’s full trajectory. Finally, the end product is obtained by further processing of sensor data, now re-projected from the optimized trajectory. In this paper we present an approach to model the reconstruction problem in a way that unifies the aforementioned problems under a single framework for a particular application: sonar-based inspection of underwater structures. This is achieved by formulating both the sonar segmentation and point cloud reconstruction problems as factor graphs, in tandem with the SLAM problem. We provide experimental results using data from a ship hull inspection test.

IROS Conference 2019 Conference Paper

GPU Accelerated Robust Scene Reconstruction

• Wei Dong
• Jaesik Park
• Yi Yang
• Michael Kaess

We propose a fast and accurate 3D reconstruction system that takes a sequence of RGB-D frames and produces a globally consistent camera trajectory and a dense 3D geometry. We redesign core modules of a state-of-the-art offline reconstruction pipeline to maximally exploit the power of GPU. We introduce GPU accelerated core modules that include RGBD odometry, geometric feature extraction and matching, point cloud registration, volumetric integration, and mesh extraction. Therefore, while being able to reproduce the results of the high-fidelity offline reconstruction system, our system runs more than 10 times faster on average. Nearly 10Hz can be achieved in medium size indoor scenes, making our offline system even comparable to online Simultaneous Localization and Mapping (SLAM) systems in terms of the speed. Experimental results show that our system produces more accurate results than several state-of-the-art online systems. The system is open source at https://github.com/theNded/Open3D.

ICRA Conference 2019 Conference Paper

MH-iSAM2: Multi-hypothesis iSAM using Bayes Tree and Hypo-tree

• Ming Hsiao
• Michael Kaess

A novel nonlinear incremental optimization algorithm MH-iSAM2 is developed to handle ambiguity in simultaneous localization and mapping (SLAM) problems in a multi-hypothesis fashion. It can output multiple possible solutions for each variable according to the ambiguous inputs, which is expected to greatly enhance the robustness of autonomous systems as a whole. The algorithm consists of two data structures: an extension of the original Bayes tree that allows efficient multi-hypothesis inference, and a Hypo-tree that is designed to explicitly track and associate the hypotheses of each variable as well as all the inference processes for optimization. With our proposed hypothesis pruning strategy, MH-iSAM2 enables fast optimization and avoids the exponential growth of hypotheses. We evaluate MH-iSAM2 using both simulated datasets and real-world experiments, demonstrating its improvements on the robustness and accuracy of SLAM systems.

ICRA Conference 2019 Conference Paper

Multi-view Reconstruction of Wires using a Catenary Model

• Ratnesh Madaan
• Michael Kaess
• Sebastian A. Scherer

Reliable detection and reconstruction of wires is one of the hardest problems in the UAV community, with a wide ranging impact in the industry in terms of wire avoidance capabilities and powerline corridor inspection. In this work, we introduce a real-time, model-based, multi-view algorithm to reconstruct wires from a set of images with known camera poses, while exploiting their natural shape - the catenary curve. Using a model-based approach helps us deal with partial wire detections in images, which may occur due to natural occlusion and false negatives. In addition, using a parsimonious model makes our algorithm efficient as we only need to optimize for 5 model parameters, as opposed to hundreds of 3D points in bundle-adjustment approaches. Our algorithm obviates the need for pixel correspondences by computing the reprojection error via the distance transform of binarized wire segmentation images. Further, we make our algorithm robust to arbitrary initializations by introducing an on-demand, approximate extrapolation of the distance transform based objective. We demonstrate the effectiveness of our algorithm against false negatives and random initializations in simulation, and show qualitative results with real data collected from a small UAV.

IROS Conference 2019 Conference Paper

Online and Consistent Occupancy Grid Mapping for Planning in Unknown Environments

• Paloma Sodhi
• Bing-Jui Ho
• Michael Kaess

Actively exploring and mapping an unknown environment requires integration of both simultaneous localization and mapping (SLAM) and path planning methods. Path planning relies on a map that contains free and occupied space information and is efficient to query, while the role of SLAM is to keep the map consistent as new measurements are continuously added. A key challenge, however, lies in ensuring a map representation compatible with both these objectives: that is, a map that maintains free space information for planning but can also adapt efficiently to dynamically changing pose estimates from a graph-based SLAM system. In this paper, we propose an online global occupancy map that can be corrected for accumulated drift efficiently based on incremental solutions from a sparse graph-based SLAM optimization. Our map maintains free space information for real-time path planning while undergoing a bounded number of updates in each loop closure iteration. We evaluate performance for both simulated and real-world datasets for an application involving underwater exploration and mapping.

ICRA Conference 2019 Conference Paper

Surfel-Based Dense RGB-D Reconstruction With Global And Local Consistency

• Yi Yang
• Wei Dong
• Michael Kaess

Achieving high surface reconstruction accuracy in dense mapping has been a desirable target for both robotics and vision communities. In the robotics literature, simultaneous localization and mapping (SLAM) systems use RGB-D cameras to reconstruct a dense map of the environment. They leverage the depth input to provide accurate local pose estimation and a locally consistent model. However, drift in the pose tracking over time leads to misalignments and artifacts. On the other hand, offline computer vision methods, such as the pipeline that combines structure-from-motion (SfM) and multi-view stereo (MVS), estimate the camera poses by performing batch optimization. These methods achieve global consistency, but suffer from heavy computation loads. We propose a novel approach that integrates both methods to achieve locally and globally consistent reconstruction. First, we estimate poses of keyframes in the offline SfM pipeline to provide strong global constraints at relatively low cost. Afterwards, we compute odometry between frames driven by off-the-shelf SLAM systems with high local accuracy. We fuse the two pose estimations using factor graph optimization to generate accurate camera poses for dense reconstruction. Experiments on real-world and synthetic datasets demonstrate that our approach produces more accurate models comparing to existing dense SLAM systems, while achieving significant speedup with respect to state-of-the-art SfM-MVS pipelines.

IROS Conference 2019 Conference Paper

Wide Aperture Imaging Sonar Reconstruction using Generative Models

• Eric Westman
• Michael Kaess

In this paper we propose a new framework for reconstructing underwater surfaces from wide aperture imaging sonar sequences. We demonstrate that when the leading object edge in each sonar image can be accurately triangulated in 3D, the remaining surface may be “filled in” using a generative sensor model. This process generates a full three-dimensional point cloud for each image in the sequence. We propose integrating these surface measurements into a cohesive global map using a truncated signed distance field (TSDF) to fuse the point clouds generated by each image. This allows for reconstructing surfaces with significantly fewer sonar images and viewpoints than previous methods. The proposed method is evaluated by reconstructing a mock-up piling structure and a real world underwater piling, in a test tank environment and in the field, respectively. Our surface reconstructions are quantitatively compared to ground-truth models and are shown to be more accurate than previous state-of-the-art algorithms.

IROS Conference 2018 Conference Paper

Automatic Extrinsic Calibration of a Camera and a 3D LiDAR Using Line and Plane Correspondences

• Lipu Zhou
• Zimo Li
• Michael Kaess

In this paper, we address the problem of extrinsic calibration of a camera and a 3D Light Detection and Ranging (LiDAR) sensor using a checkerboard. Unlike previous works which require at least three checkerboard poses, our algorithm reduces the minimal number of poses to one by combining 3D line and plane correspondences. Besides, we prove that parallel planar targets with parallel boundaries provide the same constraints in our algorithm. This allows us to place the checkerboard close to the LiDAR so that the laser points better approximate the target boundary without loss of generality. Moreover, we present an algorithm to estimate the similarity transformation between the LiDAR and the camera for the applications where only the correspondences between laser points and pixels are concerned. Using a similarity transformation can simplify the calibration process since the physical size of the checkerboard is not needed. Meanwhile, estimating the scale can yield a more accurate result due to the inevitable measurement errors of the checkerboard size and the LiDAR intrinsic scale factor that transforms the LiDAR measurement to the metric measurement. Our algorithm is validated through simulations and experiments. Compared to the plane-only algorithms, our algorithm can obtain more accurate result by fewer number of poses. This is beneficial to the large-scale commercial application.

ICRA Conference 2018 Conference Paper

Dense Planar-Inertial SLAM with Structural Constraints

• Ming Hsiao
• Eric Westman
• Michael Kaess

In this work, we develop a novel dense planar-inertial SLAM (DPI-SLAM) system to reconstruct dense 3D models of large indoor environments using a hand-held RGB-D sensor and an inertial measurement unit (IMU). The preinte-grated IMU measurements are loosely-coupled with the dense visual odometry (VO) estimation and tightly-coupled with the planar measurements in a full SLAM framework. The poses, velocities, and IMU biases are optimized together with the planar landmarks in a global factor graph using incremental smoothing and mapping with the Bayes Tree (iSAM2). With odometry estimation using both RGB-D and IMU data, our system can keep track of the poses of the sensors even without sufficient planes or visual information (e. g. textureless walls) temporarily. Modeling planes and IMU states in the fully probabilistic global optimization reduces the drift that distorts the reconstruction results of other SLAM algorithms. Moreover, structural constraints between nearby planes (e. g. right angles) are added into the DPI-SLAM system, which further recovers the drift and distortion. We test our DPI-SLAM on large indoor datasets and demonstrate its state-of-the-art performance as the first planar-inertial SLAM system.

ICRA Conference 2018 Conference Paper

Feature-Based SLAM for Imaging Sonar with Under-Constrained Landmarks

• Eric Westman
• Akshay Hinduja
• Michael Kaess

Recent algorithms have demonstrated the feasibility of underwater feature-based SLAM using imaging sonar. But previous methods have either relied on manual feature extraction and correspondence or used prior knowledge of the scene, such as the planar scene assumption. Our proposed system provides a general-purpose method for feature-point extraction and correspondence in arbitrary scenes. Additionally, we develop a method of identifying point landmarks that are likely to be well-constrained and reliably reconstructed. Finally, we demonstrate that while under-constrained landmarks cannot be accurately reconstructed themselves, they can still be used to constrain and correct the sensor motion. These advances represent a large step towards general-purpose, feature-based SLAM with imaging sonar.

IROS Conference 2018 Conference Paper

Information Sparsification in Visual-Inertial Odometry

• Jerry Hsiung
• Ming Hsiao
• Eric Westman
• Rafael Valencia
• Michael Kaess

In this paper, we present a novel approach to tightly couple visual and inertial measurements in a fixed-lag visual-inertial odometry (VIO) framework using information sparsification. To bound computational complexity, fixed-lag smoothers typically marginalize out variables, but consequently introduce a densely connected linear prior which significantly deteriorates accuracy and efficiency. Current state-of-the-art approaches account for the issue by selectively discarding measurements and marginalizing additional variables. However, such strategies are sub-optimal from an information-theoretic perspective. Instead, our approach performs a dense marginalization step and preserves the information content of the dense prior. Our method sparsifies the dense prior with a nonlinear factor graph by minimizing the information loss. The resulting factor graph maintains information sparsity, structural similarity, and nonlinearity. To validate our approach, we conduct real-time drone tests and perform comparisons to current state-of-the-art fixed-lag VIO methods in the EuRoC visual-inertial dataset. The experimental results show that the proposed method achieves competitive and superior accuracy in almost all trials. We include a detailed run-time analysis to demonstrate that the proposed algorithm is suitable for real-time applications.

IROS Conference 2018 Conference Paper

Multibeam Data Processing for Underwater Mapping

• Pedro Vaz Teixeira
• Franz S. Hover
• John J. Leonard
• Michael Kaess

From archaeology to the inspection of subsea structures, underwater mapping has become critical to many applications. Because of the balanced trade-off between range and resolution, multibeam sonars are often used as the primary sensor in underwater mapping platforms. These sonars output an image representing the intensity of the received acoustic echos over space, which must be classified into free and occupied regions before range measurements are determined and spatially registered. Most classifiers found in the underwater mapping literature use local thresholding techniques, which are highly sensitive to noise, outliers, and sonar artifacts typically found in these images. In this paper we present an overview of some of the techniques developed in the scope of our work on sonar-based underwater mapping, with the aim of improving map accuracy through better segmentation performance. We also provide experimental results using data collected with a DIDSON imaging sonar that show that these techniques improve both segmentation accuracy and robustness to outliers.

IROS Conference 2018 Conference Paper

Virtual Occupancy Grid Map for Submap-based Pose Graph SLAM and Planning in 3D Environments

• Bing-Jui Ho
• Paloma Sodhi
• Pedro Teixeira
• Ming Hsiao
• Tushar Kusnur
• Michael Kaess

In this paper, we propose a mapping approach that constructs a globally deformable virtual occupancy grid map (VOG-map) based on local submaps. Such a representation allows pose graph SLAM systems to correct globally accumulated drift via loop closures while maintaining free space information for the purpose of path planning. We demonstrate use of such a representation for implementing an underwater SLAM system in which the robot actively plans paths to generate accurate 3D scene reconstructions. We evaluate performance on simulated as well as real-world experiments. Our work furthers capabilities of mobile robots actively mapping and exploring unstructured, three dimensional environments.

IROS Conference 2017 Conference Paper

GravityFusion: Real-time dense mapping without pose graph using deformation and orientation

• Puneet Puri
• Daoyuan Jia
• Michael Kaess

In this paper, we propose a novel approach to integrating inertial sensor data into a pose-graph free dense mapping algorithm that we call GravityFusion. A range of dense mapping algorithms have recently been proposed, though few integrate inertial sensing. We build on ElasticFusion, a particularly elegant approach that fuses color and depth information directly into small surface patches called surfels. Traditional inertial integration happens at the level of camera motion, however, a pose graph is not available here. Instead, we present a novel approach that incorporates the gravity measurements directly into the map: Each surfel is annotated by a gravity measurement, and that measurement is updated with each new observation of the surfel. We use mesh deformation, the same mechanism used for loop closure in ElasticFusion, to enforce a consistent gravity direction among all the surfels. This eliminates drift in two degrees of freedom, avoiding the typical curving of maps that are particularly pronounced in long hallways, as we qualitatively show in the experimental evaluation.

ICRA Conference 2017 Conference Paper

Keyframe-based dense planar SLAM

• Ming Hsiao
• Eric Westman
• Guofeng Zhang 0001
• Michael Kaess

In this work, we develop a novel keyframe-based dense planar SLAM (KDP-SLAM) system, based on CPU only, to reconstruct large indoor environments in real-time using a hand-held RGB-D sensor. Our keyframe-based approach applies a fast dense method to estimate odometry, fuses depth measurements from small baseline images, extracts planes from the fused depth map, and optimizes the poses of the keyframes and landmark planes in a global factor graph using incremental smoothing and mapping (iSAM). Using the fast odometry estimation, correct plane correspondences may be found projectively, and the pose of each frame can be estimated accurately even without sufficient planes to fully constrain the 6 degree-of-freedom transformation. The depth map generated from the local fusion process generates higher quality reconstructions and plane segmentations by eliminating noise. Moreover, explicitly modeling plane landmarks in the fully probabilistic global optimization significantly reduces the drift that plagues other dense SLAM algorithms. We test our system on standard RGB-D benchmarks as well as additional indoor environments, demonstrating its state-of-the-art performance as a real-time dense 3D SLAM algorithm, without the use of GPU.

ICRA Conference 2017 Conference Paper

Robust stereo matching with surface normal prediction

• Shuangli Zhang
• Weijian Xie
• Guofeng Zhang 0001
• Hujun Bao
• Michael Kaess

Traditional stereo matching approaches generally have problems in handling textureless regions, strong occlusions and reflective regions that do not satisfy a Lambertian surface assumption. In this paper, we propose to combine the predicted surface normal by deep learning to overcome these inherent difficulties in stereo matching. With the selected reliable disparities from stereo matching method and effective edge fusion strategy, we can faithfully convert the predicted surface normal map to a disparity map by solving a least squares system which maintains discontinuity on object boundaries and continuity on other regions. Then we refine the disparity map iteratively by bilateral filtering-based completion and edge feature refinement. Experimental results on the Middlebury dataset and our own captured stereo sequences demonstrate the effectiveness of the proposed approach.

ICRA Conference 2017 Conference Paper

The manifold particle filter for state estimation on high-dimensional implicit manifolds

• Michael C. Koval
• Matthew Klingensmith
• Siddhartha S. Srinivasa
• Nancy S. Pollard
• Michael Kaess

We estimate the state of a noisy robot arm and underactuated hand using an implicit Manifold Particle Filter (MPF) informed by contact sensors. As the robot touches the world, its state space collapses to a contact manifold that we represent implicitly using a signed distance field. This allows us to extend the MPF to higher (six or more) dimensional state spaces. Earlier work, which explicitly represents the contact manifold, was only capable of scaling to three dimensions. Through a series of experiments, we show that the implicit MPF converges faster and is more accurate than a conventional particle filter during periods of persistent contact. We present three methods of drawing samples from an implicit contact manifold, and compare them in experiments.

IROS Conference 2016 Conference Paper

A nonparametric belief solution to the Bayes tree

• Dehann Fourie
• John J. Leonard
• Michael Kaess

We relax parametric inference to a nonparametric representation towards more general solutions on factor graphs. We use the Bayes tree factorization to maximally exploit structure in the joint posterior thereby minimizing computation. We use kernel density estimation to represent a wider class of constraint beliefs, which naturally encapsulates multi-hypothesis and non-Gaussian inference. A variety of new uncertainty models can now be directly applied in the factor graph, and have the solver recover a potentially multi-modal posterior. For example, data association for loop closure proposals can be incorporated at inference time without further modifications to the factor graph. Our implementation of the presented algorithm is written entirely in the Julia language, exploiting high performance parallel computing. We show a larger scale use case with the well known Victoria park mapping and localization data set inferring over uncertain loop closures.

IROS Conference 2016 Conference Paper

Incremental data association for acoustic structure from motion

• Tiffany A. Huang
• Michael Kaess

We provide a novel incremental data association method to complement our previous work on acoustic structure from motion (ASFM), which recovers 3D scene structure from multiple 2D sonar images, while at the same time localizing the sonar. Given point features extracted from multiple overlapping sonar images, our algorithm automatically finds the correspondences between the features. Our data association method uses information about the geometric correlations of the entire set of landmarks to reject spurious measurements or false positives that might otherwise have been accepted. For each new sonar measurement, the algorithm uses a gating procedure to narrow the landmark match search space. Using the pruned surviving candidate correspondences, we identify the correct hypothesis based on a posterior compatibility cost, penalizing for null matches to avoid all measurements being declared new landmarks. Unlike other methods, ASFM does not require any planar scene assumptions and uses constraints from more than two images to increase accuracy in both mapping and localization. We evaluate our algorithm in simulation and demonstrate successful data association results on real sonar images.

IROS Conference 2016 Conference Paper

Long-range GPS-denied aerial inertial navigation with LIDAR localization

• Garrett Hemann
• Sanjiv Singh
• Michael Kaess

Despite significant progress in GPS-denied autonomous flight, long-distance traversals (> 100 km) in the absence of GPS remain elusive. This paper demonstrates a method capable of accurately estimating the aircraft state over a 218 km flight with a final position error of 27 m, 0. 012% of the distance traveled. Our technique efficiently captures the full state dynamics of the air vehicle with semi-intermittent global corrections using LIDAR measurements matched against an a priori Digital Elevation Model (DEM). Using an error-state Kalman filter with IMU bias estimation, we are able to maintain a high-certainty state estimate, reducing the computation time to search over a global elevation map. A sub region of the DEM is scanned with the latest LIDAR projection providing a correlation map of landscape symmetry. The optimal position is extracted from the correlation map to produce a position correction that is applied to the state estimate in the filter. This method provides a GPS-denied state estimate for long range drift-free navigation. We demonstrate this method on two flight data sets from a full-sized helicopter, showing significantly longer flight distances over the current state of the art.

ICRA Conference 2016 Conference Paper

On degeneracy of optimization-based state estimation problems

• Ji Zhang 0003
• Michael Kaess
• Sanjiv Singh

Positioning and mapping can be conducted accurately by state-of-the-art state estimation methods. However, reliability of these methods is largely based on avoiding degeneracy that can arise from cases such as scarcity of texture features for vision sensors and lack of geometrical structures for range sensors. Since the problems are inevitably solved in uncontrived environments where sensors cannot function with their highest quality, it is important for the estimation methods to be robust to degeneracy. This paper proposes an online method to mitigate for degeneracy in optimization-based problems, through analysis of geometric structure of the problem constraints. The method determines and separates degenerate directions in the state space, and only partially solves the problem in well-conditioned directions. We demonstrate utility of this method with data from a camera and lidar sensor pack to estimate 6-DOF ego-motion. Experimental results show that the system is able to improve estimation in environmentally degenerate cases, resulting in enhanced robustness for online positioning and mapping.

IROS Conference 2016 Conference Paper

Pop-up SLAM: Semantic monocular plane SLAM for low-texture environments

• Shichao Yang
• Yu Song
• Michael Kaess
• Sebastian A. Scherer

Existing simultaneous localization and mapping (SLAM) algorithms are not robust in challenging low-texture environments because there are only few salient features. The resulting sparse or semi-dense map also conveys little information for motion planning. Though some work utilize plane or scene layout for dense map regularization, they require decent state estimation from other sources. In this paper, we propose real-time monocular plane SLAM to demonstrate that scene understanding could improve both state estimation and dense mapping especially in low-texture environments. The plane measurements come from a pop-up 3D plane model applied to each single image. We also combine planes with point based SLAM to improve robustness. On a public TUM dataset, our algorithm generates a dense semantic 3D model with pixel depth error of 6. 2 cm while existing SLAM algorithms fail. On a 60 m long dataset with loops, our method creates a much better 3D model with state estimation error of 0. 67%.

IROS Conference 2016 Conference Paper

Underwater inspection using sonar-based volumetric submaps

• Pedro Vaz Teixeira
• Michael Kaess
• Franz S. Hover
• John J. Leonard

We propose a submap-based technique for mapping of underwater structures with complex geometries. Our approach relies on the use of probabilistic volumetric techniques to create submaps from multibeam sonar scans, as these offer increased outlier robustness. Special attention is paid to the problem of denoising/enhancing sonar data. Pairwise submap alignment constraints are used in a factor graph framework to correct for navigation drift and improve map accuracy. We provide experimental results obtained from the inspection of the running gear and bulbous bow of a 600-foot, Wright-class supply ship.

IROS Conference 2015 Conference Paper

Bridging text spotting and SLAM with junction features

• Hsueh-Cheng Wang
• Chelsea Finn
• Liam Paull
• Michael Kaess
• Ruth Rosenholtz
• Seth J. Teller
• John J. Leonard

Navigating in a previously unknown environment and recognizing naturally occurring text in a scene are two important autonomous capabilities that are typically treated as distinct. However, these two tasks are potentially complementary, (i) scene and pose priors can benefit text spotting, and (ii) the ability to identify and associate text features can benefit navigation accuracy through loop closures. Previous approaches to autonomous text spotting typically require significant training data and are too slow for real-time implementation. In this work, we propose a novel high-level feature descriptor, the “junction”, which is particularly well-suited to text representation and is also fast to compute. We show that we are able to improve SLAM through text spotting on datasets collected with a Google Tango, illustrating how location priors enable improved loop closure with text features.

ICRA Conference 2015 Conference Paper

Building 3D mosaics from an Autonomous Underwater Vehicle, Doppler velocity log, and 2D imaging sonar

• Paul Ozog
• Giancarlo Troni
• Michael Kaess
• Ryan M. Eustice
• Matthew Johnson-Roberson

This paper reports on a 3D photomosaicing pipeline using data collected from an autonomous underwater vehicle performing simultaneous localization and mapping (SLAM). The pipeline projects and blends 2D imaging sonar data onto a large-scale 3D mesh that is either given a priori or derived from SLAM. Compared to other methods that generate a 2D-only mosaic, our approach produces 3D models that are more structurally representative of the environment being surveyed. Additionally, our system leverages recent work in underwater SLAM using sparse point clouds derived from Doppler velocity log range returns to relax the need for a prior model. We show that the method produces reasonably accurate surface reconstruction and blending consistency, with and without the use of a prior mesh. We experimentally evaluate our approach with a Hovering Autonomous Underwater Vehicle (HAUV) performing inspection of a large underwater ship hull.

ICRA Conference 2015 Conference Paper

Simultaneous localization and mapping with infinite planes

• Michael Kaess

Simultaneous localization and mapping with infinite planes is attractive because of the reduced complexity with respect to both sparse point-based and dense volumetric methods. We show how to include infinite planes into a least-squares formulation for mapping, using a homogeneous plane parametrization with a corresponding minimal representation for the optimization. Because it is a minimal representation, it is suitable for use with Gauss-Newton, Powell's Dog Leg and incremental solvers such as iSAM. We also introduce a relative plane formulation that improves convergence. We evaluate our proposed approach on simulated data to show its advantages over alternative solutions. We also introduce a simple mapping system and present experimental results, showing real-time mapping of select indoor environments with a hand-held RGB-D sensor.

IROS Conference 2015 Conference Paper

Towards acoustic structure from motion for imaging sonar

• Tiffany A. Huang
• Michael Kaess

We present a novel approach, entitled acoustic structure from motion (ASFM), for recovering 3D scene structure from multiple 2D sonar images, while at the same time localizing the sonar. Imaging sonar or forward looking sonar (FLS) is commonly used for autonomous underwater vehicle (AUV) navigation. An FLS provides bearing and range information to a target, but the elevation of the target is unknown within the sensor's field of view. Hence, current state-of-the-art techniques commonly make a flat surface (ground) assumption so that the FLS data can be used for navigation. Unlike other methods, our solution does not require a flat surface assumption and is capable of utilizing information from many frames, as opposed to pairwise methods that can only gather information from two frames at once. ASFM is inspired by structure from motion (SFM), the problem of recovering 3D structure from multiple camera images, while also recovering the position and orientation from which the images were taken. In this paper, we formulate and evaluate the optimization of several AUV sensor readings of the same scene from different poses, the sonar equivalent of bundle adjustment. We evaluate our approach on both simulated data and FLS sonar data with the assumption that feature extraction and data association have been completed. The acoustic equivalents of those two important features of SFM are left for future work.

ICRA Conference 2014 Conference Paper

Efficient incremental map segmentation in dense RGB-D maps

• Ross Finman
• Thomas Whelan
• Michael Kaess
• John J. Leonard

In this paper we present a method for incrementally segmenting large RGB-D maps as they are being created. Recent advances in dense RGB-D mapping have led to maps of increasing size and density. Segmentation of these raw maps is a first step for higher-level tasks such as object detection. Current popular methods of segmentation scale linearly with the size of the map and generally include all points. Our method takes a previously segmented map and segments new data added to that map incrementally online. Segments in the existing map are re-segmented with the new data based on an iterative voting method. Our segmentation method works in maps with loops to combine partial segmentations from each traversal into a complete segmentation model. We verify our algorithm on multiple real-world datasets spanning many meters and millions of points in real-time. We compare our method against a popular batch segmentation method for accuracy and timing complexity.

IROS Conference 2014 Conference Paper

Real-time depth enhanced monocular odometry

• Ji Zhang 0003
• Michael Kaess
• Sanjiv Singh

Visual odometry can be augmented by depth information such as provided by RGB-D cameras, or from lidars associated with cameras. However, such depth information can be limited by the sensors, leaving large areas in the visual images where depth is unavailable. Here, we propose a method to utilize the depth, even if sparsely available, in recovery of camera motion. In addition, the method utilizes depth by triangulation from the previously estimated motion, and salient visual features for which depth is unavailable. The core of our method is a bundle adjustment that refines the motion estimates in parallel by processing a sequence of images, in a batch optimization. We have evaluated our method in three sensor setups, one using an RGB-D camera, and two using combinations of a camera and a 3D lidar. Our method is rated #2 on the KITTI odometry benchmark irrespective of sensing modality, and is rated #1 among visual odometry methods.

ICRA Conference 2014 Conference Paper

Towards consistent visual-inertial navigation

• Guoquan Huang 0001
• Michael Kaess
• John J. Leonard

Visual-inertial navigation systems (VINS) have prevailed in various applications, in part because of the complementary sensing capabilities and decreasing costs as well as sizes. While many of the current VINS algorithms undergo inconsistent estimation, in this paper we introduce a new extended Kalman filter (EKF)-based approach towards consistent estimates. To this end, we impose both state-transition and obervability constraints in computing EKF Jacobians so that the resulting linearized system can best approximate the underlying nonlinear system. Specifically, we enforce the propagation Jacobian to obey the semigroup property, thus being an appropriate state-transition matrix. This is achieved by parametrizing the orientation error state in the global, instead of local, frame of reference, and then evaluating the Jacobian at the propagated, instead of the updated, state estimates. Moreover, the EKF linearized system ensures correct observability by projecting the most-accurate measurement Jacobian onto the observable subspace so that no spurious information is gained. The proposed algorithm is validated by both Monte-Carlo simulation and real-world experimental tests.

IROS Conference 2013 Conference Paper

Deformation-based loop closure for large scale dense RGB-D SLAM

• Thomas Whelan
• Michael Kaess
• John J. Leonard
• John McDonald 0001

In this paper we present a system for capturing large scale dense maps in an online setting with a low cost RGB-D sensor. Central to this work is the use of an “as-rigid-as-possible” space deformation for efficient dense map correction in a pose graph optimisation framework. By combining pose graph optimisation with non-rigid deformation of a dense map we are able to obtain highly accurate dense maps over large scale trajectories that are both locally and globally consistent. With low latency in mind we derive an incremental method for deformation graph construction, allowing multi-million point maps to be captured over hundreds of metres in real-time. We provide benchmark results on a well established RGB-D SLAM dataset demonstrating the accuracy of the system and also provide a number of our own datasets which cover a wide range of environments, both indoors, outdoors and across multiple floors.

ICRA Conference 2013 Conference Paper

Robust incremental online inference over sparse factor graphs: Beyond the Gaussian case

• David M. Rosen
• Michael Kaess
• John J. Leonard

Many online inference problems in robotics and AI are characterized by probability distributions whose factor graph representations are sparse. While there do exist some computationally efficient algorithms (e. g. incremental smoothing and mapping (iSAM) or Robust Incremental least-Squares Estimation (RISE)) for performing online incremental maximum likelihood estimation over these models, they generally require that the distribution of interest factors as a product of Gaussians, a rather restrictive assumption. In this paper, we investigate the possibility of performing efficient incremental online estimation over sparse factor graphs in the non-Gaussian case. Our main result is a method that generalizes iSAM and RISE by removing the assumption of Gaussian factors, thereby significantly expanding the class of distributions to which these algorithms can be applied. The generalization is achieved by means of a simple algebraic reduction that under relatively mild conditions (boundedness of each of the factors in the distribution of interest) enables an instance of the general maximum likelihood estimation problem to be reduced to an equivalent instance of least-squares minimization that can be solved efficiently online by application of iSAM or RISE. Through this construction we obtain robust, computationally efficient, and mathematically correct incremental online maximum likelihood estimators for non-Gaussian distributions over sparse factor graphs.

ICRA Conference 2013 Conference Paper

Robust real-time visual odometry for dense RGB-D mapping

• Thomas Whelan
• Hordur Johannsson
• Michael Kaess
• John J. Leonard
• John McDonald 0001

This paper describes extensions to the Kintinuous [1] algorithm for spatially extended KinectFusion, incorporating the following additions: (i) the integration of multiple 6DOF camera odometry estimation methods for robust tracking; (ii) a novel GPU-based implementation of an existing dense RGB-D visual odometry algorithm; (iii) advanced fused realtime surface coloring. These extensions are validated with extensive experimental results, both quantitative and qualitative, demonstrating the ability to build dense fully colored models of spatially extended environments for robotics and virtual reality applications while remaining robust against scenes with challenging sets of geometric and visual features.

ICRA Conference 2013 Conference Paper

Temporally scalable visual SLAM using a reduced pose graph

• Hordur Johannsson
• Michael Kaess
• Maurice F. Fallon
• John J. Leonard

In this paper, we demonstrate a system for temporally scalable visual SLAM using a reduced pose graph representation. Unlike previous visual SLAM approaches that maintain static keyframes, our approach uses new measurements to continually improve the map, yet achieves efficiency by avoiding adding redundant frames and not using marginalization to reduce the graph. To evaluate our approach, we present results using an online binocular visual SLAM system that uses place recognition for both robustness and multi-session operation. Additionally, to enable large-scale indoor mapping, our system automatically detects elevator rides based on accelerometer data. We demonstrate long-term mapping in a large multi-floor building, using approximately nine hours of data collected over the course of six months. Our results illustrate the capability of our visual SLAM system to map a large are over extended period of time.

ICRA Conference 2012 Conference Paper

An incremental trust-region method for Robust online sparse least-squares estimation

• David M. Rosen
• Michael Kaess
• John J. Leonard

Many online inference problems in computer vision and robotics are characterized by probability distributions whose factor graph representations are sparse and whose factors are all Gaussian functions of error residuals. Under these conditions, maximum likelihood estimation corresponds to solving a sequence of sparse least-squares minimization problems in which additional summands are added to the objective function over time. In this paper we present Robust Incremental least-Squares Estimation (RISE), an incrementalized version of the Powell's Dog-Leg trust-region method suitable for use in online sparse least-squares minimization. As a trust-region method, Powell's Dog-Leg enjoys excellent global convergence properties, and is known to be considerably faster than both Gauss-Newton and Levenberg-Marquardt when applied to sparse least-squares problems. Consequently, RISE maintains the speed of current state-of-the-art incremental sparse least-squares methods while providing superior robustness to objective function nonlinearities.

IROS Conference 2012 Conference Paper

Dynamic pose graph SLAM: Long-term mapping in low dynamic environments

• Aisha Walcott-Bryant
• Michael Kaess
• Hordur Johannsson
• John J. Leonard

Maintaining a map of an environment that changes over time is a critical challenge in the development of persistently autonomous mobile robots. Many previous approaches to mapping assume a static world. In this work we incorporate the time dimension into the mapping process to enable a robot to maintain an accurate map while operating in dynamical environments. This paper presents Dynamic Pose Graph SLAM (DPG-SLAM), an algorithm designed to enable a robot to remain localized in an environment that changes substantially over time. Using incremental smoothing and mapping (iSAM) as the underlying SLAM state estimation engine, the Dynamic Pose Graph evolves over time as the robot explores new places and revisits previously mapped areas. The approach has been implemented for planar indoor environments, using laser scan matching to derive constraints for SLAM state estimation. Laser scans for the same portion of the environment at different times are compared to perform change detection; when sufficient change has occurred in a location, the dynamic pose graph is edited to remove old poses and scans that no longer match the current state of the world. Experimental results are shown for two real-world dynamic indoor laser data sets, demonstrating the ability to maintain an up-to-date map despite long-term environmental changes.

ICRA Conference 2011 Conference Paper

Efficient AUV navigation fusing acoustic ranging and side-scan sonar

• Maurice F. Fallon
• Michael Kaess
• Hordur Johannsson
• John J. Leonard

This paper presents an on-line nonlinear least squares algorithm for multi-sensor autonomous underwater vehicle (AUV) navigation. The approach integrates the global constraints of range to and GPS position of a surface vehicle or buoy communicated via acoustic modems and relative pose constraints arising from targets detected in side-scan sonar images. The approach utilizes an efficient optimization algorithm, iSAM, which allows for consistent on-line estimation of the entire set of trajectory constraints. The optimized trajectory can then be used to more accurately navigate the AUV, to extend mission duration, and to avoid GPS surfacing. As iSAM provides efficient access to the marginal covariances of previously observed features, automatic data association is greatly simplified - particularly in sparse marine environments. A key feature of our approach is its intended scalability to single surface sensor (a vehicle or buoy) broadcasting its GPS position and simultaneous one-way travel time range (OWTT) to multiple AUVs. We discuss why our approach is scalable as well as robust to modem transmission failure. Results are provided for an ocean experiment using a Hydroid REMUS 100 AUV co-operating with one of two craft: an autonomous surface vehicle (ASV) and a manned support vessel. During these experiments the ranging portion of the algorithm ran online on-board the AUV. Extension of the paradigm to multiple missions via the optimization of successive survey missions (and the resultant sonar mosaics) is also demonstrated.

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.

IROS Conference 2010 Conference Paper

Imaging sonar-aided navigation for autonomous underwater harbor surveillance

• Hordur Johannsson
• Michael Kaess
• Brendan J. Englot
• Franz S. Hover
• John J. Leonard

In this paper we address the problem of drift-free navigation for underwater vehicles performing harbor surveillance and ship hull inspection. Maintaining accurate localization for the duration of a mission is important for a variety of tasks, such as planning the vehicle trajectory and ensuring coverage of the area to be inspected. Our approach only uses onboard sensors in a simultaneous localization and mapping setting and removes the need for any external infrastructure like acoustic beacons. We extract dense features from a forward-looking imaging sonar and apply pair-wise registration between sonar frames. The registrations are combined with onboard velocity, attitude and acceleration sensors to obtain an improved estimate of the vehicle trajectory. We show results from several experiments that demonstrate drift-free navigation in various underwater environments.

ICRA Conference 2010 Conference Paper

Multiple relative pose graphs for robust cooperative mapping

• Been Kim
• Michael Kaess
• Luke Fletcher
• John J. Leonard
• Abraham Bachrach
• Nicholas Roy
• Seth J. Teller

This paper describes a new algorithm for cooperative and persistent simultaneous localization and mapping (SLAM) using multiple robots. Recent pose graph representations have proven very successful for single robot mapping and localization. Among these methods, incremental smoothing and mapping (iSAM) gives an exact incremental solution to the SLAM problem by solving a full nonlinear optimization problem in real-time. In this paper, we present a novel extension to iSAM to facilitate online multi-robot mapping based on multiple pose graphs. Our main contribution is a relative formulation of the relationship between multiple pose graphs that avoids the initialization problem and leads to an efficient solution when compared to a completely global formulation. The relative pose graphs are optimized together to provide a globally consistent multi-robot solution. Efficient access to covariances at any time for relative parameters is provided through iSAM, facilitating data association and loop closing. The performance of the technique is illustrated on various data sets including a publicly available multi-robot data set. Further evaluation is performed in a collaborative helicopter and ground robot experiment.

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 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.

ICRA Conference 2002 Conference Paper

Learning Behavioral Parameterization using Spatio-Temporal Case-Based Reasoning

• Maxim Likhachev
• Michael Kaess
• Ronald C. Arkin

This paper presents an approach to learning an optimal behavioral parameterization in the framework of a case-based reasoning methodology for autonomous navigation tasks. It is based on our previous work on a behavior-based robotic system that also employed spatio-temporal case-based reasoning in the selection of behavioral parameters but was not capable of learning new parameterizations. The present method extends the case-based reasoning module by making it capable of learning new and optimizing the existing cases where each case is a set of behavioral parameters. The learning process can either be a separate training process or be part of the mission execution. In either case, the robot learns an optimal parameterization of its behavior for different environments it encounters. The goal of this research is not only to automatically optimize the performance of the robot but also to avoid the manual configuration of behavioral parameters and the initial configuration of a case library, both of which require the user to possess good knowledge of robot behavior and the performance of numerous experiments. The presented method was integrated within a hybrid robot architecture and evaluated in extensive computer simulations, showing a significant increase in the performance over a nonadaptive system and a performance comparable to a non-learning CBR system that uses a hand-coded case library.