Activity recognition is a challenging problem with many practical applications. In addition to the visual features, recent approaches have benefited from the use of context, e.g., inter-relationships among the activities and objects. However, these approaches require data to be labeled, entirely available beforehand, and not designed to be updated continuously, which make them unsuitable for surveillance applications. In contrast, we propose a continuous-learning framework for context-aware activity recognition from unlabeled video, which has two distinct advantages over existing methods. First, it employs a novel active-learning technique that not only exploits the informativeness of the individual activities but also utilizes their contextual information during query selection; this leads to significant reduction in expensive manual annotation effort. Second, the learned models can be adapted online as more data is available. We formulate a conditional random field model that encodes the context and devise an information-theoretic approach that utilizes entropy and mutual information of the nodes to compute the set of most informative queries, which are labeled by a human. These labels are combined with graphical inference techniques for incremental updates. We provide a theoretical formulation of the active learning framework with an analytic solution. Experiments on six challenging datasets demonstrate that our framework achieves superior performance with significantly less manual labeling.

In this paper we present a novel direct visual-inertial odometry algorithm, for estimating motion in unknown environments. The algorithm utilizes image patches extracted around image features, and formulates measurement residuals in the image intensity space directly. One key characteristic of the proposed method is that it models the true irradiance at each pixel as a random variable to be estimated and marginalized out. The formulation of the photometric residual explicitly accounts for the camera response function and lens vignetting (which can be calibrated in advance), as well as unknown illumination gains and biases, which are estimated on a per-feature or per-image basis. We present a detailed evaluation of our algorithm on 50 datasets with high-precision ground truth, which amount to approximately 1.5 hours of localization data. Through a direct comparison with a point-feature based method, we demonstrate that the use of photometric residuals results in increased pose estimation accuracy, with approximately 23% lower estimation errors, on average.

In this paper we propose a method for monocular visual-inertial odometry that utilizes image edges as measurements. In contrast to previous feature-based approaches, the proposed method does not employ any assumption on the geometry of the scene (e.g., it does not assume straight lines). It can thus use measurements from all image areas with significant gradient, similarly to direct semi-dense methods. However, in contrast to direct semi-dense approaches, the proposed method's measurement model is invariant to linear changes in the image intensity. The novel edge parameterization and measurement model we propose explicitly account for the fact that edge points can only provide useful information in the direction of the image gradient. We present both Monte-Carlo simulations, as well as results from real-world experimental testing, which demonstrate that the proposed edge-based approach to visual-inertial odometry is consistent, and outperforms the point-based one.

The sampling periods of real-time embedded control functions have a significant impact on control performance and system schedulability. Exploring period assignment for optimizing control performance while meeting schedulability constraints is very challenging, in particular for distributed systems where control loops share a network of computation and communication resources. In this work, we propose an efficient approach that approximates the performance of each control loop in the system with a piecewise linear function of its sampling period and end-to-end delay, and then optimizes the periods of tasks and messages by exploring the linear partitions of the approximated functions and solving a series of geometric programming (GP) formulations. Experiments on sample control models, an automotive industrial case study and a set of synthetic examples demonstrate the effectiveness and efficiency of our approach.

The vast majority of existing methods for vision-aided inertial navigation rely on the detection and tracking of point features in the images. However, in several man-made environments, such as indoor office spaces, straight line features are prevalent, while point features may be sparse. Therefore, developing methods that will enable the use of straight-line features for vision-aided inertial navigation can lead to improved performance. While limited prior work on the subject exists, it assumes the use of a global-shutter camera, i.e., a camera in which all image pixels are captured simultaneously. Most low-cost cameras, however, use rolling-shutter (RS) image capture, which renders the existing methods inapplicable. To address these limitations, we here present an algorithm for vision-aided inertial navigation that employs both point and line features, and is capable of operation with RS cameras. The two key contributions of this work are (i) a novel parameterization for 3D lines, which is shown to exhibit better linearity properties than existing ones, and (ii) a novel approach for the use of line observations in images. This approach forgoes line-fitting and does not assume that a straight line in 3D projects to a straight line in the image, and is thus suitable for use with RS cameras. Our results demonstrate that our proposed estimator formulation leads to improved precision, in point-feature-poor environments.

In this paper, we propose a high-precision pose estimation algorithm for systems equipped with low-cost inertial sensors and rolling-shutter cameras. The key characteristic of the proposed method is that it performs online self-calibration of the camera and the IMU, using detailed models for both sensors and for their relative configuration. Specifically, the estimated parameters include the camera intrinsics (focal length, principal point, and lens distortion), the readout time of the rolling-shutter sensor, the IMU's biases, scale factors, axis misalignment, and g-sensitivity, the spatial configuration between the camera and IMU, as well as the time offset between the timestamps of the camera and IMU. An additional contribution of this work is a novel method for processing the measurements of the rolling-shutter camera, which employs an approximate representation of the estimation errors, instead of the state itself. We demonstrate, in both simulation tests and real-world experiments, that the proposed approach is able to accurately calibrate all the considered parameters in real time, and leads to significantly improved estimation precision compared to existing approaches.

All existing methods for vision-aided inertial navigation assume a camera with a global shutter, in which all the pixels in an image are captured simultaneously. However, the vast majority of consumer-grade cameras use rolling-shutter sensors, which capture each row of pixels at a slightly different time instant. The effects of the rolling shutter distortion when a camera is in motion can be very significant, and are not modelled by existing visual-inertial motion-tracking methods. In this paper we describe the first, to the best of our knowledge, method for vision-aided inertial navigation using rolling-shutter cameras. Specifically, we present an extended Kalman filter (EKF)-based method for visual-inertial odometry, which fuses the IMU measurements with observations of visual feature tracks provided by the camera. The key contribution of this work is a computationally tractable approach for taking into account the rolling-shutter effect, incurring only minimal approximations. The experimental results from the application of the method show that it is able to track, in real time, the position of a mobile phone moving in an unknown environment with an error accumulation of approximately 0.8% of the distance travelled, over hundreds of meters.

When measurements from multiple sensors are combined for real-time motion estimation, the time instant at which each measurement was recorded must be precisely known. In practice, however, the timestamps of each sensor's measurements are typically affected by a delay, which is different for each sensor. This gives rise to a temporal misalignment (i.e., a time offset) between the sensors' data streams. In this work, we propose an online approach for estimating the time offset between the data obtained from different sensors. Specifically, we focus on the problem of motion estimation using visual and inertial sensors in extended Kalman filter (EKF)-based methods. The key idea proposed here is to explicitly include the time offset between the camera and IMU in the EKF state vector, and estimate it online along with all other variables of interest (the IMU pose, the camera-to-IMU calibration, etc). Our proposed approach is general, and can be employed in several classes of estimation problems, such as motion estimation based on mapped features, EKF-based SLAM, or visual-inertial odometry. Our simulation and experimental results demonstrate that the proposed approach yields high-precision, consistent estimates, in scenarios involving both constant and time-varying offsets.

This paper addresses two key limitations of the unscented Kalman filter (UKF) when applied to the simultaneous localization and mapping (SLAM) problem: the cubic computational complexity in the number of states and the inconsistency of the state estimates. To address the first issue, we introduce a new sampling strategy for the UKF, which has constant computational complexity. As a result, the overall computational complexity of UKF-based SLAM becomes of the same order as that of the extended Kalman filter (EKF)-based SLAM, i.e., quadratic in the size of the state vector. Furthermore, we investigate the inconsistency issue by analyzing the observability properties of the linear-regression-based model employed by the UKF. Based on this analysis, we propose a new algorithm, termed observability-constrained (OC)-UKF, which ensures the unobservable subspace of the UKF’s linear-regression-based system model is of the same dimension as that of the nonlinear SLAM system. This results in substantial improvement in the accuracy and consistency of the state estimates. The superior performance of the OC-UKF over other state-of-the-art SLAM algorithms is validated by both Monte-Carlo simulations and real-world experiments.

In this paper we focus on the problem of visual odometry, i.e., the task of tracking the pose of a moving platform using visual measurements. In recent years, several VO algorithms have been proposed that employ nonlinear minimization in a sliding window of poses for this task. Through the use of iterative re-linearization, these methods are capable of successfully addressing the nonlinearity of the measurement models, and have become the de-facto standard for high-precision VO. In this work, we conduct an analysis of the properties of marginalization, which is the process through which older states are removed from the sliding window. This analysis shows that the standard way of marginalizing older poses results in an erroneous change in the rank of the measurements' information matrix, and leads to underestimation of the uncertainty of the state estimates. Based on the analytical results, we also propose a simple modification of the way in which the measurement Jacobians are computed. This modification avoids the above problem, and results in an algorithm with superior accuracy, as demonstrated in both simulation tests and real-world experiments.

In this paper, we perform a rigorous analysis of EKF-based visual-inertial odometry (VIO) and present a method for improving its performance. Specifically, we examine the properties of EKF-based VIO, and show that the standard way of computing Jacobians in the filter inevitably causes inconsistency and loss of accuracy. This result is derived based on an observability analysis of the EKF's linearized system model, which proves that the yaw erroneously appears to be observable. In order to address this problem, we propose modifications to the multi-state constraint Kalman filter (MSCKF) algorithm [1], which ensure the correct observability properties without incurring additional computational cost. Extensive simulation tests and real-world experiments demonstrate that the modified MSCKF algorithm outperforms competing methods, both in terms of consistency and accuracy.

This paper focuses on motion estimation using inertial measurements and observations of naturally occurring point features. To date, this task has primarily been addressed using filtering methods, which track the system state starting from known initial conditions. However, when no prior knowledge of the initial system state is available, (e.g., at the onset of the system's operation), the existing approaches are not applicable. To address this problem, in this work we present algorithms for computing the system's observable quantities (platform attitude and velocity, feature positions, and IMU-camera calibration) directly from the sensor measurements, without any prior knowledge. A key contribution of this work is a convex-optimization based algorithm for computing the rotation matrix between the camera and IMU. We show that once this rotation matrix has been computed, all remaining quantities can be determined by solving a quadratically constrained least-squares problem. To increase their accuracy, the initial estimates are refined by an iterative maximum-likelihood estimator.

In this paper we present a resource-adaptive framework for real-time vision-aided inertial navigation. Specifically, we focus on the problem of visual-inertial odometry (VIO), in which the objective is to track the motion of a mobile platform in an unknown environment. Our primary interest is navigation using miniature devices with limited computational resources, similar for example to a mobile phone. Our proposed estimation framework consists of two main components: (i) a hybrid EKF estimator that integrates two algorithms with complementary computational characteristics, namely a sliding-window EKF and EKF-based SLAM, and (ii) an adaptive image-processing module that adjusts the number of detected image features based on the availability of resources. By combining the hybrid EKF estimator, which optimally utilizes the feature measurements, with the adaptive image-processing algorithm, the proposed estimation architecture fully utilizes the system's computational resources. We present experimental results showing that the proposed estimation framework is capable of real-time processing of image and inertial data on the processor of a mobile phone.

This paper presents a fixed-lag smoothing algorithm for tracking the motion of a mobile robot in real time. The algorithm processes measurements from proprioceptive (e.g., odometry, inertial measurement unit) and exteroceptive (e.g., camera, laser scanner) sensors, in order to estimate the trajectory of the vehicle. Smoothing is carried out in the information-filtering framework, and utilizes iterative minimization, which renders the method well-suited for applications where the effects of the measurements' nonlinearity are significant. The algorithm attains bounded computational complexity by marginalizing out older states. The key contribution of this work is a detailed analysis of the effects of the marginalization process on the consistency properties of the estimator. Based on this analysis, a linearization scheme that results in substantially improved accuracy, compared to the standard linearization approach, is proposed. Both simulation and real-world experimental results are presented, which demonstrate that the proposed method attains localization accuracy superior to that of competing approaches.

We propose a particle filter-based algorithm for monocular vision-aided odometry for mobile robot localization. The algorithm fuses information from odometry with observations of naturally occurring static point features in the environment. A key contribution of this work is a novel approach for computing the particle weights, which does not require including the feature positions in the state vector. As a result, the computational and sample complexities of the algorithm remain low even in feature-dense environments. We validate the effectiveness of the approach extensively with both simulations as well as real-world data, and compare its performance against that of the extended Kalman filter (EKF) and FastSLAM. Results from the simulation tests show that the particle filter approach is better than these competing approaches in terms of the RMS error. Moreover, the experiments demonstrate that the approach is capable of achieving good localization accuracy in complex environments.

A sliding window filter (SWF) is an appealing smoothing algorithm for nonlinear estimation problems such as simultaneous localization and mapping (SLAM), since it is resource-adaptive by controlling the size of the sliding window, and can better address the nonlinearity of the problem by relinearizing available measurements. However, due to the marginalization employed to discard old states from the sliding window, the standard SWF has different parameter observability properties from the optimal batch maximum-a-posterior (MAP) estimator. Specifically, the nullspace of the Fisher information matrix (or Hessian) has lower dimension than that of the batch MAP estimator. This implies that the standard SWF acquires spurious information, which can lead to inconsistency. To address this problem, we propose an observability-constrained (OC)-SWF where the linearization points are selected so as to ensure the correct dimension of the nullspace of the Hessian, as well as minimize the linearization errors. We present both Monte Carlo simulations and real-world experimental results which show that the OC-SWF's performance is superior to the standard SWF, in terms of both accuracy and consistency.

This paper addresses two key limitations of the unscented Kalman filter (UKF) when applied to the simultaneous localization and mapping (SLAM) problem: the cubic, in the number of states, computational complexity, and the inconsistency of the state estimates. In particular, we introduce a new sampling strategy that minimizes the linearization error and whose computational complexity is constant (i.e., independent of the size of the state vector). As a result, the overall computational complexity of UKF-based SLAM becomes of the same order as that of the extended Kalman filter (EKF) when applied to SLAM. Furthermore, we investigate the observability properties of the linear-regression-based model employed by the UKF, and propose a new algorithm, termed the observability-constrained (OC)-UKF, that improves the consistency of the state estimates. The superior performance of the OC-UKF compared to the standard UKF and its robustness to large linearization errors are validated by extensive simulations.

In this paper, we present the vision-aided inertial navigation (VISINAV) algorithm that enables precision planetary landing. The vision front-end of the VISINAV system extracts 2-D-to-3-D correspondences between descent images and a surface map (mapped landmarks), as well as 2-D-to-2-D feature tracks through a sequence of descent images (opportunistic features). An extended Kalman filter (EKF) tightly integrates both types of visual feature observations with measurements from an inertial measurement unit. The filter computes accurate estimates of the lander's terrain-relative position, attitude, and velocity, in a resource-adaptive and hence real-time capable fashion. In addition to the technical analysis of the algorithm, the paper presents validation results from a sounding-rocket test flight, showing estimation errors of only 0.16 m/s for velocity and 6.4 m for position at touchdown. These results vastly improve current state of the art for terminal descent navigation without visual updates, and meet the requirements of future planetary exploration missions.

In this work, we study the inconsistency of EKF-based SLAM from the perspective of observability. We analytically prove that when the Jacobians of the state and measurement models are evaluated at the latest state estimates during every time step, the linearized error-state system model of the EKF-based SLAM has observable subspace of dimension higher than that of the actual, nonlinear, SLAM system. As a result, the covariance estimates of the EKF undergo reduction in directions of the state space where no information is available, which is a primary cause of inconsistency. To address this issue, a new “First Estimates Jacobian” (FEJ) EKF is proposed, which is shown to perform better in terms of consistency. In the FEJ-EKF, the filter Jacobians are calculated using the first-ever available estimates for each state variable, which insures that the observable subspace of the error-state system model is of the same dimension as that of the underlying nonlinear SLAM system. The theoretical analysis is validated through extensive simulations.

This paper presents a method for estimating the six-degrees-of-freedom transformation between a camera and the body of the robot on which it is rigidly attached. The robot maneuvers in front of a planar mirror, allowing the camera to observe fiducial features on the robot from several vantage points. Exploiting these measurements, we form a maximum-likelihood estimate of the camera-to-body transformation, without assuming prior knowledge of the robot motion or of the mirror configuration. Additionally, we estimate the mirror configuration with respect to the camera for each image. We validate the accuracy and correctness of our method with simulations and real-world experiments.

In this paper, we study the accuracy of cooperative localization and target tracking (CLATT) in a team of mobile robots, and derive analytical upper bounds for the position uncertainty. The obtained bounds provide a description of the asymptotic positioning performance of the robots and the targets as a function of the sensor characteristics and the structure of the graph of relative position measurements. By employing an extended Kalman filter (EKF) formulation for data fusion, two key asymptotic results are derived. The first provides the guaranteed worst-case positioning accuracy, whereas the second determines an upper bound on the expected covariance of the estimates. We investigate the effects of jointly estimating the targets' and the robots' position, and demonstrate that it results in better accuracy for the robots' position estimates. The theoretical results are confirmed both in simulation and experimentally.

In this paper, we present an extended Kalman filter (EKF)-based algorithm for real-time vision-aided inertial navigation. The primary contribution of this work is the derivation of a measurement model that is able to express the geometric constraints that arise when a static feature is observed from multiple camera poses. This measurement model does not require including the 3D feature position in the state vector of the EKF and is optimal, up to linearization errors. The vision-aided inertial navigation algorithm we propose has computational complexity only linear in the number of features, and is capable of high-precision pose estimation in large-scale real-world environments. The performance of the algorithm is demonstrated in extensive experimental results, involving a camera/IMU system localizing within an urban area.

This paper presents a new method to optimally combine motion measurements provided by proprioceptive sensors, with relative-state estimates inferred from feature-based matching. Two key challenges arise in such pose tracking problems: 1) the displacement estimates relate the state of the robot at two different time instants and 2) the same exteroceptive measurements are often used for computing consecutive displacement estimates, a process that renders the errors in these correlated. We present a novel stochastic cloning Kalman filtering (SC-KF) estimation algorithm that successfully addresses these challenges, while still allowing for efficient calculation of the filter gains and covariances. The proposed algorithm is not intended to compete with simultaneous localization and mapping (SLAM) approaches. Instead, it can be merged with any extended-Kalman- filter-based SLAM algorithm to increase its precision. In this respect, the SC-KF provides a robust framework for leveraging additional motion information extracted from dense point features that most SLAM algorithms do not treat as landmarks. Extensive experimental and simulation results are presented to verify the validity of the proposed method and to demonstrate that its performance is superior to that of alternative position-tracking approaches.

In this paper, we study the problem of localization using relative-state estimates. It is shown, that when the same exteroceptive sensor measurement is processed for the computation of two consecutive displacement estimates (both forward and backward in time) these estimates are correlated, and an analysis of the exact structure of the correlations is performed. This analysis is utilized in the design of data fusion algorithms, that correctly account for the existing correlations. We examine two cases: i) pose propagation based exclusively on inferred displacement measurements, and ii) the fusion of proprioceptive sensor information with relative-state measurements. For the latter case, an efficient EKF-based estimation algorithm is proposed, that extends the approach of S.I. Roumeliotis and J. Burdick, 2002. Extensive simulation and experimental results are presented, that verify the validity of the presented method

This paper studies the accuracy of position estimation for groups of mobile robots performing cooperative localization. We consider the case of teams comprised of possibly heterogeneous robots and provide analytical expressions for the upper bound on their expected positioning uncertainty. This bound is determined as a function of the sensors' noise covariance and the eigenvalues of the relative position measurement graph (RPMG), i.e., the weighted directed graph which represents the network of robot-to-robot exteroceptive measurements. The RPMG is employed as a key element in this analysis, and its properties are related to the localization performance of the team. It is shown that, for a robot group of a certain size, the maximum expected rate of uncertainty increase is independent of the accuracy and number of relative position measurements and depends only on the accuracy of the proprioceptive and orientation sensors on the robots. Additionally, the effects of changes in the topology of the RPMG are studied, and it is shown that, at steady-state, these reconfigurations do not inflict any loss in localization precision. Experimental data, as well as simulation results that validate the theoretical analysis, are presented

This paper addresses the problem of resource allocation in formations of mobile robots localizing as a group. Each robot receives measurements from various sensors that provide relative (robot-to-robot) and absolute positioning information. Constraints on the sensors' bandwidth, as well as communication and processing requirements, limit the number of measurements that are available or can be processed at each time step. The localization uncertainty of the group, determined by the covariance matrix of the equivalent continuous-time system at steady state, is expressed as a function of the sensor measurements' frequencies. The trace of the weighted covariance matrix is selected as the optimization criterion, under linear constraints on the measuring frequency of each sensor and the cumulative rate of the extended Kalman filter updates. This formulation leads to a convex optimization problem (semidefinite program) whose solution provides the sensing frequencies, for each sensor on every robot, required in order to maximize the positioning accuracy of the group. Simulation and experimental results are presented that demonstrate the applicability of this method and provide insight into the properties of the resource-constrained cooperative localization problem

This paper studies the effects of the geometry of a mobile robot formation on the accuracy of the robots’ localization. The general case of heterogeneous (in terms of sensor accuracy) robot teams performing Cooperative Localization is considered. An analysis of the time evolution of the covariance matrix of the position estimates allows us to express the steady-state positioning uncertainty of the robots as an analytic function of the relative positions of the robots in the formation. This metric encapsulates the effect of formation geometry on the information content of the exteroceptive measurements, as well as the effect of the influx of uncertainty due to the errors in the robots’ odometry. Thus, by minimizing the trace of the steady state covariance matrix with respect to the positions of the robots, the optimal robot configuration can be determined. Numerical experiments are presented, which indicate that it is possible to derive a practical rule for determining optimal formations, without the need to resort to extensive simulations, or experimentation.

This work studies the localization accuracy of a team of mobile robots that perform cooperative localization. We provide an analytical expression for the upper bound in positioning accuracy as a function of a weighted connectivity graph for the network of relative position measurements in the robot group. This network has a time-varying topology determined by the availability of relative position measurements between pairs of robots. The weights of the connectivity graph depend on (i) the odometric and orientation accuracy of each robot, and (ii) the accuracy of the robot tracker on each member of the team that measures its relative position with respect to other robots in the group. The theoretical results are validated by extensive simulations.

This paper studies the time evolution of the covariance of the position estimates in single-robot simultaneous localization and mapping (SLAM). A closed-form expression is derived, that establishes a functional relation between the noise parameters of the robot's proprioceptive and exteroceptive sensors, the number of features being mapped, and the attainable accuracy of SLAM. Furthermore, it is demonstrated how prior information about the spatial density of landmarks can be utilized in order to compute a tight upper bound on the expected covariance of the positioning errors. The derived closed-form expressions enable the prediction of SLAM positioning performance, without resorting to extensive simulations, and thus offer an analytical tool for determining the sensor characteristics required to achieve a desired degree of accuracy. Simulation experiments are conducted, that corroborate the presented theoretical analysis.