We present GSLoc: a new visual localization method that performs dense camera alignment using 3D Gaussian Splatting as a map representation of the scene. GSLoc backpropagates pose gradients over the rendering pipeline to align the rendered and target images, while it adopts a coarse-to-fine strategy by utilizing blurring kernels to mitigate the non-convexity of the problem and improve the convergence. The results show that our approach succeeds at visual localization in challenging conditions of relatively small overlap between initial and target frames inside textureless environments when state-of-the-art neural sparse methods provide inferior results. Using the byproduct of realistic rendering from the 3DGS map representation, we show how to enhance localization results by mixing a set of observed and virtual reference keyframes when solving the image retrieval problem. We evaluate our method both on synthetic and real-world data, discussing its advantages and application potential.

Learning a feature point detector presents a challenge both due to the ambiguity of the definition of a keypoint and, correspondingly, the need for specially prepared ground truth labels for such points. In our work, we address both of these issues by utilizing a combination of a hand-crafted Shi-Tomasi detector, a specially designed metric that assesses the quality of keypoints, the stability score (SS), and a neural network. We build on the principled and localized keypoints provided by the Shi-Tomasi detector and learn the neural network to select good feature points via the stability score. The neural network incorporates the knowledge from the training targets in the form of the neural stability score (NeSS). Therefore, our method is named NeSS-ST since it combines the Shi-Tomasi detector and the properties of the neural stability score. It only requires sets of images for training without dataset pre-labeling or the need for reconstructed correspondence labels. We evaluate NeSS-ST on HPatches, ScanNet, MegaDepth and IMC-PT demonstrating state-of-the-art performance and good generalization on downstream tasks. The project repository is available at: https://github.com/KonstantinPakulev/NeSS-ST.

This paper relates to time-continuous trajectory representation using direct linear interpolation on SE(3). Our approach focuses on a novel analytical Jacobian approximation of a sequence of linearly interpolated poses on SE(3). This paper shows a derivation of the proposed analytical Jacobian using retraction mapping and an approximation to the commutativity property of infinitesimal group elements. We provide plenty of evaluations for 3 different optimization problems. For the synthetic point cloud alignment problem, our proposed Jacobian is compared with a numerical one. For the synthetic pose graph optimization problem, the proposed Jacobian approximation allows us to reduce by x7 factor the state dimensions while keeping a similar magnitude of resulting error compared to the full discrete-time trajectory. Finally, we show the validity of our approach in a time-continuous approach for real-world LIDAR odometry problem.

Lines are interesting geometrical features commonly seen in indoor and urban environments. There is missing a complete benchmark where one can evaluate lines from a sequential stream of images in all its stages: Line detection, Line Association and Pose error. To do so, we present a complete and exhaustive benchmark for visual lines in a SLAM front-end, both for RGB and RGBD, by providing a plethora of complementary metrics. We have also labeled data from well-known SLAM datasets in order to have all in one poses and accurately annotated lines. In particular, we have evaluated 17 line detection algorithms, 5 line associations methods and the resultant pose error for aligning a pair of frames with several combinations of detector-association. We have packaged all methods and evaluations metrics and made them publicly available on web-page33https://prime-slam.github.io/evolin/.

We present a dataset of 1000 video sequences of human portraits recorded in real and uncontrolled conditions by using a handheld smartphone accompanied by an external high-quality depth camera. The collected dataset contains 200 people captured in different poses and locations and its main purpose is to bridge the gap between raw measurements obtained from a smartphone and downstream applications, such as state estimation, 3D reconstruction, view synthesis, etc. The sensors employed in data collection are the smartphone's camera and Inertial Measurement Unit (IMU), and an external Azure Kinect DK depth camera software synchronized with sub-millisecond precision to the smartphone system. During the recording, the smartphone flash is used to provide a periodic secondary source of lightning. Accurate mask of the foremost person is provided as well as its impact on the camera alignment accuracy. For evaluation purposes, we compare multiple state-of-the-art camera alignment methods by using a Motion Cap-ture system. We provide a smartphone visual-inertial bench-mark for portrait capturing, where we report results for multiple methods and motivate further use of the provided trajectories, available in the dataset, in view synthesis and 3D reconstruction tasks.

Human motion prediction is key to understand social environments, with direct applications in robotics, surveil-lance, etc. We present a simple yet effective pedestrian trajectory prediction model aimed at pedestrians' positions prediction in urban-like environments conditioned by the environment: map and surround agents. Our model is a neural-based architecture that can run several layers of attention blocks and transformers in an iterative sequential fashion, allowing to capture the important features in the environment that improve prediction. We show that without explicit introduction of social masks, dynamical models, social pooling layers, or complicated graph-like structures, it is possible to produce on par results with SoTA models, which makes our approach easily extendable and configurable, depending on the data available. We report results performing similarly with SoTA models on publicly available and extensible-used datasets with uni-modal prediction metrics ADE and FDE.

This paper provides the EVOPS dataset for plane segmentation from 3D data, both from RGBD images and LiDAR point clouds. We have designed two annotation methodologies (RGBD and LiDAR) running on well-known and widely-used datasets for SLAM evaluation and we have provided a complete set of benchmarking tools including point, planes and segmentation metrics. The data includes a total number of 10k RGBD and 7K LiDAR frames over different selected scenes which consist of high quality segmented planes. The experiments report quality of SOTA methods for RGBD plane segmentation on our annotated data. We also have provided learnable baseline for plane segmentation in LiDAR point clouds. All labeled data and benchmark tools used have been made publicly available https://evops.netlify.app/.

This work is dedicated to simultaneous continuous-time trajectory estimation and mapping based on Gaussian Processes (GP). State-of-the-art GP-based models for Simultaneous Localization and Mapping (SLAM) are computationally efficient but can only be used with a restricted class of kernel functions. This paper provides the algorithm based on GP with Random Fourier Features (RFF) approximation for SLAM without any constraints. The advantages of RFF for continuous-time SLAM are that we can consider a broader class of kernels and, at the same time, maintain computational complexity at reasonably low level by operating in the Fourier space of features. The accuracy-speed trade-off can be controlled by the number of features. Our experimental results on synthetic and real-world benchmarks demonstrate the cases in which our approach provides better results compared to the current state-of-the-art.

The correct characterization of uncertainty when predicting human motion is equally important as the accuracy of this prediction. We present a new method to correctly predict the uncertainty associated with the predicted distribution of future trajectories. Our approach, CovariaceNet, is based on a Conditional Generative Model with Gaussian latent variables in order to predict the parameters of a bi-variate Gaussian distribution. The combination of CovarianceNet with a motion prediction model results in a hybrid approach that outputs a uni-modal distribution. We will show how some state of the art methods in motion prediction become overconfident when predicting uncertainty, according to our proposed metric and validated in the ETH data-set [1]. CovarianceNet correctly predicts uncertainty, which makes our method suitable for applications that use predicted distributions, e.g., planning or decision making.

In this paper we apply the low-rank Tensor Train decomposition for compression and operations on 3D objects and scenes represented by volumetric distance functions. Our study shows that not only it allows for a very efficient compression of the high-resolution TSDF maps (up to three orders of magnitude of the original memory footprint at resolution of 5123), but also allows to perform TSDF-Fusion directly in the low-rank form. This can potentially enable much more efficient 3D mapping on low-power mobile and consumer robot platforms.

In this paper, we introduce the Eigen-Factor (EF) method, which estimates a planar surface from a set of point clouds (PCs), with the peculiarity that these points have been observed from different poses, i.e. the trajectory described by a sensor. We propose to use multiple Eigen-Factors (EFs) or different planes' estimations, that allow to solve the multi-frame alignment over a sequence of observed PCs. Moreover, the complexity of the EFs optimization is independent of the number of points, but depends on the number of planes and poses. To achieve this, a closed-form of the gradient is derived by differentiating over the minimum eigenvalue with respect to poses, hence the name Eigen-Factor. In addition, a time-continuous trajectory version of EFs is proposed. The EFs approach is evaluated on a simulated environment and compared with two variants of ICP, showing that it is possible to optimize over all point errors, improving both the accuracy and computational time. Code has been made publicly available.

In Multi-Policy Decision-Making (MPDM), many computationally-expensive forward simulations are performed in order to predict the performance of a set of candidate policies. In risk-aware formulations of MPDM, only the worst outcomes affect the decision making process, and efficiently finding these influential outcomes becomes the core challenge. Recently, stochastic gradient optimization algorithms, using a heuristic function, were shown to be significantly superior to random sampling. In this paper, we show that accurate gradients can be computed - even through a complex forward simulation - using approaches similar to those in deep networks. We show that our proposed approach finds influential outcomes more reliably, and is faster than earlier methods, allowing us to evaluate more policies while simultaneously eliminating the need to design an easily-differentiable heuristic function. We demonstrate significant performance improvements in simulation as well as on a real robot platform navigating a highly dynamic environment.

For online robots, incremental SLAM algorithms offer huge potential computational savings over batch algorithms. The dominant incremental algorithms are iSAM and iSAM2 which offer radically different approaches to computing incremental updates, balancing issues like 1) the need to re-linearize, 2) changes in the desirable variable marginalization order, and 3) the underlying conceptual approach (i.e. the “matrix” story versus the “factor graph” story). In this paper, we propose a new incremental algorithm that computes solutions with lower absolute error and generally provides lower error solutions for a fixed computational budget than either iSAM or iSAM2. Key to AprilSAM's performance are a new dynamic variable reordering algorithm for fast incremental Cholesky factorizations, a method for reducing the work involved in backsubstitutions, and a new algorithm for deciding between incremental and batch updates.

Risk-aware Multi-Policy Decision Making (MPDM)is a powerful framework for reliable navigation in a dynamic social environment where rather than evaluating individual trajectories, a “library” of policies (reactive controllers)is evaluated by anticipating potentially dangerous future outcomes using an on-line forward roll-out process. There is a core tension in Multi-Policy Decision Making (MPDM)systems - it is desirable to add more policies to the system for flexibility in finding good policies, however, this increases computational cost. As a result, MPDM was limited to small (perhaps 5-10)discrete policies - a significant performance bottleneck. In this paper, we radically enhance the expressivity of MPDM by allowing policies to have continuous-valued parameters, while simultaneously satisfying real-time constraints by quickly discovering promising policy parameters through a novel iterative gradient-based algorithm. Our evaluation includes results from extensive simulation and real-world experiments in semi-crowded environments.

In the Multi-Policy Decision Making (MPDM) framework, a robot's policy is elected by sampling from the distribution of current states, predicting future outcomes through forward simulation, and selecting the policy with the best expected performance. Electing the best plan depends on sampling initial conditions with influential (very high costs) outcomes. Discovering these configurations through random sampling may require drawing many samples, which becomes a performance bottleneck. In this paper, we describe a risk-aware approach which augments this sampling with an optimization process that helps discover those influential outcomes. We describe how we overcome several practical difficulties with this approach, and demonstrate significant performance improvements on a real robot platform navigating a semi-crowded, highly dynamic environment.

This paper presents an adaptive side-by-side human-robot companion approach for navigation in urban dynamic environments, based on the anticipative kinodynamic planning. The adaptive means that the robot is capable of adjusting its motion to the behavior of the person being accompanied. Our main objective is to optimize in real time the path performed by the pair human-robot, by modifying dynamically the angle and distance between both throughout different locations of the path. We have defined a new cost function for finding the best planned path that takes into account the cost of the geometrical configuration between the human and the robot. Moreover, we have modified the Extended Social Force Model (SFM) to include the required forces to maintain the angle and distance between the robot and human while the human-robot pair is moving towards the shared goal. The method has been validated throughout a large set of simulations and real-live experiments.

In dynamic environments crowded with people, robot motion planning becomes difficult due to the complex and tightly-coupled interactions between agents. Trajectory planning methods, supported by models of typical human behavior and personal space, often produce reasonable behavior. However, they do not account for the future closed-loop interactions of other agents with the trajectory being constructed. As a consequence, the trajectories are unable to anticipate cooperative interactions (such as a human yielding), or adverse interactions (such as the robot blocking the way). In this paper, we propose a new method for navigation amongst pedestrians in which the trajectory of the robot is not explicitly planned, but instead, a planning process selects one of a set of closed-loop behaviors whose utility can be predicted through forward simulation. In particular, we extend Multi-Policy Decision Making (MPDM) [1] to this domain using the closed-loop behaviors Go-Solo, Follow-other, and Stop. By dynamically switching between these policies, we show that we can improve the performance of the robot as measured by utility functions that reward task completion and penalize inconvenience to other agents. Our evaluation includes extensive results in simulation and real-world experiments.

In our previous work [1] we introduced the Anticipative Kinodynamic Planning (AKP): a robot navigation algorithm in dynamic urban environments that seeks to minimize its disruption to nearby pedestrians. In the present paper, we maintain all the advantages of the AKP, and we overcome the previous limitations by presenting novel contributions to our approach. Firstly, we present a multi-objective cost function to consider different and independent criteria and a well-posed procedure to build a joint cost function in order to select the best path. Then, we improve the construction of the planner tree by introducing a cost-to-go function that will be shown to outperform a classical Euclidean distance approach. In order to achieve real time calculations, we have used a steering heuristic that dramatically speeds up the process. Plenty of simulations and real experiments have been carried out to demonstrate the success of the AKP.

In the present work, we propose and validate a complete probabilistic framework for human motion prediction in urban or social environments. Additionally, we formulate a powerful and useful tool: the human motion behavior estimator. Three different basic behaviors have been detected: Aware, Balanced and Unaware. Our approach is based on the Social Force Model (SFM) and the intentionality prediction BHMIP. The main contribution of the present work is to make use of the behavior estimator for formulating a reliable prediction framework of human trajectories under the influence of dynamic crowds, robots, and in general any moving obstacle. Accordingly, we have demonstrated the great performance of our long-term prediction algorithm, in real scenarios, comparing to other prediction methods.

This paper presents a novel approach for robot navigation in crowded urban environments where people and objects are moving simultaneously while a robot is navigating. Avoiding moving obstacles at their corresponding precise moment motivates the use of a robotic planner satisfying both dynamic and nonholonomic constraints, also referred as kynodynamic constraints.We present a proactive navigation approach with respect its environment, in the sense that the robot calculates the reaction produced by its actions and provides the minimum impact on nearby pedestrians. As a consequence, the proposed planner integrates seamlessly planning and prediction and calculates a complete motion prediction of the scene for each robot propagation. Making use of the Extended Social Force Model (ESFM) allows an enormous simplification for both the prediction model and the planning system under differential constraints. Simulations and real experiments have been carried out to demonstrate the success of the proactive kinodynamic planner.

Robots accompanying humans is one of the core capacities every service robot deployed in urban settings should have. We present a novel robot companion approach based on the so-called Social Force Model (SFM). A new model of robot-person interaction is obtained using the SFM which is suited for our robots Tibi and Dabo. Additionally, we propose an interactive scheme for robot's human-awareness navigation using the SFM and prediction information. Moreover, we present a new metric to evaluate the robot companion performance based on vital spaces and comfortableness criteria. Also, a multimodal human feedback is proposed to enhance the behavior of the system. The validation of the model is accomplished throughout an extensive set of simulations and real-life experiments.

The prediction of human motion intentionality is a key issue towards intelligent human robot interaction and robot navigation. In this work we present a comparative study of several prediction functions that are based on the minimum curvature variance from the current position to all the potential destination points, that means, the points that are relevant for people motion intentionality. The proposed predictor computes, at each interval of time, the trajectory from the present to the destination positions, and makes a prediction of the human motion at each interval of time using only the criterion of minimum curvature variation. The method has been validated in the Edinburgh Informatics Forum Pedestrian database.