Essential Manifold

EssentialManifold <: AbstractPowerManifold{ℝ}

The essential manifold is the space of the essential matrices which is represented as a quotient space of the Rotations manifold product $\mathrm{SO}(3)^2$.

Let $R_x(θ), R_y(θ), R_x(θ) \in ℝ^{x\times 3}$ denote the rotation around the $z$, $y$, and $x$ axis in $ℝ^3$, respectively, and further the groups

\[H_z = \bigl\{(R_z(θ),R_z(θ))\ \big|\ θ ∈ [-π,π) \bigr\}\]

and

\[H_π = \bigl\{ (I,I), (R_x(π), R_x(π)), (I,R_z(π)), (R_x(π), R_y(π)) \bigr\}\]

acting elementwise on the left from $\mathrm{SO}(3)^2$ (component wise).

Then the unsigned Essential manifold $\mathcal{M}_{\text{E}}$ can be identified with the quotient space

\[\mathcal{M}_{\text{E}} := (\text{SO}(3)×\text{SO}(3))/(H_z × H_π),\]

and for the signed Essential manifold $\mathcal{M}_{\text{Ǝ}}$, the quotient reads

\[\mathcal{M}_{\text{Ǝ}} := (\text{SO}(3)×\text{SO}(3))/(H_z).\]

An essential matrix is defined as

\[E = (R'_1)^T [T'_2 - T'_1]_{×} R'_2,\]

where the poses of two cameras $(R_i', T_i'), i=1,2$, are contained in the space of rigid body transformations $SE(3)$ and the operator $[⋅]_{×}\colon ℝ^3 \to \operatorname{SkewSym}(3)$ denotes the matrix representation of the cross product operator. For more details see [TD17].

Constructor

EssentialManifold(is_signed=true)

Generate the manifold of essential matrices, either the signed (is_signed=true) or unsigned (is_signed=false) variant.

exp(M::EssentialManifold, p, X)

Compute the exponential map on the EssentialManifold from p into direction X, i.e.

\[\text{exp}_p(X) =\text{exp}_g( \tilde X), \quad g \in \text(SO)(3)^2,\]

where $\tilde X$ is the horizontal lift of $X$[TD17].

log(M::EssentialManifold, p, q)

Compute the logarithmic map on the EssentialManifold M, i.e. the tangent vector, whose geodesic starting from p reaches q after time 1. Here, $p=(R_{p_1},R_{p_2})$ and $q=(R_{q_1},R_{q_2})$ are elements of $SO(3)^2$. We use that any essential matrix can, up to scale, be decomposed to

\[E = R_1^T [e_z]_{×}R_2,\]

where $(R_1,R_2)∈SO(3)^2$. Two points in $SO(3)^2$ are equivalent iff their corresponding essential matrices are equal (up to a sign flip). To compute the logarithm, we first move q to another representative of its equivalence class. For this, we find $t= t_{\text{opt}}$ for which the function

\[f(t) = f_1 + f_2, \quad f_i = \frac{1}{2} θ^2_i(t), \quad θ_i(t)=d(R_{p_i},R_z(t)R_{b_i}) \text{ for } i=1,2,\]

where $d(⋅,⋅)$ is the distance function in $SO(3)$, is minimized. Further, the group $H_z$ acting on the left on $SO(3)^2$ is defined as

\[H_z = \{(R_z(θ),R_z(θ))\colon θ \in [-π,π) \},\]

where $R_z(θ)$ is the rotation around the z axis with angle $θ$. Points in $H_z$ are denoted by $S_z$. Then, the logarithm is defined as

\[\log_p (S_z(t_{\text{opt}})q) = [\text{Log}(R_{p_i}^T R_z(t_{\text{opt}})R_{b_i})]_{i=1,2},\]

where $\text{Log}$ is the logarithm on $SO(3)$. For more details see [TD17].

check_point(M::EssentialManifold, p; kwargs...)

Check whether the matrix is a valid point on the EssentialManifold M, i.e. a 2-element array containing SO(3) matrices.

check_vector(M::EssentialManifold, p, X; kwargs... )

Check whether X is a tangent vector to manifold point p on the EssentialManifold M, i.e. X has to be a 2-element array of 3-by-3 skew-symmetric matrices.

distance(M::EssentialManifold, p, q)

Compute the Riemannian distance between the two points p and q on the EssentialManifold. This is done by computing the distance of the equivalence classes $[p]$ and $[q]$ of the points $p=(R_{p_1},R_{p_2}), q=(R_{q_1},R_{q_2}) ∈ SO(3)^2$, respectively. Two points in $SO(3)^2$ are equivalent iff their corresponding essential matrices, given by

\[E = R_1^T [e_z]_{×}R_2,\]

are equal (up to a sign flip). Using the logarithmic map, the distance is given by

\[\text{dist}([p],[q]) = \| \text{log}_{[p]} [q] \| = \| \log_p (S_z(t_{\text{opt}})q) \|,\]

where $S_z ∈ H_z = \{(R_z(θ),R_z(θ))\colon θ \in [-π,π) \}$ in which $R_z(θ)$ is the rotation around the z axis with angle $θ$ and $t_{\text{opt}}$ is the minimizer of the cost function

\[f(t) = f_1 + f_2, \quad f_i = \frac{1}{2} θ^2_i(t), \quad θ_i(t)=d(R_{p_i},R_z(t)R_{b_i}) \text{ for } i=1,2,\]

where $d(⋅,⋅)$ is the distance function in $SO(3)$ [TD17].

is_flat(::EssentialManifold)

Return false. EssentialManifold is not a flat manifold.

manifold_dimension(M::EssentialManifold{is_signed, ℝ})

Return the manifold dimension of the EssentialManifold, which is 5[TD17].

parallel_transport_to(M::EssentialManifold, p, X, q)

Compute the vector transport of the tangent vector X at p to q on the EssentialManifold M using left translation of the ambient group.

project(M::EssentialManifold, p, X)

Project the matrix X onto the tangent space

\[T_{p} \text{SO}(3)^2 = T_{\text{vp}}\text{SO}(3)^2 ⊕ T_{\text{hp}}\text{SO}(3)^2,\]

by first computing its projection onto the vertical space $T_{\text{vp}}\text{SO}(3)^2$ using vert_proj. Then the orthogonal projection of X onto the horizontal space $T_{\text{hp}}\text{SO}(3)^2$ is defined as

\[\Pi_h(X) = X - \frac{\text{vert\_proj}_p(X)}{2} \begin{bmatrix} R_1^T e_z \\ R_2^T e_z \end{bmatrix},\]

with $R_i = R_0 R'_i, i=1,2,$ where $R'_i$ is part of the pose of camera $i$ $g_i = (R'_i,T'_i) ∈ \text{SE}(3)$ and $R_0 ∈ \text{SO}(3)$ such that $R_0(T'_2-T'_1) = e_z$.

dist_min_angle_pair(p, q)

This function computes the global minimizer of the function

\[f(t) = f_1 + f_2, \quad f_i = \frac{1}{2} θ^2_i(t), \quad θ_i(t)=d(R_{p_i},R_z(t)R_{b_i}) \text{ for } i=1,2,\]

for the given values. This is done by finding the discontinuity points $t_{d_i}, i=1,2$ of its derivative and using Newton's method to minimize the function over the intervals $[t_{d_1},t_{d_2}]$ and $[t_{d_2},t_{d_1}+2π]$ separately. Then, the minimizer for which $f$ is minimal is chosen and given back together with the minimal value. For more details see Algorithm 1 in [TD17].

dist_min_angle_pair_compute_df_break(t_break, q)

This function computes the derivatives of each term $f_i, i=1,2,$ at discontinuity point t_break. For more details see [TD17].

dist_min_angle_pair_df_newton(m1, Φ1, c1, m2, Φ2, c2, t_min, t_low, t_high)

This function computes the minimizer of the function

\[f(t) = f_1 + f_2, \quad f_i = \frac{1}{2} θ^2_i(t), \quad θ_i(t)=d(R_{p_i},R_z(t)R_{b_i}) \text{ for } i=1,2,\]

in the interval $[$t_low, t_high$]$ using Newton's method. For more details see [TD17].

dist_min_angle_pair_discontinuity_distance(q)

This function computes the point $t_{\text{di}}$ for which the first derivative of

\[f(t) = f_1 + f_2, \quad f_i = \frac{1}{2} θ^2_i(t), \quad θ_i(t)=d(R_{p_i},R_z(t)R_{b_i}) \text{ for } i=1,2,\]

does not exist. This is the case for $\sin(θ_i(t_{\text{di}})) = 0$. For more details see Proposition 9 and its proof, as well as Lemma 1 in [TD17].

vert_proj(M::EssentialManifold, p, X)

Project X onto the vertical space $T_{\text{vp}}\text{SO}(3)^2$ with

\[\text{vert\_proj}_p(X) = e_z^T(R_1 X_1 + R_2 X_2),\]

where $e_z$ is the third unit vector, $X_i ∈ T_{p}\text{SO}(3)$ for $i=1,2,$ and it holds $R_i = R_0 R'_i, i=1,2,$ where $R'_i$ is part of the pose of camera $i$ $g_i = (R_i,T'_i) ∈ \text{SE}(3)$ and $R_0 ∈ \text{SO}(3)$ such that $R_0(T'_2-T'_1) = e_z$ [TD17].