3. Standard Estimation Problems

3.1. Range-Only Localization

class popcor.examples.RangeOnlySqLifter(n_positions: int, n_landmarks: int, d: int, W: ndarray | None = None, level: str = 'no', variable_list: list | None = None, param_level: str = 'no')

Bases: RangeOnlyLifter

Range-only localization in 2D or 3D.

Minimizes the cost function

\[f(\theta) = \sum_{n=0}^{N-1} \sum_{k=0}^{K-1} w_{nk} (d_{nk}^2 - ||p_n - a_k||^2)^2\]

where

• \(w_{nk}\) is the weight for the nth point and kth landmark (binary for missing edges).

• \(\theta\) is the flattened vector of positions \(p_n\).

• \(d_{nk}\) is the distance measurement from point n to landmark k.

• \(a_k\) is the kth landmark.

No regularization term is present, so the problem could be split into individual points.

Two substitutions for quadratic form: - level “no”: \(z_i=||p_i||^2=x_i^2 + y_i^2\) (or 3D version). - level “quad”: \(y_i=[x_i^2, x_iy_i, y_i^2]\) (or 3D version).

See https://arxiv.org/abs/2308.05783 for details.

class popcor.examples.RangeOnlyNsqLifter(n_positions: int, n_landmarks: int, d: int, W: ndarray | None = None, level: str = 'normals', variable_list: list | None = None, param_level: str = 'no')

Bases: RangeOnlyLifter

Range-only localization in 2D or 3D.

Almost same as RangeOnlySqLifter, but we do not square the distances. We minimize

\[f(\theta) = \sum_{n=0}^{N-1} \sum_{k=0}^{K-1} w_{nk} (d_{nk} - ||p_n - a_k||)^2\]

where

• \(w_{nk}\) is the weight for the nth point and kth landmark (currently assumed binary to mark missing edges).

• \(\theta\) is the flattened vector of positions \(p_n\).

• \(d_{nk}\) is the distance measurement from point n to landmark k.

• \(a_k\) is the kth landmark.

Note that in the current implementation, there is no regularization term so the problem could be split into individual points.

We experiment with two different substitutions to turn the cost function into a quadratic form:

• level “normals” uses a reformulation that introduce normal vectors, as proposed by Halstedt et al (see below).

\[f(\theta) = \sum_{n=0}^{N-1} \sum_{k=0}^{K-1} w_{nk} || z_{nk} d_{nk} - (p_n - a_k) ||^2\]

Note that \(\theta\) is now the flattened vector of positions \(p_n\) and also normal vectors \(z_{nk}\), and we need to solve a constrained problem.

• level “simple” uses substitution \(z_{nk}=||p_n - a_k||\) (or equivalent 3D version).

3.2. Stereo-Camera Localization

class popcor.examples.Stereo2DLifter(n_landmarks: int, level: str = 'no', param_level: str = 'no', variable_list: list | None = None)

Bases: StereoLifter

Stereo-camera localization in 2D.

We minimize the following cost function:

\[f(\theta) = \sum_{j=0}^{n} (u_j - M q_j / q_j[1])^2\]

where

• \(p_j\) are known landmarks (in homogeneous coordinates),

• \(u_j\) are pixel measurements (2 elements: one pixel in left “image” and one in right “image”),

• \(q_j = T(\theta) p_j\) are the (homogeneous) coordinates of landmark j in the (unknown) camera frame, parameterized by \(T(\theta)\), and

• \(M\) is the stereo camera calibration matrix. Here, it is given by

\[\begin{split}\begin{bmatrix} f_u & c_u & \frac{b f_u}{2} \\ f_v & c_v & -\frac{b f_v}{2} \\ \end{bmatrix}\end{split}\]

where \(f_u, f_v\) are horizontal and vertical focal lengths, \(c_u,c_v\) are image center points in pixels and \(b\) is the camera baseline.

This example is treated in more detail in this paper.

class popcor.examples.Stereo3DLifter(n_landmarks: int, level: str = 'no', param_level: str = 'no', variable_list: list | None = None)

Bases: StereoLifter

Stereo-camera localization in 3D.

Analogously to Stereo2DLifter, we minimize the following cost function:

\[f(\theta) = \sum_{j=0}^{n} (u_j - M q_j / q_j[2])^2\]

where

• \(p_j\) are known landmarks (in homogeneous coordinates),

• \(u_j\) are pixel measurements (four elements: two pixel coordinates in the left image and two in the right image),

• \(q_j = T(\theta) p_j\) are the (homogeneous) coordinates of landmark j in the (unknown) camera frame, parameterized by \(T(\theta)\), and

• \(M\) is the stereo camera calibration matrix. Here, it is given by

\[\begin{split}\begin{bmatrix} f_u & 0 & c_u & \frac{b f_u}{2} \\ 0 & f_v & c_v & 0 \\ f_u & 0 & c_u & -\frac{b f_u}{2} \\ 0 & f_v & c_v & 0 \\ \end{bmatrix}\end{split}\]

where \(f_u, f_v\) are horizontal and vertical focal lengths, \(c_u,c_v\) are image center points in pixels and \(b\) is the camera baseline.

This example is treated in more detail in this paper.

3.3. Rotation Averaging

class popcor.examples.RotationLifter(level: str = 'no', param_level: str = 'no', d: int = 2, n_abs: int = 2, n_rot: int = 1, n_rel: int = 1, sparsity: str = 'chain')

Bases: StateLifter

Rotation averaging problem lifter.

We solve the following optimization problem:

\[f(\theta) = \min_{R_0, R_1, \ldots, R_N \in \mathrm{SO}^d} \sum_{i,j \in \mathcal{E}} || R_i - R_j \tilde{R}_{ij} ||_F^2 + \sum_{i=\in\mathcal{A}} || R_i - \tilde{R}_i ||_F^2\]

where \(\tilde{R}_{ij}\) are the relative measurements, \(\tilde{R}_{i}\) are the absolute measurements, and the unknowns are

\[\theta = \begin{bmatrix} R_1 & R_2 & \ldots & R_N \end{bmatrix}\]

We can alternatively replace the absolute-measurement terms by

\[|| R_i - R_w \tilde{R}_{i} ||_F^2\]

where \(R_w\) is an arbitrary world frame that we can also optimize over, transforming the solutions by \(R_w^{-1}R_i\) after to move the world frame to the origin. Using this formulation, all measurements are binary factors, which may simplify implementation.

We consider two different formulations of the problem:

• level “no” corresponds to the rank-1 version:

\[x = \begin{bmatrix} 1, \mathrm{vec}(R_1), \ldots, \mathrm{vec}(R_N) \end{bmatrix}^T\]

• level “bm” corresponds to the rank-d version (bm=Burer-Monteiro).

\[\begin{split}X = \begin{bmatrix} R_1^\top \\ \vdots \\ R_N^\top \end{bmatrix}\end{split}\]