\section{Introduction}

Autonomous driving calls for powerful state estimation of the surrounding objects, including their positions, orientations, etc.
Among all the sensors available for state estimation, LiDAR is unique in providing accurate 3D sensing.
Nevertheless, the complexity of road scenarios and sparsity of point clouds pose great difficulties for state estimation.
Therefore, estimating the states of objects using LiDAR data has long been valuable yet challenging for autonomous driving research.

\begin{figure}
    \centering
    \includegraphics[width=1.0\columnwidth]{figures/cover.pdf}
    \caption{Our vehicle tracking system and its applications. \textbf{Top}. Tracking results  visualized at an interval of 2 seconds (20 frames).
    \textbf{Middle}. Shape completion by overlaying point clouds.
    \textbf{Bottom}. Applications using the estimated states and shapes, such as optical flow annotation.
    More usages are described in Sec.~\ref{sec::downstream}.}
    \label{fig::cover}
    \vspace{-20px}
\end{figure}

The mainstream method for object tracking is to utilize the so-called ``detection and tracking'' paradigm.
Specifically, it mostly focuses on the association of bounding boxes across frames.
The accuracy of tracking then heavily relies on the performance of detection,
since most tracking algorithms directly accept the bounding boxes from detection with minor modifications.
Current state-of-the-art object detection algorithms are mostly \emph{model-based}:
the algorithms carry out the detection task with their recognition power gained from learning on large scale training sets.
The benchmarks nowadays~\cite{waymo_open, argoverse, KITTI, nuScenes} also mainly focus on this \emph{model-based} MOT setting.
However, we propose to pay attention to the \emph{model-free} Single-Object Tracking (SOT) setting,
which targets on some overlooked applications and challenges in MOT.

First, model-based detection is prone to fail on sparse point clouds and unfamiliar objects (as in Fig.~\ref{fig::large_car}). By being ``model-free'', the algorithms can generalize to these cases well, since it is not assumed that similar objects have been met before. 
The task of \emph{model-free} SOT is then useful for both online and offline applications from two aspects:
1) For offline settings, model-free SOT can augment the training set for model-based method by providing different appearances of an object in different frames or viewpoints.
2) For online settings, SOT serves as a redundant system for model-based methods when they fail (as are demonstrated in Fig.~\ref{fig::large_car}). It is mandatory for a high-level autonomous system that requires no single failure point. 

Second, model-free SOT algorithms rely on richer information for tracking, such as raw point clouds, compared to MOT methods that mostly use bounding boxes only.
Therefore, the SOT algorithms specialize at providing more subtle state information with even centimeter level accuracy.
This property is important for many applications in autonomous driving, such as generating first order motion information, as discussed in Sec.~\ref{subsec::motion_generation}

\begin{figure}
    \centering
    \includegraphics[width=1.0\columnwidth]{figures/large_car_det.pdf}
    \caption{\textbf{Left:} The detection algorithm (from \url{https://github.com/open-mmlab/mmdetection3d}) fails on certain cases,
        including false negatives (low detection score or missing detection) and wrong size. 
        But our tracker still has high quality after a period of tracking, with only requiring an initial bounding box as input.
    	We sample two frames from a continuous period of failed detections for demonstration.
    Gray: Point Cloud; Red: The Ground Truth Box; Dashed Blue: SOT Box; Black: Detection Boxes.
    \textbf{Right:} We further visualize the quality of SOT with the aggregated point clouds, following~\cite{Held-Combining,IROS-Continuous-time}.
    As is demonstrated, our algorithm can provide accurate tracking and shape aggregation results.
    }
    \label{fig::large_car}
    \vspace{-20px}
\end{figure}

For a target object, the input to SOT algorithms is its initial bounding box in the first frame, which may come from human annotations or highly confident detections.
The algorithms then estimate the object's states in every subsequent frame, as is illustrated in the top of Fig.~\ref{fig::cover}.
During solving state estimation, we notice the value of its by-product: aggregating point clouds along the tracklets completes denser shapes of objects, which is shown in the middle of Fig.~\ref{fig::cover}.
In our experiments, leveraging these shapes significantly improves the performance of tracking and state estimation.
This utility of shape aggregation also benefits a series of other applications in autonomous driving.
For example, with the generated shapes and motion information,
we can create vehicle bank for creating virtual LiDAR scenes~\cite{LiDARsim}, optical flow annotations~\cite{KITTI-Flow} (our result visualized in Fig.~\ref{fig::cover}, bottom row), etc.
These tasks so far mostly require manual labeling or artificial CAD models,
so automating the process of directly using real data could greatly improve the efficiency and scalability.
The details of such downstream applications are discussed in Sec.~\ref{sec::downstream}.

To summarize, our contributions are as follows:
\begin{itemize}
    \item We propose a novel LiDAR-based object tracking task\footnote{We focus on vehicle, and are working on extending to other types of objects.}.
    To facilitate further study, we create a new benchmark called LiDAR-SOT\ based on the WOD dataset~\cite{waymo_open}.
    \item We propose a model-free SOT algorithm called SOTracker. It involves optimization for point cloud registration, object shapes and motions.
    \item We evaluate our SOTracker\ on LiDAR-SOT\ and discuss the potential usages in autonomous driving, such as motion data generation, LiDAR scan simulation~\cite{LiDARsim}, and optical flow annotation~\cite{KITTI-Flow}.
\end{itemize}

\section{Related Work}
\label{sec::related-work}

\subsection{Datasets and Benchmarks}
\label{subsec::related-work-benchmarks}

Many existing autonomous driving benchmarks, including Waymo Open Dataset (WOD)~\cite{waymo_open}, Argoverse~\cite{argoverse}, KITTI~\cite{KITTI}, nuScenes~\cite{nuScenes} have rich point clouds and tracklets for object tracking.
However, they mainly adopt the MOT setting while our task demands the SOT setting and not all the tracklets for MOT evaluation suits the requirements for SOT.
So we set out to design a test bed specially for SOT evaluation.
Among the mentioned benchmarks, WOD~\cite{waymo_open} is superior in scale and comprehensiveness, so we build the benchmark LiDAR-SOT\ based on the data of WOD.

On designing the evaluation metrics, we inherit the idea from VOT~\cite{VOT} for tracking performance, concerning both the accuracy and robustness.
As for shape completion, we include it in evaluation because of its usages in downstream applications.
We refer to ShapeNet~\cite{shapenet} and use Chamfer-Distance(CD) for shape evaluation.

\subsection{State Estimation and Tracking in Point Cloud}
\label{subsec::related-work-estimation}

State estimation in raw point clouds is challenging because it involves associating the LiDAR points with the target object (a.k.a. tracking) and then accurately estimate the state.

Many methods adopt a model-based approach to discover the object in LiDAR points.
\cite{darpa-tracking, darms2008classification, tracking-and-detection-icra09, Generative-object-detection}
fit the vehicles with boxes and~\cite{TanzmeisterTWB14, DanescuON11} utilizes occupancy grid for detection.
In the era of deep learning, the learned neural networks replace the manually designed models.
P2B~\cite{P2B} generates and verifies bounding boxes guided by a learned PointNet++~\cite{pointnet++}, 
Zou \textit{\latinabbrev{et al}}~\cite{zou2020f} manages to fuse RGB information using frustums, 
Zarzar \textit{\latinabbrev{et al}}~\cite{PointRGCN} refines the detection results by association,
while~\cite{PIXOR, fast-and-furious} apply convolutional neural networks(CNN) on the Bird-Eye-View images of LiDAR,
and Giancola \textit{\latinabbrev{et al}}~\cite{LeveragingShapeCompletion} learns to complete the partial point cloud then searches for it.

These model-based approaches heavily rely on detection or shape completion in every single frame.
So point cloud sparsity of unfamiliar objects could easily lead to failures.
However, our model-free setting seeks to tackle this since it does not need to fit a model and considers multi-frame information from LiDAR sequences.

Different from the aforementioned works involving searching for the target object in raw LiDAR data, some studies assume the point cloud could be well-segmented or the segmentation information is available.
Hence their focus is state estimation in relatively clean point cloud data.
Some representative methods are as follows,
Held \textit{\latinabbrev{et al}}~\cite{Held-Combining} minimizes an energy function on the inconsistency between observed point cloud and object states.
Gro{\ss} \textit{\latinabbrev{et al}}~\cite{alignnet-3d} registers the motion between point cloud snapshots.
Goforth \textit{\latinabbrev{et al}}~\cite{joint-lucey} learns to complete the shapes of vehicles and jointly decode the states.
The problem of these methods lies in the assumption of well-segmented point cloud, which is rare in practice.
Therefore, by only providing the first frame annotation, our setting enforces the algorithms to face the challenge of solving tracking and state estimation simultaneously.

Many previous model-free works~\cite{moosmann2013joint, wang2015model, dewan2016motion, pomerleau2014long, azim2012detection, tipaldi2009motion, van2010integrated} 
mainly focus on the problem of MOT.
They first carry out the detection task leveraging either the motion information or assuming that LiDAR points draw the boundaries of objects.
Then these methods associate the detected bounding boxes according to the estimated motion information or motion models.  
Therefore, these methods differ from our objectives for using SOT. 

The only work we found falls into our setting is Ushani \textit{\latinabbrev{et al}}~\cite{IROS-Continuous-time}.
Dating back to that period, there is no large-scale dataset fits the evaluation needs,
thus~\cite{IROS-Continuous-time} only tests on a private dataset, which makes the comparisons with their work infeasible.
This is just the reason to propose our new LiDAR-SOT benchmark, which aims to promote the studies in this area.

\subsection{Point Cloud Completion}
\label{subsec::related-work-completion}

Following the paradigm of PCN~\cite{PCN}, people can complete point clouds with neural networks trained from artificial CAD models in graphics datasets, such as ShapeNet~\cite{shapenet}.
~\cite{joint-lucey, LeveragingShapeCompletion} adopt the same idea for vehicles, which is the specific class of interest in this paper.
However, these methods could hardly scale up as CAD models are expensive to acquire and limited to some categories.
Recently, Gu \textit{\latinabbrev{et al}}~\cite{weakly-completion} discards the need for CAD models by leveraging multi-view sensors.
But multi-view sensor information are also not universal for real-world scenarios.

Considering the mentioned drawbacks of supervised approaches, completing point cloud by tracking interests us most.
\cite{DaraeiVM17, christie20163d, JiangCPD17} jointly use LiDAR and camera for this task.
~\cite{Held-Combining, IROS-Continuous-time} are closest to our objective in using LiDAR only:
Held \textit{\latinabbrev{et al}}~\cite{Held-Combining} aggregates the observed point clouds, while Ushani \textit{\latinabbrev{et al}}~\cite{IROS-Continuous-time} directly optimizes a point cloud model.

\section{LiDAR-SOT Benchmark}
\label{sec::benchmark}

\subsection{Problem Formulation}
\label{subsec::prob_formulation}

Suppose the dataset has $N$ tracklets $\mathcal{T}^{1:N}=\{\mathcal{T}^{1}, \mathcal{T}^{2}, ...\mathcal{T}^{N}\}$ with lengths $L^{1:N}=\{L^{1}, L^{2}, ..., L^{N}\}$.
We refer to the $i$-th frame in the $j$-th sequence $\mathcal{T}^j$ as $\mathcal{T}^{j}_{i}$,
which contains the point cloud $P^{j}_{i}$ and a bounding box $\widetilde{B}^{j}_{i}$.
The box $\widetilde{B}^{j}_{i} \triangleq \{\widetilde{C}^j_i, \widetilde{S}^{j}_{i}\}$ describes the size and state of an object.
In our benchmark, $\widetilde{C}_i^j$ are the length, width and height of a 3D cuboid,
and the state $\widetilde{S}_{i}^{j}\triangleq[\widetilde{x}_i^j, \widetilde{y}_i^j, \widetilde{z}_i^j, \widetilde{\theta}_i^j]$ represents the 3D coordinates of the object's center and the heading.
We only consider 4DOF instead of 6DOF because roll and pitch deviations are usually aligned to the road for autonomous driving.
For a tracklet $\mathcal{T}^j$, it also contains the shape ground-truth of target object $\widetilde{M}^j$.

For an arbitrary tracklet $\mathcal{T}^j$, the algorithms are fed with point cloud $\{P_{1:L^j}^j\}$ and first frame ground-truth bounding box $\widetilde{B}_1^j$.
The outputs are the estimated states $\{S_{2:L^{j}}^{j}\}$ and completed object shape $M^j$.
Note that we assume the size of an object remains unchanged, thus it keeps the value as $\widetilde{C}_{1}^{j}$ in subsequent frames. 
This assumption is generally held for rigid objects such as vehicles.

The algorithms can run under either the online or the offline setting.
The online setting restricts the algorithms to access point cloud data $\{P_{1:L^{j}}^L\}$ by order, 
and output and fix the estimation results before getting the next frame input.
The offline setting, in comparison, poses no constraints.
In this paper, we concentrate on the online setting because it fits both on-the-road autonomous driving and offline applications.

\subsection{Discussion with Closely Related Tasks}
In this section, we highlight the differences between our proposed task and several relevant tasks.

\noindent
\textbf{Object Detection:} 
Object detection aims to detect some predefined types of object in raw point clouds. 
So it must be model-based and rely on manual priors or large scale training data. 
Our task does not assume known appearances of the objects to be tracked, but only its rigidity.
Instead, we only takes the initial states of objects as input.

\noindent
\textbf{Multiple Object Tracking:} 
As mentioned above, MOT relies on detection and mainly focuses on the association of bounding boxes. 
Moreover, MOT usually does not modify or generate new bounding boxes, while our SOT task creates bounding boxes according to the point cloud and motion information.

\noindent
\textbf{Shape Completion:} 
Generally speaking, shape completion complements the partial point cloud in a single frame under the supervised setting. 
It needs paired training data to learn generative models, while ours could utilize temporal LiDAR data to complete the shapes in an unsupervised manner.

\noindent
\textbf{Point Cloud Registration:}
Registration focuses on matching the point clouds between two frames, while the algorithms could access more previous frames in our task. 
Consequently, rich priors could be incorporated to improve the performance, as demonstrated in the experiments, the performance of our state estimation is significantly higher than using ICP only.

\subsection{Dataset Construction}

To facilitate the study on the aforementioned task, we propose a new dataset called LiDAR-SOT.
It is largely based on the most comprehensive and largest point cloud dataset: Waymo Open Dataset (WOD)~\cite{waymo_open}. 
Considering the vehicles are the most abundant rigid objects for autonomous driving, we only consider the type of vehicles for now.
The WOD dataset contains full annotations of 3D bounding boxes and their associations across frames for the evaluation of MOT, thus, enabling our evaluation for state estimation in point cloud sequences.
As for the evaluation for shape completion, which WOD contains no ground truth, we create pseudo-ground-truth shapes (PGTs) for each tracklet by selecting and aggregating the LiDAR points from every frame guided by the ground-truth bounding boxes.

Our algorithm proposed in the paper require no additional training sets.
However, we still include a training set for LiDAR-SOT\ by inheriting all the sequences from WOD training set and leaving users with freedom of training/validation split.
As for the test set, we select the suitable tracklets from WOD validation set to ensure meaningful evaluation.
In total, 1121 vehicle tracklets are included based on the following criteria:

\noindent
\textbf{Tracklet length.} Short sequences may result in unstable evaluation results and they are also not challenging enough for SOT tasks.
We therefore filter out the tracklets less than 100 frames.

\noindent
\textbf{Initial number of points.} 
The numbers of LiDAR points in the beginning frames are crucial for the meaningful initialization of single-object trackers. VOT~\cite{VOT} sets up a warm-up period of 10 frames. And enough number of LiDAR points is also necessary for human annotators to provide a initial bounding box with high quality for offline applications.
We therefore take both aspects into consideration.
Specifically, we only keep the tracklets whose initial 10 frames all have more than 20 points. 

\noindent
\textbf{Vehicle mobility.}
We exclude the tracklets in which the vehicle stays static all the time, as these cases have trivial solutions and bring negative biases to the evaluation.
Note that we still keep the tracklets where the vehicles start or end with static states.

As visualized in the left of Fig.~\ref{fig::benchmark}, although we guarantee the number of LiDAR points in the beginning, the long-tail distribution still indicates the challenges in point cloud sparsity. Moreover, the right of Fig.~\ref{fig::benchmark} illustrates the difficulty of LiDAR-SOT\ in long tracklets (note that the original WOD~\cite{waymo_open} has a maximum of 200 frames).

\begin{figure}
    \centering
    \includegraphics[width=1.0\columnwidth]{figures/benchmark.pdf}
    \caption{Statistics of LiDAR-SOT. 
        \textbf{Left}. Average per-frame LiDAR point number in the first 10 frames.
        \textbf{Right}. Distribution of tracklet lengths. }
    \vspace{-20px}
    \label{fig::benchmark}
\end{figure}

For future analyses, we follow the paradigm of KITTI~\cite{KITTI} and divide the test set into easy, medium and hard subsets.
Intending on the difficulty of point cloud sparsity, we compute the average point number of each tracklet's first 10 frames, then equally split the tracklets into three sets according to the average point number.
The specific thresholds are 38.2 and 808.3 points per frame.

\subsection{Evaluation Metrics}
\label{subsec::metrics}

Regarding the output on tracklet $\mathcal{T}^{j}$: states $\{S_{2:L^{j}}^{j}\}$ and shape $M^j$,
we use multiple metrics for thorough evaluation.
Our focus is on the quality of estimated states in SOT and shape completion.

\noindent
\textbf{State Estimation}
For the state estimation task, we are interested in how long the algorithm could track the object and its tracking quality. 
Consequently, inspired by Visual Object Tracking (VOT)~\cite{VOT} work, we propose two metrics for single pass evaluation.

\textbf{Accuracy.}
We denote $\mathtt{IOU}(B^{j}_{i}, \widetilde{B}^{j}_{i})$ as the $\mathtt{IOU}$ of the estimated and ground-truth bounding boxes.
We then propose a metric reflecting the state estimation accuracy by averaging the $\mathtt{IOU}$ on each frame as: 
\begin{equation}
	\mathtt{Acc}=\frac{1}{\sum_{k=1}^{N}{L^{k}}}\sum_{k=1}^{N}{\sum_{j=1}^{L^{k}}{\mathtt{IOU}(B^{k}_{j}, \widetilde{B}^{k}_{j})}}.
	\label{eq::metric_overall}
\end{equation}

\textbf{Robustness.}
Given a pre-defined failure threshold $t$, we let the tracking length $l^{j}(t)$ be the first frame number before $\mathtt{IOU}$ drops lower than $t$ in tracklet $\mathcal{T}^j$.
This can be served as measurement for robustness.
However, as a single threshold $t$ may not be comprehensive, we compute the area under $t$ - $\mathtt{Rob}(t)$ curve by Eq.~\ref{eq::metric_rob}. 
In implementation, we approximate the integral by sampling $t$ from 0 to 1 with intervals of 0.05.
\begin{equation}
    \mathtt{Rob}(t) = \frac{\sum_{k=1}^{N}{l^{k}(t)}}{\sum_{k=1}^{N}{L^{k}}}, \quad \mathtt{Rob} = \int_0^1 \mathtt{Rob}(t)\mathrm{d}t
    \label{eq::metric_rob}
\end{equation}

\noindent
\textbf{Shape Completion}
As the PGTs comes from overlaying all the point cloud from tracklets, their densities are non-uniform.
To avoid unstable evaluation, we downsample both the PGTs $\{\widetilde{M}^{1:N}\}$ and our completion $\{M^{1:N}\}$ to the resolution of 5cm during evaluation.
For shape $M^{i}$, we use Chamfer Distance (CD) to measure its distance to $\widetilde{M}^{i}$ as Eq.~\ref{eq::metric_cd}. 
Then the $\mathtt{Shape}$ metric is the average of $\mathtt{CD}$ on all tracklets.
\begin{equation}
	\begin{small}
    \begin{aligned}
        \mathtt{CD}^i = & \frac{1}{|M^i|}\sum_{a\in M^i}\min_{b\in \widetilde{M}^i}||a-b||_2 + 
        \frac{1}{|\widetilde{M}^{i}|}\sum_{b\in \widetilde{M}^i}\min_{a\in M^i}||a-b||_2 \label{eq::metric_cd} \\
    \end{aligned}    
    \end{small}
\end{equation}
\vspace{-8px}
\section{SOTracker}
\label{sec::method}
In this section, we elaborate our proposed SOTracker. 
The SOT task is essentially a cross frame registration task, but a series of priors could be explored for enhancement. 
Briefly speaking, our method estimates the relative motions between frames using registration and various motion priors, and aggregate them into absolute state estimation. 
Along the process of tracking, we also combine the shapes on the fly for further improvement.

\subsection{Pipeline}
\label{subsec::pipeline}

We take an arbitrary tracklet $\mathcal{T}$ for example to explain the algorithm.
The notation $\Delta S_{k} = S_k - S_{k-1}$ is the motion between $S_{k-1}$ and $S_k$. 
Its entries $\Delta S_{k}=[\Delta x_k, \Delta y_k, \Delta z_k, \Delta \theta_k]$ are the 3D translation and rotation of heading.
In the following sections, we use $O_{k}$ to denote the estimated LiDAR points on the surface of the object in frame $\mathcal{T}_k$, in comparison with the raw point cloud $P_k$. 

The pipeline of SOTracker\ is in Algo.~\ref{algo::track}, where
it successively estimates the state on each frame with function $\mathtt{FrameEstimate}$ in Algo.~\ref{algo::iter}.
In frame $\mathcal{T}_k$, SOTracker\ first solves $S_k$ with $\mathtt{FrameEstimate}$; 
then forms shape $M_k$ by combining previous shape $M_{k-1}$ and object point cloud $O_{k}$ together;
it eventually updates the motion information $\Delta S_{prior}$, which is used for initializing the next frame's state estimation. 
In $\mathtt{FrameEstimate}$, the function initializes the state $\widehat{S}_k$ from adding the $\Delta S_{prior}$ onto previous state $S_{k-1}$.
Then it iteratively discovers the relevant LiDAR points $\widehat{O}_{k}$ and optimizes the state $S_{k}$ towards a loss function $\mathcal{L}$. 

Some other details are clarified as follows:

\subsubsection{}We implement $\Delta S_{prior}$ as the moving average of motions and use $\alpha=0.5$ for the update in Algo.~\ref{algo::track}.

\subsubsection{}$\mathtt{PointInBox}(P, S, C, \gamma)$ selects LiDAR points from $P$ that locates in the cuboid with center $S$ and size $\gamma C$.
We set different $\gamma$ values for different usages.
$\gamma_{in}=1.5$ is for the selection inside an estimation loop, while $\gamma_{aft}=1.1$ is for updating shapes.

\subsubsection{}The shape updating happens in $\mathtt{OverlayShape}$, where the incoming LiDAR points are overlaid at $\widetilde{S}_1$ by default.
As taking LiDAR points from every frame brings heavy burden for computation, 
we only combine new point clouds at key frames (that is every 5 frames in our implementation).

\subsubsection{}The second frame is special for $\mathtt{FrameEstimate}$ as previous motion information $\Delta S_{prior}$ is not available.
During estimating $S_2$, we let $\Delta S_{prior}=0$ and use a larger search region for $\mathtt{PointInBox}$ by setting $\gamma_{in}=3.0$.
After this, we initialize the motion prior by $\Delta S_{prior}\xleftarrow{}\Delta S_2$.

\vspace{-8px}
\begin{algorithm}[!]
    \renewcommand{\algorithmicrequire}{\textbf{Input:}}
	\renewcommand{\algorithmicensure}{\textbf{Output:}}
    \caption{Algorithm for tracklet $\mathcal{T}$}
    \label{algo::track}
    \begin{algorithmic}[1]
        \REQUIRE Point cloud: $P_{1:L}$, bounding box $\widetilde{B}_1$, 
        Size Scaling: $\gamma$,
        Motion Prior Update Factor: $\alpha$ 
        \ENSURE vehicle states: $S_{2:L}$, shape $M_{L}$
        \STATE Frame number $k \xleftarrow{} 2$
        \STATE $\Delta S_{prior} \xleftarrow{} 0$
        \STATE $O_1 \xleftarrow{} \mathtt{PointInBox}(P_1, \widetilde{S}_1, \widetilde{C}_1, 1.0)$
        \STATE $M_1 \xleftarrow{} O_1$
       
        \FOR{$k$ $<=$ $L$}
        \STATE $S_{k}\xleftarrow{}\mathtt{FrameEstimate}(P_k, O_{k-1}, \Delta S_{prior}, S_{k-1},$\\
        \qquad\qquad\qquad\qquad$\widetilde{C}_1, M_1, M_{k-1}, \gamma_{in})$
        \STATE $O_{k} \xleftarrow{} \mathtt{PointInBox}(P_k, S_K, \widetilde{C}_1, \gamma_{aft})$
        \STATE $M_{k} \xleftarrow{}\mathtt{OverlayShape}(O_{k}, S_{k}, M_{k}, k)$
        \STATE $\Delta S_{prior} \xleftarrow{} \alpha\Delta S_{prior} + (1-\alpha)\Delta S_{k}$
        \STATE $k \xleftarrow{} k + 1$
        \ENDFOR
    \end{algorithmic}
\end{algorithm}
\vspace{-15px}
\begin{algorithm}[!]
    \renewcommand{\algorithmicrequire}{\textbf{Input:}}
	\renewcommand{\algorithmicensure}{\textbf{Output:}}
    \caption{$\mathtt{FrameEstimate}$ on frame $\mathcal{T}_k$}
    \label{algo::iter}
    \begin{algorithmic}[1]
        \REQUIRE Point Cloud $P_{k}$ and $O_{k-1}$, Motion Prior: $\Delta S_{prior}$, State: $S_{k-1}$, Size $\widetilde{C}_1$, 
        Shape $M_1, M_{k-1}$, parameters $\gamma_{in}$
        \ENSURE State Estimation $S_k$
        \STATE $\widehat{S}_{k} \xleftarrow{} S_{k-1} + \Delta S_{prior}$
        \FOR{Iteration Number $<$ MaxIter}
        \STATE $\widehat{O}_{k} \xleftarrow{} \mathtt{PointInBox}(P_{k}, \widehat{S}_k, \widetilde{C}_{1}, \gamma_{in})$
        \STATE $S_{k}^*\xleftarrow{} \mathop{\arg\min}\limits_{S_{k}} \mathcal{L}(S_{k}, \widehat{O}_k, O_{k-1}, S_{k-1}, M_{k-1}$)
        \STATE $\widehat{S}_{k} \xleftarrow{} S_{k}^*$
        \ENDFOR
        \STATE $S_{k} \xleftarrow{} \widehat{S}_{k}$, 
    \end{algorithmic}
\end{algorithm}
\vspace{-15px}

\subsection{Objective Function} 
\label{subsec::factor}

The objective function is as Eq.~\ref{eq::loss}. It is the weighted sum of the four terms: ICP Term, Shape Term, Motion Consistency Term, and Motion Prior Term.
We set their weights by $[1, 1, 0.1, 0.1]$.
Eventually, we use the BFGS solver from scipy~\cite{virtanen2020scipy} to optimize $\mathcal{L}$.
\begin{equation}
	\label{eq::loss}
    \mathcal{L} = w_{I}\mathcal{L}_{ICP} + w_{S}\mathcal{L}_{Shape} + w_{MC}\mathcal{L}_{MC} + w_{MP}\mathcal{L}_{MP}
\end{equation}

In the sequel, we use the notation $\Delta S \otimes P_{S}$ to denote apply motion $\Delta S=[\Delta x, \Delta y, \Delta z, \Delta \theta]$ to the point cloud $P$, relative to the coordinate system centered at $S=[x, y, z ,\theta]$.
$\Delta S \otimes P_S$ equals to the first 3 rows of Eq.~\ref{eq::motion_notation}.

\begin{equation}
\begin{small}
	\begin{aligned}
	\begin{bmatrix}
		\cos\Delta\theta & -\sin\Delta\theta & 0 & x+\Delta x-x\cos\Delta\theta+y\sin\Delta\theta\\
		\sin\Delta\theta &  \cos\Delta\theta & 0 & y+\Delta y-x\sin\Delta\theta-y\cos\Delta\theta \\
		0                & 0                 & 1 & \Delta z \\
		0                & 0                 & 0 & 1
	\end{bmatrix}
	\begin{bmatrix}
        P \\
        1
    \end{bmatrix}
	\end{aligned}
\end{small}
\label{eq::motion_notation}
\end{equation}

We explain each term in the following sections. 

\subsubsection{ICP Term}
\label{subsubsec::icp}

Registering the point cloud $\widehat{O}_k$ and $O_{k-1}$ indicates the motion between these two frames.
Therefore, we propose a registration term $\mathcal{L}_{ICP}$ as in Eq.~\ref{eq::icp}.
The details for finding associated point pairs $\mathop{\mathcal{A}}_{I}^{O_{k - 1}, \widehat{O}_k}$ are in Sec.~\ref{subsubsec::lidar_pa}.

\begin{equation}
    \begin{small}
        \begin{aligned}
    \mathcal{L}_{ICP}=\frac{1}{|\mathop{\mathcal{A}}_{I}^{O_{k - 1}, \widehat{O}_k}|}\sum_{(p, q) \in \mathop{\mathcal{A}}_{I}^{O_{k - 1}, \widehat{O}_k}}||\Delta S_k \otimes p_{S_{k-1}}-q||_2^2
        \end{aligned}
    \end{small}
    \label{eq::icp}
\end{equation}

\subsubsection{Shape Term}
\label{subsubsec::shape}

Registering the point cloud in neighboring frames fails in long term tracking due to drifting or shifting in point cloud distribution.
We ease this by adding regularization between the shapes of objects and current frame point clouds. 
For the shape $M$ of the object, we penalize the inconsistency between the current frame point cloud $\widehat{O_k}$ and the latest shape $M_{k-1}$ by registering them.
In the implementation, we first consider using the first frame point cloud as a baseline, since the initial bounding box has high quality and the related point clouds are accurate.
Then we aggregate new LiDAR points along the process of tracking, and use this updated shape to compute the loss function.
Formally, the Shape Term is computed as Eq.~\ref{eq::shape}. 
$\mathop{\mathcal{A}}_{S}^{M_{k-1}, \widehat{O}_k}$ is the set of associate point pairs, explained in Sec.~\ref{subsubsec::lidar_pa}.

\begin{equation}
    \begin{small}
        \begin{aligned}
    \mathcal{L}_{Shape} = \frac{1}{|\mathop{\mathcal{A}}_{S}^{M_{k-1}, \widehat{O}_k}|}\sum_{(p, q) \in \mathop{\mathcal{A}}_{S}^{M_{k-1}, \widehat{O}_k}}||(S_k-\widetilde{S}_1)\otimes p_{\widetilde{S}_1} - q||_2^2
        \end{aligned}
    \end{small}
    \label{eq::shape}
\end{equation}  

\subsubsection{Motion Consistency Term}
\label{subsubsec::motion_consistency}

The objects cannot be treated as point mass, so their movements have to follow certain constraints.
For simplicity, we enforce the most general constraint: the directions of motions should be consistent with the headings of objects.   
The corresponding term is computed as Eq.~\ref{eq::motion_consistency_term}, 
where $v = [\Delta x_{k}, \Delta y_{k}]$ represents the velocity.
\begin{equation}
    \label{eq::motion_consistency_term}
    \mathcal{L}_{MC}=\left\|||v||_2\cos(\frac{\theta_{k-1} + \theta_{k}}{2})-\Delta x_{k}\right\|_2^2
\end{equation}

\subsubsection{Motion Prior Term}
\label{subsubsec::motion_prior}

To combine the motion model into the optimization framework, we design the ``Motion Prior'' term. 
It penalizes the inconsistency between the optimization results $\Delta S_k$ and the prediction from motion models $\Delta S_{prior}$.
This term encourages the consensus between the latest optimization and trajectory histories.
The computation of Motion Prior Term is as Eq.~\ref{eq::motion_prior_term}.
\begin{equation}
	\label{eq::motion_prior_term}
	\mathcal{L}_{MP}=\left\| \Delta S -\Delta S_{prior} \right\|_2^2
\end{equation}
 
\subsection{Design Choices}
\label{subsec::design}

We identify several key implementation factors that are helpful for the performance of state estimation.

\subsubsection{Ground Removal}
The ground points are irrelevant with the vehicle's states and shapes, and they usually confuse the algorithm.
So we remove the ground points using~\cite{zermas2017fast} as a pre-processing operation.

\subsubsection{Subshape}
Due to the sparsity of LiDAR point clouds, it is challenging to only use single frame for registration.
In the ICP term, we can enhance the performance by overlaying the LiDAR points in nearby frames.
In the implementation, we overlay additional 2 frames $O_{k-3:k-2}$ onto $\widehat{O}_{k-1}$ in Eq.~\ref{eq::icp}.

\subsubsection{LiDAR Point Association}
\label{subsubsec::lidar_pa}
As a prerequisite of computing the ICP Term and Shape Term in Sec.~\ref{subsec::factor}, 
the LiDAR points have to be associated according to the rules of nearest neighbors.
As for the ICP term described in Sec.~\ref{subsubsec::icp}, 
we follow the common approach and construct the associated point set $\mathop{\mathcal{A}}_{I}^{O_{k - 1}, \widehat{O}_k}$ as Eq.~\ref{eq::forward}.
In the equation, the function $\mathtt{NN}(p, \mathcal{B})$ means finding the nearest neighbor for point $p$ in the point set $\mathcal{B}$.
\begin{equation}
    \label{eq::forward}
    \begin{footnotesize}
    \begin{aligned}
        \left\{ (x, y) \mid y = \mathtt{NN}(\Delta S_{k}\otimes x_{S_{k-1}}, \widehat{O}_{k}), \forall x\in O_{k-1} \right\}
    \end{aligned}
    \end{footnotesize}
\end{equation}

However, as for the Shape Term in Sec.~\ref{subsubsec::shape}, the point clouds $M_{k-1}$ and $\widehat{O}_k$ may differ seriously during the process of tracking.
The outliers of association lead to deviated solutions and degrade the state estimation.
Therefore, we use RANSAC for Shape Term to exclude the outlier pairs.
The result $\mathcal{A}_{S}^{M', \widehat{O}_k}$ is computed as Eq.~\ref{eq::shape_outlier}.

\begin{equation}
    \label{eq::shape_outlier}
    \begin{footnotesize}
    \begin{aligned}
        \mathtt{RANSAC}(\left\{ (x, y) \mid y = \mathtt{NN}((\Delta S_{k}-\widetilde{S}_{1})\otimes x_{\widetilde{S}_{1}}, \widehat{O}_{k}), \forall x\in M_{k-1} \right\})
    \end{aligned}
    \end{footnotesize}
\end{equation}

\vspace{-8px}

\subsection{Limitations and Future Work}

For simplicity, our LiDAR-SOT\ is currently limited to the rigid type of objects: vehicles.
Extending it to more general types of objects, such as cyclists and pedestrians, 
requires the modeling for the periodically and continuously changing shapes. Using multiple templates or implicit functions~\cite{park2019deepsdf} for their shapes could be the solution.

Combining SOT methods with the multiple detections in different frames also poses great challenges, 
as the quality of detection bounding boxes may vary across different frames.
Therefore, we have to model the quality of them and combine our optimization-based algorithm into the MOT framework. 
Also, the disappearance or occlusion of an object need to be considered in the setting. 
We leave these two issues, and will try to solve them in future work.
\section{Results and Analyses}
\label{sec::results}

\begin{table*}[!ht]
    \centering
    \resizebox{1.0\linewidth}{!}{
    \begin{tabular}{@{\hspace{2.7mm}}l|@{\hspace{2.0mm}}c@{\hspace{2.0mm}}c@{\hspace{2.0mm}}c|@{\hspace{2.0mm}}c@{\hspace{2.0mm}}c@{\hspace{2.0mm}}c|
        c@{\hspace{2.0mm}}c@{\hspace{2.0mm}}c|@{\hspace{2.0mm}}c@{\hspace{2.0mm}}c@{\hspace{2.0mm}}c@{\hspace{2.0mm}}c}
        \hline
        \multirow{2}{*}{Method} & \multicolumn{3}{c}{All} & \multicolumn{3}{c}{Easy} & \multicolumn{3}{c}{Medium} & \multicolumn{3}{c}{Hard} \\
        \cline{2-13} & Acc$\uparrow$ & Rob$\uparrow$ & Shape$\downarrow$  &Acc$\uparrow$ & Rob$\uparrow$ & Shape$\downarrow$  & Acc$\uparrow$ & Rob$\uparrow$ & Shape$\downarrow$  & Acc$\uparrow$ & Rob$\uparrow$ & Shape$\downarrow$  \\
        \hline\hline
        ICP+MP           & 0.4214 & 0.4009 & 0.1836 & 0.5509 & 0.5357 & 0.1067 & 0.3861 & 0.3609 & 0.1792 & 0.3038 & 0.2824 & 0.2652 \\
        ICP+MP+S(1)      & 0.6142 & 0.5336 & 0.1226 & 0.7582 & 0.7049 & 0.0612 & 0.5598 & 0.4747 & 0.1218 & 0.4998 & 0.3915 & 0.1851 \\
        ICP+MP+S(All)    & 0.6279 & 0.5616 & 0.1239 & 0.7734 & 0.7285 & 0.0591 & 0.5878 & 0.5104 & 0.1261 & 0.4967 & 0.4163 & 0.1867 \\
        \textbf{ICP+MP+S(All)+MC}  & \textbf{0.6409} & \textbf{0.5758} & \textbf{0.1164} & \textbf{0.7751} & \textbf{0.7309} & \textbf{0.0589} & \textbf{0.6019} & \textbf{0.5216} & \textbf{0.1173} & \textbf{0.5219} & \textbf{0.4481} & \textbf{0.1727} \\
        \hline
        w/o GM        & 0.5944 & 0.5385 & 0.1284 & 0.7229 & 0.6701 & 0.0676 & 0.5421 & 0.4752 & 0.1279 & 0.4968 & 0.4299 & 0.1889 \\
        w/o RANSAC    & 0.5945 & 0.5238 & 0.1276 & 0.7149 & 0.6701 & 0.0705 & 0.5571 & 0.4761 & 0.1239 & 0.4901 & 0.3996 & 0.1883 \\
        w/o SUB       & 0.6292 & 0.5664 & 0.1221 & 0.7699 & 0.7269 & 0.0595 & 0.5853 & 0.5122 & 0.1199 & 0.5075 & 0.4119 & 0.1871 \\  
        \hline    
    \end{tabular}
    }
    \caption{Ablation studies. $\uparrow$($\downarrow$) means the performance is better with larger(smaller) values. 
    As for the optimization terms: \textbf{ICP} for ICP Term; \textbf{S} for Shape Term, \textbf{S(1)} means only using the first frame point cloud, and \textbf{S(all)} means using the updated shape; 
    \textbf{MC} for Motion Consistency Term, and \textbf{MP} for Motion Prior Term. 
    As for the implementation details:
    \textbf{GM} for ground removal, \textbf{RANSAC} for using RANSAC, \textbf{SUB} for subshape.}
    \vspace{-15px}
    \label{tab::ablation}
\end{table*}

\begin{figure}
	\centering
	\includegraphics[width=1.0\columnwidth]{figures/vehicle_visualization.pdf}
	\caption{Visualization SOT results.
		\textbf{1}. The evolution of shapes during the tracking process.
		\textbf{2}. Comparison between the results from SOT and ground truth.  
		\textbf{3}. Visualization of failure cases.}
	\label{fig::visualizations}
	\vspace{-15px}
\end{figure}

\subsection{Quantitative Results}
\label{subsec::quantitative}

Table~\ref{tab::ablation} evaluates the contributions of each individual factor.
We start from the baseline of using ICP Term (ICP) and Motion Prior Term (MP), which represents the basic combination of point cloud registration and motion model.
Then the experiments expand to considering the variants of Shape Term (S) and Motion Consistency Term (MC).
When ablating the optimization factors from Sec.~\ref{subsec::factor}, we adopt all the design details from Sec.~\ref{subsec::design}, as they are all beneficial to the performance.  

Compared to the baseline of using ICP and motion model only (ICP+MP), adding the Shape-Term (S(1), using the first frame point cloud as shape in the Shape Term) significantly improves the performance.
During this process, aggregating shapes along the process of tracking (S(All), using aggregated shape in the Shape Term) assist the state estimation even further.
On top of the point cloud related factors, the regularization on motion consistency (MC) is helpful especially on medium and hard cases.
As for the design choices in Sec.~\ref{subsec::design}, removing any of them greatly affects both the accuracy and robustness, 
where Ground Removal and RANSAC are the most critical ones.

\subsection{Analyses}

\subsubsection{\textbf{Vehicle Shape Visualization}}

Following the approach in~\cite{Held-Combining,IROS-Continuous-time}, we visualize the aggregated point clouds in Fig.~\ref{fig::visualizations} to indicate the quality of tracking. 
Moreover, the vehicle banks constructed during this process are also beneficial to a series of tasks discussed in Sec.~\ref{sec::downstream}. 
In Fig.~\ref{fig::visualizations} and Fig.~\ref{fig::cover}, 
the evolution of point clouds and the quality of shapes prove that our algorithm can produce high quality state information and 3D shapes.
This further poses the necessity and effectiveness of using LiDAR sequences for creating 3D shapes.

\subsubsection{\textbf{Shape Completion $\not=$ State Estimation}}
\label{subsubsec::shape_not_track}

Although in Table.~\ref{tab::ablation}, better shapes generally come along with better state estimation performance,
we argue that these two are not equivalent tasks.
The LiDAR points are aggregated until the last frame in the experiments of Table~\ref{tab::ablation},
but this is not the optimal approach and leads to worse shapes on the failures of state estimation, just as the last row in Fig.~\ref{fig::visualizations}.
Therefore, ideal shape completion need to reason whether and how to aggregate the point clouds, thus differs from mere state estimation.
We will investigate this direction in future study.

\subsection{Challenging Scenarios}
\subsubsection{Point Cloud Sparsity} 
In Table~\ref{tab::ablation}, the performance gaps among the three difficulty levels reflect the challenge of sparse point clouds.
The hard set has less LiDAR points, therefore causes inaccurate registration at the beginning and leads to lower performance.
In long-term tracking, the widely existing cases of occlusion or distant objects indicate the needs for handling this challenge. 

\subsubsection{Drifting}
Accumulated error is well known in SLAM and commonly exists in odometry based approaches. 
The errors can accumulate and cause drifting over time if without absolute observation or extra constraints like loop closure. 
We alleviate this by adding the shape terms, which treats the shape as ``Map'' and enforce its consistency with the observed point cloud. 
Although the terms are effective in many cases as in Table~\ref{tab::ablation}, 
drifting still exists as in Fig.~\ref{fig::cover} and requires future research on it.

\subsubsection{Abrupt Motion Change}
Accurate motion initialization is crucial since it provides the initial point set correspondence.
Although current motion models are able to adapt to most cases,
they still struggle where the objects change the motions greatly in short periods,
as the wrongly associated point clouds can quickly lead to failures.
We will explore more advanced motion prediction methods, and close the loop of state estimation and motion prediction in the future.

\section{Experiments on Downstream Applications}
\label{sec::downstream}

\subsection{Motion Ground Truth Generation}
\label{subsec::motion_generation}

SOTracker\ can estimate the motion of a designated vehicle in wild point cloud sequences.
Therefore, it is capable of providing motion data for prediction and planning systems at a low cost.
As in Fig.~\ref{fig::motion_generation}, the deviation of generated motion is less than 10cm within 30 frames, which is even smaller than the size of a finger.
With Argoverse~\cite{argoverse} providing 30 data points for this task, we believe that our system meets the standard.

\begin{figure}
    \centering
    \includegraphics[width=1.0\columnwidth]{figures/motion_generation.pdf}
    \caption{Motion generation results.
    The average distance (meters) between the motion from ground-truth and our algorithm at frame k.
    }
    \label{fig::motion_generation}
    \vspace{-15px}
\end{figure}

\subsection{Optical Flow Annotation}

With the estimated shapes $M$ and states $S_{k:k+1}$, we can annotate optical flows for the tracked objects in corresponding images $I_{k:k+1}$.
For each point $m\in M$, we compute its location $(qx_{k}, qy_{k})$ on $I_{k}$ by moving the shape according to $S_k$, then project it onto the image plane.
In the same way, we compute $(qx_{k+1}, qy_{k+1})$ on image $I_{k+1}$, then the optical flow for pixel $(qx_{k}, qy_{k})$ is $(qx_{k+1}-qx_k, qy_{k+1}-qy_k)$. 
In Fig.~\ref{fig::cover}, our annotated optical flow can act as high quality pseudo-ground-truth.
Such an effect is unreachable for the single frame point cloud (the left image), as it contains neither enough points nor the motion information for annotating the optical flow.
In KITTI~\cite{KITTI-Flow} researchers requires all three of LiDAR data, CAD models and ground-truth 3D bounding boxes to infer the optical flows of vehicles,
while our algorithm only requires an initial bounding box and raw LiDAR scenes.
Therefore, our method can reduce the cost of optical flow annotation and scale it up.
\vspace{-3px}
\subsection{Simulating LiDAR Scans Using Vehicle Bank}

\begin{figure}
	\centering
	\includegraphics[width=1.0\columnwidth]{figures/lidar_sim.pdf}
    \caption{
    LiDAR scans created using our completed shapes and physical simulation. 
    The visualization includes two types of vehicles: car and truck.
    The LiDAR simulation fits different angles and different distances (20m and 40m).
    }
    \vspace{-20px}
	\label{fig::lidar_sim}
\end{figure}

In LiDARsim~\cite{LiDARsim} researchers propose to simulate single LiDAR scans for autonomous driving by creating object shapes with manually labeled 3D locations.
As mentioned in LiDARsim~\cite{LiDARsim}, augmenting the training data with the simulation can improve the performance of perception systems.
In Fig.~\ref{fig::lidar_sim} we visualize the results of our LiDAR scan simulation on different types of vehicles, angles, and distances.
For both types of vehicles, we can build up high quality shapes and simulated scans, which proves the effectiveness of our model-free SOT approach. 
Moreover, unlike LiDARsim~\cite{LiDARsim}, which requires exhausted annotation of objects, we only require an initial bounding box and raw point clouds.
Thus, we make the process cheaper and more scalable, and produce unlimited LiDAR data with minimum human annotation.
\vspace{-3px}



\section{Conclusion}

In this paper, we calls for the attention on a new setting of object tracking in point cloud sequences: 
model-free single-object tracking.
This new setting targets on the setbacks of model-based detection and MOT methods, and can benefit a series of applications.
To facilitate the research, we construct the LiDAR-SOT\ from WOD and propose a strong algorithm called SOTracker.
According to the qualitative and quantitative results, 
our method not only achieves strong results on state estimation,
but also generalizes its estimated states and 3D shapes to the applications of simulating LiDAR scans, optical flow annotation, and motion data generation.


\section*{Acknowledgement}

{\small
\bibliographystyle{ieee}
