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_z(θ) \in ℝ^{3×3}$ denote the rotation around the $x$, $y$, and $z$ 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 → \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].

isapprox(M::EssentialManifold, p, q; kwargs...)

Test whether two points p, q on the EssentialManifold M are (approximately) the same. Since this is a quotient manifold, the test is performed by checking their distance, that is $d_{\mathcal M}(p,q) \approx 0$ with the classical isapprox. The kwargs... are passed on to this; atol defaults to 18 times the machine epsilon of the entries of q.

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].

rand(M::EssentialManifold; vector_at = nothing, σ::Real = 1.0)

When vector_at is nothing, return a random point on the EssentialManifold M, a random point on each of the two Rotations.

When vector_at is a point $p = (R_1, R_2)$, draw a random tangent vector $X_i$ at $R_i$ on the Rotations with standard deviation σ for $i = 1, 2$ and return the projection project(M, p, (R_1X_1, R_2X_2)) of $X = (X_1, X_2)$ onto the horizontal space,

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

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].

get_basis(M::EssentialManifold, p, ::DefaultOrthonormalBasis)

Return the five orthonormal basis vectors of the horizontal space at p that get_coordinates on the EssentialManifold uses.

get_coordinates(M::EssentialManifold, p, X, ::DefaultOrthonormalBasis)
get_coordinates!(M::EssentialManifold, c, p, X, ::DefaultOrthonormalBasis)

Compute the five coordinates of the horizontal tangent vector X at p $= (R_1, R_2)$ in an orthonormal basis of the horizontal space.

The vertical space is spanned by the velocity of the action of $H_z$ at $θ = 0$, which in the coordinates of the two rotations reads $(R_1^{\mathrm{T}}e_z, R_2^{\mathrm{T}}e_z)$, see vert_proj and [TD17]. The Householder reflection that maps this direction to the last of the six coordinates maps the first five coordinate directions to an orthonormal basis of its orthogonal complement, the horizontal space.

get_vector(M::EssentialManifold, p, c, ::DefaultOrthonormalBasis)
get_vector!(M::EssentialManifold, Y, p, c, ::DefaultOrthonormalBasis)

Compute the horizontal tangent vector at p with the five coordinates c in the basis that get_coordinates on the EssentialManifold uses.

is_flat(::EssentialManifold)

Return false. EssentialManifold is not a flat manifold.

manifold_dimension(M::EssentialManifold)

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].