\section{Automatic Calibration System}
\label{sec:AutomaticCalibrationSystem}
Target-based LiDAR-camera~ extrinsic calibration methods often suffer from manual target
extraction and feature association. The only manual process in our previous software
release \cite{huang2019improvements}, determining the image corners, has now been
made automatic. Because experimentally comparing and validating the intrinsic
calibration methods of Sec.~\ref{sec:IntrinsicCalibration} and~\ref{sec:Baseline}
would require significant data collection, we decided to create a fully automatic
pipeline for both intrinsic and extrinsic calibration, as shown in
Fig~\ref{fig:AutomaticPipeline}. We demonstrate the capability of this pipeline in
the experimental section by collecting a significant amount of data allowing us to
check the quality of the calibration of our sensor suite (a LiDAR-camera~ pair) at different
distances; see Sec.~\ref{sec:EXPExtrinsicCalibration}.


\begin{figure}[t]
    \centering
    \includegraphics[width=0.7\columnwidth]{graphics/pipeline2.png}
    \caption{A system diagram for
        automatic intrinsic and extrinsic calibration. The top shows the front-end of
        the pipeline. Its input is raw camera and LiDAR~ data, which are subsequently
        synchronized. The AprilTag~ and LiDARTag~ packages are used to extract the target
        information, which is then examined to remove outliers and ensure that
        targets are still properly synchronized. Each run of the front-end saves all
        related information into a ROS bagfile, which can then be sent to the
        back-end for further processing. The back-end takes in (possibly many) ROS
        bagfiles and does the following: (i) refines the image corners; and (ii)
        extracts vertices of the LiDAR~ targets. The correspondences are established
        between corners and vertices and an extrinsic transformation is determined
        PnP as in \cite{lepetit2009epnp}. For intrinsic calibration, the resulting vertices of
        the LiDAR~ targets are used to extract normal vectors and a point on the
        plane. The calibration parameters are determined to minimize the P2P distance
        from the plane to the target points provided by the LiDARTag~ package.} 
\label{fig:AutomaticPipeline}
\end{figure}


\subsection{Front-End}
Fiducial markers are widely used in vision-based perception tasks because they can
be easily identified and automatically extracted from the background.  Inspired by AprilTags \cite{olson2011apriltag, wang2016apriltag,
krogiusflexible}, we introduced a similar concept for LiDAR~ point clouds,
called a LiDARTag, consisting of a payload and a pattern that
makes each marker uniquely distinguishable and identifiable in real-time by both a
LiDAR~ and a camera 
\cite{huang2019lidartag}. An open-source package for processing LiDARTags~ is available
\cite{githubFileLiDARTag}, while \atagN3 is available as a ROS package
\cite{githubFileAprilTagROS}. 

As LiDAR~ and camera data streams arrive, they are synchronized, examined for
fiducial markers, and checked for false positives. For LiDAR-camera~ extrinsic calibration, we use target
vertices and image corners as features. The relevant data (images, AprilTag corners, points on
LiDARTags) are saved as ROS bagfiles for processing in MATLAB. Technical details and software are
available at~\cite{githubFileAutoCalibration}.


\subsection{Back-End}

The back-end begins by estimating the LiDARTag~ vertices, which, due to sparsity of the LiDAR~ point cloud, are not directly observable. We use the method developed in
\cite[Sec.~II]{huang2019improvements} to determine the vertices of a diamond-shaped target. A rigid-body transformation is determined that ``best fits'' the target's LiDAR~ points to the target's known geometry. Best fit is defined in an $L_1$ sense. 


From the vertices, the target normal $\n[t]$ and point on the target, $\p[0,t]$, can
be easily determined for evaluating the P2P cost function \eqref{eq:OptSolution} for intrinsic calibration, while the vertices in combination with the camera corners are used to determine the extrinsic calibration via a PnP problem. 




 The corners obtained from the AprilTag~ package are automatically refined using a process detailed in \cite[Sec.V-C]{huang2019improvements}. Given the camera corners, denoted ($\{ Y_i\}_{i=1}^4$), and LiDAR~ vertices, denoted $\{ X_i\}_{i=1}^4$, we use the vertical and horizontal positions in their own coordinates to sort them and establish the correspondences.


\subsection{Intrinsic and Extrinsic Calibration}
\label{sec:InExCalibration}
For extrinsic calibration, the rigid body transformation between LiDAR~ to camera is found by solving a Perspective-n-Point (PnP) problem as in \cite{lepetit2009epnp}, namely 
\begin{equation} 
    \label{eq:ConceptualProblem} (\R[L][C*], \t[L][C*])= \mathrm{arg}
    \min_{(\R,\t)}{\mathrm{dist}(P(\R X_i + \t),Y_i)}, 
\end{equation} 
where $X_i$ are coordinates of the LiDAR~ vertices, $Y_i$ are the
coordinates of the camera corners, and $P$ is the standard 3D$\to$2D camera ``projection map'', based in its intrinsic parameters. Furthermore, $\R[L][C],\t[L][C]$ is the LiDAR-frame to camera-frame rigid body transformation, and $\mathrm{dist}$ is a distance or error
measure (typically $L_2$). For example results, see~\cite{huang2019improvements}.

With the method in \cite{huang2019improvements}, the target vertices are automatically co-planar and hence uniquely define a unit normal vector and a
point on the target. The P2P distance is minimized for each of the three calibration
models, \BL1, \BL2, and $\mathrm{Sim}(3)$, for a collection of targets, yielding $\alpha^*$
(for \BL1 and \BL2) and  $\H[*]$ for $\mathrm{Sim}(3)$.

For experimental data, the process of intrinsic calibration involves iterating the estimation of the
target vertices and the parameters in the LiDAR~ model until a given stopping
criterion is met. Here, we stopped when the maximum change $\delta_m$ over the set of target vertices was less
than $1e-5$.

\begin{algorithmic}
\While{$\delta_m>10^{-5}$} 
\State \textbf{Step 1:} Re-estimate the target vertices $\{ X_i\}_{i=1}^4$
after applying the current intrinsic calibration parameters to the point
cloud. This gives updated values for the target normals and points on the
targets.
\State \noindent \textbf{Step 2:} Using the updated target vertices, re-estimate the intrinsic calibration parameters for the three calibration models.
\EndWhile
\end{algorithmic}
For simulation data, no iteration is required as the target information is known from ``ground truth''.




\section{Conclusions}
\label{sec:Conclusions}
We proposed a universal method for LiDAR intrinsic calibration that abstracts away
the physics of a LiDAR~ type (spinning head vs. solid-state, for example) and focuses
instead on the spatial geometry of the point cloud generated by the sensor. The
calibration parameter becomes an element of $\mathrm{Sim}(3)$, a matrix Lie group. We
mathematically prove that given four targets with appropriate orientations, the
proposed model is well-constrained (i.e., a unique answer exists). Because $\mathrm{Sim}(3)$
is closely related to $\mathrm{SE}(3)$, we showed how to profitably apply efficient, globally
convergent algorithms for $\mathrm{SE}(3)$ to determine a solution to our problem in
$\mathrm{Sim}(3)$. The resulting algorithm was evaluated in simulation for a solid-state
LiDAR and simulation and experiment for a spinning LiDAR. The P2P distance of the validation
scene for the spinning LiDAR~ was reduced 44.7\% and 68.6\% in simulation and
experiment, respectively. The P2P distance of the validation scene for the solid-state LiDAR~
was reduced by 48.7\%. Both simulation and experiments showed that the proposed method
can serve as a generic model for intrinsic calibration of both spinning and
solid-state LiDARs. 


\comment{
and nicely generalized previous parametrization
models (from 3-parameter to 10-parameter models) into a unifying problem that can
solve both spinning LiDARs~ and solid-state LiDARs. We also mathematically proved that when solving
a transformation in $\mathrm{Sim}(3)$, a minimum of four appropriately orientated targets are
required to ensure a unique answer exists. The results of the proof provide a
guideline for target positioning where the four target should form a tetrahedron.
We introduced a global algorithm to find the unique answer of the intrinsic
calibration problem. The algorithm utilizes the bisection algorithm to solve the
scaling parameter and the rest of the problem is formulated as QCQP where Lagrangian
duality is used. The relaxed problem becomes an SDP and convex. Therefore, the
problem can be solved efficiently and globally.

\add{
We developed a novel LiDAR intrinsic calibration method that uses the spatial
geometry of the point cloud generated by the sensor. The calibration parameter was
modeled using $\mathrm{Sim}(3)$ Lie group and nicely generalized previous parametrization
models (from 3-parameter to 10-parameter models) into a unifying problem that can
solve both spinning LiDARs~ and solid-state LiDARs. We also mathematically proved that when solving
a transformation in $\mathrm{Sim}(3)$, a minimum of four appropriately orientated targets are
required to ensure a unique answer exists. The results of the proof provide a
guideline for target positioning where the four target should form a tetrahedron.
We introduced a global algorithm to find the unique answer of the intrinsic
calibration problem. The algorithm utilizes the bisection algorithm to solve the
scaling parameter and the rest of the problem is formulated as QCQP where Lagrangian
duality is used. The relaxed problem becomes an SDP and convex. Therefore, the
problem can be solved efficiently and globally.
}

\add{
We extensively validated this finding both in simulation and experiments. The P2P
distance of the validation scene for the spinning LiDAR~ was reduced \bh{44.7\%} and
\bh{68.6\%} in simulation and experiments, respectively. The P2P distance of the
validation scene for the solid-state LiDAR~ was reduced 48.7\%.
Both simulation and experiments showed that the proposed method can serve as a
generic model for intrinsic calibration of both spinning and solid-state LiDARs. 
}
}



\section{Experimental and Simulation Results}
\label{sec:SimulationResults}
This section first presents a simulation study of the intrinsic calibration problem
that will show the importance of properly constructed scenes for eliminating over
parameterization in two of the three calibration models. Results here will be used to
inform the experimental work. All experiments are conducted from a \textit{32-Beam Velodyne ULTRA Puck LiDAR~} and an
\textit{Intel RealSense Depth Camera D435}, mounted on an in-house designed torso for
a Cassie-series bipedal robot~\cite{CassieAutonomy2019ExtendedEdition}. 


\subsection{Simulated LiDAR~ Environment}
\label{sec:Simulator}
Due to lack of ground truth, we built a LiDAR~ simulator from scratch to compare our
calibration method against two baselines and to illustrate the role of target
positioning in the construction of a calibration environment. As an example of the
latter, Fig.~\ref{fig:PropositionIllustration} illustrates a scene where a unique
solution does not exist for two of the three calibration models, namely,
\eqref{eq:OptSolution} and \eqref{eq:ComplexCalibration}. Our simulator can model
LiDAR~ sensors of different working principles (spinning vs solid-state). With our
simulator, it is easy to control sources of uncertainty, including both mechanical
model uncertainty and measurement noise. Targets are assumed to be planar and
polygonal.

To simulate 3D points on a planar target, we generate rays from the LiDAR~ sensor,
and define a ``target'' point as the point at which the ray intersects the target.
The simulator has an option to account for shadowing or not. After locating the exact
LiDAR~ returns on the target, based on the LiDAR~ type, we then add two types of
uncertainty to the returns and report them as measured data. 

\begin{remark} 
    The simulator first finds all intersection points with the (infinite) plane
    defined by the target.  To determine if a point on the plane is within the
    boundary of the target polygon is a \add{well-known} problem in computer
    graphics, called the Point-In-Polygon (PIP) problem. The Winding Number algorithm
    \cite{alciatore1995winding} is implemented in this simulator. If the winding
    number of a point is not zero, the point lies inside the boundary; otherwise it
    is outside. 
\end{remark}

\subsection{Calibration Scenes}
\label{sec:AdequateEnvironment}
 To illustrate Theorem~\ref{thm:uniqueness} and the role of target selection and
 placement in the case of a spinning LiDAR, we define five different scenes, where
 five will be used for ``training'' and one will be used for ``validation''. Because
 the calibration is being conducted for a spinning LiDAR, the points are parsed
 ``per ring'' as discussed in Sec.~\ref{sec:IntrinsicCalibration}. The target sizes
 vary with distance to ensure a minimum of ten points per ring on each target.


\begin{table*}[t]
    \caption{\bh{Check again}Induce 3-parameter perturbation model $(\N[1])$,  6-parameter perturbation
model $(\N[2])$, and $\mathrm{Sim}(3)$ noise model $(\N[3])$ for a spinning LiDAR~ in the
simulator. The error is the P2P cost in meter and validated in a complex scene.} 
\label{tab:N3}
\center
\begin{tabular}{|c|c|c|c|c|c|c|c|}
\hline
\cellcolor[HTML]{C0C0C0}Noise Model &\cellcolor[HTML]{C0C0C0} Methods & \cellcolor[HTML]{C0C0C0}scene 1 & \cellcolor[HTML]{C0C0C0}scene 2 & \cellcolor[HTML]{C0C0C0}scene 3 & \cellcolor[HTML]{C0C0C0}scene 4 & \cellcolor[HTML]{C0C0C0}scene 5 & \cellcolor[HTML]{C0C0C0}scene 6 \\ \hline
\cellcolor[HTML]{C0C0C0} & \cellcolor[HTML]{C0C0C0}Baseline1 & 0.036998 & 0.037076 & 0.034804 & 0.032165 & 0.017921 & 0.028471 \\ \cline{2-8} 
\cellcolor[HTML]{C0C0C0} & \cellcolor[HTML]{C0C0C0}Baseline2 & 0.16108 & 0.15412 & 0.15745 & 0.035424 & 0.023221 & 0.023275 \\ \cline{2-8} 
\multirow{-3}{*}{\cellcolor[HTML]{C0C0C0}N1} & \cellcolor[HTML]{C0C0C0}Our Method & \textbf{0.020421} & \textbf{0.028072} & \textbf{0.013583} & \textbf{0.010428} & \textbf{0.0016329} & \textbf{0.0010792} \\ \hline
\cellcolor[HTML]{C0C0C0} & \cellcolor[HTML]{C0C0C0}Baseline1 & 0.024689 & 0.025205 & 0.027227 & 0.021967 & 0.010316 & 0.012955 \\ \cline{2-8} 
\cellcolor[HTML]{C0C0C0} & \cellcolor[HTML]{C0C0C0}Baseline2 & 0.14897 & 0.14993 & 0.14192 & 0.033905 & 0.012305 & 0.0086185 \\ \cline{2-8} 
\multirow{-3}{*}{\cellcolor[HTML]{C0C0C0}N2} & \cellcolor[HTML]{C0C0C0}Our Method & \textbf{0.015302} & \textbf{0.018722} & \textbf{0.012405} & \textbf{0.01152} & \textbf{0.003471} & \textbf{0.00076376} \\ \hline
\cellcolor[HTML]{C0C0C0} & \cellcolor[HTML]{C0C0C0}Baseline1 & 0.037758 & 0.037428 & 0.038231 & 0.033412 & 0.023682 & 0.031802 \\ \cline{2-8} 
\cellcolor[HTML]{C0C0C0} & \cellcolor[HTML]{C0C0C0}Baseline2 & 0.16082 & 0.15571 & 0.15756 & 0.04164 & 0.022272 & 0.024862 \\ \cline{2-8} 
\multirow{-3}{*}{\cellcolor[HTML]{C0C0C0}N3} & \cellcolor[HTML]{C0C0C0}Our Method & \textbf{0.02297} & \textbf{0.025774} & \textbf{0.012842} & \textbf{0.013986} & \textbf{0.0053697} & \textbf{0.00010772} \\ \hline
\end{tabular}
\end{table*}

\begin{itemize} 
    \item \bh{check all the scenes again.}Scene 1: Two targets are placed along the same side of the $x$-axis, parallel
    to each other and to the $z$-axis .
\item Scene 2: Two targets are placed at different distances and opposite sides along
    the $x$-axis of the LiDAR, parallel to one another and to the $z$-axis.
\item Scene 3: Four targets are placed at right angles. This models the use the
    common use of vertical walls for calibration. 
\item Scene 4: It is a complex scene for validation. Eight targets are placed at
    various distance and orientations. Four of them are close the LiDAR and the other
    four are far from LiDAR.
\item Scene 5: We place 12 targets at different distances with various angles. 
\item Scene 6: We place 24 targets at different distances with various angles. 
\end{itemize}



\begin{remark}
    The placement of only a few targets in a scene is done to demonstrate the ``minimum''
requirements to calibrate a spinning LiDAR. One can place as many targets as desired in
the simulator. 
\end{remark}

\begin{figure}[b]%
\centering
\subfloat{%
\label{fig:NormalsVSP2P}%
\includegraphics[width=0.95\columnwidth, trim={0cm 7cm 0cm 0cm},clip]{placeholder.png}}\\
\subfloat{%
    \label{fig:OrientationVSP2P}%
\includegraphics[width=0.95\columnwidth, trim={0cm 7cm 0cm 0cm},clip]{placeholder.png}}
\caption[]{\bhp{}}
\label{fig:P2PResults}
\end{figure}

\subsection{LiDAR~ Intrinsic Calibration of a Spinning LiDAR~}
\label{sec:SimSpingHead}
For each scene, we generate deterministic measurements by applying the following
perturbations to the data on a per-ring basis (i.e, values vary by ring number): (i)
simple model $(\delta_\rho, \delta_\theta, \delta_\phi)$, (ii) complex model $(\delta_\rho, \delta_\theta, \delta_\phi,
s, h, v)$, and (iii) $\mathrm{Sim}(3)$ model $(\R, \t, s)$, denoted as $\N[1], \N[2], \N[3]$,
respectively. The exact values used are in a \texttt{.mat} file uploaded to GitHub
\cite{githubFileIntrinsic}. For each target, its true normal vector $\n$ and point
$\p[0]$ on the target are assumed known so that the P2P cost functions can be
evaluated. For each of the three cost functions \eqref{eq:OptSolution},
\eqref{eq:SimpleCalibration}, \eqref{eq:ComplexCalibration}, the \texttt{fmincon}
function in MATLAB is used to determine calibration parameters achieving a local
minimum for each Scene. The initializations for \texttt{fmincon} are selected to
correspond to zero perturbation error, that is the parameters are zero, one or the
identify matrix as the case requires. The optimized calibration parameters are then
applied to the data of Scene 5 and the P2P cost of \eqref{eq:OptSolution} is
evaluated and reported as validation data in Table~\ref{tab:N3}.

The simulated results confirm that a single target results in degeneracy when
calibrating with the $\textbf{BL}_2$ and $\mathrm{Sim}(3)$ models, and while $\textbf{BL}_1$ works fine with
a single target, it drops in performance as the complexity of the calibration errors
is increased. The well-posedness of the $\textbf{BL}_2$ and $\mathrm{Sim}(3)$ calibration models was
confirmed when the conditions of Theorem.~\ref{thm:uniqueness} are respected.

\addd{To further analyze the proposed method and verify Theorem~\ref{thm:uniqueness},
    we consider the following two scenarios: \textbf{1)} We generate \bh{10 thousand}
    training scenes in which we randomly place 4 targets at different
    distances with various angles. \textbf{2)} We generate 10 thousand training
    scenes in which we randomly rotate the tetrahedron, as shown in
    Fig.~\ref{fig:oriented-tetrahedron}. We compute the condition number of normal
    vectors (Assumption~\ref{asm:assumtionN}), and the condition number of bases
    (Assumption~\ref{asm:assumtionB}) for both scenarios. The optimized calibration
    parameters are then applied to the data of Scene 5 and the P2P cost of
\eqref{eq:OptSolution} is evaluated. The results are reported in
Fig.~\ref{fig:P2PResults}. Again, we can
see, as expected, when the condition number of normal vectors
worsens, the validation P2P cost increases. However, when the condition number of bases increases, the
P2P cost does not increase as we expected. Therefore, in practice, 
a set of well-conditioned target's normal vectors is sufficient to calibrate a
spinning LiDAR.}






\subsection{LiDAR~ Intrinsic Calibration of a Solid State LiDAR~}
\label{sec:ExpSSLiDAR}
An OPA solid-state LiDAR~ is fabricated on a planar wafer with a large number of emitters, see
Sec.~\ref{sec:intro}. We induce geometric uncertainty into the system by assuming the
plane of the wafer is slightly warped. In the simulator, the OPA solid-state LiDAR~ has
$20\times20$ emitters with 160, 40 horizontal and vertical field of view,
respectively. We place four targets of the same size.
\addd{Theorem~\ref{thm:uniqueness} requires a ring plane, as mentioned in
    Sec.~\ref{sec:TargetPlacementGuideline}. We therefore parse the LiDAR~ returns to
an $20\times4$ uniform grid over the planar targets, so each of the grids can form its
own ring plane. This results in a total of 80 calibration models.} For each grid, the
cost function \eqref{eq:OptSolution} is optimized using the \texttt{fmincon} function
in MATLAB to determine calibration parameters achieving a local minimum. The
calibrated sensor is then validated on the other four scenes that have different
angles and distances. The mean P2P cost of \eqref{eq:OptSolution} improves 37\%, and
the result is shown in Fig.~\bh{fig:sslidar}.

\begin{figure}[t]%
\centering
\includegraphics[width=0.75\columnwidth, trim={0.5cm 1.0cm 0cm 1.5cm},clip]{validation-objs.jpg}
\caption{\bhi{}}
\label{fig:sslidar}%
\end{figure}

\begin{table}[t]
\caption{\bh{check all the scenes again.} Geometric uncertainty of a solid-state LiDAR~ by assuming the plane of the wafer is slightly warped.}
\label{tab:sslidar}
\center
\resizebox{\columnwidth}{!}{
\begin{tabular}{|
>{\columncolor[HTML]{C0C0C0}}c |c|c|c|}
\hline
Solid-State LiDAR & \cellcolor[HTML]{C0C0C0}Before Calibration & \cellcolor[HTML]{C0C0C0}After Calibration & \cellcolor[HTML]{C0C0C0}Improvement \\ \hline
Training results & 0.3315 & 0.0029 & 99.12 \% \\ \hline
Validation Scene1 & 0.2659 & 0.0197 & 92.59 \% \\ \hline
Validation Scene2 & 0.3193 & 0.0323 & 89.89 \% \\ \hline
Validation Scene3 & 0.2958 & 0.0202 & 93.16 \% \\ \hline
\end{tabular}
}
\end{table}



\begin{table}[]
\caption{Experiments data collected using different numbers of targets in the calibration scene.}
\label{tab:RealExp}
\center
\begin{tabular}{|c|c|c|c|c|}
\hline
\rowcolor[HTML]{C0C0C0}
{\color[HTML]{000000} No.Tags} & 4 & 8 & 12 & 24 \\ \hline
\cellcolor[HTML]{C0C0C0}{\color[HTML]{000000} Baseline1} & \textbf{0.0064} & \textbf{0.0066} & 0.0066 & 0.0068 \\ \hline
\cellcolor[HTML]{C0C0C0}{\color[HTML]{000000} Baseline2} & 0.0146 & 0.0103 & 0.0083 & 0.0082 \\ \hline
\cellcolor[HTML]{C0C0C0}{\color[HTML]{000000} Our Method} & {\color[HTML]{000000} 0.0201} & {\color[HTML]{000000} 0.0084} & {\color[HTML]{000000} \textbf{0.0062}} & {\color[HTML]{000000} \textbf{0.0052}} \\ \hline
\end{tabular}
\end{table}





\subsection{Experiments of Intrinsic Calibration}
\label{sec:EXPIntrinsicCalibration}
Informed by the simulation study, a set of LiDAR~ data was collected using the
LiDARTag~ package\cite{githubFileLiDARTag,huang2019lidartag} with targets arranged to
avoid the degeneracies indicated in Theorem~\ref{thm:uniqueness}. The targets' normal
vectors are estimated using the L1-inspired method in \cite{huang2020improvements}.
All together, 39 targets were collected as potential training data. On a separate
day, an additional 12 targets were collected for validation. Table~\ref{tab:RealExp}
reports validation data for the three calibration methods on various subsets of the
targets based on number of targets: four, seven, 10 of them.

It is seen that the trends observed in the simulation data for a spinning LiDAR~
continue to hold in experiments. We conclude that the proposed idea of reformulating
the calibration problem in terms of shape-preserving transformations on the observed
point cloud has been supported (i.e., not invalidated). With the open-source release
of our code and data, it will be straightforward for other groups to confirm or deny
this finding.


\section{Global Convergence and Convexity}

\section{Proposed LiDAR~ Intrinsic Model and Analysis}
\label{sec:IntrinsicCalibration}
The proposed calibration parameters are modeled as an element of the similarity Lie
group $\mathrm{Sim}(3)$, which can be applied to both
spinning LiDARs~ and solid-state LiDARs. An element of the group is an isometry composed with an additional
isotropic scaling, i.e., a rigid-body transformation with scaling. We note that this
is the most general form of of a shape-preserving transformation in 3D space (i.e. preserves ratios of lengths and angles). We also note that \eqref{eq:SimpleCalibration} and \eqref{eq:ComplexCalibration} are not shape preserving. It will be important to see in real data if our assumption of shape preservation is general enough to provide useful calibrations. 

An element of the group is given by
\begin{equation}
\label{eq:sim3tf}
        \H = \begin{bmatrix}
         s \R & \v \\
         \zeros & 1
     \end{bmatrix} \in \mathrm{Sim}(3),
\end{equation}
where $\R \in \mathrm{SO}(3)$ is the 3D rotation matrix, $\v \in \mathbb{R}^3$ is the
translation vector, and $s \in \mathbb{R}^+$ is the scale parameter. In particular,
an element has seven degrees of freedom and the action of $\mathrm{Sim}(3)$ on $\mathbb{R}^3$
is $\H \cdot \x = s\R \x + \v$, where $\x \in \mathbb{R}^3$ and where $\H \cdot \x$ uses homogeneous coordinates while $s\R \x + \v$ uses Cartesian coordinates. From hereon, wherever convenient, we abuse notation by passing from regular coordinates to homogeneous coordinates without noting the distinction.


\subsection{Point-to-Plane Distance}
\label{sec:KernelizedP2P}
Let $\n[t]$ be a unit normal vector of a planar target and let $\p[0, t]$ be a fixed
point on the target. Let $\mathcal{X} = \{ \x[i] | i=1,\dots,M~\text{and}~\x[i] \in
\mathbb{R}^3 \}$ denote a collection of LiDAR~ returns on the target, and $M$ be
the number of points in the collection. Given the collection of LiDAR~ returns $\mathcal{X}$,
the cost of the intrinsic calibration problem is commonly defined by the
P2P distance:
\begin{equation}
    \label{eq:P2PDistance}
    J_t := \sum_{i=1}^M J_i(\x[i], \n[t], \p[0,t]):= \sum_{i=1}^M
   \left( \n[t][\mathsf{T}] \left(\x[i] - \p[0,t] \right)\right)^2,
\end{equation}
where $\n[t][\mathsf{T}] \left(\x[i] - \p[0,t] \right)$ is the orthogonal projection
of the measurement onto the normal vector. Then the calibration problem can be formulated as follows
\begin{problem}
\label{prob:intrinsic_calibration}
For a given collection of points $\mathcal{X}_t = \{\x[i]\}_{i=1}^{M_t}$, possibly
from multiple targets $t\in\{1, \ldots, T\}$, we seek a similarity transformation
$\H[][\star]$ that solves
\begin{equation}
\label{eq:OptSolution}
    \min_{\H \in \mathrm{Sim}(3)}
   \sum_{t=1}^T\sum_{i=1}^{M_t}
    \left( \n[t][\mathsf{T}] \left( \H \cdot \x[i] - \p[0,t] \right) \right)^2.
\end{equation}
\end{problem}

\comment{
\addd{The underlying assumption of \eqref{eq:OptSolution} is the independence
of different collections of points, as each collection of points has its own
calibration parameters and there is no correlation between different collections. 
However, points from a solid-state LiDAR~ share the same geometry such as a wafer. Therefore,
the independence assumption is not true and leads to discontinuity of different
collection of points.}

\addd{We propose using all the solid-state LiDAR~ returns (possibly consisting of multiple
collections) on a target and kernelizing the points while calibrating a collection
of points. The kernelized P2P cost is defined as
\begin{problem}
\label{prob:KernelizedIntrinsicCalibration}
\add{Let the LiDAR~ returns ($\mathcal{PC}_t$) on a target composite of $K$ collections and each
collection has $M_k$ points and $\mathcal{PC}_t = \{\mathcal{X}_k|\mathcal{X}_k = \{\x[i]\in
\mathbb{R}^3|i=1,...M_k\}\}_{k=1}^K$ be the LiDAR~ returns of a target.} The
equation~\eqref{eq:OptSolution}becomes 
\begin{equation}
\label{eq:KernelizedOptSolution}
    \min_{\H \in \mathrm{Sim}(3)}
    \sum_{t=1}^T\sum_{k=1}^{K_t}\sum_{i=1}^{M_k}
   k(\x[i], \mu_k) \lvert \n[t][\mathsf{T}] \left( \H \cdot \x[i] - \p[0,t] \right)
   \rvert,
\end{equation}
where $\mu_k$ is the centroid of the $k$-th collection on the $t$-th target and
$k(\cdot, \cdot)$ is the squared exponential kernel~\cite[Chapter
4]{rasmussen2006gaussian}
\begin{equation}
\label{eq:Kernel}
k(\x[i], \mu_t) = \sigma\text{exp}\left(\frac{-(\x[i]-\mu_t)^2}{2l}\right),
\end{equation}
where $\sigma$ and $l$ are the signal variance the length scale, respectively.
\end{problem}
}
}

\begin{remark} The transformation $H$ applies to the entire collection of points $\mathcal{X}$. How to define the collection is addressed in Sec.~\ref{sec:CollectionOfPoints}.
\end{remark}

\begin{remark}
In practice each target's normal vector and a point on the target must be estimated
from data; see \cite{huang2019lidartag,huang2020improvements}.
\end{remark}


\subsection{Parsing Points on a Target to Collections of Points}
\label{sec:CollectionOfPoints}
The specific method for parsing a target’s point cloud and the number of calibration
transformations depends on the nature of the sensor. For a spinning LiDAR, the points $\mathcal{PC}$
on a target are typically separated by beam number so each beam has its own
optimization problem~\eqref{eq:OptSolution}. For example, for a $K$-beam spinning LiDAR, a natural choice for
the set of calibration parameters is $\{\H[k] | k=1,\dots,K~\text{and}~\H[k] \in
\mathrm{Sim}(3)\}$, where $H_k$ is the similarity transform for the $k$-th ring. Similarly, one could also split each ring into quarter circles ($90^\circ$ arcs), which would lead to $4K$ calibration parameters. On the one hand, for an OPA-based solid-state LiDAR, we propose to form an $m\times n$ grid over the planar target and
then parse the point cloud based on each point's projection onto the target along the
normal of each grid. This way, the set of calibration parameters becomes
$\{\H[k] | k=1,\dots,m\times n~\text{and}~\H[k] \in \mathrm{Sim}(3)\}$. On the other hand, one may wish to stick with parsing via ``rows or columns''. In the end, it's a choice that has to be made by the calibrator.



\squeezeup
\subsection{Overfitting, Underfitting, and Model Mismatch}
\label{sec:ModelDicussion}

We first point out that the proposed calibration model can suffer from overfitting (i.e., a
unique $\H[][*]$ does not exist) if less than four targets are used to calibrate a
LiDAR. In particular, for a single planar target, the rotation about the target
normal $\n[t]$, two components of the translation vector $\v$ (translation in the
plane of the target), and the scale $s$ are unconstrained. For two planar targets,
translation along the line orthogonal to the targets' normal vectors and scale are
unconstrained. Therefore, understanding how target positioning constrains the
calibration model is essential. As mentioned above, underfitting could occur if the
data is parsed into rings, but say one of the rings has distance errors that vary with rotation angle. In this case, splitting each ring into different arcs will eliminate the underfitting.

Our calibration model is not a phenomenological model. The existing published models postulate how a LiDAR works and hence are phenomenological models. These postulated models result in different numbers of parameters, ranging from three to eight. Our ``calibration model'' abstracts away from the physical device and focuses on transforming the point cloud so that the $(x,y,z)$ values are correct.

\subsection{Target Placement Guideline}
 \label{sec:TargetPlacementGuideline}
Theorem~\ref{thm:uniqueness} given below provides
practically realizable conditions under which a unique answer exists to
Problem~\ref{prob:intrinsic_calibration}. The proof is given in
Appendix~A in~\cite{huang2020global}. We further propose a method to globally compute the
unique solution, see Sec.~\ref{sec:ConvexRelaxation}.


\comment{
\addd{Theorem~\ref{thm:uniqueness} states that there exists one unique answer to
both Problem~\ref{prob:intrinsic_calibration} and
Problem~\ref{prob:KernelizedIntrinsicCalibration}. The proof is given in
Appendix~\ref{sec:Uniqueness}. We further introduce the Lagrangian Duality~ and formulate the
problem as a QCQP. The resulting problem becomes a convex SDP; therefore, 
the global optimum can be found, see Sec.~\ref{sec:ConvexRelaxation}.}
}

\begin{figure}[t]%
\centering
\includegraphics[width=0.9\columnwidth, trim={0.0cm 00cm 0cm 0cm},clip]{orientated_tetrahedron.png}
\caption{An oriented tetrahedron (black) and a ring plane (blue) for target positioning that satisfies the conditions of
Theorem~\ref{thm:uniqueness}.}
\label{fig:oriented-tetrahedron}%
\end{figure}

For a subset $\mathcal{S} \subset \mathbb{R}^3$, let $[\mathcal{S}] \text{ and } [\mathcal{S}]^\perp$ denote its span and
its orthogonal complement, respectively. Let $\{\e[1], \e[2], \e[3]\}$ be the canonical basis for
$\mathbb{R}^3$. We also denote $\P\subset\mathbb{R}^3$ a set traced out by a single ring (i.e., a ring plane) of
a perfectly calibrated spinning LiDAR. Without loss of generality, we assume
$\P=[\e[3]]^\perp$.


Consider four targets with unit normal vectors $\n[i]$ and let $\p[0,i]$ be a point
on the $i$-th target. For $1\le i \le 4$, the plane defined by the $i$-th target is
\begin{align*}
\V[i] &:= \p[0,i] + [\n[i]]^\perp\\
&:=  \p[0,i] + \{ v \in \mathbb{R}^3~|~ \n[i][\top] v =0  \},
\end{align*}
the plane formed by the set of vectors orthogonal to the normal vector $ \n[i]$, and subsequently translated by $\p[0,i]$.



\begin{assumptionN}[Normal Vectors]
    \label{asm:assumtionN}
    All sets of three distinct vectors from $\{ \n[1], \n[2], \n[3], \n[4], \e[3]\}$
    are linearly independent.
\end{assumptionN}

Under Assumption~\ref{asm:assumtionN}, for each $i \neq j \in \{1, 2, 3, 4 \}$, there exist unique points $\p[ij]:=\P \cap \V[i] \cap \V[j]$. All total, there are $\binom{4}{2} = 6$ such intersection points.


\begin{assumptionB}[Basis Vectors]
    \label{asm:assumtionB}
     Each of the following sets of vectors\footnote{There are $\binom{6}{2}=15$ possible pairs of vectors, of which we use 13 to establish uniqueness.} is a basis of the ring plane: 
    \begin{enumerate}[(a)]
        \item $\{ \p[12], \p[13] \}$, $\{ \p[13], \p[14] \}$, $\{ \p[14], \p[12] \}$,
        \item $\{ \p[12], \p[23] \}$, $\{ \p[23], \p[24] \}$, $\{ \p[24], \p[12] \}$,
        \item $\{ \p[13], \p[23] \}$, $\{ \p[23], \p[34] \}$, $\{ \p[34], \p[13] \}$,
        \item $\{ \p[14], \p[24] \}$, $\{ \p[24], \p[34] \}$, $\{ \p[34], \p[14] \}$,
        \item $\{ \p[14], \p[23] \}$
    \end{enumerate}
\end{assumptionB}

   


We note that for a perfectly calibrated LiDAR, if $\H$ is
         the identity element of $\mathrm{SE}(3)$, then for all $i
    \neq j \in \{1, 2, 3, 4 \}$, it must be true that $\H\cdot\p[ij] \in  \V[i] \cap
     \V[j].$ The following shows that the converse is true when the targets are appropriately positioned and oriented.




\begin{theorem}
    \label{thm:uniqueness}
    Assume that Assumptions~\ref{asm:assumtionN} and~\ref{asm:assumtionB}
     hold and let $\H \in \mathrm{Sim}(3)$. If for each $i
    \neq j \in \{1, 2, 3, 4 \}$, $\H\cdot\p[ij] \in  \V[i] \cap
     \V[j]$, then
     \begin{equation}
         \H =
         \begin{bmatrix}
            \I & \zeros \\
            \zeros & 1
         \end{bmatrix}.
     \end{equation}
 \end{theorem}

\textbf{Proof:} See Appendix~\ref{sec:Uniqueness} in~\cite{huang2020global}.\\
An immediate Corollary of Theorem~\ref{thm:uniqueness} is the uniqueness of the transformation, as stated below.\\
\textbf{Corollary}. Let $\widetilde{\H} \in \mathrm{Sim}(3)$ and define $\widetilde{\p}_{ij}:=\widetilde{\H}(\p[ij])$. Then under Assumptions~\ref{asm:assumtionN} and~\ref{asm:assumtionB}, the only element $\G \in \mathrm{Sim}(3) $ satisfying $\G(\widetilde{\p}_{ij}) \in \V[ij]$ all $i \neq j$ is $\G = \widetilde{H}^{-1}$.\\

Assumption~\ref{asm:assumtionN} assumes linear independence
($\binom{5}{3}=10$) for any three of the target normal vectors, but does not directly address their ``degree of independence,'' that is, their condition number.
Similarly, Assumption~\ref{asm:assumtionB} only involves linear independence. In practice, one wonders if it is important to position the targets so that the two sets of vectors are well conditioned? We will later show that the normal vectors are most important. In Fig.~\ref{fig:oriented-tetrahedron}, we show an oriented tetrahedron\footnote{Each of the four planar targets defines an
infinite plane. The infinite planes create a tetrahedron.} where the
normal vector of each face\footnote{Faces of the tetrahedron are extended planes
of the targets. Therefore, normal vectors of faces are normal vectors of targets.}
satisfies Assumption~\ref{asm:assumtionN} and the orientation of the
tetrahedron intersecting with the ring plane fulfills
Assumption~\ref{asm:assumtionB}.

   











\begin{remark}
    The plane defined by the $i$-th target is $\mathcal{V}_i:= \p[0,i] +
    \n[i][\perp]$. If we were to only use three targets, there would exist a unique point $\q[0] \in \mathbb{R}^3$ defined by
    \begin{equation}
    \label{eq:PoleOfCalibration}
        \q[0] :=\bigcap\limits_{i=1}^3 \mathcal{V}_i.
    \end{equation} 
    When \eqref{eq:PoleOfCalibration} holds, the scale factor, $s$, is not unique;
    see Fig.~\ref{fig:PropositionIllustration}. 
    In other words, a ring plane passing through the point $\q[0]$ leads to the scale factor $s$ being unconstrained and therefore a unique answer does not exist. Therefore, it was critical to analyze target placement for the proposed method. A similar analysis seems to be missing for the baseline methods.
   
   
   
\end{remark}

\begin{figure}[t]%
\centering
\includegraphics[width=0.9\columnwidth, trim={0cm 0cm 0cm 0cm},clip]{uniqueness3.png}
\caption[]{Illustration of a degenerate case. The black squares are 
different planes with the normal vectors being linearly independent. The dotted green lines indicate the intersection of two planes. The magenta circle shows the
LiDAR~ ring passing through $q_0$ (red center bullet) in \eqref{eq:PoleOfCalibration}. The thick blue, cyan
and black lines are the intersections of the ring planes and the targets. The P2P cost of
the intersections are therefore zero. The red lines show the results of 
applying a transformation $\H = \begin{bmatrix}s\I & (1-s)\q[0]\\\zeros&
1\end{bmatrix}$ to the intersection lines. The cost remains zero because the transformed lines
stay on the targets. 

\label{fig:PropositionIllustration}%
\squeezeup
\end{figure}










\section{Baseline Intrinsic Calibration Methods}
\label{sec:Baseline}
There are two standard calibration models for spinning LiDARs. They are
based on spherical coordinates $(\rho, \theta, \phi)$, referred to as range,
elevation and azimuth, 
\begin{equation}
\label{eq:spherical2Cartesian}
\begin{bmatrix}
x\\y\\z
\end{bmatrix} = f(\rho, \theta, \phi)
=\begin{bmatrix}
\rho\cos\theta\sin\phi\\
\rho\cos\theta\cos\phi\\
\rho\sin\theta
\end{bmatrix},
\end{equation}
and the inverse function, 
\begin{equation}
\label{eq:Cartesian2spherical2}
\begin{bmatrix}
\rho\\\theta\\\phi
\end{bmatrix} = f^{-1}(x,y,z)
=\begin{bmatrix}
    \sqrt{x^2+y^2+z^2}\\
    \asin[\frac{z}{\sqrt{x^2+y^2+z^2}}]\\
    {\rm atan2}(x,y)
\end{bmatrix}.
\end{equation}



\subsection{3-Parameter Model}
The most basic model~\cite{pandey2010extrinsic} assumes the LiDAR~ measurements are
made in spherical coordinates, $(\rho, \theta,\phi)$. Corrections to a measurement
are given by a collection of offsets \mbox{$\alpha:=(\delta_\rho, \delta_\theta,
\delta_\phi).$}  Expressing the calibrated measurement in Cartesian coordinates gives
\begin{equation}
\label{eq:SimpleCalibration}
        \Gamma_{\alpha}(\rho, \theta, \phi) := 
        \begin{bmatrix}
    (\rho+\delta_\rho)\cos(\delta_\theta)\sin(\phi-\delta_\phi)\\
    (\rho+\delta_\rho)\cos(\delta_\theta)\cos(\phi-\delta_\phi) \\
    (\rho+\delta_\rho)\sin(\delta_\theta)
        \end{bmatrix}
\end{equation}

\begin{remark} 
    \textbf{(a)} The nominal elevation for each ring is taken as zero;,
i.e., $\theta=0.$  \textbf{(b)} While it is not necessarily a drawback, most LiDAR~
 interfaces only return a Cartesian representation of a measured point. For example, the ROS driver from Velodyne only provides the Cartesian coordinate and the intensity of a point. Hence, the
user must assure the transformation to spherical coordinates, make the measurement
correction coming from the calibration, and then transform back to Cartesian
coordinates. 
\end{remark}

\squeezeup
\subsection{6-Parameter Model}
This model \cite{glennie2010static, nouira2016point} also works in spherical coordinates. In addition to the three offsets
above, it includes $s$, a scale factor of $\rho$, $h$, a horizontal offset of the
origin, and $v$, a vertical offset for the origin. The correction model becomes
\begin{equation}
\label{eq:ComplexCalibration}
         \bar{\Gamma}_{\alpha}:= 
\left[
\begin{array}{l}
    (s\rho+\delta_\rho)\cos\delta_\theta\sin(\phi-\delta_\phi) - h\cos(\phi-\delta_\phi)\\
    (s\rho+\delta_\rho)\cos\delta_\theta\cos(\phi-\delta_\phi) + h\sin(\phi-\delta_\phi)\\
    (s\rho+\delta_\rho)\sin\delta_\theta + v\\
\end{array}\right],
\end{equation}
and therefore $\alpha:=(\delta_\rho, \delta_\theta,
\delta_\phi, s, h, v).$ 

\subsection{Can these transformations be expressed by elements of $\mathrm{Sim}(3)$?}
The short answer is no. When viewed in Cartesian coordinates, the calibration model
\eqref{eq:ComplexCalibration} is fundamentally nonlinear and can be expressed as $\R[1](\R[2]\t[1] + \t[2])$,
where $\R[1], \R[2], \t[1] \text{ and } \t[2]$ depend on the measured point, 
$$\x[][\mathsf{T}] = [x,y,z] \xrightarrow[]{f^{-1}} (\rho, \theta, \phi),$$
\begin{align}
    \label{eq:ExpandComplexModelMatrixForm}
    \R[1]&=
             \begin{bmatrix}
                 \sin(\phi-\delta_\phi) & -\cos(\phi-\delta_\phi) & 0\\
                 \cos(\phi-\delta_\phi) &  \sin(\phi-\delta_\phi) & 0\\
                 0 &0 &1
             \end{bmatrix},\\ \nonumber
    \R[2]\t[1]+\t[2] &=
             \begin{bmatrix}
                 \cos(\delta_\theta) & 0 & -\sin(\delta_\theta)\\
                 0 &1 &0\\
                 \sin(\delta_\theta) & 0 & \cos(\delta_\theta)
             \end{bmatrix}
             \begin{bmatrix}
                 s\rho+\delta_\rho\\
                 0
                 \\0
             \end{bmatrix} +
             \begin{bmatrix}
                 0\\h\\
                 v
             \end{bmatrix}.
\end{align}
The calibration first applies a corrected elevation angle to a range value that has been offset and scaled. Next, the  $(y,z)$ position of the origin is offset, and lastly, a corrected rotation about the $z$-axis is applied.  





\squeezeup
\subsection{Cost function for baseline models}
\label{sec:ProblemStatement}

 For a given collection of points ${\cal PC}$, possibly from multiple targets
 $t\in\{1, \ldots, T\}$, we seek calibration parameters that solve 
\begin{equation}
\label{eq:OptSolutionBaseline}
    \min_{\alpha} 
   \sum_{t=1}^T\sum_{i=1}^{M_t} 
    | \n[t][\mathsf{T}] \left(F(\x[i], \alpha) - \p[0,t] \right) |,
\end{equation}
where 
\begin{enumerate}
       \item for baseline model $\textbf{BL}_1$~\cite{pandey2010extrinsic} in \eqref{eq:SimpleCalibration}, $\alpha =(\delta_\rho, \delta_\theta, \delta_\phi)$ and
     $$F({\x}, \alpha):= \Gamma_{\alpha} \circ f^{-1}({\x});$$
      \item for baseline model $\textbf{BL}_2$~\cite{glennie2010static, nouira2016point} in \eqref{eq:ComplexCalibration}, $\alpha =(\delta_\rho, \delta_\theta, \delta_\phi, s, h, v)$ and 
      $$F({\x}, \alpha):= \bar{\Gamma}_{\alpha} \circ f^{-1}({\x}).$$
\end{enumerate}

\begin{remark}
   For the 3-parameter model $\textbf{BL}_1$, a single planar target is sufficient to
   uniquely determine a calibration. For the 6-parameter model $\textbf{BL}_2$,
   non-uniqueness can occur as outlined in Theorem~\ref{thm:uniqueness} and
   Fig.~\ref{fig:PropositionIllustration}.
\end{remark}












\section*{Acknowledgment}
Toyota Research Institute (TRI) provided funds to support this work. Funding for J. Grizzle was in part provided by TRI and in part by NSF
Award No.~1808051. The first author thanks Wonhui Kim for useful conversations. The
authors thank Carl Olsson for kindly providing the experimental datasets
in~\cite{olsson2008solving}. 




\balance

\bibliographystyle{bib_/IEEEtran}

\section{Introduction and Contributions}
\section{ Introduction}
\label{sec:intro}


Camera and Light Detection And Ranging (LiDAR) sensors and supporting software are common system elements in current
robotic and autonomous systems. For real-world operation, such systems' performance depends on the quality of intrinsic and extrinsic calibration parameters. Intrinsic calibration of a sensor is the process of ensuring that
obtained measurements are meaningful and valid. Intrinsic calibration of cameras is
relatively mature~\cite{zhang2000flexible, liebowitz1998metric, stein1995accurate} and many open-source packages are available~\cite{maye2013self, opencv, ros}. Similarly, intrinsic calibration is also required for LiDARs; otherwise, the obtained point clouds are potentially inaccurate. However, even though spinning LiDARs~ have been on the market for over ten years, no publication supported by open-source calibration software is currently available, forcing academic researchers to either spend time developing their own solutions or to ignore concerns about the accuracy of their LiDAR~ point clouds. 


\begin{figure}[t]
\centering
\includegraphics[width=1\columnwidth]{FirstImg3.png}
\caption{Is it possible to calibrate a LiDAR~ without modeling the physical mechanism itself? This paper says YES it is and it allows a unifying means to calibrate LiDARs~ of all types. Top left: shows a simple physics-based calibration model for a spinning
    LiDAR. Top right: shows a similar model with more parameters, also for a
    spinning LiDAR. In the literature, the number of parameters can vary from three
    to ten. Bottom left and bottom right show, respectively, an optical phased array
    solid-state LiDAR~ and a MEMS-based solid-state LiDAR. There are many design options and many
    manufacturing steps to fabricate these LiDARs. This paper moves the attention to the
    geometry of the LiDAR's point cloud and proposes that calibration can be
    achieved via shape-preserving transformations.
 } 
\label{fig:FirstImage}
\squeezeup
\end{figure}

    Recently, another type of LiDAR, solid-state LiDAR, is being brought to the market.
    Industry is turning to solid-state LiDARs~ because they are small, quiet and produce less
vibration. Due to their small size, they can be nicely integrated with headlights or carried by mobile robots such as drones. Moreover, solid-state LiDARs~ are more reliable especially for automotive applications due to the absence of the rotating mechanism. Because spinning LiDARs~ have a relatively
simple working mechanism, it has been possible to propose physics-based calibration models~\cite{mirzaei20123d,pandey2010extrinsic,glennie2010static,
nouira2016point,chan2013temporal,atanacio2011lidar,
muhammad2010calibration,bergelt2017improving},
see Sec.~\ref{sec:SpinningLiDAR}. For solid-state LiDARs, however, multiple types and design options are being proposed (see Sec.~\ref{sec:SolidStateLiDAR}), and thus it is going to be difficult to hand-craft a calibration model for each design of a solid-state LiDAR. We ask, is it necessary to do this? Are type-specific calibration models even the right way to approach the problem?

In this work, we propose a unifying view of calibration for different types of
LiDARs, as shown in Fig.~\ref{fig:FirstImage}: we tie the calibration process to the data produced by the sensor, and not the sensor itself, by applying shape-preserving transformations to a LiDAR's point cloud. Additionally, we prove
mathematically that the proposed calibration model is well-constrained (i.e., a
unique answer exists), whereas existing physics-based calibration models do not guarantee a unique set of calibration parameters. The uniqueness proof for the proposed method provides a guideline for target
positioning. In addition, uniqueness of the parameters is necessary if one is to investigate a globally convergent parameter solver.



\comment{
In this work, we are concerned with the problem of
multi-beam LiDAR~ intrinsic calibration. We focus on narrow-pulsed time of flight
(ToF) method: spinning LiDARs~ and solid-state LiDARs, as shown in Fig.~\ref{fig:FirstImage}.
}

\squeezeup
\subsection{Our Contributions}
\label{sec:Contributions}
\begin{enumerate}
     \item We propose a model for intrinsic LiDAR~ calibration that is independent of the underlying technology. The new error model is based on
         shape-preserving transformations\footnote{Called \textit{similarity
         transformations} in projective geometry.}, which is a rigid body
         transformation plus a scale factor. Currently, intrinsic calibration models
         have been published for spinning LiDARs, while for solid-state LiDARs, the calibration models used by manufacturers have not found their way into the published literature. Current calibration models are tied to
         postulated physical mechanisms for how a LiDAR~ functions, resulting in
         anywhere from three to ten parameters to be estimated from data. Instead, we
         \textit{abstract away from the physics of a LiDAR~ type} (spinning head vs.
         solid-state, for example) and focus on the spatial geometry of the point
         cloud generated by the sensor. \textit{This leads to a unifying view of
         calibration} and results in an error model that can be represented as the
         action of the seven-dimensional matrix Lie group, $\mathrm{Sim}(3)$, on subsets of
         the measurements returned by the LiDAR.
   
   
   

     \item One may suspect that our proposed model is over parameterized. We, therefore, mathematically prove that given four targets with appropriate orientations, the proposed model is well-constrained (i.e., only one answer exists). The proof provides a guideline for target placement, which is an oriented tetrahedron.
     
     
   
   
   
   
   

    
    
    
    
    

     \item We give a globally convergent means to determine the calibration parameters. It utilizes an alternating optimization technique to solve the
         calibration problem, where the scaling parameter is determined by the
     bisection method, and the rest of the parameters ($\mathrm{SE}(3)$) are formulated as a
     Quadratically Constrained Quadratic Program (QCQP) where the Lagrangian dual
     relaxation is used. The relaxed problem on $\mathrm{SE}(3)$ becomes a Semidefinite Program (SDP) and
     convex~\cite{tron2015inclusion, carlone2015lagrangian, olsson2008solving,
     briales2017convex}. Therefore, the calibration problem can be solved globally and
     efficiently by off-the-shelf solvers~\cite{grant2014cvx}.

     \item We provide a MATLAB-based simulator for intrinsic LiDAR~ calibration and
        validation that allows one to study the effects of target placement and the
        effectiveness of various calibration methods.

    
    
    
     \item In simulations, we validate that positioning the four
         targets as a tetrahedron can calibrate equally well a
         spinning LiDAR~ and a solid-state LiDAR. In experiments on a spinning LiDAR,
         we show that the proposed method works as well as two physics-based models in terms of Point-to-Plane (P2P) distance, while it is more robust when we induce systematic errors to the experimental measurements.

     \item We open-source all of the related software for intrinsic
        calibration, including implementations of the baseline methods, the simulator,
        the data sets and the solver; see~\cite{githubFileIntrinsic,
        githubFileLiDARSimulator, githubFileGlobalSim3Solver}. 

\end{enumerate}


\squeezeup
\section{Related Work}
This section overviews related work as well as spinning and solid-state LiDARs. 

\squeezeup
\subsection{Spinning LiDAR~}
\label{sec:SpinningLiDAR}
A spinning LiDAR~ as used in this paper consists of $K$ laser emitter-receiver pairs
mounted at various positions and elevation angles on a rotating head, as shown in
Fig.~\ref{fig:FirstImage}. The lasers are pulsed at a fixed frequency. Each
revolution of the rotating head sweeps out a cone in space, called a LiDAR~ scan,
composed of discrete return points traced out by each pulsed emitter-receiver. The
points associated with a single laser beam are called a ring. 

The measurements are typically viewed as being made in a spherical coordinate system.
The range measurement is made by estimating time-of-flight (ToF) through a clocking system on a
circuit board, the azimuth is estimated from the position of the rotating head, and
the elevation angle of a beam is determined by its mounting angle on the rotating
head. 




Existing error models for LiDARs~ are phenomenological in nature, that is, they are
tied to physical explanations of a sensor's design and/or
operation~\cite{mirzaei20123d,pandey2010extrinsic,glennie2010static,
nouira2016point,chan2013temporal,atanacio2011lidar,
muhammad2010calibration,bergelt2017improving}. Broadly speaking, two common sources of
uncertainty considered in spinning LiDAR~ models are (1) errors in the
clocking systems, (one for ToF and one for pulsing the lasers) and (2) mechanical
errors associated with positioning of the beams on the spinning LiDAR. Less commonly considered are affects are due to ambient temperature or target size. Some authors
overlook that the measurements are actually made in a local coordinate frame for each
emitter-receiver pair, and are then transformed to a single ``global'' frame for the
LiDAR~ unit, resulting in models with three to six parameters
\cite{pandey2010extrinsic, muhammad2010calibration, glennie2010static}, while others
take this into account and use as many as ten parameters~\cite{bergelt2017improving}.
Moreover, none of these models provides a guideline for target placement so that
the calibration model is well-constrained (i.e., a unique solution exists), see
Sec.~\ref{sec:TargetPlacementGuideline}.


\squeezeup
\subsection{Solid-State LiDAR~}
\label{sec:SolidStateLiDAR}
In comparison  to spinning LiDARs, solid-state LiDARs~ are smaller, quieter, lower-cost, and
produce less vibration. Several types of solid-state LiDAR~ are coming to the market, including
those based on (1) Optical Phased Array (OPA) \cite{frederiksen2020lidar,
poulton2019long, poulton2018high}, and (2) Micro-Electro-Mechanical System (MEMS)
mirrors\footnote{Some authors argue MEMS-based solid-state LiDARs~ are not truly solid-state
devices due to the movable mirrors; the mirrors, however, are tiny compared to a
spinning LiDAR.}\cite{yoo2018mems, pelz2020matrix, druml20181d, lee2015optical,
wang2020mems, garcia2020geometric}. 

OPA-based solid-state LiDARs~ function similarly to a phased array in antenna theory. A single
OPA contains a significant number of emitters patterned on a chip. By varying the
phase shift (i.e., time delay) of each emitter, the direction of the resulting
wave-front or the spread angle can be adjusted. In particular, the sensor is able to
zoom in or out of an object on command. On the other hand, MEMS-based solid-state LiDARs~ use
only one or a few emitters, with the beams guided by a mirror system regulated by a
sophisticated controller. The controller seeks to steer the beam (or beams) in a
scanning pattern, such as a zig-zag \cite{yoo2018mems}. 

To perfectly align OPA emitters or MEMS mirrors is challenging. It is also hard to
maintain high accuracy under different temperatures and weather conditions. Thus, how
to properly calibrate a solid-state LiDAR~ is a critical problem.

The variety of solid-state LiDARs~ raises the question: is it necessary, or even possible, to
design/create parameterized models for every type of solid-state LiDAR? This paper seeks to
present a unifying view of LiDAR~ calibration by abstracting away from the physics of
a LiDAR~ type (spinning vs solid state, for example), and focusing instead on the
spatial geometry of the point cloud generated by the sensor. This leads to a unifying
view of calibration. 








       


\section{Introduction and Contributions}
\label{sec:intro}
Camera and Light Detection And Ranging (LiDAR) are common sensors for current
robotics and autonomous systems. For real-world operations, the performance of such
systems highly depends on the quality of intrinsic and extrinsic calibration
parameters. 
Intrinsic calibration of a sensor is the process of ensuring that obtained
measurements are meaningful and valid. Intrinsic calibration of a camera is
relatively mature and open-sourced packages are available~\cite{maye2013self,
itseez2015opencv, quigley2009ros}. In this work, we are concerned with the problem of
multi-beam LiDAR~ intrinsic calibration. We focus on narrow-pulsed time of flight
(ToF) method: spinning LiDARs~ and solid-state LiDARs, as shown in Fig.~\ref{fig:FirstImage}.
\addd{Figure~\ref{fig:SystematicError} shows a factory-calibrated high-end LiDAR~
(\textit{32-Beam Velodyne ULTRA Puck LiDAR}) measuring a planer target.} \add{The measurements are not consistent (mis-calibrated
    rings) and result in a point cloud with \bh{7 cm} thickness.
}

\jwg{Bruce will create tension! See writing video.}

\subsection{Spinning LiDAR~}
A spinning LiDAR~ as used in this paper consists of $K$ laser emitter-receiver pairs
mounted at various positions and elevation angles on the rotating head, as shown in
Fig.~\ref{fig:FirstImage}. The lasers are pulsed at a fixed frequency. Each
revolution of the rotating head sweeps out a cone in space, called a LiDAR~ scan,
composed of discrete return points traced out by each pulsed emitter-receiver. The
points associated with a single laser beam are also called a ring. 

The measurements are typically viewed as being made in a spherical coordinate system.
The range measurement is made by estimating ToF through a clocking system on a
circuit board, the azimuth is estimated from the position of the rotating head, and
the elevation angle of a beam is determined by its mounting angle on the rotating
head. 

\begin{figure}[t]
\centering
\includegraphics[width=1\columnwidth]{FirstFigure.png}
\caption{This figure motivates the main contribution of the
    paper, a means of calibrating a LiDAR~ without modeling the physical mechanism
    itself. Top left: shows a simple physics-based calibration model for a spinning
    LiDAR. Top right: shows a similar model with more parameters, also for a spinning
    LiDAR. In the literature, the number of parameters can vary from three to 10.
    Bottom left and bottom right show, respectively, an optical phased array solid-state LiDAR~ and a
    MEMS-based solid-state LiDAR. There are many design options and many manufacturing steps
    to fabricate these LiDARs~. Is it necessary, or even possible, to parameterize
    all of them? This paper moves the attention to the geometry of the LiDAR's
    point cloud  and proposes that calibration can be achieved via shape-preserving
    transformations. 
    In experiments, the proposed method outperforms the methods showed in top figures
for spinning LiDARs, and via simulation, we support that it may be used to
calibrate solid-state LiDARs~ as well.} 
\label{fig:FirstImage}
\squeezeup
\squeezeup
\end{figure}

\begin{figure}[t]%
\centering
\subfloat{%
\includegraphics[height=0.65\columnwidth, trim={0cm 0cm 0cm 0cm},clip]{BeforeCalibration.png}} 
\hfill
\subfloat{%
\includegraphics[height=0.65\columnwidth, trim={0cm 0cm 0cm 0cm},clip]{AfterCalibration.png}}~
\caption[]{On the left shows a factory-calibrated \textit{32-Beam Velodyne ULTRA Puck LiDAR~} measuring on a planer
    target. The measurements are not consistent and lead to a thickness of \bh{7 cm} point
cloud with \bh{4 cm} P2P distance. On the right shows the calibration results for the
proposed method. The thickness and P2P distance after calibration are \bh{3 cm} and
\bh{0.002 cm}.}%
\label{fig:SystematicError}%
\end{figure}


Existing error models for LiDARs~ are phenomenological in nature, that is, they are
tied to physical explanations of a sensor's design and/or
operation~\cite{mirzaei20123d,pandey2010extrinsic,glennie2010static,
nouira2016point,chan2013temporal,atanacio2011lidar,
muhammad2010calibration,bergelt2017improving}. Broadly speaking, two sources of
uncertainty are typically considered in spinning LiDAR~ models: (1) errors in the
clocking systems, (one for ToF and one for pulsing the lasers) and (2) mechanical
errors associated with positioning of the beams on the spinning LiDAR. Some authors
overlook that the measurements are actually made in a local coordinate frame for each
emitter-receiver pair, and are then transformed to a single ``global'' frame for the
LiDAR~ unit, resulting in models with three to six parameters
\cite{pandey2010extrinsic, muhammad2010calibration, glennie2010static}, while others
take this into account and use as many as ten parameters~\cite{bergelt2017improving}.
Moreover, none of these models provides a guideline for target placement so that
the calibration model is well-constrained (i.e., a unique solution exists), see
Sec.~\ref{sec:TargetPlacementGuideline}.


\subsection{Solid-State LiDAR~}
\label{sec:SolidStateLiDAR}
In comparison  to spinning LiDARs, solid-state LiDARs~ are smaller, quieter, lower-cost, and
produce less vibration. Several types of solid-state LiDAR~ are coming to the market, including
those based on (1) Optical Phased Array (OPA) \cite{frederiksen2020lidar,
poulton2019long, poulton2018high}, and (2) Micro-Electro-Mechanical System (MEMS)
mirrors\footnote{Some authors argue MEMS-based solid-state LiDARs~ are not truly solid-state
devices due to the movable mirrors; the mirrors, however, are tiny compared to a
spinning LiDAR.}\cite{yoo2018mems, pelz2020matrix, druml20181d, lee2015optical}. 

OPA-based solid-state LiDARs~ function similarly to a phased array in antenna theory. A single
OPA contains a significant number of emitters patterned on the chip. By varying the
phase shift (i.e., time delay) of each emitter, the direction of the resulting
wave-front or the spread angle can be adjusted. In particular, the sensor is able to
zoom in or out of an object on command. On the other hand, MEMS-based solid-state LiDARs~ use
only one or a few emitters, with the beams guided by a mirror system regulated by a
sophisticated controller. The controller seeks to steer the beam (or beams) in a
scanning pattern, such as a zig-zag \cite{yoo2018mems}. 

To perfectly align OPA emitters or MEMS mirrors is challenging. It is also hard to
maintain high accuracy under different temperatures and weather conditions. Thus, how
to properly calibrate a solid-state LiDAR~ is a critical problem.

The variety of solid-state LiDARs~ raises the question: is it necessary, or even possible, to
design/create parameterized models for every type of solid-state LiDAR? This paper seeks to
present a unifying view of LiDAR~ calibration by abstracting away from the physics of
a LiDAR~ type (spinning vs solid state, for example), and focusing instead on the
spatial geometry of the point cloud generated by the sensor. This leads to a unifying
view of calibration. 


\subsection{Our Contributions}
\label{sec:Contributions}
\begin{enumerate}
   
     \item We propose a model for intrinsic LiDAR~ calibration that is independent of the underlying technology. The new error model is based on
         shape-preserving transformations\footnote{Called \textit{similarity
         transformations} in projective geometry.}, that is a rigid body
         transformation plus a scale factor. Currently, intrinsic calibration models
         exist only for spinning LiDARs. The calibration models are tied to
         postulated physical mechanisms for how the LiDAR~ functions, resulting in
         anywhere from three to ten parameters to be estimated from data. Instead, we
         \textit{abstract away from the physics of a LiDAR~ type} (spinning head vs
         solid state, for example) and focus on the spatial geometry of the point
         cloud generated by the sensor. \textit{This leads to a unifying view of
         calibration} and results in an error model that can be represented as the
         action of the seven-dimensional matrix Lie group, $\mathrm{Sim}(3)$, on subsets of
         the measurements returned by the LiDAR.
   
   
   

     \item One may suspect that our proposed model is over parameterized. We therefore  mathematically prove that given four targets with appropriate orientations, the proposed model is well-constrained (i.e., only one unique answer exists). The proof provides a guideline for target placement, which is an oriented tetrahedron.
     
     
   
   
   
   
   

    
    
    
    
    

     \item We propose a globally convergent algorithm to determine the calibration parameters. It utilizes an alternating optimization technique to solve the
         calibration problem where the scaling parameter is determined by the
     bisection method and the rest of the parameters ($\mathrm{SE}(3)$) are formulated as a
     Quadratically Constrained Quadratic Problem (QCQP) where the Langrangian dual
     relaxation is used. The relaxed problem becomes a Semidefinite Program (SDP) and
     convex~\cite{tron2015inclusion, carlone2015lagrangian, olsson2008solving,
     briales2017convex}. Therefore, the problem can be solved globally and
     efficiently by off-the-shelf specialized solvers~\cite{grant2014cvx}.

     \item We provide a MATLAB-based simulator for intrinsic LiDAR~ calibration and
        validation that allows one to study the effects of target placement and the
        effectiveness of various calibration methods.

    
    
    
     \item In simulation, we validate that positioning the four
         targets as a tetrahedron can calibrate equally well a
         spinning LiDAR~ and a solid-state LiDAR. In experiments on a spinning LiDAR,
         we show that the proposed method works as well as two physics-based models in terms of P2P distance, while it is more robust when we induce systematic errors to the experimental measurements.
    
     \item We open-source all of the related software for intrinsic
        calibration, including implementations of the baseline methods, the simulator and the solver; see~\cite{githubFileIntrinsic, githubFileLiDARSimulator}\bh{add solver}. 

\end{enumerate}








\section{Lagrangian Duality Relaxation \\for P2P Distance on $\mathrm{SE}(3)$}
\label{sec:DualFormulation}
At the $k$-th iteration, the scaling parameter is determined (see
Sec.~\ref{sec:ConvexRelaxation}) and the rest of the parameters are $\mathrm{SE}(3)$. To
solve the remaining parameters, we adopt techniques that were used to solve 3D
registration or 3D SLAM~\cite{tron2015inclusion, carlone2015lagrangian,
olsson2008solving, briales2017convex}. We summarize below for completeness.
The action of $\mathrm{SE}(3)$ on $\mathbb{R}^3$ can be rewritten as:
\begin{equation}
    \H\cdot\x = \R\x+\v =
    \begin{bmatrix}
        \x[][\top] \otimes I_3 & I_3
    \end{bmatrix}
    \underbrace{\begin{bmatrix}
    \mathrm{vec}{\R} \\\v
    \end{bmatrix}}_{\tau},
\end{equation}
where $\otimes$ and $\mathrm{vec}{\cdot}$ are the Kronecker
product~\cite{brewer1978kronecker} and the vectorization
operation~\cite{NotesOnMatrix}, respectively. 

\subsection{P2P Distance Reformulation and Quadratic Formulation}
The P2P distance~\eqref{eq:P2PDistance} can be equivalently reformulated into a
quadratic form:
\begin{align}
    \label{eq:QuadraticForm1}
    \sum_{i=1}^M J_i &:= \sum_{i=1}^M|(\H\cdot \x[i]-\p[0,t])^\top (\n[t]\n[t][\top]) (\H\cdot\x[i]\p[0,t])|\nonumber\\
                     &= \sum_{i=1}^M\begin{bmatrix}\tau\\1\end{bmatrix}^\top N_i^\top
                     (\n[t]\n[t][\top]) N_i \begin{bmatrix}\tau\\1\end{bmatrix} =
                 \tilde{\tau}^\top W_t \tilde{\tau},
\end{align}
where $N_i = \begin{bmatrix} \x[i]\otimes I_3|-\p[0,t]\end{bmatrix}$ and
$W_t=\sum_{i}^{M}N_i^\top(\n[t]\n[t][\top])N_i$.  After rearranging
\eqref{eq:QuadraticForm1}, the resulting problem in \eqref{eq:OptSolution} becomes
\begin{align}
    \label{eq:QuadraticForm2}
    &\min_{H\in\mathrm{SE}(3)} \sum_{t=1}^T
    \begin{bmatrix}
        \mathrm{vec}{\R}\\1\\\v
\end{bmatrix}^\top
\tilde{W}_t^\prime
\begin{bmatrix}
    \mathrm{vec}{\R}\\1\\\v
\end{bmatrix}\nonumber\\
&= \min_{H\in\mathrm{SE}(3)} \sum_{t=1}^T
    \begin{bmatrix}
        \tilde{\r}\\\v
\end{bmatrix}^\top
\begin{bmatrix}
    \tilde{W}_{\tilde{\r},\tilde{\r}} & \tilde{W}_{\tilde{\r},\v}\\
    \tilde{W}_{\v,\tilde{\r}} & W_{\v,\v}
\end{bmatrix}_t
    \begin{bmatrix}
        \tilde{\r}\\\v
\end{bmatrix}\nonumber\\
&=\min_{H\in\mathrm{SE}(3)}\tilde{\r}^\top\tilde{W}_{\tilde{\r},\tilde{\r}}\tilde{\r} + 2\v[][\top]\tilde{W}_{\v,\tilde{\r}}\tilde{\r}+\v[][\top] W_{\v,\v} \v
\end{align}
where $\tilde{\r} = \begin{bmatrix}\mathrm{vec}{\R}^\top&1\end{bmatrix}^\top$. We then introduce the Lagrangian
multipliers. Due to $R\in\mathrm{SO}(3)$ constraints, the derivative with respect to $\v$ is
zero: $\partial \mathrm{L}(H, \lambda)/\partial \v=0$, which leads to
\begin{equation}
\label{eq:optimalT}
\v[][*] = -\inv{(W_{\v,\v})}\tilde{W}_{\v,\tilde{\r}}\tilde{\r}.
\end{equation}
By substituting \eqref{eq:optimalT} into \eqref{eq:QuadraticForm2}, we have
\begin{equation}
    \label{eq:QuadraticForm3}
    \min_{\R\in\mathrm{SO}(3)} \tilde{\r}^\top \tilde{\Q} \tilde{\r},
\end{equation}
where $\tilde{\Q}$ is the Schur complement of $\tilde{W}_t$ and equal to
$\tilde{W}_{\tilde{\r},\tilde{\r}}-\tilde{W}_{\tilde{\r},\v}W^{-1}_{\v,\v}\tilde{W}_{\v,\tilde{\r}}$.



\subsection{Primal Problem and Its Dual}
From~\cite{tron2015inclusion,carlone2015lagrangian,briales2017convex}, we re-define
\eqref{eq:QuadraticForm3} to an \textit{equivalent, homogeneous, strengthened} primal
problem:
\begin{align}
    \label{eq:Primal}
    &\min_{R}f(\tilde{\q}) = \min_{R} \tilde{\q}^\top \tilde{\Q}\tilde{\q},~\tilde{\q}=\begin{bmatrix}\mathrm{vec}{\R}^\top
    & y\end{bmatrix}^\top\\
    \text{s.t.~~}&R^\top R=y^2 I_3\nonumber\\
    &RR^\top=y^2I_3\nonumber\\
    &R^{(i)}\times R^{(j)}=yR^{(k)},~i,j,k=\text{permute}\{1,2,3\}\nonumber\\
    &y^2=1\nonumber.
\end{align}
The primal problem \eqref{eq:Primal} is a QCQP and the corresponding dual
problem is defined as
\begin{equation}
    \mathrm{L}(\tilde{\q}, \boldsymbol \lambda) = \gamma + \tilde{\q}^\top (\tilde{\Q}
    + \tilde{\P}(\boldsymbol \lambda))\tilde{\q} = \gamma + \tilde{\q}^\top \Z\tilde{\q},
\end{equation}
where $\P$ is the penalization matrix~\cite{briales2017convex}. The Lagrangian
relaxation is an unconstrained problem and has a closed-form solution:
\begin{align}
    \label{eq:Dual}
    g(\boldsymbol \lambda)  &= \min_{\tilde{\q}}\mathrm{L}(\tilde{\q}, \boldsymbol
    \lambda) = \min_{\tilde{\q}} \gamma + \tilde{\q}^\top \Z\tilde{\q}\\
                            &= \begin{cases}
                                \gamma, \text{~if~} \Z \succeq 0\\
                                -\infty, \text{~otherwise}.
                            \end{cases}
\end{align}
Therefore, the maximization of the dual problem \eqref{eq:Dual} is a SDP:
\begin{equation}
\label{eq:Dual2}
    g^* = \max_{\boldsymbol \lambda} \gamma, \text{~s.t.~} \Z(\boldsymbol \lambda)\succeq 0
\end{equation}
This problem is convex and can be solved globally by off-the-shelf specialized
solvers~\cite{grant2014cvx}. It is shown in \cite{briales2017convex} that the
relaxation is empirically always tight (the duality gap is zero). 






\begin{figure*}[b]%
\centering
\subfloat{%
    \label{fig:RandomBefore}%
\includegraphics[width=0.3\textwidth, trim={0cm 0cm 0cm 0.1cm},clip]{Random1.png}}~
\subfloat{%
\label{fig:RubikCubeBefore}%
\includegraphics[width=0.3\textwidth, trim={0cm 0cm 0cm 0cm},clip]{Cube1.png}}~
\subfloat{%
    \label{fig:SpaceStationBefore}%
\includegraphics[width=0.3\textwidth, trim={0cm 0cm 0cm 0cm},clip]{SpaceStation1.png}}\\
\subfloat{%
    \label{fig:RandomAfter}%
\includegraphics[width=0.3\textwidth, trim={0cm 0cm 0cm 0cm},clip]{Random2.png}}~
\subfloat{%
    \label{fig:RubikCubeAfter}%
\includegraphics[width=0.3\textwidth, trim={0cm 0cm 0cm 0cm},clip]{Cube2.png}}~
\subfloat{%
    \label{fig:SpaceStationAfter}%
\includegraphics[width=0.3\textwidth, trim={0cm 0cm 0cm 0.2cm},clip]{SpaceStation2.png}}
\caption[]{This figure shows the proposed method can be used in 3D registration
problems. The correspondences are point-to-point (marked in blue and red),
point-to-line (marked in blue and black), and point-to-plane (not shown to keep
the figure readable). The top and bottom row is the initial status
and the results of the registration problems, respectively. The simulation (Random)
with five point-to-point and two point-to-line correspondences are shown in the left
column. The result of the experimental data (RubikCube) with three point-to-point and
two point-to-plane correspondences is shown in the second column. The last column
shows the result of the experimental data (SpaceStation) with ten point-to-point and 12
point-to-line correspondences.
}
\label{fig:Resgistration}
\end{figure*}




\newpage
















\section{Observations on Global Convergence and Convexity}
\label{sec:ConvexityFigures}
In this section, we provide numerical results that suggest the proposed method is also
applicable to 3D registration problems and may achieve global convergence. Additionally,
we show more figures about the potential convexity of \eqref{eq:minS}.






\subsection{Toward Global Convergence of the Proposed Method}
We show that the proposed method in the experimental data collected by the \textit{32-Beam Velodyne ULTRA Puck LiDAR~} 
reduces the P2P distance by 68.6\%, as shown in Fig.~\ref{fig:SystematicError}.
In addition, we illustrate the proposed algorithm can be used in 3D
registration problems (point-to-point, point-to-line, point-to-plane) by scaling the
simulation (named Random) and experimental data (named RubikCube and SpaceStation) in
\cite{olsson2008solving, briales2017convex}. The results of the registration problems
are shown in Fig.~\ref{fig:Resgistration} and the experiment setup from
\cite{olsson2008solving} is shown in Fig.~\ref{fig:SpaceStationExpSetup}.

\begin{figure}[b]%
\centering
\includegraphics[width=0.81\columnwidth, trim={0.0cm 0cm 0cm 0cm},clip]{SpaceStationExpSetup.png}
\caption{This figure is taken from \cite{olsson2008solving} to illustrate the
experimental setup.} 
\label{fig:SpaceStationExpSetup}%
\end{figure}



\newpage
\subsection{Potential Convexity of $f(s)$ and Bisection Method}
Theoretically, a dense search on the scaling $s$ should be performed on a range of
interest. However, we have empirically observed that \eqref{eq:minS} is convex for all of
our data sets.
Figure~\ref{fig:convexIntrinsicAll} shows $f(s)$ is convex for all the sets of the
calibration parameters for our LiDAR~ intrinsic calibration datasets. Similarly, in
Fig.~\ref{fig:Convex3DResgistration} shows the convex property of $f(s)$ for the
simulation and experimental data in \cite{olsson2008solving, briales2017convex}. We, therefore, utilize the bisection method
to determine the scaling $s$. It is emphasized that the convexity of
$\eqref{eq:minS}$ is unknown at this time.

Let $\pre[k][L]s$ and $\pre[k][U]s$ be the lower and upper bounds
of the scaling parameters at the $k$-th iteration. Let $\pre[k+1]s =
(\pre[k][U]s + \pre[k][L]s)/2$ be the update for $(k+1)$-th iteration. The
updates of the scaling parameters are defined as:
\begin{equation}
\begin{aligned}
    \label{eq:UpdateScaling}
    \pre[k+1][L]s &=
\begin{cases}
    \pre[k+1]s, \text{~if~}\nabla f(\pre[k+1][]s) < -\epsilon\\
\pre[k][L]s, \text{~~~~~~otherwise}
\end{cases}\\
    \pre[k+1][U]s &=
\begin{cases}
    \pre[k+1]s, \text{~if~}\nabla f(\pre[k+1][]s) > \epsilon \\
\pre[k][U]s, \text{~~~~~~otherwise}
\end{cases},
\end{aligned}
\end{equation}
where the symmetric difference is used to approximate $\nabla f(\pre[k+1][]s)$:
\begin{equation}
    \label{eq:gradApprox}
    \nabla f(\pre[k+1]s)\approx \frac{f(\pre[k+1]s+h) - f(\pre[k+1]s-h)}{2h},
\end{equation}
where $h>0$ is a small value (taken as $10^{-3}$).



\begin{remark}
    To obtain more accurate results, $\R(\pre[k+1]s + h),~\v(\pre[k+1]s + h),~\R(\pre[k+1]s - h)$ and $\v(\pre[k+1]s - h)$ should be computed separately.
   
   
\end{remark}











\begin{figure}[b]%
\centering
\subfloat[]{%
\label{fig:convexIntrinsicAll}%
\includegraphics[width=0.95\columnwidth, trim={0cm 0cm 0cm 0cm},clip]{Convex-all.png}}\\
\subfloat[]{%
\label{fig:Convex3DResgistration}%
\includegraphics[width=0.95\columnwidth, trim={0cm 0cm 0cm 0cm},clip]{Convex-all2.png}}
\caption[]{
    This figure shows $f$ vs $s$ for the calibration parameters. The top figure shows
    the calibration parameters for all the rings, for $f$ defined in \eqref{eq:minS}. 
    The bottom figure show $f$ for the 3D registration problems (point-to-point,
    point-to-line, point-to-plane). We suspect that the convex shape seen in the plot
is true in general. On going work is seeking a proof. 
}
\label{fig:ConvexALL}
\end{figure}

\section{Simulation and Experimental Results}
\label{sec:SimulationResults}

This section first presents a simulation study of the intrinsic calibration problem
that will show the importance of Theorem~\ref{thm:uniqueness} for eliminating over
parameterization of the proposed model. Results here will then be used to inform the
experimental work. All experiments are conducted with a \textit{32-Beam Velodyne ULTRA Puck LiDAR~}, mounted on an
in-house designed torso for a Cassie-series bipedal
robot~\cite{CassieAutonomy2019ExtendedEdition}. All the optimizations related to the
proposed methods are solved via the proposed algorithm (see
Algorithm~\ref{alg:ConvexAlg}). As for the two baselines, we implemented them and
solved via the \texttt{fmincon} function in MATLAB. All the methods, simulator,
solver, and datasets are open-sourced:~\cite{githubFileIntrinsic,
githubFileLiDARSimulator, githubFileGlobalSim3Solver}. 

\input{ExperimentTables}
\subsection{Simulated LiDAR~ Environment}
\label{sec:Simulator}
Due to lack of ground truth, we built a LiDAR~ simulator to compare our
calibration method against the two baselines and to illustrate the role of target
positioning. Our simulator can model LiDAR~ sensors of different
working principles (spinning vs solid-state). With our simulator, it is easy to
control sources of uncertainty, including both mechanical model uncertainty and
measurement noise. Targets are assumed to be planar and polygonal.

To simulate 3D points on a planar target, we generate rays from the LiDAR~ sensor,
and define a ``target'' point as the point at which the ray intersects the target.
The simulator has an option to account for shadowing or not. After locating the exact
LiDAR~ returns on the target, based on the LiDAR~ type, we then add two types of
uncertainty to the returns and report them as measured data. 

\begin{remark} 
   
   
   
   
   
   
   
        The simulator first finds all intersection points with the (infinite) plane
    defined by the target.  To determine if a point on the plane is within the
    boundary of the target polygon is a well-known problem in computer
    graphics, called the Point-In-Polygon (PIP) problem~\cite{moscato2019provably, el2020enhanced}. The Winding Number algorithm
    \cite{alciatore1995winding} is implemented in this simulator. If the winding
    number of a point is not zero, the point lies inside the boundary; otherwise it
    is outside.
\end{remark}



\begin{figure}[t]%
\centering
\includegraphics[width=1\columnwidth, trim={0cm 0cm 0cm 0cm},clip]{graphics/CN-vs-P2P2.png}
\caption[]{P2P cost improvement is insensitive to condition number of the basis. The figure illustrates validation results for ten
thousand orientations of four targets arranged as a tetrahedron. Each orientation is a calibration scene from which a set of
calibration parameters is obtained. The calibration parameters are then
applied to another complex scene for validation. This figure shows that regardless of
the orientation of the tetrahedron, we are able to reduce the P2P cost by
44.7\% on average.
} 
\label{fig:OrientationVSP2P}%
\squeezeup
\end{figure}



\squeezeup
\subsection{Intrinsic Calibration of Spinning LiDARs~}
\label{sec:SimSpingHead}

\subsubsection{Illustration of Theorem~\ref{thm:uniqueness} in simulation} We randomly create 32 deterministic noise sequences (of length appropriate for each ring) and apply them to the ideal measurements of each ring in the LiDAR~ simulator. As a calibration scene, we arrange four targets
around the LiDAR in the shape of a regular tetrahedron and then minimize the cost function in \eqref{eq:OptSolution}. For validation, we collect simulated data from a complex scene with 24 targets at various orientations and distances, using the
same uncalibrated LiDAR~ corrupted by the same deterministic noise. We then applied the calibration parameters to the perturbed uncalibrated LiDAR~ measurements and reduced the P2P distance of the validation scene by 45.43\% compared to the uncalibrated P2P distance.
Additionally, to study the influence of different 
condition numbers in Assumption~\ref{asm:assumtionB} 
on the calibration results, we rotate the tetrahedron with
various orientations (ten thousand different orientations in total, yielding ten thousand different bases).
We observe that the P2P cost improvement is insensitive to the condition number of the basis.
In Fig.~\ref{fig:OrientationVSP2P}, the simulation results show that by using the
tetrahedron (regardless of the orientation) to calibrate the LiDAR, 
the P2P distance was reduced by 44.7\% on average.




\subsubsection{Illustration of Theorem~\ref{thm:uniqueness} in experiments} 
Informed by the simulation study, two scenes (calibration and validation) of data from a \textit{32-Beam Velodyne ULTRA Puck LiDAR~} were collected using the
LiDARTag~ package~\cite{githubFileLiDARTag,huang2019lidartag}. In Fig.~\ref{fig:Training}, the calibration scene contains four targets\footnote{The targets were placed a distance of 0.7 meter so that most of the rings would lie on the targets.} arranged as an oriented tetrahedron as stated in Theorem~\ref{thm:uniqueness}. The targets'
normal vectors are estimated using the L1-inspired method
in~\cite{huang2020improvements}. 


In the raw data, the rings are mis-calibrated (by assumption). Because the target planes are estimated using 
data corresponding to mis-calibrated rings, the estimated normal vectors are inaccurate. We therefore propose a refinement of the normal vectors conditioned on the current
estimate of the similarity transformation $\mathrm{Sim}(3)$, that is, based on the current
estimate of the intrinsic parameters $(s, \R,\v)$. Once the normal vectors are updated,
new estimates for $(s, \R, \v)$ can be obtained, resulting in an alternating two-step
process. 


Figure~\ref{fig:SystematicError} shows the experimental result of applying the calibration parameters to another set of targets (validation scene) placed at 0.7 meter. The proposed alternating method reduces the P2P distance by 68.6\% with respect to the factory-calibrated LiDAR.
Additionally, we induce
varying levels of systematic errors (from 1 cm to 7 cm, corresponding to level 1 to level 7) to the experiment data to
ensure the robustness of the proposed method,
as shown in Table~\ref{tab:Exp}. We observe that our proposed method is insensitive to systematic error. In addition, Table~\ref{tab:ScansVsPnP} shows the consistency of the proposed method as we accumulate different numbers of scans with different noise levels. It is seen that with higher levels of induced deterministic noise, the performance of the proposed method is invariant to the number of accumulated scans, while with lower levels of deterministic noise, the more scans included, the better is the performance. Both of these properties are desirable.







\begin{remark}
    There are two ways to orient the targets to form a tetrahedron: \textbf{1)} put the targets around the LiDAR~ and keep the LiDAR~ pose fixed; \textbf{2)} keep the target poses fixed and rotate the LiDAR~. This will allow the targets to form a tetrahedron even if a LiDAR~ does not have a $360^\circ$ field of view.
\end{remark}
\begin{remark}
    To decide if the P2P distance of a ring to a target is random noise or
    systematic errors, we first compute the standard deviation of the LiDAR~ returns
    on the target and consider the P2P distance greater than $3\sigma$ as
    systematic error. 
\end{remark}

\begin{figure}[t]%
\centering
\subfloat{%
\includegraphics[height=0.21\textheight, trim={0cm 0cm 0cm 0cm},clip]{graphics/TrainingTargets-XY.png}}~
\subfloat{%
\includegraphics[height=0.21\textheight, trim={0cm 0cm 1.5cm 0cm},clip]{graphics/TrainingTargets-XZ.png}}
\caption[]{Four targets are placed to form a tetrahedron. The left shows the top-down view ($X$-$Y$ plane) of a single scan of the point cloud from the calibration scene and the right shows the side view ($X$-$Z$ plane) of the scene. Different colors stand for different targets.}
\label{fig:Training}%
\squeezeup
\end{figure}




\begin{figure}[t]%
\centering
\subfloat{%
\includegraphics[height=0.8\columnwidth, trim={0cm 0cm 4cm 0cm},clip]{BeforeCalibration.png}}
~
\subfloat{%
\includegraphics[height=0.8\columnwidth, trim={2cm 0cm 6cm 0cm},clip]{AfterCalibration-Global.png}}~
\caption[]{On the left shows a factory-calibrated \textit{32-Beam Velodyne ULTRA Puck LiDAR~} measuring on a planer
target placed at 0.7 meter. The measurements are not consistent and lead to a thickness of 6.2 cm
point cloud with 1.4 cm P2P distance. 
On the right shows the calibration results of the calibration parameters globally optimized over the tetrahedron calibration scene in Fig.~\ref{fig:Training} and then the parameters of each ring are applied to this dataset as validation.
The thickness and P2P distance after calibration are 4.4 cm and
0.47 cm.}%
\label{fig:SystematicError}%
\squeezeup
\end{figure}

\comment{
All together, 39 targets were collected as potential training data. On a separate
day, an additional 12 targets were collected for validation. Table~\ref{tab:RealExp}
reports validation data for the three calibration methods on various subsets of the
targets based on number of targets: four, seven, 10 of them.

It is seen that the trends observed in the simulation data for a spinning LiDAR~
continue to hold in experiments. We conclude that the proposed idea of reformulating
the calibration problem in terms of shape-preserving transformations on the observed
point cloud has been supported (i.e., not invalidated). With the open-source release
of our code and data, it will be straightforward for other groups to confirm or deny
this finding.

Informed by
the simulation study, a set of LiDAR~ data was collected and processed by the LiDARTag~
package~\cite{githubFileLiDARTag,huang2019lidartag}.
we placed
four targets around our \textit{32-Beam Velodyne ULTRA Puck LiDAR~}.
}

\input{ScansVsPnP}


\comment{
We generate deterministic measurements by applying the following
perturbations to the data on a per-ring basis (i.e, values vary by ring number): (i)
simple model $(\delta_\rho, \delta_\theta, \delta_\phi)$, (ii) complex model $(\delta_\rho, \delta_\theta, \delta_\phi,
s, h, v)$, and (iii) $\mathrm{Sim}(3)$ model $(\R, \t, s)$, denoted as $\N[1], \N[2], \N[3]$,
respectively. The exact values used are in a \texttt{.mat} file uploaded to GitHub
\cite{githubFileIntrinsic}. For each target, its true normal vector $\n$ and point
$\p[0]$ on the target are assumed known so that the P2P cost functions can be
evaluated. For each of the three cost functions \eqref{eq:OptSolution},
\eqref{eq:SimpleCalibration}, \eqref{eq:ComplexCalibration}, the \texttt{fmincon}
function in MATLAB is used to determine calibration parameters achieving a local
minimum for each Scene. The initializations for \texttt{fmincon} are selected to
correspond to zero perturbation error, that is the parameters are zero, one or the
identify matrix as the case requires. The optimized calibration parameters are then
applied to the data of Scene 5 and the P2P cost of \eqref{eq:OptSolution} is
evaluated and reported as validation data in Table~\ref{tab:N3}.

The simulated results confirm that a single target results in degeneracy when
calibrating with the $\textbf{BL}_2$ and $\mathrm{Sim}(3)$ models, and while $\textbf{BL}_1$ works fine with
a single target, it drops in performance as the complexity of the calibration errors
is increased. The well-posedness of the $\textbf{BL}_2$ and $\mathrm{Sim}(3)$ calibration models was
confirmed when the conditions of Theorem.~\ref{thm:uniqueness} are respected.

\addd{To further analyze the proposed method and verify Theorem~\ref{thm:uniqueness},
    we consider the following two scenarios: \textbf{1)} We generate \bh{10 thousand}
    training scenes in which we randomly place 4 targets at different
    distances with various angles. \textbf{2)} We generate 10 thousand training
    scenes in which we randomly rotate the tetrahedron, as shown in
    Fig.~\ref{fig:oriented-tetrahedron}. We compute the condition number of normal
    vectors (Assumption~\ref{asm:assumtionN}), and the condition number of bases
    (Assumption~\ref{asm:assumtionB}) for both scenarios. The optimized calibration
    parameters are then applied to the data of Scene 5 and the P2P cost of
\eqref{eq:OptSolution} is evaluated. The results are reported in
Fig.~\ref{fig:P2PResults}. Again, we can
see, as expected, when the condition number of normal vectors
worsens, the validation P2P cost increases. However, when the condition number of bases increases, the
P2P cost does not increase as we expected. Therefore, in practice, 
a set of well-conditioned target's normal vectors is sufficient to calibrate a
spinning LiDAR.}
}



\begin{figure}[t]%
\centering
\subfloat{%
\label{fig:SSLiDARInitial}%
\includegraphics[height=0.23\textheight, trim={0cm 0cm 0cm 0cm},clip]{sslidar1.png}}~
\subfloat{%
    \label{fig:SSLiDARCalibrated}%
\includegraphics[height=0.23\textheight, trim={0cm 0cm 0cm 0cm},clip]{sslidar2.png}}
\caption[]{A solid-state LiDAR~ is (hypothetically) built on a warped wafer and measures a planar target (black). The un-calibrated measurements
and calibrated measurements are marked in red and blue, respectively. The left image shows a top-down view and the right image shows a side view of a target. The proposed method reduces the P2P distance by
48.7\%.}
\label{fig:SSLiDAR}
\squeezeup
\end{figure}



\squeezeup
\subsection{LiDAR~ Simulated Intrinsic Calibration of Solid-State LiDARs~}
\label{sec:ExpSSLiDAR}
At the time of writing, we do not have access to a solid-state LiDAR; therefore only simulation results are provided. However, we are open to collaborating if the reader can share data. OPA solid-state LiDARs~ are fabricated on a planar wafer with a large number of emitters, see
Sec.~\ref{sec:intro}. For such a LiDAR, we induce geometric uncertainty into a simulation of the system by assuming the
plane of the wafer is slightly warped. In the simulator, the OPA solid-state LiDAR~ has
$20\times20$ emitters with $160^\circ\times40^\circ$ (horizontal, vertical) field of view. We place four targets of the same size in the simulator.

Theorem~\ref{thm:uniqueness} requires a ring plane, as mentioned in Sec.~\ref{sec:TargetPlacementGuideline}. We therefore parse the LiDAR~ returns into $20\times4$ uniform grids over the planar targets, with each grid containing five points. Each grid plays the role of a ring plane in Theorem~\ref{thm:uniqueness}, resulting in a total of 80 calibration models. We use Weighted Least Squares to estimate the plane of the target. The calibrated sensor is then validated on four other scenes that have different angles and distances. The mean P2P cost of \eqref{eq:OptSolution} improves by 48.7\%; the result is visualized in Fig.~\ref{fig:SSLiDAR}.

\begin{remark}
The solid-state LiDAR~ is used to measure the same target four times with different orientations. The four target planes form a tetrahedron. Therefore, the same five points can scan the four planes and result in a ring plane. Instead of five points, we can group a row/column of points, as mentioned in Sec.~\ref{sec:CollectionOfPoints}.
\end{remark}








\section{Global Optimization: from Min to Min-Min}
\label{sec:ConvexRelaxation}
Theorem~\ref{thm:uniqueness} states that a unique answer exists. In this section, we
propose a method to find it. In particular, we show a
globally convergent algorithm to determine the element of the Lie Group that globally
minimizes the cost function~\eqref{eq:P2PDistance}. This is done by converting the minimization
problem in~\eqref{eq:OptSolution} to a Min-Min problem. The inner minimization problem has an efficient global solution by results in~\cite{olsson2008solving, briales2017convex}. The outer minimization is scalar, and it is easy to limit the range of interest to a compact set.

\squeezeup
\subsection{Problem Statement and Compact Sets}
\label{sec:CompactSets}
The intrinsic calibration problem is defined in~\eqref{eq:OptSolution}:
\begin{align}
    \label{eq:minOverSim}
    \min_{s,\R,\v}J(s,\R,\v; \mathcal{X}_t, \n[t],\p[0,t]),
\end{align}
where the cost is defined as
\begin{equation}
    \label{eq:P2PDistance2}
  J(s, \R, \v):= \sum_{t=1}^T\sum_{i=1}^{M_t}
    \lvert \n[t][\mathsf{T}] \left( s\R \cdot \x[i] +\v- \p[0,t] \right) \rvert^2.
\end{equation}
It is straightforward to show that $J$ is a continuous function on $\mathrm{Sim}(3)$. 
As a set, we write $\mathrm{Sim}(3) = \mathbb{R}^+ \times \mathrm{SO}(3) \times \mathbb{R}^3$, where $\mathbb{R}^+:= \mathbb{R} > 0$. Note that $\mathrm{SO}(3)$ is compact. To guarantee that a minimum exists, we consider compact subsets of $s\in\mathbb{R}^+$ and $\v\in\mathbb{R}^3$ and define
\eqref{eq:minOverSim}:
\begin{equation}
\begin{aligned}  
    J^\ast &:= \min_{s,\R,\v} J(s,\R,\v)\\
    s^\ast, \R[][\ast], \v[][\ast] &:= \argmin_{s,\R,\v} J(s,\R,\v).
\end{aligned}
\label{eq:GoldMine}
\end{equation}




\input{SolverAlg}
\squeezeup
\squeezeup
\subsection{From Min to Min-Min}
\label{sec:MinMin}
We use again the fact that $\mathrm{Sim}(3) = \mathbb{R}^+ \times \mathrm{SO}(3) \times \mathbb{R}^3$ to redefine \eqref{eq:minOverSim}
as a Min-Min~ problem.
\begin{proposition}
\label{prop:MinToMinmin}
\begin{equation}
    \label{eq:MinEqualMinmin}
    \min_{s,\R,\v} J(s,\R,\v) = \min_s\min_{\R,\v}J(s,\R,\v),
\end{equation}
where the minimums are taken over the same compact sets used in \eqref{eq:GoldMine}.
\end{proposition}
\textbf{Proof:} 
From \eqref{eq:GoldMine}, we have that
\begin{equation}
    J^\ast = J(s^\ast, \R[][\ast], \v[][\ast]).
\end{equation}
We further introduce
\begin{equation}
\begin{aligned}
h(s) &:= \min_{\R,\v}J(s,\R,\v)\\
h^\ast&:=\min_s h(s) 
\end{aligned}
\end{equation}
and note that by definition, we have
\begin{equation}
    \label{eq:JF}
    J^\ast \le h^\ast.
\end{equation}
To show $h^\ast \le J^\ast$, we use that minimizing values of $J$ exist and hence
\begin{equation}
    \label{eq:FJ}
    h^\ast \le h(s^\ast) := {\min_{\R,\v}J(s^\ast, \R,\v)} \le J(s^\ast, \R[][\ast], \v[][\ast]) = J^\ast.
\end{equation}
Equations \eqref{eq:JF} and \eqref{eq:FJ} conclude the equivalency of \eqref{eq:MinEqualMinmin}.\\
\qed


From Proposition~\ref{prop:MinToMinmin}, we can define the intrinsic calibration
problem as
\begin{align}
    \label{eq:minS}
    f(s) &:= \min_{\R, \v}J(s,\R,\v;\mathcal{X}_t, \n[t],\p[0,t])\\
   
    J^\ast &= \min_{s}f(s).
\end{align}




\comment{
\begin{itemize}
    \item $J^\ast=\min_{s,\R,\v}J(s,\R,\v) \implies \exists s^\ast, R^\ast, t^\ast$ such that $$ J^\ast = J(s^\ast, R^\ast, t^\ast)$$ 
    \item $f(s) := {\rm inf}_{\R,\v}J(s, \R,\v)$
    \item $f^\ast={\rm inf}_{s}f(s) \implies f^\ast \le f(s)$ for all $s$
    \item By definition, $J^\ast \le f^\ast $
\item  To show: $f^\ast \le J^\ast$. Consider 
$$f^\ast \le f(s^\ast) := {\inf_{\R,\v}J(s^\ast, \R,\v)} \le J(s^\ast, R^\ast, t^\ast) = J^\ast$$
\end{itemize}


Due to the property of the Cartesian product, given a scaling
$s\in\mathbb{R}^+$, \eqref{eq:minOverSim} can be written as
\begin{align}
    \label{eq:minS}
    f(s) = \min_{\R,\v}J(\R,\v;s,\mathcal{X}_t, \n[t],\p[0,t]),
\end{align}
which has an efficient global solution~\cite{olsson2008solving, briales2017convex}.
Therefore, \eqref{eq:minOverSim} is a Min-Min~ problem:
\begin{equation}
    \label{eq:minmin}
    \min_{s}\min_{\R,\v}\sum_{t=1}^T J_t(s,\R,\v; \mathcal{X}_t, \n[t],\p[0,t]).
\end{equation}
\begin{itemize}
    \item $J^\ast=\min_{s,\R,\v}J(s,\R,\v) \implies \exists s^\ast, R^\ast, t^\ast$ such that $$ J^\ast = J(s^\ast, R^\ast, t^\ast)$$ 
    \item $f(s) := {\rm inf}_{\R,\v}J(s, \R,\v)$
    \item $f^\ast={\rm inf}_{s}f(s) \implies f^\ast \le f(s)$ for all $s$
    \item By definition, $J^\ast \le f^\ast $
\item  To show: $f^\ast \le J^\ast$. Consider 
$$f^\ast \le f(s^\ast) := {\inf_{\R,\v}J(s^\ast, \R,\v)} \le J(s^\ast, R^\ast, t^\ast) = J^\ast$$
\end{itemize}
}










\squeezeup
\subsection{Determining the Calibration Parameters}
Because we can bound the scaling $s$ to a compact set, a minimizing value can be found by dense search. In the Appendix~C\cite{huang2020global}, we discuss a heuristic to speed up the algorithm if one feels the need.  


At the $(k+1)$-th iteration, the scaling parameter is determined and the remaining
parameters correspond to $\mathrm{SE}(3)$. To solve for them, we adopt techniques that
were used to solve 3D registration or 3D SLAM~\cite{tron2015inclusion,
carlone2015lagrangian, olsson2008solving, briales2017convex} where the problem is
formulated as a QCQP, and the Lagrangian dual relaxation is used. The relaxed
problem becomes a Semidefinite Program (SDP) and convex. The problem
can thus be solved globally and efficiently by off-the-shelf specialized
solvers~\cite{grant2014cvx}. This process is summarized in
Appendix~B in~\cite{huang2020global}. As shown in~\cite{briales2017convex}, the dual
relaxation is empirically always tight (the duality gap is zero). We also use the
same simulation and experimental data set in~\cite{olsson2008solving,
briales2017convex} to verify that the proposed algorithm is able to converge
globally, see Appendix~C in~\cite{huang2020global} for more visual results.


\begin{remark}
One may argue that the above is a standard optimization problem in that other solvers, such as Google ceres or g2o, can easily solve it. However, they are local solvers and will suffer if a good initial guess is not provided.
\end{remark}

\section{Global Optimization: from Min to Min-Min}
\label{sec:ConvexRelaxation}
Theorem~\ref{thm:uniqueness} states that a unique answer exists. In this section, we
propose a method to find it. In particular, we show a
globally convergent algorithm to determine the element of the Lie Group that globally
minimizes the cost function~\eqref{eq:P2PDistance}. This is done by converting the minimization
problem in~\eqref{eq:OptSolution} to a Min-Min problem. The inner minimization problem has an efficient global solution by results in~\cite{olsson2008solving, briales2017convex}. The outer minimization is scalar, and it is easy to limit the range of interest to a compact set. 

\subsection{Problem Statement and Compact Sets}
\label{sec:CompactSets}
The intrinsic calibration problem is defined in~\eqref{eq:OptSolution}:
\begin{align}
    \label{eq:minOverSim}
    \min_{s,\R,\v}J(s,\R,\v; \mathcal{X}_t, \n[t],\p[0,t]),
\end{align}
where $J$ is defined as
\begin{equation}
    \label{eq:P2PDistance2}
   \sum_{t=1}^T\sum_{i=1}^{M_t}
    \lvert \n[t][\mathsf{T}] \left( s\R \cdot \x[i] +\v- \p[0,t] \right) \rvert^2.
\end{equation}
Due to the unbounded $s\in\mathbb{R}^+(\mathbb{R} > 0)$ and $\v\in\mathbb{R}^3$, we
consider a compact subset of $\v$ and $s$ so that there exists a solution ($J^\ast$)
to \eqref{eq:minOverSim}:
\begin{equation}
    J^\ast = \min_{s,\R,\v} J(s,\R,\v).
\end{equation}




\input{SolverAlg}

\subsection{From Min to Min-Min}
\label{sec:MinMin}
$\mathrm{Sim}(3)$ can be written as a semi-direct product of the subgroups
$\mathbb{R}^+$ and $\mathrm{SE}(3)$. We will use the fact that as a set, $\mathrm{Sim}(3)$ is
the Cartesian product of $\mathrm{SE}(3)$ and $\mathbb{R}^+$, to redefine \eqref{eq:minOverSim}
as a Min-Min~ problem.
\begin{proposition}
\label{prop:MinToMinmin}
\begin{equation}
    \label{eq:MinEqualMinmin}
    \min_{s,\R,\v} J(s,\R,\v) = \min_s\min_{\R,\v}J(s,\R,\v)
\end{equation}
\end{proposition}
\textbf{Proof:} 
In Sec.~\ref{sec:CompactSets}, we assume compact
subsets of $s$ and $\v$. Therefore, $\exists s^\ast, R^\ast, t^\ast$ such that
\begin{equation}
    J^\ast = J(s^\ast, R^\ast, t^\ast).
\end{equation}
We further define $h(s)$ as
\begin{equation}
h(s) := \inf_{\R,\v}J(s,\R,\v).
\end{equation}
Due to the existent of $J^\ast$, $\exists h^\ast$ such that
\begin{equation}
    h^\ast = \inf_s h(s) \implies h^\ast \le h(s), \forall s.
\end{equation}
By definition, we have
\begin{equation}
    \label{eq:JF}
    J^\ast \le h^\ast.
\end{equation}
To show $h^\ast \le J^\ast$, we consider 
\begin{equation}
    \label{eq:FJ}
h^\ast \le h(s^\ast) := {\inf_{\R,\v}J(s^\ast, \R,\v)} \le J(s^\ast, R^\ast, t^\ast) = J^\ast.
\end{equation}
\eqref{eq:JF} and \eqref{eq:FJ} concludes the equivalency of \eqref{eq:MinEqualMinmin}.\\
\qed


From Proposition~\ref{prop:MinToMinmin}, we can define the intrinsic calibration
problem as:
\begin{align}
    \label{eq:minS}
    f(s) = \min_{s}J(s;\R,\v,\mathcal{X}_t, \n[t],\p[0,t]).
\end{align}




\comment{
\begin{itemize}
    \item $J^\ast=\min_{s,\R,\v}J(s,\R,\v) \implies \exists s^\ast, R^\ast, t^\ast$ such that $$ J^\ast = J(s^\ast, R^\ast, t^\ast)$$ 
    \item $f(s) := {\rm inf}_{\R,\v}J(s, \R,\v)$
    \item $f^\ast={\rm inf}_{s}f(s) \implies f^\ast \le f(s)$ for all $s$
    \item By definition, $J^\ast \le f^\ast $
\item  To show: $f^\ast \le J^\ast$. Consider 
$$f^\ast \le f(s^\ast) := {\inf_{\R,\v}J(s^\ast, \R,\v)} \le J(s^\ast, R^\ast, t^\ast) = J^\ast$$
\end{itemize}


Due to the property of the Cartesian product, given a scaling
$s\in\mathbb{R}^+$, \eqref{eq:minOverSim} can be written as
\begin{align}
    \label{eq:minS}
    f(s) = \min_{\R,\v}J(\R,\v;s,\mathcal{X}_t, \n[t],\p[0,t]),
\end{align}
which has an efficient global solution~\cite{olsson2008solving, briales2017convex}.
Therefore, \eqref{eq:minOverSim} is a Min-Min~ problem:
\begin{equation}
    \label{eq:minmin}
    \min_{s}\min_{\R,\v}\sum_{t=1}^T J_t(s,\R,\v; \mathcal{X}_t, \n[t],\p[0,t]).
\end{equation}
\begin{itemize}
    \item $J^\ast=\min_{s,\R,\v}J(s,\R,\v) \implies \exists s^\ast, R^\ast, t^\ast$ such that $$ J^\ast = J(s^\ast, R^\ast, t^\ast)$$ 
    \item $f(s) := {\rm inf}_{\R,\v}J(s, \R,\v)$
    \item $f^\ast={\rm inf}_{s}f(s) \implies f^\ast \le f(s)$ for all $s$
    \item By definition, $J^\ast \le f^\ast $
\item  To show: $f^\ast \le J^\ast$. Consider 
$$f^\ast \le f(s^\ast) := {\inf_{\R,\v}J(s^\ast, \R,\v)} \le J(s^\ast, R^\ast, t^\ast) = J^\ast$$
\end{itemize}
}

\add{
\begin{remark}
    If we have one unique answer to Problem~\ref{prob:intrinsic_calibration} (states in
    Theorem~\ref{thm:uniqueness} and Sec.~\ref{sec:CompactSets}), we strongly
    conjecture that \eqref{eq:minS} is convex, because all of our
    datasets and the datasets from~\cite{olsson2008solving, briales2017convex} show
    the convexity, as shown in Fig.~\ref{fig:Solver}. The proof is still an ongoing
work. 
\end{remark}
}

\subsection{Parameter of Scaling Determination}
\addd{
Theoretically, a dense search on the scaling $s$ should be performed on a range of
interest. However, we empirically observed that \eqref{eq:minS} is always convex, see 
Sec.~\ref{sec:MinMin}. We, therefore, utilize the bisection method to determine
the scaling $s$. The proof of the convexity is an ongoing work.
}


\addd{
    Let $\pre[k][L]s$ and $\pre[k][U]s$ be the lower and upper bounds
    of the scaling parameters at the $k$-th iteration. Let $\pre[k+1]s^\prime =
    (\pre[k][U]s + \pre[k][L]s)/2$ be the update for $(k+1)$-th iteration. The
    updates of the scaling parameters are defined as:
\begin{align}
    \label{eq:UpdateScaling}
    \pre[k+1][L]s &=
\begin{cases}
    s^\prime, \text{~if~}f(\pre[k][U]s)<f(\pre[k][L]s) \\
\pre[k][L]s, \text{~~~~~~otherwise}
\end{cases}\nonumber\\
    \pre[k+1][U]s &=
\begin{cases}
    s^\prime, \text{~if~}f(\pre[k][L]s)<f(\pre[k][U]s) \\
\pre[k][U]s, \text{~~~~~~otherwise}
\end{cases}.
\end{align}
}
\begin{figure}[b]%
\centering
\includegraphics[width=0.9\columnwidth, trim={0cm 0cm 0cm 0cm},clip]{convexity2.png}%
\caption[]{\add{This figure illustrates \eqref{eq:minS} is potential convex and the
        proof is an ongoing work. If \eqref{eq:minS} is indeed proven as convex, then
        the bisection method can be performed instead of the dense search to find the
        global optimum.}}
\label{fig:Solver}
\end{figure}
\addd{
At the $(k+1)$-th iteration, the scaling parameter is determined and the rest of the
parameters are $\mathrm{SE}(3)$. To solve the remaining parameters, we adopt techniques that
were used to solve 3D registration or 3D SLAM~\cite{tron2015inclusion,
carlone2015lagrangian, olsson2008solving, briales2017convex} where the problem is
formulated as a QCQP, and the Lagrangian dual relaxation is used. The relaxed
problem becomes a Semidefinite Program (SDP) and convex. The problem
can thus be solved globally and efficiently by off-the-shelf specialized
solvers~\cite{grant2014cvx}. This process is summarized in
Appendix~\ref{sec:DualFormulation}. As shown in~\cite{briales2017convex} the dual
relaxation is empirically always tight (the duality gap is zero). We also use the
same simulation and experimental dataset in~\cite{olsson2008solving,
briales2017convex} to verify that the proposed algorithm is able to converge
globally, see Appendix~\ref{sec:DualFormulation} for more visual results.
}



\section{Plane Estimation}
\label{sec:lidarVertices}
\addd{Indicated in Sec.~\ref{sec:IntrinsicCalibration}, the normal vector of a target
(i.e. the plane equation), must be estimated from data. Due to the assumption that the
wafer of an OPA solid-state LiDAR~ is slightly warped (see Sec.~\ref{sec:ExpSSLiDAR}),
directly estimating the plane equation using existing methods in the literature is
inviable because they do not take this geometric distortion into account. This
section therefore provides two means to estimate the normal vectors from a spinning LiDAR~
and solid-state LiDAR~.}

\subsection{Plane Estimation for spinning LiDARs~}
\addd{The following is partially taken from \cite{huang2020improvements} and provided for
clarity.
Let $\mathcal{PC} := \{\mathcal{X}_i\}_{i=1}^{M}$ be a collection of 3D points on a LiDAR~ target,
where $M$ is the number of points. Given the target geometry, we define an ideal
target with vertices $\{ \bar{X}_i \}_{i=1}^4$ located at the origin of the LiDAR.
We seek a rigid body transformation from LiDAR~ to the target, $H_L^T$, that ``best
fits'' the ideal target onto the point cloud.}

\addd{In practice, it is actually easier to pull the $\mathcal{PC}$ back to the origin of the LiDAR
through the inverse of the current estimate of transformation $H_T^L:=(H_L^T)^{-1}$
and measure the error there. For $a \ge 0$ and $\lambda \in \mathbb{R}^+$, an
$L_1$-inspired cost is defined as
\begin{equation}
    \label{eq:L1cost}
    c(\lambda,a):=\begin{cases}
    \min\{ |\lambda-a|, |\lambda + a| \} & \text{if}~|\lambda| >a \\
    0 & \text{otherwise}
    \end{cases}.
\end{equation}
Let $\{\tilde{\mathcal{X}}_i\}_{i=1}^N:=H_T^L(\mathcal{PC}):=\{ H_T^L(\mathcal{X}_i) \}_{i=1}^N$ denote
the pullback of the point cloud by $H_T^L$, and denote a point's $(x,y,z)$-entries by
$(\tilde{x}_i,\tilde{y}_i,\tilde{z}_i)$.
The total fitting error of the point cloud is defined as
\begin{equation}
	\label{eq:JKHcost}
    C(H_T^L(\mathcal{PC})) :=\sum_{i=1}^{M} c(\tilde{x}_i,\epsilon) +
    c(\tilde{y}_i,d/2) +  c(\tilde{z}_i,d/2),
\end{equation}
where $\epsilon \ge 0$ is a parameter (estimated by the smallest variance of the
principal axis), to account for uncertainty in the depth
measurement of the planar target. For more detail about this method and target
placement for extrinsic calibration, see~\cite{huang2020improvements}. Once we
estimate the four vertices of the target, the plane equation of the target is
trivial. The estimated plane is shown in Fig.~\ref{fig:L1PlaneEstimation}.}


\subsection{Plane Estimation for solid-state LiDARs~}
\addd{Due to the warped wafer, the closer to the edge of the wafer, the larger the
distortion is. We therefore estimate the target's normal vector by weighted least squares
where the weight is the distance of each point to the mean of all the points on the
target. Figure~\ref{fig:WeightedPlaneEstimation} shows the result of plane estimation
against the ground truth from the simulator.}

\begin{figure}[b]%
\centering
\subfloat{%
    \label{fig:L1PlaneEstimation}%
\includegraphics[width=0.48\columnwidth, trim={0cm 0cm 0cm 0cm},clip]{placeholder.png}}~
\subfloat{%
    \label{fig:WeightedPlaneEstimation}%
\includegraphics[width=0.48\columnwidth, trim={0cm 0cm 0cm 0cm},clip]{placeholder.png}}
\caption[]{\bhp{}}
\end{figure}






\section{Automatic Calibration System}
\label{sec:AutomaticCalibrationSystem}
Target-based LiDAR-camera~ extrinsic calibration methods often suffer from manual target
extraction and feature association. In our previous
work~\cite{huang2019improvements} that addressed the estimation of  LiDAR~ target vertices and extrinsic
calibration, we assumed LiDAR~ points falling on the target were extracted by hand and the camera corners were obtained manually by clicking on an image. Because experimentally comparing and validating the intrinsic calibration methods of
Sec.~\ref{sec:IntrinsicCalibration} and~\ref{sec:Baseline} would require significant
data collection, we decided to create a fully automatic pipeline for both intrinsic
and extrinsic calibration, as shown in Fig~\ref{fig:AutomaticPipeline}. We demonstrate the capability of this
pipeline by collecting a significant amount of data allowing us to check the quality
of the calibration of our sensor suite (a LiDAR-camera~ pair) at different distances;
see~\ref{sec:EXPExtrinsicCalibration}.


\begin{figure*}[t]
    \centering
    \includegraphics[width=1.3\columnwidth]{graphics/AutomaticPipeline.png}
    \caption{\bh{add intrinsic calibration to the system diagram}A system diagram for
        automatic intrinsic and extrinsic calibration. The top shows the front-end of
        the pipeline. Its input is raw camera and LiDAR~ data, which are subsequently
        synchronized. The AprilTag~ and LiDARTag~ packages are used to extract the target
        information, which is then examined to remove outliers and ensure that
        targets are still properly synchronized. Each run of the front-end saves all
        related information into a ROS bagfile, which can then be sent to the
        back-end for further processing. The back-end takes in (possibly many) ROS
        bagfiles and does the following: (i) refines the image corners; and (ii)
        extracts vertices of the LiDAR~ targets. The correspondences are established
        between corners and vertices and an extrinsic transformation is determined
        PnP as in \cite{lepetit2009epnp}. For intrinsic calibration, the resulting vertices of
        the LiDAR~ targets are used to extract normal vectors and a point on the
        plane. The calibration parameters are determined to minimize the P2P distance
        from the plane to the target points provided by the LiDARTag~ package.} 
\label{fig:AutomaticPipeline}
\end{figure*}


\subsection{Front-End: Data Pre-processing}

Fiducial markers, such AprilTags\cite{olson2011apriltag, wang2016apriltag,
krogiusflexible}, are widely used in vision-based perception tasks because they can
be  easily identified and automatically extracted from background. In
\cite{huang2019lidartag}, we introduced a similar concept for LiDAR~ point clouds,
called a LiDARTag.  Inspired by AprilTags~, they consist of a payload and a pattern that
makes each marker uniquely distinguishable and identifiable in real-time by both a
LiDAR~ and a camera. An open-source package for processing LiDARTags~ is available
\cite{githubFileLiDARTag}, while \atagN3 is available as a ROS package
\cite{githubFileAprilTagROS}. 

As the LiDAR~ and camera data streams arrive, they are synchronized, examined for
fiducial markers, and checked for false positives. For LiDAR-camera~ extrinsic calibration,
corresponding features from camera data and LiDAR~ data must be identified. We use
target vertices as our features in this pipeline. In the image plane, the target
vertices are typically called corners and are extracted in the front end using the
AprilTag~ ROS package. The relevant information is saved as ROS bagfiles so that they can
be loaded directly into MATLAB for post-processing. The technical details and
the software are available on Github\cite{githubFileAutoCalibration}.





\subsection{Back-End: Data Post-processing}
At this stage, extracted features (AprilTag~ corners and LiDARTag~ points) have been
properly synchronized and examined automatically for false positives. The next step
is to estimate the LiDARTag~ vertices, which, due to sparsity of the LiDAR~ point
cloud, are not directly observable. We use the method developed in
\cite{huang2019improvements} to determine the vertices of a diamond-shaped target.
From the vertices, the target normal $\n[t]$ and point on the target, $\p[0,t]$, can
be easily determined for evaluating the P2P cost function
\eqref{eq:OptSolution} for intrinsic calibration, while the vertices in
combination with the camera corners are used to
determine the extrinsic calibration via a PnP problem.
 


\begin{figure}[t]%
\centering
\subfloat[]{%
\label{fig:IdealFrame}%
\includegraphics[width=0.45\columnwidth, trim={0.05cm 0cm 0cm 1cm},clip] {graphics/reference_frame2.png}}~
\subfloat[]{%
\label{fig:CannyLineFitted}%
\includegraphics[width=0.48\columnwidth, trim={3.5cm 12.6cm 3cm 10cm},clip]{CannyLineFitted.png}}\\
\subfloat[]{%
\label{fig:CalibrationResults}%
\includegraphics[width=0.45\columnwidth, trim={0.5cm 1.0cm 0cm 0.5cm},clip]{test6_3.png}}~
\hspace{2pt}%
\subfloat[]{%
\label{fig:CannyLineFitted}%
\includegraphics[width=0.5\columnwidth, trim={2cm 1.7cm 2cm 0.7cm},clip]{test6_3.png}}
\caption[]{\bh{Notation, not much space, may remove
later}\subref{fig:IdealFrame}Target reference frame and real point cloud data.
The dotted blue square is the reference target; its vertices are denoted
$\{\bar{X}_i\}_{i=1}^4$.\subref{fig:CannyLineFitted} shows the result of edge
detection. 
\label{fig:ImageEdgeDetection}%
\squeezeup
\end{figure}


\subsubsection{Finding LiDAR~ vertices}
\label{sec:lidarVertices}
The following is taken from \cite{huang2019improvements} and provided for clarity. \jwg{Check consistency of our notation for points in a PC.}\bh{currently, I leave math notation as it is. I will closely read our notation and then fix them altogether.}
Let $\mathcal{PC} := \{\mathcal{X}_i\}_{i=1}^{M}$ be a collection of 3D points on a LiDAR~ target,
where $M$ is the number of points. Given the target geometry, we define an ideal
target with vertices $\{ \bar{X}_i \}_{i=1}^4$ located at the origin of the LiDAR~ as
defined in Fig.~\ref{fig:IdealFrame}. We seek a rigid body transformation from LiDAR~
to the target, $H_L^T$, that ``best fits'' the ideal target onto the point cloud. 

In practice, it is actually
easier to pull the $\mathcal{PC}$ back to the origin of the LiDAR through the inverse of the
current estimate of transformation $H_T^L:=(H_L^T)^{-1}$ and measure the error there. 
For $a \ge 0$ and $\lambda \in \mathbb{R}^+$, an $L_1$-inspired cost
is defined as 
\begin{equation}
    \label{eq:L1cost}
    c(\lambda,a):=\begin{cases} 
    \min\{ |\lambda-a|, |\lambda + a| \} & \text{if}~|\lambda| >a \\
    0 & \text{otherwise}
    \end{cases}.
\end{equation}
Let $\{\tilde{\chi}_i\}_{i=1}^N:=H_T^L(\mathcal{PC}):=\{ H_T^L(\chi_i) \}_{i=1}^N$ denote
the pullback of the point cloud by $H_T^L$, and denote a point's $(x,y,z)$-entries by
$(\tilde{x}_i,\tilde{y}_i,\tilde{z}_i)$.
The total fitting error of the point cloud is defined as 
\begin{equation}
	\label{eq:JKHcost}
    C(H_T^L(\mathcal{PC})) :=\sum_{i=1}^{M} c(\tilde{x}_i,\epsilon) +
    c(\tilde{y}_i,d/2) +  c(\tilde{z}_i,d/2),
\end{equation}
where $\epsilon \ge 0$ is a tuning parameter to account for uncertainty in the depth
measurement of the planar target. For more detail about this method and target
placement for extrinsic calibration, see~\cite{huang2019improvements}.


\subsubsection{Corner Extraction and Association with Vertices}
The only manual process in our previous software release, determining the image corners, has now
been made automatic. The corners obtained from the AprilTag~ package are automatically
refined as shown in Fig.~\ref{fig:CannyLineFitted} using a process detailed in
\cite{huang2019improvements}. Given the camera corners ($\{ Y_i\}_{i=1}^4$) and LiDAR~ vertices
$\{ X_i\}_{i=1}^4$, we use the vertical and horizontal positions in their own
coordinates to sort them and establish the correspondences.


\subsection{Intrinsic and Extrinsic Calibration}
\label{sec:InExCalibration}
For extrinsic calibration, once ``enough'' independent target vertices are estimated
from both camera data and LiDAR~ point cloud, the LiDAR~ to camera extrinsic
transformation is solved by Perspective-n-Point (PnP) as in \cite{lepetit2009epnp},
\begin{equation} 
    \label{eq:ConceptualProblem} (R_{C}^{L*}, T_{C}^{L*})= \mathrm{arg}
    \min_{(R,T)}{\mathrm{dist}(P(H_{L}^{C}(X_i)),Y_i)}, 
\end{equation} 
where $X_i$ are the (homogeneous) coordinates of the LiDAR~ features, $Y_i$ are the
coordinates of the camera features, $P$ is the often-called `projection map'',
$H_{L}^{C}$ is the LiDAR-frame to camera-frame transformation with rotation matrix
$R_{C}^{L}$ and translation $T_{C}^{L}$, and $\mathrm{dist}$ is a distance or error
measure. An example of extrinsic calibration is shown in
Fig.~\ref{fig:CalibrationResults}. More detail is provided
in~\cite{huang2019improvements}.

For intrinsic calibration, the target vertices are used to compute a normal vector and a
point on the target. The P2P distance is minimized for each of the three calibration
models, \BL1, \BL2, and $\mathrm{Sim}(3)$, for a collection of targets, yielding $\alpha^*$
(for \BL1 and \BL2) and  $\Sigma^*$ for $\mathrm{Sim}(3)$.

The processes of intrinsic calibration involves iterating the estimation of the
target vertices and the parameters in the LiDAR~ model until a given stopping
criterion is met. Here, we stopped when the maximum change $\delta_m$ over the set of target vertices was less
than $1e-5$.

\begin{algorithmic}
\While{$\delta_m>1e^{-5}$} 
\State \textbf{Step 1:} Re-estimate the target vertices $\{ X_i\}_{i=1}^4$
after applying the  current intrinsic calibration parameters to the point
cloud. This gives updated values for the target normals and points on the
targets.
\State \noindent \textbf{Step 2:} Using the updated target vertices, re-estimate the
intrinsic calibration parameters for the three calibration models.

\EndWhile
\end{algorithmic}




\section{Related Work}
\label{sec:related_work}


\section{Proof on Uniqueness of Similarity Transformation}
\label{sec:Uniqueness}
The proof provides a guideline to place targets such that an answer to the optimization
problem is unique. In particular, under Assumption~\ref{asm:assumtionN} and
Assumption~\ref{asm:assumtionB}, the optimization problem has
only one unique answer. As stated in Sec.~\ref{sec:IntrinsicCalibration}, the
intrinsic calibration parameters are modeled as an element of the similarity Lie
group ($\mathrm{Sim}(3)$). An element of this group in matrix from is
\begin{equation}
    \H = 
    \begin{bmatrix}
        s\R & \t \\
        \zeros & 1
    \end{bmatrix} \in \mathrm{Sim}(3),
\end{equation}
where $\R\in\mathrm{SO}(3), \t \in \mathbb{R}^3$ and $s \in \mathbb{R}^+$.


\subsection{Mathematical Definitions and Preliminaries}
Let $[\mathcal{S}]$ and $[\mathcal{S}]^\perp$ be the span and its orthogonal complement of
$\mathcal{S}\subset \mathbb{R}^3$, respectively\footnote{$\because \mathcal{S} \subset
\mathbb{R}^3$, $[\mathcal{S}]=\mathbb{R}^3 \iff [\mathcal{S}]^\perp =0$}. We denote the union of
intersection of $K$ spans as $\left( [\mathcal{S}_1] + [\mathcal{S}_2] + \cdots +
[\mathcal{S}_K]\right) = \cap_{i=1}^K[\mathcal{S}_i]$. Let $\P \subset \mathbb{R}^3$ be a plane.
$\P$ modeled the set traced out by a single ring of a perfectly calibrated
spinning LiDAR. Let $\{\e[1], \e[2], \e[3] \}$ denote the canonical basis for
$\mathbb{R}^3$. Without loss of generality, we assume $\P=[\e[3]]^\perp = \{\e[1],
\e[2]\}$. Therefore, $\forall s>0, \R\in\mathrm{SO}(3)$, 
$$ (s\R -\I)(\P)=0 \iff s=1, \R=\I.
\footnote{$(s\R-\I)(\P) = [(s\R-\I)\e[1]] + [(s\R-\I)\e[2])]$}$$


\subsection{Assumptions}
Consider four targets with unit normal vectors $\n[i]$ and let $\p[0, i]$ be a points
on the $i$-th target. 

\begin{assumptionN}[Normal Vectors]
    \label{eq:assumtionN}
    All sets of three distinct vectors from $\{ \n[1], \n[2], \n[3], \n[4], \e[3]\}$
    are linearly independent.
\end{assumptionN}

For $1\le i \le 4$, the plane defined by the $i$-th target is $\V[i]:= \p[0,i] +
[\n[i]]^\perp.$ Under Assumption~\ref{asm:assumtionN}, some facts are listed below
without proofs:

\begin{enumerate}[(a)]
    \item For each $i \neq j \in \{1, 2, 3, 4 \}$, $\p[ij]:=\P \cap \V[i] \cap \V[j]$
        exists and is unique. There are $\binom{4}{2}$ intersection points.

    \item $\V[i] \cap \V[j]=\p[ij] + [\n[i], \n[j]]^\perp$.

    \item  $\H\cdot \p[ij] \in \V[i] \cap \V[j]$ if, and only if, $$(s \R-\I)\p[ij]
        +\t \in [\n[i], \n[j]]^\perp. $$

    \item Because $\dim [\n[i], \n[j]]^\perp=1$,  $(s \R-\I)\p[ij] +\t  \neq 0$ if,
        and only if, $ [(s \R-\I)\p[ij] +\t] = [\n[i], \n[j]]^\perp. $

    \item Let $\{\p[a], \p[b] \}$ be a basis for $\P$. Then $(s \R-\I)(\P)=0$ if,
        and only if, $(s \R-\I)(\p[a])=0$ and $(s \R-\I)(\p[b])=0$.
\end{enumerate}
\vspace{3cm}

\begin{assumptionB}[Basis Vectors]
    \label{eq:assumtionB}
    Given the six intersection points, any two form a set of basis for the ring
    plane. There are $\binom{6}{2}$ sets of basis. Subsets of the basis' are linearly independent.
    \begin{enumerate}[(a)]
        \item $\{ \p[12], \p[13] \}$, $\{ \p[13], \p[14] \}$, $\{ \p[14], \p[12] \}$,
        \item $\{ \p[12], \p[23] \}$, $\{ \p[23], \p[24] \}$, $\{ \p[24], \p[12] \}$,
        \item $\{ \p[13], \p[23] \}$, $\{ \p[23], \p[34] \}$, $\{ \p[34], \p[13] \}$,
        \item $\{ \p[14], \p[24] \}$, $\{ \p[24], \p[34] \}$, $\{ \p[34], \p[14] \}$,
        \item $\{ \p[14], \p[23] \}$
    \end{enumerate}
\end{assumptionB}

\subsection{A Complete Proof}
\begin{theorem} 
    Assume that Assumptions~\ref{asm:assumtionN} and \ref{asm:assumtionB} hold and let $\H \in \mathrm{Sim}(3)$. If for each $i
    \neq j \in \{1, 2, 3, 4 \}$, $\H\cdot\p[ij] \in  \V[i] \cap
     \V[j]$, then 
     \begin{equation} 
         \H = 
         \begin{bmatrix}
            \I & \zeros \\
            \zeros & 1
         \end{bmatrix}.
     \end{equation}
 \end{theorem}


\begin{proof} 
The proof is by exhaustion on the dimension of $(s\R-\I)(\P)$. We have that
$\H\cdot\p[ij] = s\R \p[ij] + \t$, and therefore 
\begin{equation}
\label{eq:NecssSuff}
   \H\cdot\p[ij] \in  \V[i] \cap \V[j] \iff (s\R-\I) \p[ij] + \t \in [\n[i], \n[j]]^\perp. 
\end{equation}
 
\begin{itemize}

\item \textbf{Case 1:} $\dim (s\R-\I)(\P) =0$. \par
    Then $\t \in [\n[i],
\n[j]]^\perp$ for all $i\neq j$. Hence, $\t \in [\n[1], \n[2]]^\perp \cap
[\n[2], \n[3]]^\perp =0$, which implies that $\t=0$. Therefore, by
\textbf{Simple Fact 1}, we are done. We next show that $\dim (s\R-\I)(\P) > 0$
and $\H\cdot\p[ij] \in  \V[i] \cap \V[j]$ for all $i \ne j$ lead to
contradictions.
 
 
\item  \textbf{Case 2:} $\dim (s\R-\I)(\P) =1$ \par
    (a) Suppose $\t \not \in (s\R-\I)(\P)$. Then, for any $\p[ij]$, $(s\R-\I)\p[ij] +
    \t \neq 0$. Hence, 
    \begin{align*}
     [\n[1], \n[2]]^\perp &= [(s\R-\I)(\p[12]) + \t] \\
     [\n[2], \n[3]]^\perp &= [(s\R-\I)(\p[23]) +\t] \\
      [\n[3], \n[4]]^\perp &= [(s\R-\I)(\p[34]) +\t].
    \end{align*}
Because $[\n[1], \n[2]] \cap [\n[2], \n[3]]\cap [\n[3], \n[4]] =0$, we have
that $[\n[1], \n[2]]^\perp + [\n[2], \n[3]]^\perp +  [\n[3], \n[4]]^\perp =
\mathbb{R}^3$. We deduce that $\dim (s\R-\I)(\P) =2$, which is a contradiction. \\

(b) Suppose $\t \in (s\R-\I)(\P)$ and thus there exists $\p[t]\in \P$ such that
$\t=(s\R-\I)(\p[t])$. The condition \eqref{eq:NecssSuff} can therefore be written as
\begin{equation}
\label{eq:NecssSuffWithT}
   \H\cdot\p[ij] \in  \V[i] \cap \V[j] \iff (s\R-\I)( \p[ij] + \p[t]) \perp \{\n[i],\n[j] \}. 
\end{equation}

  Because  $\{ \p[12], \p[13] \}$,  $\{ \p[13], \p[14] \}$,  $\{ \p[14], \p[12] \}$
  are bases for $\P$, we conclude that 
  \begin{equation}
      [\p[12]+\p[t], \p[13]+\p[t], \p[14]+\p[t] ]=\P.
  \end{equation}
  Applying \eqref{eq:NecssSuffWithT} to this set of vectors, we deduce that
  $$(s\R-\I)(\P) \perp \n[1].$$ 
  Applying the same reasoning to $\{ \p[12], \p[23] \}$,
  $\{ \p[23], \p[24] \}$, $\{ \p[24], \p[12] \}$ , we deduce that $(s\R-\I)(\P) \perp
  \n[2]$. Repeating again, we have $(s\R-\I)(\P) \perp \n[3]$ and thus $\dim
  (s\R-\I)(\P)=0$. 
 

\item  \textbf{Case 3:} $\dim (s\R-\I)(\P) =2$ \par
    We rewrite \eqref{eq:NecssSuff} as
    \begin{equation}
    \label{eq:NecssSuffv02}
       \H\cdot\p[ij] \in  \V[i] \cap \V[j] \iff -\t \in  (s\R-\I) \p[ij] +[\n[i], \n[j]]^\perp. 
    \end{equation}
Hence, if for all  $i \neq j \in \{1, 2, 3, 4 \}$, $\H\cdot\p[ij] \in  \V[i] \cap
\V[j]$, then the lines $(s\R-\I) \p[ij] +[\n[i], \n[j]]^\perp$ in $\mathbb{R}^3$ must have
a common point of intersection, namely $-\t$ and hence
\begin{equation} 
\label{eq:MasternIntersection}
\bigcap \limits_{i \neq j \in \{1, 2, 3, 4 \}} \{ (s\R-\I) \p[ij] + [\n[i], \n[j]]^\perp \} \neq \emptyset.
\end{equation}

\begin{remark}  
    When $(s\R-\I)(\P)=0$, the intersections in \eqref{eq:MasternIntersection} are
    non-empty; indeed, the equation reduces to 
    $$\bigcap \limits_{i \neq j \in \{1,
    2, 3, 4 \}} [\n[i], \n[j]]^\perp = \left(~ \sum \limits_{i \neq j} [\n[i], \n[j]]
    ~\right)^\perp = (\mathbb{R}^3)^\perp =0,$$
and hence $\t=0$.
\end{remark}

In the remainder of the proof, we show the intersection being non-empty
contradicts ${\rm dim}~(s\R-\I)(\P) = 2$. We do this by examining the
intersections in \eqref{eq:MasternIntersection} pairwise to arrive at a set of
necessary conditions for $\H \cdot \p[ij] \in  \V[i] \cap \V[j]$ for $i \neq
j$, and then use the necessary conditions to complete the proof. We note that for $ij
\neq kl$, 
  
\begin{align*}
 &\{ (s\R-\I) \p[ij] + [\n[i], \n[j]]^\perp \} \cap \{ (s\R-\I) \p[kl] +
[\n[k], \n[l]]^\perp \} \neq \emptyset \\
 &\iff (s\R-\I)(\p[ij] -  \p[kl]) \in
[\n[i], \n[j]]^\perp  + [\n[k], \n[l]]^\perp.
\end{align*}
 
 The indices are a bit easier to keep track of if we set 
 \begin{align}
 &\q[1]:=(s\R-\I)(\p[12]), ~\U[1]:=[\n[1], \n[2]]^\perp \nonumber\\
 &\q[2]:=(s\R-\I)(\p[13]), ~\U[2]:=[\n[1], \n[3]]^\perp \nonumber\\
 &\q[3]:=(s\R-\I)(\p[23]), ~\U[3]:=[\n[1], \n[4]]^\perp \nonumber\\
 &\q[4]:=(s\R-\I)(\p[14]), ~\U[4]:=[\n[2], \n[3]]^\perp \nonumber,
 \end{align}
 where $\{\U[k]|k=1,\cdots,4\}$ denote the indicated one-dimensional subspaces. Then,
 for each $i \neq j \in \{1, 2, 3, 4 \}$, $\U[i] \cap \U[j] =0$, and we have $\U[1]
 \oplus \U[2] \oplus \U[3]=\mathbb{R}^3$. Let $\u[i]$ be a basis for $\U[i]$, so that
 $\U[i] = [\u[i]]$, and write $$\u[4] =\alpha \u[1] + \beta \u[2] + \gamma \u[3].$$


\begin{claim} 
\label{claim:abg}
Each of the coefficients $\alpha, \beta, \gamma$ is non-zero.
\end{claim}
\begin{proof}  Suppose $\alpha=0$. Then $\U[4] \subset \U[2] + \U[3]$, that is,
$$[\n[2], \n[3]]^\perp  \subset [\n[1], \n[3]]^\perp +[\n[1], \n[4]]^\perp $$ But this is equivalent to
$$ [\n[1]] = [\n[1], \n[3]] \cap [\n[1], \n[4]]  \subset [\n[2], \n[3]], $$ and hence $\{
\n[1], \n[2], \n[3]\}$ is not linearly independent, contradicting Assumption~\ref{asm:assumtionN}. The same argument holds for the other coefficients. 
 \end{proof}
 
 \begin{claim}  A necessary condition for 
\begin{equation} 
\label{eq:MasternIntersection2}
\bigcap \limits_{i=1}^{4}  \{\q[i] + \U[i] \} \neq \emptyset
\end{equation}
is that there exist real numbers $c_1, c_2, c_3, c_4$ such that
\begin{equation}
\label{eq:SimulEqns}
\begin{aligned}
    \Delta \q[12]:=\q[1]-\q[2]&=c_1 \u[1] + c_2\u[2] \\
    \Delta \q[13]:=\q[1]-\q[3]&=c_1 \u[1] + c_3\u[3] \\
    \Delta \q[14]:=\q[1]-\q[4]&=c_1 \u[1] + c_4 \u[4] \\
    \Delta \q[23]:=\q[2]-\q[3]&=c_3 \u[3] - c_2\u[2] \\
    \Delta \q[24]:=\q[2]-\q[4]&=c_4 \u[4] - c_2\u[2] \\
    \Delta \q[34]:=\q[3]-\q[4]&=c_3 \u[3] - c_4\u[4] \\
\end{aligned}
\end{equation}
\end{claim}

\begin{proof}
Each row of \eqref{eq:SimulEqns} corresponds to a condition of the form $\q[i]-\q[j] \in \U[i] \oplus \U[j]$. 
The proof proceeds by expressing each of the six rows in \eqref{eq:SimulEqns} with distinct coefficients (12 in total), and then writing down three necessary compatibility conditions, 
\begin{equation}
\label{eq:CompatibilityEqns}
\begin{aligned}
\Delta \q[12]-\Delta \q[13]+\Delta \q[23]&=0 \\
\Delta \q[12]-\Delta \q[14]+\Delta \q[24]&=0\\
\Delta \q[14]-\Delta \q[13]+\Delta \q[34]&=0.
\end{aligned}
\end{equation}
Because $-\t$ is in the intersection of the lines in \eqref{eq:MasternIntersection2},
the resulting linear equations must have a solution, and indeed direct computation
shows that the set of solutions can be parameterized as given in the claim.
\end{proof}

The next step is to note that $[\Delta \q[12], \cdots, \Delta \q[34] ] \subset
(s\R-\I)(\P)$, and hence its dimension must be less than three. Additional
straightforward calculations show that
$$\dim ~[\Delta \q[12], \Delta \q[13], \Delta \q[34] ] =  {\rm rank~} 
\begin{bmatrix}
    c_1 & c_1 & -\alpha c_4 \\ c_2 & 0 & c_2 - \beta c_4 \\ 0 & c_3 &-\gamma c_4
\end{bmatrix}.
$$

In light of Claim~\ref{claim:abg}, the rank is less than three if, and only if, any
two coefficients of $\{ c_1, c_2, c_3, c_4\}$ are zero. But if this is the case, then
at least one row of \eqref{eq:SimulEqns} must be zero. Each row of
\eqref{eq:SimulEqns}, however, has the form $(s\R-\I)(\p[a] - \p[b])$, where
$\{\p[a], \p[b] \}$ is a basis for $\P$, and thus it cannot be the case that $\dim
(s\R-\I)(\P) = 2.$ This completes the proof. 
\end{itemize}
\end{proof}

\newpage




\section*{Notes for ourselves}
Color code: \add{green} and \addd{brown} are the newly added sentences/
paragraphs. All of the automatic calibration related paragraphs are removed. 
The paper is too long now; I would suggest removing the plane estimation section. It
is still not enough (it would be around 8.6 pages). By reducing some white space and
shrinking some figures or shorten some paragraphs might fit to 8-page. 


Several points from the reviewers (I am still working on addressing them and
cross-out means the issue is addressed): 
\begin{enumerate}
\item More experiments with real data are needed. \bh{Shows some rings are
    poorly calibrated before and well-calibrated after.}
\item Provide more experimental validation from real data (both spinning lidar and
    solid state lidar). \bh{We don't have a solid state lidar yet but for spinning
    lidars, it could be the same as above.}
\item The computation time should be included.
\item How to get the initialization of the optimization problem? The authors should
    discuss the estimation of an initial solution, and how the error of an initial
    solution can impact on the convergence of the optimization algorithm.\bh{I will
    add this to experimental results.} 
\item Maybe add noise N1/N2 as described in the paper to the real data. \bh{This is
    actually a good idea.}
\item What is Table 3 reporting? \bh{Need to explain more}
\item What is the interest of this model? You claim that it is better than others,
    but what is your criterion ? How do you assess / prove it ? The experiments on
    simulated data only are weak.
\item What are the two type of uncertainities in Section V.A. \bh{Need to explain
    more}
\item Video: The presentation is good, but the image is deteriorated by
the compression and it is hard to read the captions.
Consider shortening the length of the video or using audio
for the conference video. \bh{Make the video shorter and larger captions}
\item \dels{What is the minimal case to determine the model parameters? How many
    solutions are there for the minimal case?}
\item \dels{The authors should focus only on the intrinsic calibration.}
\item \dels{How is error calculated in Table 1, what is the unit of
error?}
\item \dels{How is geometric uncertainty defined for Table 2?}
\item \dels{Table 3 shows the misleading results since the values in "Our Method" are in
    bold where the minimum value is originally in bold.}
\item \dels{It is interesting if the 7-parameters rigid body
transformation can approximate the intrinsic parameter.
However, the novelty looks week because the method just
replacing the model. Therefore, deeper evaluation and
discussion is required.}
\end{enumerate}