Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Certifiably Optimal Estimation and Calibration in Robotics via Trace-Constrained Semi-Definite Programming

Liangting Wu, Roberto Tron

TL;DR

This work introduces Trace-Constrained SDP (TCSDP) as a convex-relaxation framework for nonconvex robotics problems, leveraging fixed-trace PSD embeddings to connect rank-1 recovery with optimality guarantees. It provides specialized embeddings for rotations in $\mathrm{SO}(3)$ and translations in $\mathbb{R}\times\mathcal{S}^2$, together with a modular SP robot abstraction that recasts estimation and calibration tasks (e.g., Perspective-n-Point, hand–eye, and dual-robot calibration) into TCSDPs amenable to gradient-based refinement and dual-certification. The paper develops rank-minimization, low-rank-channel, and scheduling strategies to produce low-cost, rank-1 solutions and presents proofs of exact recoverability under rank-1, enabling recovery of original manifold variables from fixed-trace matrices. Simulation results demonstrate accurate pose estimation under noise and certify near-zero duality gaps, highlighting the practical impact for certifiable robotics estimation and calibration across multiple problem classes.

Abstract

Many nonconvex problems in robotics can be relaxed into convex formulations via Semi-Definite Programming (SDP) that can be solved to global optimality. The practical quality of these solutions, however, critically depends on rounding them to rank-1 matrices, a condition that can be challenging to achieve. In this work, we focus on trace-constrained SDPs (TCSDPs), where the decision variables are Positive Semi-Definite (PSD) matrices with fixed trace values. We show that the latter can be used to design a gradient-based refinement procedure that projects relaxed SDP solutions toward rank-1, low-cost candidates. We also provide fixed-trace SDP relaxations for common robotic quantities, such as rotations and translations, and a modular virtual robot abstraction that simplifies modeling across different problem settings. We demonstrate that our trace-constrained SDP framework can be applied to many robotics tasks, and we showcase its effectiveness through simulations in Perspective-n-Point (PnP) estimation, hand-eye calibration, and dual-robot system calibration.

Certifiably Optimal Estimation and Calibration in Robotics via Trace-Constrained Semi-Definite Programming

TL;DR

This work introduces Trace-Constrained SDP (TCSDP) as a convex-relaxation framework for nonconvex robotics problems, leveraging fixed-trace PSD embeddings to connect rank-1 recovery with optimality guarantees. It provides specialized embeddings for rotations in and translations in , together with a modular SP robot abstraction that recasts estimation and calibration tasks (e.g., Perspective-n-Point, hand–eye, and dual-robot calibration) into TCSDPs amenable to gradient-based refinement and dual-certification. The paper develops rank-minimization, low-rank-channel, and scheduling strategies to produce low-cost, rank-1 solutions and presents proofs of exact recoverability under rank-1, enabling recovery of original manifold variables from fixed-trace matrices. Simulation results demonstrate accurate pose estimation under noise and certify near-zero duality gaps, highlighting the practical impact for certifiable robotics estimation and calibration across multiple problem classes.

Abstract

Many nonconvex problems in robotics can be relaxed into convex formulations via Semi-Definite Programming (SDP) that can be solved to global optimality. The practical quality of these solutions, however, critically depends on rounding them to rank-1 matrices, a condition that can be challenging to achieve. In this work, we focus on trace-constrained SDPs (TCSDPs), where the decision variables are Positive Semi-Definite (PSD) matrices with fixed trace values. We show that the latter can be used to design a gradient-based refinement procedure that projects relaxed SDP solutions toward rank-1, low-cost candidates. We also provide fixed-trace SDP relaxations for common robotic quantities, such as rotations and translations, and a modular virtual robot abstraction that simplifies modeling across different problem settings. We demonstrate that our trace-constrained SDP framework can be applied to many robotics tasks, and we showcase its effectiveness through simulations in Perspective-n-Point (PnP) estimation, hand-eye calibration, and dual-robot system calibration.

Paper Structure

This paper contains 30 sections, 11 theorems, 72 equations, 4 figures, 4 tables, 1 algorithm.

Table of Contents

  1. Introduction
  2. Preliminaries
  3. Notation
  4. SDP Relaxations
  5. Rank and Cost Optimization of TCSDP
  6. A gradient-based approach for rank minimization
  7. Low-rank channel
  8. Trace-Constrained SDP Relaxations
  9. Relaxation of $\mathrm{SO}(3)$
  10. Relaxation of a subset of $\mathbb{R}^{}{}\times\mathcal{S}^2$
  11. Applications
  12. Rigid translation using forward kinematics of a virtual kinematic chain
  13. Perspective-n-Point problem
  14. Hand-eye calibration
  15. Simultaneous calibration of dual-robot systems
  16. ...and 15 more sections

Key Result

Proposition 1

Given $\gamma\in(0,1)$ and a $\mathbf{Y}^{k-1}$ that satisfies $\lambda_1(\mathbf{Y}^{k-1})\geq \gamma\bar{\lambda}_s$, for an optimal solution $\Delta\mathbf{Y}^k$ of Problem prob:low_rank_channel, we have

Figures (4)

  • Figure 1: Some estimation and calibration problems in robotics can be formulated as kinematics problems of virtual robots.
  • Figure 2: For any two reference frames $\mathsf{b_1}$ and $\mathsf{b_2}$, the rigid translation ${}^{w}{\mathbf{t}}_{b_2}$ can be represented using the forward kinematics of an SP robot.
  • Figure 3: The consistency of transformations between reference frames with unknown absolute poses, $\mathbf{T}_{1}^{-1}\mathbf{T}_{2} = \mathbf{T}_{1'}^{-1}\mathbf{T}_{2'}$, can be enforced through constraints on $\hat{\mathbf{R}}_1$, $\hat{\mathbf{R}}_2$, $\hat{\mathbf{R}}_{1'}$, $\hat{\mathbf{R}}_{2'}$, $\tau$, $\tau'$, $\mathbf{v}$, and $\mathbf{v}'$.
  • Figure 4: Over the iterations of Algorithm \ref{['alg:solve_sdp']}, the rank is minimized in the rank minimization and scheduling phases, while the cost is reduced during the scheduling and the low-rank channel phases.

Theorems & Definitions (33)

  • Proposition 1
  • proof
  • Definition 1
  • Theorem 1
  • Definition 2
  • Theorem 2
  • proof
  • Proposition 2
  • proof
  • Proposition 3
  • ...and 23 more