\section{Introduction}
	\label{sec:introduction}


Robust environmental perception is a fundamental aspect of autonomous driving that is important for road safety and efficiency and contributes to technical problems such as path planning, control, and localization.
Highly dynamic driving environments can pose critical safety challenges for self-driving vehicles. Objects (stationary or moving), as well as road construction, can change the geometry of the road and result in degradation to localization and planning. Curbs define the road boundary and provide useful information for vehicle navigation; as a result, accurately detecting and tracking them is important. 

Over the past few years, there have been numerous methods proposed to detect and extract curb features using either a single sensor or a combination of sensor modalities. Most curb detection systems use lidar and cameras \cite{BarHillel2014}. 
Lidar sensors have been frequently used to detect curb features as the curbs are inherently geometric features
\cite{Wijesoma_2004, Wang2019, fernandez2014road, chen2015velodyne}.
Vision based processing techniques have also been proposed including
\cite{oniga2010polynomial, siegemund2010curb, kellner2014road}.
Because lidar and vision have different failure modes, sensor fusion has become popular in recent years which exploits the best properties of both the sensors -- camera images for semantics and lidar for depth information\cite{goga2018fusing, deac2019fusion, baek2020curbscan}.


	\begin{figure} [bth!]
		\vspace{-3mm}
		\centering
		\includegraphics[width=1.\columnwidth, trim =-140 0 -60 10,clip]{figures/images/map.png}
		\includegraphics[width=1.\columnwidth, trim =5 0 4 0,clip]{figures/images/curb_extraction.png}
		\vspace{-5mm}
		\caption{\textit{Top:} The route of the data collection vehicle. \textit{Bottom:} Detected curb features using our proposed method (blue points) and the ground truth curb features (green points) from a commercial map supplier.}
		\label{fig:curb_traj}
		\vspace{-0.5cm}
	\end{figure}
	





	\subsection{Motivation}

	Temporary change to an environment may occur due to various factors. Hence, it is important to incorporate those updates in the mapping module so that the planner and control modules may react accordingly. Consequently, we need an in-house methodology from which we can extract the necessary features of an environment into our mapping module. In addition, we would have the ability to create curb maps in restricted areas to which commercial map suppliers do not have access.  
		
	
	\subsection{Contribution}
	\label{sec:contribution}
	Our work falls under the category of association of camera semantics with lidar depth. We show the results of our proposed method in \autoref{fig:curb_traj} and to achieve that our specific contributions are:
	\begin{itemize}
	
		\item Detection and association of multiple curbs with unsupervised DBSCAN clustering.    
		\item Outlier removal using Delaunay-based filtering method which needs less parametric tuning than RANSAC-based polynomial fitting regression constraints.
	\end{itemize}





	\section{Related Work}
	\label{sec:related-work}
	
	Aspects of curb detection have been studied extensively in the context of autonomous driving. Multiple solutions have been proposed that use camera, lidar, or fusion of both. 

	
	In the early 2000s, traditional computer vision methods were proposed for curb detection. In \cite{aufrere2003multiple}, the authors proposed a histogram filter with a threshold to locate curb points. Whereas, in \cite{Wijesoma_2004}, a Kalman filter based tracking of the threshold-based detected curb points was proposed. By the mid-2000s, Hough transform based method \cite{kim2007outdoor} on 2D lidar data was explored under the assumption that the terrain was flat. Polyfit on a digital elevation map (DEM) created from the stereo vision was explored in  \cite{oniga2010polynomial, siegemund2010curb}, while the authors in \cite{stuckler2008lane} used height images from lidars to create DEM and applied polynomial fitting. In \cite{kellner2014road} the authors proposed to create a DEM from ego-motion and filtering for curb points. As 3D lidar was adopted by researchers after the mid-2000s, a lot of the work for curb detection was based on ground plane segmentation on 3D data \cite{el2011detection, fernandez2014road, chen2015velodyne}.
	
	In recent years, with better network architectures for semantic segmentation, works began to fuse camera semantics with lidar depth \cite{goga2018fusing, deac2019fusion}. Instead of relying on geometric attributes of curbs, the semantic information identified specific curb pixels. Then the depth was obtained from the lidar projected point clouds in the image plane. A recent work, CurbScan \cite{baek2020curbscan}, proposed to fuse an additional ultrasonic sensor for lateral
	distance information with curb tracking using a Kalman filter.
	
	In contrast to prior works, we propose an agglomerative unsupervised clustering to detect multiple curbs in unfiltered point clouds from re-projected camera semantics. We also apply a Delaunay-based filter to remove outliers among the detected curb points. Finally, we transform all the detected curbs into the global frame using GNSS (Global Navigation Satellite System) and check their consistency quantitatively with respect to the ground truth (GT) curb points obtained from a prior hand annotated HD curb map.






	

	

	


	

	

	

	
	\section{Methodology}
	\label{sec:methodology}
	
	Prior calibration of the sensors is a fundamental prerequisite for sensor fusion. Additionally, an important feature of our fusion technique for feature association is that the lidar point clouds are motion corrected and transformed so as to be equivalent to that recorded at the time stamp of an available camera frame using the approach introduced in \cite{wisth2021unified}. This helps in data from all the sensors need to be properly time synchronized. In the following sub-sections we outline the procedure we adapted for our approach. 

	\subsection{Sensor setup and reference frames}
	\begin{figure}
		\centering
		\vspace{2mm}
		
		\includegraphics[width=0.75\columnwidth]{figures/images/scania-bus-coverage.pdf}
	
		\caption{Illustration of the FoV of two front lidars and four cameras positioned around the data collection vehicle. However, only the two front facing cameras were used for our experiment. The vehicle base frame $\mathtt{{B}}$ is located at the center of the rear axle. The sensor frames of the lidars are $\mathtt{L}_{L}$ and $\mathtt{L}_{R}$, representing front-left and front-right lidars respectively. The IMU frame of the GNSS is represented as $\mathtt{I}$. }
		\label{fig:scania-bevda}
		\vspace{-0.4cm}
	\end{figure}
	
	The data collection vehicle consisted of two lidars and two cameras. The reference frames and the field of view (FoV) of the sensors are shown in \autoref{fig:scania-bevda}. The vehicle base frame $\mathtt{{B}}$ is located on the center of the rear-axle of the vehicle. Sensor readings from lidars and cameras are represented in base frame $\mathtt{{B}}$, as $\tensor[_\mathtt{{B}}]{\mathtt{L}}{_{k}}$ and $\tensor[_\mathtt{{B}}]{\mathtt{C}}{_{k}}$ respectively, where $k$ represents the location of the sensor in the vehicle. For example, $\mathtt{L}_{L}$ and $\mathtt{L}_{R}$ represent the front left and front right lidar frames. The pose information from the GNSS is reported in the UTM frame, $\mathtt{W}$ (World frame). The IMU frame, $\mathtt{I}$, of the GNSS is shown in \autoref{fig:scania-bevda}. All the inter-sensor transformations are carried out using prior calibration parameters.  


	\subsection{Semantic association with lidar points}
	\label{sebsection:semantic-association}
	
	The semantic features from lidar were extracted by associating the projected point clouds with the segmented camera images. The image segmentation was performed using a trained Efficient-Net model.

	
	\subsubsection{Efficient-Net for semantic segmentation}
	
	Since AlexNet \cite{alexnet_2012} won the 2012 ImageNet competition, Convolutional Neural Networks (ConvNets) have been increasingly used as the de facto standard for image segmentation tasks. Although higher accuracy is critical for autonomous driving applications, we have already hit the hardware memory limit for the image segmentation tasks using ConvNets. Thus further accuracy gains will require better efficiency. 
	
	The authors of EfficientNet \cite{tan2019efficientnet} illustrated that model scaling can be achieved by carefully balancing network depth, width, and resolution, leading to a better performance with the fixed amount of computation resources. Based on this study, we used a modified architecture of EfficientNet for semantic segmentation. The network architecture backbone is shown in \autoref{tab:efficientNet}, where each row describes a stage $i$ with $\hat{L}_{i}$ layers, with input resolution $\langle\hat{H}_{i}, \hat{W}_{i}\rangle $ and output channels $\hat{C}_{i}$. MBConv layers represent mobile inverted bottlenecks from the MobileNetV2 architecture \cite{sandler2018mobilenetv2}, where squeeze-and-excitation optimization \cite{hu2018squeeze} has also been added on top of it. To upsample the network output to its original input resolution a bilinear interpolation was used in the decoder architecture.

	\begin{table}[!h]
		\vspace{0.3cm}
		\normalsize
		\centering
		\resizebox{\columnwidth}{!}{
			\begin{tabular}{c|cccc} \toprule
				Stage & Operator & Resolution & \#Channels & \#Layers \\
				\( i \) & \( \hat{\mathcal{F}}_{i} \) & \( \hat{H}_{i} \times \hat{W}_{i} \) & \( \hat{C}_{i} \) & \( \hat{L}_{i} \) \\
				\midrule
				1 & Conv3x3 & \( 640 \times 1024 \) & 32 & 1 \\
				2 & MBConv1, k3x3 & \( 320 \times 512 \) & 16 & 1 \\
				3 & MBConv6, k3x3 & \( 160 \times 256 \) & 24 & 2 \\
				4 & MBConv6, k5x5 & \( 80 \times 128 \) & 40 & 2 \\
				5 & MBConv6, k3x3 & \( 40 \times 64 \) & 80 & 3 \\
				6 & MBConv6, k5x5 & \( 40 \times 64 \) & 112 & 3 \\
				7 & MBConv6, k5x5 & \( 20 \times 32 \) & 192 & 4 \\
				8 & MBConv6, k3x3 & \( 20 \times 32 \) & 320 & 1 \\
				9 & Segmentation Head & \( 640 \times 1024 \) & 24 & - \\
				\bottomrule
		\end{tabular}}
		\caption{ EfficientNet-B0 baseline network architecture.}
		\label{tab:efficientNet}
		\vspace{-0.5cm}
	\end{table}
	

	To achieve robustness we trained the EfficientNet model on a diversified set of scenarios (city driving, snowy conditions, dessert area, suburban area). The overall training accuracy is 79.10\% and the mean IoU (Intersection over Union) is 0.495. For the curb class the accuracy is 67.20\% and the IoU is 0.557. The results of the segmentation on a sample frame can be visualized in \autoref{fig:image_semantics}. 
	

	
	\subsubsection{Association of curb semantics with lidar depth}
	The cameras mounted on our platform have fisheye lenses. Hence, we extracted the curb points by doing a fish-eye projection \cite{kannala2006generic} of the fused lidar points in image space and chose points closer to the curb pixels within a bound of $\pm$3 pixels. Finally, the curb points are re-projected back to the base frame $\mathtt{{B}}$. The results of curb extraction from semantic association can be visualized in \autoref{fig:lidar_reprojection}.
	
	\begin{figure}[!h]
		\centering
		\vspace{-0.2cm}
		\begin{multicols}{2}
			\includegraphics[width=1\columnwidth, trim = 0 100 0 0,clip]{figures/images/curb_semantics.png}
			\subcaption{Semantic segmentation results using our modified EfficientNet \cite{tan2019efficientnet}.}
			\label{fig:image_semantics}
			\includegraphics[width=1\columnwidth, trim = 0 0 0 50,clip]{figures/images/fused_lidar.png}
			\subcaption{Fused lidar point clouds from lidar sensors.}
			\label{fig:lidar_fused}
			\includegraphics[width=1\columnwidth, trim = 0 100 0 0,clip]{figures/images/lidar_projection.png}
			\subcaption{Lidar point clouds (white points) overlaid on the segmented curb pixels.}
			\label{fig:lidar_projection}
			\includegraphics[width=1\columnwidth, trim = 0 0 0 50,clip]{figures/images/lidar_reprojection.png}
			\subcaption{Curb semantics (blue points) with the fused point cloud.}
			\label{fig:lidar_reprojection}
		\end{multicols}
		\vspace{-0.4cm}
		\caption{Semantic association with lidar point cloud.}
		\label{fig:lidar-semantic-matching}
		\vspace{-0.5cm}
	\end{figure}
	
	\subsection{Unsupervised clustering and filtering}
	\label{sebsection:clustering}
	The point cloud association technique may give us noisy points due to various reasons such as synchronization quality of the logs, calibration parameters, or camera projection model. To remove outliers we can apply filtering based on the geometric structures of the curbs. However, since we do not know the pre-defined number of curbs in advance, it makes it difficult to apply a polynomial fitting on the extracted curb points. To overcome this problem we first find a set of unsupervised clusters and associate newly detected curb points to the relevant clusters based on spatial density.
	 
	\subsubsection{Iterative cluster association}
	We iteratively chose the extracted curb point clouds and applied unsupervised spatial clustering. The boundary points of a cluster are the points which are furthest from the cluster centroids. 
	If the  $L_2$-norm of the boundary points in the new clusters were less than a pre-defined threshold from the old cluster boundary points we merged the clusters. This operation helped in identifying the number of curb segments. The result of our clustering with DBSCAN \cite{ester1996density} is shown in \autoref{fig:curb-extraction}. 
	 
		\begin{figure}[hbt!]
		\centering
		\vspace{-2mm}
	
		\includegraphics[width=1\columnwidth]{figures/images/curbs_clustering.png} 
	
		\caption{Iterative feature point clustering using DBSCAN \cite{ester1996density}. Random colors indicate different clusters detected.}
	
		\label{fig:curb-extraction}
	\end{figure}

	\subsubsection{Delaunay filtering}
	Let ${\cal S}$ be a set of points in $\mathbb{R}^n$ with distance Euclidean function $d$. The Voronoi diagram \cite{brandt1992continuous} is the partition of the space into Voronoi regions, $\displaystyle R$, such that 
	\begin{equation}
		{\displaystyle R_{k}=\{s\in {\cal S}\mid d(s,R_{k})\leq d(s,R_{j})\;\forall\;j\neq k\}}.
	\end{equation} 
	Voronoi graphs have been used in motion planning algorithms for obstacle avoidance \cite{vornoi_mp_1989, garber2004constraint}. But in the context of filtering of curb points, this is a novel approach. Delaunay triangulation is the dual of the Voronoi diagram. Delaunay triangulation \cite{fortune1995voronoi} on ${\cal S}$, DT(${\cal S}$) is a triangulation such that no point in ${\cal S}$ is inside the circum-hypersphere of any $n$-simplex in DT(${\cal S}$).
	For $\mathbb{R}^3$ it is called Delaunay tetrahedralization. The Voronoi vertex corresponding to a Delaunay tetrahedron is the center of the circumscribing sphere of the tetrahedron. Let $[x \quad y \quad z]$ be the center and the four points of the tetrahedron be $[x_i \quad y_i \quad z_i], \forall i=1(1)4$. Then the center is found by solving the following equation:
	\begin{equation}
	\begin{aligned}
		&(x-x_1)^2 + (y-y_1)^2 + (z-z_1)^2 = \\ &(x-x_j)^2 + (y-y_j)^2 + (z-z_j)^2  , \forall j=2,3,4\\
		\implies &2(x_j-x_1)x + 2(y_j-y_1)y + 2(z_j-z_1)y = \\&(x_j^2 + y_j^2 + z_j^2) - (x_1^2 + y_1^2 + z_1^2) , \forall j=2,3,4
		\label{eq:dt}
	\end{aligned}
	\end{equation}
	

	
	Voronoi sub-graph of the Delaunay tetrahedra is computed by filtering large radii of circumscribing spheres from the centers computed. This removes tetrahedra outside the point volume and removes the outliers. The shortest Euclidean path in the Voronoi sub-graph connecting the start and the end points give us the medial axis. The points closer to the medial axis gives us the filtered point cloud corresponding to the curbs. An illustration of the process is shown in \autoref{fig:delaunay_filtering}.

	
	\begin{figure}[h!]
		\centering
		\vspace{0.14cm}
		\begin{multicols}{2}
			\includegraphics[width=1.0\linewidth]{figures/images/curbs_raw.png}
			\subcaption{Unfiltered curb point cloud}
			\label{fig:curb_raw}
			\includegraphics[width=1.0\linewidth]{figures/images/curbs_vornoi.png}
			\subcaption{Voronoi regions}
			\label{fig:curb_vornoi}
			\includegraphics[width=1.0\linewidth]{figures/images/curbs_delaunay.png}
			\subcaption{Delaunay tetrahedron}
			\label{fig:curb_delaunay}
			\includegraphics[width=1.0\linewidth]{figures/images/curbs_medial.png}
			\subcaption{Medial axis}
			\label{fig:curb_delaunay_original_data}
		\end{multicols}
		\vspace{-0.6cm}
		\caption{Curb inlier selection with Delaunay filtering.}
		\label{fig:delaunay_filtering}
		\vspace{-0.56cm}
	\end{figure}

	\subsubsection{RANSAC filtering}
	RANSAC \cite{ransac_1981} is an iterative algorithm for the robust estimation of parameters from a subset of inliers from the complete data set. For fitting the curb points to an estimator we used a third-order polynomial. However, an automatic parameter tuning to estimate the degree of the polynomial was performed for each curb segment to find out the best set of inliers.
	
	




	
	
	\section{Experimental Results}
	
	\label{sec:results}
	
	\subsection{Dataset}
	
	We collected the data from a Scania autonomous bus platform mounted with two lidars and two front facing cameras for a route length of $\approx$1.5 Kms. The ground truth (GT) curb features are provided by a map supplier. All the sensing data was synchronized using PTP (Precision Time Protocol) synchronization and converted to rosbags for evaluation. We evaluated the generated curb points by manually selecting the corresponding GT curb points. The point cloud selection tool for association was developed using open3d \cite{zhou2018open3d}. Since manual association is a tedious process we also propose a mechanism for automatic evaluation of the clusters. The clustering algorithms are evaluated offline from scikit-learn \cite{scikit-learn} package.
	

	\subsection{Manual segment-wise association and evaluation}


	We associate the GT points from the map supplier segment-wise. For evaluation, we fit a polynomial to the GT points. Then we sample points from the polynomial and associate them to the Delaunay filtered points and RANSAC filtered points as shown in \autoref{fig:curb_fitting}. We compute the normalized $L_2$-norm (based on the number of points selected) for evaluation metrics. 

	\begin{figure}[hbt!]
		\centering
	
		\includegraphics[width=1\columnwidth, trim = 10 0 0 0,clip]{figures/images/curbs_fitting.png}
		\caption{The curb points are extracted by applying RANSAC (blue points) and Delaunay filtering (red points). A polynomial fitting is performed (black line) to the GT points (green) after which the $L_2$-norm between the GT polyline and the filtered points are calculated.}
		\label{fig:curb_fitting}
		\vspace{-0.5cm}
	\end{figure}

	\subsection{Automatic segment-wise association and evaluation}
	To automatically evaluate the generated curb points we measure the Chamfer distance (CD) \cite{barrow1977parametric} of each cluster segment. The CD between two set of point clouds $P_{1}, P_{2} \subseteq \mathbb{R}^{3}$ is defined as:
	\begin{equation}
	\begin{aligned}
	& \text{CD}\left(P_{1}, P_{2}\right) = \\ &\frac{1}{|P_1|}\sum_{a \in P_{1}} \min _{b \in P_{2}}\|a-b\|_{2}^{2} + \frac{1}{|P_2|}\sum_{b \in P_{2}} \min _{a \in P_{1}}\|b-a\|_{2}^{2}
	\end{aligned}
	\end{equation}
	We also evaluate the effect of different unsupervised clustering algorithms including Agglomerative Clustering \cite{beeferman2000agglomerative}, BIRCH \cite{zhang1996birch}, DBSCAN \cite{ester1996density} and OPTICS \cite{ankerst1999optics} for our evaluation metrics. The CD and the number of detected filtered curb points for our evaluation are reported in \autoref{tab:analysis}. 
	

	We observe that Delaunay filtering identifies inliers that more closely correspond to the GT points than a generic RANSAC-based filtering both for manual and automatic segment wise association as the $L_2$-norm and CD are lower for Delaunay filtering compared to RANSAC. We also observe that the Delaunay filtering selects lesser inlier points compared to RANSAC. However, the inliers correspond to the GT points more closely as shown in \autoref{fig:curb_fitting}. We conclude that density based unsupervised algorithm for segment clustering works best for fitting an arbitrary number of curbs based on the computed CD. The result of our proposed method showing the final selected curb points is shown in \autoref{fig:curb_traj}.



	

	\begin{table}[!h]
	\vspace{-0.1cm}
	\centering
	\resizebox{\columnwidth}{!}{
	\begin{tabular}{l|cc}  \toprule
	\multicolumn{3}{c}{\textbf{Manual segment-wise association}}
	\\
	\midrule
	\textbf{No Clustering} & \textbf{Normalized $L_2$-Norm} & 
	\textbf{\# Detected Points}\\
	\midrule
	RANSAC Filtering & 27.659 & 9578\\
	Delaunay Filtering & \textbf{19.947} & 6904\\
	\midrule
	\multicolumn{3}{c}{\textbf{Automatic segment-wise association}} \\
	\midrule
	\textbf{Outlier Removal (RANSAC)} & \textbf{Chamfer Distance}& \textbf{\# Detected Points}\\
	\midrule

	Agglomerative Clustering & 17.427 & 3489 \\
	BIRCH & 19.596 & 1351\\
	DBSCAN & \textbf{17.220} & 5314\\
	OPTICS & 18.370 & 7446\\
	\midrule
	\textbf{Outlier Removal (Delaunay)} & \textbf{Chamfer Distance}& \textbf{\# Detected Points}\\
	\midrule
	Agglomerative Clustering & 15.418 & 3924 \\
	BIRCH & 16.165 & 3492\\
	DBSCAN & \textbf{14.753} & 6678\\
	OPTICS & 15.870 & 4415\\
	\bottomrule
	\end{tabular}}
	\caption{Evaluation Metrics}
	\label{tab:analysis}
	\vspace{-6mm}
	\end{table}
	\begin{comment}
	\begin{figure} [!htbp]
	\centering
	\includegraphics[width=0.49\columnwidth]{figures/images/raw-front.png}
	\includegraphics[width=0.49\columnwidth]{figures/images/seg-front.png} \\
	\vspace{0.013\columnwidth}
	\includegraphics[width=0.49\columnwidth]{figures/images/raw-back.png}
	\includegraphics[width=0.49\columnwidth]{figures/images/seg-back.png}
	\caption{Semantic segmentation results using modified EfficientNet-B0 network architecture as shown in Table \ref{tab:efficientNet}. \textit{Top:} Front camera raw image and its corresponding segmentation. \textit{Bottom:} Results from rear camera.}
	\label{fig:semseg-results}
	\vspace{-3mm}
	\end{figure}
	\end{comment}
	


	
	\section{Conclusion}
	\label{sec:conclusion}

	We proposed a multi-modal curb detection and mapping algorithm with a novel filtering approach using 3D-Delaunay tetrahedra. We demonstrated the detection of arbitrary number of curbs with our clustering approach. Our evaluation indicates that Delaunay filtering outperforms traditional RANSAC based filtering approach for curb outlier removal.

   
	To extend this work further, we would study the instability of the medial axis generation under different noise conditions. We would also like to extend the semantic association to other infrastructure features like road lines, traffic lights, pedestrian paths. We also strive to benchmark our solution on open source datasets by retraining our semantic segmentation model on un-distorted images. 

\begin{comment}	
	\begin{table}[!htbp]
	\normalsize
	\centering
	\resizebox{\columnwidth}{!}{
	\begin{tabular}{l|cccccc} \toprule
	\multirow{2}{*}{\makecell[l]{Class}} & \multicolumn{2}{c}{\makecell[c]{Training}} & \multicolumn{2}{c}{\makecell[c]{Validation}} & \multicolumn{2}{c}{\makecell[c]{Testing}} \\
	\cline{2-7}
	& \textbf{Accuracy (\%)} & \textbf{Mean IoU} & \textbf{Accuracy (\%)} & \textbf{Mean IoU} & \textbf{Accuracy (\%)} & \textbf{Mean IoU}\\
	\midrule
	Drivable Road & 00.00 & 00.00 & 00.00 & 00.00 & 00.00 & 00.00\\
	Side Walk & 00.00 & 00.00 & 00.00 & 00.00 & 00.00 & 00.00\\
	Curbs & 00.00 & 00.00 & 00.00 & 00.00 & 00.00 & 00.00\\
	Vehicles & 00.00 & 00.00 & 00.00 & 00.00 & 00.00 & 00.00\\
	Buildings & 00.00 & 00.00 & 00.00 & 00.00 & 00.00 & 00.00\\
	Vegetation & 00.00 & 00.00 & 00.00 & 00.00 & 00.00 & 00.00\\
	\bottomrule
	\end{tabular}}
	\caption{Modified EfficientNet-B0 baseline network segmentation results.}
	\label{tab:efficientNet-results}
	\vspace{-6mm}
	\end{table}
\end{comment}		
	
	\begin{comment}
	We have presented a novel factor graph formulation for state estimation that
	tightly fuses camera, lidar, and IMU measurements. This fusion enables for
	graceful handling of degenerate modes -- blending between lidar-only feature
	tracking and visual tracking (with lidar depth), depending on the constraints
	which each modality can provide in a particular environment. We have
	demonstrated comparable performance to state-of-the-art lidar odometry systems
	in typical conditions and better performance in extreme conditions, such as
	aggressive motions or abrupt light changes.
	Our approach also presents a novel method of jointly optimizing lidar and visual
	features in the same factor graph. This allows for robust estimation in
	difficult environments such as long corridors, open fields, and dark
	environments.
	\end{comment}
	


	


	
	






	


	\bibliographystyle{./IEEEtran}
	
