A Novel Feature-Level Data Fusion Method for Indoor Autonomous Localization

Abstract

We present a novel feature-level data fusion method for autonomous localization in an inactive multiple reference unknown indoor environment. Since monocular sensors cannot provide the depth information directly, the proposed method incorporates the edge information of images from a camera with homologous depth information received from an infrared sensor. Real-time experimental results demonstrate that the accuracies of position and orientation are greatly improved by using the proposed fusion method in an unknown complex indoor environment. Compared to monocular localization, the proposed method is found to have up to 70 percent improvement in accuracy.

1. Introduction

For achieving seamless positioning and navigation in degraded or denied unknown areas such as indoors [1, 2], various new technologies have replaced the Global Positioning System (GPS) [3–6] and Inertial Navigation System (INS) [7] as the mainstream technologies in indoor autonomous localization field. The most popular one is visual Simultaneous Localization and Mapping (SLAM) [3, 8, 9]. One of the key technologies in SLAM is the autonomous localization which is localizing oneself in an unknown environment. The other one is recreating a map of this environment. These two tasks are coupled to each other: an accurate localization is benefited to mapping, and a good map is crucial to localize oneself. For this reason, both of the two tasks must be performed at the same time [10].

The key point of good localization is to ensure the correct processing of geometrical information gathered by cameras. Three types of methods have been considered for autonomous localization estimation. (a) Monocular Autonomous Localization: in an early stage, researchers prefer to use a monocular method for its simple device structure. A good example of this method was introduced by Civera et al. in [11, 12]. 1-point Random Sample Consensus (RANSAC) based on Extend Kalman Filter (EKF) was used as the data association method. They also showed real-time feasibility of monocular SLAM with affordable hardware which cannot provide depth information, using the EKF algorithm to get the depth information from features information [13], but a long convergence time for obtaining depth information of the monocular method is required. (b) Multisensor of same “type”: stereovision SLAM is a typical example of multisensor of same “type” which is considering two linked cameras as a single 3D sensor [14, 15]; SLAM has the defects of limited 3D estimation range and mechanical fragility. A stereovision sensor can only provide a reasonable 3D estimation up to infinity. (c) Multi-sensor of different “types”: with the using of data fusion method and filtering in the field of multi-sensor, researchers are paying attention to the multi-sensor methods recently [16–18]. Multi-sensor of different “types” can provide more types of feature information for enhancing estimation accuracy and reducing strong anti-interference. In order to further improve the performance of the multi-sensor of different “types” in SLAM, we introduce a novel feature-level data fusion for indoor autonomous localization. The basic idea is of fusing camera and infrared sensor information with each other. In contrast to the laser sensor, the advantage of low cost is also outstanding with this type of sensor.

This paper is organized as follows. Our system framework is introduced in Section 2. Section 3 describes proposed feature-level data fusion based indoor localization method. Section 4 presents the experimental results. Section 5 gives the conclusion of the paper.

2. Framework of Proposed System

Among the various SLAM methods [10], the most popular one is the monocular SLAM [11]. In the literature, feature detection [19] and data association and prediction are important parts of SLAM. Feature detection is to extract features in images, and one can localize the camera by analyzing the relative position between these points and the camera. Data association maps the feature points between two adjacent frames. Because of the lack of depth information, the initial depth information is usually stated before the localization, and the depth information is then estimated after several iterations of the sequential estimators such as EKF.

In this section, we present a framework that supports the feature-level data fusion based autonomous localization as shown in Figure 1. In the proposed localization method, features in the unknown area are first detected. The feature-level data fusion based autonomous localization in Section 3 will then be performed. It is designed to deal with two problems: (a) the unavailable initialized depth information of the monocular method and (b) self-calibration of the multiple dissimilar sensors.

Figure 1 

Framework of proposed system.

Figure 1 is the block diagram of the proposed system. The state vector 𝑋^⊤ can be represented by the set of camera 𝑥^⊤ and the set of feature points information 𝑝^⊤_𝑖: 𝑋^⊤=𝑥^⊤𝑝^⊤₁⋯𝑝^⊤_𝑛,𝑥=𝑟^WC;𝑞^WC;𝑣^𝑊;𝜔^𝑊, (1) where 𝑥^⊤ contains the position and orientation as a 6-DOF [20] constant velocity model, 𝑝^⊤_𝑖 contains the world coordinate of feature points, WC represents the world coordinate, 𝑞 is the direction of the sensor view, and 𝑣 and 𝜔 are the velocity and angular velocity.

Consider the following:𝑓_𝑘+1=𝑟^WC_𝑘+1𝑞^WC_𝑘+1𝑣^WC_𝑘+1𝜔^WC_𝑘+1=𝑟^WC_𝑘+𝑣^𝑊_𝑘+𝑉^𝑊_𝑘𝑞Δ𝑡_𝑊𝑘×𝑞𝜔^𝑊_𝑘+Ω^𝑊𝑣Δ𝑡_𝑊𝑘+𝑉^𝑊𝜔^𝑊_𝑘+Ω^𝑊. (2)

In (2), 𝑘+1 refers to the step number, 𝑞(𝜔^𝑊_𝑘+Ω^𝑊) transforms the local orientation vector into a quaternion, 𝑉^𝑊_𝑘 and Ω^𝑊 are the impulse of velocity and angular velocity, and Δ𝑡 is the delay between 𝑘 and 𝑘+1. From the velocity model, the EKF prediction of the state vector can be described: 𝑥_𝑣(𝑘+1∣𝑘)=𝑓_𝑣(𝑥(,𝑃𝑘∣𝑘))_𝑣(𝑘+1∣𝑘)=𝐹_𝑣𝑃_𝑣(𝑘∣𝑘)𝐹^𝑇_𝑣+𝐺_𝑘−1𝑄_𝑘−1𝐺^𝑇_𝑘−1, (3) where 𝑥_𝑣 stands for the prediction of position and orientation of camera, 𝐹_𝑣 is the Jacobian of 𝑓_𝑣, 𝑄_𝑘 is the covariance of the zero-mean Gaussian noise assumed for the dynamic model, and 𝐺_𝑘 is the Jacobian of this noise.

Consider the following:𝑥(𝑘+1∣𝑘+1)=𝑥(𝑘+1∣𝑘)+𝐾×𝑧−𝑧^′,𝑃(𝑘+1∣𝑘+1)=(𝐼−𝐾𝐻)𝑃_{𝑘+1∣𝑘}, (4) where 𝑧 is the updated features information, 𝑧^′ is the observation of 𝑥(𝑘+1∣𝑘), 𝐾 stands for Kalman gain, and 𝐻 is the Jacobian of the measurement equation. With the state vector and covariance matrix, the prediction of the camera motion is available.

The data association block in the framework can reduce the size of the data set and 1-point RANSAC [11] is a paradigm for the model. The number of RANSAC iterations can be greatly reduced and hence the computational cost. After the data association, the features of the adjacent two frames will be linked, deleted, or created.

3. Proposed Feature-Level Data Fusion Based Autonomous Localization

In this section, we present an autonomous localization method based on a novel method of feature-level data fusion. Because of the difference between camera and infrared sensor, depth information cannot be projected to an image with high accuracy. The proposed method provides the fused information of depth and image through the edge information as shown in Figure 2. With the known pose of the camera, the world coordinates of feature points can be inferred. Similarly for the features, the pose can also be calculated.

Figure 2 

Replacement of fused information to monocular information.

3.1. Camera and Infrared Sensor Geometric Modeling

A 3D point 𝑝 in the scene and its corresponding projection 𝑝_𝑖 are related by the following equations. We use 𝑋, 𝑌, 𝑍 to represent the world coordinates and 𝑢, 𝑣 for the image coordinates: 𝑋𝑌𝑍1𝑝=,𝑝_𝑖=𝑢𝑣1,𝑝_𝑖=𝐾[𝑅∣𝑡]𝑝, (5) where 𝐾 is the camera calibration matrix, 𝐾^′ is the infrared sensor calibration matrix, 𝑅 is the rotation matrix, and 𝑡 is the translation vector between world and camera coordinate systems.

Consider the following:𝑓𝐾=^𝑥0𝐶_𝑥0𝑓_𝑦𝐶_𝑦001,𝐾^′=𝑓^′_𝑥0𝐶^′_𝑥0𝑓^′_𝑦𝐶^′_𝑦001, (6) where 𝐾 is defined by 𝑓_𝑥 and 𝑓_𝑦 referred to the focal length of the camera in the 𝑥-axis and 𝑦-axis and 𝐶_𝑥 and 𝐶_𝑦 are the elements of the principal point vector. Infrared sensor has the same 𝑅 and 𝑡 with the camera. The corresponding projection in the infrared sensor is 𝑝_𝑑, and the relation between 𝑝_𝑖 and 𝑝_𝑑 can be expressed as 𝐾⁻¹𝑢𝑣1=[]𝑋𝑌𝑍1𝑅∣𝑡=𝐾^′−1𝑢^′𝑣^′1. (7)

3.2. Feature-Level Data Fusion Method of Image and Depth

While in Section 2 most feature points extraction methods are concentrated on the edges of objects, the depth information of these places is not accurate. In order to solve this problem, we use a feature-level data fusion method based on edge information by three steps. Before performing data fusion, edge information is chosen as features by analyzing the image and depth information. Comparing various methods, we select the canny operator to extract. The block diagram of the proposed method is shown in Figure 3.

Figure 3 

The architecture of autonomous localization estimation using multisensor.

First, 𝐸=𝐼(𝑥,𝑦) is one edge point of the image 𝐼, 𝐸^′=𝐷(𝑥,𝑦) is the projection of 𝐼(𝑥,𝑦) on the depth information 𝐷, and 𝑀 is the cross-shaped matrix with center point at 𝐸^′. It is constituted by the next 4𝑖 points around 𝐸^′ as shown in Figure 4.

(a)

(b)

(c)

Figure 4 

The processes of line alignment in the feature-level data fusion.

Consider the following:𝑀(𝑖,𝑖)=𝐸^′,𝐷.=𝐷𝑥,𝑦𝑀=0⋯𝐷𝑥,𝑦−𝑖⋯0⋮⋱⋮⋰⋮𝑥−𝑖,𝑦⋯𝐷𝑥,𝑦⋯𝐷𝑥+𝑖,𝑦⋮⋰⋮⋱⋮0⋯𝐷𝑥,𝑦+𝑖⋯0 (8)

Second, the nearest nonzero point to 𝐸^′ in 𝑀 named 𝑃 which is the point used for edge alignment should be obtained. We assume that 𝑃=𝑀(𝑚,𝑛) which means that 𝑚+𝑛 is the minimum among all non-zero points in 𝑀.

Third, moving 𝑃=𝑀(𝑚,𝑛) forward 𝐸^′ in horizontal or vertical direction, and then extracting the depth information of the point next to 𝑃 (the one further than 𝑃 from 𝐸^′), we can fill in all the empty points between 𝑃 and 𝐸^′. Since the relations between 𝑚, 𝑛, and 𝑖, there are 4 solutions that should be considered. 𝐷^′ is the fused depth information described in the following: 𝐷^′(),𝐷𝑥+𝑚−𝑖∶𝑥,𝑦=𝐷𝑥+𝑚−𝑖−1,𝑦𝑚<𝑖,𝑛=𝑖_′(),𝐷𝑥∶𝑥+𝑚−𝑖,𝑦=𝐷𝑥+𝑚−𝑖+1,𝑦𝑚>𝑖,𝑛=𝑖_′(),𝐷𝑥,𝑦+𝑛−𝑖∶𝑦=𝐷𝑥,𝑦+𝑛−𝑖−1𝑚=𝑖,𝑛>𝑖_′𝑥,𝑦∶𝑦+𝑛−𝑖=𝐷𝑥,𝑦+𝑛−𝑖+1(𝑚=𝑖,𝑛<𝑖). (9)

3.3. Pose Estimation

In order to register a feature point 𝑥_𝑖=[𝐼_𝑥,𝐼_𝑦]^⊤ into the world coordinate as a 3D point 𝑥=[𝑋,𝑌,𝑍]^⊤, these parameters should be known, the pose (world coordinate 𝐶=[𝐶_𝑥,𝐶_𝑦,𝐶_𝑧]^⊤, angles between camera view and initial camera view are 𝜃 and 𝜑, 𝜃 is horizontal and 𝜑 is vertical) of camera, depth 𝑑 of the feature point, and vertical and horizontal visual angles of camera view which are 𝛼 and 𝛽 and 𝑚×𝑛 is the sizes of image. The equation for registering 𝑥_𝑖 into the world coordinate 𝑥 will be 𝑋=𝑑cos𝜑cos𝐼_𝑥𝐼𝛼/𝑚sin𝜃+^𝑥𝛼𝑚+𝐶_𝑥,𝑌=𝑑cos𝜃cos𝐼_𝑦𝐼𝛽/𝑚sin𝜑+^𝑦𝛽𝑚+𝐶_𝑦,𝑍=𝑑cos𝜃cos𝜑+𝐶_𝑧,𝑥=𝑑cos𝜑cos𝐼_𝑥𝐼𝛼/𝑚sin𝜃+^𝑥𝛼𝑚+𝐶_𝑥,𝑑cos𝜃cos𝐼_𝑦𝐼𝛽/𝑚×sin𝜑+^𝑦𝛽𝑚+𝐶_𝑦,𝑑cos𝜃cos𝜑+𝐶_𝑧. (10)

After introducing the projection of features from image plane into the 3D world coordinate, the relation between camera pose and 3D feature points in world coordinate will be described in this paragraph. Before the depiction of the processes, a problem should to be noticed. Assume a feature point 𝑃₁ is registered in world coordinate and the depth information is available. In that cases the camera move along the tangent of the circle whose center is feature point 𝑃₁. Even though there is no change in the image projection and depth of 𝑃₁, the pose of the camera should not be considered as being changeless. So, the number of features used for camera pose estimation should be more than two. In order to finish this calculation, the following parameters must be known as shown in Figure 5: (1) the world coordinate of feature points 𝑥₁=[𝑋₁,𝑌₁,𝑍₁]^⊤ and 𝑥₂=[𝑋₂,𝑌₂,𝑍₂]^⊤ (𝑋₁≠𝑋₂, 𝑌₁≠𝑌₂), (2) the projections of feature points on image 𝐼₁=[𝐼_𝑥1,𝐼_𝑦1]^⊤ and 𝐼₂=[𝐼_𝑥2,𝐼_𝑦2]^⊤, (3) the depth information of feature points 𝑑₁ and 𝑑₂, (4) the vertical and horizontal visual angles of camera view 𝛼 and 𝛽, (5) the size of images 𝑚 and 𝑛. Assume that the camera world coordinate is 𝐶=[𝐶_𝑥,𝐶_𝑦,𝐶_𝑧]^⊤, angles between camera view and 𝑥-axis and 𝑦-axis are 𝜃 and 𝜑. One can calculate the camera pose using following equations. Obviously, 𝐶_𝑥, 𝐶_𝑦, 𝐶_𝑧, 𝜃, and 𝜑 can be derived by solving the previous equations. So, the pose of camera can be obtained through the previously mentioned parameters.

Figure 5 

The relationships between camera pose and features.

Consider the following:𝐼_𝑥1=tan⁻¹𝐶_𝑥−𝑋₁𝐶_𝑧−𝑍₁𝑚−𝜃𝛼,𝐼_𝑦1=tan⁻¹𝐶_𝑦−𝑌₁𝐶_𝑧−𝑍₁𝑛−𝜑𝛽,𝐼_𝑥2=tan⁻¹𝐶_𝑥−𝑋₂𝐶_𝑧−𝑍₂𝑚−𝜃𝛼,𝐼_𝑦2=tan⁻¹𝐶_𝑦−𝑌₂𝐶_𝑧−𝑍₂𝑛−𝜑𝛽,𝑑₁cos𝜑cos𝐼_𝑥1()𝐼𝛼/𝑚sin_𝑥1𝛼𝑚+𝜃=𝑥₁. (11)

At last, consolidating the previous equations, if the camera pose in time 𝐾 is known, the image coordinates of feature points and their depth are known all the time, and feature points information is stable, then camera pose in time 𝐾+1 can be calculated, and the world coordinates of feature points in time 𝐾+1 can also be calculated through the previously described data. Therefore, considering the mathematical induction, as long as there are more than two feature points that can be found in the image later, the camera pose can be obtained in every moment. The algorithm ultimately achieves autonomous localization estimation.

4. Experiments and Results Analysis

4.1. Experimental Platform

In this experiment, the Kinect [21, 22] sensor is used. Kinect is equipped with two sets of lenses. The middle sensor is the RGB camera (color CMOS) with a resolution of VGA (640∗480). The infrared transmitter in the left and the receiver in the right comprise a set of 3D depth sensors with a resolution of QVGA (320×240). The hardware operational principle which consists of camera, infrared transmitter and infrared receiver is show in Figure 6. The feature on image plane projects in world coordinates through the information from these sensors. The frame rate is 30 frames per second. The software platforms for Kinect are OpenNI [23] and OpenCV [24] which are under 𝐶. Kinect classifies all the information into two types: color images and depth information. In a depth information, each item of one pixel (one pixel is 16 bits or 2 bytes) expresses the distance of the object closest to the sensor under the specified 𝑥, 𝑦 coordinates. Depth distance ranges generally from 850 mm to 9000 mm. The depth 0 indicates unknown situation, for example, the projection of glass (low reflection) and mirror (high reflection). Stream support: 640×480, 320×240, and 80×60.

Figure 6 

Operational principle of the combined hardware of camera and infrared sensors.

4.2. Experimental Results and Analyzation of Data Fusion

The image and the depth information of the carton shown in Figure 7 are used for experiments.(1)After calibration, the internal reference of infrared sensors and cameras is 𝐾 and 𝐾^′. Since the rotation matrixes and translation matrixes of them are the same, mapping relation of the pixel between depth information and images can be calculated through the equation as (7).(2)The edge detection results of the depth information and image are shown in Figure 8, the red lines represent the image edges and the blue lines represent the depth information edges.(3)After the alignment of the edges, the results of the feature-level fused data fusion are shown as follow. The left one is the original data and the fused data is on right.

(a)

(b)

Figure 7 

Image from CCD (a) and infrared sensor data (b).

Figure 8 

Edges of image (red line) and depth information (blue line).

From the edge parts in Figure 9, we can find that there are a lot of glitches in the raw data. Comparing to the left one, the right image has a qualitative leap on the carton edges. After removing the glitch noise, the fused information is ready for further data analysis.

(a)

(b)

Figure 9 

Comparison of original data (a) and fused data (b).

4.3. Indoor Autonomous Localization Estimation Error and Analysis

In the autonomous localization experiments, the chosen scene is a complex indoor environment shown in Figure 10. All of the depth information is continuously distributed in a range of 0.85 m to 9 m. In order to evaluate the positioning method comprehensively, two experiments are conducted in this part. The sensors are in uniform rectilinear motion in the first one and nonuniform nonlinear motion in the second one. In the first experiment, the camera moves 2.2 m from original point along the negative 𝑥-axis. The sensor Kinect is placed on a cart moving at a speed of 2 cm/s in uniform motion. In order to simulate human behavior, we conduct the second experiment. In the second one, the camera moves 1.6 m uniformly along the positive 𝑦-axis from the original point at first, then moves along a quarter circle with a radius of 1 m and the tangent of the cycle is 𝑦-axis. The speeds of the uniform motion and the quarter circle are 4 cm/s and 2 cm/s.

(a)

(b)

Figure 10 

Experimental environment and its depth information.

The infrared sensor and the camera obtain information of depth and images at the frequency of 2.5 Hz, the images are of 640×480 color pixels and the depth information is of 16-bit grey level 320×240 pixels. The raw data from sensors are recorded in a hard drive for postexperiment analysis. There are three kinds of data as follows: (1) the images without depth information, (2) the images and the raw depth information, and (3) the images and the fused depth information.

The positioning cumulative errors and the positioning Root-Mean-Square (RMS) errors of the experiments are shown as figures. Figures 11 and 12 are for the first experiment and others are for the second experiment.

(a)

(b)

(c)

Figure 11 

The results of the first experiment. The cumulative errors of the positioning along 𝑥-axis (a), the positioning along 𝑦-axis (b), and the positioning error (c).

(a)

(b)

(c)

(d)

Figure 12 

The results of the first experiment. The RMS errors of the positioning (a), the orientation estimation (b), positioning along 𝑥-axis (c), and positioning along 𝑦-axis (d).

Figure 11 explains the positioning cumulative errors from the respects of the error in the Euclidean coordinate, along the 𝑥-axis and the 𝑦-axis. The first site represents that fused data can improve accuracy by 70 percent along 𝑥-axis with reducing the error from 0.5 m to 0.15 m. The second site explains that the fused data can improve accuracy by 67 percent along 𝑦-axis with error reducing from 0.45 m to 0.15 m. The third one explains that the fused data can improve accuracy by 67 percent in the Euclid distance. So, these results show that the localization method based on feature-level data fusion holds more advantages in the direction of motion and this method increases about 70 percent accuracy of the positioning.

In Figure 12, the positioning RMS errors are shown from 4 respects: the error in the Euclidean coordinate (position and attitude), the error along the 𝑥-axis and the 𝑦-axis; they are the evaluations for stability of the localization method. By comparing the third and the fourth sites, we find that the localization method based on fused information has excellent performance in the direction of motion. At the same time, by comparing the first and the second sites, the significant improvement on the stability of the pose estimation is also considerable.

The same arrangement with Figures 11 and 13 can also be divided into three parts: the error in the Euclidean coordinate, along the 𝑥-axis and the 𝑦-axis. The first site shows the improvement of the error along 𝑥-axis with reducing the error from 0.35 m to 0.02 m. The accuracy of localization is improved about 60 percent along 𝑦-axis and in the Euclidean coordinate with the error reducing from 1 m to 0.3 m.

(a)

(b)

(c)

Figure 13 

The results of the second experiment. The cumulative errors of the positioning along 𝑥-axis (a), the positioning along 𝑦-axis (b), and the positioning error (c).

Figure 14 shows the results of the positioning RMS errors of the second experiment; the method also can be evaluated from four respects as in Figure 12. From these sites, we can find that the localization method based on feature-level fused information holds a low RMS error level by contrastive analysis and practical calculation.

(a)

(b)

(c)

(d)

Figure 14 

The results of the second experiment. The RMS errors of the positioning (a), the orientation estimation (b), positioning along 𝑥-axis (c), and positioning along 𝑦-axis (d).

5. Conclusion

We showed a novel feature-level data fusion based autonomous localization method for unknown indoor environment in this paper. Accuracy estimation was discussed for this method with real-time experiments. The experiment was conducted in an inactive multiple reference unknown indoor environment, and the platform consisted of a Kinect and a laptop. In the first experiment, the sensors move in uniform linear motion. The sensors move in a nonlinear and nonuniform motion in the second experiment. The positioning error of the three types of information from different data fusion methods was compared in the experiment. The first experimental results proved that the accuracy improvement of the autonomous localization based on feature-level data fusion guarantees up to 70 percent by comparing to monocular SLAM (without depth information) and 50 percent to pixel-level data fused information basing positioning method. The second experimental results show that the positioning method also holds practicality in the real environment. So, the advantage of feature-level data fused information is applied into two problems: data fusion method of multi-sensor of different “types” and the reliability of the visual SLAM.

Acknowledgments

This project is supported by the National Natural Science Foundation of China (no. 61261010), the China Postdoctoral Science Foundation funded project (no. 2012M521291), the Jiangxi Province Postdoctoral Science Foundation funded project, the Science and Technology Project of Jiangxi Provincial Department of Education (no. GJJ12006), and the National 863 Program (no. 2013AA013804).