Convex Global 3D Registration with Lagrangian Duality

The registration of 3D models by a Euclidean transformation is a fundamental task at the core of many application in computer vision. This problem is non-convex due to the presence of rotational constraints, making traditional local optimization methods prone to getting stuck in local minima. This paper addresses finding the globally optimal transformation in various 3D registration problems by a unified formulation that integrates common geometric registration modalities (namely point-to-point, point-to-line and point-to-plane). This formulation renders the optimization problem independent of both the number and nature of the correspondences. The main novelty of our proposal is the introduction of a strengthened Lagrangian dual relaxation for this problem, which surpasses previous similar approaches [32] in effectiveness. In fact, even though with no theoretical guarantees, exhaustive empirical evaluation in both synthetic and real experiments always resulted on a tight relaxation that allowed to recover a guaranteed globally optimal solution by exploiting duality theory. Thus, our approach allows for effectively solving the 3D registration with global optimality guarantees while running at a fraction of the time for the state-of-the-art alternative [34], based on a more computationally intensive Branch and Bound method.Universidad de Málaga. Campus de Excelencia Internacional Andalucía Tech

Implementation, analysis and comparison of path planners based on generation of random point trees

The purpose of this work is to implement, analyze and compare two different route planning algorithms in three different static environments which include: avoiding a single major obstacle, solving a navigation problem and finally going through narrow passages. The algorithms used are sampling-based algorithms, in particular they are bidirectional RRTs. They have been studied in order to find the best performance in terms of computational time and length of the final path

A convex relaxation for approximate global optimization in simultaneous localization and mapping

Modern approaches to simultaneous localization and mapping (SLAM) formulate the inference problem as a high-dimensional but sparse nonconvex M-estimation, and then apply general first- or second-order smooth optimization methods to recover a local minimizer of the objective function. The performance of any such approach depends crucially upon initializing the optimization algorithm near a good solution for the inference problem, a condition that is often difficult or impossible to guarantee in practice. To address this limitation, in this paper we present a formulation of the SLAM M-estimation with the property that, by expanding the feasible set of the estimation program, we obtain a convex relaxation whose solution approximates the globally optimal solution of the SLAM inference problem and can be recovered using a smooth optimization method initialized at any feasible point. Our formulation thus provides a means to obtain a high-quality solution to the SLAM problem without requiring high-quality initialization.Google (Firm) (Software Engineering Internship)United States. Office of Naval Research (Grants N00014-10-1-0936, N00014-11-1-0688 and N00014- 13-1-0588)National Science Foundation (U.S.) (Award IIS-1318392

The Relationship between “C-Space”, “Heuristic Methods”, and “Sampling Based Planner”

Defining the collision-free C-space is crucial in robotics to find whether a robot can successfully perform a motion. However, the complexity of defining this space increases according to the robot’s degree of freedom and the number of obstacles. Heuristics techniques, such as Monte Carlo’s simulation, help developers address this problem and speed up the whole process. Many well-known motion planning algorithms, such as RRT, base their popularity on their ability to find sufficiently good representations of the collision-free C-space very quickly by exploiting heuristics methods, but this mathematical relationship is not highlighted in most textbooks and publications. Each book focuses the attention of the reader on C-space at the beginning, but this concept is left behind page after page. Moreover, even though heuristics methods are widely used to boost algorithms, they are never formalized as part of the Optimization techniques subject. The major goal of this chapter is to highlight the mathematical and intuitive relationship between C-space, heuristic methods, and sampling based planner

Accelerating Motion Planning via Optimal Transport

Motion planning is still an open problem for many disciplines, e.g., robotics, autonomous driving, due to their need for high computational resources that hinder real-time, efficient decision-making. A class of methods striving to provide smooth solutions is gradient-based trajectory optimization. However, those methods usually suffer from bad local minima, while for many settings, they may be inapplicable due to the absence of easy-to-access gradients of the optimization objectives. In response to these issues, we introduce Motion Planning via Optimal Transport (MPOT) -- a \textit{gradient-free} method that optimizes a batch of smooth trajectories over highly nonlinear costs, even for high-dimensional tasks, while imposing smoothness through a Gaussian Process dynamics prior via the planning-as-inference perspective. To facilitate batch trajectory optimization, we introduce an original zero-order and highly-parallelizable update rule: the Sinkhorn Step, which uses the regular polytope family for its search directions. Each regular polytope, centered on trajectory waypoints, serves as a local cost-probing neighborhood, acting as a \textit{trust region} where the Sinkhorn Step "transports" local waypoints toward low-cost regions. We theoretically show that Sinkhorn Step guides the optimizing parameters toward local minima regions of non-convex objective functions. We then show the efficiency of MPOT in a range of problems from low-dimensional point-mass navigation to high-dimensional whole-body robot motion planning, evincing its superiority compared to popular motion planners, paving the way for new applications of optimal transport in motion planning.Comment: Published as a conference paper at NeurIPS 2023. Project website: https://sites.google.com/view/sinkhorn-step

Continuous Multiclass Labeling Approaches and Algorithms

We study convex relaxations of the image labeling problem on a continuous domain with regularizers based on metric interaction potentials. The generic framework ensures existence of minimizers and covers a wide range of relaxations of the originally combinatorial problem. We focus on two specific relaxations that differ in flexibility and simplicity -- one can be used to tightly relax any metric interaction potential, while the other one only covers Euclidean metrics but requires less computational effort. For solving the nonsmooth discretized problem, we propose a globally convergent Douglas-Rachford scheme, and show that a sequence of dual iterates can be recovered in order to provide a posteriori optimality bounds. In a quantitative comparison to two other first-order methods, the approach shows competitive performance on synthetical and real-world images. By combining the method with an improved binarization technique for nonstandard potentials, we were able to routinely recover discrete solutions within 1%--5% of the global optimum for the combinatorial image labeling problem