# Extrinsic calibration for highly accurate trajectories reconstruction

Maxime Vaidis1, William Dubois1, Alexandre Guénette1, Johann Laconte1,
Vladimír Kubelka2, François Pomerleau1 1Northern Robotics Laboratory,
Université Laval, Québec City, Canada, $\\{$maxime.vaidis,
francois.pomerleau$\\}$ @norlab.ulaval.ca2Mobile Robotics and Olfaction lab of
the AASS research center at Örebro University, Sweden.

###### Abstract

In the context of robotics, accurate ground-truth positioning is the
cornerstone for the development of mapping and localization algorithms. In
outdoor environments and over long distances, total stations provide accurate
and precise measurements, that are unaffected by the usual factors that
deteriorate the accuracy of Global Navigation Satellite System (GNSS). While a
single robotic total station can track the position of a target in three
Degrees Of Freedom (DOF), three robotic total stations and three targets are
necessary to yield the full six DOF pose reference. Since it is crucial to
express the position of targets in a common coordinate frame, we present a
novel extrinsic calibration method of multiple robotic total stations with
field deployment in mind. The proposed method does not require the manual
collection of ground control points during the system setup, nor does it
require tedious synchronous measurement on each robotic total station. Based
on extensive experimental work, we compare our approach to the classical
extrinsic calibration methods used in geomatics for surveying and demonstrate
that our approach brings substantial time savings during the deployment.
Tested on more than $30\text{\,}\mathrm{k}\mathrm{m}$ of trajectories, our new
method increases the precision of the extrinsic calibration by $25\text{\,}\%$
compared to the best state-of-the-art method, which is the one taking manually
static ground control points.

##  I Introduction

In mobile robotics, obtaining reference trajectories is vital for the
development and evaluation of mapping and control algorithms [1], while being
critical to cornerstone datasets [2]. In outdoor environments, total stations
are the preferred choice to obtain high-accuracy measurements in the order of
millimeters [3]. A total station is a precision measurement instrument
equipped with optics that allow it to be precisely aimed at a given target.
Two angles (i.e., elevation and azimuth) and the range between the total
station and the target are measured. This information is sufficient to express
the position of the target in the coordinate system of the total station. They
are not affected by the factors that may otherwise inhibit the usage of the
Real Time Kinematics (RTK) GNSS localization, such as dense foliage or urban
and natural canyon environments [4]. The only major requirement is the line-
of-sight between a total station and the tracked target. In the case of a
static robotic platform with several targets attached to it, it is possible to
obtain its six DOF pose (i.e., position and orientation) as shown by [5]. This
procedure requires a series of manual measurements that capture each of these
targets. If the robotic platform moves during the measurement, a combination
of a prismatic retro-reflector (i.e., simply _prism_ in the remaining of this
article) and a Robotic Total Station (RTS) is required [6]. The term _robotic_
in RTS denotes the ability to automatically track a prism in motion. Since a
single RTS can continuously track only one prism at a time, it is necessary to
use at least three RTS and three prisms to achieve the six DOF pose tracking
of a dynamic robotic platform. In recent years, manufacturers, such as
Trimble, provide active prisms with infrared signature insuring that a RTS
will only track a given prism without being disturbed by other prisms in
proximity. This new feature allowed us to investigate novel solutions to
reconstruct trajectories of moving vehicles.

Figure 1: Setup to record reference trajectories using three robotic total
stations to track three active prisms located on a HD2 robotic platform to
reconstruct its six DOFs in a $230\text{\,}\mathrm{m}$ tunnel deployment at
Université Laval.

As shown in Figure 1, each of the three RTS is tracking its assigned prism
that is mounted on the robotic platform and outputs the position of the
associated prism expressed in its coordinate system. To obtain the pose of a
robot, there are two necessary conditions: 1) All data must be expressed in a
common coordinate frame based on an extrinsic calibration, also called
_resection_ or _free-stationing_ in the surveying field. In the literature,
there are several extrinsic calibration methods for multiple RTS based on
static Ground Control Points [7, 8]. Yet, they require a significant effort in
preparation and special equipment such as geodesic pillars to achieve the
desired accuracy. 2) The RTS measurements need to be synchronized. Contrary to
position measurements from GNSS receivers, the measurements are not executed
synchronously between multiple RTS by default since this functionality is
usually not required in surveying [9]. Therefore, temporal interpolation is
necessary to exploit the data from these different RTS to obtain the robotic
platform’s pose.

In this paper, we propose an extrinsic calibration method for three RTS while
a vehicle is in motion (i.e., without having to set up static reference points
in the environment as done in our previous work [10]). This method does not
require manual registration of additional reference points, and the same
configuration is used both for the calibration and the subsequent ground truth
positioning, drastically reducing the setup time. The method exploits the
known distances between the prisms attached to the robotic platform and only
requires the robot to be driven along a random trajectory throughout the
experimental area. We evaluate the proposed approach against existing
extrinsic calibration methods for RTS using a dataset consisting of more than
$30\text{\,}\mathrm{km}$ of indoor and outdoor trajectories. These datasets
and the code are available online to the
community.111https://github.com/norlab-ulaval/RTS_Extrinsic_Calibration

##  II Related work

First, we describe extrinsic calibration methods found in the surveying
literature for multiple-RTS setups. Then, we present works related to using
multiple RTS together. Finally, we list various robotic applications of RTS
used for acquiring reference trajectories of vehicles, and we put our work in
this context.

For all applications using multiple RTS, measurements need to be expressed in
a common coordinate frame. The process of finding appropriate transformations
is called extrinsic calibration. The most common extrinsic calibration methods
use multiple static GCPs. The minimum number of required static GCPs is two,
and this method is called _two-point resection_ in surveying [11, 7]. This
method requires the knowledge of the relative position of two GCPs with
millimeter accuracy. This requirement can be very difficult to comply with
during field deployment. Therefore, in most applications, three or more static
GCPs with unknown global coordinates are used [8]. In outdoor environments
with good GNSS coverage, the GNSS can be used to obtain the GCP coordinates
[12]. In that case, the extrinsic calibration expresses the pose of RTS in the
global frame of the GNSS. Although all of these methods with static GCPs are
accurate in the order of a few millimeters, they can take hours to be carried
out to achieve the desired accuracy [13]. To address this issue, a new
extrinsic calibration method, which dynamically captured GCPs, was implemented
by [14]. The GCPs were generated by two RTS tracking one prism carried by an
Unmanned Aerial Vehicle (UAV). Although such measurements are less accurate
than the static ones, the large number of GCPs obtained allows to compensate
for the inaccuracy and provides a five-millimeter-accurate result in two
minutes. In this paper, a new dynamic extrinsic calibration that uses multiple
prisms is presented, which does not need GCPs.

To properly analyze the results obtained by RTS, it is necessary to take into
account the different types of measurement noise. The first type of noise
originates in extrinsic calibration. The works of [15, 16] searched for the
optimal number of GCPs to minimize the uncertainty of the extrinsic
calibration. The method we propose removes the requirement of GCPs altogether
while still providing a precise extrinsic calibration. Another source of noise
is the measurement equipment itself. The contributing factors are the
alignment of the prism with respect to the line of sight of the instrument and
the type of electronic distance measurement unit inside the instrument. Errors
of two to four millimeters can occur [17]. Weather conditions also have a
significant impact on range accuracy [18]. The differences in temperature and
pressure need to be compensated as well [19]. In multiple-RTS configurations,
the temporal synchronization of the instrument clocks significantly affects
the accuracy [20]. An error of a one millisecond in the synchronization can
lead to inaccuracy of one millimeter in the resulting measured position for a
speed of $1\text{\,}\mathrm{m}\,\mathrm{s}^{-1}$. The usage of the GNSS’s
clock can mitigate this problem [9]. Moreover, RTS configurations that require
communication over large distances can benefit from the long-range radio
protocol with time synchronization [10]. Finally, the last type of noise to be
considered is interlinked with the application of tracking mobile robots. The
motion of the robotic platform can lead to outlier measurements. Kalman
filtering can be applied to the raw data to increase the precision as
demonstrated by [14]. Some applications may require interpolation of the RTS
measurements, which adds another source of errors. A simple linear
interpolation can be used to process the data and synchronize them [10].
Although not used in surveying for interpolation, Gaussian Processes are
widely used in robotics to obtain continuous trajectories of robotic platforms
and can be applied to prism trajectories [21]. In this paper, a new pre-
processing pipeline applied to the raw RTS data is introduced to increase the
precision of the proposed extrinsic calibration.

In mobile robotics, a wide variety of position-referencing systems are based
on RTS, but their use remains overall atypical. An application of these
systems is to register many robots in the same global frame before beginning
swarm exploration of extreme environments [22]. The design of these position-
referencing systems also depends on the number of DOF required, and also on
the payload capacity of the platform carrying prisms. Most of the referencing
systems use only one RTS to track the position of the robotic platform, being
a skid steered robot [23], a tracked robot [24], a tethered wheeled robot
[25], a planetary rover [26], an unmanned surface vessel [27], a UAV [28] or a
walking robot [29]. Adding a second RTS leads to a reduction in the
uncertainty of the position as shown by [30]. In the work of [31], a compost
turner with two different prisms attached to it was tracked by two RTS. This
configuration provided ground truth measurement on four DOF, namely the
position and the yaw angle. To obtain the full position and orientation
reference of a robotic platform, it is possible to use a single RTS with three
targets attached to the platform [5]. The single RTS was used to manually
measure three different prisms while the platform remained static. To the best
of our knowledge, [10] was the first proposed solution to track the full pose
of a vehicle in a continuous manner. To achieve this trajectory
reconstruction, three RTS coupled with three prisms, thus highlighting
challenges related to extrinsic calibration required to transform all RTS into
a common reference frame. In this article, we propose a new dynamic extrinsic
calibration based on the inter-prism distances. This new method can be applied
in various types of environments and does not require any GCPs, thus saving
time during the field deployments.

##  III Theory

We first present the static calibrations used in surveying. Then, we describe
our new pre-processing pipeline applied on the raw RTS data for dynamic
tracking. Finally, we introduce the new dynamic calibration we proposed.

###  III-A Static methods

Two-point resection – The two-point resection [11] is the first calibration
method used as a standard referential solution. The method requires two GCPs
with precisely known relative position. It exploits the geometry of a triangle
formed by two GCPs and the RTS to locate the RTS in the coordinate frame
defined by the GCPs. The RTS is assumed to be perfectly leveled, hence two
points are enough to compute the solution which yields four DOF that can be
directly solved, the position and the yaw angle of the RTS. To obtain accurate
results, the relative position of the two known GCPs needs to be known in the
order of millimeters or better.

Figure 2: Notation used to define the cost functions minimized with the
dynamic inter-prism calibration. The cost function minimizes the difference
between the apparent inter-prism distance $\widehat{\alpha}$,
$\widehat{\beta}$ and $\widehat{\gamma}$ and their real values. They are
computed from the interpolated prism trajectories ${}^{i}\mathcal{Q}$ given by
the raw data ${}^{i}\mathcal{P}$ being pre-processed by the pipeline. The
result of the minimization are the rigid transformations
${}^{1}_{2}\widehat{\bm{T}}$ and ${}^{1}_{3}\widehat{\bm{T}}$ between the
frame $\mathcal{F}_{1}$ of the first RTS taken as the global frame, and the
other two RTS frames $\mathcal{F}_{2}$ and $\mathcal{F}_{3}$.

Static GCPs calibration – This method requires at least three static GCPs
($n\geq 3$). A list of $n$ GCP measurements ${}^{i}\mathcal{P}$ is collected
by each RTS, where $i\in\\{1,2,3\\}$ is the index of the RTS used. If the GCP
positions are also known in a global frame $\mathcal{F}_{W}$ (i.e., typically
in GNSS coordinates), a point-to-point alignment minimization is carried out
between the global coordinates of the GCP and their local coordinates in the
RTS frames, namely $\mathcal{F}_{1}$, $\mathcal{F}_{2}$ and $\mathcal{F}_{3}$
(see Figure 2). The problem can be summarized as minimizing the point-to-point
cost function as

$\displaystyle{}^{W}_{i}{\widehat{\bm{T}}}=\operatorname*{arg\,min}_{\bm{T}}\>\sum_{k=1}^{n}(\bm{e}_{k}^{T}\bm{e}_{k})\text{,
with \hskip 0.28436pt}\bm{e}_{k}=\bm{q}_{k}-\bm{T}\bm{p}_{k}\text{,}$ ((1))

where $\bm{q}_{k}$ is the $k$th point of ${}^{W}\mathcal{P}$ representing the
measured target positions taken as reference in the frame $\mathcal{F}_{W}$,
$\bm{p}_{k}$ is the $k$th point of ${}^{i}\mathcal{P}$ representing the
positions of the targets in the RTS frame $\mathcal{F}_{i}$, and
$\bm{T}\in\mathrm{\mathbf{SE}}(3)$ is a rigid transformation matrix used
during the minimization. The result given by Eq. (1) is a rigid transformation
matrix ${}^{W}_{i}\widehat{\bm{T}}\in\mathrm{\mathbf{SE}}(3)$ between the
frame $\mathcal{F}_{W}$ and the RTS frame $\mathcal{F}_{i}$. Cartesian
coordinates of each RTS measurement can then be projected into
$\mathcal{F}_{W}$ using ${}^{W}_{1}{\widehat{\bm{T}}}$,
${}^{W}_{2}{\widehat{\bm{T}}}$ and ${}^{W}_{3}{\widehat{\bm{T}}}$. If the
global coordinates of the GCPs are not known, it is possible to use the frame
of the first RTS $\mathcal{F}_{1}$ as the origin and simply solve for
${}^{1}_{2}{\widehat{\bm{T}}}$ and ${}^{1}_{3}{\widehat{\bm{T}}}$.

###  III-B Dynamic methods

Figure 3: Pre-processing pipeline composed of four different modules applied
on the raw RTS data. The inputs ${}^{i}\mathcal{P}$ are lists of position
measurements expressed in polar coordinates defined in the respective RTS
frames. Modules $2)$, and $4)$ (solid line) are necessary to process the RTS
data. The optional modules $1)$ and $3)$ (dotted line) are used to remove the
outliers to increase the precision of the results. The output of the pre-
processing pipeline are the interpolated prism trajectories
${}^{i}\mathcal{Q}$ expressed in Cartesian coordinates. They serve as the
input to the extrinsic calibration block. The calibration computes the rigid
transformations ${}^{1}_{2}\widehat{\bm{T}}$ and ${}^{1}_{3}\widehat{\bm{T}}$
between the frame of the first RTS and the frames of the second and third RTS,
respectively.

Pre-processing pipeline – Methods for dynamic calibration use coordinates of
moving prisms typically attached to a mobile robotic platform. Therefore, it
is beneficial to pre-process the RTS measurements to limit the influence of
outliers and to interpolate the data. While the outlier removal improves the
calibration accuracy, interpolation is required when the RTS do not capture
the target coordinates synchronously. The complete pre-processing pipeline we
propose is shown in Figure 3. Inputs of the pipeline ${}^{i}\mathcal{P}$ are
lists of raw polar coordinates (i.e., elevation, azimuth, and range) measured
by the three RTS. The outputs $\mathcal{Q}$ are the filtered and interpolated
Cartesian coordinates of the targets in their respective RTS coordinate frame.
More precisely, the pipeline is composed of four distinct blocks:

####  III-B1 Outlier filter

This module removes outliers that have too high derivative values of range,
elevation, and azimuth when compared to their respective thresholds
$\tau_{r}$, $\tau_{e}$ and $\tau_{a}$. This filter also transforms the polar
coordinates into Cartesian coordinates.

####  III-B2 Split intervals

In the ideal case, each RTS would provide an uninterrupted stream of prism
position measurements. However, due to obstacles or loss of a satisfactory
lock onto the prism, the RTS pauses the output until the lock is re-
established with the required precision. In such case, this module splits the
trajectory into sub-intervals that exclude the outage. It ensures that the
time difference between two subsequent prisms positions is not greater than a
threshold $\tau_{s}$. Then, it keeps only the intersection of the sub-
intervals found, i.e., the time intervals when at least three RTS were
measuring uninterrupted. The output is a list of sub-trajectories for each
individual RTS.

####  III-B3 Filter intervals

Advance interpolation methods, such as GP, can struggle when having only
access to few supporting points. Thus, this module is applied on intervals of
valid sub-trajectories to mitigate this limitation. The filtering criterion is
their length; only intervals longer than a threshold $\tau_{l}$ are kept.

####  III-B4 Interpolation

The interpolation is necessary for obtaining synchronized prism positions, as
they are required by the Extrinsic Calibration module. In this paper, we
compare two types of interpolation: the linear interpolation and a
GP222Library _Stheno_ : https://github.com/wesselb/stheno using an exponential
quadratic kernel [32].

The Split and Interpolation modules are necessary for our calibration method.
The rest of the modules (i.e., Outlier filer and Interval filter) are optional
and increase the precision of the final results, as discussed in Section V.

Dynamic GCPs calibration – This method is based on the same idea as the Static
GCPs calibration, with the difference of having GCPs collected dynamically, as
proposed by [14]. We assume RTS are tracking dynamically the same prism, and
their raw data ${}^{i}\mathcal{P}$ are pre-processed by the pipeline presented
in Figure 3. This pipeline produces three lists of $n$ interpolated prism
positions ${}^{i}\mathcal{Q}$ captured by each RTS in its own frame
$\mathcal{F}_{i}$. We apply the same method as the static GCPs calibration
with (1), with one of the three RTS coordinates frames serving as the global
frame. The result is rigid transformations that express the data from the
other two RTS in the common global frame.

Dynamic inter-prism calibration – The final calibration method is based on the
inter-prism distances constraint. It allows three RTS to track different
prisms under the assumption that the relative positions between the prisms
stay constant during the experiment. This assumption is simple to implement as
long as the robot is large enough for three prisms to be rigidly mounted onto
the body. Raw data from RTS are also pre-processed following the pipeline of
Figure 3. Based on the list of premeasured inter-prism distances
$\Delta=\left\\{\alpha,\beta,\gamma\right\\}$ representing the distances
between the prisms mounted on the robot, we can define an optimization task
that finds the optimal rigid transformations between the RTS frames (see
Figure 2).

The optimization problem is defined as the minimization of the distance
between the vector of apparent inter-prism distances
$\widehat{\Delta}=\left\\{\widehat{\alpha},\widehat{\beta}_{,}\widehat{\gamma}\right\\}$
and their real values in $\Delta$. The vector $\widehat{\Delta}$ is obtained
from the trajectories ${}^{1}\mathcal{Q}$, ${}^{2}\mathcal{Q}$ and
${}^{3}\mathcal{Q}$, which depend on the rigid transformations in question. We
define the parameters of the rigid transformations using the Lie algebra
$\mathfrak{se}(3)$ as ${}^{W}_{i}\bm{\xi}^{\wedge}$ [21]:

$\displaystyle{}^{W}_{i}\bm{\xi}^{\wedge}=\begin{bmatrix}\bm{\rho}\\\
\bm{\phi}\end{bmatrix}^{\wedge}=\begin{bmatrix}\bm{\phi}^{\wedge}&\bm{\rho}\\\
\bm{0}^{T}&0\end{bmatrix}\,\text{with \hskip
1.42271pt}\,\bm{\rho},\bm{\phi}\in\mathbb{R}^{3},$ ((2))

where $\bm{\phi}=[0,0,\phi],\phi\in\mathbb{R}$, as the RTS are considered
perfectly leveled. The translations $\bm{\rho}$ and rotation $\bm{\phi}$ are a
vector in $\mathfrak{se}(3)$. Following this definition, we can recompute
${}^{W}_{i}\bm{T}$ by taking the exponential map of
${}^{W}_{i}\bm{\xi}^{\wedge}$. The cost function to minimize is then given by:

$^{1}_{2}\widehat{\bm{\xi}},{}^{1}_{3}\widehat{\bm{\xi}}=\operatorname*{arg\,min}_{{}^{1}_{2}\bm{\xi},{}^{1}_{3}\bm{\xi}}\frac{1}{3n}\sum_{j=1}^{n}\Biggl{[}\left(\mathinner{\\!\left\lVert{}\bm{q}^{j}_{1}-{}^{1}_{2}\bm{T}{}\bm{q}^{j}_{2}\right\rVert}-\alpha\right)^{2}+\\\
\left(\mathinner{\\!\left\lVert\bm{q}^{j}_{1}-{}^{1}_{3}\bm{T}\bm{q}^{j}_{3}\right\rVert}-\beta\right)^{2}+\left(\mathinner{\\!\left\lVert{}^{1}_{2}\bm{T}\bm{q}^{j}_{2}-{}^{1}_{3}\bm{T}\bm{q}^{j}_{3}\right\rVert}-\gamma\right)^{2}\Biggr{]}$
((3))

where $\bm{q}_{i}^{j}$ is the $j^{\text{th}}$ point in ${}^{i}\mathcal{Q}$ and
$\mathinner{\\!\left\lVert\cdot\right\rVert}$ is the standard Euclidean norm.
The minimization is performed by the iterative least squares method. The
resulting vectors ${}^{1}_{2}\widehat{\bm{\xi}}$ and
${}^{1}_{3}\widehat{\bm{\xi}}$ lead to the final rigid transformations
${}^{1}_{2}\widehat{\bm{T}}$ and ${}^{1}_{3}\widehat{\bm{T}}$. As such, this
method can get stuck in local minima, so we propose in the next paragraph an
iterative method to identify a proper initial value to provide the optimizer.

Search of prior – We developed a two-step iterative method based on the
velocity of each points of ${}^{i}\mathcal{Q}$ to find a good prior for Eq.
(3). The first step is to use a prior given by a point-to-point minimization
to minimize Eq. (3) with the points considered static, which have less
uncertainty, following a speed threshold defined as
$\tau_{v}=$1\text{\,}\mathrm{c}\mathrm{m}\,\mathrm{s}^{-1}$$. Because the
prism position are close enough to each other, their trajectories will be also
close enough by applying this prior. Subsequently, Eq. (3) is reused
iteratively following an incremental step of
$10\text{\,}\mathrm{c}\mathrm{m}\,\mathrm{s}^{-1}$ for $\tau_{v}$ applied on
the robot speed range to take more and more interpolated points for the
minimization. The rigid transformations used as priors are replaced by the
successive ones obtained by the preceding $\tau_{v}$. At the end of this first
step, the inter-prism metric is used to find the best convergence obtained.
The second step is to redo the first step with the rigid transformations
obtained by the best convergence of the first step as prior for all increment
$\tau_{v}$. At the end of this second step, the inter-prism metric is used
again to find the best convergence obtained. For both steps, an optimal
convergence is validated if at least three other convergences have similar
results (i.e., having rigid transformations whose translation and angular
differences are respectively less than $5\text{\,}\mathrm{c}\mathrm{m}$ and
$0.5\text{\,}\mathrm{d}\mathrm{e}\mathrm{g}$ in our case).

##  IV Experiments

The RTS used in our experiments were three Trimble S7 tracking three Trimble
MultiTrack Active Target MT1000 prisms. The experimental setup was similar to
the one presented by [10] with an improved communication protocol. The rate of
the measurements was increased to $2.5\text{\,}\mathrm{Hz}$ per RTS and the
radio communication protocol was optimized for long-range measurements at
$1\text{\,}\mathrm{km}$ distance. In nominal conditions, the range measurement
accuracy is $2\text{\,}\mathrm{mm}$ while the angular accuracy is
$1\text{\,}\mathrm{\SIUnitSymbolArcsecond}$. To perform the experiments, a
Clearpath Warthog mobile robot and a HD2 tracked platform from SuperDroid
Robots were used in the outdoor and indoor experiments, respectively. The
three prisms were mounted on these robotic platforms (see Figure 1 for the HD2
example). Additionally, three RTK GNSSs were installed on the Warthog mobile
robot and used to gather data.

Figure 4: Large outdoor environment on the campus of Université Laval used for
experiments. _Left_ : the setup of three RTS tracking three prisms on the
Warthog robotic platform. _Right_ : a prism mounted on one of four available
geodesic pillars to perform the two-point resection method.

In total, $15\text{\,}$ different deployments were carried out to evaluate the
precision of the calibration methods presented in Section III. The complete
dataset consists of $40\text{\,}$ experiments, summing up to
$30.4\text{\,}\mathrm{km}$ of prism trajectories. The data were collected in
two different types of environment at Université Laval, in tunnels and
outdoors, as shown respectively in Figure 1 and Figure 4. Four geodesic
pillars are located in the outdoor environment, which positions are known with
a millimeter precision and surveyed every year by a specialized team. These
pillars were used to evaluate the two-point resection method. After each
deployment, prism positions on the robotic platforms were measured by a single
RTS to compute the inter-prism distances $\Delta$. For each prism position,
ten measurements were averaged to reduce the impact of noise.

##  V Results

To evaluate our results, we use two different error measurements. The first
one is called the GCPs metric and is defined as the median distance between
corresponding points after transforming them from their original coordinate
frame to the global frame. Therefore, for $n$ GCP, the GCP metric will be the
median of the $3n$ distances between the corresponding triplets of points
measured by the RTS. The second metric called inter-prism metric is defined as
the distance between $\Delta$ and $\widehat{\Delta}$. The GCPs metric is
considered to be the more accurate one because the measurements are static
with less noise. However, the inter-prism metric evaluate the precision of the
results on the same set of data taken dynamically. Therefore, the precision
achieve by this metric is more pertinent to use for dynamic tracking of
multiple prisms.

###  V-A Sensitivity and ablation tests

To find the best parameter values for our pipeline presented in Section III-B,
we performed sensitivity tests. We evaluated the impact of the thresholds
$\tau_{a}$, $\tau_{e}$, $\tau_{s}$ and $\tau_{l}$. The range threshold
$\tau_{r}$ was excluded as it depends on the robot dynamics: $\tau_{r}$ was
set to the maximum speed of the corresponding platform (i.e.,
$2\text{\,}\mathrm{m}\,\mathrm{s}^{-1}$ for the Warthog robot and
$1\text{\,}\mathrm{m}\,\mathrm{s}^{-1}$ for the HD2 robot). The inter-prism
metric was used to compare the results obtained with the different thresholds.
These results were acquired using the static GCP calibration to avoid the bias
that would be introduced with the dynamic calibration methods which use the
processed data. Both the tunnels and the outdoor environment were used for
this analysis. The results have shown that the values of $\tau_{a}$,
$\tau_{e}$ and $\tau_{l}$ have little impact on the results. Indeed, our RTS
only produced a few outliers caused by errors in the measured angles.
Similarly, there were not too many RTS outages in the data, thus majority of
the sub-intervals are sufficiently long. Therefore, we set the values to
$\tau_{a}=$1\text{\,}\mathrm{d}\mathrm{e}\mathrm{g}\,\mathrm{s}^{-1}$$ ,
$\tau_{e}=$1\text{\,}\mathrm{d}\mathrm{e}\mathrm{g}\,\mathrm{s}^{-1}$$ and
$\tau_{l}=$6\text{\,}\mathrm{s}$$ following reasonable physical quantities
given our equipment. On the other hand, the sensitivity test for $\tau_{s}$
give us a minimum error which is reached for a value of
$\tau_{s}=$1\text{\,}\mathrm{s}$$. In our experimental setup, this translates
to cutting the trajectory if there are more than two missing data points in a
row.

Figure 5: Error resulting from the ablation tests of different modules
presented in Section III-B. The linear interpolation denoted by blue and the
GP by red. Median error are written in the middle of the box and the
Interquartile Range (IQR) on the whiskers.

After determining the pipeline parameter values, we performed ablation tests
over the pipeline modules to study their impact on the calibration accuracy.
The tests used the same experimental data and the same metric as the parameter
search. We compared the minimum necessary set of the modules $2)$ and $4)$, to
the results achieved by adding the outlier filter module $1)$ and the interval
filter module $3)$. In parallel, we compared the performance of the linear
interpolation against the GP option. The results are shown in Figure 5. First,
we observe that GP shows worse performance than the linear interpolation. The
median error is equivalent, but the IQR is approximately one to two
millimeters larger. The probable cause is the limited amount of training data
available in the sub-intervals that prevent the GP from precisely
interpolating the prism position. The ablation test shows that the outlier
filtering module positively affects the results by decreasing the median error
by $7\text{\,}\%$, and the error IQR by $15\text{\,}\%$. It also confirms the
parameter sensitivity analysis conclusions: in our dataset, the interval
filtering module does not have a high impact. It decreases the median error
and the IQR by approximately $2\text{\,}\%$. Finally, the complete pipeline
decreases the median error by $9\text{\,}\%$, and the error IQR by
$18\text{\,}\%$, compared to the unfiltered RTS data. Additionally, we further
consider only the linear interpolation in the calibration comparison.

###  V-B Calibration comparison

The results of the comparison between all presented calibration methods are
shown in Figure 6. Both the GCPs and the inter-prism metric were used for the
comparison. The two-point resection _(A)_ is less accurate than the static GCP
calibration _B_ according to both metrics. Moreover, the dynamic GCP
calibration _(C)_ is better than _(A)_ , but more noisy than the static GCP
calibration _(B)_ , as also noticed by [14]. On the other hand, the dynamic
inter-prism calibration _(D)_ result strongly depends on the metric type.
Compared to the calibration _(B)_ , the GCPs metric yields a high median error
of $12.1\text{\,}\mathrm{m}\mathrm{m}$. However, the inter-prism metric
indicates that the dynamic inter-prism calibration _D_ also decreases the
median and IQR error by $29\text{\,}\%$ and $25\text{\,}\%$ respectively,
compared to _B_. The best result in the inter-prism metric from _(D)_ is
explained by the fact that this metric is directly minimized by Eq. (3). On
the other hand, a difference of $7\text{\,}\mathrm{m}\mathrm{m}$ in
translation and $0.01\text{\,}\mathrm{d}\mathrm{e}\mathrm{g}$ for the yaw
angle was observed between the results of _(B)_ and _(D)_. Combined, these
values give the error difference for the GCPs metric. Since the noise level of
the measurements is of the order of $2\text{\,}\mathrm{m}\mathrm{m}$, these
convergence differences could be attributed to the minimization. Compared to
the inter-RTK GNSS distance measured during the outdoor experiments with the
Warthog, the results coming from the calibration methods have a better
precision as seen in Figure 6.

Figure 6: Comparison of the calibrations methods. Both the median error and
the error IQR are shown for the GCPs metric (blue) and the inter-prism metric
(red). _A_ : Two-point resection. _B_ : Static GCPs calibration. _C_ : Dynamic
GCPs calibration. _D_ : Dynamic inter-prism calibration. The upper dashed line
represent the RTK GNSS precision and the lower dashed line represent the RTS
noise level.

##  VI Conclusion

We have proposed a new dynamic extrinsic calibration method and a pipeline for
pre-processing the RTS data. This new dynamic calibration exploits the inter-
prism distance measured by a setup of three RTS during a robotic deployment to
compute the rigid transformations between the frames of different RTS.
Moreover, our new calibration method does not rely on GCPs to be measured on
the field, which saves from 20 to 45 minutes at the beginning of each robotic
deployment. Even with a higher error compared to other tested methods, the
Dynamic inter-prism calibration still produce better results than a RTK
solution in optimal condition (i.e., open sky). Additionally, the new pre-
processing pipeline increased by $18\text{\,}\%$ the precision of the results,
alongside the inter-prism calibration, which increased it by $25\text{\,}\%$
compared to the best state-of-the-art method. As field experiment can be
messy, if GCPs were forgotten or mishandled, the Dynamic inter-prism
calibration can also be used to recover the calibration of the RTS as it does
not require a different set up than the one used to reconstruct a six DOF
trajectory.

Throughout the experiments, a limiting condition to our calibration method has
been found to be long straight lines or L-shaped trajectories. The lack of
rotation in these trajectories under-constrained the minimization. Thus
causing the results to be off by more than $5\text{\,}\mathrm{c}\mathrm{m}$
for the GCP metric for the HD2’s tunnel experiments even though the millimeter
order was achieved for the inter-prism metric. Based on our experience,
another possible contributor to unsatisfactory results is the distance between
the prisms on the HD2, which was around $35\text{\,}\mathrm{c}\mathrm{m}$,
compared to the one on the Warthog which respect the minimum distance of
$80\text{\,}\mathrm{c}\mathrm{m}$ advised by Trimble. For future work, we
hypothesize that these short distances have had an impact on the experiment in
the tunnel and must be further investigated.

## Acknowledgment

We thank Christian Larouche, director of the metrology laboratory at
Université Laval, for his advises and providing us the precise coordinates of
the geodesic pillars. This research was supported by the Natural Sciences and
Engineering Research Council of Canada (NSERC) through the grant CRDPJ
527642-18 SNOW (Self-driving Navigation Optimized for Winter).

## References

  * [1] Pedro J. Sanchez-Cuevas et al.  “Fully-actuated aerial manipulator for infrastructure contact inspection: Design, modeling, localization, and control”  In _Sensors (Switzerland)_ 20.17, 2020, pp. 1–27  DOI: 10.3390/s20174708
  * [2] Michael Helmberger et al.  “The Hilti SLAM Challenge Dataset”  In _IEEE Robotics and Automation Letters_ 7.3 IEEE, 2022, pp. 7518–7525  DOI: 10.1109/LRA.2022.3183759
  * [3] Ursula Kälin, Louis Staffa, David Eugen Grimm and Axel Wendt  “Highly Accurate Pose Estimation as a Reference for Autonomous Vehicles in Near-Range Scenarios”  In _Remote Sensing_ 14.1, 2022  DOI: 10.3390/rs14010090
  * [4] V. Kubelka et al.  “Radio propagation models for differential GNSS based on dense point clouds”  In _Journal of Field Robotics_ , 2020  DOI: 10.1002/rob.21988
  * [5] François Pomerleau, Ming Liu, Francis Colas and Roland Siegwart  “Challenging data sets for point cloud registration algorithms”  In _The International Journal of Robotics Research_ 31.14, 2012, pp. 1705–1711  DOI: 10.1177/0278364912458814
  * [6] T. Cheng, M. Venugopal, J. Teizer and P.. Vela  “Performance evaluation of ultra wideband technology for construction resource location tracking in harsh environments”  In _Automation in Construction_ 20.8 Elsevier B.V., 2011, pp. 1173–1184  DOI: 10.1016/j.autcon.2011.05.001
  * [7] Akajiaku C. Chukwuocha  “Using reorientation traversing on a single-unknown station or multiple-unknown stations to solve the two-point resection (free station) problem”  In _Surveying and Land Information Science_ 77.1, 2018, pp. 45–54
  * [8] Jianguo Zhou et al.  “Automatic subway tunnel displacement monitoring using robotic total station”  In _Measurement: Journal of the International Measurement Confederation_ 151 Elsevier Ltd, 2020, pp. 107251  DOI: 10.1016/j.measurement.2019.107251
  * [9] Tomas Thalmann and Hans Neuner  “Temporal calibration and synchronization of robotic total stations for kinematic multi-sensor-systems”  In _Journal of Applied Geodesy_ 15.1, 2021, pp. 13–30  DOI: 10.1515/jag-2019-0070
  * [10] Maxime Vaidis, Philippe Giguere, Francois Pomerleau and Vladimir Kubelka  “Accurate outdoor ground truth based on total stations”  In _Proceedings - 2021 18th Conference on Robots and Vision, CRV 2021_ , 2021, pp. 1–8  DOI: 10.1109/CRV52889.2021.00012
  * [11] P. Milburn and R. Allaby  “Field-ready solution to the resection problem given two coordinated points.”  In _Canadian Agricultural Engineering_ 29.1, 1987, pp. 93–96
  * [12] M. Alizadeh-Khameneh, Anna B.O. Jensen, Milan Horemuž and Johan Vium Andersson  “Investigation of the RUFRIS method with GNSS and total station for leveling”  In _2017 International Conference on Localization and GNSS, ICL-GNSS 2017_ , 2018, pp. 1–6  DOI: 10.1109/ICL-GNSS.2017.8376251
  * [13] Wesley J. Merkle and John J. Myers  “Use of the Total Station for Serviceability Monitoring of Bridges with Limited Access in Missouri, USA”, 2004
  * [14] Di Zhang et al.  “UAV/RTS system based on MMCPF theory for fast and precise determination of position and orientation”  In _Measurement: Journal of the International Measurement Confederation_ 187.October 2021 Elsevier Ltd, 2022, pp. 110342  DOI: 10.1016/j.measurement.2021.110342
  * [15] Milan Horemuž and Johan Vium Andersson  “Analysis of the precision in free station establishment by RTK GPS”  In _Survey Review_ 43.323, 2011, pp. 679–686  DOI: 10.1179/003962611X13117748892515
  * [16] M. Amin Alizadeh-Khameneh, Milan Horemuž, Anna B.. Jensen and Johan Vium Andersson  “Optimal Vertical Placement of Total Station”  In _Journal of Surveying Engineering_ 144.3, 2018, pp. 06018001  DOI: 10.1061/(asce)su.1943-5428.0000255
  * [17] S Lackner and W Lienhart  “Impact of Prism Type and Prism Orientation on the Accuracy of Automated Total Station Measurements”  In _JISDM 2016, Vienna_ 1100, 2016, pp. 1–8
  * [18] TB. Afeni and FT. Cawood  “Slope monitoring using Total Station: What are the challenges and how should these be mitigated?”  In _South African Journal of geomatics_ 2.1, 2014, pp. 41–53
  * [19] Felipe A.C. Rodriguez, Luis A.K. Veiga and Wilson A. Soares  “Temperature Acquisition System for Real Time Application of First Velocity Correction by EDM (Electronic Distance Measurement)”  In _Geoplanning_ 8.1, 2021, pp. 61–74  DOI: 10.14710/geoplanning.8.1.61-74
  * [20] S. Mao, M. Lu, X. Shen and U. Hermann  “Multi-point concurrent tracking and surveying in construction field”, 2018
  * [21] Sean Anderson and Timothy D. Barfoot  “Full STEAM ahead: Exactly sparse Gaussian process regression for batch continuous-time trajectory estimation on SE(3)”  In _IEEE International Conference on Intelligent Robots and Systems_ 2015-Decem.3 IEEE, 2015, pp. 157–164  DOI: 10.1109/IROS.2015.7353368
  * [22] Kamak Ebadi et al.  “Present and Future of SLAM in Extreme Underground Environments”  In _arXiv preprint arXiv:2208.01787_ , 2022
  * [23] Kirk MacTavish, Michael Paton and Timothy D. Barfoot  “Selective memory: Recalling relevant experience for long-term visual localization”  In _Journal of Field Robotics_ 35.8, 2018, pp. 1265–1292  DOI: 10.1002/rob.21838
  * [24] Vladimir Kubelka et al.  “Robust Data Fusion of Multimodal Sensory Information for Mobile Robots”  In _Journal of Field Robotics_ 32.4, 2015, pp. 447–473  DOI: 10.1002/robb.21535
  * [25] P McGarey et al.  “Field Report: Developing and Deploying a Tethered Robot to Map Extremely Steep Terrain”  In _Journal of Field Robotics_ 35.8, 2018, pp. 1241–1327
  * [26] Daniel Loret de Mola Lemus, David Kohanbash, Scott Moreland and David Wettergreen  “Slope Descent using Plowing to Minimize Slip for Planetary Rovers”  In _Journal of Field Robotics_ 31.5, 2014, pp. 803–819  DOI: 10.1002/rob.21518
  * [27] G Hitz, F Pomerleau, F Colas and R Siegwart  “Relaxing the planar assumption: 3D state estimation for an autonomous surface vessel”  In _International Journal of Robotics Research_ 34.13, 2015, pp. 1604–1621
  * [28] Patrik Schmuck and Margarita Chli  “CCM-SLAM: Robust and efficient centralized collaborative monocular simultaneous localization and mapping for robotic teams”  In _Journal of Field Robotics_ 36.4, 2019, pp. 763–781  DOI: 10.1002/rob.21854
  * [29] Marko Bjelonic et al.  “Weaver: Hexapod robot for autonomous navigation on unstructured terrain”  In _Journal of Field Robotics_ 35.7, 2018, pp. 1063–1079  DOI: 10.1002/rob.21795
  * [30] Gabriel Kerekes and Volker Schwieger  “Kinematic Positioning in a Real Time Robotic Total Station Network System”, 2018, pp. 35–43
  * [31] Eva Reitbauer, Christoph Schmied and Manfred Wieser  “Autonomous navigation module for tracked compost turners”  In _2020 European Navigation Conference, ENC 2020_ , 2020, pp. 1–10  DOI: 10.23919/ENC48637.2020.9317465
  * [32] C.E Rasmussen and C.K.I Williams  “Gaussian Processes for Machine Learning”  In _MIT press_ 7.5, 2006

  *[GNSS]: Global Navigation Satellite System
  *[DOF]: Degrees Of Freedom
  *[RTK]: Real Time Kinematics
  *[RTS]: Robotic Total Station
  *[DOFs]: Degrees Of Freedom
  *[GCPs]: Ground Control Point
  *[GCP]: Ground Control Point
  *[UAV]: Unmanned Aerial Vehicle
  *[GP]: Gaussian Process
  *[GNSSs]: Global Navigation Satellite System
  *[IQR]: Interquartile Range