Visual Tilt Estimation for Planar-Motion Methods in Indoor Mobile Robots

Abstract

Visual methods have many applications in mobile robotics problems, such as localization, navigation, and mapping. Some methods require that the robot moves in a plane without tilting. This planar-motion assumption simplifies the problem, and can lead to improved results. However, tilting the robot violates this assumption, and may cause planar-motion methods to fail. Such a tilt should therefore be corrected. In this work, we estimate a robot’s tilt relative to a ground plane from individual panoramic images. This estimate is based on the vanishing point of vertical elements, which commonly occur in indoor environments. We test the quality of two methods on images from several environments: An image-space method exploits several approximations to detect the vanishing point in a panoramic fisheye image. The vector-consensus method uses a calibrated camera model to solve the tilt-estimation problem in 3D space. In addition, we measure the time required on desktop and embedded systems. We previously studied visual pose-estimation for a domestic robot, including the effect of tilts. We use these earlier results to establish meaningful standards for the estimation error and time. Overall, we find the methods to be accurate and fast enough for real-time use on embedded systems. However, the tilt-estimation error increases markedly in environments containing relatively few vertical edges.

1. Introduction

Visual methods operating on images from a robot’s onboard camera have many applications in mobile robotics. These include relative-pose estimation [1], visual odometry [2,3], place recognition [4], and simultaneous localization and mapping (SLAM) [5]. Some of these visual methods are based on a planar-motion assumption: under this assumption, the robot travels in a flat plane without pitching or rolling. As a result, the robot moves with only three instead of six degrees of freedom (DOF), as illustrated in Figure 1. This simplification is used, for example, in visual relative-pose estimation [6,7,8,9,10,11] or place recognition [12]. We previously compared visual relative-pose estimation methods in the context of a domestic cleaning robot [1]. We found that planar-motion methods [9,10] can be more accurate and faster than their nonplanar counterparts [13,14].

However, even in a benign indoor environment, uneven ground may cause the robot to pitch or roll. Tilting the robot in such a manner violates the planar-motion assumption. For our pose-estimation experiments, this introduced large errors in the results [1]. Here, even a small tilt of ≈2° eliminated the quality advantage of the planar-motion methods. Larger tilts then increased these errors beyond those of the nonplanar motions. Such a small angle can be caused even by a slight roughness of the movement surface (see Section 2.3). Booij et al. [11] encountered a similar effect while testing planar-motion pose-estimation methods. The authors suspected tilts from an accelerating robot as the cause.

To tackle this problem, we estimate the tilt angle $\alpha$ and tilt direction $\beta$ (Figure 1) with which an image was captured. A planar-motion method can then use this information to correct for tilts in the input image. The tilt could be measured using an additional sensor, such as an inertial measurement unit (IMU). However, this increases the cost and complexity of the robot, more so because the sensor needs to be calibrated [15] and synchronized relative to the camera. Instead, we estimate the tilt parameters $(\alpha ,\beta )$ from a single panoramic camera image by following the algorithm outlined in Figure 3. This estimate is based on vertical elements, which are commonly found in indoor environments. These elements are orthogonal to the floor, and thus orthogonal to the robot’s movement plane. Some of the elements will appear as edges in the camera image. Locating the vanishing point of these edges lets us determine the robot’s tilt. We evaluate our results in the context of the visual pose-estimation methods compared in [1]: the tilt-estimation accuracy should allow the planar-motion methods to remain competitive with their nonplanar counterparts. Furthermore, tilt estimation should add only a small overhead to the time required for planar-motion pose estimation.

1.1. Related Works

Visually estimating a camera’s orientation relative to the world is a prominent research problem. As in our work, vanishing points are widely used, and offer several advantages [16]: because they lie at infinity, their position is not affected by camera translation. Furthermore, the vanishing points are determined from an image’s edge pixels. Thus, many non-edge pixels can be disregarded, which speeds up processing time.

A popular class of methods assumes a Manhattan world, as described by Coughlan and Yuille [17]: here, image edges are assumed to belong to world elements that are either parallel or orthogonal to each other. These three orthogonal sets of parallel elements serve as the axes of a world coordinate system. One example for such a world is an environment consisting only of parallel cuboids.

Košecká and Zhang [18] present one early work based on the Manhattan world. The authors group edge pixels from nonpanoramic images into distinct straight lines. They then determine the Manhattan-world vanishing points from these lines using expectation maximization [19]. Denis et al. [20] compare several early Manhattan-world orientation-estimation methods. This work also introduces additional methods based on straight lines in nonpanoramic images. Tardif [21] uses the J-Linkage algorithm [22] to identify distinct vanishing points from straight image edges. This method then selects those three vanishing points that best correspond to the Manhattan directions. All of these works require straight image edges. For this reason, they are not directly applicable to our fisheye images, which contain strong radial distortions.

Bazin et al. [16] search panoramic images for edges belonging to straight elements in the world [23]. They then perform a coarse-fine search over the space of possible camera orientations. For a Manhattan world, each hypothetical orientation predicts a distinct set of vanishing points. This method selects the orientation under which the highest number of images edges are consistent with these predicted vanishing points. Another work by Bazin et al. [16] follows the same Manhattan-world approach. As before, orientations are scored by comparing their predicted vanishing points to the image edges. To find the best orientation, a branch-and-bound algorithm [24] divides the orientation space into intervals. Those intervals that contain only inferior solutions are discarded using interval analysis. The orientation estimate is then narrowed down by subdividing the remaining intervals.

In the Manhattan world, there are six (three orthogonal, plus their antipodals) vanishing points: one vertical pair, and two additional orthogonal pairs that lie on the horizon. Schindler and Dellaert [25] generalize this to an Atlanta world. In contrast to the Manhattan case, this world may contain additional vanishing point pairs located on the horizon. The authors estimate these vanishing points for nonpanoramic images using expectation maximization [19]. Tretyak et al. [26] also use an Atlanta-like world. They propose a scene model that encompasses lines, their vanishing points, and the resulting horizon and zenith location. The model parameters are then jointly optimized based on the edge pixels in a nonpanoramic image. Thus, the detection of lines, vanishing points, and the horizon and zenith are performed in a single step. However, these two methods assume images without radial distortion; thus, they cannot directly be used for our panoramic fisheye images.

Antone and Teller [27] also estimate camera orientations using vanishing points, but do not assume a specific world. Vanishing-point candidates are found by a Hough transform (survey: [28]), and then refined through expectation maximization [19]. The authors then identify a set of global vanishing points that appear across an entire collection of images. Camera orientations relative to these global vanishing points are then jointly estimated for all images. Lee and Yoon [29] also do not require a Manhattan or Atlanta world. Using an extended Kalman filter, they jointly estimate the vanishing points and camera orientations over a sequence. These two methods place no prior restrictions on the locations of vanishing points. Consequently, the alignment between the vanishing points and the robot’s movement plane is not known. To use them for tilt correction, this alignment must first be determined. Thus, these methods are not directly suitable to estimate the tilt from a single image.

The works discussed above estimate camera orientations relative to global vanishing points. Many other works determine the relative camera poses between two images [2,3]: one popular approach works by matching local visual features, such as those from the Scale-Invariant Feature Transform (SIFT) [30], between two images. The relative pose can then estimated from epipolar geometry [14]. However, these relative poses provide no information about the ground plane. Subsequently, they cannot be used to estimate tilts for planar-motion methods.

Some of the methods discussed here may also solve the tilt-estimation problem. For example, Bazin et al. [16] determine the camera orientation relative to a Manhattan world from a single panoramic image. However, we seek a solution that is optimized specifically towards tilt-estimation for planar-motion methods: within this problem, we need only the tilt angle and direction, and can ignore the robot’s yaw. These angles can be estimated from vertical elements alone, without requiring a full Manhattan or Atlanta world. Additionally, the tilt angle $\alpha$ is bound to be small, which allows further simplifications. By exploiting these properties, we can can achieve good and fast tilt estimates using considerably simpler methods.

1.2. Contributions of This Study

In this work, we use two different visual methods to determine a robot’s tilt relative to a ground plane. We evaluate and discuss these methods within the context of a domestic floor-cleaning robot. These robots are a popular application of mobile robotics, being extensively sold on the commercial market. Our research group also has a long-standing interest in this application [31,32,33]. Specifically, we have previously developed a prototype for an autonomous floor-cleaning robot, shown in Figure 2.

Our prototype uses a planar-motion visual pose-estimation method [10] for localization and mapping. This method accurately estimates the relative camera pose between two images using very few computational resources [1]. However, the robot may tilt when driving over uneven ground, such as carpets or door thresholds. This violates the planar-motion assumption, which degrades the pose-estimation quality [1]. In this work, we use these results as a guide for designing experiments (Section 2.3 and Section 2.4) or evaluating results (Section 4). We also use our robot prototype to record a plausible image database, as described in Section 2.3. However, the methods presented here are not limited to this specific scenario; they may be used with other robots or planar-motion methods.

Both methods in this work operate on panoramic fisheye images, as captured by the robot’s on-board camera (Figure 3). These images show the hemisphere above the robot, but exclude everything below the horizon. Thus, the camera cannot see the ground plane, relative to which the tilt should be measured. Instead, we use vertical elements in the environment, which are orthogonal to the movement plane. Examples include room corners, door- and window frames, as well as the edges of furniture such as shelves. Some of these parallel elements appear as visually distinct edges in the robot’s camera images.

We now estimate the tilt parameters from the vanishing point of these edges: first, we apply an edge filter to the camera image and extract pixels with a strong edge response. Second, we identify the set of edge pixels belonging to vertical elements in the world, while rejecting those from non-vertical elements. Here, we assume that the tilt angle is small and that vertical elements far outnumber the near-vertical ones. Third, we determine the tilt angle $\alpha$ and direction $\beta$ from the remaining edge pixels. Step one is identical for both methods, but steps two and three differ. The image-space method uses several approximations to simplify vanishing-point detection in the fisheye images. We then apply a correction factor to reduce the tilt-estimation errors introduced by these approximations. Operating directly on the panoramic fisheye image, this method is fast and simple to implement. In contrast, the vector-consensus method solves the tilt-estimation problem in 3D world coordinates. This method makes fewer approximations, but requires a calibrated camera model. It is also more similar to the existing vanishing-point methods discussed in Section 1.1.

We test the two methods on images recorded by the on-board camera of our cleaning-robot prototype. The robot was positioned at 43 locations spread across six different environments. At each location, we recorded images for the untilted and six different tilted configurations. The tilts estimated from these images were then compared to the ground truth, giving a tilt estimation error. We also measure the execution time required for tilt correction on a desktop and embedded system.

The rest of this work is structured as follows: Section 2.1 and Section 2.2 describe the two methods used in this work. We introduce the image database in Section 2.3, and the experiments in Section 2.4. Section 3 contains the results of these experiments, which we discuss in Section 4. Finally, we summarize our results and give an outlook to possible future developments in Section 5.

2. Materials and Methods

Our specific goal is to estimate the robot’s tilt angle $\alpha$ and tilt direction $\beta$, as shown in Figure 1. $\beta$ is given relative to the robot’s heading, with $\beta$ = 0° and $\beta$ = 90° describing a forward and leftward tilt, respectively. $\alpha$ is measured relative to the robot’s untilted pose on a planar surface, thus, for an untilted robot, we have $\alpha$ = 0°. We choose our world coordinate system so that $\overrightarrow{n}={(0,0,1)}^T$ is the surface normal of the movement plane. If ${\overrightarrow{n}}_R=R_{\alpha ,\beta }·\overrightarrow{n}={(n_{x,R},n_{y,R},n_{z,R})}^T$ is the normal vector in the coordinate system of a tilted robot, we then find that

$$\begin{array}{cc}\hfill \alpha & =\arccos \left(n_{z,R}\right),\hfill \\ \hfill \beta & =\mathrm{atan}2(-n_{y,R},-n_{x,R}).\hfill \end{array}$$

(1)

Here, $R_{\alpha ,\beta }$ is the rotation matrix for the tilt $(\alpha ,\beta )$, and atan2 is the quadrant-aware arctangent function. We can therefore determine $(\alpha ,\beta )$ by first determining ${\overrightarrow{n}}_R$. Both methods perform this step using visually distinct environment elements that are parallel to $\overrightarrow{n}$.

2.1. Image-Space Method

This method is based on the apparent location ${\overrightarrow{p}}_n$ of the vertical elements’ vanishing point. ${\overrightarrow{p}}_n$ is the point in the camera image where the vertical elements would appear to meet, if they were extended to infinity. We estimate ${\overrightarrow{p}}_n$ using several problem-specific approximations, which simplify and speed up the method. We then determine the tilt parameters $(\alpha ,\beta )$ using the location of ${\overrightarrow{p}}_n$.

2.1.1. Preliminary Calculations

We first determine the vanishing point for an untilted robot, which we call ${\overrightarrow{p}}_c$. For convenience, we find ${\overrightarrow{p}}_c$ using an existing camera calibration based on [34]. However, this point could also be determined separately, without requiring a fully calibrated camera. The calibrated camera model provides a projection function P. P maps the camera-relative bearing vector $\overrightarrow{v}$ to the image point $\overrightarrow{p}=P\left(\overrightarrow{v}\right)$. Multiplying with the extrinsic rotation matrix R gives the bearing vector ${\overrightarrow{v}}_R=R·\overrightarrow{v}$ in robot coordinates. By definition, the surface normal is ${\overrightarrow{n}}_{R,\alpha =0}=\overrightarrow{n}$ for an untilted robot. ${\overrightarrow{n}}_{R,\alpha =0}$ is parallel to the environment’s vertical elements, and consequently shares their vanishing point. Transforming ${\overrightarrow{n}}_{R,\alpha =0}$ into camera coordinates and projecting it gives us

$${\overrightarrow{p}}_c=P\left(R^{-1}·{\overrightarrow{n}}_{R,\alpha =0}\right).$$

(2)

If the robot is tilted, the apparent vanishing point ${\overrightarrow{p}}_n$ will be shifted from ${\overrightarrow{p}}_c$. We will determine the tilt parameters $(\alpha ,\beta )$ from this shift. Figure 4 illustrates ${\overrightarrow{p}}_n$ and ${\overrightarrow{p}}_c$ for an untilted example image.

Note that it is irrelevant whether or not the camera image actually shows the environment at or around ${\overrightarrow{p}}_c$. Thus, this method would still work for robots where this part of the field of view is obscured. For a robot with an upward-facing fisheye camera, ${\overrightarrow{p}}_c$ lies close to the geometric center of the image. However, this may not apply for an upward-facing camera that captures panoramic images using a mirror. Here, the image center corresponds to a direction below instead of above the robot. In this case, we can simply use the antipodal vanishing point of the vertical elements. This vanishing point lies below the robot, and thus we calculate ${\overrightarrow{p}}_c$ from ${\overrightarrow{n}}_{R,\alpha =0}=-\overrightarrow{n}$ instead. Subsequent steps in the method then use this antipodal vanishing point.

2.1.2. Edge Pixel Extraction

As a first step towards tilt estimation, we detect edges in the camera image. We apply the Scharr operator to compute the horizontal and vertical edge gradients $g_x$ and $g_y$ for each pixel. This edge detector is somewhat similar to the popular Sobel operator, but was optimized to be invariant to edge orientation [35,36]. Since vertical elements can appear at any orientation within the image, this is a highly desirable property. In this work, we use the implementation provided by the OpenCV library [37]. Figure 5 contains an example of the resulting edge gradients.

We ignore pixels below the camera’s horizon, since they mostly show the robot’s chassis. We also reject pixels with a bearing of more than 45° above the camera horizon. Such pixels mainly show the ceiling, which contains horizontal elements that may be mistaken for vertical ones. To speed up computations, we only apply the Scharr operator to a bounding box around the remaining pixels. Finally, we discard pixels with a low gradient intensity $g_x^2+g_y^2<I_{\min }^2$. Such pixels are of limited use to us, since camera noise may strongly disturb their edge gradients. Eliminating these pixels also speeds up subsequent processing steps.

We do not try to identify straight lines of edge pixels, instead considering each pixel individually. This simplifies and speeds up our method, and lets us extract information even from very short edges. In contrast, combining connected pixels into long edges (as in [23]) may reduce the number of incorrect edge pixels, and may make the edge-gradient estimate more accurate [20]. However, within the scope of this work, we do not evaluate the trade-off between using individual edge pixels or long edges.

2.1.3. Edge Pixel Processing

Next, we estimate the tilt from the edge pixels identified in Section 2.1.2. For a camera with linear projection, a linear element in the environment would appear as a straight edge in the image. We can derive the edge direction for each pixel from the gradient $\overrightarrow{g}={(g_x,g_y)}^T$. Extending each edge pixel along its edge direction result in a vanishing point, similar to Figure 4.

However, here we assume that the robot’s camera fits the model proposed by Scaramuzza et al. [34]; this panoramic camera model is not linear. Due to radial distortion, straight environment elements commonly appear as curves in the camera image. One example is given by the horizontal elements in Figure 4. However, for this tilt-estimation problem, we can use two simplifying approximations: first, we assume that vertical elements appear as straight edges in the panoramic image. This is approximately true for small tilt angles, as for example in Figure 6. Second, since $\alpha$ is small, we assume that a tilt appears as a shift in the image. The edges and their vanishing point thus appear to move by a uniform amount (Figure 7c). In reality, tilting changes the orientation of these edges in the image. Due to the radial distortions of our fisheye lens, the vertical elements may also appear as curves (Figure 7b). Under our approximations, we ignore these effects and determine only an approximated vanishing point ${\overrightarrow{p}}_a$ (Figure 6) instead of the actual vanishing point ${\overrightarrow{p}}_n$. This greatly simplifies our method, but also introduces tilt-estimation errors. We therefore apply a correction factor after estimating the tilt from the shift between ${\overrightarrow{p}}_a$ and ${\overrightarrow{p}}_c$.

Panoramic Projections of Vertical Elements

We use the camera model to justify our previous assumption that vertical elements are projected linearly. According to the projection P [34], a bearing vector $\overrightarrow{v}={(x,y,z)}^T$ and corresponding image point $\overrightarrow{p}={(u^{\prime },v^{\prime })}^T=P\left(\overrightarrow{v}\right)$ are related by

$$\begin{array}{}\left(3\right)& \hfill \overrightarrow{v}={(x,y,z)}^T& ={(u,v,f\left(\rho \right))}^T,\hfill \left(4\right)& \hfill \left(\begin{array}{c}{u}^{\prime }\\ v^{\prime }\end{array}\right)& =\left(\begin{array}{cc}c& d\\ e& 1\end{array}\right)\left(\begin{array}{c}u\\ v\end{array}\right)+\left(\begin{array}{c}{x}_c^{\prime }\\ y_c^{\prime }\end{array}\right).\hfill \end{array}$$

Here, $c,d,e,x_c^{\prime }$ and $y_c^{\prime }$ are camera parameters, while $\rho =\sqrt{u^2+v^2}$. While ${(u^{\prime },v^{\prime })}^T$ refers to the actual pixel coordinates in the image, ${(u,v)}^T$ is the corresponding point in an idealized sensor plane. This idealized sensor plane is orthogonal to the optical axis, which corresponds to the camera’s z-axis. Figure 8 illustrates this camera model.

The $n+1$ coefficients $a_k$ of the polynomial function

$$f\left(\rho \right)={\displaystyle \sum _{k=0}^n}{a}_k{\rho }^k$$

(5)

are also camera parameters determined during calibration; in our experiments, we use $n=5$. We now consider a straight line in space that is parallel to the camera’s optical axis. In camera coordinates, such a line consists of all points $\overrightarrow{X}$, which satisfy

$$\overrightarrow{X}={(X,Y,Z)}^T={(X_0,Y_0,Z_0+\mu )}^T,$$

(6)

where ${(X_0,Y_0,Z_0)}^T$ is a point on the line and $\mu \in \mathbb{R}$. The bearing vector from the projection center to $\overrightarrow{X}$ is simply $\overrightarrow{v}={\lambda }^{-1}\overrightarrow{X}$; here, ${\lambda }^{-1}\ge 0$ is the unknown distance factor. From Equation (3), we see that the sensor-plane projection of this line must fulfill

$${(u_{\mu },v_{\mu },f\left({\rho }_{\mu }\right))}^T={\lambda }^{-1}{(X,Y,Z)}^T={\lambda }^{-1}{(X_0,Y_0,Z_0+\mu )}^T.$$

(7)

We now express this projection in polar coordinates

$${(u_{\mu },v_{\mu })}^T={\rho }_{\mu }{(\cos \left(\gamma \right),\sin \left(\gamma \right))}^T$$

(8)

and, from Equation (7), find that

$$\gamma =\mathrm{atan}2(v_{\mu },u_{\mu })=\mathrm{atan}2({\lambda }^{-1}{Y}_0,{\lambda }^{-1}{X}_0)=\mathrm{atan}2(Y_0,X_0)$$

(9)

is constant. Since the orientation $\gamma$ for the projection of $\overrightarrow{X}$ is constant, the vertical element $\overrightarrow{X}$ appears as a straight line in the sensor plane. Because Equation (4) is linear, this also corresponds to a line in the final image.

From Equation (8), we note that the sensor-plane projections of all lines $\overrightarrow{X}$ intersect at the origin point ${(u_{\mu },v_{\mu })}^T=\overrightarrow{0}$ for ${\rho }_{\mu }=0$. At this origin point, we have $u_{\mu }=v_{\mu }=0$, and thus the distance factor from Equation (7) must be ${\lambda }^{-1}=0$ (except for lines with $X_0=Y_0=0$). We now wish to find the specific point along a line $\overrightarrow{X}$ that is projected to the sensor-plane origin. At this origin ${\rho }_{\mu }=0$, and we thus evaluate

$$\underset{{\rho }_{\mu }\to 0}{\lim }Z=\underset{{\rho }_{\mu }\to 0}{\lim }\frac{\stackrel{\to a_0}{\overbrace{f\left({\rho }_{\mu }\right)}}}{\underset{\to 0^{+}}{\underbrace{{\lambda }^{-1}}}}=\infty ·\mathrm{sgn}\left(a_0\right).$$

(10)

We can assume that $a_0\ne 0$, since Equation (3) would otherwise give a malformed bearing of $\overrightarrow{v}=\overrightarrow{0}$ for the origin ${(u,v)}^T=\overrightarrow{0}$.

We have now shown that lines parallel to the optical axis are projected linearly. Furthermore, these projections all meet at the vanishing point ${(u,v)}^T=\overrightarrow{0}$ as Z approaches infinity. As per Equation (4), this point corresponds to ${(u^{\prime },v^{\prime })}^T={(x_c^{\prime },y_c^{\prime })}^T$ in image coordinates. However, even for an untilted robot, the optical axis is usually not perfectly parallel to the vertical elements. Here, we assume that this misalignment is small for an upward-facing panoramic camera (≈0.16° in our cleaning-robot prototype, as calculated from the extrinsic calibration matrix R). We therefore choose to ignore this effect.

Approximating Tilts through Image Shifts

Next, we determine how tilting the robot affects the vanishing point of the vertical elements. A tilt is a rotation around an axis $\overrightarrow{w}={(w_x,w_y,0)}^T$ parallel to the movement plane, with $\parallel \overrightarrow{w}\parallel =1$. We derive the rotation matrix $S^{\prime }$ for such a tilt from by Rodrigues’ formula [38], with

$$S^{\prime }=\left(\begin{array}{ccc}\cos \left(\alpha \right)+w_x^2(1-\cos \left(\alpha \right))& w_x{w}_y(1-\cos \left(\alpha \right))& w_y\sin \left(\alpha \right)\\ w_y{w}_x(1-\cos \left(\alpha \right))& \cos \left(\alpha \right)+w_y^2(1-\cos \left(\alpha \right))& -w_x\sin \left(\alpha \right)\\ -w_y\sin \left(\alpha \right)& w_x\sin \left(\alpha \right)& \cos \left(\alpha \right)\end{array}\right).$$

(11)

For a small tilt angle $\alpha$, we approximate $S^{\prime }$ using $\cos \left(\alpha \right)\approx 1$ and $\sin \left(\alpha \right)\approx \alpha$, which gives us

$$S=\left(\begin{array}{ccc}1& 0& w_y\alpha \\ 0& 1& -w_x\alpha \\ -w_y\alpha & w_x\alpha & 1\end{array}\right).$$

(12)

As discussed above, we assume that the optical axis is parallel to the vertical elements for an untilted robot. Thus, these elements are parallel to ${\overrightarrow{n}}_C={(0,0,a_0)}^T$ in camera coordinates. After rotating the camera around $\overrightarrow{w}$ by a small angle $\alpha$, this is ${\overrightarrow{n}}_{C,t}\approx S^{-1}{\overrightarrow{n}}_C=a_0{(-w_y\alpha ,w_x\alpha ,1)}^T$. According to Equation (3), the sensor-plane projection $(u_t,v_t)$ of ${\overrightarrow{n}}_{C,t}$ obeys

$${(u_t,v_t,f\left({\rho }_t\right))}^T=a_0{(-w_y\alpha ,w_x\alpha ,1)}^T.$$

(13)

Here, $(u_t,v_t)$ is sensor-plane location of the vanishing point for the tilted camera.

Since $\alpha$ is small, this vanishing point shifts only a small distance from its untilted location of ${(u,v)}^T=\overrightarrow{0}$. In that case, ${\rho }_t=\sqrt{u_t^2+v_t^2}\approx 0$, and we can approximate Equation (5) as $f\left({\rho }_t\right)\approx f\left(0\right)=a_0$. This approximation is favorable because $a_1=0$ for cameras with fisheye lenses, or parabolic or hyperbolic mirrors [34]. A tilt around the axis $\overrightarrow{w}$ therefore shifts the vanishing point in proportion to the tilt angle $\alpha$:

$$\begin{array}{}\left(14\right)& \hfill {(u_t,v_t)}^T& =a_0\alpha {(-w_y,w_x)}^T,\hfill \left(15\right)& \hfill \left(\begin{array}{c}{u}_t^{\prime }\\ v_t^{\prime }\end{array}\right)& =a_0\alpha \left(\begin{array}{c}-c{w}_y+d{w}_x\\ -e{w}_y+w_x\end{array}\right)+\left(\begin{array}{c}{x}_c^{\prime }\\ y_c^{\prime }\end{array}\right).\hfill \end{array}$$

Here, ${(u_t^{\prime },v_t^{\prime })}^T$ is the vanishing-point location in the actual image. The direction of this shift depends $\overrightarrow{w}$, and thus on the tilt direction $\beta$.

Next, we estimate the shift of ${\overrightarrow{p}}_a$ from the edge pixels extracted in Section 2.1.2. For each edge pixel k, we know the position ${\overrightarrow{p}}_k={(u_k^{\prime },v_k^{\prime })}^T$, as well as the horizontal and vertical edge gradients $g_{k,x}$ and $g_{k,y}$. From this, we calculate the gradient direction angle

$${\phi }_k=\left\{\begin{array}{cc}\mathrm{atan}2(-g_{k,y},-g_{k,x}),\hfill & \mathrm{if}{g}_{k,y}<0\vee (g_{k,y}=0\wedge g_{k,x}<0),\hfill \\ \mathrm{atan}2(g_y,g_x),\hfill & \mathrm{otherwise}.\hfill \end{array}\right.$$

(16)

This two-part definition ensures that ${\phi }_k\in [0,\pi )$ is the same for both light-dark and dark-light edges. We also calculate the edge offset

$$s_k={(\cos \left({\phi }_k\right),\sin \left({\phi }_k\right))}^T\left({\overrightarrow{p}}_k-{\overrightarrow{p}}_c\right).$$

(17)

$s_k$ is the distance between ${\overrightarrow{p}}_c$ and a line orthogonal to ${\phi }_k$ that passes through ${\overrightarrow{p}}_k$. Figure 9 illustrates this geometry for a single edge pixel. If we treat k as part of an infinite, straight edge, $s_k$ would be the distance between that edge and ${\overrightarrow{p}}_c$. A similar parameterization for edge pixels was previously suggested by Davies [39]. For edge pixels k from vertical elements, we expect $s_k=0$ for an untilted robot. If the robot is tilted, $s_k$ will change based on the tilt parameters $(\alpha ,\beta )$.

Tilt Parameters from Vanishing Point Shifts

In Figure 9, a tilt has shifted the vanishing point’s location from the expected point ${\overrightarrow{p}}_c$. The new vanishing point at ${\overrightarrow{p}}_a$ is specified by the angle ${\beta }^{\prime }$ and the distance l from ${\overrightarrow{p}}_c$. Here, ${\beta }^{\prime }$ is the image-space tilt direction, from which we will later calculate the tilt direction $\beta$ of the robot. Due to this shift, the blue line extended from ${\overrightarrow{p}}_k$ no longer passes through ${\overrightarrow{p}}_c$. According to Figure 9, the edge offset $s_k$ between the blue line and ${\overrightarrow{p}}_c$ is

$$s_k=l·\sin \left({ϵ}_k\right)=l·\sin \left(\frac{\pi }{2}+{\phi }_k-{\beta }^{\prime }\right)=l·\cos \left({\beta }^{\prime }-{\phi }_k\right).$$

(18)

Using the trigonometric theorem of addition, we can rewrite this as

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill s_k& =\left[l·\cos \left({\beta }^{\prime }\right)\right]\cos \left({\phi }_k\right)+\left[l·\sin \left({\beta }^{\prime }\right)\right]\sin \left({\phi }_k\right)\hfill \\ & =A\cos \left({\phi }_k\right)+B\sin \left({\phi }_k\right).\hfill \end{array}\end{array}$$

(19)

Each edge pixel k from a vertical element provides us with one instance of Equation (19). For N such edge pixels, we then have a system with two unknowns $A,B$ and N linear equations. We can solve this overdetermined system for $(A,B)$ using a linear least squares approach. This lets us determine

$$\begin{array}{}\left(20\right)& \hfill l& =\sqrt{A^2+B^2}\mathrm{and}\hfill \left(21\right)& \hfill {\beta }^{\prime }& =\mathrm{atan}2(B,A).\hfill \end{array}$$

Knowing ${\beta }^{\prime }$ and l gives us the position ${\overrightarrow{p}}_a$ of the approximate vanishing point.

Using ${\overrightarrow{p}}_a$ as a substitute for the true location ${(u_t^{\prime },v_t^{\prime })}^T$ in Equation (15), we get

$$a_0\alpha \left(\begin{array}{c}-c{w}_y+d{w}_x\\ -e{w}_y+w_x\end{array}\right)+\left(\begin{array}{c}{x}_c^{\prime }\\ y_c^{\prime }\end{array}\right)\approx l·\left(\begin{array}{c}\cos \left({\beta }^{\prime }\right)\\ \sin \left({\beta }^{\prime }\right)\end{array}\right)+{\overrightarrow{p}}_c.$$

(22)

Recall that, for an untilted robot, we assume the optical axis to be orthogonal to the movement plane. In this case, Equation (2) simplifies to ${\overrightarrow{p}}_c\approx P({(0,0,1)}^T)$, and from this we can show that ${\overrightarrow{p}}_c\approx {(x_c^{\prime },y_c^{\prime })}^T$ using Equations (3), (4) and (7). With this result, we can write Equation (22) as

$$a_0\alpha \eta \left[{\eta }^{-1}\left(\begin{array}{c}-c{w}_y+d{w}_x\\ -e{w}_y+w_x\end{array}\right)\right]\approx l·\left[\left(\begin{array}{c}\cos \left({\beta }^{\prime }\right)\\ \sin \left({\beta }^{\prime }\right)\end{array}\right)\right]$$

(23)

with the magnitude $\eta =\sqrt{{(-c{w}_y+d{w}_x)}^2+{(-e{w}_y+w_x)}^2}$. We note that the two vectors in square brackets both have a length of 1, and can therefore write

$$\begin{array}{}\left(24\right)& \hfill \alpha & =\frac{l}{a_0·\eta },\hfill \left(25\right)& \hfill {\eta }^{-1}\left(\begin{array}{c}-c{w}_y+d{w}_x\\ -e{w}_y+w_x\end{array}\right)& =\left(\begin{array}{c}\cos \left({\beta }^{\prime }\right)\\ \sin \left({\beta }^{\prime }\right)\end{array}\right).\hfill \end{array}$$

For a conventional camera with square pixels, we can approximate $c=1$ and $d=e=0$, resulting in

$$\begin{array}{}\left(26\right)& \hfill \alpha & =\frac{l}{a_0},\hfill \left(27\right)& \hfill {\left(w_x,w_y\right)}^T& ={\left(\sin \left({\beta }^{\prime }\right),-\cos \left({\beta }^{\prime }\right)\right)}^T.\hfill \end{array}$$

Note that the tilt axis $\overrightarrow{w}$ is given in the camera reference frame. Since we assume that the camera is mounted facing upwards, the z-axis of the robot and the camera should be parallel. However, the $x,y$-axes of the robot and the camera may be rotated relative to each other. This rotation is described by the extrinsic calibration matrix R (Section 2.1.1). To estimate the tilt direction $\beta$ in the robot reference frame, we first determine the robot tilt axis

$${\overrightarrow{w}}_R=\left(\begin{array}{c}{w}_{R,x}\\ w_{R,y}\\ w_{R,z}\end{array}\right)=R\left(\begin{array}{c}{w}_x\\ w_y\\ 0\end{array}\right)=\left(\begin{array}{c}{R}_{1,1}\sin \left({\beta }^{\prime }\right)-R_{1,2}\cos \left({\beta }^{\prime }\right)\\ R_{2,1}\sin \left({\beta }^{\prime }\right)-R_{2,2}\cos \left({\beta }^{\prime }\right)\\ R_{3,1}\sin \left({\beta }^{\prime }\right)-R_{3,2}\cos \left({\beta }^{\prime }\right)\end{array}\right).$$

(28)

From ${\overrightarrow{w}}_R$, we can finally calculate the tilt direction

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill \beta & =\mathrm{atan}2(w_{R,y},w_{R,x})-\frac{\pi }{2}=\mathrm{atan}2(-w_{R,x},w_{R,y})\hfill \\ & =\mathrm{atan}2\left(-R_{1,1}\sin \left({\beta }^{\prime }\right)+R_{1,2}\cos \left({\beta }^{\prime }\right),R_{2,1}\sin \left({\beta }^{\prime }\right)-R_{2,2}\cos \left({\beta }^{\prime }\right)\right).\hfill \end{array}\end{array}$$

(29)

Note that Equation (29) includes the term $-\frac{\pi }{2}$ because the tilt direction is at a right angle to the tilt axis.

We have made a number of approximations during this derivation. Most noticeably, we assume that a tilt causes a shift in the camera image, disregarding the effects shown in Figure 7. During preliminary experiments, we found that this method exhibits systematic errors in the estimated tilt angle $\alpha$. As per Equation (26), $\alpha$ should be proportional to the shift distance l with a coefficient of $a_0^{-1}$. Since this gives poor results in practice, we replace the camera parameter $a_0$ with a correction factor a, so that

$$\widehat{\alpha }=\frac{l}{a}.$$

(30)

a is the proportionality constant relating l and $\alpha$, which we estimate from a set of training images using

$$a=\frac{1}{m}{\displaystyle \sum _{i=1}^m}\frac{l_i}{{\alpha }_{T,i}}.$$

(31)

Here, $l_i$ is the shift distance l measured for the image with index i. ${\alpha }_{T,i}>$ 0° is the ground truth for $\alpha$ in the ith image, determined according to Section 2.3. Thus, a equals the mean ratio between the shift $l_i$ and true tilt angle ${\alpha }_{T,i}$ over a training set of m tilted images. Using this heuristic, we can improve the accuracy of our estimates while still benefiting from the numerous simplifications made above.

2.1.4. Rejecting Incorrect Pixels

So far, we have assumed that all edge pixels correspond to vertical elements. This is not the case in a real environment, as shown by the many non-vertical elements in Figure 5. We therefore have to reject incorrect edge pixels caused by these non-vertical elements. Edge pixels disturbed by image artifacts and camera noise should also be filtered out.

As a first step, we apply a prefilter that discards all pixels with a large edge offset $s_k$: as before, we assume a small tilt angle $\alpha \le {\alpha }_{\max }$. According to Equation (30), an upper bound for $\alpha$ also limits l. From Equation (18), we see that $|s_k|\le l$, and thus a limit on l implies a limit $|s_k|\le s_{\max }$. We therefore discard edge pixels k with large $|s_k|$, as they are not consistent with $\alpha \le {\alpha }_{\max }$. After this prefiltering, the remaining edge pixels form the set F. In this work, we chose $s_{\max }=40$, which corresponds to ${\alpha }_{\max }\approx$ 10°; the precise value of ${\alpha }_{\max }$ depends on the choice of a in Equation (30). Figure 10 shows the result of this prefiltering step.

We now estimate the tilt parameters $(\alpha ,\beta )$ by fitting a cosine to the $({\phi }_k,s_k)$ of the edge pixels in the prefiltered set F. This step is described in Section 2.1.3 and illustrated in Figure 11. Here, incorrect edge pixels may cause large errors, as demonstrated by Figure 12a. Such pixels can arise from non-vertical elements or noise, as seen in Figure 13. One solution uses the popular Random Sample Consensus (RANSAC) hypothesize-and-test scheme [40]. RANSAC has previously been used to identify vanishing points from straight lines, for example by Aguilera et al. [41,42]. Under RANSAC, we generate tilt hypotheses from randomly-chosen edge pixels in the prefiltered set F. We then select the hypothesis $(\alpha ,\beta )$ in consensus with the highest number of pixels. Alternatively, we try a reject-refit scheme based on repeated least-squares estimation. Here, we reject edge pixels that disagree with our most recent $(\alpha ,\beta )$ estimate. $(\alpha ,\beta )$ are then re-estimated from the remaining pixels. This repeats until the estimate converges, or an iteration limit is reached.

RANSAC Variant

For the RANSAC approach, we randomly select two pixels i and $j\ne i$ from the prefiltered set F. Solving a system of Equation (19) for the pixels $i,j$ gives us the hypothesis $(A_{i,j},B_{i,j})$. We use Equation (19) to identify the set of edge pixels in consensus with this hypothesis as

$$C_{i,j}=\bigcup \left\{k|k\in F\wedge \left|(A_{i,j}\cos \left({\phi }_k\right)+B_{i,j}\sin \left({\phi }_k\right)-s_k\right|<{\delta }_{s,\max }\right\}.$$

(32)

We repeat this up to $N_R=1000$ times, and select the edge pixels $\breve{i},\breve{j},$ which maximize $\parallel C_{\breve{i},\breve{j}}\parallel$. In our experiments, we use a systematic search to select the threshold ${\delta }_{s,\max }$ (Section 2.4). If RANSAC is successful, $C_{\breve{i},\breve{j}}$ contains no incorrect edge pixels. We now construct a linear system of Equation (19) using the $({\phi }_{k^{\prime }},s_{k^{\prime }})$ of the edge pixels $k^{\prime }\in C_{\breve{i},\breve{j}}$. Solving this system through linear least-squares gives us $(\breve{A},\breve{B})$; here, we use the QR decomposition implemented by the Eigen library [43]. From these $(\breve{A},\breve{B})$, we can finally estimate $(\alpha ,\beta )$ according to Section 2.1.3.

To speed up this process, RANSAC may terminate before reaching the iteration limit $N_R$ [40]: let p be the fraction of correct edge pixels in the prefiltered set F, with $\parallel F\parallel \gg 2$. The probability of drawing at least one pair of correct edge pixels in n attempts is $q\approx 1-{(1-p^2)}^n$. Since we do not know p, we estimate its lower bound as $p_n=\parallel C_n\parallel /\parallel F\parallel$; $C_n$ is the largest set $C_{i,j}$ encountered after n iterations [44]. The probability of encountering at least one correct pixel pair after n iterations is thus estimated as $q_n\approx 1-{(1-p_n^2)}^n$. We then terminate once $q_n>q_{\min }$, here using a value of $q_{\min }=0.9999$.

Reject-Refit-Variant

In our reject-refit scheme, we alternate between estimating $(A,B)$ from Equation (19) and rejecting incorrect pixels. We use least squares to estimate $(A_n,B_n)$ from the edge pixels in $F_n$, beginning with $F_0=F$. This estimate is affected by incorrect pixels in F, as shown in Figure 12a. To reject these pixels, we calculate the residual error

$${\delta }_{k,n}=\left(A_n\cos \left({\phi }_k\right)+B_n\sin \left({\phi }_k\right)\right)-s_k$$

(33)

for each pixel in $F_n$. Next, we form $F_{n+1}$ by rejecting a fraction Q of pixels in $F_n$ that have the largest absolute error $|{\delta }_{k,n}|$. Identifying the actual set of pixels with the largest $|{\delta }_{k,n}|$ requires sorting $F_n$. This is computationally expensive, and thus we use another heuristic: we assume that the ${\delta }_{k,n}$ of the pixels in $F_n$ are normally distributed, with mean ${\mu }_{\delta ,n}=0$ and standard deviation ${\sigma }_{\delta ,n}$. In an idealized case, only a fraction $1-Q=2\mathrm{\Phi }\left(z\right)-1$ of the edge pixels k in $F_n$ satisfies ${\delta }_{k,n}\in [-z{\sigma }_{\delta ,n},z{\sigma }_{\delta ,n}]$ [45]; here, $\mathrm{\Phi }$ is the cumulative distribution function of the standard normal distribution. For the remaining fraction Q with ${\delta }_{k,n}$ outside of this interval, we therefore expect

$$|{\delta }_{k,n}|>{\sigma }_{\delta ,n}{\mathrm{\Phi }}^{-1}\left(1-\frac{Q}{2}\right).$$

(34)

We can thus simply construct the next, smaller set

$$F_{n+1}=\left\{k|k\in F_n\wedge \left|{\delta }_{k,n}\right|\le {\sigma }_{\delta ,n}{\mathrm{\Phi }}^{-1}\left(1-\frac{Q}{2}\right)\right\}$$

(35)

without sorting $F_n$.

While the ${\delta }_{k,n}$ in $F_n$ are not actually normally distributed, we found that this fast heuristic nonetheless gives good results. After these two fitting and rejection steps, we increment n and repeat the procedure. This continues until the tilt estimate converges, or until n reaches the limit $N_E=15$. We assume that the estimate has converged once $|l_n-l_{n-1}|<0.1$, as calculated from Equation (20). Figure 14 demonstrates the effect of this reject-refit heuristic.

2.2. Vector-Consensus Method

The vector-consensus method is the second tilt-estimation scheme used in this work. It operates on the same edge pixels as the image-space method described in Section 2.1.2. As before, we assume that many of these pixels correspond to vertical elements k in the environment. Since they are vertical, their orientations ${\overrightarrow{o}}_k$ are parallel to the movement-plane normal $\overrightarrow{n}$. We illustrate this relationship in Figure 15a. Knowing their orientation ${\overrightarrow{o}}_{k,R}\parallel {\overrightarrow{n}}_R$ in robot coordinates lets us calculate $(\alpha ,\beta )$ using Equation (1). However, as seen in Figure 15b, our camera image shows only projections of the elements. We therefore cannot determine ${\overrightarrow{o}}_{k,R}$ from any single edge pixel k, and instead use several pixels from different vertical elements. We identify a set of such edge pixels by combining a problem-specific prefilter with RANSAC. This approach is highly similar to RANSAC-based vanishing-point detection, as for example in [41].

2.2.1. Orientation Estimation

We first estimate ${\overrightarrow{o}}_{i,C}\parallel {\overrightarrow{o}}_{j,C}$ of two separate parallel elements from the edge pixels $i,j$; here, ${\overrightarrow{o}}_{i,C}$ is ${\overrightarrow{o}}_i$ in the camera reference frame. From Section 2.1.2, we know the image position ${\overrightarrow{p}}_k$ and gradient vector ${\overrightarrow{g}}_k={(g_{k,x},g_{k,y})}^T$ for both i and j. The camera’s inverse projection function $P^{-1}$ provides the bearing ${\overrightarrow{v}}_i=P^{-1}\left({\overrightarrow{p}}_i\right)$ from the projection center towards the edge pixel i. As illustrated in Figure 16, ${\overrightarrow{o}}_{i,C}$ and ${\overrightarrow{v}}_i$ define a plane in space. This plane has the normal vector ${\overrightarrow{m}}_i={\overrightarrow{o}}_{i,C}\times {\overrightarrow{v}}_i$. We repeat this step for a second edge pixel j, giving us the normal ${\overrightarrow{m}}_j$ for a plane containing ${\overrightarrow{o}}_{j,C}$ and ${\overrightarrow{v}}_j$. Since the orientations ${\overrightarrow{o}}_{i,C}\parallel {\overrightarrow{o}}_{j,C}$ are orthogonal to both ${\overrightarrow{m}}_i$ and ${\overrightarrow{m}}_j$, we have

$${\overrightarrow{o}}_{i,C}\parallel {\overrightarrow{o}}_{j,C}\parallel ({\overrightarrow{m}}_i\times {\overrightarrow{m}}_j).$$

(36)

This plane-based formalism [46] is commonly used in vanishing-point estimation, for example in [27,29,47].

While we do not know ${\overrightarrow{m}}_i$, we can estimate it using the edge gradient ${\overrightarrow{g}}_i$. Shifting the pixel position ${\overrightarrow{p}}_i$ by a small distance $\Delta$ along the gradient ${\overrightarrow{g}}_i$, we calculate ${\overrightarrow{u}}_i=P^{-1}({\overrightarrow{p}}_i+\Delta {\overrightarrow{g}}_i)$; in this work we use $\Delta =0.01$. As illustrated in Figure 16, ${\overrightarrow{g}}_i$ is a projection of ${\overrightarrow{m}}_i$, and ${\overrightarrow{m}}_i$, ${\overrightarrow{v}}_i$, and ${\overrightarrow{u}}_i$ lie in the same plane. ${\overrightarrow{u}}_i\times {\overrightarrow{v}}_i$ is a normal vector of this plane, and therefore orthogonal to ${\overrightarrow{m}}_i$. By definition, ${\overrightarrow{m}}_i$ is also orthogonal to ${\overrightarrow{v}}_i$, leading us to

$${\overrightarrow{m}}_i\parallel \left({\overrightarrow{v}}_i\times ({\overrightarrow{u}}_i\times {\overrightarrow{v}}_i)\right).$$

(37)

Note that we have assumed a continuous $P^{-1}$ when calculating ${\overrightarrow{u}}_i$. Therefore, shifting $\overrightarrow{p}$ by a small distance $\Delta$ leads to only a small change in $P^{-1}\left(\overrightarrow{p}\right)$. In addition, the camera should use central projection. The camera model used in this work [34] generally fulfills these requirements. From Equation (36) and Equation (37), we estimate the orientation ${\overrightarrow{o}}_{i,C}\parallel {\overrightarrow{o}}_{j,C}$ from the pixel positions ${\overrightarrow{p}}_i,{\overrightarrow{p}}_j$ and gradients ${\overrightarrow{g}}_i,{\overrightarrow{g}}_j$.

In practice, estimating ${\overrightarrow{o}}_{i,C}$ from just two pixels will give poor results due to noise. We therefore use a set E of two or more edge pixels with ${\overrightarrow{o}}_{i,C}\parallel {\overrightarrow{o}}_{j,C}\forall i,j\in E$. Using Equation (37), we determine ${\overrightarrow{m}}_k$ for each pixel $k\in E$. Each ${\overrightarrow{m}}_k$ is orthogonal to ${\overrightarrow{o}}_{k,C}$, with ${\overrightarrow{m}}_k·{\overrightarrow{o}}_{k,C}=0$. Since all ${\overrightarrow{o}}_{k,C}$ in E are parallel, we replace them with a general orientation $\overrightarrow{o_C}\parallel {\overrightarrow{o}}_{k,C}\forall k\in E$, giving us the constraint

$${\overrightarrow{m}}_k·\overrightarrow{o_C}=0.$$

(38)

We now construct a system of equations that contains one instance of Equation (38) for each pixel $k\in E$. To avoid the trivial solution of $\overrightarrow{o_C}=\overrightarrow{0}$, we add the additional equation of ${(1,1,1)}^T\overrightarrow{o_C}=1$ to the system. Solving this overdetermined system using linear least squares gives us an estimate for $\overrightarrow{o_C}$. In practice, we use Singular Value Decomposition (SVD) implemented in the Eigen library [43].

2.2.2. Vertical Edge Selection

Next, we identify a set of edge pixels that corresponds to vertical elements in the environment. Using a prefilter, we discard any edge pixels that indicate an implausibly large tilt angle $\alpha$. Note that ${\overrightarrow{m}}_k$ is given in the camera reference frame, which may be rotated relative to the robot reference frame. We therefore transform ${\overrightarrow{m}}_k$ to the robot frame by multiplying with the extrinsic calibration matrix R. If ${\overrightarrow{o}}_k$ is vertical, then $R·{\overrightarrow{m}}_k$ is parallel to the robot’s movement plane, and

$${\widehat{\alpha }}_k=\left|\arcsin \left({\overrightarrow{n}}^T\frac{R·{\overrightarrow{m}}_k}{\parallel {\overrightarrow{m}}_k\parallel }\right)\right|\le \alpha .$$

(39)

If ${\widehat{\alpha }}_k>{\alpha }_{\max }$, we therefore reject the edge pixel k. The remaining edge pixels form the set E. Note that ${\widehat{\alpha }}_k$ is always 0 if the tilt axis is parallel to $R·{\overrightarrow{m}}_k$.

Similar to Section 2.1.3, we apply RANSAC to identify edge pixels with vertical ${\overrightarrow{o}}_k$ [40]. We randomly select two edge pixels $i,j\ne i$ from E, and use Equation (36) to generate a hypothesis ${\overrightarrow{o}}_{i,j,C}$. The set $C_{i,j}$ of edge pixels in consensus with this hypothesis is

$$C_{i,j}=\bigcup \left\{k|k\in E\wedge \left|{\overrightarrow{m}}_k·{\overrightarrow{o}}_{i,j,C}\right|<{\delta }_{o,\max }\right\}.$$

(40)

As in Section 2.1.3, we repeat this step until termination ($N_R=1000$ and $q_{\min }=0.9999$). If RANSAC is successful, the largest consensus set $C_{\breve{i},\breve{j}}$ contains only vertical edge pixels with $\overrightarrow{n}\parallel {\overrightarrow{o}}_k\forall k\in C_{\breve{i},\breve{j}}$. Figure 17 shows an example for the pixels in $C_{\breve{i},\breve{j}}$. Figure 18 shows examples of the edge pixels in $C_{\breve{i},\breve{j}}$.

We now use the least-squares approach from Section 2.2.1 to determine the common orientation $\overrightarrow{o_C}$ from the pixels in $C_{\breve{i},\breve{j}}$. Since $\overrightarrow{o_C}$ should be vertical, we estimate the movement plane normal ${\overrightarrow{n}}_R$ as

$${\overrightarrow{n}}_{R,\mathrm{est}}=R·\frac{\overrightarrow{o_C}}{\parallel \overrightarrow{o_C}\parallel }.$$

(41)

Finally, we calculate the tilt parameters $(\alpha ,\beta )$ from ${\overrightarrow{n}}_{R,\mathrm{est}}$ using Equation (1). For some R, the constraint ${(1,1,1)}^T\overrightarrow{o_C}=1$ in Section 2.2.1 may result in a ${\overrightarrow{n}}_{R,\mathrm{est}}$ that is antiparallel to ${\overrightarrow{n}}_R$. Since we know that $\alpha \ll$ 90°, we simply flip ${\overrightarrow{n}}_{R,\mathrm{est}}$ for results of $\alpha >90$°.

2.3. Image Database

To evaluate the methods from Section 2.1 and Section 2.2, we recorded an image database using our cleaning-robot prototype. Our robot carries a center-mounted, upward-facing monochrome camera with a panoramic fisheye lens, as shown in Figure 2. While resolutions up to $1280\times 1024$ are supported, a lower resolution can offer advantages for visual pose estimation [1]. We therefore capture $640\times 480$ pixel images, which show a 185° field of view in azimuthal equidistant projection. This reduced resolution also speeds up processing time. As per Section 2.1.2, we mask out pixels showing areas below the horizon. The remaining pixels form a disc-shaped area with a diameter of 439 pixels, as seen in Figure 4. While capturing images, the exposure time is automatically adjusted to maintain a set average image brightness. For further details on image acquisition, we point the reader to [1]; we also use the intrinsic and extrinsic camera parameters from this work.

We captured images in 43 different locations spread across six different environments. The six environments consist of four different offices, as well as two lab environments. In some cases, we positioned the robot in narrow spaces or under furniture, as in Figure 19. The restricted field of view may pose a special challenge for tilt estimation. Our image database contains between four and eleven locations per environment. Due to this limited number, the locations do not provide a representative sample of their environment. However, completely covering each environment would require a large number of images. We also expect that many of these locations would be visually similar. Instead, we focused on picking a smaller, but highly varied set of locations.

At each location, we captured one untilted plus six tilted image, for a total of $43\times (1+6)=301$. We produced six distinct tilts by placing a 5 mm or 10.1 mm metal spacer under one of the robot’s three wheels. The thickness of these spacers is similar to common sources of uneven ground: in preliminary experiments, various types of carpet raised the robot’s wheels by 4.4 mm to 7.4 mm. Similarly, a typical door threshold raised it by 5.3 mm.

Table 1. The ground-truth tilt angle ${\alpha }_T$ when raising one wheel by a distance h, as calculated from Equation (42). Raising one of the main wheels results in ${\beta }_T=\mp 137$° for the left and right wheel, respectively (Equation (43)). ${\beta }_T=0$° when raising the caster wheel, which holds up the rear of the robot. Spacer refers to the metal spacers used to capture tilted images. We also included common objects from a domestic environment that may cause the robot to tilt. The ${\alpha }_T$ values in this table were calculated for a stationary robot. For a moving robot, driving-related forces or torques may affect the true tilt angle or direction.

Object	h	${\mathit{\alpha }}_{\mathit{T}}$ (Main Wheel)	${\mathit{\alpha }}_{\mathit{T}}$ (Caster)
Spacer (thin)	5.0 mm	1.38°	2.06°
Spacer (thick)	10.1 mm	2.80°	4.15°
Carpets (various)	4.4 mm to 7.4 mm	1.22° to 2.05°	1.81° to 3.04°
Door threshold	5.3 mm	1.47°	2.18°

lists the tilts experienced by the robot for these wheel heights. Note that placing the spacers may cause slight changes in robot position or heading between images.

We use a simple static model to calculate the true tilt angle ${\alpha }_T$ and direction ${\beta }_T$ from the measured wheel heights $l,r$ and c. Assuming that the left wheel as our reference point, we calculate the relative heights $h_r=r-l$ and $h_c=c-l$. From the robot geometry shown in Figure 20, we find that

$$\sin \left({\alpha }_T\right)=\frac{1}{2}\sqrt{{\left(\frac{2h_c-h_r}{r_c}\right)}^2+{\left(\frac{h_r}{r_w}\right)}^2}$$

(42)

for the tilt angle ${\alpha }_T$. If the robot is tilted with $\sin \left({\alpha }_T\right)\ne 0$, the tilt direction is

$${\beta }_T=\mathrm{atan}2\left(\frac{h_r}{2r_w\sin \left({\alpha }_T\right)},\frac{2h_c-h_r}{2r_c\sin \left({\alpha }_T\right)}\right).$$

(43)

2.4. Experiment Design

We test the methods from Section 2.1 and Section 2.2 on the images gathered in Section 2.3. The estimation-error angle

$$ϵ=\arccos \left(\frac{{\overrightarrow{n}}_R}{\parallel {\overrightarrow{n}}_R\parallel }·\frac{{\overrightarrow{n}}_{R,\mathrm{est}}}{\parallel {\overrightarrow{n}}_{R,\mathrm{est}}\parallel }\right)$$

(44)

serves as our measure of tilt-estimation accuracy. $ϵ$ is the residual, uncorrected tilt angle that remains after a tilt correction based on the estimate ${\overrightarrow{n}}_{R,\mathrm{est}}$. ${\overrightarrow{n}}_R$ and ${\overrightarrow{n}}_{R,\mathrm{est}}$ are calculated from $({\alpha }_T,{\beta }_T)$ and $(\alpha ,\beta ),$ respectively, by inverting Equation (1).

We evaluate numerous parameter values for our methods, seeking to minimize the mean error $\overline{ϵ}$. For both methods, we try several gradient-intensity thresholds $I_{\min }$. With the image-space method, we also vary the rejection fraction Q or the RANSAC threshold ${\delta }_{s,\max }$ from Section 2.1.4. Similarly, we investigated multiple values for the RANSAC threshold ${\delta }_{o,\max }$ in Section 2.2.2.

Table 2. Parameter values evaluated in our experiments. Depending on the method and variant used, each value for $I_{\min }$ was tested with all other values for Q, ${\delta }_{s,\max }$, and ${\delta }_{o,\max }$. This results in $6\times 5=30$ combinations for $(I_{\min },Q)$, $6\times 6=36$ for $(I_{\min },{\delta }_{s,\max })$, and $6\times 10=60$ for $(I_{\min },{\delta }_{o,\max })$.

Parameter	Values Tested
Gradient intensity threshold $I_{\min }$	100, 150, 200, 250, 300, 600
Rejection fraction Q	0.5, 0.7, 0.8, 0.9
Random Sample Consensus (RANSAC) threshold ${\delta }_{s,\max }$	2.5, 5, 7.5, 10, 12.5, 15 pixels
RANSAC threshold ${\delta }_{o,\max }$	0.5°, 1°, 1.5°, 2°, 2.5°, 3°, 3.5°, 4°, 4.5°, 5°

lists the specific values tested during our experiments.

For the image-space method, we determine the factor a from Equation (30) using cross-validation: Given a location L, the set $T_L$ contains all tilted images not captured at L. Next, we compute the factor $a_L$ by applying Equation (31) to the $m=(43-1)\times 6$ images in $T_L$. For images taken at location L, we then calculate the tilt angle $\alpha$ by using Equation (30) with $a=a_L$. Using the parameters from

Table 3. The mean tilt-estimation error $\overline{ϵ}$ and standard deviation $\left({\sigma }_{ϵ}\right)$ for the methods and variants being tested. The last line lists the vector-consensus results achieved when correcting the estimated tilt angle $\alpha$ using the factor $a_L^{\prime }$ (Section 2.4). For each method, we only list the results for the parameters given in the third column. Out of the possibilities from Table 2, these values gave the lowest $\overline{ϵ}$. We also included the 50th percentile (median) and 95th percentile of $ϵ$.

Method	Variant	Parameters	Estimation error $\mathit{ϵ}$ (°)
			$\overline{\mathit{ϵ}}$ (${\mathit{\sigma }}_{\mathit{ϵ}}$)	50%	95%
Image Space	RANSAC	$I_{\min }=150$, ${\delta }_{s,\max }=10.0$	1.17 (1.03)	0.97	3.16
	reject-refit	$I_{\min }=200$, $Q=0.8$	0.85 (0.81)	0.61	2.27
Vector Consensus	RANSAC	$I_{\min }=600$, ${\delta }_{o,\max }=5.0$°	1.63 (0.93)	1.48	3.38
	”, corrected	$I_{\min }=300$, ${\delta }_{o,\max }=2.0$°	1.03 (0.72)	0.92	2.30

, we find $a_L\in [298,314]$ pixels per radian when using RANSAC. Likewise, $a_L\in [234,239]$ pixels per radian for the reject-refit variant. By comparison, the coefficient determined through camera calibration is $a_0=147$ pixels per radians (Equation (26)).

The vector-consensus method estimates the tilt angle $\alpha$ without major approximations, and should not require a correction factor. However, for the sake of fairness, we also tested this method with an optional correction factor $a^{\prime }$. The corrected tilt-angle estimate is then

$${\alpha }^{\prime }=\frac{\alpha }{a^{\prime }}.$$

(45)

$ϵ$ for this corrected vector-consensus variant is then calculated from ${\alpha }^{\prime }$ instead of $\alpha$. We determine the specific $a_L^{\prime }$ for the location L using the same cross-validation scheme as for $a_L$. For the parameters in Table 3, we find a unitless factor $a_L^{\prime }\in [1.77,1.80]$.

When running on a robot’s onboard CPU, real-time constraints may subject the tilt-estimation methods to strict time limits. As part of our experiments, we therefore measure execution times on two different systems. The first system uses the 1.6 GHz dual-core Intel (Santa Clara, CA, USA) Atom N2600 CPU, and represents a typical embedded platform. This CPU is used for autonomous control of our cleaning-robot prototype. For comparison, we also include a modern desktop system with a quad-core Intel Core i7-4790K.

We measure the wall-clock execution time using each system’s monotonic, high-resolution clock. This includes all major steps required to estimate $(\alpha ,\beta )$ from a camera image. Non-essential operations, such as loading and converting input data, were excluded from this measurement. All experiments involving RANSAC use the same random seed on both platforms. This ensures that the number of RANSAC iterations is identical. So far, our implementations do not consistently support multi-core execution. We therefore use Linux’s Non-Uniform Memory Access (NUMA) policies to restrict each experiment to a single, fixed CPU core. These systems and procedures correspond to those used in [1]. Thus, we can directly compare the tilt-estimation execution times to our previous pose-estimation measurements.

3. Results

3.1. Tilt Estimation Error

Table 3 lists the tilt-estimation error $ϵ$. Figure 21 shows the cumulative distributions of $ϵ$. It specifies the fraction of images for which $ϵ$ was below a given value. Figure 22 plots the error $ϵ$ depending on the true tilt angle ${\alpha }_T$. We also studied how the methods perform across the different environments in our database, resulting in Figure 23.

3.2. Computation Time

Table 4. The time required for tilt estimation, calculated according to Section 2.4. This table lists the mean, standard deviation ${\sigma }_t$ and 50th (median) and 95th percentile, in milliseconds. Times are given for the modern desktop CPU, as well as the embedded CPU carried by our robot prototype. Each method used the parameters listed in Table 3, which gave the lowest mean error $\overline{ϵ}$. Note that the corrected vector-consensus method appears to be much slower than the uncorrected variant. This is not due to the correction step, which consumes little time. Instead, the corrected variant achieves its lowest $\overline{ϵ}$ for different parameter values (Table 3). However, these values also lead to longer execution times. Compared to the uncorrected variant, the corrected variant actually gives better $\overline{ϵ}$ in similar time (Figure 24 and Figure 25).

Method	Variant	Desktop (ms)			Embedded (ms)
		Mean (${\mathit{\sigma }}_{\mathit{t}}$)	50%	95%	Mean (${\mathit{\sigma }}_{\mathit{t}}$)	50%	95%
Image Space	RANSAC	3.8 (1.43)	3.7	6.3	26.0 (6.89)	25.2	41.8
	reject-refit	2.5 (0.85)	2.5	4.1	15.7 (3.02)	15.3	20.7
Vector Consensus	RANSAC	3.6 (1.22)	3.5	5.7	22.9 (4.69)	23.2	29.4
	corrected	6.2 (1.88)	6.0	9.3	38.1 (9.72)	36.9	51.8

contains the execution time taken by the methods, using the parameters from Table 3. In our experiments, this time depends heavily on the parameters used. However, these parameters also affect the quality of the results. Figure 24 and Figure 25 therefore show the execution times in relation to the mean error $\overline{ϵ}$.

4. Discussion

As shown by Table 3, all variants achieve at least fair results, with $\overline{ϵ}<2$°. The image-space method with the reject-refit scheme achieves the lowest mean and median error. Combining the image-space method with a basic RANSAC scheme gives only mediocre results. In contrast, the vector-consensus approach led to the highest $\overline{ϵ}$. However, its accuracy improves noticeably when the tilt-angle estimate is corrected by the factor $a^{\prime }$.

The practical impact of the tilt-estimation error $ϵ$ depends on the application. For example, our cleaning-robot prototype uses Min-Warping [10] for planar-motion pose estimation. Here, the pose-estimation error increases rapidly for $\alpha$ over ≈1°, as seen in Figure 26 [1]. Beyond ≈2°, the pose-estimation error exceeds that of a nonplanar method [1,14]. In Figure 21, we see that the best candidate (image space, reject-refit) achieves $ϵ<1$° for $>70\%$ of all images. All but the worst candidate (vector consensus, no correction) also attain $ϵ\le 2$° for at least $\approx 90\%$ of images. For tilt correction, $ϵ$ is the angle $\alpha$ of the remaining uncorrected tilt. We thus expect that a correction based on these estimates should make planar-motion pose estimation more resilient to tilts. However, we believe that comprehensive tests of this specific application lie beyond the scope of this initial work; we therefore do not include them here.

Besides the comparatively low average errors, all methods do exhibit occasional high $ϵ$. These errors can even exceed the highest tilt angle $\alpha \approx 4.15$° in the database. A correction with such an erroneous estimate would likely give worse results than any uncorrected tilt. The fraction of images for which this occurs varies by method, as shown in Figure 21. In our experiments, such failures generally result from a violation of our world assumption: both methods rely on visually-distinct vertical elements in the environment. In some locations, such elements are rare, or are drowned out by many near-vertical elements. As an example, Figure 19c shows the robot surrounded by a chair and table with angled legs. For the image-space method, this produces the worst tilt-estimation error out of all locations: using the parameters from Table 3, $ϵ=7.4$° for the reject-refit scheme, and $ϵ=7.7$° for RANSAC. The reason for this poor estimate is illustrated in Figure 27: most of the edge pixels extracted from the camera image do not correspond to vertical elements. The $(\phi ,s)$ curve fitted to these incorrect edge pixels thus gives a poor tilt estimate. Conversely, the curve for the correct tilt parameters matches few of the edge pixels.

While individual locations may produce high tilt-estimation errors, no environment causes a general failure for any method. Figure 23 illustrates this by showing $\overline{ϵ}$ for each method and environment. Although there is some variation in $\overline{ϵ}$, it remains below 2° in all cases.

In this discussion, we have frequently used the mean tilt-estimation error $\overline{ϵ}$. This measure depends on the composition of the image set on which it was calculated. For example, a method may give especially high $ϵ$ for images with a high ${\alpha }_T$. In this case, a set with many such images gives a higher mean $\overline{ϵ}$, compared to a set with few such images. We therefore calculated $\overline{ϵ}$ for each true tilt angle ${\alpha }_T$. In Figure 22, $\overline{ϵ}$ shows only a moderate dependence on the tilt angle ${\alpha }_T$. We note that tilt-estimation errors also occur for an untilted robot (${\alpha }_T=0$°).

In Section 2.4, we included a corrected vector-consensus variant that uses the factor $a^{\prime }$ (Equation (45)). We first introduced a similar factor a (Equation (30)) to compensate for the approximations made in the image-space method. Figure 28 plots the true and estimated tilt angle with and without these corrections. We note that, on average, the uncorrected methods overestimate $\alpha$ for all true tilt angles ${\alpha }_T$. However, the corrected methods merely divide $\alpha$ by a finite constant. Thus, they cannot correct the difference between $\alpha$ and ${\alpha }_T$ for ${\alpha }_T=0$. In future experiments, it might be worthwhile to include an additive correction with $\alpha =l/a+b$ and $\alpha ={\alpha }^{\prime }/a^{\prime }+b^{\prime }$.

As shown in Figure 24 and Figure 25, the tilt estimation time noticeably depends on the parameters used. Choosing parameters with a somewhat higher mean error $\overline{ϵ}$ can reduce the mean execution time by more than half. Under such a trade-off, all methods achieve a mean execution time below 30 ms on the embedded CPU (Figure 25). On the desktop system, this time is less than 5 ms. However the time required for some images can be considerably higher, as seen in the 95th percentile values from Table 4. This must be taken into account for systems with strict real-time constraints.

For our cleaning-robot example, one onboard pose-estimation process requires $\approx 165$ ms Table 11 [1]. In comparison, the reject-refit image-space method requires only $\approx 16$ ms to estimate the tilt (Table 4). Adding tilt estimation to the pose-estimation process thus increases the total time by only $\approx 10\%$. Overall, we deem all methods suitable for onboard use on a typical mobile robot.

The utility of tilt corrections for planar-motion methods depends on the prevalence and magnitude of tilts encountered in an environment. Corrections can be beneficial if the robot is frequently tilted, or if uncorrected tilts have a large effect. In other cases, the tilt-estimation errors may cause greater problems than the actual tilts. Such errors occur even if robot motion was perfectly planar. The use of tilt correction thus needs to be evaluated for each application. We once more use the pose-estimation example from cleaning robot prototype: in a previous experiment, a nonplanar method gave more accurate results for tilts over ≈2°, as shown in Figure 26 [1]. However for the image-space method, the residual error $ϵ$ will usually remain below this value (Figure 21). The combined execution time for tilt estimation and planar pose estimation is $\approx 16\mathrm{m}\mathrm{s}+165\mathrm{m}\mathrm{s}$. This is much faster than the fastest nonplanar pose-estimation method ($\approx 1807\mathrm{m}\mathrm{s}$) ([1] Table 11). Thus, tilt estimation should help preserve the advantages of the planar-motion method if the robot is tilted. However, this is limited to small tilt angles and requires visually distinct vertical elements—limitations not shared by the nonplanar method.

5. Conclusions

In this work, we wanted to measure a robot’s tilt relative to a movement plane in an indoor environment. All methods tested here solve this problem based on panoramic images (Table 3). They do so across different environments (Figure 23) and tilt angles (Figure 22). Their fast execution time makes them suitable for real-time use, even on a modest embedded CPU (Table 4 and Figure 25). Although average errors are low, tilt-estimation failures can occasionally occur (Figure 21). Such failures are likely if the environment lacks visually-distinct vertical elements (Figure 27). Furthermore, even an untilted robot will experience some tilt-estimation errors (Figure 22).

Overall, the image-space reject-refit variant had the lowest estimation error (Table 3) and execution time (Table 4 and Figure 25). A variant that uses RANSAC to reject incorrect edge pixels offered no advantage in quality or speed. The vector-consensus method was also slower, and resulted in higher errors. However, using a correction factor (Equation (45)) reduced these errors. The vector-consensus method makes fewer approximations than the image-space method. It may thus still be useful in applications where these approximations are invalid.

5.1. Outlook

In light of these results, we note several possibilities for future improvements.

This work uses the visually-distinct vertical elements that appear in a typical indoor environment. If these elements are rare or drowned out by near-vertical ones, large errors $ϵ$ may ensure (Figure 27). A confidence measure would be useful to detect these incorrect results. Such a measure might be based on the fraction of incorrect edge pixels rejected during tilt estimation.

The methods in this initial study used only a basic RANSAC scheme. However, the literature contains numerous advanced RANSAC variants, which may achieve better results [44,49,50,51,52]. We currently use a very simple heuristic to correct the tilt-angle estimate $\alpha$ (Equations (30) and (45)). However, as seen in Figure 28, this is not sufficient to fully correct the error in $\alpha$. A more sophisticated approach may calculate $P\left(\widehat{\alpha }\right|l)$. This is the probability that the robot is tilted by $\widehat{\alpha }$ given the vanishing-point shift l. We could use Bayesian techniques to evaluate $P\left(\widehat{\alpha }\right|l)=P\left(l\right|\widehat{\alpha })P\left(\widehat{\alpha }\right)P{\left(l\right)}^{-1}$. This also lets us incorporate the tilt-angle distribution $P\left(\widehat{\alpha }\right)$ for a specific environment. However, finding the parameters for such a probabilistic model may require a large amount of training data. We therefore leave such attempts for future works.

The implementations used in this work are not fully optimized. For example, a greater use of Single Instruction Multiple Data (SIMD) instructions would likely improve the execution speed. Such instructions are supported on both the desktop and embedded processors. However, we feel the speed of our implementations is adequate, and did not attempt such improvements here.

In this work, we focused on simple methods that leverage the specific properties of the tilt-estimation problem. As discussed in Section 1.1, other works present more general solutions to estimate camera orientations. We believe that a comprehensive comparison between these two approaches would be highly useful.