I, I/5

# LEVERAGING DYNAMIC OBJECTS FOR RELATIVE LOCALIZATION CORRECTION IN A
CONNECTED AUTONOMOUS VEHICLE NETWORK

Y. Yuan1  Corresponding author M. Sester1 1 Institute of Cartography and
Geoinformatics, Leibniz Universität Hannover, Germany - (yuan,
<EMAIL_ADDRESS>
###### Abstract

High-accurate localization is crucial for the safety and reliability of
autonomous driving, especially for the information fusion of collective
perception that aims to further improve road safety by sharing information in
a communication network of Connected Autonomous Vehicles (CAV). In this
scenario, small localization errors can impose additional difficulty on fusing
the information from different CAVs. In this paper, we propose a RANSAC-based
(RANdom SAmple Consensus) method to correct the relative localization errors
between two CAVs in order to ease the information fusion among the CAVs.
Different from previous LiDAR-based localization algorithms that only take the
static environmental information into consideration, this method also
leverages the dynamic objects for localization thanks to the real-time data
sharing between CAVs. Specifically, in addition to the static objects like
poles, fences, and facades, the object centers of the detected dynamic
vehicles are also used as keypoints for the matching of two point sets. The
experiments on the synthetic dataset COMAP show that the proposed method can
greatly decrease the relative localization error between two CAVs to less than
20$\text{\,}\mathrm{c}\mathrm{m}$ as far as there are enough vehicles and
poles are correctly detected by both CAVs. Besides, our proposed method is
also highly efficient in runtime and can be used in real-time scenarios of
autonomous driving.

###### keywords:

Localization, Sensor Network, Sensor Fusion, Collective Perception, Point
Cloud, Registration.

##  1 INTRODUCTION

Localization accuracy, robustness, and reliability are important criteria for
the operation and the safety of autonomous vehicles, which need localization
accuracy in the order of decimeters ($\sim$30cm) to stay in a specific lane
(Levinson et al.,, 2007). This is not an easy task. The localization provided
by the Global Navigation Satellite System (GNSS) can be easily influenced by
weather and environmental situations such as buildings, trees, and tunnels.
Its accuracy ranges from a few meters to over 20 meters (Tan and Huang,,
2006). Solutions like Differential GPS (DGPS) can reduce the localization
errors to less than two meters or more, but they need additional
infrastructures and are not reliable all the time (Kuutti et al.,, 2018). IMU
measures accelerations and angular velocities of the vehicle body which can be
used to calculate the trajectory of the vehicle in a Dead Reckoning manner.
However, it suffers from the error drifting effect. By integrating GNSS and
IMU, (Zhang et al.,, 2012) reduced the localization error to several meters
which is less than half of the result of the standalone solution with GNSS or
IMU. Similarly, (Wahyudi et al.,, 2018) tested the localization errors of the
integration system of GPS and a low-cost IMU (MPU6050). It shows an average
error of 2.67$\text{\,}\mathrm{m}$ and 0.96
$\text{\,}\mathrm{\SIUnitSymbolDegree}$. However, they tested in an open space
where good GPS signals can be obtained. In urban areas, the performance of
this solution can be worse when not enough satellites are in sight. To reduce
the errors caused by GNSS signal interruptions, Tightly-Coupled (TC) fusion of
GNSS and IMU uses filtering techniques to improve high-rate positioning
results. The TC IMU/GNSS fusion framework proposed by (Kourabbaslou et al.,,
2019) can reduce the Root-mean-square error (RMSE) to less than
2$\text{\,}\mathrm{m}$ when at least four satellites are visible and to
8$\text{\,}\mathrm{m}$ when no satellite is available at a travel distance of
450$\text{\,}\mathrm{m}$.

To further improve the localization accuracy to the order of decimeters or
even centimeters, more reference infrastructures such as base stations with
know positions or onboard sensors such as cameras, Radars, or LiDARs are
needed.

On the one hand, Real-Time Kinematic (RTK) techniques improve the GNSS
accuracy by the GNSS correction information received from GNSS base stations.
With multiple GNSS receivers, it can reach an accuracy of centimeters.
However, because of the limited communication range and power strength of base
stations, it is hard or impossible to use this solution in some urban areas
such as tunnels, tree canopies, or multi-path effect areas that are close to
the buildings (Uradziński,, 2011). Besides, RTK-GNSS solutions also rely on
the good visibility of GNSS signals. The accuracy decreases along with the
outage duration of GNSS signals (Li et al.,, 2018).

On the other hand, methods that are only based on the onboard sensors either
match the information of consecutive frames or register the local environment
information to the global map to get the global localization. Using the first
method, (Ward and Folkesson,, 2016) integrates short-range Radar with GPS/IMU
and has reached an RMSE of 7.3$\text{\,}\mathrm{m}$ and
37.7$\text{\,}\mathrm{c}\mathrm{m}$ laterally and longitudinally,
respectively. But in the worst case, the longitudinal error has reached more
than one meter. By registering the local radar reflection of underground
features to the global underground feature map, a new method of using
Localizing Ground-Penetrating Radar (LGPR) proposed by (Cornick et al.,, 2016)
has shown a RMS error of 4$\text{\,}\mathrm{c}\mathrm{m}$ in different weather
conditions and driving speed. However, the experiments in (Ort et al.,, 2020)
of using LGPR have reported a mean total error of 0.34$\text{\,}\mathrm{m}$ in
clear weather. In rainy weather, the result has degraded to
0.77$\text{\,}\mathrm{m}$. Different from LGPR and Radar, camera and LiDAR not
only can be used for localization but also are necessary for the perception of
the autonomous driving system. Therefore, it is convenient and efficient to
use any stage of information from the perception process for localization.
This information can both be used for local matching and global registration.

An early work by (Parra et al.,, 2009) has proposed a visual odometry method
with a mono-camera to estimate the trajectory of vehicle motion. It estimates
the relative locations of the same vehicle in consecutive frames by matching
SIFT feature points with RANdom SAmple Consensus (RANSAC) algorithm. The
corrected trajectory is visually improved. However, no error metric is
reported in this work. Instead of using RANSAC, (Li et al.,, 2013) first
matches GIST (Oliva and Torralba,, 2006) descriptor representations of images
to get coarse localization, which is then utilized as an initial localization
for the fine localization step to find the nearest neighbors of SIFT (Lowe,,
1999) feature points. This work has achieved a localization error of
74$\text{\,}\mathrm{c}\mathrm{m}$. To further improve the performance of
image-based visual odometry, (Suhr et al.,, 2017) integrates GPS and IMU with
a camera sensor. It registers the detected lanes and symbolic road markings in
the images to these in the digital High-Definition (HD) map. The average
result of their experiments shows that the mean lateral error is about
0.5$\text{\,}\mathrm{m}$ and the longitudinal error about
1.2$\text{\,}\mathrm{m}$. Furthermore, driven by deep-learning (DL) based
object detection and semantic segmentation, more high semantic level of
objects or points can be regarded as landmarks for the localization.
(Nicholson et al.,, 2019) represents object-wise landmarks with 3D quadrics
which are estimated by fusing the detected objects of images from different
views. (Tian et al.,, 2021) extended this work by decoupling the quadric
parameters to improve the robustness against the observation noise. It
reported an average ellipsoid central translation error of
2.13$\text{\,}\mathrm{m}$.

In general, image-based visual odometry can reduce the localization error to
less than one meter in some specific scenarios and can offer low-cost
solutions. However, the accuracy of this method depends on the resolution of
images. Higher resolution leads to higher accuracy but also to higher
computational cost. Besides, small environmental changes such as illumination
can introduce huge domain gaps to the captured images. This can greatly
deteriorate the performance of the developed algorithms. In comparison to
cameras, LiDARs actively sense the 3D environment by projecting 3D laser beams
and collecting reflection information. The generated 3D point clouds are more
accurate and more robust against weather changes than images. Therefore, they
are more suitable for autonomous driving. Although they are currently high-
cost sensors, the prices have a downward trend as many manufacturers are
putting a big effort into decreasing the cost of LiDAR sensors.

Based on LiDAR sensors, (Hata and Wolf,, 2016) localizes vehicles by
registering the curbs and road marking detected from point clouds data into
the global feature map of the environment. It reduces the lateral and
longitudinal errors to less than 30$\text{\,}\mathrm{c}\mathrm{m}$. By the
integration of GPS/IMU, filtering algorithms and more 3D features, the
accuracy of LiDAR-based localization can be further improved. (Wei et al.,,
2020) uses 3D curb features, the intensity of road marker points as well as
the height information of the surrounding high constructions to implement such
an integration. It has achieved an accuracy of
9c$\text{\,}\mathrm{c}\mathrm{m}$ and 18$\text{\,}\mathrm{c}\mathrm{m}$ for
lateral and longitudinal directions, respectively. (Levinson and Thrun,, 2010)
further improved the robustness of the localization against the dynamic
changes of the environment by creating a probabilistic infrared intensity map
for the road surface. Their result shows a positional RMSE of
9$\text{\,}\mathrm{c}\mathrm{m}$. Similar to images, LiDAR-based localization
also has many DL-based achievements (Elbaz et al.,, 2017; Wang et al.,, 2019;
Yin et al.,, 2018), they all achieved better accuracy than the minimum
required accuracy of 30$\text{\,}\mathrm{c}\mathrm{m}$ for autonomous driving.
In summary, with better data, LiDAR-based methods can easily surpass the
performance of image-based ones.

Both image- and LiDAR-based methods discussed previously suffer from the
presence of dynamic objects such as cars, pedestrians, and the occlusions
introduced by these objects. They attempt to reduce the negative influence of
the dynamic objects either by predicting them and then removing them (Li et
al.,, 2019) or by selecting the most relevant features for localization(Lu et
al.,, 2019). Nevertheless, in a CAV network, these dynamic objects can be very
useful information for localization because they are spatially better-
distributed landmarks than most of the static features that might have
unfavorable repetitive patterns for matching or registration, such as facades,
curbs, and road markings.

With CAV networks, or in other words, Vehicle-to-Vehicle (V2V) communications,
some works (Fujii et al.,, 2011; Golestan et al.,, 2012; Altoaimy and
Mahgoub,, 2014) have achieved the accuracy of several decimeters based on the
travel properties of radio signals, such as time-of-arrival (TOA) and time-
difference-of-arrival (TDOA). However, this is still not accurate enough for
the lane-keeping standard or for fusing the perception data coming from
different vehicles that are supposed to improve the perception performance.
Such collective perception can effectively reduce occlusion and increase road
safety. However, (Wang et al.,, 2020) show that data fusion among vehicles is
very sensitive to localization errors. Especially, for object-wise fusion, a
position error of only 0.4$\text{\,}\mathrm{m}$ has led to a large performance
drop in average precision of vehicle detection from about 85% to 20%.
Therefore, high accurate localization is one of the key challenges for the
perception system of autonomous vehicles to benefit from V2V communications.

In this paper, we propose an efficient method to correct the relative
locations of vehicles in a CAV network. This correction can not only improve
the information fusion performance of collective perception but also has the
potential to improve the absolute localization by registering the fused data
that has fewer occlusions, larger coverage, and more distinguishable patterns,
to the global map. The proposed method uses the RANSAC algorithm to match the
keypoints that are extracted from the result of the perception module of the
autonomous system, specifically, the LiDAR-based object detection and semantic
segmentation. It then uses the matched consensus to calculate the relative
transformation between two vehicles.

##  2 METHOD

###  2.1 Problem formulation

In a mixed traffic scenario of collective perception which contains both
autonomous vehicles and normal vehicles, we assume an ego CAV $C_{0}$ in the
CAV networks. Other $N_{\text{cav}}$ CAVs ($C_{i}$, $i=1,...,N_{\text{cav}}$)
that are in the communication range $R_{\text{c}}$ of the ego vehicle $C_{0}$
can share information to the ego vehicle. The ground truth poses of these CAVs
are notated as $X_{i}=(x_{i},y_{i},\theta_{i})$, where $x_{i}$ and $y_{i}$ are
the coordinates in the global coordinate system and $\theta_{i}$ is the
orientation of the vehicle relative to the global x-axis. Correspondingly, we
use the symbol $\sim$ on the top of variables to represent the erroneous
observations of these variables. Therefore, the observed erroneous poses by
GPS/IMU system are noted as
$\stackrel{{\scriptstyle\sim}}{{\smash{X_{i}}\rule{0.0pt}{4.73611pt}}}=(\tilde{x_{i}},\tilde{y_{i}},\tilde{\theta_{i}})$.
All vehicles in the CAV network are collecting point cloud data which are
notated as $PC_{i}$ ($i=0,...,N_{\text{cav}}$). Based on each point cloud
$PC_{i}$, object detection, and point-wise semantic segmentation are
implemented in the perception module of the autonomous system. From this
module, we extract three kinds of point information for localization. First,
the bounding box centers of the detected vehicles $P_{\text{b}}$, then the
points of poles $P_{\text{p}}$ and the points of big planar structures
$P_{\text{f}}$, such as facades and fences. Each point in the set
$P_{\text{b}}$, $P_{\text{p}}$ and $P_{\text{f}}$ contains a vector of 2D $x$
and $y$ coordinates. We call $P_{\text{b}}$ and $P_{\text{p}}$ as anchor
points for the simplicity of later explanation because they are the keypoints
to be matched and then used for calculating the final transformation between
the coordinate systems of the CAVs in the network.

The goal of the relative localization error correction is to find the
optimized transformation parameters $\Delta x,\Delta y,\Delta\theta$ that
transform the point set
$P^{i,i}=\\{P_{\text{b}}^{i,i},P_{\text{p}}^{i,i},P_{\text{f}}^{i,i}\\}$ from
the local coordinate system $CS_{i}$ of the cooperative vehicle to the
coordinate system $CS_{0}$ of the ego vehicle so that the transformed point
set $P^{i,0}$ has the maximum nearest neighborhood consensus with the point
set of ego vehicle
$P^{0,0}=\\{P_{\text{b}}^{0,0},P_{\text{p}}^{0,0},P_{\text{f}}^{0,0}\\}$. The
superscripts of $P^{*,*}$ represents the index of vehicles and the coordinate
systems. For example, $P^{i,0}$ is the points that are observed by $C_{i}$ and
represented in $CS_{0}$. The objective function of this optimization problem
can be formulated as

$Obj=\operatorname*{argmax}_{\Delta x,\Delta
y,\Delta\theta}\sum_{k=1}^{N_{i}}\mathbb{1}(\left\|\Delta
T\cdot\stackrel{{\scriptstyle\sim}}{{\smash{T}\rule{0.0pt}{4.73611pt}}}\cdot\stackrel{{\scriptstyle\sim}}{{\smash{p_{k}}\rule{0.0pt}{4.73611pt}}}^{i,i}-\stackrel{{\scriptstyle\sim}}{{\smash{p_{k}}\rule{0.0pt}{4.73611pt}}}^{0,0}\right\|^{2}<\epsilon)$
(1)

$\stackrel{{\scriptstyle\sim}}{{\smash{T}\rule{0.0pt}{4.73611pt}}}=\begin{bmatrix}cos(\tilde{\theta_{i}}-\tilde{\theta_{0}}))&-sin(\tilde{\theta_{i}}-\tilde{\theta_{0}})&\tilde{x_{i}}-\tilde{x_{0}}\\\
sin(\tilde{\theta_{i}}-\tilde{\theta_{0}})&cos(\tilde{\theta_{i}}-\tilde{\theta_{0}})&\tilde{y_{i}}-\tilde{y_{0}}\\\
0&0&1\end{bmatrix}$ (2)

$\Delta T=\begin{bmatrix}cos(\Delta\theta)&-sin(\Delta\theta)&\Delta x\\\
sin(\Delta\theta)&cos(\Delta\theta)&\Delta y\\\ 0&0&1\end{bmatrix}$ (3)

where $\epsilon$ is the threshold for counting nearest neighbors and point
$\stackrel{{\scriptstyle\sim}}{{\smash{p_{k}}\rule{0.0pt}{4.73611pt}}}^{i,i}$
and
$\stackrel{{\scriptstyle\sim}}{{\smash{p_{k}}\rule{0.0pt}{4.73611pt}}}^{0,0}$
are the homogeneous representations of the original erroneous 2D points in set
$\stackrel{{\scriptstyle\sim}}{{\smash{P}\rule{0.0pt}{4.73611pt}}}^{i,i}$ and
$\stackrel{{\scriptstyle\sim}}{{\smash{P}\rule{0.0pt}{4.73611pt}}}^{0,0}$.
$\stackrel{{\scriptstyle\sim}}{{\smash{p_{k}}\rule{0.0pt}{4.73611pt}}}^{0,0}$
is the nearest neighbor of
$\stackrel{{\scriptstyle\sim}}{{\smash{p_{k}}\rule{0.0pt}{4.73611pt}}}^{i,0}=\Delta
T\cdot\stackrel{{\scriptstyle\sim}}{{\smash{T}\rule{0.0pt}{4.73611pt}}}\cdot\stackrel{{\scriptstyle\sim}}{{\smash{p_{k}}\rule{0.0pt}{4.73611pt}}}^{i,i}$.

###  2.2 Properties of relative errors

Since the true global location is not known, choosing the local coordinate
system of any of the two CAVs as the reference for calculating the relative
localization parameters will lead to an error propagation through a non-linear
transformation from the global to the local coordinate system. The proof of
this error propagation can be found in the Appendix. Assume the localization
error of both ego CAV $C_{0}$ and cooperative $C_{i}$ has the Gaussian
distribution of $x,y\sim N(0,\sigma_{x,y}^{2})$\text{\,}\mathrm{m}$$ and
$\theta\sim N(0,\sigma_{\theta}^{2})$\text{\,}\mathrm{\SIUnitSymbolDegree}$$.
In order to fuse the point cloud data of the ego and cooperative CAV, the
relative localization errors between the CAVs should be estimated. We
accomplish this estimation in the coordinate system of ego vehicle $CS_{0}$,
in which the three parameters $\Delta x,\Delta y,\Delta\theta$ should be
estimated so that $PC_{i}$ can be correctly transformed to $CS_{0}$ and better
match $PC_{0}$. The derivation of the error propagation from the global to the
local coordinate system can be found in the appendix A.

Taking the error of
$(-0.5$\text{\,}\mathrm{m}$,-0.5$\text{\,}\mathrm{m}$,-5$\text{\,}\mathrm{\SIUnitSymbolDegree}$)$
for $C_{0}$ and
$(0.5$\text{\,}\mathrm{m}$,0.5$\text{\,}\mathrm{m}$,5$\text{\,}\mathrm{\SIUnitSymbolDegree}$)$
for $C_{1}$ as an example, the relative errors along x and y axis are shown in
figure 1, in which the relative translation error $\Delta x,\Delta y$ is in a
range of about -6$\text{\,}\mathrm{m}$ to 6$\text{\,}\mathrm{m}$. The
magnitude of $\Delta x,\Delta y$ is positive correlated to the distance
between $C_{0}$ and $C_{i}$ and is enlarged by this distance in comparison to
the original error of 0.5$\text{\,}\mathrm{m}$. Besides, the translation
errors oscillate as the orientation of the ego vehicle changes. Different to
the translation errors, the rotation error $\Delta\theta$ keeps constant and
equals to the summation of the absolute global localization error of both
$C_{0}$ and $C_{i}$.

As a summary, the optimum relative errors $\Delta x,\Delta y,\Delta\theta$
should be searched in a large 3D space. It is time-consuming to conduct a
refined search using a method like maximum consensus (Axmann and Brenner,,
2021), although it can provide the global optimum if the maximum search space
is previously known. For the example shown in figure 1, the search space is
about $12$\text{\,}\mathrm{m}$\times 12$\text{\,}\mathrm{m}$\times
10$\text{\,}\mathrm{\SIUnitSymbolDegree}$$. If the searching resolution is set
to 0.1$\text{\,}\mathrm{m}$ and $0.1$\text{\,}\mathrm{\SIUnitSymbolDegree}$$,
the result of consensus should be counted for 1.44 ($120\times 120\times 100$)
million times. Therefore, a more efficient method using RANSAC is proposed in
this paper.

Figure 1: Minimum and Maximum relative localization error of two vehicles in x
(blue) and y (orange) axis. The initial global localization errors
$(-0.5$\text{\,}\mathrm{m}$,-0.5$\text{\,}\mathrm{m}$,-5$\text{\,}\mathrm{\SIUnitSymbolDegree}$)$
and
$(0.5$\text{\,}\mathrm{m}$,0.5$\text{\,}\mathrm{m}$,5$\text{\,}\mathrm{\SIUnitSymbolDegree}$)$
are set for the two vehicles, respectively, as an example.

###  2.3 Error correction with RANSAC

To obtain the classified points $P_{\text{b}}$, $P_{\text{p}}$ and
$P_{\text{f}}$ for localization error correction, we use the pre-trained
network that is proposed in (Yuan et al.,, 2022) to generate bounding box
predictions and the down-sampled points of poles, facades and fences. We use
all the detected bounding boxes to generate $P_{\text{b}}$ and all points of
poles for $P_{\text{p}}$. However, only a subset of $N_{f}$ points of facades
and fences are selected using Furthest Point Sampling (FPS) for $P_{\text{f}}$
because these points can be any point on the planes and provide less accurate
matches than $P_{b}$ and $P_{p}$, in which each point corresponds to the
center of a specific object. A reduction in the number of points $P_{f}$ not
only reduces the computational overhead but also makes the algorithm rely more
on $P_{b}$ and $P_{p}$ so that it is more robust against the variation of
point density in the point cloud data.

The workflow of proposed relative localization error estimation is described
in algorithm 1. Line 1 and 2 define the notations of the algorithm.
$\mathcal{A}$ and $\mathcal{B}$ are the set of the anchor points (center
points of the detected vehicles and pole points) that belong to the ego point
cloud $PC_{0}$ and the cooperative point cloud $PC_{i}$, respectively. Line 3
defines the initial values that need to be updated during the workflow. For
each point, $b$ in set $\mathcal{B}$, at most $n$ nearest neighbors are found
in set $\mathcal{A}$ with a maximum Euclidian distance of $\epsilon_{1}$,
because the correct matching points might not have the smallest distance among
the $n$ nearest neighbors.We set $n$ to two to ensure the efficiency of the
algorithm. Besides, the neighbors should have the same category label as point
$b$ as described in line 5. The two neighbors $(\alpha,\beta)$ of $b$ are then
put into set $M$ as dictionary format as shown in line 6. $\alpha$ and $\beta$
can be None if not enough neighbors are found.

Based on the neighborhood matchings in set $M$, we start our RANSAC matching
iterations from line 11. The total number of iterations is defined by the
minimum value between the predefined maximum total number of iterations
$N_{\text{ransac}}$ and the number of the combinations $C^{2}_{|M^{\prime}|}$
(line 9), where $|M^{\prime}|$ is the cardinality of $M^{\prime}$ (line 8).

Algorithm 1 Relative localization error estimation

1:

2:$\mathcal{A}=\stackrel{{\scriptstyle\sim}}{{\smash{P_{b}}\rule{0.0pt}{4.73611pt}}}^{0,0}\cup\stackrel{{\scriptstyle\sim}}{{\smash{P_{p}}\rule{0.0pt}{4.73611pt}}}^{0,0}$,
$\mathcal{B}=\stackrel{{\scriptstyle\sim}}{{\smash{P_{b}}\rule{0.0pt}{4.73611pt}}}^{i,0}\cup\stackrel{{\scriptstyle\sim}}{{\smash{P_{p}}\rule{0.0pt}{4.73611pt}}}^{i,0}$,

3:$c_{*}$ : category label (box, pole or fence/facade) of point $*$,

4:$N_{\text{cons,max}}=0$, $M=\emptyset$.

5:for each $b\in\mathcal{B}$ do

6: $(\alpha,\beta)\leftarrow NearestNeighbors(\mathcal{A},b,2)$, with
$\left\|b-\alpha\right\|^{2}<\epsilon_{1},\quad\left\|b-\beta\right\|^{2}<\epsilon_{1},\quad
c_{b}=c_{\alpha}=c_{\beta}$

7: $M\leftarrow\\{b:(\alpha,\beta)\\}\cup M$

8:end for

9:$M^{\prime}=\\{b:a\mid a\in(\alpha,\beta),b:(\alpha,\beta)\in M\\}$

10:$N^{\prime}_{\text{ransac}}=min(N_{\text{ransac}},\leavevmode\nobreak\
C^{2}_{|M^{\prime}|})$

11:for all $i=1,\dots,N^{\prime}_{\text{ransac}}$ do

12: $i1,i2=Choice(M,2)$

13: $m_{i1}={b_{i1}:(\alpha_{i1},\beta_{i1})},\quad
m_{i2}={b_{i2}:(\alpha_{i2},\beta_{i2})}$

14: $a_{i1}=Choice((\alpha_{i1},\beta_{i1}),1)$

15: $a_{i2}=Choice((\alpha_{i2},\beta_{i2}),1)$

16: $T\leftarrow CalTF((a_{i1},b_{i1}),(a_{i2},b_{i2}))$

17: $\mathcal{B}^{\prime}\leftarrow T\cdot(\mathcal{B}\leavevmode\nobreak\
\cup\stackrel{{\scriptstyle\sim}}{{\smash{P_{f}}\rule{0.0pt}{4.73611pt}}}^{i,0}),\mathcal{A}^{\prime}\leftarrow\mathcal{A}\leavevmode\nobreak\
\cup\stackrel{{\scriptstyle\sim}}{{\smash{P_{f}}\rule{0.0pt}{4.73611pt}}}^{0,0}$

18:
$N_{\text{cons}}=\sum_{k=1}^{|\mathcal{B^{\prime}}|}\mathbb{1}(\left\|b_{k}-a_{k}\right\|<\epsilon_{2})$

19: if $N_{\text{cons, max}}<N_{\text{cons}}$ then

20:
$M^{\prime}=\\{b:a\mid\left\|b-a\right\|^{2}<\epsilon_{2},a\in\mathcal{A},b\in\mathcal{B}\\}$

21: if $|M^{\prime}|\leq 2$ then

22: $T_{\text{out}}\leftarrow CalTF(\\{(a,b)\mid b:a\in M^{\prime}\\})$

23: else

24: $T_{out}=I_{3}$

25: end if

26: end if

27:end for

28:return $T_{\text{out}}$

In each iteration, two indices of the matches in set $M$ are randomly selected
(line 11). The resulting matches chosen are given in line 12. Since the two
nearest neighboring points $(\alpha,\beta)$ are found for each point $b$, only
one from these two is selected for the current iteration (line 13-14) to
calculate the relative transformation between the set $\mathcal{A}$ and
$\mathcal{B}$ (line 15). Afterwards, the full selected localization points
$\stackrel{{\scriptstyle\sim}}{{\smash{P}\rule{0.0pt}{4.73611pt}}}^{i,0}=\\{\stackrel{{\scriptstyle\sim}}{{\smash{P_{b}}\rule{0.0pt}{4.73611pt}}}^{i,0},\stackrel{{\scriptstyle\sim}}{{\smash{P_{p}}\rule{0.0pt}{4.73611pt}}}^{i,0},\stackrel{{\scriptstyle\sim}}{{\smash{P_{f}}\rule{0.0pt}{4.73611pt}}}^{i,0}\\}=\mathcal{B}\leavevmode\nobreak\
\cup\stackrel{{\scriptstyle\sim}}{{\smash{P_{f}}\rule{0.0pt}{4.73611pt}}}^{i,0}$
are transformed with the resulting transformation matrix $T$ (line 16). The
outcome $\mathcal{B}^{\prime}$ as well as the full localization point set of
ego vehicle
$\mathcal{A}^{\prime}=\stackrel{{\scriptstyle\sim}}{{\smash{P}\rule{0.0pt}{4.73611pt}}}^{0,0}=\\{\stackrel{{\scriptstyle\sim}}{{\smash{P_{b}}\rule{0.0pt}{4.73611pt}}}^{0,0},\stackrel{{\scriptstyle\sim}}{{\smash{P_{p}}\rule{0.0pt}{4.73611pt}}}^{0,0},\stackrel{{\scriptstyle\sim}}{{\smash{P_{f}}\rule{0.0pt}{4.73611pt}}}^{0,0}\\}=\mathcal{A}\leavevmode\nobreak\
\cup\stackrel{{\scriptstyle\sim}}{{\smash{P_{f}}\rule{0.0pt}{4.73611pt}}}^{0,0}$
are used for counting the consensus for this iteration of RANSAC matching
(line 17), for each point $b_{k}$ in $\mathcal{B}^{\prime}$, if it has a
nearest neighbor $a_{k}$ whose distance to $b_{k}$ is less than the threshold
$\epsilon_{2}$, the counting variable $N_{\text{cons}}$ will be increased by
1. If the counting result $N_{\text{cons}}$ is larger than the last iteration,
the matching correspondences $M^{\prime}$ of set $\mathcal{A}$ and
$\mathcal{B}$ and the refined transformation matrix $T_{\text{out}}$ should be
updated (line 18-25). Different to $T$ which is calculated only based on two
matched point pairs, $T_{\text{out}}$ uses all consensus anchor point pairs
found between $\mathcal{A}$ and $\mathcal{B}$ to calculate a refined
transformation. There are three parameters that need to be determined by
$CalTF$ with at least two point pairs. Therefore, it is possible that not
enough matchings are found for the refined error correction. In this case, we
return an identity matrix as shown in line 21.

The detailed implementation of function $CalTF$ is described in algorithm 2.
The input of the function is the $N$ matched 2D point pairs. The mean
($\overline{a},\overline{b}$) and the reduced coordinates
($a_{i}^{\prime},b_{i}^{\prime}$) of there points are first calculated
respectively (lines 2-3). Then the correlations between x- and y-coordinates
of the reduced points are calculated (lines 4-7). The rotation between the
point set $A=\\{a_{i}\mid i=1,\dots,N\\}$ and $B=\\{b_{i}\mid i=1,\dots,N\\}$
can then be calculated with the equation described in line 8. Finally, the
equation in line 9 calculates the translation between the two point sets. The
returned transformation matrix is composed from the parameter $\Delta x,\Delta
y$ and $\Delta\theta$.

Algorithm 2 Similarity transformation

1:procedure CalTF(Point pairs $M=\\{(a_{i},b_{i})\mid i=1,\dots,N\\}$)

2: $\overline{a}=\frac{\sum_{i=1}^{N}a_{i}}{N}$,
$\overline{b}=\frac{\sum_{i=1}^{N}b_{i}}{N}$

3: $a_{i}^{\prime}=a_{i}-\overline{a}$, $b_{i}^{\prime}=b_{i}-\overline{b}$

4: $S_{xx}=\sum_{i=1}^{N}(a_{i,x}^{\prime}\cdot b_{i,x}^{\prime})$

5: $S_{yy}=\sum_{i=1}^{N}(a_{i,y}^{\prime}\cdot b_{i,y}^{\prime})$

6: $S_{xy}=\sum_{i=1}^{N}(a_{i,x}^{\prime}\cdot b_{i,y}^{\prime})$

7: $S_{yx}=\sum_{i=1}^{N}(a_{i,y}^{\prime}\cdot b_{i,x}^{\prime})$

8: $\Delta\theta=arctan2(S_{xy}-S_{yx},S_{xx}+S_{yy})$

9: $[\Delta x,\Delta
y]=\overline{b}-\begin{bmatrix}cos\Delta\theta&-sin\Delta\theta\\\
sin\Delta\theta&cos\Delta\theta\end{bmatrix}\cdot\overline{a}$

10: return $T=\begin{bmatrix}cos\Delta\theta&-sin\Delta\theta&\Delta x\\\
sin\Delta\theta&cos\Delta\theta&\Delta y\\\ 0&0&1\end{bmatrix}$

11:end procedure

##  3 Experiments

###  3.1 Dataset

We use a synthetic dataset COMAP (Yuan and Sester,, 2021) that is specially
designed for collective perception with CAVs to validate the effectiveness of
our proposed method. The reasons for this are two folds: first, it is
extremely expensive to collect real data with networked CAVs; second, the
synthetic dataset can also provide 100% accurate ground truth localization
which is beneficial for the evaluation of the localization error estimation
algorithm.

COMAP contains 7788 frames of data collected by 20 CAVs and from 21 different
junctions scenarios. Since 4155 frames are used for training the network
proposed as in (Yuan et al.,, 2022), we use the other 2148 frames from the
test set to evaluate our method. In each frame, we select one ego vehicle and
at most five cooperative vehicles that are in the communication range
$R_{\text{c}}=40$\text{\,}\mathrm{m}$$ of the ego vehicle for evaluation. The
relative localization error between each pair of ego and the cooperative
vehicle is estimated using algorithm 1.

Figure 2: Sample result before (left) and after (middle and right) the
relative localization error correction

###  3.2 Experiment settings

To generate efficient number of labeled points for localization, we
empirically set $N_{f}=50$ which ensures that algorithm 1 finds the correct
consensus for $P_{f}$ with the lowest number of points. For nearest neighbor
searching, the distance threshold $\epsilon_{1}$ is set according to equation

$\epsilon_{1}=\eta\cdot
R_{\text{c}}\cdot\sigma_{r}\cdot\pi/180$\text{\,}\mathrm{\SIUnitSymbolDegree}$$
(4)

where $R_{\text{c}}$ is the communication range and $\sigma_{r}$ the standard
deviation of the relative rotation error in degree.
$R_{\text{c}}\cdot\sigma_{r}\cdot\pi/180$\text{\,}\mathrm{\SIUnitSymbolDegree}$$
is the standard deviation of the relative location error derived from
$\sigma_{r}$. The factor $\eta=2.58$ is the z-score of standard normal
distribution at the confidence level of 99%,which means that the relative
positin error caused by $\sigma_{r}$ has a confidence of 99% to be less than
$\epsilon_{1}$. This setting ensures that RANSAC has a high chance to find
correct point matchings in a limited searching range. After the rough error
correction with the estimated transformation matrix $T$, the threshold
$\epsilon_{2}$ for the second round of nearest neighbor searching is reduced
to $1m$. To investigate the influence of the number of RANSAC iterations
($N_{ransac}$) and different standard deviations for localization errors
($\sigma_{x},\sigma_{y},\sigma_{r}$) on the performance of the proposed
algorithm, we configure our experiments by varying $N_{ransac}$ from 10 to 50
with an offset of 10, and assuming the localization errors are normally
distributed and varying the standard deviation of the translation error
$\sigma_{x}$ and $\sigma_{y}$ from 0.2$\text{\,}\mathrm{m}$ to
1.0$\text{\,}\mathrm{m}$ with an offset of 0.2$\text{\,}\mathrm{m}$, and the
rotation error $\sigma_{r}$ from 2$\text{\,}\mathrm{\SIUnitSymbolDegree}$ to
10$\text{\,}\mathrm{\SIUnitSymbolDegree}$ with an offset of
2$\text{\,}\mathrm{\SIUnitSymbolDegree}$. All experiment runs are processed on
a 6-Core Intel i7-8700 CPU.

###  3.3 Evaluation metric

The Root Mean Squared Error (RMSE) is used to evaluate localization accuracy
after the error correction with the proposed algorithm. In order to have an
intuitive comparison with the originally introduced localization errors in the
global coordinate system, we take the residuals between the estimated and the
ground truth relative errors that are represented in the global coordinate
system for calculating RMSE.

However, the randomness of RANSAC might lead to failures in finding the
optimal result. Besides, it is also possible that the field-of-view (FOV) of
the two vehicles do not have sufficient overlapping detected vehicles and
poles to calculate the relative localization error between them. Therefore, we
also evaluate the rate of valid error correction results by filtering the
results with the consensus numbers $N_{\text{cons}}$. Specifically, the valid
rate is calculated with

$\gamma=\frac{\sum_{1}^{N}\mathbb{1}(N_{\text{cons}}>thr_{\text{cons}})}{N_{\text{s}}}$
(5)

where $N_{\text{s}}$ here is the total number of sample pairs of ego and
cooperative vehicles, between which the relative localization error should be
corrected, and $thr_{\text{cons}}$ is the threshold to filter the valid sample
pairs. Finally, we use the processing frames per second (FPS) to evaluate the
runtime of the proposed method.

##  4 RESULTS AND EVALUATION

In general, the proposed method can correct the relative errors to less than
20$\text{\,}\mathrm{c}\mathrm{m}$ (see figure 4) as far as there are enough
localization anchor points of the ego and cooperative vehicles can be matched
and these matches have a good spatial distribution. An example result is shown
in figure 2. All observed information of the ego vehicle is shown in blue, and
cooperative vehicle in red. The detected vehicles are notated with rotated
bounding boxes (BBoxes). The thick bars of the BBoxes indicate the front of
the vehicle. Poles are shown with stars and other small points in the left two
figures are the down-sampled points of fences and facades. Besides, all points
before down-sampling are shown in the right subplot in order to better
illustrate the performance of the error correction algorithm. As shown in the
left subplot, the large initial relative error has led to wrong matches
between the detected vehicles of the ego and the cooperative CAVs. After error
correction, they are well matched as shown in the middle subplot. The right
subplot shows that the point clouds of the two CAVs are also well aligned to
each other.

To further give some quantitative details about the result, in the following
subsections, we first show the relative error properties through error
propagation based on the dataset. Then the result of parameter studies of the
proposed method is demonstrated.

###  4.1 Error propagation

Figure 3: PDFs of errors in x-axis with initial local error of 0.4m

As discussed in section 2.3, the relative localization error between two
vehicles can be amplified along with the error propagation through the non-
linear transformation. This is also verified with the test dataset. After
adding the normally distributed error with $\sigma_{x,y}=0.4m$ and
$\sigma_{r}=4$\text{\,}\mathrm{\SIUnitSymbolDegree}$$ to the localization of
each vehicle, the standard deviation of relative localization errors is
greatly increased as shown in figure 3. The blue high peak shows that most
vehicles have a very small initial localization error in the global coordinate
system ($CS_{g}$), the addition of the errors of two vehicles is much more
flatly distributed with a bigger standard deviation (orange). After
transforming this relative error from the $CS_{g}$ to the local coordinate
system $CS_{0}$, the standard deviation of the errors increased again (green).

###  4.2 Performance on different RANSAC iterations

Figure 4: Error distributions after correction with different
$N_{\text{ransac}}$. Init: errors before correction. Figure 5: Overall RMSEs
after correction with different $N_{\text{ransac}}$.

After error correction with our method, the standard deviation of the relative
localization is greatly decreased as the example correction result of the
initial error of
$\sigma=[0.4$\text{\,}\mathrm{m}$,0.4$\text{\,}\mathrm{m}$,4$\text{\,}\mathrm{\SIUnitSymbolDegree}$]$
in figure 4 shows. The black lines indicate the initial error distributions,
other lines illustrate the error distributions after correction with a
different number of RANSAC iterations $N_{\text{ransac}}$. With all
configurations of $N_{\text{ransac}}$, the relative errors for most samples
can be reduced by a large margin, especially the rotation errors. As shown in
the right subplot of figure 4, the initial distribution of rotation is nearly
flat because of the large standard deviation of the rotation angle in degree.
However, after correction, these errors are mostly reduced to less than one
degree.

As a quantitative result, figure 5 shows the RMSEs of different
$N_{\text{ransac}}$ by averaging over all samples including the outliers. In
these outliers, either not enough matchings are found or wrong matchings are
used for calculating the final transformation. This will lead to very large
errors and therefore result in larger RMSE values. The RMSEs in figure 5 that
are averaged over all samples and initial error configurations can reflect an
overall performance on the dataset. As $N_{\text{ransac}}$ increases, RMSEs of
both translation errors (x, y, and the Euclidean norm of x and y) and rotation
errors have a decreasing trend. In the beginning, this trend is stronger than
that at the end when $N_{\text{ransac}}>30$. Therefore, 30 to 40 iterations
are the most sufficient configuration of $N_{\text{ransac}}$ for the proposed
method to correct the relative localization errors caused by the global
localization errors introduced in the experiments.

Figure 6: Top left: valid rates with different $N_{\text{RANSAC}}$ (vertical
axis), the other 3 subplots: RMSEs after correction with different initial
errors (vertical axis). Horizontal axis represents $thr_{\text{cons}}$.

Different to the overall RMSEs, the top left subplot of figure 6 shows the
valid rates based on different number of iterations (vertical axis) and the
thresholds of the number of minimum consensus $thr_{\text{cons}}$ (horizontal
axis). For all $thr_{\text{cons}}$, the valid rates are increasing as
$N_{\text{ransac}}$ increases and the improvement at smaller
$N_{\text{ransac}}$ is more significant. Besides, the valid rates are, as
expected, all decreasing in all $N_{\text{ransac}}$ configurations as
$thr_{\text{cons}}$ increases. Because larger $thr_{\text{cons}}$ would lead
to more exclusions of wrong estimation outliers.

###  4.3 Performance on different errors

Figure 7: RMSEs with initial different errors, samples filtered by
$thr_{\text{cons}}=10$

Beside $N_{\text{ransac}}$, we also plotted RMSEs ($E_{x},E_{x},E_{r}$) of
different initial localization errors with respect to the consensus thresholds
in figure 6. These plots are all evaluated with $N_{\text{ransac}}=30$ because
it is a sufficient setting regarding the RANSAC iterations and the performance
as discussed in the last section. As $thr_{\text{cons}}$ increases, the RMSEs
after correction have mostly a decreasing trend, especially for those with
larger initial errors. When the error reduces to a specific level, it reaches
its best performance and stops decreasing. For example, the RMSE in the x-axis
($E_{x}$) decreased from 0.19$\text{\,}\mathrm{m}$ to
0.15$\text{\,}\mathrm{m}$ and then to 0.09$\text{\,}\mathrm{m}$ as the
consensus threshold increased from 2 to 6, it then fluctuates in a small
range. This fluctuation effect is more apparent in the last row of the RMSE
plot of $E_{y}$ and $E_{r}$. Besides, as we enlarge the initial standard
deviation of the errors, the RMSEs after correction are dropping. This is
because our proposed method only searches correspondences in a predefined
range. Larger errors will lead to a larger searching range, and therefore also
needs more RANSAC iterations. Within limited iterations of searching, the best
matching result might not be found and this will, therefore, lead to larger
RMSEs. This effect can be better observed with the result of
$thr_{\text{cons}}=10$ in figure 7. At the smaller initial errors, the RMSEs
after error correction are all less than 20$\text{\,}\mathrm{c}\mathrm{m}$ in
the x- and y-axis and the rotation error under
0.25$\text{\,}\mathrm{\SIUnitSymbolDegree}$. The resulting lateral error in
y-axis is smaller than the longitudinal error in x-axis because most points of
poles and planar objects are regularly aligned along the longitudinal
direction which increases the difficulty for matching in this direction. As
the error increased to
$[1$\text{\,}\mathrm{m}$,1$\text{\,}\mathrm{m}$,1$\text{\,}\mathrm{\SIUnitSymbolDegree}$]$,
the RMSEs are greatly increased—the Euclidean norm of the RMSE of x and y
increased from 15$\text{\,}\mathrm{c}\mathrm{m}$ to
40$\text{\,}\mathrm{c}\mathrm{m}$, and the RMSE of rotation error from
0.25$\text{\,}\mathrm{\SIUnitSymbolDegree}$ to
0.4$\text{\,}\mathrm{\SIUnitSymbolDegree}$. In this case, a brute-force search
in all possible matchings can be used as only a limited number of detected
vehicles and poles are in the scenario. However, the benefit of RANSAC in
reducing runtime will be lost.

###  4.4 Runtime

The number of RANSAC iterations is the most correlated variable to the
runtime. As shown in the left plot of figure 8, the FPS drops as we increase
$N_{\text{ransac}}$. Besides, the dropping slope is becoming more gentle as
$N_{\text{ransac}}$ increases because $N_{\text{ransac}}$ gradually reaches
the number of all possible matches $C^{2}_{|M^{\prime}|}$ (see alg. 1) in some
samples that only have very few anchor points for matching. In the right plot,
the FPS with respect to RMSEs of the experiments with initial errors of
$\sigma=[0.4,0.4m,4$\text{\,}\mathrm{\SIUnitSymbolDegree}$]$ and
$thr_{\text{cons}}=10$ is shown. At a high FPS rate ($\sim$150), the algorithm
generates significantly worse RMSE results than the lower FPS. However, no
RMSE reduction is observed in lower FPS. In combination with the previous
discussions of valid rate, this reveals that only a small number of RANSAC
iterations can already sufficiently find correct matchings between the anchor
points of two CAVs, further increasing $N_{\text{ransac}}$ with the sacrifice
of the runtime can only improve the overall performance by increasing the
valid rate but not the by reducing the RMSE of all valid estimations which are
filtered with $thr_{\text{cons}}=10$. In other words, if more error
estimations are regarded as valid, more hard samples will be included in
computing RMSEs, which, therefore, leads to larger RMSEs as shown in the left
bottom corner of the right plot in figure 8.

Figure 8: Evaluation on runtimes.

##  5 CONCLUSION

In this paper, we proposed an efficient RANSAC-based method to correct the
relative localization error between two CAVs that can share the point-cloud-
based object detection and semantic segmentation result with each other via
CPMs. Our method takes the center points of the detected vehicles and the pole
points as anchor points for the RANSAC algorithm to find matches between the
observed anchor points of two CAVs and calculate the transformation between
the two CAVs based on these matched anchor points. In addition to the anchor
points, the points of fences and facades are also used for counting the
consensus of RANSAC algorithm and finding the best matches. The result of our
experiments on the synthetic dataset COMAP shows that the proposed method can
significantly reduce the relative localization errors. The overall performance
of the proposed method is related to the initial error, the number of RANSAC
iterations, and the consensus threshold for counting the valid error
estimations. As far as the anchor points of the two CAVs have enough overlaps
and good spatial distribution, the relative localization error can be reduced
to less than 20$\text{\,}\mathrm{c}\mathrm{m}$.

Although the proposed method can efficiently and effectively reduce the
relative localization error, it has no guarantee to find the best matches
because of the randomness of RANSAC. In the worst case, wrong matches can lead
to enlarged errors. Therefore, we plan to improve the performance by combining
our method, for example, with maximum consensus so that the rotation can be
roughly corrected first and therefore the searching range of RANSAC can be
reduced. Besides, it is also necessary and meaningful to validate the
effectiveness of our method on the real dataset which might have different
features on keypoints. For example, the difference of the real traffic pattern
might lead to different geometric distributions of the selected keypoints for
matching. Moreover, real dataset with more data noise as well as weaker
performance quality of object detection and semantic segmentation may result
in a lower quality of the detected keypoints. However, it is inauthentic to
simulate the patterns and the quality of the detected keypoints, for example,
by randomly introducing errors to the keypoints. Instead, it might be more
meaningful to test our error correction method on the real dataset and
different object detection and semantic segmentation frameworks in the future
work.

## REFERENCES

  * Altoaimy and Mahgoub, (2014) Altoaimy, L., Mahgoub, I., 2014. Fuzzy logic based localization for vehicular ad hoc networks. _2014 IEEE Symposium on Computational Intelligence in Vehicles and Transportation Systems (CIVTS)_ , 121–128.
  * Axmann and Brenner, (2021) Axmann, J., Brenner, C., 2021. Maximum Consensus Localization Using Lidar Sensors. ISPRS Annals of the Photogrammetry, Remote Sensing and Spatial Information Sciences, V-2-2021, 9–16.
  * Cornick et al., (2016) Cornick, M. T., Koechling, J., Stanley, B., Zhang, B., 2016. Localizing Ground Penetrating RADAR: A Step Toward Robust Autonomous Ground Vehicle Localization. J. Field Robotics, 33, 82-102.
  * Elbaz et al., (2017) Elbaz, G., Avraham, T., Fischer, A., 2017. 3d point cloud registration for localization using a deep neural network auto-encoder. _2017 IEEE Conference on Computer Vision and Pattern Recognition (CVPR)_ , 2472–2481.
  * Fujii et al., (2011) Fujii, S., Fujita, A., Umedu, T., Kaneda, S., Yamaguchi, H., Higashino, T., Takai, M., 2011. Cooperative vehicle positioning via v2v communications and onboard sensors. _2011 IEEE Vehicular Technology Conference (VTC Fall)_ , 1–5.
  * Golestan et al., (2012) Golestan, K., Seifzadeh, S., Kamel, M., Karray, F., Sattar, F., 2012. Vehicle localization in vanets using data fusion and v2v communication. _Proceedings of the Second ACM International Symposium on Design and Analysis of Intelligent Vehicular Networks and Applications_ , Association for Computing Machinery, 123–130.
  * Hata and Wolf, (2016) Hata, A. Y., Wolf, D. F., 2016. Feature Detection for Vehicle Localization in Urban Environments Using a Multilayer LIDAR. IEEE Transactions on Intelligent Transportation Systems, 17(2), 420-429.
  * Kourabbaslou et al., (2019) Kourabbaslou, S. S., Zhang, A., Atia, M. M., 2019. A Novel Design Framework for Tightly Coupled IMU/GNSS Sensor Fusion Using Inverse-Kinematics, Symbolic Engines, and Genetic Algorithms. IEEE Sensors Journal, 19(23), 11424-11436.
  * Kuutti et al., (2018) Kuutti, S., Fallah, S., Katsaros, K. V., Dianati, M., Mccullough, F., Mouzakitis, A., 2018. A Survey of the State-of-the-Art Localization Techniques and Their Potentials for Autonomous Vehicle Applications. IEEE Internet of Things Journal, 5, 829-846.
  * Levinson et al., (2007) Levinson, J., Montemerlo, M., Thrun, S., 2007. Map-based precision vehicle localization in urban environments. _Robotics: Science and Systems_.
  * Levinson and Thrun, (2010) Levinson, J., Thrun, S., 2010. Robust vehicle localization in urban environments using probabilistic maps. 2010 IEEE International Conference on Robotics and Automation, 4372-4378.
  * Li et al., (2013) Li, C., Dai, B., Wu, T., 2013. Vision-based precision vehicle localization in urban environments. _2013 Chinese Automation Congress_ , 599–604.
  * Li et al., (2019) Li, Q., Chen, S., Wang, C., Li, X., Wen, C., Cheng, M., Li, J., 2019. LO-Net: Deep Real-Time Lidar Odometry. 2019 IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR), 8465-8474.
  * Li et al., (2018) Li, T., Zhang, H., Gao, Z., Chen, Q., Niu, X., 2018. High-Accuracy Positioning in Urban Environments Using Single-Frequency Multi-GNSS RTK/MEMS-IMU Integration. Remote. Sens., 10, 205.
  * Lowe, (1999) Lowe, D. G., 1999. Object recognition from local scale-invariant features. Proceedings of the Seventh IEEE International Conference on Computer Vision, 2, 1150-1157 vol.2.
  * Lu et al., (2019) Lu, W., Wan, G., Zhou, Y., Fu, X., Yuan, P., Song, S., 2019. DeepVCP: An End-to-End Deep Neural Network for Point Cloud Registration. 2019 IEEE/CVF International Conference on Computer Vision (ICCV), 12-21.
  * Nicholson et al., (2019) Nicholson, L., Milford, M., Sünderhauf, N., 2019. QuadricSLAM: Dual Quadrics From Object Detections as Landmarks in Object-Oriented SLAM. IEEE Robotics and Automation Letters, 4(1), 1-8.
  * Oliva and Torralba, (2006) Oliva, A., Torralba, A., 2006. Building the gist of a scene: the role of global image features in recognition. Progress in brain research, 155, 23-36.
  * Ort et al., (2020) Ort, T., Gilitschenski, I., Rus, D., 2020. Autonomous Navigation in Inclement Weather Based on a Localizing Ground Penetrating Radar. IEEE Robotics and Automation Letters, 5(2), 3267-3274.
  * Parra et al., (2009) Parra, I., Sotelo, M. Á., Llorca, D. F., Ocaña, M., 2009. Robust visual odometry for vehicle localization in urban environments. Robotica, 28, 441 - 452.
  * Suhr et al., (2017) Suhr, J. K., Jang, J., Min, D., Jung, H. G., 2017. Sensor Fusion-Based Low-Cost Vehicle Localization System for Complex Urban Environments. IEEE Transactions on Intelligent Transportation Systems, 18(5), 1078-1086.
  * Tan and Huang, (2006) Tan, H.-S., Huang, J., 2006. DGPS-Based Vehicle-to-Vehicle Cooperative Collision Warning: Engineering Feasibility Viewpoints. IEEE Transactions on Intelligent Transportation Systems, 7(4), 415-428.
  * Tian et al., (2021) Tian, R., Zhang, Y., Feng, Y., Yang, L., Cao, Z., Coleman, S., Kerr, D., 2021. Accurate and Robust Object SLAM with 3D Quadric Landmark Reconstruction in Outdoors. IEEE Robotics and Automation Letters, 1-1.
  * Uradziński, (2011) Uradziński, M., 2011. Range Analysis of RTK Base Station in Urban Environment. FIG Working Week 2011 & 6th National Congress of ONIGT.
  * Wahyudi et al., (2018) Wahyudi, Listiyana, M. S., Sudjadi, Ngatelan, 2018. Tracking object based on gps and imu sensor. _2018 5th International Conference on Information Technology, Computer, and Electrical Engineering (ICITACEE)_ , 214–218.
  * Wang et al., (2020) Wang, T.-H., Manivasagam, S., Liang, M., Yang, B., Zeng, W., Urtasun, R., 2020. V2VNet: vehicle-to-vehicle communication for joint perception and prediction. _Computer Vision - ECCV 2020 - 16th European Conference, Proceedings, Part II_ , 12347, Springer, 605–621.
  * Wang et al., (2019) Wang, W., Saputra, M. R. U., Zhao, P., de Gusmão, P. P. B., Yang, B., Chen, C., Markham, A., Trigoni, A., 2019. DeepPCO: End-to-End Point Cloud Odometry through Deep Parallel Neural Network. 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 3248-3254.
  * Ward and Folkesson, (2016) Ward, E., Folkesson, J., 2016. Vehicle localization with low cost radar sensors. _2016 IEEE Intelligent Vehicles Symposium (IV)_ , 864–870.
  * Wei et al., (2020) Wei, P., Wang, X., Guo, Y., 2020. 3d-lidar feature based localization for autonomous vehicles. _2020 IEEE 16th International Conference on Automation Science and Engineering (CASE)_ , 288–293.
  * Yin et al., (2018) Yin, H., Tang, L., Ding, X., Wang, Y., Xiong, R., 2018. Locnet: Global localization in 3d point clouds for mobile vehicles. _2018 IEEE Intelligent Vehicles Symposium (IV)_ , 728–733.
  * Yuan et al., (2022) Yuan, Y., Cheng, H., Sester, M., 2022. Keypoints-Based Deep Feature Fusion for Cooperative Vehicle Detection of Autonomous Driving. IEEE Robotics and Automation Letters, 1-8.
  * Yuan and Sester, (2021) Yuan, Y., Sester, M., 2021. COMAP: A Synthetic Dataset for Collective Multi-Agent Perception of Autonomous Driving. The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, XLIII-B2-2021, 255–263.
  * Zhang et al., (2012) Zhang, F., Stähle, H., Chen, G., Simon, C. C. C., Buckl, C., Knoll, A., 2012. A sensor fusion approach for localization with cumulative error elimination. _2012 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI)_ , 1–6.

## APPENDIX

A: Propagation of the global absolute localization errors of ego vehicle
$C_{0}$ and cooperative vehicle $C_{i}$ to local relative error.

Assume the true localization of $C_{0}$ and $C_{1}$ is
$X_{0}=(x_{0},y_{0},\theta_{0})$ and $X_{i}=(x_{i},y_{i},\theta_{i})$,
respectively. The localization errors are $dX_{0}$, $dX_{i}$. The erroneous
localizations are $\overset{\sim}{X}_{0}=X_{0}+dX_{0}$,
$\overset{\sim}{X}_{i}=X_{i}+dX_{i}$.

$S(m,n)=\begin{bmatrix}1&0&m\\\ 0&1&n\\\ 0&0&1\end{bmatrix},\quad
R(l)=\begin{bmatrix}cos(l)&-sin(l)&0\\\ sin(l)&cos(l)&0\\\ 0&0&1\end{bmatrix}$
$\displaystyle PC^{i,0}$ $\displaystyle=R(-\theta_{0})S(-x_{0},-y_{0})\cdot
PC^{i,g}$
$\displaystyle=R(-\theta_{0})S(-x_{0},-y_{0})S(x_{i},y_{i})R(\theta_{i})\cdot
PC^{i,i}$ $\displaystyle=\Delta T\cdot\overset{\sim}{PC}^{i,0}=\Delta T\cdot
R(-\overset{\sim}{\theta_{0}})S(-\overset{\sim}{x_{0}},-\overset{\sim}{y_{0}})\cdot\overset{\sim}{PC}^{i,g}$
$\displaystyle=\Delta T\cdot
R(-\overset{\sim}{\theta_{0}})S(-\overset{\sim}{x_{0}},-\overset{\sim}{y_{0}})S(\overset{\sim}{x_{i}},\overset{\sim}{y_{i}})R(\overset{\sim}{\theta_{i}})\cdot
PC^{i,i}$ $\displaystyle\Longrightarrow$ $\displaystyle
R(-\theta_{0})S(-x_{0},-y_{0})S(x_{i},y_{i})R(\theta_{i})$
$\displaystyle=\Delta T\cdot
R(-\overset{\sim}{\theta_{0}})S(-\overset{\sim}{x_{0}},-\overset{\sim}{y_{0}})S(\overset{\sim}{x_{i}},\overset{\sim}{y_{i}})R(\overset{\sim}{\theta_{i}})$
$\displaystyle\Longrightarrow$ $\displaystyle\Delta
T=R(-\theta_{0})S(-x_{0},-y_{0})S(x_{i},y_{i})R(\theta_{i})\cdot$
$\displaystyle
R^{T}(\overset{\sim}{\theta_{i}})S^{T}(\overset{\sim}{x_{i}},\overset{\sim}{y_{i}})S^{T}(-\overset{\sim}{x_{0}},-\overset{\sim}{y_{0}})R^{T}(-\overset{\sim}{\theta_{0}})$