# Intensity Image-based LiDAR Fiducial Marker System

Yibo Liu, Hunter Schofield, Jinjun Shan This work was supported in part by
NSERC Alliance Program under Grant ALLRP 555847-20, and in part by Mitacs
Accelerate Program under Grant IT26108. The authors are with Department of
Earth and Space Science and Engineering, York University, Toronto, Ontario M3J
1P3, Canada<EMAIL_ADDRESS>
###### Abstract

The fiducial marker system for LiDAR is crucial for the robotic application
but it is still rare to date. In this paper, an Intensity Image-based LiDAR
Fiducial Marker (IILFM) system is developed. This system only requires an
unstructured point cloud with intensity as the input and it has no restriction
on marker placement and shape. A marker detection method that locates the
predefined 3D fiducials in the point cloud through the intensity image is
introduced. Then, an approach that utilizes the detected 3D fiducials to
estimate the LiDAR 6-DOF pose that describes the transmission from the world
coordinate system to the LiDAR coordinate system is developed. Moreover, all
these processes run in real-time (approx 40 Hz on Livox Mid-40 and approx 143
Hz on VLP-16). Qualitative and quantitative experiments are conducted to
demonstrate that the proposed system has similar convenience and accuracy as
the conventional visual fiducial marker system. The codes and results are
available at: https://github.com/York-SDCNLab/IILFM.

##  I INTRODUCTION

Cameras and LiDARs are two types of vital sensors found in autonomous driving
and robotic applications [1, 2]. Research on the Visual Fiducial Marker (VFM)
system, the fiducial marker system developed for the camera, has a long
history [3] and lavish experience and research achievements have been
accumulated [4, 5, 6, 7, 8]. The VFM provides the environment with
controllable artificial features such that the feature extraction and matching
become simpler and more reliable. The VFM systems have been applied in
augmented reality [9], human-robot interaction [4, 5], multi-sensor
calibration [10, 11] and Simultaneous Localization and Mapping (SLAM) [12,
13].

Development of a LiDAR fiducial marker system also plays a key role in LiDAR
applications since feature extraction and matching are fundamental
requirements in LiDAR applications [11, 14, 15]. Moreover, as introduced in
[16], the SLAM framework that utilizes the artificial features provided by the
LiDAR fiducial marker becomes feasible, which will be an interesting new
robotic application. In contrast with the VFM system, the fiducial marker
system for LiDAR is rare. To the best of our knowledge, up to date the only
fiducial marker system for LiDAR that has similar functionality as the VFM is
LiDARTag [16]. However, LiDARTag has a spatial restriction on marker placement
on account of the fact that it employs conventional single-linkage clustering
algorithm to find the clustering of the marker in the point cloud [16].
Consequently, to satisfy the spatial restriction on marker placement, it is
required to add an extra 3D object to the environment when adding a LiDARTag.
This negatively affects the spatial environment.

Figure 1: The proposed system has ample flexibility. Different visual fiducial
marker systems (Apriltag [6], ArUco [7] and CCTag [8], etc.) can be easily
embedded; There is no restriction on marker placement; The system is
applicable for both solid-state LiDAR (a, b) and mechanical LiDAR (c). An
augmented reality demo using the proposed system is shown in (d): the teapot
point cloud is transmitted to the location of the marker in the LiDAR’s point
cloud based on the pose provided by the IILFM system. Figure 2: An overview of
the proposed Intensity Image-based LiDAR Fiducial Marker detection and pose
estimation pipeline. The 3D point cloud is first transformed into an intensity
image by spherical projection. Then the embedded VFM detector is adopted on
the preprocessed intensity image. The detected 2D features are then projected
back to 3D space. Finally, the pose of the LiDAR is estimated by optimally
aligning the point set of the predefined 3D features and the point set of the
detected 3D features.

Reviewing the VFM systems, it is found that there are no restrictions on
marker placement [6, 7, 8]. Namely, the marker can be placed anywhere without
affecting the environment in terms of space. According to [16], the
abandonment of this free-placement virtue is owed to the gap between the
structured image and unstructured point cloud. Nevertheless, this gap is not
insurmountable. In particular, there is a notable hot trend in LiDAR-based 3D
object detection/segmentation [17, 18]: Neural Networks that are originally
developed for 2D object detection/segmentation can be utilized to
detect/segment the objects in the range/intensity image(s) of the 3D LiDAR
point cloud. This indicates that the range/intensity image(s) generated from
the LiDAR point cloud can be a pathway to transfer the research
accomplishments on the 2D image to the 3D point cloud. Following this
inspiration and also considering that the black-and-white marker is explicitly
visible in the point cloud rendered by the intensity (See Fig. 2), we propose
an Intensity Image-based LiDAR Fiducial Marker (IILFM) system in this paper.
The ample flexibility of the IILFM system is illustrated in Fig. 1 and the
overview pipeline of the system is shown in Fig. 2. The contributions of this
paper are:

  * •

The development of a novel fiducial marker system for the LiDAR, the IILFM
system. Unlike LiDARTag [16] which requires extra 3D objects to be added to
the environment, the application of the IILFM is as convenient as the VFM
systems [6, 7].

  * •

The proposal of a novel marker detection method to detect the predefined 3D
features through the intensity image. Thanks to this, the VFM systems proposed
in the past, present, and even future can be easily embedded into the IILFM
system.

  * •

The introduction of a pose estimation approach for the LiDAR via the proposed
IILFM, which has similar accuracy as the VFM-based pose estimation for the
camera. In addition, dissimilar to the VFM system, the proposed pose
estimation is free of the rotation ambiguity problem [19, 20].

The paper is organized as follows. Section II introduces the related work.
Section III presents the pipeline for detecting 3D fiducials in the point
cloud through the intensity image. Section IV illustrates the method to
estimate the LiDAR’s pose with respect to the world coordinate system. Section
V provides the qualitative and quantitative experimental evaluations regarding
marker detection and pose estimation. Section VI gives the conclusion and
future work.

##  II Related Work

The previous research on the VFM system mainly focuses on the following four
aspects:

(1) A higher detection rate. Take the process to extract the line segments in
the image as the example, ARToolkit [3] adopts naive thresholding; the first
generation of Apriltag [5] employs image gradients to detect high-contrast
edges, which has a similar effect as binarization; and the second generation
of Apriltag [4] uses adaptive thresholding. The objective of these
improvements is to boost the detection of line segments, candidate quads [6,
7], or circles [8], thus the marker detection rate under varying ambient light
is improved.

(2) A lower false positive rate. The coding/decoding systems of the latest VFM
systems [6, 7] are upgraded with regard to the old generations [4, 5], such
that the identification of a candidate marker is more reliable and the false
positive cases are reduced when decoding.

(3) A lower computational time. Again, through the amelioration of methods for
graphic segmentation and algorithms adopted in coding/decoding systems, the
VFM systems are accelerated. For instance, the speed of the third generation
of Apirltag [6] is almost 5 times faster than that of the second generation
[4].

(4) Resolving the rotation ambiguity problem. The rotational ambiguity means a
planar marker could project onto the same pixels from two different poses when
the perspective effect is weak [19]. Much research [20, 21, 22] has been
conducted to resolve the duo solutions problem. Unlike the first three
aspects, the last aspect is a remaining fundamental challenge.

The VFM system is embedded in the proposed system, such that the achievements
mentioned in aspects (1)-(3) are inherited in the proposed one. Furthermore,
as shown in Section IV, the proposed system is free from the problem
introduced in aspect (4).

Although LiDARTag [16] also borrows the dictionary-based coding idea from the
Apriltag system [6], most of the accumulations mentioned above are not
preserved since LiDARTag follows the conventional approaches to process the 3D
point cloud, which brings forward two inconveniences. Firstly, to utilize the
single-linkage clustering algorithm, it is needed to guarantee that the
representation of the marker in the point cloud meets a spatial requirement:
the marker must have $t\sqrt{2}/4$ clearance around it [16] where $t$
represents the marker’s size. If the marker is attached to a wall or a box, it
is required to make sure $\tau>t\sqrt{2}/4$ where $\tau$ is the thickness of
the marker’s 3D object. Therefore, consider the case that 10 LiDAR Tags are
placed in an indoor environment. Such a situation will result in a room
crowded with tripods and large markers. This apparently wrecks the
environment. The second inconvenience is that the algorithms to process the
point cloud and point clustering are packaged in the implementation of
LiDARTag which assumes that the marker is square and the pattern belongs to
Apriltag [6]. Suppose that it is desired to adopt a non-square marker system,
such as CCTag [8], then it is required to reprogram the LiDARTag system.

##  III Marker Detection

###  III-A Generation of the Intensity Image

The generation of an intensity image from a given unstructured point cloud can
be summarized as transferring all the 3D points in the point cloud onto a 2D
image plane by spherical projection and rendering the corresponding pixels
with intensity values.

Figure 3: An illustration of the coordinate systems and notations.

Fig. 3 shows the coordinate systems and notations.
$\textbf{p}=[x_{l},y_{l},z_{l},i]^{T}$ is an observed point in the 3D point
cloud, with ${[x_{l},y_{l},z_{l}]^{T}}$ denoting its Cartesian coordinates
w.r.t the LiDAR coordinate system {L} and $i$ being the intensity. u is the
projection of p onto the image plane, which has coordinates $[u,v]^{T}$ w.r.t
the image coordinate system, {I}. Following [2], the Cartesian coordinates of
p are first transformed to spherical coordinates $[\theta,\phi,r]^{T}$:

$\displaystyle\theta=\arctan(\frac{y_{l}}{x_{l}})$ (1)
$\displaystyle\phi=\arctan(\frac{z_{l}}{\sqrt{x_{l}^{2}+y_{l}^{2}}})$
$\displaystyle r=\sqrt{x_{l}^{2}+y_{l}^{2}+z_{l}^{2}}$

where $\theta$ and $\phi$ denote the azimuth and inclination, respectively.
$r$ is the range from p to the origin of {L}.

Then, the image coordinates $[u,v]^{T}$ of u are given by:

$u=\lceil\frac{\theta}{\Theta_{a}}\rfloor+u_{o},\;v=\lceil\frac{\phi}{\Theta_{i}}\rfloor+v_{o}$
(2)

where $\lceil\;\rfloor$ represents rounding a value to the nearest integer.
$\Theta_{a}$ and $\Theta_{i}$ are the angular resolutions in $u$ (azimuth) and
$v$ (inclination) directions, respectively. $u_{o}$ and $v_{o}$ are the
offsets.

Assume it is desired that the point with zero-azimuth and zero-inclination to
be projected to the center of the image, then the offsets will be:
$u_{o}=I_{w}/2$ and $v_{o}=I_{h}/2$, where $I_{w}$ and $I_{h}$ being the image
width and height which are determined by the maximum angular width $P_{w}$ and
height $P_{h}$ of the point cloud
$I_{w}=\lceil\frac{P_{w}}{\Theta_{a}}\rfloor,\;I_{h}=\lceil\frac{P_{h}}{\Theta_{i}}\rfloor$.
The pixel $[u,v]^{T}$ is then rendered using the corresponding intensity value
$i$, and the range information $r$ is also saved for the sake of the following
pose estimation. Thereafter, we step through the point cloud and repeat the
above process. The pixels that are not mapped to any points will remain
unobserved and are rendered by a unique predefined value. Note that if these
pixels are visited later on in the marker detection process, they will not
return any 3D points because they represent unobserved regions.

Remark 1: As a matter of fact, the rounding in the Eq. (2) causes information
loss if the pixels are non-integer values at the beginning. However, this is
inevitable when transferring the points in the point cloud onto an image plane
where the points are expressed with integer coordinates. Furthermore, this
approximation is not fatal as can be seen in Section V, the pose estimation
precision is still neck-to-neck with the VFM system despite the information
loss.

###  III-B Selections of the Angular Resolutions

Figure 4: The intensity images generated under different angular resolution
settings. The unobserved regions are rendered light green. (a), (b), and (c)
are intensity images generated from the same point cloud given by one LiDAR
scan of a Velodyne ULTRA Puck (mechanical LiDAR,
$\Theta_{h}=\textbf{0.4}^{\circ}$ and $\Theta_{v}=\textbf{0.33}^{\circ}$). The
LiDAR scan is extracted from the dataset provided by [16]. (d), (e), and (f)
are intensity images generated from the same point cloud which is the
integration of multiple LiDAR scans of a Livox Mid-40 (solid-state LiDAR,
$\Theta_{h}=\Theta_{v}=\textbf{0.05}^{\circ}$). (a), (b), and (c) are cropped
to display. (a) $\Theta_{a}=0.05^{\circ}$ and $\Theta_{i}=0.05^{\circ}$. Raw
image size = $7200\times 800$. (b) $\Theta_{a}=0.4^{\circ}$ and
$\Theta_{i}=0.33^{\circ}$. Raw image size = $900\times 121$. (c)
$\Theta_{a}=0.6^{\circ}$ and $\Theta_{i}=1.0^{\circ}$. Raw image size =
$600\times 40$. (d) $\Theta_{a}=\Theta_{i}=0.025^{\circ}$. Raw image size =
$1538\times 1178$. (e) $\Theta_{a}=\Theta_{i}=0.05^{\circ}$. Raw image size =
$771\times 591$. (f) $\Theta_{a}=\Theta_{i}=0.5^{\circ}$. Raw image size =
$81\times 62$.

The selections of $\Theta_{a}$ and $\Theta_{i}$ are crucial as they affect the
quality of the intensity image directly. Yet, the resolution of this problem
is quite simple: suppose that $\Theta_{h}$ is the horizontal angular
resolution and $\Theta_{v}$ is the vertical angular resolution of the employed
LiDAR, set $\Theta_{a}=\Theta_{h}$ and $\Theta_{i}=\Theta_{v}$. Fig. 4
illustrates the reasoning behind this setting. There are two objectives when
generating the intensity image: 1) we want fewer unobserved regions to appear
around or inside the marker’s area since they do not provide any intensity or
range information. Moreover, they could become image noise after the image
preprocessing phase. Thus, the settings shown in Fig. 4 (a) and (d) cannot be
used, where $\Theta_{a}<\Theta_{h}$ and $\Theta_{i}<\Theta_{v}$; 2) However,
if a dense image is desired, we need to be aware that too much information
will be lost if too many points overlap for the same pixels based on Eq. (2).
For instance, the marker’s pattern disappears in Fig. 4 (c) and (f), where
$\Theta_{a}>\Theta_{h}$ and $\Theta_{i}>\Theta_{v}$, even though they are
dense images. For the sake of the two objectives, the following settings
should be used: $\Theta_{a}=\Theta_{h}$ and $\Theta_{i}=\Theta_{v}$.
Nevertheless, as shown in Fig. 5, the LiDAR cannot sample perfectly evenly in
the inclination/azimuth space. Hence, even if the user manual of the employed
LiDAR is followed to set $\Theta_{a}=\Theta_{h}$ and $\Theta_{i}=\Theta_{v}$,
the unwanted unobserved regions around or inside the marker’s area and the
points overlapping issue are still inevitable. Based on the experiments, the
points overlapping issue under $\Theta_{a}=\Theta_{h}$ and
$\Theta_{i}=\Theta_{v}$ could have an acceptable influence on pose estimation
while the unobserved regions impede marker detection. This is the reason why
image preprocessing (Refer to Section III-C) and the approach to deal with
unobserved regions (Refer to Section III-D) are necessary.

Figure 5: Sampling patterns of the mechanical LiDAR and the solid-state LiDAR.
Sampling points are represented by red scatters. (a) General sampling pattern
of a mechanical LiDAR expressed in the azimuth/inclination coordinate system.
This is a general schematic that does not correspond to any LiDAR model. (b)
Sampling pattern of the Livox Mid-40 LiDAR after one-second integration. Note
that the sampling patterns vary when it comes to different solid-state LiDAR
models. But generally, the unscanned regions within the valid Field of View
(FoV) appear as spots.

###  III-C Image Preprocessing

Figure 6: An illustration of image preprocessing in the system. The input is
the raw intensity image (a). The image is first converted to a grayscale image
(b). Then it becomes the binary image (c) using naive thresholding. Gaussian
Blur is recommended when the embedded VFM system, such as CCTag [8], does not
contain it. (d): The intensity image of a CCTag after the naive thresholding
while the detector cannot detect the marker in it. (e): The intensity image
after applying Gaussian Blur [23] on (d). Now the marker is detectable.

Fig. 6 shows the image preprocessing pipeline before inputting the raw
intensity image into the embedded VFM system. Ideally, the intensity values
inside the zoomed white-dotted area shown in Fig. 6(a) should be similar
whereas these regions have varying low intensity values in the real
application. After grayscale conversion, the image becomes Fig. 6(b), where
the image noise caused by varying low intensity values still exists. It is
found experimentally that if the grayscale intensity image with the quality
shown in Fig. 6(b) is inputted into Apriltag 3 [6], the marker detection
fails. To remove the image noise shown in Fig. 6(b), following grayscale
conversion, binarization is conducted using naive thresholding, which
indicates binarization of the entire image with a specified threshold. Fig.
6(c) shows the binarized image. As seen, the noise caused by the varying low
intensity values is significantly reduced.

Despite the fact that binarization is a common process in the VFM systems, a
mandatory binarization is still added after the grayscale conversion. This is
because it is desired that the proposed system have a robust generality when
used along with different VFM systems. Detailed explanations are as follows.
The binarization processes vary when it comes to different VFM systems as
introduced in Section II. However, it cannot be guaranteed that the
binarization method adopted in the VFM system embedded in our system can
handle the image noise shown in Fig. 6(b). Hence, a straightforward but
general solution is to add an appropriate and mandatory binarization for the
grayscale intensity image. A naive thresholding is adopted due to the
following reason. As illustrated in [16], the LiDAR measurement is not
sensitive to ambient light, which is unlike the camera measurement. When the
LiDAR model and the material of the marker are fixed, naive thresholding shows
robust denoising performance since the two major elements that affect the
intensity are fixed. Some recommended thresholds are provided for different
LiDAR models in our program. This threshold is also a custom parameter that
the user can adjust according to their LiDAR model and marker material.

Additionally, as seen in Fig. 6(d), random noise could be preserved after the
naive thresholding in some cases though this is harmless for VFM systems [6,
7] that adopt Gaussian Blur [23]. But for systems that do not contain it, such
as CCTag [8], Gaussian Blur should be adopted. Otherwise, the marker detection
will fail.

###  III-D 3D Feature Points Estimation

The preprocessed image introduced in the previous section is then inputted
into the embedded VFM system. Thereafter, the VFM system provides the
detection information. In this section, we introduce how to project the 2D
feature points given by the detection information back to the 3D space, such
that they become 3D feature points expressed in the LiDAR coordinate system.
As for the square markers, the feature points refer to the four vertices of
the quad. While some of the VFM systems adopt the non-square design, the
proposed method has no restriction on the marker shape.

As mentioned in Section III-A, the range information is stored for each pixel
in the intensity image as well. Thus, a 2D pixel with range information
($\textbf{u}^{r}=[u,v,r]^{T}$) can be projected back to the 3D Cartesian
coordinate system by solving the inverse of Eqs. (1-2) to find the
corresponding $[x_{l},y_{l},z_{l}]^{T}$. Yet in the real world, it cannot be
guaranteed that the feature points of the marker are exactly scanned by the
LiDAR. Namely, the detected 2D features in Fig. 6(c) could correspond to the
unobserved regions in the raw intensity image. As a matter of fact, this
occurs frequently in our experiments, as well as in the dataset provided by
[16]. Hence, when these detected but unscanned features are checked, there
will be no range value $r$ returned and it is not feasible to solve the
inverse of Eqs. (1-2).

Figure 7: An illustration of the algorithm to estimate the 3D coordinates of a
detected but unscanned 2D feature point.

To resolve this problem, we propose an algorithm as illustrated in Fig. 7. The
algorithm is based on the fact that the markers are planar. Suppose that there
is a 2D feature point $\textbf{u}_{k}=[u_{k},v_{k}]^{T}$ that is detected but
corresponds to an unobserved region in the raw intensity image. The azimuth
$\theta_{k}$ of $\textbf{u}_{k}$ is determined by Eq. (2) through
$\theta_{k}=\Theta_{a}(u_{k}-u_{0})$. Define the unknown 3D point
corresponding to $\textbf{u}_{k}$ as $\textbf{p}_{k}=[x_{k},y_{k},z_{k}]^{T}$
(the yellow point in Fig. 7). Hereafter, suppose that in the same column as
$\textbf{u}_{k}$, there is a pair of observed pixels that are symmetric about
$\textbf{u}_{k}$, and define their corresponding points as
$\textbf{p}_{u}=[x_{u},y_{u},z_{u}]^{T}$ and
$\textbf{p}_{d}=[x_{d},y_{d},z_{d}]^{T}$ (the black point in Fig. 7). As
illustrated in Eq. (2), pixels in the same column approximately share the same
azimuth. Thus, $\textbf{p}_{u}$, $\textbf{p}_{k}$, and $\textbf{p}_{d}$ are on
the same plane, $\alpha$, which is specified by fixing the azimuth
($\theta=\theta_{k}$). After that, define the plane where the marker is
located as $\beta$. Obviously, the intersection of $\alpha$ and $\beta$ is a
straight line l. Under the assumption that $\textbf{p}_{u}$ and
$\textbf{p}_{d}$ are on the marker plane $\beta$, it can be shown that
$\textbf{p}_{u}$, $\textbf{p}_{k}$, and $\textbf{p}_{d}$ are collinear and
they all fall on l. To give a more explicit introduction to the algorithm, the
side view of the plane $\alpha$ is presented in Fig. 8.

Figure 8: The side view of $\alpha$. $\phi_{d}$, $\phi_{k}$, and $\phi_{u}$
are the inclinations of $\textbf{p}_{d}$, $\textbf{p}_{k}$, and
$\textbf{p}_{u}$, respectively.

Undoubtedly, $\textbf{O}_{L}\textbf{p}_{k}$ is the angle bisector of
$\angle\textbf{p}_{u}\textbf{O}_{L}\textbf{p}_{d}$ owing to
$\phi_{d}-\phi_{k}=\phi_{k}-\phi_{u}$. Hence, in the light of the angle
bisector properties, we have
$\textbf{p}_{k}\textbf{p}_{d}/\textbf{p}_{u}\textbf{p}_{k}=\textbf{O}_{L}\textbf{p}_{d}/\textbf{O}_{L}\textbf{p}_{u}$.
Note that $\textbf{O}_{L}\textbf{p}_{d}$ and $\textbf{O}_{L}\textbf{p}_{u}$
are the ranges of $\textbf{p}_{d}$ and $\textbf{p}_{u}$ which can be obtained
if they are scanned. Thus, the unknown 3D coordinates of $\textbf{p}_{k}$ are
estimated by
$\textbf{p}_{k}=\textbf{M}_{1}\textbf{p}_{d}+\textbf{M}_{2}\textbf{p}_{u}$,
where
$\textbf{M}_{1}=\mathrm{diag}(\frac{\mu}{1+\mu},\frac{\mu}{1+\mu},\frac{\mu}{1+\mu})$
and
$\textbf{M}_{2}=\mathrm{diag}(\frac{1}{1+\mu},\frac{1}{1+\mu},\frac{1}{1+\mu})$
with $\mu$ being the ratio
$\textbf{O}_{L}\textbf{p}_{d}/\textbf{O}_{L}\textbf{p}_{u}$.

So far we projected the 2D feature points in {I}, observed and unobserved by
the LiDAR, back to {L}.

Remark 2: The fundamental assumption of this section is that the 3D points
corresponding to nearby pixels of the unobserved feature are still on the
marker plane. This is not a harsh requirement since the vertical angular
resolution of most LiDAR is less than $2^{\circ}$.

##  IV LiDAR Pose Estimation

The aim of LiDAR pose estimation is to seek the Euclidean transformation,
$\textbf{{T}}\in\mathrm{SE(3)}$, from the world coordinate system {W} to the
LiDAR coordinate system {L}. Suppose that $\textbf{{f}}\in\mathbb{R}^{3}$ is
the 3D coordinates of a feature point, the operation of
$\textbf{{T}}\in\mathrm{SE(3)}$ on $\textbf{{f}}\in\mathbb{R}^{3}$ is
$\textbf{{T}}\cdot\textbf{{f}}=\textbf{{R}}\textbf{{f}}+\textbf{{t}}$, where
$\textbf{{R}}\in\mathrm{SO(3)}$ is the rotation matrix and
$\textbf{{t}}\in\mathbb{R}^{3}$ is the translation vector. Refer to [2] for
the basics of $\mathrm{SE(3)}$ and $\mathrm{SO(3)}$.

Thus far two sets of 3D points are obtained. 1) $n$ feature points w.r.t {L},
denoted by $\textbf{P}_{L}=\\{\textbf{{f}}_{1},\ \cdots,\
\textbf{{f}}_{n}\\}$, which are given by the 3D feature points estimation
introduced in Section III-D; 2) $n$ feature points w.r.t {W}, denoted by
$\textbf{P}_{W}=\\{\textbf{{f}}_{1}^{\prime},\ \cdots,\
\textbf{{f}}_{n}^{\prime}\\}$, which are predefined. Furthermore, the points
in $\textbf{P}_{L}$ and $\textbf{P}_{W}$ are matched based on the ID number
and vertex index given by the marker detection. Hence, the LiDAR pose
estimation can be transformed into finding $\textbf{{R}}\in\mathrm{SO(3)}$ and
$\textbf{{t}}\in\mathbb{R}^{3}$ that optimally align $\textbf{P}_{L}$ and
$\textbf{P}_{W}$. This is inherently a least square problem:

$\textbf{{R}}^{*},\ \textbf{{t}}^{*}=\underset{\textbf{{R}},\
\textbf{{t}}}{\arg\min}\sum_{j=1}^{n}\left\|\left(\textbf{{R}}\textbf{{f}}_{j}^{\prime}+\textbf{{t}}\right)-\textbf{{f}}_{j}\right\|^{2}$
(3)

Considering that the point-correspondence between $\textbf{P}_{L}$ and
$\textbf{P}_{W}$ is quite reliable thanks to the embedded VFM system,
$\textbf{{R}}^{*}$ and $\textbf{{t}}^{*}$ can be calculated in closed form by
the Singular Value Decomposition (SVD) method [2, 24]. Moreover, the points in
$\textbf{P}_{W}$ are coplanar but not collinear, thus the solution of Eq. (3)
is also unique and the proof is given in [24]. This is the reason why the
proposed system is free from the multi-solution problem, which is unlike the
VFM systems troubled by the rotation ambiguity problem [12, 13, 19, 20]. This
is a superiority of the proposed system over the planar VFM systems [6, 7].

Remarkably, in the perspective-n-point problem, four correspondences are
already enough to estimate the camera pose [19]. But when the VFM systems are
reviewed, only the four vertices of a marker (or a limited number of points if
the marker is non-square) are utilized even though it is definitely feasible
to acquire more feature points from the inner coding area of the marker though
this increases the complexity of the system. Hence, following this design
concept, simply but effectively, only the four vertices of a marker are taken
as the feature points. As a result, Table III shows that the pose estimation
accuracy of the proposed system is slightly inferior to LiDARTag [16], which
takes thousands of points inside the cluster of the marker as feature points.
If one insists on a higher accuracy, it is feasible to use the proposed marker
detection information to find the points clustering of the marker in the point
cloud and then input the points clustering into the pose estimation block of
LiDARTag [16].

##  V Experimental Results

TABLE I: pose estimation accuracy of the IILFM system at different distances Distance (m) | Sensor | Number of Scans | System | x (m) | y (m) | z (m) | roll (deg) | pitch (deg) | yaw (deg)
---|---|---|---|---|---|---|---|---|---
$\approx$ 2 m | LiDAR | – | Ground Truth | 1.604 | -0.158 | 0.612 | -0.015 | -0.090 | -0.014
40 | IILFM | 1.591 | -0.069 | 0.550 | 0.174 | 4.185 | 1.775
Error | 0.013 | -0.089 | 0.062 | -0.189 | -4.275 | -1.789
100 | IILFM | 1.601 | -0.164 | 0.629 | -0.438 | 1.930 | -0.873
Error | 0.003 | 0.006 | -0.017 | 0.423 | -2.020 | 0.859
Camera | – | Ground Truth | 1.615 | -0.086 | 0.167 | 0.020 | 1.879 | 0.275
Apriltag 3 | 1.631 | -0.120 | 0.183 | -0.787 | 5.045 | -0.969
| Error | -0.016 | 0.034 | -0.016 | 0.807 | -3.166 | 1.244
$\approx$ 3 m | LiDAR | – | Ground Truth | 0.612 | -0.152 | 0.624 | -0.007 | -0.103 | -0.031
40 | IILFM | 0.583 | -0.133 | 0.625 | -0.733 | 2.821 | 1.016
Error | 0.029 | -0.019 | -0.001 | 0.726 | -2.924 | -1.047
100 | IILFM | 0.586 | -0.131 | 0.717 | -0.937 | 1.079 | 1.080
Error | 0.026 | -0.021 | -0.093 | 0.930 | -1.182 | -1.111
Camera | – | Ground Truth | 0.599 | -0.051 | 0.184 | 0.073 | 0.360 | 0.321
Apriltag 3 | 0.527 | 0.073 | 0.228 | 1.442 | -7.603 | 3.225
| Error | 0.072 | -0.124 | -1.369 | 0.423 | 7.963 | -2.904
$\approx$ 4 m | LiDAR | – | Ground Truth | -0.400 | -0.163 | 0.632 | -0.005 | -0.088 | -0.015
40 | IILFM | -0.422 | -0.311 | 0.597 | -1.437 | 2.288 | -2.138
Error | 0.022 | 0.148 | 0.035 | 1.432 | -2.376 | 2.123
100 | IILFM | -0.424 | -0.187 | 0.739 | -0.347 | 0.369 | -0.406
Error | 0.024 | 0.024 | -0.107 | 0.342 | -0.457 | 0.391
Camera | – | Ground Truth | -0.399 | -0.091 | 0.199 | 0.072 | 1.395 | 0.845
Apriltag 3 | -0.341 | 0.316 | 0.432 | 1.505 | -7.897 | 14.279
| Error | -0.058 | -0.407 | -0.233 | -1.433 | 9.292 | -13.434

###  V-A Qualitative Evaluation

To qualitatively demonstrate the flexibility of the proposed IILFM system,
Fig. 9 is presented. The usage of the IILFM system is as convenient as the VFM
systems. In particular, the user can place the Letter size markers [6, 7]
densely to compose a marker grid, as shown in Fig. 9(a), as well as attach
some non-square markers [8] to the wall freely, as shown in Fig. 9(b). In
summary, there is no spatial restriction on marker placement and this is the
most important virtue of our system. It should be noted that if the angular
resolution of the LiDAR is relatively large, large marker size and simpler
marker pattern are recommended for the sake of the intensity image quality.
For instance, the vertical angular resolution of the VLP-16 is 1.33∘, thus a
40 cm$\times$40 cm original ArUco [7] is adopted, as shown in Fig. 9(c).

Figure 9: Marker detection results on the preprocessed intensity images. (a)
corresponds to the scenario shown in Fig. 1(a): a Livox Mid-40 is scanning an
Apriltag grid and an ArUco grid. All the markers, 35 Apriltags (family:
tag36h11, marker size: 17.2 cm$\times$17.2 cm), and 4 ArUcos (family:
4$\times$4, marker size: 16 cm$\times$16 cm), are detected. (b) corresponds to
Fig. 1(b): a Livox Mid-40 is scanning three CCTag (family: 3 rings, marker
radius: 7.2 cm) attached to the wall. All the markers are detected. (c)
corresponds to Fig. 1(c): a VLP-16 is scanning an ArUco (family: original,
maker size: 40 cm$\times$40 cm). The marker is detected.

###  V-B Quantitative Evaluation

To further verify the pose estimation precision of the IILFM system, we
compare the pose estimation result given by our system with the ground truth
provided by the OptiTrack Motion Capture (MoCap) system (See Fig. 10). Again,
we adopt the low-cost solid-state LiDAR, Livox Mid-40, to scan a marker pasted
on the wall. The marker (ID = 0), with the size of 17.2 cm$\times$17.2 cm,
belongs to the tag36h11 family of Apriltag 3 [6] and is printed on Letter
paper. The location of the marker’s center is at (3.620, 0.00, 0.485) m w.r.t
{W}.

Firstly, the orientation of the LiDAR is fixed, with the roll, pitch, and yaw
angles approximately being zeros. Only the distance from the marker’s plane to
the LiDAR’s $O_{L}Y_{L}\mbox{-}O_{L}Z_{L}$ plane, see Table I, is changed.
Then, to compare the conventional VFM system with the proposed system, we test
Apriltag 3 [6] with a camera (Omnivision OV7251) under the same experimental
setup. Three issues are illustrated in Table I: 1) When more LiDAR scans are
utilized, the pose estimation precision is slightly boosted. The reason behind
it is that more LiDAR scans indicate a higher coverage percentage in the FoV
[15], which implies better intensity image quality. 2) The IILFM system shows
comparable precision as the VFM system. 3) Unlike the VFM system [4, 5, 6],
the pose estimation precision does not degrade evidently as the distance
increases.

Figure 10: An illustration of the experimental setup. The MoCap system is
composed of 16 OptiTrack cameras and provides the 6-DOF pose information of
the predefined rigid body at 100 Hz.

Thereafter, the distance from the marker to the LiDAR is set as 2 meters and
only the rotation of the LiDAR is adjusted (See Table II where the number of
scans = 40). By comparing the errors in Table II and Table I, it is seen that
when the plane of the LiDAR is angled towards the marker plane, the pose
estimation performance of the IILFM system is as good as that when the LiDAR
is perpendicular to the marker.

TABLE II: pose estimation accuracy of the IILFM system with different Euler angles Setup | Term | Ground Truth | IILFM | Error
---|---|---|---|---
pitch $\approx-15^{\circ}$ | x (m) | 1.629 | 1.661 | -0.032
y (m) | -0.066 | -0.011 | -0.055
z (m) | 0.618 | 0.699 | -0.081
roll (deg) | -2.673 | -4.936 | 2.263
pitch (deg) | -15.060 | -21.145 | 6.085
yaw (deg) | -0.171 | 0.546 | -0.717
pitch $\approx 15^{\circ}$ | x (m) | 1.684 | 1.622 | 0.062
y (m) | -0.063 | -0.088 | 0.026
z (m) | 0.590 | 0.660 | -0.070
roll (deg) | 3.657 | 0.900 | 2.757
pitch (deg) | 14.849 | 10.961 | 3.888
yaw (deg) | 1.136 | -1.921 | 3.057
yaw $\approx-15^{\circ}$ | x (m) | 1.638 | 1.612 | 0.027
y (m) | -0.618 | -0.237 | -0.381
z (m) | 0.610 | 0.585 | 0.025
roll (deg) | -0.009 | -0.792 | 0.783
pitch (deg) | -0.662 | -2.088 | 1.426
yaw (deg) | -15.387 | -17.937 | 2.550
yaw $\approx 15^{\circ}$ | x (m) | 1.627 | 1.632 | -0.005
y (m) | -0.194 | -0.146 | -0.048
z (m) | 0.609 | 0.553 | 0.057
roll (deg) | 0.490 | -1.994 | 2.484
pitch (deg) | -0.388 | -0.359 | -0.028
yaw (deg) | 14.924 | 10.678 | 4.246

To validate the pose estimation accuracy of our system on the mechanical LiDAR
as well as to compare the proposed system with the state-of-the-art LiDAR
fiducial marker system, LiDARTag [16], which is also the only existing
fiducial marker system for the LiDAR as far as we know, Table III is
presented. Specifically, we use the rosbag (ccw_10m.bag) provided by [16] as
the benchmark on which we conduct the comparison. ccw_10m.bag records the raw
data of a 32-Beam Velodyne ULTRA Puck LiDAR scanning a 1.22 m$\times$1.22 m
Apriltag (tag16h6), from a distance of 10 meters while the relative angle
between the LiDAR plane and the marker is around 45∘. Only the vertices
estimation is compared on account that [16] solely provides the ground truth
of the vertices and the vertices estimation is a follow-up process after the
pose estimation in the LiDARTag system [16, 11]. Hence, the vertices
estimation can also reflect the pose estimation precision. Table III
illustrates that the IILFM system is slightly inferior to LiDARTag in terms of
accuracy, whereases as mentioned previously, the superiority of the IILFM
system is the flexibility and convenience. Marker detection in the scenarios
shown in Fig. 1 and Fig. 10 is infeasible for the LiDARTag system since the
marker placement does not satisfy the requirement of LiDARTag, and in
addition, the current version of LiDARTag does not support any non-square
marker, such as CCTag [8].

TABLE III: Comparison of the IILFM system and LiDARTag System | Vertex | x (m) | y (m) | z (m) | Error (m)
---|---|---|---|---|---
Ground Truth | 1 | 9.739 | 0.758 | -0.161 | –
2 | 9.940 | 0.072 | -0.732 | –
3 | 10.272 | -0.414 | -0.032 | –
[16] | 4 | 10.072 | 0.271 | 0.539 | –
LiDARTag | 1 | 9.736 | 0.762 | -0.174 | 0.015
2 | 9.944 | 0.066 | -0.730 | 0.009
3 | 10.271 | -0.405 | -0.016 | 0.019
[16] | 4 | 10.063 | 0.291 | 0.539 | 0.022
IILFM | 1 | 9.728 | 0.766 | -0.184 | 0.013
2 | 9.963 | 0.052 | -0.705 | 0.033
3 | 10.307 | -0.432 | -0.016 | 0.044
4 | 10.045 | 0.263 | 0.564 | 0.041

###  V-C Computational Time Analysis

A computational time analysis is conducted on a desktop with Intel Xeon
W-1290P CPU. The LiDARTag [16] runs at around 100 Hz on it. The time
consumption of the marker detection of our system, which includes intensity
image generation, preprocessing, 2D features detection, and computation of 3D
features, mainly depends on the size of the intensity image and the embedded
VFM system. Suppose that the embedded VFM system is Apriltag 3 [6], the marker
detection takes around 7 ms (approx 143 Hz) in the case shown in Fig. 9(c)
where $\Theta_{a}=0.3^{\circ}$, $\Theta_{i}=1.33^{\circ}$, and intensity image
size = $1201\times 27$. For the case shown in Fig. 9(a), the marker detection
takes around 25 ms (approx 40 Hz), where intensity
$\Theta_{a}=\Theta_{i}=0.05^{\circ}$ and image size = $771\times 591$. The
time consumption of the following pose estimation process is around 1.8
$\upmu$s as the closed-form solution can be obtained directly through SVD [24]
(Refer to Section IV).

###  V-D Limitations Analysis

There are some limitations on the application of the LiDAR fiducial marker
systems. First, the distance from the object (marker) to the LiDAR must exceed
the minimum detectable range. This limitation is caused by the hardware
attributes and it affects all the LiDAR applications not only the LiDAR
fiducial marker systems. Secondly, to utilize the solid-state LiDAR, it is
required to wait for the growth of the scanned area inside the FoV to obtain a
relatively dense point cloud. Again this is a limitation caused by the
hardware attributes. In contrast, the mechanical LiDAR only requires one LiDAR
scan to work, however, a larger and simpler marker is recommended if the
angular resolution is large. Finally, the false positives are occasionally
found in the experiments while the issue of wrong ID detection (not exactly
the same as the false positive but similar) is also reported in LiDARTag [16].
For the proposed system, the false positive rate is mainly determined by the
embedded VFM system, thus novel VFM systems with lower false positive rate,
such as [6, 7], are preferred.

##  VI Conclusions and Future Work

In this work, a novel intensity image-based LiDAR fiducial marker is proposed.
The main novelty of the system is its flexibility and convenient application.
Namely, the users can place the markers the same way as placing the
conventional visual fiducial markers without worrying about any spatial
restriction on marker placement. Following the method introduced in this work,
the popular visual fiducial marker systems, such as Apriltag, ArUco, CCTag,
and newly proposed ones in the future can be easily integrated into the
system, thus the proposed system has no restriction on marker shape.
Experimental results demonstrate that the proposed system has the similar
accuracy as the visual fiducial marker system in terms of pose estimation.

As for future work, motion compensation is a vital direction since it is
assumed that the unstructured point cloud inputted to the system is already
motion compensated. Furthermore, the intensity image quality could be unideal
due to the over-sparsity of the point cloud, which hinders the marker
detection. Hence, migrating the deep-learning-based 3D point cloud completion
method [25] into the system is also an interesting topic. In addition, we are
developing a LiDAR fiducial marker-based SLAM framework.

## ACKNOWLEDGMENT

The authors would like to thank Yicong Fu, Hassan Alkomy, and Brian Lynch for
constructive discussions, Jiunn-Kai Huang for providing his experimental
results.

## References

  * [1] M. Menze and A. Geiger, “Object scene flow for autonomous vehicles,” in _Proc. IEEE conference on computer vision and pattern recognition_ , 2015, pp. 3061–3070.
  * [2] T. D. Barfoot, _State estimation for robotics_. Cambridge University Press, 2017.
  * [3] H. Kato and M. Billinghurst, “Marker tracking and hmd calibration for a video-based augmented reality conferencing system,” in _Proc. 2nd IEEE and ACM International Workshop on Augmented Reality_ , 1999, pp. 85–94.
  * [4] J. Wang and E. Olson, “Apriltag 2: Efficient and robust fiducial detection,” in _Proc. IEEE/RSJ International Conference on Intelligent Robots and Systems_ , 2016, pp. 4193–4198.
  * [5] E. Olson, “Apriltag: A robust and flexible visual fiducial system,” in _Proc. IEEE International Conference on Robotics and Automation_ , 2011, pp. 3400–3407.
  * [6] M. Krogius, A. Haggenmiller, and E. Olson, “Flexible layouts for fiducial tags,” in _Proc. IEEE/RSJ International Conference on Intelligent Robots and Systems_ , 2019, pp. 1898–1903.
  * [7] F. J. Romero-Ramirez, R. Muñoz-Salinas, and R. Medina-Carnicer, “Speeded up detection of squared fiducial markers,” _Image and vision Computing_ , vol. 76, pp. 38–47, 2018.
  * [8] L. Calvet, P. Gurdjos, C. Griwodz, and S. Gasparini, “Detection and Accurate Localization of Circular Fiducials under Highly Challenging Conditions,” in _Proc. 2016 IEEE Conference on Computer Vision and Pattern Recognition_ , 2016, pp. 562 – 570.
  * [9] D. Avola, L. Cinque, G. L. Foresti, C. Mercuri, and D. Pannone, “A practical framework for the development of augmented reality applications by using aruco markers,” in _Proc. International Conference on Pattern Recognition Applications and Methods_ , vol. 2, 2016, pp. 645–654.
  * [10] J. Rehder, J. Nikolic, T. Schneider, T. Hinzmann, and R. Siegwart, “Extending kalibr: Calibrating the extrinsics of multiple imus and of individual axes,” in _Proc. IEEE International Conference on Robotics and Automation_ , 2016, pp. 4304–4311.
  * [11] J.-K. Huang and J. W. Grizzle, “Improvements to target-based 3d lidar to camera calibration,” _IEEE Access_ , vol. 8, pp. 134 101–134 110, 2020\.
  * [12] R. Muñoz-Salinas, M. J. Marín-Jimenez, E. Yeguas-Bolivar, and R. Medina-Carnicer, “Mapping and localization from planar markers,” _Pattern Recognition_ , vol. 73, pp. 158–171, 2018.
  * [13] R. Muñoz-Salinas, M. J. Marin-Jimenez, and R. Medina-Carnicer, “Spm-slam: Simultaneous localization and mapping with squared planar markers,” _Pattern Recognition_ , vol. 86, pp. 156–171, 2019.
  * [14] H. Ye, Y. Chen, and M. Liu, “Tightly coupled 3d lidar inertial odometry and mapping,” in _Proc. International Conference on Robotics and Automation_ , 2019, pp. 3144–3150.
  * [15] J. Lin and F. Zhang, “Loam livox: A fast, robust, high-precision lidar odometry and mapping package for lidars of small fov,” in _Proc. IEEE International Conference on Robotics and Automation_ , 2020, pp. 3126–3131.
  * [16] J.-K. Huang, S. Wang, M. Ghaffari, and J. W. Grizzle, “Lidartag: A real-time fiducial tag system for point clouds,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 3, pp. 4875–4882, 2021.
  * [17] L. Fan, X. Xiong, F. Wang, N. Wang, and Z. Zhang, “Rangedet: In defense of range view for lidar-based 3d object detection,” in _Proc. IEEE/CVF International Conference on Computer Vision_ , 2021.
  * [18] G. P. Meyer, A. Laddha, E. Kee, C. Vallespi-Gonzalez, and C. K. Wellington, “Lasernet: An efficient probabilistic 3d object detector for autonomous driving,” in _Proc. IEEE/CVF Conference on Computer Vision and Pattern Recognition_ , 2019, pp. 12 677–12 686.
  * [19] T. Collins and A. Bartoli, “Infinitesimal plane-based pose estimation,” _International journal of computer vision_ , vol. 109, no. 3, pp. 252–286, 2014.
  * [20] Y. Liu, H. Schofield, and J. Shan, “Navigation of a self-driving vehicle using one fiducial marker,” in _Proc. IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems_ , 2021, pp. 1–6.
  * [21] P. Jin, P. Matikainen, and S. S. Srinivasa, “Sensor fusion for fiducial tags: Highly robust pose estimation from single frame rgbd,” in _Proc. IEEE/RSJ International Conference on Intelligent Robots and Systems_ , 2017, pp. 5770–5776.
  * [22] S.-F. Ch’ng, N. Sogi, P. Purkait, T.-J. Chin, and K. Fukui, “Resolving marker pose ambiguity by robust rotation averaging with clique constraints,” in _Proc. IEEE International Conference on Robotics and Automation_ , 2020, pp. 9680–9686.
  * [23] R. A. Haddad, A. N. Akansu _et al._ , “A class of fast gaussian binomial filters for speech and image processing,” _IEEE Transactions on Signal Processing_ , vol. 39, no. 3, pp. 723–727, 1991.
  * [24] K. S. Arun, T. S. Huang, and S. D. Blostein, “Least-squares fitting of two 3-d point sets,” _IEEE Transactions on pattern analysis and machine intelligence_ , no. 5, pp. 698–700, 1987.
  * [25] H. Xie, H. Yao, S. Zhou, J. Mao, S. Zhang, and W. Sun, “Grnet: Gridding residual network for dense point cloud completion,” in _Proc. European Conference on Computer Vision_ , 2020, pp. 365–381.