1.

INTRODUCTION

Relative pose estimation is a popular topic in computer vision and robotics. It usually uses the environmental information captured by the camera to compute the 6-degree-of-freedom pose estimation. Recently, many camera motion estimation methods have been proposed by using different kinds of sensors, such as monocular, stereo camera, Lidar and RGB-D camera, which meets various application scenarios. Among them, the RGB-D camera brings more attention to the researchers since it can provide both depth and visual information.

2.

RELATED WORK

Visual Simultaneous Localization and Mapping and relative pose estimation are two topics, which are tightly related. They finally get 3D information results from visual sensor data usually in real-time. The feature-based methods^1-4 generally estimate the camera poses by tracking keypoints and minimizing the geometric projection error. Among them, ORB-SLAM⁴ is a popular SLAM system, which is point-based and monocular. However, in low-textured environments, where feature points are very few, such point-based methods may fail.

To solve these problems, sensor fusion based methods with an RGB-D camera have been proposed^5,6. Their availability at low cost has facilitated a large number of methods to solve fundamental problems in computer vision in the last decade. ORB-SLAM2⁷ further expand to stereo and RGB-D sensors. Some persons⁸ make the depth data more reliable and correct it. And Reference⁹ reduces the noise in depth data by using a per-pixel polynomial model calibration technique. They all improve the localization accuracy.

But traditional geometric methods may be easy to drift if only use point features for camera pose estimation. If points, lines, and planes features are all used, this limitation can be overcome in some way. Reference¹⁰ used points and lines and extracted 2D and 3D information from them to build a geometric constraint model. As for ORB-SLAM3¹¹, lines and planes are both used to get better performance in low-textured scenes. Reference¹² also uses points, lines, and planes as structural features to estimate the pose.

In addition, the Manhattan-world constraint is a good way to avoid rotation drift in SLAM algorithms for the indoor scene. In Reference¹³, authors thought that inaccurate rotation estimations can cause drift mostly. So improving the accuracy of rotation estimation is the key to produce lower drift. For example, Reference¹⁴ utilized environmental regularity to track keypoints and estimate the translation. Reference¹⁵ proposed a framework which can robustly benefit the MW structure. For the benefit of the Manhattan assumption, many approaches can produce a small accumulated rotational error.

In this paper, focusing on the case where the robot is moving in indoor environments, we propose an efficient pose estimation system with an RGB-D camera under weak Manhattan world assumption. In general, indoor environments usually have many planar structures, such as floor, ceiling and walls, and the weak Manhattan world assumption is to assume that the floor and ceiling are perpendicular to the walls. It is practical in our real world. In addition, we consider the indoor floor flat, which means that the roll and pitch angles remain constant when the robot is moving. In this case, the 6-degree-of-freedom (DOF) problem can be reduced to a 3-DOF problem. With the above assumptions, we propose a new method for robot motion estimation and the novelties of this paper are:

3.

PROPOSED METHOD

In our framework, see the overview in Figure 1, since the robot is with an RGB-D camera rigidly, we can estimate the normal vector of the floor using the depth information, and the y-axis of the camera can be aligned with the normal vector of the floor (illustrated in Figure 2a)). In this case, the camera pose can be simplified and the motion model can be parameterized efficiently. The inverse-depth histogram image is used to extract the normal vector and estimate the relative rotation. We align the local camera coordinate with the world coordinate so that we can calculate a drift-free rotation. Once the rotation is obtained, the translation can be found using a 1-point correspondence.

Figure 1.

Overview of the proposed motion estimation framework.

Figure 2.

(a): Align the $y$-axis of the camera with the normal vector of the floor; (b): A view of a corridor map. It consists of several Manhattan structures. Different color represents different local Manhattan structure; (c): Walls in different Descartes frames. For example, the Red Manhattan frame can align Walls 1 and 3. The Blue Manhattan frame can be with Wall 2.

We extract and match keypoints in the RGB image and fit lines in the inverse-depth image. Then we use the u-intercept induced motion estimation.

3.1

Aligning the camera with a known direction

With this processing, the rotation between frames is simplified to 1-DOF. be the unit normal vector of the floor in the robot’s camera coordinate. In this stage, our goal is to get a rotation matrix R_n which can make the y-axis of robot’s camera align with the normal vector of the floor: . As we know, the matrix R_n can be represented by the Euler-Rodrigues formula. Finally, the matrix we look for can be calculated as:

With , . Our relative pose estimation framework is based on this initialization.

3.2

Estimating the normal of planes via inverse-depth

Enlightened by the u-v disparity of the stereo vision, the inverse-depth induced histograms show up. And We propose an efficient method based on that to directly extract the normal of the horizontal plane. A horizontal plane in the i^th camera coordinate and the pinhole camera model can be defined as follows:

where [X Y Z]^T and [u υ 1]^T are the 3D point and the point of the image acquired by the camera respectively.

Using equation (2), we can get the relationship between v and Y. And let which is defined as the inverse-depth up to scale, we finally obtain:

We can find that equation (3) (right) represents a straight line of (l_h, υ) respectively in the horizontal-histogram image as shown in Figure 3b. Let s_i, υ_i represent the slope and v-intercept of the line, we have the following relation:

Using equations (2) (left) and (4) (right), the normal n_ih of a horizontal plane in the i^th frame can be written as:

Once the y-axis of the robot’s camera is aligned with the normal vector of the floor, a vertical plane in the i^th camera coordinate can be defined as X = a_iZ + b_i. In a similar way to the above, the normal n_iυ of a vertical plane in the i^th frame can be written into: n_iυ = (1 0 – a_i)^T.

Figure 3.

(a): An RGB image; (b): The horizontal histograms of inverse-depth; (c): The line fitting result using RANSAC algorithm; (d): The corresponding depth image; (e): The vertical histograms of inverse-depth; (f): The line fitting result using RANSAC algorithm.

Since the camera is calibrated, f_y, c_y, f_x, c_x are known and the normal of the plane is only associated with the v, u-intercept. Thus, we can extract the horizontal normal directly with line fitting (Figure 3c) by using RANSAC algorithm from the horizontal-histogram image (Figure 3b)), and (Figures 3f and 3e) for the vertical normal. In addition, this scheme can eliminate the impact of the moving objects, e.g., people in Figure 3a. Once we calculate the normal of the vertical plane, we obtain a local Manhattan frame.

4.

EXPERIMENTS

We test our method and result on large-scale real data. The real-world RGB-D datasets that satisfy weak Manhattan constraints are collected by the authors. We use a robot mounted with a Kinect V2 to capture the scenes (Figures 4a and 4c). The resolution of these images is set to 960 x 540. The other datasets are from Reference⁹, called Lee’s data (Figure 4b), at a resolution of 640 x 480. These real-world datasets do not have ground truth. Though there are some public datasets containing ground truth, most of them focus on general motion and do not apply to our application.

Figure 4.

Comparing our method with ORB-RGBD SLAM in three largescale indoor sequences. (a): Basement (200 m); (b): Laboratory building (190 m); (c): School building (300 m). First column: Example images. Second column: Reconstructed point cloud. Third column: Trajectories of ORB-RGBD SLAM. Fourth column: Trajectories of ours.

5.

CONCLUSION

We propose a novel relative pose estimation framework for an indoor robot with an RGB-D camera. Unlike previous work, without fitting planes, we estimate the relative camera motion using planar structures that are extracted directly from the inverse-depth induced histograms. By aligning the current local coordinate with the world coordinate, the rotation estimation is drift-free. Furthermore, we present datasets that satisfy the weak Manhattan world assumption in indoor environments. The experiments on both public and the authors recorded data suggest that our method gives very excellent results in very challenging indoor scenarios.