7.2.1 Graph theory
We first introduce definitions that allow us to model the types of problems for which we can show our localizability result. We assume that the network of agents can be modeled as a graphG= (V ∪V0, E∪E0) whereV ={1, . . . , N} represents the set of frames (agents) to localize,V0 is a set of auxiliary nodes (e.g., 3-D points with unknown location),E ⊂V ×V
represents the set of frame-to-frame measurements such that (i, j)∈ E implies that node
i∈ V can sense and communicate with nodej ∈V, andE0 ⊂V ×V0 represents the set of frame-to-point measurements between agents that need to be localized and auxiliary nodes. We denote byNi the set of frame-to-frame neighbors of agent i, that is
Ni ={j ∈V : (i, j)∈E}, (7.1)
We assume that each agent i∈ V can freely exchange data with any of the neighbors Ni
(auxiliary points inV0 are not expected to exchange data, unless they are also agents inV).
Furthermore, we make the assumption that the graph Gsatisfies the following definition.
conditions hold:
• The set of edges E is symmetric, that is, (i, j)∈ E =⇒ (j, i)∈E;
• For any edge (i, j)∈ E, there exist at least one k ∈ V0 such that the two edges
(i, k),(j, k) are in E0.
Note that this definition includes the important particular case where V ∩V0 6= ∅, and
E∩E0 6=∅; in this case, nodes inV ∩V0 correspond to agents that need to be localized, but that can also serve as an auxiliary node for other edges.
This assumption can be somewhat relaxed by simply assuming that the graph G contains a plated graph as a subgraph (although our algorithms, as presented here, might not make use of all available measurements in this case).
Definition 7.2.1 is at the core of our sufficiency results, as it partitions the vertices inGinto triples.
Definition 7.2.2. The set of triples of a plated graph G is defined as TG = {(i, j, k) ∈
V ×V ×V0:j ∈ Ni,(i, k),(j, k)∈E0}.
Each triple is ordered (i.e., the order in whichi,j, andkappear is important). Every agent can become a aware of the triples it belongs to through simple distributed mechanisms (e.g., by exchanging information about possible k with every neighbor j ∈ Ni, assuming i
is always the first index for its triplets). For convenience, we define the set Nij of frame-to-
point neighbors of two frame-to-frame neighbors i, j as
Nij ={k: (i, j, k)∈TG}. (7.2)
7.2.2 Poses, points and vectors
Let W represent an arbitrary global reference frame. For each agent i∈ V, we associate a reference frame Bi, and we define the pose (Ri, Ti)∈ SE(3)to be the rigid body transfor-
mation from Bi to W; Ri ∈SO(3) is a rotation, while Ti ∈R3 is a translation. Note that
Ti can be also interpreted as the coordinates of agentiin the world frame.
We denote as wx, and ix the coordinates of a point x expressed, respectively, in the world andi-th local frame. With this notation, a pointx∈R3 transforms under the pose(Ri, Ti)
according to
wx=R
iix+Ti. (7.3)
In this work, it will be necessary to distinguish between points and vectors. We define a vector to be the difference of two points. The coordinates for a vector in a given reference frame can be obtained by taking the difference of the coordinates of the points in that frame; it then follows from (7.3) that vectors transform across reference frames according to
wv=R
iiv, (7.4)
that is, they are not affected by the translation component.
7.3
Problem statement
Lettij denote the normalized vector between Tj and Ti, that is,
wt ij =
Tj −Ti kTj−Tik2
. (7.5)
We assume that, for every(i, j)∈ E ∪E0, agent imeasures the bearing itij of agentj in its
own reference frame, which, according to (7.4), is given by:
it
Using this relation and the definition (7.5), we obtain the following relation between the bearings associated to the two edge directions between the same pair of nodesi, j:
Riitij =−Rjjtji. (7.7)
Throughout the rest of this chapter, we make the standing assumption that the locations of the nodes inV ∪V0 are in general position. More precisely, we assume that for every triplet
(i, j, k)∈TG, the locationsTi, Tj, Tk do not belong to a common line inR3. Thanks to this
assumption, each triplet (i, j, k) ∈ TG uniquely defines a plane; let nijk denote a normal
vector to this plane.
Note thattij,tji,tik,tjk all belong to the plane of triplet(i, j, k)∈TG. Using the properties
of the cross product inR3 (denoted as ×), the normal nijk can be separately computed in
thei-th and j-th local frame as:
in ijk =RTi wnijk = (itij ×itik)/kitij ×itikk2, (7.8) jn ijk =RTjwnijk = (jtjk×jtji)/kjtjk×jtjik2, (7.9) wn ijk = (wtij ×wtik)/kwtij ×wtikk2, (7.10)
see also Fig. 7.1 for a graphical representation. It is important to notice that the ordering of the cross products in (7.8) and (7.9) is important, as they guarantee thatinijk and jnijk
are the local expression of the same world-frame vectorwnijk (i.e., they do not correspond
to opposite orientations of the triangle). Moreover, (7.8) and (7.9) can be readily and independently computed at each one of the nodes i and j, without any knowledge of the absolute or relative poses of the agents.
The goal of this work is to solve the following rotation localization problem [106]:
Problem 1. Given a set of nodes G = (V, E), and a set of local bearings {it
ij}(i,j)∈E,
Figure 7.1: Illustration of the measurements available to the nodes in a triplet. Black arrows: local reference frames at nodesiandj; cyan arrows: available bearing measurements; orange arrows: normals obtained from the bearing measurements.
common global reference frame W.
Note that the conditions for translation localizability, and the closely related concept of bearing rigidity, are well understood [132]. However, these conditions are not the same as those for rotation localizability.