\section{INTRODUCTION}
\label{sec:introduction}
Simultaneous Localization and  Mapping (SLAM) are research hotspots in the field of robots, autonomous driving, augmented reality, etc. Camera and inertial measurement units (IMU) are low-cost and effective sensors. The Visual Inertial Odometry (VIO) combines the complementarity of the two sensors to improve the accuracy and robustness of the pose estimation. Existing VIO methods \cite{VINS, ORBSLAM3, mourikis2007multi} generally use the feature-based (indirect) method as visual measurements. However, in the weak texture environment, the number of effective point features extracted by the indirect method is small, leading to the failure of pose estimation. On the other hand, the direct method \cite{engel2017direct, von2018direct} can utilize all available pixels of images to generate a more complete model, which is more robust. 

Since the VIO can build the map of the surrounding environment in real-time, by using the geometric constraint information of the map, we can improve the positioning accuracy and the quality of the map.
The widely existing edges in the environment are the most common features which add different types of visual measurement to the estimator \cite{PL-VIO,pumarola2017pl,structslam, li2018direct}. Meanwhile, the planar features in the environment can also be used to increase the robustness of the estimator. However, the reconstructed map with the indirect method is too sparse, which makes it difficult to extract planar regularities, and the planar features will increase the computational burden that affects the real-time performance of the system.

For the VIO based on the direct method, the visual module tracks the pixels with large enough intensity gradients, sufficient visual observation makes the reconstructed map denser. Meanwhile, with the aid of the IMU information,  it is easier to extract the plane information in the map, as is shown in Fig.\ref{fig:cover_grap}. Furthermore, introducing the geometric information that is less sensitive to the luminance changes into the direct method makes the system more robust.  
\begin{figure}[t]
	\centering
	\includegraphics[width=1.0\linewidth]{image/cover_graph.pdf}
	\caption{The proposed direct sparse visual-inertial odometry builds a sparse map running on the V11 sequence of the  EuRoC dataset. (a) is the reconstruction map of the whole scenes, (b) is the coplanar points on different planes, the different colors are used to distinguish the planes. (c) is the  2D Delaunay triangulation, depth map, and reconstructed 3D mesh of the corner in the scene. }
	\label{fig:cover_grap}
	\vspace{-6mm}
\end{figure}

This paper proposes a direct sparse visual inertial odometry that leverages planar regularities called PVI-DSO. The method is an extension of DSO, we complement the IMU measurement constraints and coplanar constraints. Unlike the method of \cite{von2018direct}, we use a novel way to fuse the IMU measurement information, which doesn't estimate the transformation between the DSO frame and the metric frame. Meanwhile, we extract the coplanar regularities from the generated 3D mesh, and inspired by \cite{li2020co}, in the photometric error function, we adopt the plane parametric representation whose performance has been proved in \cite{li2020co}. Through the above methods, adding plane features in the system doesn't increase much computational burden. In summary, the main contribution of this work include: 

\begin{itemize}
\item To our best knowledge, PVI-DSO is the first direct sparse visual-inertial odometry that fuses coplanar constraint regularities to improve the accuracy of localization. 

\item { We introduce a coplanar point parameterization in the direct method that constructs photometric error constraints, which is an extension of \cite{li2020co}. The plane-distance cost used in this paper converts the coplanar constraints into the plane prior, which can enforce the coplanar constraints in the optimization without increasing extra computation cost.}

\item We design extensive experiments on the challenging EuRoC and TUMVI datasets. The experimental results show that the proposed PVI-DSO fusing IMU constraints and coplanar regularities into the direct method outperforms the state-of-the-art.
\end{itemize}
\section{RELATED WORK}\label{sec:related work}

{The most common features used in the SLAM / VIO algorithms are the points \cite{mourikis2007multi, VINS, ORBSLAM3},  \cite{engel2017direct}, \cite{forster2014svo}, as a complement to the point features, the geometric information (line, plane) introduced in the system based on points can improve the accuracy of the pose estimation and mapping, which have received extensive attention in recent years.}

\textbf{Indirect method with geometric Regularities} \ Since the line features exist widely in the environment, it seems natural to fuse the line features into the framework based on points \cite{PL-VIO,pumarola2017pl, Tsai2018}. Furthermore, in the structural scenario, the lines with three perpendicular directions of the Manhattan world model can encode the global orientations of the local environments, which are leveraged to improve the robustness and accuracy of pose estimation \cite{Zou2019, structslam, xu2021leveraging}. For the planar information, the main difficulty is how to accurately extract the planar regularities in the environment. \cite{salas2014dense,zhang2019point,ma2016cpa} extract plane features with the assistance of depth map obtained by the RGBD camera. Lu \textit{et al.} proposed \cite{lu2015visual} uses the RANSAC method to fit the plane among the estimated 3D points with RGB camera. Nevertheless, the RANSAC-based method can only be used when there is only one potential large plane in the environment and consumes a lot of time. More Recently, \cite{rosinol2019incremental, li2020leveraging} get the planar regularities from the 3D mesh generated by the VIO. However, Sparse point clouds generated by indirect-based vision algorithms make the location of the 3D mesh inaccurate.


\textbf{Direct method with geometric Regularities} \ Direct methods minimize photometric errors to estimate pose and reconstruct the map. Geometric information can reduce the impact of drastic lighting changes on the system. \cite{krombach2016combining} combines the point feature-based matching method with semi-dense direct image alignment to initialize the system robustly, which takes advantage of the two methods. For the line features utilized in the direct method, \cite{yang2017direct, li2018direct, gomez2016pl} only use the straight lines to force the 3D points in the map, which satisfies the collinear constraint, not optimize jointly optimize the pose of the system. \cite{zhou2021dplvo} introduces the collinear constraint into the DSO and improves the pose estimation accuracy with the line of 2 degrees of freedom (DoF) representation. \cite{cheng2020direct} expresses the line features in the image as Manhattan world regularity and merges the structural information into the optimization framework of photometric error. 










\section{SYSTEM OVERVIEW}\label{sec:system overview}



    



    
    


    


    


 

\begin{figure}[t]
	    \centering
	    \includegraphics[width=1.0\linewidth]{image/Framework.pdf}
        \vspace{-5mm}
	    \caption{Overview of our PVI-DSO system.}	\label{fig:pipeline}
	    \vspace{-3mm}
	\end{figure}
The system proposed in this paper is based on DSO\cite{engel2017direct}. To improve the accuracy and robustness of the system, we introduce the IMU measurement and geometric information in the environment. As shown in Fig. \ref{fig:pipeline}, the proposed system contains two main modules running in parallel: the front end and the back end.
	
In the front end, raw measurements from the IMU are processed. With the aid of IMU information, the most recent frame's photometric parameters and pose can be estimated robustly by coarse tracking. And then, we judge whether the current frame is the keyframe. If the current frame is a keyframe, it will be delivered to the back end module. Otherwise, it is only used to track the candidate points to update the depth.

In the back end, the candidate points are tracked and refined with the latest image to obtain more accurate depth, they are selected as the activate points if satisfy some criterion as described in \cite{engel2017direct}. And then, the depth map generated by the activate points is used to form the 3D mesh and planes. The coplanar constraints extracted from the planes are used to constraint the pose of the system. Finally, the operations for the visual-inertial bundle adjustment are executed. We add the non-coplanar point residual, coplanar point residual, inertial residual, and corresponding prior residual into the optimizer. The plane detection, residual construction, optimization, and marginalization of the system will be described in Sec.\ref{sec: direct sparse visual-inertial odometry}.
	
\section{Notations and Preliminaries} \label{sec:algorithm}
	
 
 Throughout the paper, we will use the following notation: the bold upper letters represent matrices, bold lower letters represent vectors, and light lower letters represent scalars.  In this section,  we introduce the coordinate  transformation and the representation of point and plane features.


\subsection{Coordinate Transformation} \label{alg: coordinate system}
The world coordinate system is defined as a fixed inertial framework that the Z axis is aligned with the gravity direction.  We define the transformation from the IMU frame to world frame is $\textbf{T}_{wi} \in \textbf{SE}\left(3\right)$, the transformation from camera frame to IMU frame is $\textbf{T}_{ic} \in \textbf{SE}\left(3\right) $(which is calibrated in advance and fixed in the system). So the transformation from camera frame to world frame is calculated by $\textbf{T}_{wc} = \textbf{T}_{wi} * \textbf{T}_{ic}$. We denote Lie algebra elements as $\bm{\hat{\xi}} \in \mathfrak{se}\left(3\right)$ where $\bm{\xi} \in  \mathbb{R}^{6}$, exponential and logarithmic map associate $\textbf{SE}\left(3\right)$ and $\mathfrak{se}\left(3\right)$.
\begin{figure}[t]
    \vspace{2mm}
	\centering
	\includegraphics[width=0.8\linewidth]{image/point_planar_nonplanar.pdf}
	\vspace{-5mm}
	\caption{The coplanar regularities of pixels in the image are detected, and the coplanar points are on the planar region $\bm{\pi}_i$. The planar parameters are expressed in the world frame with a normal vector $\bm{n}_i$ and a distance $d_i$. }
	\label{fig:point_planar_nonplanar}
	\vspace{-3mm}
\end{figure}
\subsection{Point Representation} 
The pixels extracted are expressed by the inverse depth $d_p\in \mathbb{R}$ in the host image frame. In order to construct the photometric error cost function, we need to transform the pixel in the host image frame $I_h$ into the target image frame $I_t$. Assuming $\bm{p}$ is the pixel in the $I_h$ and it is observed at $\bm{p}'$ in the $I_t$, the relationship of the pixel is: 
\begin{flalign} 
	\begin{array}{c}
	\bm{p'} = \Pi_c\left(\bm{R}_{th}\Pi_c^{-1}\left(\bm{p}, d_p\right) + \bm{t}_{th}\right)
	\end{array}\label{fla: point transform}
\end{flalign} 
where $\bm{R}_{th}$ and $\bm{t}_{th}$ are the relative rotation and translation from $I_h$ to $I_t$, $\Pi_c$ and $\Pi_c^{-1}$ are the projecton and back projection of the camera.
\subsection{Plane Representation}
As is shown in Fig.\ref{fig:point_planar_nonplanar}, a plane in the world frame can be represented by $\bm{\pi} =\begin{bmatrix}\bm{n}^{\rm T}, d^{\rm T} \end{bmatrix}^{\rm T} $ where $\bm{n} \in \mathbb{R}^3$ is the normal vector of plane and  $d \in \mathbb{R}$ is the distance from origin of the world frame to the plane. The normal vector $\bm{n}$ has three parameters but only two Degrees of Freedom (DoF) with $||\bm{n}||_2 = 1$. 

To get a minimal parameterization of $\bm{\pi}$ for optimization, we specialize the parameters of a general plane into the vertical plane $^v\bm{\pi}$ that the normal vector is perpendicular to the gravity direction or the horizontal plane $^h\bm{\pi}$ that the normal vector is parallel for the gravity direction. For horizontal plane, we fix the normal vector $^h\bm{n}$ to  $\begin{bmatrix} 0, 0, 1 \end{bmatrix}^{\rm T} $, and only need to optimize the distance $^h d$. For vertical plane, since the normal vector is perpendicular to  the gravity direction, it has only one DoF, we represent it by the form
 \begin{flalign} 
	^v\bm{\pi}= \left( cos(\psi)cos(\phi), cos(\psi) sin(\phi), sin(\psi) , d \right)^{\rm T}
\end{flalign}
where $\phi$ and  $\psi$ are the azimuth and elevation angles of the normal vector and $d$ is the distance from the origin of the world frame. As shown in Fig. \ref{fig:plane_parameter}, we fix the elevation angle $\psi$  to zero and only optimize the azimuth angle $\phi$ and the distance $d$. 

The transformation of the plane parameters from world frame to camera frame has the form: 
\begin{flalign} 
	\bm{\pi}{_c}= \textbf{T}_{cw}^{-\rm T}\bm{\pi}{_w}
	\label{fla: transformation matrix of plane}
\end{flalign}
Using (\ref{fla: transformation matrix of plane}), we can convert the plane of world frame and construct the photometric error function about plane parameters in the camera frame.
\begin{figure}[t]
	\centering
	\includegraphics[width=0.55\linewidth]{image/plane_parameter.pdf}
	\vspace{-5mm}
	\caption{ The 2 DoF parameter representation of the normal vector $\bm{n}$.}
	\label{fig:plane_parameter}
	\vspace{-3mm}
\end{figure}
\section{VIO WITH COPLANAR REGULARITIES } \label{sec: direct sparse visual-inertial odometry}
{In this section, the method of coplanarity detection is firstly described. And then, we will fuse the inertial constraint and coplanar constraint into the non-linear optimization framework of \cite{engel2017direct} to estimate the body state and 3D landmarks. Meanwhile, in the optimization, we use the coplanar parameter expression to construct the photometric error, which not only guarantees accuracy but also reduces the number of optimization state variables. When the coplanar point-anchored frame is marginalized, we convert these constraints into plane distance-costs as the plane prior.
Finally, since the minimized energy functional is highly non-convex, the system proposed in this paper adopts dynamic initialization, a good initial value ensures the robustness of the system initialization in complex environments.}

We optimize all the state variables in the sliding window by minimizing the sum of the energy function from visual residual, IMU residual, and prior residual:
\begin{flalign} 
E_{total} = \lambda \cdot E_{point} + \lambda \cdot E_{plane} + E_{inertial} \label{fla: total cost function} + E_{prior}
\end{flalign}
where $E_{point}$ is the photometric error of non-coplanar points (section \ref{alg: photometric error of non-coplanar point}), $E_{plane}$ is the photometric error of coplanar points (section \ref{alg: photometric error of coplanar point}), $E_{inertial}$ is the the inertial error term  (section \ref{alg: photometric error of inertial error}), and $E_{prior}$ is the prior from marginalization operator (section \ref{alg: Marginalization with coplanar constraint}), respectively. $\lambda$ is the weight between visual photometric error and inertial error.

\subsection {Coplanarity Detection}\label{alg: coplanarity detection}
In this paper, we detect plane information from the 3D mesh. Since it is challenging to construct 3D mesh from 3D landmarks directly, we build 2D Delaunay triangulation \cite{hosseinzadeh2017sparse} in the image and project the triangular regularities to 3D landmarks. The direct method does not compute the matching relationship of points, making organizing the same triangular patch observations in different image frames difficult. Therefore, we generate 2D Delaunay triangulation in {the depth map of DSO \cite{engel2017direct}, which spans multiple image frames. In this way, the 3D mesh is anchored to the time frame of the sliding window, limiting memory usage.}


{For plane detection, we only detect the planes that are either vertical (i.e., walls, the normal is perpendicular to the gravity direction) or horizontal (i.e., floor, the normal is parallel for the gravity direction), which are commonly found in man-made environments.} For horizontal plane detection, we build a 1D histogram of the average height of triangular patches that the normal vector parallel for the gravity direction, and then the local maximums of the histogram are extracted after a Gaussian filter, as described in \cite{rosinol2019incremental}. We consider the local maximum to be the horizontal plane when it exceeds a certain threshold ($\sigma_t = 20$). For vertical plane detection, a 2D histogram is built,  one axis is the azimuth of the plane's normal vector, the other axis is the distance from the origin of the plane, we only count the triangular patches that the normal vector is perpendicular to the gravity direction, after statistics, we select the vertical planes in a similar way of horizontal plane detection.

We maintain all historical planes in the system. Therefore, for the newly detected planes, we need to detect whether the historical plane already exists according to its parameters and merge the similar planes when the angle and distance parameters are less than a certain threshold. Meanwhile, the coplanar points associated with the latest plane are also transferred to the historical plane, for efficiency, we only add the coplanar constraints detected from the current local 3D mesh into the sliding window. In the marginalization, the coplanar constraints on the plane parameters are removed and converted to the plane-distance cost, which contains the information of historical plane and is used as the prior in the  subsequent optimization. 

\subsection{Photometric Error of Non-coplanar Point } \label{alg: photometric error of non-coplanar point}
 The direct method is based on the  photometric invariance hypothesis to minimize the photometric error. In a similar way as \cite{engel2017direct}, the photometric error for a point $\bm{p}$ with inverse depth $d_{\bm{p}}$ in the host image  $I_h$ observed by the target image $I_t$ is defined as:
 \begin{flalign} 
  E_{p_j} = \sum\limits_{\bm{p} \in \mathcal{N}_{\bm{p}}} w_{\bm{p}} \|
  \left( I_t\left[\bm{p'}\right] - b_t\right) - \frac{t_t e^{a_t}}{t_he^{a_h}}\left(I_h\left[\bm{p}\right]-b_h\right) \|_{\gamma} \label{fla: photometric error}
 \end{flalign}
where $t_h$, $t_t$ are the exposure times of the respective image $I_h$ and $I_t$, $a_h$, $a_t$, $b_h$ and $b_t$ are the affine illumination  transform parameters, $\mathcal{N}_p$ is a small set of pixels around the point $\bm{p}$, 
$w_{\bm{p}}$ is a gradient-dependent weighting and $\| \cdot\|_{\gamma}$ is the Huber norm, $\bm{p}'$ is the point projected into $I_t$ and the relationship between $\bm{p}$ and $\bm{p}'$ is given by (\ref{fla: point transform}). 

So the sum of the photometric error of non-coplanar points  observed by all the keyframes in the sliding window is defined as:
 \begin{flalign} 
	E_{point} = \sum\limits_{i \in \mathcal{F}}  \sum\limits_{\bm{p} \in \mathcal{P}_i} \sum\limits_{j \in obs(\bm{p})} E_{\bm{p}_j}  
\end{flalign}
where $\mathcal{F}$ is the keyframes in the sliding window, $\mathcal{P}_i$ is the set of non-coplanar points in the host keyframe $i$, $obs(\bm{p})$ is a set of observations of the $\bm{p}$ in the other keyframes. 
\begin{figure}[t]
	\centering
	\includegraphics[width=1\linewidth]{image/hessian_matrix_of_coplanar_points.pdf}
	\vspace{-5mm}
	\caption{The Hessian matrix of the photometric error. (a) 2 coplanar points and 2 non-coplanar points are observed by camera $O_1$ and camera $O_2$, the coplanar points lie on the plane $\bm{\pi}$; (b) the traditional cost function of coplanar points needs to optimize the inverse depth; (c) the cost function of this paper only optimizes the parameters of plane.}
	\label{fig:hessian matrix of coplanar points}
	\vspace{-3mm}
\end{figure}
\subsection{Photometric Error of Coplanar Point } \label{alg: photometric error of coplanar point}
The plane $\bm \pi_w$ detected from the 3D mesh is  expressed in the world frame, in order to construct the photometric error between the host image and the target image, we convert the $\bm \pi_w$ into the host image frame to get  $\bm \pi_c$ using (\ref{fla: transformation matrix of plane}).
When we detect the point $\bm p_c $ in the image that the corresponding 3D point $\bm{p}_{\bm{\pi}_c}$ lies on the plane $\bm{\pi}_c$ , the point $\bm p_c$ satisfies the coplanar equation
\begin{flalign} 
z \cdot \bm{n}_c^{\rm T} \Pi_c^{-1}(\bm{p}_c, 1) + d_c = 0
\end{flalign}   
where $z$ is the depth from the origin of the camera frame to the 3D point of $\bm p_c$, $\bm n_c$ is the normal vector of $\bm \pi_c$, and $d_c$ is the distance of $\bm \pi_c$.
With the depth $z$, we can rewrite (\ref{fla: point transform}) as (\ref{fla: coplanar_point transform}) and construct the photometric error $E_{\bm p_c}$ of coplanar point by (\ref{fla: photometric error})
\begin{flalign} 
	\begin{array}{c}
		\bm{p_c}' = \Pi_c\left(\bm{R}_{th}\Pi_c^{-1}\left(\bm{p}_c, 1/z\right) + \bm{t}_{th}\right)
	\end{array}\label{fla: coplanar_point transform}
\end{flalign} 
The sum of the photometric error of coplanar points observed by all the keyframes in the sliding window is:
 \begin{flalign} 
	E_{plane} = \sum\limits_{i \in \mathcal{F}}  \sum\limits_{\bm{p} \in \mathcal{C}_i} \sum\limits_{j \in obs(\bm{p})} E_{\bm{p}_c}  
\end{flalign}
where $\mathcal{C}_i$ is the set of coplanar points in the host keyframe $i$. For a single residual, the jacobian corresponding to the camera frame can be decomposed as \cite{engel2017direct}:
\begin{flalign}
\bm{J}_k = [\bm{J}_I \cdot \bm{J}_{geo}, \bm{J}_{photo}]
\end{flalign}
where $\bm{J}_I$ is the image gradient, $\bm{J}_{geo}$ is the jacobian of geometric parameters ($\textbf{T}_h$, $\textbf{T}_t$), $\bm{J}_{photo}$ is the jacobian of photometric parameters ($a_h$, $a_t$, $b_h$, $b_t$). Specially, we note the jacobian terms in $\bm{J}_k$ that  related to the host image frame as $\bm{J}_h$, the jacobian terms in $\bm{J}_k$ that related to the target image frame as $\bm{J}_t$, the jacobian of $\bm{\pi_{w}}$ as $\bm{J}_{\pi}$ so the jacobian of the coplanar point on the plane can be written as :
\begin{flalign}
\bm{J}_{coplanar} = [\bm{J}_h, \bm{J}_t, \bm{J}_{\pi}]
\end{flalign}

we only need to construct the Hessian matrix of the host image frame $\bm{H}_{h}$, target image frame $\bm{H}_{t}$ and  plane parameters $\bm{H}_{\pi}$, not the inverse depth of coplanar points, as is shown in Fig.\ref{fig:hessian matrix of coplanar points}, in this way, the number of state variables added to the optimizer can be effectively reduced, which limits the computation cost of the optimization.
\subsection{Inertial Error}  \label{alg: photometric error of inertial error}
To construct the inertial error with angular velocity and linear acceleration, we use the pre-integration method proposed in \cite{forster2015imu} to handle the high frequencies of IMU measurements. 
This gives a pseudo-measured value of IMU between consecutive keyframes. 
Similar to \cite{forster2015imu}, the pre-integration IMU factors we build include relative rotation and translation, velocity, and gyroscope and accelerometer biases. The biases are fixed in each single pre-integration block.





\subsection{Marginalization with Coplanar Constraints}\label{alg: Marginalization with coplanar constraint}
{We utilize the bundle adjustment to optimize the state variables in the sliding window. When the new image frame is added into the sliding window, all the variables corresponding to the older frame are marginalized using the Schur complement.
For the state variable of the plane, to avoid maintaining too many historical planes in the sliding window, we also need to "marginalize" them out.
Since the removed planes may be added into the optimizer again later, we convert the coplanar constraints related to the plane into the plane-distance cost,  which is regarded as the plane prior to enforcing coplanar constraints. In this way, we do not need to increase much computation cost to generate the plane prior.}





\subsubsection{Marginalization of non-coplanar and inertial constraints}
When the number of keyframes in the sliding window is greater than a certain threshold, we select the marginal keyframe similar to the criteria in \cite{engel2017direct}, which considers the luminance change of the images and the geometry distribution of the poses. To maintain the consistency of the system, once the variable is connected to the marginalization factor, we use the First Jacobian Estimation (FEJ) \cite{huang2009first} to fix the variable's linearization point at the same value.



\subsubsection{Plane-distance cost for coplanar constraints}

For coplanar points, instead of using the photometric error term as the prior, we consider the coplanar points as a whole plane and directly use the plane-distance cost as the prior constraint. The plane-distance cost can be expressed by the distance from the prior plane $\bm{\pi}'= (\phi',  \psi', d')$ to the current plane $\bm{\pi} = ( \phi ,  \psi , d)$:
\begin{flalign}
	 E_{\bm{\pi}_p} = w_n  \|\left[ \phi', \psi', d' \right]^{\rm T}- \left[ \phi ,  \psi , d \right]^{\rm T}\|_{\sum_{\bm{\pi}}}^2
\end{flalign}
where $\sum_{\bm{\pi}}$ is the covariance matrix of constraint, $w_n$ is the number of the points on the plane.


\subsection{Initialization and Observability}
{Compared to pure visual odometry, visual-inertial odometry fusing IMU data makes metric scale, roll, and pitch observable.
This means that the state variables must be properly initialized. Otherwise, the optimization may diverge, leading to system failure.
Initialization can be divided into dynamic initialization and static initialization.
For the problem of insufficient visual parallax during static initialization, we use dynamic initialization in the system.
Optical flow tracking is introduced in the initialization, which improves the robustness of VIO initialization based on the direct method.
Similar to \cite{VINS}, we run SFM (Structure From Motion) to compute the rough pose of the frame, which is then aligned with the state variables obtained through the IMU integration for joint initialization.}



	
	
\begin{table*}[htbp]
	\label{tab:1}
	\begin{center}
	\vspace{5mm}
		\caption{The trajectory error (m) of different methods on EuRoC dataset. In \textbf{bold} the best of RMSE gt-scaled results, in \textcolor{blue}{blue} the best of RMSE results. }
		\label{tab:ATE Trans of EuRoC}
		\centering
		\setlength{\tabcolsep}{2.0mm}{
			\begin{tabular}{cc|ccccccccccc}
				\toprule
				Sequence& & MH1& MH2&MH3&MH4&MH5&V11&V12&V13&V21&V22&V23  \\\midrule
				\multirow{3}{*}{\makecell[c]{VI-DSO \\ (raw data from \cite{von2018direct})} } &RMSE& 0.062  & \textcolor{blue}{0.044}& 0.117  & 0.132 & 0.121 & 0.059 & \textcolor{blue}{0.067} & 0.096 &\textcolor{blue}{0.040} & 0.062 & 0.174 \\
				&RMSE gt-scaled& 0.041 &\bf 0.041  & 0.116 & 0.129 & \bf 0.106 & 0.057 &\bf 0.066  & 0.095 & \bf 0.031 & 0.060 & 0.173 \\
				&Scale Error(\%) &1.1  & 0.5 & 0.4 & \bf 0.2 & 0.8 & 1.1 & \bf 1.1 & \bf 0.8 & 1.2 & \bf 0.3 & 0.4 \\\midrule
				\multirow{3}{*}{our method no plane}  & RMSE  & 0.066 & 0.057 & 0.065& 0.110 & 0.117& 0.057 & 0.090 & 0.092 & 0.045 & 0.060 & 0.123\\
				& RMSE gt-scaled  & 0.052 & 0.056 & 0.062 & 0.099 & \bf 0.106 & 0.054 & 0.087 & 0.089 & 0.045 & 0.056 & 0.123 \\
				& Scale Error(\%)  & 0.9 & \bf 0.2 & 0.5 & 0.7 & 0.7 & 0.9 & 1.3 & 1.5 & \bf 0.3 & 1.0 & \bf 0.3 \\ \midrule
				\multirow{3}{*}{our method with plane}  &RMSE   &\textcolor{blue}{0.051} & 0.051 &  \textcolor{blue}{0.057} & \textcolor{blue}{0.103} & \textcolor{blue}{0.115} & \textcolor{blue}{0.051} & 0.082 & \textcolor{blue}{0.082} & 0.043  &\textcolor{blue}{0.048} & \textcolor{blue}{0.111} \\
				&RMSE gt-scaled   & \bf 0.039  & 0.048 & \bf 0.055 & \bf 0.087 & 0.109 & \bf 0.049 & 0.079 & \bf 0.078 & 0.042 & \bf 0.044 & \bf 0.110 \\
				& Scale Error(\%)   & \bf 0.8 & 0.3 & \bf 0.3 & 0.8 & \bf 0.5 & \bf 0.7 & \bf 1.1 & 1.6 & 0.5  & 0.9 & \bf 0.3 \\ \midrule
				\makecell[c]{ mesh-VIO}& RMSE  & 0.145 & 0.130 & 0.212 & 0.217 & 0.226 & 0.057 &0.074  & 0.167 & 0.081 & 0.103 & 0.272 \\ \midrule
				\makecell[c]{ PVIO}& RMSE  & 0.163 & 0.111 & 0.119 & 0.353 & 0.225 & 0.082 &0.113  & 0.201 & 0.063 & 0.157 & 0.280 \\ \midrule
			\end{tabular}
		}
	\end{center}
	\vspace{-5mm}
\end{table*}
\section{Experiments} 
\label{sec:experiment}
We evaluate our system on the publicly available EuRoC MAV dataset \cite{Burri2016} and TUM VI dataset \cite{Schubert2018}. Besides, we provide a video
in the supplementary to reflect the results of the experiment more intuitively. We run the system in the environment with Intel Core i7-9750H@ 2.6GHz, 16GB memory.
\subsection{Quantitative Evaluation}
To verify the performance of the system, we compared the accuracy of our system with VI-DSO \cite{von2018direct}, DSO \cite{engel2017direct}, ORBSLAM3 without loop closure \cite{ORBSLAM3}, OKVIS \cite{Leutenegger2014}, PVIO \cite{li2019robust} and mesh-VIO \cite{rosinol2019incremental}.  VI-DSO and DSO are the typical slam based on the direct method, ORBSLAM3 and OKVIS are state-of-art with indirect method, mesh-VIO and PVIO use the plane information in the VIO system,  through comparison, the system performance can be fully reflected. {The accuracy of the trajectory is measured by aligning the estimated trajectory with groundtruth, we calculated the RMSE of the trajectory error with SE3 alignment and Sim(3) alignment (with the description "RMSE gt-scaled"). Scale error is computed using scale $s$ from Sim(3) alignment, as $| 1-s|$.} 
\subsubsection{The EuRoC Dataset}
The EuRoC micro aerial vehicle (MAV) dataset consists of two scenes, the machine hall and the ordinary room, 
{which contain scenes with different scales and complexity. We compared our method with VI-DSO, mesh-VIO, and PVIO. Meanwhile, the ablation experiments are also conducted, as shown in Tab.\ref{tab:ATE Trans of EuRoC}. 
Whether from the results of RMSE or RMSE gt-scaled, PVI-DSO achieves the smallest translation error on most of the sequences, which means using the planar information in the direct method VIO can improve the accuracy of pose estimation. 
Especially, by comparing the experiment results of VI-DSO and our method without plane information, the positioning accuracy is basically the same. 
However after introducing the coplanar constraints into VIO, the error of RMSE was reduced by an average of 11\%, the error of RMSE gt-scaled reduced by an average of 12\%.
We can see that the structure information in the environment can help improve the accuracy and maintain the global scale estimation, which is also proved by the results of scale error.
Meanwhile, from the experimental results of PVIO and mesh-VIO, since the direct method can obtain more visual measurement compared with the indirect method, our method thereby improves the estimation of the pose.
}
\begin{figure}[b]
	\centering
	\vspace{-2mm}
	\includegraphics[scale=0.16]{image/color_map}
	\caption{Translation error of RMSE gt-scaled for diffferent methods run 11 times (rows) on each corridor sequence (colums) of TUM VI dataset.}
	\label{fig:color map of tum vi}
\end{figure}
\begin{table}[b!]
	\label{tab:1}
	\begin{center}
		\caption{The RMSE gt-scaled of the state-of-art methods compare to our method on TUM VI dataset. The translation (m) and rotation (rad) error are list as follows. In \textbf{bold} the best result}
		\label{tab:ATE Trans and Rots on TUM VI}
		\centering
		\setlength{\tabcolsep}{1.3mm}{
			\renewcommand{\arraystretch}{1.1}
			\begin{tabular}{ccccccccc}
				\hline
				\multirow{2}{*}{Seq.} & \multicolumn{2}{c}{ORBSLAM3} & \multicolumn{2}{c}{DSO} & \multicolumn{2}{c}{OKVIS} & \multicolumn{2}{c}{ours} \\ \cline{2-9} 
				& trans.  & rot.  & trans. & rot. & trans. & rot.  & trans. & rot.  \\ \hline
				corridor1  &0.230&0.216&0.747&0.968&0.451&0.800 &\bf0.114&\bf0.129           \\
				corridor2  &\bf0.052&\bf0.058&0.783&1.318&0.478&0.528 &0.379&0.421           \\
				corridor3  &0.169&0.127&0.924&2.161&0.485&1.033 &\bf0.159&\bf0.123           \\
				corridor4  &0.414&0.510&0.305&0.150& 0.300&0.359&\bf0.118&\bf0.145           \\
				corridor5  &0.186&0.034& 0.823&2.000&0.415&0.613&\bf 0.098&\bf0.013           \\ \hline
			\end{tabular}
		}
	\end{center}
\end{table}
\subsubsection{The TUM VI Dataset} 
We also evaluate our proposed system on the corridor sequences of the TUM VI benchmark dataset, which is collected in a long corridor and several offices and contains many images with blurring and lighting changes due to high-speed motion. We independently ran the algorithm 11 times on all sequences and calculated RMSE gt-scaled. Fig. \ref{fig:color map of tum vi} shows the different performance results of different algorithms running 11 times. Tab. \ref{tab:ATE Trans and Rots on TUM VI} shows the median translation and rotation error of the 11 results. {Compared to other algorithms, our method achieves the highest accuracy on almost all the sequences except for the corridor2 sequence, on which ORBSLAM3 performs better, this demonstrates the effectiveness of IMU measurements and geometric coplanarity information for improving localization accuracy. The VIO based on the direct method can build a more dense map, as shown in Fig. \ref{fig:map of tum vi}, the detailed structure of the indoor environment constructs by our method makes it easy to detect the plane information.}
\begin{figure}[b!]
	\centering
	\vspace{-4mm}
	\includegraphics[scale=0.21]{image/tumvi_map}
	\caption{
	Map of corridor3 sequence generated by our method. (a) is the 3D coplanar points of the vertical planes and the horizontal planes in the map. (b) is the reconstructed map, two sub-images show the 2D Delaunay triangulation and the corresponding 3D mesh of the scene, the pink points in the image are the detected 2D coplanar points. }
	\label{fig:map of tum vi}
	\vspace{-3mm}
\end{figure}
\subsection{ Weight Determination of Photometric Error}
Since the camera's photometric calibration and distortion correction have significant influences on the direct VIO methods, the weight ratio of photometric and inertial errors is often difficult to determine. In this paper, we use the method of parametric study to obtain the standard deviation of the photometric error to determine the weight, {for the EuRoC MAV dataset and TUM VI dataset, we drew the cumulative error curve of pose estimation in Fig.\ref{fig:v2_02_standard_derivation} after setting different standard deviations.} The results in Fig.\ref{fig:v2_02_standard_derivation} (a) show that setting the standard deviation of the photometric error to 11 performs best in the EuRoC dataset, as shown in Fig.\ref{fig:v2_02_standard_derivation} (b), for the TUM VI dataset, a standard deviation of 16 is optimal.

\begin{figure}[h!]
	\centering
	\vspace{2mm}
	\includegraphics[scale=0.17]{image/parameter_study}
	\caption{Cumulative error plot on the V22 sequence of EuRoC dataset (a) and corridor4 sequence of TUM VI dataset (b). Different standard deviation of photometric error affects the accuray and robustness of pose estimation.}
	\label{fig:v2_02_standard_derivation}
	\vspace{-3mm}
\end{figure}

\subsection{Effect of Plane Prior}
To verify the effectiveness of our proposed plane-distance prior, we test the plane parameters  consistent on the EuRoC dataset V11 sequence. As shown in Fig. \ref{fig:curve of plane prior}: 
We can observe that the distance estimate for the plane varies greatly without the prior, while the estimation remains stable after adding the plane prior.
The planar prior contains historical information constraints that can provide constraints on the pose in the sliding window. Meanwhile, the range of curve variation without prior information also indicates that the elevation of ground points in the reconstructed map changes from -0.98m to -0.86m.





\begin{figure}[t]
	\centering
	\includegraphics[scale=0.18]{image/V101_plane_prior}
	\caption{
	The distance estimation of the horizontal plane in V11 sequence of the EuRoC dataset, we count the keyframes in which planar features are observed.}
	\label{fig:curve of plane prior}
	\vspace{-3mm}
\end{figure}
\subsection{Runtime Evaluation}
Tab.\ref{tab:running time} shows the run-time ablation experiments for systems containing different modules.
We measure the mean execution time of different modules of the system using pure VO, VIO, and our proposed PVI-DSO on EuRoC's V11 sequence, respectively. Considering the real-time  performance of the system, we set the number of extracted pixels in the keyframe to 800 and the size of  the sliding window to 7 according to \cite{engel2017direct}. In the pure VO method, two-thirds of the time is spent on optimization, and the rest is spent on tracking pixels, selecting candidate, activation points, and extracting pixels, etc. After adding inertia constraints to become a VIO system, there is only a slight time increase in IMU information processing and optimization.
For our proposed PVI-DSO, the system adds a plane detection part and a mesh generation part, but both parts are very efficient and only need 0.72ms and 1.08ms, respectively.
Furthermore, compared with VIO, the time of marginalization is reduced from 0.79ms to 0.72ms, the time of problem solving (compute x from $\rm Ax = b$) and state variables updating ($\rm x = x + \delta x$) is also reduced from 2.41ms to 2.16 ms, due to the novel parametric method adopted by the system to reduce the size of parameters. 
The optimization time is slightly increased because the coplanar parameters are more complicated to calculate the photometric error and Jacobian. In the sliding window, the average number of the state variables (plane number $\approx$ 2, point number $\approx$ 1182) for PVI-DSO is fewer than that (point number $\approx$ 1422) of the VIO, proving that the coplanar parameter dramatically reduces the number of optimized state variables.

\begin{comment}


\begin{table}[htbp]
	\label{tab:3}
	\begin{center}
		\caption{Mean execution time (Unit: millisecond) of pure VO, VIO, and our proposed PVI-DSO running on the sequence V11.}
		\label{tab:running time}
		\centering
		\setlength{\tabcolsep}{2.9mm}{
			\renewcommand{\arraystretch}{1.2}
			\begin{tabular}{cccc}
			\hline
			\multicolumn{1}{c}{Module} & \multicolumn{1}{c}{VO} & \multicolumn{1}{c}{VIO} & \multicolumn{1}{c}{PVI-DSO} \\ \hline
			\texttt{Plane detection } &      0                  &    0                   &  0.72                     \\
			\texttt{mesh Creation}    &       0                 &    0                    &  1.08                   \\
			\texttt{Optimization}      &    19.89            &    20.46                      &    21.65                   \\
			\texttt{Construct Cost Function}      &   16.44            &    16.99                      &    17.74                   \\
			\texttt{Solve Equation update increament}      &    2.39            &    2.41                      &    2.16                   \\
			\texttt{Marginalization}&     0.77               &     0.79                  &   0.72                    \\ \hline
			\texttt{Total}                        &   31.97                &    32.54                &   35.03                    \\ \hline
		\end{tabular}
		}
	\end{center}
\end{table}

\end{comment}

\begin{table}[htbp]
	\label{tab:3}
	\begin{center}
		\caption{Mean execution time (Unit: millisecond) of pure VO, VIO, and our proposed PVI-DSO running on the sequence V11.}
		\label{tab:running time}
		\centering
		\setlength{\tabcolsep}{1.3mm}{
			\renewcommand{\arraystretch}{1.2}
			\begin{threeparttable}
			\begin{tabular}{cccc}
			\hline
			\multicolumn{1}{c}{Module} & \multicolumn{1}{c}{VO} & \multicolumn{1}{c}{VIO} & \multicolumn{1}{c}{PVI-DSO} \\ \hline
			\texttt{Plane detection } &      0                  &    0                   &  0.72                     \\
			\texttt{mesh Creation}    &       0                 &    0                    &  1.08                   \\
			\texttt{ Cost Function Construction\tnote{1}}      &   16.44            &    16.99                      &    17.74                   \\
			\texttt{\makecell[c]{Problem Solving\&Var Updating\tnote{1}}}      &    2.39            &    2.41                      &    2.16                   \\
			\texttt{Marginalization}&     0.77               &     0.79                  &   0.72                    \\ \hline
			\texttt{Total}                        &   31.97                &    32.54                &   34.58                    \\ \hline
		\end{tabular}
		\begin{tablenotes}   
        \footnotesize              
        \item[1] The optimization contains cost function construction, problem solving, and state variables updating.          
         \end{tablenotes}           
         \end{threeparttable}      
		}
	\end{center}
\end{table}
\section{Conclusion} 
\label{sec:conclusion}
In this paper, we present a novel VIO system based on the direct method, as well as leveraging the planar regularities, which is called PVI-DSO. The VIO based on the direct method constructs a more dense map, which contains rich geometric features. The plane regularities in the map are extracted by generating the 3D mesh, and a novel parameterization for co-planar points is used to introduce the plane constraints into the VIO system, meanwhile, the plane-distance cost used in the optimization converts the coplanar constraints into the plane prior,  by these methods, introducing the planar information into the system doesn't increase much computational burden. The experiments show that the trajectory accuracy of our approach is better than the state-of-the-art visual-inertial odometry.  In the future, we will fuse line and plane geometric information in the VIO based on the direct method to further improve the accuracy and robustness of positioning. 
\section*{ACKNOWLEDGMENT}
We would like to thank Dr. Yijia He for the helpful discussion. This work was supported by the Foundation for Innovative Research Groups at the National Natural Science Foundation of China (Grant No. 41721003) and the fellowship of China National Postdoctoral Program for Innovative Talents (Grant No. BX20200251).

\begin{comment}

\section*{APPENDIX}


To simplify the formula, we define variables $nwr =  (
\bm{n}_w^{\rm T}  \bm{R}_{wh}) \cdot \bm{p}_c^n$ and $nwt = 
\bm{n}_w \cdot \bm{t}_{wh} + dw$ 
The Jacobian of host image frame is the form:
\begin{flalign} 
\bm{J}_{Th} = -\bm{R}_{th}  \bm{p}_c^n \cdot \bm{n}_w^{\rm T} / ((
\bm{n}_w^{\rm T}  \bm{R}_{wh}) \cdot {\bm{p}_c^n}) + \bm{R}_{wt}^{\rm T}
\end{flalign}

\begin{flalign} 
	\bm{J}_{rh} = -\bm{R}_{th}  \bm{p}_c^n \cdot \bm{n}_w^{\rm T} / ((
	\bm{n}_w^{\rm T}  \bm{R}_{wh}) \cdot {\bm{p}_c^n}) + \bm{R}_{wt}^{\rm T}
\end{flalign}


The jacobian of target image frame
\begin{flalign} 
E_{plane} = \sum\limits_{i \in \mathcal{F}}  \sum\limits_{\bm{p} \in \mathcal{P}_i} \sum\limits_{j \in obs(\bm{p})} E_{\bm{p}_c}  
\end{flalign}
The jacobian of ground
\begin{flalign} 
E_{plane} = \sum\limits_{i \in \mathcal{F}}  \sum\limits_{\bm{p} \in \mathcal{P}_i} \sum\limits_{j \in obs(\bm{p})} E_{\bm{p}_c}  
\end{flalign}
The jacobian of wall 
\begin{flalign} 
E_{plane} = \sum\limits_{i \in \mathcal{F}}  \sum\limits_{\bm{p} \in \mathcal{P}_i} \sum\limits_{j \in obs(\bm{p})} E_{\bm{p}_c}  
\end{flalign}

\end{comment}
	


	\balance
	
	\bibliographystyle{IEEEtran}
	
\section{INTRODUCTION}
\label{sec:introduction}
Simultaneous Localization and  Mapping (SLAM) are research hotspots in the field of robots, autonomous driving, augmented reality, etc. Camera and inertial measurement units (IMU) are low-cost and effective sensors. The Visual Inertial Odometry (VIO) combines the complementarity of the two sensors to improve the accuracy and robustness of the pose estimation. Existing VIO methods \cite{VINS, ORBSLAM3, mourikis2007multi} generally use the feature-based (indirect) method as visual measurements. However, in the weak texture environment, the number of effective point features extracted by the indirect method is small, leading to the failure of pose estimation. On the other hand, the direct method \cite{engel2017direct, von2018direct} can utilize all available pixels of images to generate a more complete model, which is more robust. 

Since the VIO can build the map of the surrounding environment in real-time, by using the geometric constraint information of the map, we can improve the positioning accuracy and the quality of the map.
The widely existing edges in the environment are the most common features which add different types of visual measurement to the estimator \cite{PL-VIO,pumarola2017pl,structslam, li2018direct}. Meanwhile, the planar features in the environment can also be used to increase the robustness of the estimator. However, the reconstructed map with the indirect method is too sparse, which makes it difficult to extract planar regularities, and the planar features will increase the computational burden that affects the real-time performance of the system.

For the VIO based on the direct method, the visual module tracks the pixels with large enough intensity gradients, sufficient visual observation makes the reconstructed map denser. Meanwhile, with the aid of the IMU information,  it is easier to extract the plane information in the map, as is shown in Fig.\ref{fig:cover_grap}. Furthermore, introducing the geometric information that is less sensitive to the luminance changes into the direct method makes the system more robust.  
\begin{figure}[t]
	\centering
	\includegraphics[width=1.0\linewidth]{image/cover_graph.pdf}
	\caption{The proposed direct sparse visual-inertial odometry builds a sparse map running on the V11 sequence of the  EuRoC dataset. (a) is the reconstruction map of the whole scenes, (b) is the coplanar points on different planes, the different colors are used to distinguish the planes. (c) is the  2D Delaunay triangulation, depth map, and reconstructed 3D mesh of the corner in the scene. }
	\label{fig:cover_grap}
	\vspace{-6mm}
\end{figure}

This paper proposes a direct sparse visual inertial odometry that leverages planar regularities called PVI-DSO. The method is an extension of DSO, we complement the IMU measurement constraints and coplanar constraints. Unlike the method of \cite{von2018direct}, we use a novel way to fuse the IMU measurement information, which doesn't estimate the transformation between the DSO frame and the metric frame. Meanwhile, we extract the coplanar regularities from the generated 3D mesh, and inspired by \cite{li2020co}, in the photometric error function, we adopt the plane parametric representation whose performance has been proved in \cite{li2020co}. Through the above methods, adding plane features in the system doesn't increase much computational burden. In summary, the main contribution of this work include: 

\begin{itemize}
\item To our best knowledge, PVI-DSO is the first direct sparse visual-inertial odometry that fuses coplanar constraint regularities to improve the accuracy of localization. 

\item { We introduce a coplanar point parameterization in the direct method that constructs photometric error constraints, which is an extension of \cite{li2020co}. The plane-distance cost used in this paper converts the coplanar constraints into the plane prior, which can enforce the coplanar constraints in the optimization without increasing extra computation cost.}

\item We design extensive experiments on the challenging EuRoC and TUMVI datasets. The experimental results show that the proposed PVI-DSO fusing IMU constraints and coplanar regularities into the direct method outperforms the state-of-the-art.
\end{itemize}
\section{RELATED WORK}\label{sec:related work}

{The most common features used in the SLAM / VIO algorithms are the points \cite{mourikis2007multi, VINS, ORBSLAM3},  \cite{engel2017direct}, \cite{forster2014svo}, as a complement to the point features, the geometric information (line, plane) introduced in the system based on points can improve the accuracy of the pose estimation and mapping, which have received extensive attention in recent years.}

\textbf{Indirect method with geometric Regularities} \ Since the line features exist widely in the environment, it seems natural to fuse the line features into the framework based on points \cite{PL-VIO,pumarola2017pl, Tsai2018}. Furthermore, in the structural scenario, the lines with three perpendicular directions of the Manhattan world model can encode the global orientations of the local environments, which are leveraged to improve the robustness and accuracy of pose estimation \cite{Zou2019, structslam, xu2021leveraging}. For the planar information, the main difficulty is how to accurately extract the planar regularities in the environment. \cite{salas2014dense,zhang2019point,ma2016cpa} extract plane features with the assistance of depth map obtained by the RGBD camera. Lu \textit{et al.} proposed \cite{lu2015visual} uses the RANSAC method to fit the plane among the estimated 3D points with RGB camera. Nevertheless, the RANSAC-based method can only be used when there is only one potential large plane in the environment and consumes a lot of time. More Recently, \cite{rosinol2019incremental, li2020leveraging} get the planar regularities from the 3D mesh generated by the VIO. However, Sparse point clouds generated by indirect-based vision algorithms make the location of the 3D mesh inaccurate.


\textbf{Direct method with geometric Regularities} \ Direct methods minimize photometric errors to estimate pose and reconstruct the map. Geometric information can reduce the impact of drastic lighting changes on the system. \cite{krombach2016combining} combines the point feature-based matching method with semi-dense direct image alignment to initialize the system robustly, which takes advantage of the two methods. For the line features utilized in the direct method, \cite{yang2017direct, li2018direct, gomez2016pl} only use the straight lines to force the 3D points in the map, which satisfies the collinear constraint, not optimize jointly optimize the pose of the system. \cite{zhou2021dplvo} introduces the collinear constraint into the DSO and improves the pose estimation accuracy with the line of 2 degrees of freedom (DoF) representation. \cite{cheng2020direct} expresses the line features in the image as Manhattan world regularity and merges the structural information into the optimization framework of photometric error. 










\section{SYSTEM OVERVIEW}\label{sec:system overview}



    



    
    


    


    


 

\begin{figure}[t]
	    \centering
	    \includegraphics[width=1.0\linewidth]{image/Framework.pdf}
        \vspace{-5mm}
	    \caption{Overview of our PVI-DSO system.}	\label{fig:pipeline}
	    \vspace{-3mm}
	\end{figure}
The system proposed in this paper is based on DSO\cite{engel2017direct}. To improve the accuracy and robustness of the system, we introduce the IMU measurement and geometric information in the environment. As shown in Fig. \ref{fig:pipeline}, the proposed system contains two main modules running in parallel: the front end and the back end.
	
In the front end, raw measurements from the IMU are processed. With the aid of IMU information, the most recent frame's photometric parameters and pose can be estimated robustly by coarse tracking. And then, we judge whether the current frame is the keyframe. If the current frame is a keyframe, it will be delivered to the back end module. Otherwise, it is only used to track the candidate points to update the depth.

In the back end, the candidate points are tracked and refined with the latest image to obtain more accurate depth, they are selected as the activate points if satisfy some criterion as described in \cite{engel2017direct}. And then, the depth map generated by the activate points is used to form the 3D mesh and planes. The coplanar constraints extracted from the planes are used to constraint the pose of the system. Finally, the operations for the visual-inertial bundle adjustment are executed. We add the non-coplanar point residual, coplanar point residual, inertial residual, and corresponding prior residual into the optimizer. The plane detection, residual construction, optimization, and marginalization of the system will be described in Sec.\ref{sec: direct sparse visual-inertial odometry}.
	
\section{Notations and Preliminaries} \label{sec:algorithm}
	
 
 Throughout the paper, we will use the following notation: the bold upper letters represent matrices, bold lower letters represent vectors, and light lower letters represent scalars.  In this section,  we introduce the coordinate  transformation and the representation of point and plane features.


\subsection{Coordinate Transformation} \label{alg: coordinate system}
The world coordinate system is defined as a fixed inertial framework that the Z axis is aligned with the gravity direction.  We define the transformation from the IMU frame to world frame is $\textbf{T}_{wi} \in \textbf{SE}\left(3\right)$, the transformation from camera frame to IMU frame is $\textbf{T}_{ic} \in \textbf{SE}\left(3\right) $(which is calibrated in advance and fixed in the system). So the transformation from camera frame to world frame is calculated by $\textbf{T}_{wc} = \textbf{T}_{wi} * \textbf{T}_{ic}$. We denote Lie algebra elements as $\bm{\hat{\xi}} \in \mathfrak{se}\left(3\right)$ where $\bm{\xi} \in  \mathbb{R}^{6}$, exponential and logarithmic map associate $\textbf{SE}\left(3\right)$ and $\mathfrak{se}\left(3\right)$.
\begin{figure}[t]
    \vspace{2mm}
	\centering
	\includegraphics[width=0.8\linewidth]{image/point_planar_nonplanar.pdf}
	\vspace{-5mm}
	\caption{The coplanar regularities of pixels in the image are detected, and the coplanar points are on the planar region $\bm{\pi}_i$. The planar parameters are expressed in the world frame with a normal vector $\bm{n}_i$ and a distance $d_i$. }
	\label{fig:point_planar_nonplanar}
	\vspace{-3mm}
\end{figure}
\subsection{Point Representation} 
The pixels extracted are expressed by the inverse depth $d_p\in \mathbb{R}$ in the host image frame. In order to construct the photometric error cost function, we need to transform the pixel in the host image frame $I_h$ into the target image frame $I_t$. Assuming $\bm{p}$ is the pixel in the $I_h$ and it is observed at $\bm{p}'$ in the $I_t$, the relationship of the pixel is: 
\begin{flalign} 
	\begin{array}{c}
	\bm{p'} = \Pi_c\left(\bm{R}_{th}\Pi_c^{-1}\left(\bm{p}, d_p\right) + \bm{t}_{th}\right)
	\end{array}\label{fla: point transform}
\end{flalign} 
where $\bm{R}_{th}$ and $\bm{t}_{th}$ are the relative rotation and translation from $I_h$ to $I_t$, $\Pi_c$ and $\Pi_c^{-1}$ are the projecton and back projection of the camera.
\subsection{Plane Representation}
As is shown in Fig.\ref{fig:point_planar_nonplanar}, a plane in the world frame can be represented by $\bm{\pi} =\begin{bmatrix}\bm{n}^{\rm T}, d^{\rm T} \end{bmatrix}^{\rm T} $ where $\bm{n} \in \mathbb{R}^3$ is the normal vector of plane and  $d \in \mathbb{R}$ is the distance from origin of the world frame to the plane. The normal vector $\bm{n}$ has three parameters but only two Degrees of Freedom (DoF) with $||\bm{n}||_2 = 1$. 

To get a minimal parameterization of $\bm{\pi}$ for optimization, we specialize the parameters of a general plane into the vertical plane $^v\bm{\pi}$ that the normal vector is perpendicular to the gravity direction or the horizontal plane $^h\bm{\pi}$ that the normal vector is parallel for the gravity direction. For horizontal plane, we fix the normal vector $^h\bm{n}$ to  $\begin{bmatrix} 0, 0, 1 \end{bmatrix}^{\rm T} $, and only need to optimize the distance $^h d$. For vertical plane, since the normal vector is perpendicular to  the gravity direction, it has only one DoF, we represent it by the form
 \begin{flalign} 
	^v\bm{\pi}= \left( cos(\psi)cos(\phi), cos(\psi) sin(\phi), sin(\psi) , d \right)^{\rm T}
\end{flalign}
where $\phi$ and  $\psi$ are the azimuth and elevation angles of the normal vector and $d$ is the distance from the origin of the world frame. As shown in Fig. \ref{fig:plane_parameter}, we fix the elevation angle $\psi$  to zero and only optimize the azimuth angle $\phi$ and the distance $d$. 

The transformation of the plane parameters from world frame to camera frame has the form: 
\begin{flalign} 
	\bm{\pi}{_c}= \textbf{T}_{cw}^{-\rm T}\bm{\pi}{_w}
	\label{fla: transformation matrix of plane}
\end{flalign}
Using (\ref{fla: transformation matrix of plane}), we can convert the plane of world frame and construct the photometric error function about plane parameters in the camera frame.
\begin{figure}[t]
	\centering
	\includegraphics[width=0.55\linewidth]{image/plane_parameter.pdf}
	\vspace{-5mm}
	\caption{ The 2 DoF parameter representation of the normal vector $\bm{n}$.}
	\label{fig:plane_parameter}
	\vspace{-3mm}
\end{figure}
\section{VIO WITH COPLANAR REGULARITIES } \label{sec: direct sparse visual-inertial odometry}
{In this section, the method of coplanarity detection is firstly described. And then, we will fuse the inertial constraint and coplanar constraint into the non-linear optimization framework of \cite{engel2017direct} to estimate the body state and 3D landmarks. Meanwhile, in the optimization, we use the coplanar parameter expression to construct the photometric error, which not only guarantees accuracy but also reduces the number of optimization state variables. When the coplanar point-anchored frame is marginalized, we convert these constraints into plane distance-costs as the plane prior.
Finally, since the minimized energy functional is highly non-convex, the system proposed in this paper adopts dynamic initialization, a good initial value ensures the robustness of the system initialization in complex environments.}

We optimize all the state variables in the sliding window by minimizing the sum of the energy function from visual residual, IMU residual, and prior residual:
\begin{flalign} 
E_{total} = \lambda \cdot E_{point} + \lambda \cdot E_{plane} + E_{inertial} \label{fla: total cost function} + E_{prior}
\end{flalign}
where $E_{point}$ is the photometric error of non-coplanar points (section \ref{alg: photometric error of non-coplanar point}), $E_{plane}$ is the photometric error of coplanar points (section \ref{alg: photometric error of coplanar point}), $E_{inertial}$ is the the inertial error term  (section \ref{alg: photometric error of inertial error}), and $E_{prior}$ is the prior from marginalization operator (section \ref{alg: Marginalization with coplanar constraint}), respectively. $\lambda$ is the weight between visual photometric error and inertial error.

\subsection {Coplanarity Detection}\label{alg: coplanarity detection}
In this paper, we detect plane information from the 3D mesh. Since it is challenging to construct 3D mesh from 3D landmarks directly, we build 2D Delaunay triangulation \cite{hosseinzadeh2017sparse} in the image and project the triangular regularities to 3D landmarks. The direct method does not compute the matching relationship of points, making organizing the same triangular patch observations in different image frames difficult. Therefore, we generate 2D Delaunay triangulation in {the depth map of DSO \cite{engel2017direct}, which spans multiple image frames. In this way, the 3D mesh is anchored to the time frame of the sliding window, limiting memory usage.}


{For plane detection, we only detect the planes that are either vertical (i.e., walls, the normal is perpendicular to the gravity direction) or horizontal (i.e., floor, the normal is parallel for the gravity direction), which are commonly found in man-made environments.} For horizontal plane detection, we build a 1D histogram of the average height of triangular patches that the normal vector parallel for the gravity direction, and then the local maximums of the histogram are extracted after a Gaussian filter, as described in \cite{rosinol2019incremental}. We consider the local maximum to be the horizontal plane when it exceeds a certain threshold ($\sigma_t = 20$). For vertical plane detection, a 2D histogram is built,  one axis is the azimuth of the plane's normal vector, the other axis is the distance from the origin of the plane, we only count the triangular patches that the normal vector is perpendicular to the gravity direction, after statistics, we select the vertical planes in a similar way of horizontal plane detection.

We maintain all historical planes in the system. Therefore, for the newly detected planes, we need to detect whether the historical plane already exists according to its parameters and merge the similar planes when the angle and distance parameters are less than a certain threshold. Meanwhile, the coplanar points associated with the latest plane are also transferred to the historical plane, for efficiency, we only add the coplanar constraints detected from the current local 3D mesh into the sliding window. In the marginalization, the coplanar constraints on the plane parameters are removed and converted to the plane-distance cost, which contains the information of historical plane and is used as the prior in the  subsequent optimization. 

\subsection{Photometric Error of Non-coplanar Point } \label{alg: photometric error of non-coplanar point}
 The direct method is based on the  photometric invariance hypothesis to minimize the photometric error. In a similar way as \cite{engel2017direct}, the photometric error for a point $\bm{p}$ with inverse depth $d_{\bm{p}}$ in the host image  $I_h$ observed by the target image $I_t$ is defined as:
 \begin{flalign} 
  E_{p_j} = \sum\limits_{\bm{p} \in \mathcal{N}_{\bm{p}}} w_{\bm{p}} \|
  \left( I_t\left[\bm{p'}\right] - b_t\right) - \frac{t_t e^{a_t}}{t_he^{a_h}}\left(I_h\left[\bm{p}\right]-b_h\right) \|_{\gamma} \label{fla: photometric error}
 \end{flalign}
where $t_h$, $t_t$ are the exposure times of the respective image $I_h$ and $I_t$, $a_h$, $a_t$, $b_h$ and $b_t$ are the affine illumination  transform parameters, $\mathcal{N}_p$ is a small set of pixels around the point $\bm{p}$, 
$w_{\bm{p}}$ is a gradient-dependent weighting and $\| \cdot\|_{\gamma}$ is the Huber norm, $\bm{p}'$ is the point projected into $I_t$ and the relationship between $\bm{p}$ and $\bm{p}'$ is given by (\ref{fla: point transform}). 

So the sum of the photometric error of non-coplanar points  observed by all the keyframes in the sliding window is defined as:
 \begin{flalign} 
	E_{point} = \sum\limits_{i \in \mathcal{F}}  \sum\limits_{\bm{p} \in \mathcal{P}_i} \sum\limits_{j \in obs(\bm{p})} E_{\bm{p}_j}  
\end{flalign}
where $\mathcal{F}$ is the keyframes in the sliding window, $\mathcal{P}_i$ is the set of non-coplanar points in the host keyframe $i$, $obs(\bm{p})$ is a set of observations of the $\bm{p}$ in the other keyframes. 
\begin{figure}[t]
	\centering
	\includegraphics[width=1\linewidth]{image/hessian_matrix_of_coplanar_points.pdf}
	\vspace{-5mm}
	\caption{The Hessian matrix of the photometric error. (a) 2 coplanar points and 2 non-coplanar points are observed by camera $O_1$ and camera $O_2$, the coplanar points lie on the plane $\bm{\pi}$; (b) the traditional cost function of coplanar points needs to optimize the inverse depth; (c) the cost function of this paper only optimizes the parameters of plane.}
	\label{fig:hessian matrix of coplanar points}
	\vspace{-3mm}
\end{figure}
\subsection{Photometric Error of Coplanar Point } \label{alg: photometric error of coplanar point}
The plane $\bm \pi_w$ detected from the 3D mesh is  expressed in the world frame, in order to construct the photometric error between the host image and the target image, we convert the $\bm \pi_w$ into the host image frame to get  $\bm \pi_c$ using (\ref{fla: transformation matrix of plane}).
When we detect the point $\bm p_c $ in the image that the corresponding 3D point $\bm{p}_{\bm{\pi}_c}$ lies on the plane $\bm{\pi}_c$ , the point $\bm p_c$ satisfies the coplanar equation
\begin{flalign} 
z \cdot \bm{n}_c^{\rm T} \Pi_c^{-1}(\bm{p}_c, 1) + d_c = 0
\end{flalign}   
where $z$ is the depth from the origin of the camera frame to the 3D point of $\bm p_c$, $\bm n_c$ is the normal vector of $\bm \pi_c$, and $d_c$ is the distance of $\bm \pi_c$.
With the depth $z$, we can rewrite (\ref{fla: point transform}) as (\ref{fla: coplanar_point transform}) and construct the photometric error $E_{\bm p_c}$ of coplanar point by (\ref{fla: photometric error})
\begin{flalign} 
	\begin{array}{c}
		\bm{p_c}' = \Pi_c\left(\bm{R}_{th}\Pi_c^{-1}\left(\bm{p}_c, 1/z\right) + \bm{t}_{th}\right)
	\end{array}\label{fla: coplanar_point transform}
\end{flalign} 
The sum of the photometric error of coplanar points observed by all the keyframes in the sliding window is:
 \begin{flalign} 
	E_{plane} = \sum\limits_{i \in \mathcal{F}}  \sum\limits_{\bm{p} \in \mathcal{C}_i} \sum\limits_{j \in obs(\bm{p})} E_{\bm{p}_c}  
\end{flalign}
where $\mathcal{C}_i$ is the set of coplanar points in the host keyframe $i$. For a single residual, the jacobian corresponding to the camera frame can be decomposed as \cite{engel2017direct}:
\begin{flalign}
\bm{J}_k = [\bm{J}_I \cdot \bm{J}_{geo}, \bm{J}_{photo}]
\end{flalign}
where $\bm{J}_I$ is the image gradient, $\bm{J}_{geo}$ is the jacobian of geometric parameters ($\textbf{T}_h$, $\textbf{T}_t$), $\bm{J}_{photo}$ is the jacobian of photometric parameters ($a_h$, $a_t$, $b_h$, $b_t$). Specially, we note the jacobian terms in $\bm{J}_k$ that  related to the host image frame as $\bm{J}_h$, the jacobian terms in $\bm{J}_k$ that related to the target image frame as $\bm{J}_t$, the jacobian of $\bm{\pi_{w}}$ as $\bm{J}_{\pi}$ so the jacobian of the coplanar point on the plane can be written as :
\begin{flalign}
\bm{J}_{coplanar} = [\bm{J}_h, \bm{J}_t, \bm{J}_{\pi}]
\end{flalign}

we only need to construct the Hessian matrix of the host image frame $\bm{H}_{h}$, target image frame $\bm{H}_{t}$ and  plane parameters $\bm{H}_{\pi}$, not the inverse depth of coplanar points, as is shown in Fig.\ref{fig:hessian matrix of coplanar points}, in this way, the number of state variables added to the optimizer can be effectively reduced, which limits the computation cost of the optimization.
\subsection{Inertial Error}  \label{alg: photometric error of inertial error}
To construct the inertial error with angular velocity and linear acceleration, we use the pre-integration method proposed in \cite{forster2015imu} to handle the high frequencies of IMU measurements. 
This gives a pseudo-measured value of IMU between consecutive keyframes. 
Similar to \cite{forster2015imu}, the pre-integration IMU factors we build include relative rotation and translation, velocity, and gyroscope and accelerometer biases. The biases are fixed in each single pre-integration block.





\subsection{Marginalization with Coplanar Constraints}\label{alg: Marginalization with coplanar constraint}
{We utilize the bundle adjustment to optimize the state variables in the sliding window. When the new image frame is added into the sliding window, all the variables corresponding to the older frame are marginalized using the Schur complement.
For the state variable of the plane, to avoid maintaining too many historical planes in the sliding window, we also need to "marginalize" them out.
Since the removed planes may be added into the optimizer again later, we convert the coplanar constraints related to the plane into the plane-distance cost,  which is regarded as the plane prior to enforcing coplanar constraints. In this way, we do not need to increase much computation cost to generate the plane prior.}





\subsubsection{Marginalization of non-coplanar and inertial constraints}
When the number of keyframes in the sliding window is greater than a certain threshold, we select the marginal keyframe similar to the criteria in \cite{engel2017direct}, which considers the luminance change of the images and the geometry distribution of the poses. To maintain the consistency of the system, once the variable is connected to the marginalization factor, we use the First Jacobian Estimation (FEJ) \cite{huang2009first} to fix the variable's linearization point at the same value.



\subsubsection{Plane-distance cost for coplanar constraints}

For coplanar points, instead of using the photometric error term as the prior, we consider the coplanar points as a whole plane and directly use the plane-distance cost as the prior constraint. The plane-distance cost can be expressed by the distance from the prior plane $\bm{\pi}'= (\phi',  \psi', d')$ to the current plane $\bm{\pi} = ( \phi ,  \psi , d)$:
\begin{flalign}
	 E_{\bm{\pi}_p} = w_n  \|\left[ \phi', \psi', d' \right]^{\rm T}- \left[ \phi ,  \psi , d \right]^{\rm T}\|_{\sum_{\bm{\pi}}}^2
\end{flalign}
where $\sum_{\bm{\pi}}$ is the covariance matrix of constraint, $w_n$ is the number of the points on the plane.


\subsection{Initialization and Observability}
{Compared to pure visual odometry, visual-inertial odometry fusing IMU data makes metric scale, roll, and pitch observable.
This means that the state variables must be properly initialized. Otherwise, the optimization may diverge, leading to system failure.
Initialization can be divided into dynamic initialization and static initialization.
For the problem of insufficient visual parallax during static initialization, we use dynamic initialization in the system.
Optical flow tracking is introduced in the initialization, which improves the robustness of VIO initialization based on the direct method.
Similar to \cite{VINS}, we run SFM (Structure From Motion) to compute the rough pose of the frame, which is then aligned with the state variables obtained through the IMU integration for joint initialization.}



	
	
\begin{table*}[htbp]
	\label{tab:1}
	\begin{center}
	\vspace{5mm}
		\caption{The trajectory error (m) of different methods on EuRoC dataset. In \textbf{bold} the best of RMSE gt-scaled results, in \textcolor{blue}{blue} the best of RMSE results. }
		\label{tab:ATE Trans of EuRoC}
		\centering
		\setlength{\tabcolsep}{2.0mm}{
			\begin{tabular}{cc|ccccccccccc}
				\toprule
				Sequence& & MH1& MH2&MH3&MH4&MH5&V11&V12&V13&V21&V22&V23  \\\midrule
				\multirow{3}{*}{\makecell[c]{VI-DSO \\ (raw data from \cite{von2018direct})} } &RMSE& 0.062  & \textcolor{blue}{0.044}& 0.117  & 0.132 & 0.121 & 0.059 & \textcolor{blue}{0.067} & 0.096 &\textcolor{blue}{0.040} & 0.062 & 0.174 \\
				&RMSE gt-scaled& 0.041 &\bf 0.041  & 0.116 & 0.129 & \bf 0.106 & 0.057 &\bf 0.066  & 0.095 & \bf 0.031 & 0.060 & 0.173 \\
				&Scale Error(\%) &1.1  & 0.5 & 0.4 & \bf 0.2 & 0.8 & 1.1 & \bf 1.1 & \bf 0.8 & 1.2 & \bf 0.3 & 0.4 \\\midrule
				\multirow{3}{*}{our method no plane}  & RMSE  & 0.066 & 0.057 & 0.065& 0.110 & 0.117& 0.057 & 0.090 & 0.092 & 0.045 & 0.060 & 0.123\\
				& RMSE gt-scaled  & 0.052 & 0.056 & 0.062 & 0.099 & \bf 0.106 & 0.054 & 0.087 & 0.089 & 0.045 & 0.056 & 0.123 \\
				& Scale Error(\%)  & 0.9 & \bf 0.2 & 0.5 & 0.7 & 0.7 & 0.9 & 1.3 & 1.5 & \bf 0.3 & 1.0 & \bf 0.3 \\ \midrule
				\multirow{3}{*}{our method with plane}  &RMSE   &\textcolor{blue}{0.051} & 0.051 &  \textcolor{blue}{0.057} & \textcolor{blue}{0.103} & \textcolor{blue}{0.115} & \textcolor{blue}{0.051} & 0.082 & \textcolor{blue}{0.082} & 0.043  &\textcolor{blue}{0.048} & \textcolor{blue}{0.111} \\
				&RMSE gt-scaled   & \bf 0.039  & 0.048 & \bf 0.055 & \bf 0.087 & 0.109 & \bf 0.049 & 0.079 & \bf 0.078 & 0.042 & \bf 0.044 & \bf 0.110 \\
				& Scale Error(\%)   & \bf 0.8 & 0.3 & \bf 0.3 & 0.8 & \bf 0.5 & \bf 0.7 & \bf 1.1 & 1.6 & 0.5  & 0.9 & \bf 0.3 \\ \midrule
				\makecell[c]{ mesh-VIO}& RMSE  & 0.145 & 0.130 & 0.212 & 0.217 & 0.226 & 0.057 &0.074  & 0.167 & 0.081 & 0.103 & 0.272 \\ \midrule
				\makecell[c]{ PVIO}& RMSE  & 0.163 & 0.111 & 0.119 & 0.353 & 0.225 & 0.082 &0.113  & 0.201 & 0.063 & 0.157 & 0.280 \\ \midrule
			\end{tabular}
		}
	\end{center}
	\vspace{-5mm}
\end{table*}
\section{Experiments} 
\label{sec:experiment}
We evaluate our system on the publicly available EuRoC MAV dataset \cite{Burri2016} and TUM VI dataset \cite{Schubert2018}. Besides, we provide a video
in the supplementary to reflect the results of the experiment more intuitively. We run the system in the environment with Intel Core i7-9750H@ 2.6GHz, 16GB memory.
\subsection{Quantitative Evaluation}
To verify the performance of the system, we compared the accuracy of our system with VI-DSO \cite{von2018direct}, DSO \cite{engel2017direct}, ORBSLAM3 without loop closure \cite{ORBSLAM3}, OKVIS \cite{Leutenegger2014}, PVIO \cite{li2019robust} and mesh-VIO \cite{rosinol2019incremental}.  VI-DSO and DSO are the typical slam based on the direct method, ORBSLAM3 and OKVIS are state-of-art with indirect method, mesh-VIO and PVIO use the plane information in the VIO system,  through comparison, the system performance can be fully reflected. {The accuracy of the trajectory is measured by aligning the estimated trajectory with groundtruth, we calculated the RMSE of the trajectory error with SE3 alignment and Sim(3) alignment (with the description "RMSE gt-scaled"). Scale error is computed using scale $s$ from Sim(3) alignment, as $| 1-s|$.} 
\subsubsection{The EuRoC Dataset}
The EuRoC micro aerial vehicle (MAV) dataset consists of two scenes, the machine hall and the ordinary room, 
{which contain scenes with different scales and complexity. We compared our method with VI-DSO, mesh-VIO, and PVIO. Meanwhile, the ablation experiments are also conducted, as shown in Tab.\ref{tab:ATE Trans of EuRoC}. 
Whether from the results of RMSE or RMSE gt-scaled, PVI-DSO achieves the smallest translation error on most of the sequences, which means using the planar information in the direct method VIO can improve the accuracy of pose estimation. 
Especially, by comparing the experiment results of VI-DSO and our method without plane information, the positioning accuracy is basically the same. 
However after introducing the coplanar constraints into VIO, the error of RMSE was reduced by an average of 11\%, the error of RMSE gt-scaled reduced by an average of 12\%.
We can see that the structure information in the environment can help improve the accuracy and maintain the global scale estimation, which is also proved by the results of scale error.
Meanwhile, from the experimental results of PVIO and mesh-VIO, since the direct method can obtain more visual measurement compared with the indirect method, our method thereby improves the estimation of the pose.
}
\begin{figure}[b]
	\centering
	\vspace{-2mm}
	\includegraphics[scale=0.16]{image/color_map}
	\caption{Translation error of RMSE gt-scaled for diffferent methods run 11 times (rows) on each corridor sequence (colums) of TUM VI dataset.}
	\label{fig:color map of tum vi}
\end{figure}
\begin{table}[b!]
	\label{tab:1}
	\begin{center}
		\caption{The RMSE gt-scaled of the state-of-art methods compare to our method on TUM VI dataset. The translation (m) and rotation (rad) error are list as follows. In \textbf{bold} the best result}
		\label{tab:ATE Trans and Rots on TUM VI}
		\centering
		\setlength{\tabcolsep}{1.3mm}{
			\renewcommand{\arraystretch}{1.1}
			\begin{tabular}{ccccccccc}
				\hline
				\multirow{2}{*}{Seq.} & \multicolumn{2}{c}{ORBSLAM3} & \multicolumn{2}{c}{DSO} & \multicolumn{2}{c}{OKVIS} & \multicolumn{2}{c}{ours} \\ \cline{2-9} 
				& trans.  & rot.  & trans. & rot. & trans. & rot.  & trans. & rot.  \\ \hline
				corridor1  &0.230&0.216&0.747&0.968&0.451&0.800 &\bf0.114&\bf0.129           \\
				corridor2  &\bf0.052&\bf0.058&0.783&1.318&0.478&0.528 &0.379&0.421           \\
				corridor3  &0.169&0.127&0.924&2.161&0.485&1.033 &\bf0.159&\bf0.123           \\
				corridor4  &0.414&0.510&0.305&0.150& 0.300&0.359&\bf0.118&\bf0.145           \\
				corridor5  &0.186&0.034& 0.823&2.000&0.415&0.613&\bf 0.098&\bf0.013           \\ \hline
			\end{tabular}
		}
	\end{center}
\end{table}
\subsubsection{The TUM VI Dataset} 
We also evaluate our proposed system on the corridor sequences of the TUM VI benchmark dataset, which is collected in a long corridor and several offices and contains many images with blurring and lighting changes due to high-speed motion. We independently ran the algorithm 11 times on all sequences and calculated RMSE gt-scaled. Fig. \ref{fig:color map of tum vi} shows the different performance results of different algorithms running 11 times. Tab. \ref{tab:ATE Trans and Rots on TUM VI} shows the median translation and rotation error of the 11 results. {Compared to other algorithms, our method achieves the highest accuracy on almost all the sequences except for the corridor2 sequence, on which ORBSLAM3 performs better, this demonstrates the effectiveness of IMU measurements and geometric coplanarity information for improving localization accuracy. The VIO based on the direct method can build a more dense map, as shown in Fig. \ref{fig:map of tum vi}, the detailed structure of the indoor environment constructs by our method makes it easy to detect the plane information.}
\begin{figure}[b!]
	\centering
	\vspace{-4mm}
	\includegraphics[scale=0.21]{image/tumvi_map}
	\caption{
	Map of corridor3 sequence generated by our method. (a) is the 3D coplanar points of the vertical planes and the horizontal planes in the map. (b) is the reconstructed map, two sub-images show the 2D Delaunay triangulation and the corresponding 3D mesh of the scene, the pink points in the image are the detected 2D coplanar points. }
	\label{fig:map of tum vi}
	\vspace{-3mm}
\end{figure}
\subsection{ Weight Determination of Photometric Error}
Since the camera's photometric calibration and distortion correction have significant influences on the direct VIO methods, the weight ratio of photometric and inertial errors is often difficult to determine. In this paper, we use the method of parametric study to obtain the standard deviation of the photometric error to determine the weight, {for the EuRoC MAV dataset and TUM VI dataset, we drew the cumulative error curve of pose estimation in Fig.\ref{fig:v2_02_standard_derivation} after setting different standard deviations.} The results in Fig.\ref{fig:v2_02_standard_derivation} (a) show that setting the standard deviation of the photometric error to 11 performs best in the EuRoC dataset, as shown in Fig.\ref{fig:v2_02_standard_derivation} (b), for the TUM VI dataset, a standard deviation of 16 is optimal.

\begin{figure}[h!]
	\centering
	\vspace{2mm}
	\includegraphics[scale=0.17]{image/parameter_study}
	\caption{Cumulative error plot on the V22 sequence of EuRoC dataset (a) and corridor4 sequence of TUM VI dataset (b). Different standard deviation of photometric error affects the accuray and robustness of pose estimation.}
	\label{fig:v2_02_standard_derivation}
	\vspace{-3mm}
\end{figure}

\subsection{Effect of Plane Prior}
To verify the effectiveness of our proposed plane-distance prior, we test the plane parameters  consistent on the EuRoC dataset V11 sequence. As shown in Fig. \ref{fig:curve of plane prior}: 
We can observe that the distance estimate for the plane varies greatly without the prior, while the estimation remains stable after adding the plane prior.
The planar prior contains historical information constraints that can provide constraints on the pose in the sliding window. Meanwhile, the range of curve variation without prior information also indicates that the elevation of ground points in the reconstructed map changes from -0.98m to -0.86m.





\begin{figure}[t]
	\centering
	\includegraphics[scale=0.18]{image/V101_plane_prior}
	\caption{
	The distance estimation of the horizontal plane in V11 sequence of the EuRoC dataset, we count the keyframes in which planar features are observed.}
	\label{fig:curve of plane prior}
	\vspace{-3mm}
\end{figure}
\subsection{Runtime Evaluation}
Tab.\ref{tab:running time} shows the run-time ablation experiments for systems containing different modules.
We measure the mean execution time of different modules of the system using pure VO, VIO, and our proposed PVI-DSO on EuRoC's V11 sequence, respectively. Considering the real-time  performance of the system, we set the number of extracted pixels in the keyframe to 800 and the size of  the sliding window to 7 according to \cite{engel2017direct}. In the pure VO method, two-thirds of the time is spent on optimization, and the rest is spent on tracking pixels, selecting candidate, activation points, and extracting pixels, etc. After adding inertia constraints to become a VIO system, there is only a slight time increase in IMU information processing and optimization.
For our proposed PVI-DSO, the system adds a plane detection part and a mesh generation part, but both parts are very efficient and only need 0.72ms and 1.08ms, respectively.
Furthermore, compared with VIO, the time of marginalization is reduced from 0.79ms to 0.72ms, the time of problem solving (compute x from $\rm Ax = b$) and state variables updating ($\rm x = x + \delta x$) is also reduced from 2.41ms to 2.16 ms, due to the novel parametric method adopted by the system to reduce the size of parameters. 
The optimization time is slightly increased because the coplanar parameters are more complicated to calculate the photometric error and Jacobian. In the sliding window, the average number of the state variables (plane number $\approx$ 2, point number $\approx$ 1182) for PVI-DSO is fewer than that (point number $\approx$ 1422) of the VIO, proving that the coplanar parameter dramatically reduces the number of optimized state variables.

\begin{comment}


\begin{table}[htbp]
	\label{tab:3}
	\begin{center}
		\caption{Mean execution time (Unit: millisecond) of pure VO, VIO, and our proposed PVI-DSO running on the sequence V11.}
		\label{tab:running time}
		\centering
		\setlength{\tabcolsep}{2.9mm}{
			\renewcommand{\arraystretch}{1.2}
			\begin{tabular}{cccc}
			\hline
			\multicolumn{1}{c}{Module} & \multicolumn{1}{c}{VO} & \multicolumn{1}{c}{VIO} & \multicolumn{1}{c}{PVI-DSO} \\ \hline
			\texttt{Plane detection } &      0                  &    0                   &  0.72                     \\
			\texttt{mesh Creation}    &       0                 &    0                    &  1.08                   \\
			\texttt{Optimization}      &    19.89            &    20.46                      &    21.65                   \\
			\texttt{Construct Cost Function}      &   16.44            &    16.99                      &    17.74                   \\
			\texttt{Solve Equation update increament}      &    2.39            &    2.41                      &    2.16                   \\
			\texttt{Marginalization}&     0.77               &     0.79                  &   0.72                    \\ \hline
			\texttt{Total}                        &   31.97                &    32.54                &   35.03                    \\ \hline
		\end{tabular}
		}
	\end{center}
\end{table}

\end{comment}

\begin{table}[htbp]
	\label{tab:3}
	\begin{center}
		\caption{Mean execution time (Unit: millisecond) of pure VO, VIO, and our proposed PVI-DSO running on the sequence V11.}
		\label{tab:running time}
		\centering
		\setlength{\tabcolsep}{1.3mm}{
			\renewcommand{\arraystretch}{1.2}
			\begin{threeparttable}
			\begin{tabular}{cccc}
			\hline
			\multicolumn{1}{c}{Module} & \multicolumn{1}{c}{VO} & \multicolumn{1}{c}{VIO} & \multicolumn{1}{c}{PVI-DSO} \\ \hline
			\texttt{Plane detection } &      0                  &    0                   &  0.72                     \\
			\texttt{mesh Creation}    &       0                 &    0                    &  1.08                   \\
			\texttt{ Cost Function Construction\tnote{1}}      &   16.44            &    16.99                      &    17.74                   \\
			\texttt{\makecell[c]{Problem Solving\&Var Updating\tnote{1}}}      &    2.39            &    2.41                      &    2.16                   \\
			\texttt{Marginalization}&     0.77               &     0.79                  &   0.72                    \\ \hline
			\texttt{Total}                        &   31.97                &    32.54                &   34.58                    \\ \hline
		\end{tabular}
		\begin{tablenotes}   
        \footnotesize              
        \item[1] The optimization contains cost function construction, problem solving, and state variables updating.          
         \end{tablenotes}           
         \end{threeparttable}      
		}
	\end{center}
\end{table}
\section{Conclusion} 
\label{sec:conclusion}
In this paper, we present a novel VIO system based on the direct method, as well as leveraging the planar regularities, which is called PVI-DSO. The VIO based on the direct method constructs a more dense map, which contains rich geometric features. The plane regularities in the map are extracted by generating the 3D mesh, and a novel parameterization for co-planar points is used to introduce the plane constraints into the VIO system, meanwhile, the plane-distance cost used in the optimization converts the coplanar constraints into the plane prior,  by these methods, introducing the planar information into the system doesn't increase much computational burden. The experiments show that the trajectory accuracy of our approach is better than the state-of-the-art visual-inertial odometry.  In the future, we will fuse line and plane geometric information in the VIO based on the direct method to further improve the accuracy and robustness of positioning. 
\section*{ACKNOWLEDGMENT}
We would like to thank Dr. Yijia He for the helpful discussion. This work was supported by the Foundation for Innovative Research Groups at the National Natural Science Foundation of China (Grant No. 41721003) and the fellowship of China National Postdoctoral Program for Innovative Talents (Grant No. BX20200251).

\begin{comment}

\section*{APPENDIX}


To simplify the formula, we define variables $nwr =  (
\bm{n}_w^{\rm T}  \bm{R}_{wh}) \cdot \bm{p}_c^n$ and $nwt = 
\bm{n}_w \cdot \bm{t}_{wh} + dw$ 
The Jacobian of host image frame is the form:
\begin{flalign} 
\bm{J}_{Th} = -\bm{R}_{th}  \bm{p}_c^n \cdot \bm{n}_w^{\rm T} / ((
\bm{n}_w^{\rm T}  \bm{R}_{wh}) \cdot {\bm{p}_c^n}) + \bm{R}_{wt}^{\rm T}
\end{flalign}

\begin{flalign} 
	\bm{J}_{rh} = -\bm{R}_{th}  \bm{p}_c^n \cdot \bm{n}_w^{\rm T} / ((
	\bm{n}_w^{\rm T}  \bm{R}_{wh}) \cdot {\bm{p}_c^n}) + \bm{R}_{wt}^{\rm T}
\end{flalign}


The jacobian of target image frame
\begin{flalign} 
E_{plane} = \sum\limits_{i \in \mathcal{F}}  \sum\limits_{\bm{p} \in \mathcal{P}_i} \sum\limits_{j \in obs(\bm{p})} E_{\bm{p}_c}  
\end{flalign}
The jacobian of ground
\begin{flalign} 
E_{plane} = \sum\limits_{i \in \mathcal{F}}  \sum\limits_{\bm{p} \in \mathcal{P}_i} \sum\limits_{j \in obs(\bm{p})} E_{\bm{p}_c}  
\end{flalign}
The jacobian of wall 
\begin{flalign} 
E_{plane} = \sum\limits_{i \in \mathcal{F}}  \sum\limits_{\bm{p} \in \mathcal{P}_i} \sum\limits_{j \in obs(\bm{p})} E_{\bm{p}_c}  
\end{flalign}

\end{comment}
	


	\balance
	
	\bibliographystyle{IEEEtran}
	