Generalizing metric monocular depth estimation presents a significant challenge due to its ill-posed nature, while the entanglement between camera parameters and depth amplifies issues further, hindering multi-dataset training and zero-shot accuracy. This challenge is particularly evident in autonomous vehicles and mobile robotics, where data is collected with fixed camera setups, limiting the geometric diversity. Yet, this context also presents an opportunity: the fixed relationship between the camera and the ground plane imposes additional perspective geometry constraints, enabling depth regression via vertical image positions of objects. However, this cue is highly susceptible to overfitting, thus we propose a novel canonical representation that maintains consistency across varied camera setups, effectively disentangling depth from specific parameters and enhancing generalization across datasets. We also propose a novel architecture that adaptively and probabilistically fuses depths estimated via object size and vertical image position cues. A comprehensive evaluation demonstrates the effectiveness of the proposed approach on five autonomous driving datasets, achieving accurate metric depth estimation for varying resolutions, aspect ratios and camera setups. Notably, we achieve comparable accuracy to existing zero-shot methods, despite training on a single dataset with a single-camera setup. Project website: https://unizgfer-lamor.github.io/gvdepth/

Accurate ego-motion estimation is a critical component of any autonomous system. Conventional ego-motion sensors, such as cameras and LiDARs, may be compromised in adverse environmental conditions, such as fog, heavy rain, or dust. Automotive radars, known for their robustness to such conditions, present themselves as complementary sensors or a promising alternative within the ego-motion estimation frameworks. In this paper we propose a novel Radar-Inertial Odometry (RIO) system that integrates an automotive radar and an inertial measurement unit. The key contribution is the integration of online temporal delay calibration within the factor graph optimization framework that compensates for potential time offsets between radar and IMU measurements. To validate the proposed approach we have conducted thorough experimental analysis on real-world radar and IMU data. The results show that, even without scan matching or target tracking, integration of online temporal calibration significantly reduces localization error compared to systems that disregard time synchronization, thus highlighting the important role of, often neglected, accurate temporal alignment in radar-based sensor fusion systems for autonomous navigation. Project website: https://rio-online-t.github.io/.

The operational reliability of an autonomous robot depends crucially on extrinsic sensor calibration as a prerequisite for precise and accurate data fusion. Exploring the calibration of unscaled sensors (e.g., monocular cameras) and the effective utilization of uncertainties are difficult and often overlooked. The development of a solution for the simultaneous calibration of hand-eye sensors and scale estimation based on the Gauss–Helmert model aims to utilize the valuable information contained in the uncertainty of odometry. In this work, we propose a versatile and robust solution for batch calibration based on the analytical on-manifold approach for estimation. The versatility of our method is demonstrated by its ability to calibrate multiple unscaled and metric-scaled sensors while dealing with odometry failures and reinitializations. Importantly, all estimated parameters are provided with their corresponding uncertainties. The validation of our method and its comparison with five competing state-of-the-art calibration methods in both simulations and real-world experiments show its superior accuracy, with particularly promising results observed in high-noise scenarios.

Autonomous localization in unknown environments is a fundamental problem in many emerging fields and the monocular visual approach offers many advantages, due to being a rich source of information and avoiding comparatively more complicated setups and multisensor calibration. Deep learning opened new venues for monocular odometry yielding not only end-to-end approaches but also hybrid methods combining the well studied geometry with specific deep components. In this paper we propose a monocular odometry that leverages deep depth within a feature based geometrical framework yielding a lightweight frame-to-frame approach with metrically scaled trajectories and state-of-the-art accuracy. The front-end is based on a multihypothesis matcher with perspective correction coupled with deep depth predictions that enables careful feature selection and tracking; especially of ground plane features that are suitable for translation estimation. The back-end is based on point-to-epipolar line minimization for rotation and unit translation estimation, followed by deep depth aided reprojection error minimization for metrically correct translation estimation. Furthermore, we also present a domain shift adaptation approach that allows for generalization over different camera intrinsic and extrinsic setups. The proposed approach is evaluated on the KITTI and KITTI-360 datasets, showing competitive results and in most cases outperforming other state-of-the-art stereo and monocular methods.

Accurate localization constitutes a fundamental building block of any autonomous system. In this article, we focus on stereo cameras and present a novel approach, dubbed SOFT2, that is currently the highest-ranking algorithm on the KITTI scoreboard. SOFT2 relies on the constraints imposed by the epipolar geometry and kinematics, i.e., it is developed for configurations that cannot exhibit pure rotation. We minimize point-to-epipolar-line distances, which makes the approach resilient to object depth uncertainty, and as the first step, we estimate motion up to scale using just a single camera. Then, we propose to jointly estimate the absolute scale and the extrinsic rotation of the second camera in order to alleviate the effects of varying stereo rig extrinsics. Finally, we smooth the motion estimates in a temporal window of frames by using the proposed epipolar line bundle adjustment procedure. We also introduce a multiple hypothesis feature-matching approach for self-similar planar surfaces that account for appearance change due to perspective. We evaluate SOFT2 and compare it to ORB-SLAM2, OV2SLAM, and VINS-FUSION on the KITTI-360 dataset, KITTI train sequences, Málaga Urban dataset, Oxford Robotics Car dataset, and Multivehicle Stereo Event Camera dataset.

Robot motion planning methods based on trajectory optimization can efficiently generate feasible and optimal trajectories by minimizing a suitable cost function, even in high-dimensional spaces. However, the main drawback of these methods lies in their proneness to infeasible local minima, especially in complex environments. To mitigate this issue, we propose a novel motion planning method that represents trajectories as samples from a mixture of continuous-time Gaussian processes (MGP) and employs stochastic optimization in order to update the MGP parameters in a cost-minimizing manner. The contributions of the proposed trajectory optimization method arise from the introduced mixture representation and stochastic gradient estimation, dominantly enabling better exploration of the trajectory space and including nondifferentiable optimizing costs. We evaluated the proposed method in multiple simulation benchmarks featuring 7 degree-of-freedom (DOF) robot arms and a 10DOF mobile manipulator. We also conducted a real-world experiment with a 14DOF dual-arm robot. The experimental results demonstrated that the proposed method achieves higher success rate than several state-of-the-art methods, while the advantages stemming from MGPs and stochastic optimization, like trajectory smoothness, support of nondifferentiable cost functions, multiple trajectory solutions, and the ability to tackle high-dimensional planning problems, are inherently kept.

Riemannian manifolds are attracting much interest in various technical disciplines, since generated data can often be naturally represented as points on a Riemannian manifold. Due to the non-Euclidean geometry of such manifolds, usual Euclidean methods yield inferior results, thus motivating development of tools adapted or specially tailored to the true underlying geometry. In this letter we propose a method for tracking multiple targets residing on smooth manifolds via probabilistic data association. By using tools of differential geometry, such as exponential and logarithmic mapping along with the parallel transport, we extend the Euclidean multi-target tracking techniques based on probabilistic data association to systems constrained to a Riemannian manifold. The performance of the proposed method was extensively tested in experiments simulating multi-target tracking on unit hyperspheres, where we compared our approach to the von Mises-Fisher and the Kalman filters in the embedding space that projects the estimated state back to the manifold. Obtained results show that the proposed method outperforms the competitive trackers in the optimal sub-pattern assignment metric for all the tested hypersphere dimensions. Although our use case geometry is that of a unit hypersphere, our approach is by no means limited to it and can be applied to any Riemannian manifold with closed-form expressions for exponential/logarithmic maps and parallel transport along the geodesic curve. The paper code is publicly available1.

Robust and reliable perception of autonomous systems often relies on fusion of heterogeneous sensors, which poses great challenges for multisensor calibration. In this article, we propose a method for multisensor calibration based on Gaussian processes (GPs) estimated moving target trajectories, resulting with spatiotemporal calibration. Unlike competing approaches, the proposed method is characterized by the following: first, joint multisensor on-manifold spatiotemporal optimization framework, second, batch state estimation and interpolation using GPs, and, third, computational efficiency with O(n) complexity. It only requires that all sensors can track the same target. The method is validated in simulation and real-world experiments on the following five different multisensor setups: first, hardware triggered stereo camera, second, camera and motion capture system, third, camera and automotive radar, fourth, camera and rotating 3-D lidar, and, fifth, camera, 3-D lidar, and the motion capture system. The method estimates time delays with the accuracy up to a fraction of the fastest sensor sampling time, outperforming a state-of-the-art ego-motion method. Furthermore, this article is complemented by an open-source toolbox implementing the calibration method available at bitbucket.org/unizg-fer-lamor/calirad.

This article proposes a framework for human-pose estimation from the wearable sensors that rely on a Lie group representation to model the geometry of the human movement. Human body joints are modeled by matrix Lie groups, using special orthogonal groups SO(2) and SO(3) for joint pose and special Euclidean group SE(3) for base-link pose representation. To estimate the human joint pose, velocity, and acceleration, we develop the equations for employing the extended Kalman filter on Lie groups (LG-EKF) to explicitly account for the non-Euclidean geometry of the state space. We present the observability analysis of an arbitrarily long kinematic chain of SO(3) elements based on a differential geometric approach, representing a generalization of kinematic chains of a human body. The observability is investigated for the system using marker position measurements. The proposed algorithm is compared with two competing approaches: 1) the extended Kalman filter (EKF) and 2) unscented KF (UKF) based on the Euler angle parametrization, in both simulations and extensive real-world experiments. The results show that the proposed approach achieves significant improvements over the Euler angle-based filters. It provides more accurate pose estimates, is not sensitive to gimbal lock, and more consistently estimates the covariances.

Bayesian filters are often used in statistical inference and consist of recursively alternating between two steps: prediction and correction. Most commonly the Gaussian distribution is used within the Bayes filtering framework, but other distributions, which could model better the nature of the estimated phenomenon like the von Mises-Fisher distribution on the unit sphere, have also been subject of research interest. However, the von Mises-Fisher filter requires approximations since the prediction step does not yield an another von Mises-Fisher distribution. Furthermore, other advanced filtering methods require approximating a mixture of distributions with just a single component. In this paper we propose to use the score matching within the context of Bayesian assumed density filtering inlieu of the more common moment matching. Moment matching functions by assuming the type of the resulting distribution and then matching its moments with the prior distribution, which in the end minimizes the Kullback-Leibler divergence. Score matching also assumes the resulting distribution type, but finds optimal parameters by minimizing the relative Fisher information. In the paper we show that the score matching procedure results with identical performance, but with simpler equations that, unlike moment matching, do not require tedious numerical methods. In the end, we corroborate theoretical results by running the moment and score matching based filters for single and multiple object tracking on a large number of randomly generated trajectories on the unit sphere.

This paper proposes a new algorithm for human motion estimation using inertial measurement unit (IMU) measurements. We model the joints by matrix Lie groups, namely the special orthogonal groups SO(2) and SO(3), representing rotations in 2D and 3D space, respectively. The state space is defined by the Cartesian product of the rotation groups and their velocities and accelerations, given a kinematic model of the articulated body. In order to estimate the state, we propose the Lie Group Extended Kalman Filter (LG-EKF), thus explicitly accounting for the non-Euclidean geometry of the state space, and we derive the LG-EKF recursion for articulated motion estimation based on IMU measurements. The performance of the proposed algorithm is compared to the EKF based on Euler angle parametrization in both simulation and real-world experiments. The results show that for motion near gimbal lock regions, which is common for shoulder movement, the proposed filter is a significant improvement over the Euler angles EKF.

Simultaneous localization and mapping (SLAM) is a core element of every autonomous mobile robot. The underlying engine of a SLAM system is its back-end, which aims at optimally estimating the trajectory and map of the environment based on sensor data abstractions. Over the past decade, SLAM solutions based on graph optimization approaches prevailed over the filtering based solutions, since they dominated in performance over a wider range of applications. In this paper we propose a novel filtering based SLAM back-end based on the exactly sparse delayed state filter (ESDSF) derived on Lie groups (LG-ESDSF). The proposed filter retains all the good characteristics of the classic ESDSF, but also respects the state space geometry by employing filtering equations directly on Lie groups. We have compared our SLAM system with two current state-of-the-art SLAM solutions, namely ORB-SLAM and LSD-SLAM, on the KITTI vision benchmark suite. Test results show that the proposed SLAM based on the LG-ESDSF back-end can achieve same level of accuracy as the methods based on the graph optimization techniques, while maintaining lower computation times.

Many physical systems evolve on matrix Lie groups and mixture filtering designed for such manifolds represent an inevitable tool for challenging estimation problems. However, mixture filtering faces the issue of a constantly growing number of components, hence require appropriate mixture reduction techniques. In this letter we propose a mixture reduction approach for distributions on matrix Lie groups, called the concentrated Gaussian distributions (CGDs). This entails appropriate reparametrization of CGD parameters to compute the KL divergence, pick and merge the mixture components. Furthermore, we also introduce a multitarget tracking filter on Lie groups as a mixture filtering study example for the proposed reduction method. In particular, we implemented the probability hypothesis density filter on matrix Lie groups. We validate the filter performance using the optimal subpattern assignment metric on a synthetic dataset consisting of 100 randomly generated multitarget scenarios.

This letter deals with the problem of tracking multiple targets on the unit circle, a problem that arises whenever the state and the sensor measurements are circular, i.e. angular-only, random variables. To tackle this problem, we propose a novel mixture approximation of the probability hypothesis density filter based on the von Mises distribution, thus constructing a method that globally captures the non-Euclidean nature of the state and the measurement space. We derive a closed-form recursion of the filter and apply principled approximations where necessary. We compared the performance of the proposed filter with the Gaussian mixture probability hypothesis density filter on a synthetic dataset of 100 randomly generated multitarget trajectory examples corrupted with noise and clutter, and on the PETS2009 dataset. We achieved respectively a decrease of 10.5% and 2.8% in the optimal subpattern assignement metric (notably 16.9% and 10.8% in the localization component).

In this paper we present an algorithm for detection, extraction and tracking of moving objects using a 3D laser range sensor. First, ground extraction is performed using random sample consensus for model parameter estimation. Afterwards, to downsample the point cloud, a voxel grid filtering is executed and octree data structure is used. This data structure enables an efficient detection of differences between two consecutive point clouds, based on which clustering of dynamic parts of the cloud is performed. The obtained clusters are then expanded over the set of static voxels in order to cover entire objects. In order to account for ego-motion an iterative closest point registration technique with an initial transformation guess obtained by odometry of the platform is used. As the final step, we present a tracking algorithm based on joint probabilistic data association (JPDA) filter with variable process and measurement noise taking into account velocity and position of the tracked objects. However, JPDA filter assumes a constant and known number of objects in the scene, and therefore we use track management based on entropy. Experiments are performed using a setup consisting of a Velodyne HDL-32E mounted on top of a mobile platform in order to verify the developed algorithms.

Equipping mobile robots with an omnidirectional camera is very advantageous in numerous applications as all information about the surrounding scene is stored in a single image frame. In the given context, the present paper is concerned with detection, tracking and following of a moving object with an omnidirectional camera. The camera calibration and image formation is based on the spherical unified projection model thus yielding a representation of the omnidirectional image on the unit sphere. Detection of moving objects is performed by calculating a sparse optical flow in the image and then lifting the flow vectors on the unit sphere where they are discriminated as dynamic or static by analytically calculating the distance of the terminal vector point to a great circle arc. The flow vectors are then clustered and the center of gravity is calculated to form the sensor measurement. Furthermore, the tracking is posed as a Bayesian estimation problem on the unit sphere and the solution based on the von Mises-Fisher distribution is utilized. Visual servoing is performed for the object following task where the control law calculation is based on the projection of a point on the unit sphere. Experimental results obtained by a camera with a fish-eye lens mounted on a differential drive mobile robot are presented and discussed.

This paper deals with speaker localization in two dimensions from a mobile binaural head. A bootstrap particle filtering scheme is used to perform active localization, i.e. to infer source location by fusing the binaural perception with the sensor motor commands. It relies on an original pseudo-likelihood of the source azimuth which captures both the interaural level and phase differences. Since the pseudo-likelihood is discrete, it is fitted with a mixture of circular distributions in order to enhance its resolution. For the fitting task two mixtures are compared and evaluated, namely the mixture of von Mises and wrapped Cauchy distributions. Furthermore, a solution is presented for calculating the von Mises curvefitting with low uncertainty, since the direct implementation can quickly surpass double precision floating number representation. The performance of the filter is compared using both the raw and fitted pseudo-likelihoods on experiments recorded in an acoustically prepared room with ground-truth obtained from a motion capture system. The results show that the proposed algorithm successfully localizes the speaker with an advantage in the direction of the fitted von Mises mixture likelihood.

This paper presents a novel method for Bayesian bearing-only tracking. Unlike the classical approaches, which involve using Gaussian distribution, the tracking procedure is completely covered with the von Mises distribution, including state representation, transitional probability, and measurement model, since it captures and models well the peculiarities of directional data. The state is represented with a mixture of von Mises distributions, thus offering advantages of being able to model multimodal distributions, handle nonlinear state transition and measurement models, and to completely cover the whole state space, all with a modest number of parameters. The tracking procedure is solved by convolution with a von Mises distribution (prediction step) and multiplication with a mixture representing the measurement model (update step). Since in the update step the number of mixture components grows exponentially, a method is presented for component reduction of a von Mises mixture. Furthermore, a closed-form solution is derived for quadratic Rényi entropy of the von Mises mixture. The algorithm is tested and compared to a particle filter representation in a speaker tracking scenario on a synthetic data set and real-world recordings. The results supported the proposed approach and showed similar performance to the particle filter.

Broad knowledge is required when a business process is modeled by a business analyst. We argue that existing Business Process Management methodologies do not consider business goals at the appropriate level. In this paper we present an approach to integrate business goals and business process models. We design a Business Goal Ontology for modeling business goals. Furthermore, we devise a modeling pattern for linking the goals to process models and show how the ontology can be used in query answering. In this way, we integrate the intentional perspective into our business process ontology framework, enriching the process description and enabling new types of business process analysis.