Abstract
This paper presents a novel improved RANSAC algorithm based on probability and DS evidence theory to deal with the robust pose estimation in robot 3D map building. In this proposed RANSAC algorithm, a parameter model is estimated by using a random sampling test set. Based on this estimated model, all points are tested to evaluate the fitness of current parameter model and their probabilities are updated by using a total probability formula during the iterations. The maximum size of inlier set containing the test point is taken into account to get a more reliable evaluation for test points by using DS evidence theory. Furthermore, the theories of forgetting are utilized to filter out the unstable inliers and improve the stability of the proposed algorithm. In order to boost a high performance, an inverse mapping sampling strategy is adopted based on the updated probabilities of points. Both the simulations and real experimental results demonstrate the feasibility and effectiveness of the proposed algorithm.
1. Introduction
RANSAC algorithm is one of the popular methods for sensor data registration and modeling. In some vision-based SLAM (Simultaneous Localization and Mapping) algorithms, RANSAC algorithm provides an efficient solution for image matching procedure and establishes the data association among different views [1–3]. There are two typical types of algorithm for image matching, the dense way [4, 5] and sparse way [6, 7]. In dense match approach, the whole image is used for parameter estimation [8, 9]. Although this method is quite robust, it may be inaccurate when occlusion regions exist in the matching images. Sometimes, the influence of occlusions is reduced by using a robust weighted cost function. In sparse match step, image features are detected by using SIFT, SURF, or any other feature detection algorithms. The features are matched by using the distance of feature descriptor, and the matching pairs are sometimes ambiguous. To efficiently achieve a correct matching result, some robust algorithms were adopted to remove mismatching pairs, such as M-estimation [10], LMedS (Least Median of Squares) [11], or RANSAC (Random Sample Consensus) [12] algorithm. M-estimation established a new cost function with a robust weight. It worked well in some cases but was vulnerable to the noise. LMedS optimized the model by minimizing the median of errors. When the outlier rate was larger than 50%, M-estimation and LMedS might be no longer applicable. With the advantages of easy implementation and strong robustness, RANSAC algorithm was widely used in model parameter estimation problem. In standard RANSAC algorithm, a hypothesis set was randomly selected to estimate a parameter model. And an inlier set was detected by testing all input data with the estimated parameter model. A maximum size of inlier set was expected to be found within a predetermined iteration. However, the performance of this standard RANSAC was sometimes low. Even worse was the fact that the solution may not be reached when all iterations were finished.
To efficiently improve the performance of standard RANSAC algorithm, some methods have been proposed in recent decades. A hypothesis evaluation function and local optimization procedure were adopted to achieve a more accurate result. MSAC (M-estimation SAC) [10] evaluated the test point set with a bounded loss function to achieve a maximum likelihood consensus set. MLESAC (Maximum Likelihood SAC) [10] evaluated the hypothesis set by using the probability distribution of errors. The inlier error was modeled with an unbiased Gaussian distribution and outlier error used a uniform distribution. The maximum likelihood estimation was solved by minimizing a cost function. MAPSAC (Maximum A Posteriori Estimation SAC) [13] followed a Bayesian approach to solve the RANSAC problem with a robust MAP estimation. LO-RANSAC (Local Optimized RANSAC) [14] adopted an inner model reestimation procedure to improve the accuracy of the RANSAC algorithm.
Moreover, some heuristic mechanism sampling strategies and partial evaluation procedures were adopted to speed up the convergence of the algorithm. It seemed that a good sample strategy will reduce the time cost which was spent in finding the solution. A hypothesis set was selected based on the probabilities of test points in the Guided MLESAC [15]. In PROSAC (Progressive SAC) [16], the matching score was used as a prior knowledge for sorting the test data. A hypothesis set was selected among the data which was in the top-ranked matching score. It was also progressively tested on the less ranked data. In some extreme cases, the whole data would be tried in this algorithm. According to the assumption that an inlier tends to be closer to the inliers, NAPSAC (N Adjacent Points SAC) [17] sampled the data within a defined radius around a selected point. Based on the preliminary test of the hypothesis, Chum and Matas proposed R-RANSAC (Randomized RANSAC) [18] and R-RANSAC SPPR (Sequential Probability Ratio Test) [19]. These two methods performed a preliminary test based on and SPPR test after evaluating test points in every iteration. And a full test procedure was performed only when a hypothesis set passed the preliminary test. The preliminary test procedure effectively removed the obvious mistakes of hypothesis sets and improved the efficiency of RANSAC algorithm. Optimal RANSAC [20] adopted an inlier sample procedure to achieve a more accurate model estimation. When the size of the current inlier set was larger than a threshold, an inlier sample procedure would be performed to achieve a more reliable solution in the inlier set.
Furthermore, some intelligent algorithms such as Genetic Algorithm (GA) and multilayered feed-forward neural networks (NFNN) were also proposed in RANSAC algorithm. Rodehorst proposed a novel RANSAC algorithm based on GA [21]. In GASAC, the parents were generated by a standard RANSAC algorithm with a robust cost evaluation. Then, the best solution was achieved by using crossover and mutation operators on parents in GA procedure. Moumen presented a rather comprehensive study of robust supervised training of MFNN in a RANSAC framework from the standpoint of both accuracy and time [22]. In the iteration of RANSAC, the parameter model was estimated by using a small MFNN which was minimizing the mean squared error (MSE) with a standard back propagation algorithm. All inlier points were used to reestimate a new parameter model by training a new MFNN. And a new hypothesis set was achieved by using this new MFNN model. The convergence solution was achieved until the inlier set did not change any more.
In this paper, we propose a novel improved probability-guided RANSAC (IPGSAC) algorithm for mobile robot 3D map building. Under the framework of standard RANSAC algorithm, two types of probabilities are evaluated for test points by using a total probability formula and the statistics of maximum size of inlier set. To achieve a more robust evaluation of test points, DS evidence theory [23] is adopted to synthesize the multisource evaluation of test points. Moreover, the theories of forgetting are employed to filter out the unstable inliers. Based on the probability of test points, an inverse mapping sampling strategy is utilized to improve the convergence rate of the proposed algorithm. Finally, this proposed IPGSAC algorithm is applied for the mobile robot 3D map building. All the experimental results show the feasibility and effectiveness of the proposed algorithm.
The rest of this paper is organized as follows: in Section 2, we summarize IPGSAC before explaining each in detail. The components of our robot map building procedure are detailed in Section 3. Our simulation and real experimental results are described in Section 4. Finally, we give our conclusions and future work in Section 5.
2. IPGSAC Algorithm
2.1. Methodology Overview
The proposed IPGSAC algorithm is illustrated in Figure 1. At the beginning of IPGSAC algorithm, the probabilities of test points are initialized with a hybrid distribution. Based on those probabilities, points are selected for model estimation by employing an inverse mapping sampling strategy. The inlier and outlier sets are distinguished with a tolerance threshold . When the residual error of point is larger than , the is identified as an outlier point. Then, two types of probabilities are evaluated for test points by using a total probability formula and the statistics of maximum size of inlier set. According to the average observations of inlier points, the theories of forgetting are employed to reduce the redundancy of unstable inliers. To achieve a more reliable evaluation, the probability evaluations are synthesized by using DS evidence theory. When the maximum iteration limit is arrived at or the stopping criterion is reached, the main loop of IPGSAC will be finished. Finally, we reestimate the model parameter by using all inliers to achieve a more reliable inlier set with 3~5 iterations. In the standard RANSAC, the minimum number of iterations ensures that a correct hypothesis set is achieved with a determined confidence level at least once and it can be estimated bywhere is the confidence level, indicates the inlier rate, and indicates the minimum number of test points for model estimation. In the proposed IPGSAC algorithm, the maximum number of iterations is limited by (1) with the confidence level 98%.
In this proposed algorithm, a maximum inlier set is expected to be found when the algorithm is converged. To avoid falling into local optimum, a pseudo inlier rate is used to limit the minimum size of inlier set. The stopping criterion is described as follows:where is the inlier set which is acquired in the current iteration, is the number of points in , indicates the maximum inlier set so far, and is the pseudo minimum inlier rate. is the total number of test points. Operator “” means that two sets are very similar. The detailed description of each component of the proposed algorithm is given in the following sections.
2.2. Initialization of Probabilities
Inspired by MLESAC algorithm, we assume that the inlier data is satisfied to a normal distribution and the outlier data is fitted to a uniform distribution. The hybrid distribution is described as follows:where is the residual error of with the model estimated by using all data, is the expectation of inlier errors, is the variance of , determines the probability of outlier point, and indicates whether is an inlier or not. When is an inlier point, then ; otherwise . To evaluate the distribution, the length of error interval is calculated with the minimum and maximum of error. The mean and variance of error are used as an initial estimation of and , respectively. By limiting the length of searching interval with the maximum value of and , and are solved by using a standard normal distribution with several iterations. In this proposed algorithm, and .
2.3. Evaluation Inlier Probabilities
In standard RANSAC algorithm, the parameter model is estimated by using a random sample test set. According to this estimated model, the inlier and outlier set are determined in this iteration. While this procedure should be aimless, it needs a large number of iterations to find an optimal solution. This paper aims to provide an effective and reliable inlier evaluation method and speed up the convergence of RANSAC algorithm. In the BaySAC [24], Botterill et al. assumed that the probabilities of test points were independence in the same hypothesis set. Firstly, points were selected to estimate a parameter model . Then, all input points were tested to find an inlier set. The probabilities of inlier points were updated by using (4) and did not change the outlier’s probabilities:where is the consensus set that contains all inlier points and all correct models and indicates a test point. evaluates the probability of a point being an inlier. describes the probability of a point being an inlier, under the condition that the estimated model is not a subset of . The BaySAC is poor when the probabilities of test points are the same. With the proposed initialization of probability, it is sometimes difficult to achieve the convergence of BaySAC. In order to improve the convergence of the proposed algorithm, a multisource evaluation strategy is employed to achieve a higher confident probability analysis of test points. First of all, a full probability update procedure is used to achieve a more reliable and comprehensive evaluation of test points. The total probability update procedure is expressed aswhere is calculated by using (4) and indicates the probability of being an inlier, under the condition that the estimated model is a subset of :where is the residual error of point with the current estimated model. evaluates the probability of the estimated model being a correct model. As the number of the inliers illustrates the correctness of the estimated model, is estimated by using the proportion of inlier in this iteration:where and are the number of inlier and outlier points, respectively. is the total number of test points. Moreover, not only indicates the correctness of current model estimation, but also evaluates the possibility that the test point is an inlier. The statistics of maximum size of inlier set which contains the test points is employed to reevaluate the probabilities of test points in the iteration. The evaluation of inlier and outlier points are defined aswhere is the maximum size of inlier set which contains the test point and indicates the current predicted size of inlier set.
2.4. Probability Fusion with DS Evidence Theory
During the iteration of IPGSAC algorithm, the probabilities of test points are updated by using our evaluation criteria. As it is a type of uncertain information, the probabilities indicate the possibility that a point belongs to an inlier set. Generally, the information is characterized as inherently uncertain, imprecise, and incomplete. The representation and reasoning of uncertainty information are one of the important research issues in artificial intelligence (AI). Many researchers devote themselves to modeling the representation and reasoning of the uncertain information. According to the inherently uncertain characteristics of information, a more reasonable and accurate evaluation is achieved by synthesizing some different sources of information with some specified rules.
DS evidence theory, proposed by Dempster and Shafer, is one of the popular methods for the multisource information fusion. The evidence theory describes the uncertainty of information by using the discernment framework, the basic probability belief functions, and likelihood functions [23]. Suppose that there is a problem written as and the discernment set contains all the possible solutions of . All the elements are mutually independent in the discernment set . The solutions of only take from in any case. is defined as a discernment framework and written as . is a really defined focal element of discernment framework ; is the number of elements in . The power set of a discernment framework contains all subsets of the discernment framework. There are elements in and each of the elements corresponds to a solution for . The power set is composed of the following subsets:(1)It contains the empty set and all of the elements in ;(2)If subset , then ;(3)Excepting (1) and (2), it no longer contains any other subsets.
In DS evidence theory, a probability belief function is adopted to describe the uncertainty of evidence. This probability belief function is defined as , such thatwhere is an empty set, is a probability belief function in , and describes the confidence probability of solution .
Generally, the probability belief function is used to evaluate the probability of a reasoning that endows the supposing of the solutions. The final judgment is generally influenced by many factors. According to the DS evidence theory, a synthesis method is illustrated to fuse two or more basic probability belief functions by using a sum of orthogonal operations. Assume that and are the corresponding probability belief functions which are mutually independent in the same discernment set . For the focal elements , and , , the synthesis operation is described aswhere is conflict coefficient and is regularization factor.
In this proposed IPGSAC algorithm, the status of test samples is assigned as or and the discernment framework Θ is . The probability brief function is written as for each test point. According to Section 2.3, the probability brief function is defined as
2.5. Probability Filtering with Theories of Forgetting
For some low constrained model estimation problem, there exist a few of outlier points which easily satisfy the current estimated model but are not stable. During the DS fusion procedure, this incorrect judgment will have a great influence on the convergence of the proposed algorithm. Even worse is the fact that the sample probability of inlier is finally much lower than the outlier and the algorithm is hardly converged. To further improve the proposed algorithm, the theories of forgetting are used to filter out the unstable inliers and achieve a more stable solution.
According to theories of forgetting, the forgetting curve can be roughly described aswhere is memory retention, is the relative strength of memory, and describes the time. As our proposed algorithm is an online learning procedure, we use (13) to describe the forgetting and reviewing procedure:where indicates the average observations when the inlier set is similar to current maximum set. And the maximum inlier size of is refreshed by .
Figure 2 shows the estimated parameters for the fundamental matrix estimation when the outlier rate is 80%. Figure 2(a) demonstrates a typical online learning procedure with 50 iterations. During the iteration, the probabilities of test points are updated by using a total probability formula. After 30 iterations, a convergent solution is achieved with a stable sample probability of inlier and outlier. In Figure 2(b), the maximum inlier size of point is adopted to reinforce the probabilities of test points by using proposed DS fusion procedure. Due to the incorrect judgment of test points, the probability of outlier is waved between 0 and 0.8. And the sample probability of inlier is decreased after several iterations. Although a correct inlier set is achieved in the 3rd iteration, it becomes more and more difficult to reach the convergence of the algorithm. Figure 2(c) illustrates the result of our proposed algorithm which is guided by proposed probability update procedure. After a correct hypothesis set is found, the probabilities of test points obviously distinguished that the probability of inlier is near to 1 and outlier is near to 0. The influence of the incorrect judgment is filtered by using the theories of forgetting. After a few iterations, the curve of the predicted inlier size is much smoother than illustrated in Figures 2(a) and 2(b) and it is highly consistent with the presetting of the simulation. The results demonstrate the high performance and reliability of the proposed algorithm.
(a)
(b)
(c)
2.6. Inverse Mapping Sampling Strategy
During the procedure of RANSAC, a good sampling strategy is an important guarantee of convergence of algorithm. In IPGSAC, the probabilities of test points are evaluated and merged by using DS evidence theory. To speed up the convergence rate of the proposed algorithm, an inverse mapping sampling strategy [25, 26] is utilized for the hypothesis set selection according to the updated probability of test points. The principle of this sampling procedure is as follows: assume that the probability distribution function of a continuous random variable is written as . is an inverse function of where . and have the same probability distribution function and the probability distribution is expressed as
For a discrete random variable , a cumulative probability distribution function is expressed as
In the inverse mapping sampling strategy, is divided into subsections which is corresponding to sample points. The th subsection is expressed as . When a uniform sampled variable is achieved in the th subsection, the corresponding data of this subsection is the expected output. In our proposed algorithm, the probability of test point is normalized by . And a sampling cumulative density function (CDF) is defined by . The expected output is when the random variable is in the subsection .
In our proposed IPGSAC algorithm, a hypothesis set is randomly selected by using the inverse mapping sampling strategy with the probabilities of test points. When the probabilities of test points are equal, the sampling method obeys a uniform sampling approach and it is consistent with the standard RANSAC algorithm. When those probabilities are different, the procedure will improve the sample probability of the point which has a high probability. When a correct hypothesis set is achieved, especially, the probabilities of inlier will be greatly improved with the proposed evaluation procedure. Then, the sampling probability of these inliers will be increased rapidly. Therefore, it will effectively speed up the convergence rate of the proposed algorithm.
2.7. Implementation of IPGSAC Algorithm
A general framework of the proposed IPGSAC algorithm is shown in Algorithm 1. The proposed algorithm can be divided into the initialization of probability, the main loop, and the final reestimation. At the beginning of this algorithm, some parameters should be defined before the procedure of each part. In the initialization step, the test points’ probabilities are evaluated by the hybrid distribution. Then, the main loop of the proposed algorithm starts to find the solution of model estimation. In this procedure, a hypothesis set containing test points is randomly selected for model estimation by using the inverse mapping sampling strategy. When the model estimation error is larger than the tolerance threshold , the model estimation fails and the probabilities of test points are updated by using (4) with . When the model estimation succeeds, the probabilities of test points are evaluated by using the total probability formula. And the maximum inlier size of test point in is refreshed by current inlier size . To reduce the influence of incorrect judgment, a forgetting factor is computed with the average of observation when an inlier set is similar to current maximum set. Other evaluations of test points are estimated with the maximum inlier size and forgetting factor. A more reliable probability evaluation of test points is achieved by employing the DS evidence theory. Finally, the convergent constraint should be checked in every iteration. When the convergent constraint arrives, the reestimation step is performed in several times to achieve a more reliable inlier set and model estimation.
| ||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
3. Robot 3D Map Building Using IGPSAC
3.1. Preliminaries Definition
Kinect is a low cost depth sensor which is widely used in 3D map building system. Assume that the image and depth captured by Kinect are precalibrated by using the method presented in [27] and the depth is prewarped into the image frame. The intrinsic parameter matrix of RGB camera is written aswhere and are the focal and image center of RGB cameras, respectively. According to these intrinsic parameters, the projection from a 3D point to image pixel is described aswhere is a 3D point in current camera frame and .
During the map building procedure, Kinect is fixed on a mobile robot with a consistent translational speed. We assume that the pose of robot is the same as the Kinect and the world coordinate is set at the location where robot starts to run. At the time , robot’s pose is described aswhere ; is a special rigid transformation group. is the rotation matrix; is the translation vector. According to the Lie algebra, is expressed aswhere , and is define as
3.2. Pose Estimation Based on IPGSAC
In this paper, FAST corner algorithm [28, 29] is employed to detect the features and the BRIEF [30] descriptor is utilized for feature matching. With the image matching pairs, a derivative ICP [31] algorithm is adopted based on the projection errors. Generally, there exist some invalid depth measurement regions in the captured depth images. Suppose that all matching pairs mentioned in this section have the valid depth measurement. The pose estimation model is described aswhere is the projection error and defined as ; is a point in the reference point cloud which is corresponding to the new captured feature . indicates a set of image matching pairs; is the size of .
To achieve the optimization of (21), the projection error is approximated by first-order Taylor expansionwhere .
According to Euler-Lagrange formula, the optimal solution of (21) is solved by using a Newton iteration step with
Due to the ambiguity of image matching procedure, the proposed IPGSAC algorithm is utilized for removing the outliers (mismatching pairs) and achieving an accurate pose estimation. The detail of this pose estimation procedure is similar to Algorithm 1.
3.3. Keyframe Selection with Maximum Matching Distance
With the accurate estimated poses, a 3D Map is built with the captured depth and color data. Considering that there are about 300,000 points in every frame, it is necessary to create a more concise representation of the 3D map. A keyframe selection mechanism is employed with maximum matching distance in the algorithm. The map building procedure will start only when a new keyframe is captured. However, the accuracy of pose estimation is largely dependent on the overlap of the input data. Therefore, a distance threshold and an angle threshold are employed for the keyframes selection. In the procedure of keyframe selection, if the pose estimation between current frame and keyframe fails or the distance or the angle between them is larger than the thresholds, the frame will be chosen as a new keyframe. The principle of keyframe selection is described as follows:where evaluates the procedure of pose estimation and when pose estimation fails or or ; for others, .
3.4. Loop Detection and Optimization
During the map building, the quality of global map is largely dependent on the accuracy of the pose estimation. However, a drift arises with the accumulation of the pose estimation error during the 3D map building experiment. To effectively achieve the global optimization of the 3D map, a vision-based loop detecting method is used to check the loop closure when a new keyframe is captured. During the loop detecting procedure, the relative poses between current keyframe and previous nonadjacent keyframes are estimated by using the pose estimation algorithm. A successful estimation of relative pose means that a loop closure is detected and the optimization algorithm will be performed immediately. In this paper, TORO (tree-based network optimizer) [32, 33] algorithm is employed to achieve a global optimization of the generated map. In TORO algorithm, a tree structure is constructed by using the robot’s poses and relative poses among keyframes. The error function which evaluates the observed and calculated relative pose is minimized to optimize all the estimated poses and achieve a global consistent 3D map. The optimization function is described aswhere is the vector descriptor of robot’s poses, is an observed relative pose between th keyframe and th keyframe, Λ represents, respectively, the mean and the information matrix of a constraint relating the poses, and is a relative tree. is an error function that measures how well the poses satisfy the observed constraint .
4. Experimental Results
4.1. Simulation Results
4.1.1. Line Fitting Simulation
The principle of line fitting is as follows: assume that there exists a point set , the fitting line is expressed as , and satisfy
We generated a random simulation data set which fitted with a given line. The standard deviation of inlier points was set to 1, and the outlier points obeying a uniform distribution in the minimum rectangular region contained the all inliers. The size of test points was set to 500. The outlier rates were set to 20%, 30%, 40%, 50%, 60%, 70%, and 80%, respectively. During the simulation, 2 points were used to estimate the line function. And , were set to 0.05 and 2, respectively. We preformed the RANSAC algorithm with fixed iterations and recorded the recall ratio in different outlier rates.
Figure 3 shows the average recall ratios of different RANSAC algorithms by recording 50 repeat experiments of the line fitting. In Figure 3, the recall ratio of the proposed IPGSAC algorithm is the largest in those five algorithms and it is nearly 100% when the outlier rate is lower than 60%. With the increasing of outlier rate, the recall ratios of other four RANSAC algorithms are rapidly decreased and the proposed algorithm still keeps the highest recall ratio with a small reduction. Table 1 illustrates the statistics of iterations when IPGSAC is convergent in 50 repeat experiments. The result illustrates that the average iteration of the proposed algorithm is much smaller than the maximum number of iterations estimated by (1).
(a) 30 iterations
(b) 50 iterations
Figure 4 shows the estimated parameters when the outlier rate is 80% within 50 iterations. At the beginning of iteration, the sample probability of outlier points was much larger than the inliers. During the iteration, the probabilities of test points were updated by using proposed evaluation rule. After 20 iterations, the probabilities of inlier points were near to 1 and outlier was near to 0. The sample probability of inlier points was increased rapidly with the increasing of the probability of inlier points. Based on the principle of inverse mapping sampling strategy, the selection of test sample set trended to the points with high probability in our proposed algorithm. It effectively improved the convergence rate of our algorithm. In fact, the algorithm reached the stop criterion in the 21st iteration. After 26 iterations, the estimated number of inlier set was converged to determined values, and it was high consistent with the actual acquired number of inlier points. Figure 5 illustrates the line fitting results when the outliers are 70% and 80%. The line fitting simulation results show that the proposed IPGSAC algorithm can rapidly localize inlier points. The probability updating algorithm can effectively improve the selection probability of inlier point and speed up the convergence rate of the proposed algorithm.
(a) The outlier rate is 70%
(b) The outlier rate is 80%
4.1.2. Homography Estimation Simulation
Assume that and are two correspondent image feature point sets. The homography relationship is defined as where is a scaling factor and is a homography matrix. According to Lie algebra, homography matrix can be expressed as an exponent mapping by using an 8-variable :
In order to estimate the homography matrix , an energy function is established by using the projection errors:where is the number of matching pairs which are used for the homography estimation. To optimize this object function, we follow the linear expansion and Newton iteration method. The minimum of energy function (29) is achieved by solving (30) iteratively:
A random simulation data set was generated which fitted with a given homography relationship. To enhance reliability and authenticity of our simulation, the standard deviation of inlier points was set to 1 pixel and the outlier points obeyed the uniform distribution within the image. The size of test points was set to 1,000, and the outlier rates were set to different values which were the same as in the line fitting simulation. During the simulation, we selected four points to estimate the homography. , were set as 0.05 and 2, respectively. The simulation was repeated 50 times to achieve a more reliable result in each outlier rate.
Figure 6 shows the compared results of the simulation. In the simulation, the recall ratio of the proposed algorithm is much higher than other four RANSAC algorithms in any tested outlier rates. Even when the outlier is 80%, this proposed algorithm is still available with the recall ratio 0.85962 after 20 iterations. Table 2 demonstrates the statistics of iterations when IPGSAC is converged in the homography estimation simulation. These results illustrate that the proposed IPGSAC algorithm is more effective than other four RANSAC algorithms and demonstrate the high performance of the proposed algorithm.
(a) 10 iterations
(b) 20 iterations
4.1.3. Fundament Matrix Estimation Simulation
In this paper, 8-point method is used for fundamental matrix estimation. Assume that and are an image matching pair; the fundamental matrix is limited with following equation:where is a matrix. We follow the method in [6] to estimate , and the error function is defined as
The simulation data was generated similarly as in homography simulation which fitted with a given fundamental relationship. The standard deviation of inlier points was set to 1 pixel and the outlier points obeyed the uniform distribution within the image. During the experiment, and were set to 0.05 and 1, respectively. We also recorded the average recall ratios in 50 repeat simulations when the outlier rates were 20%, 30%, 40%, 50%, 60%, 70%, and 80%, respectively.
Figure 7 shows the recall ratios of different RANSAC algorithms in 10 and 20 iterations, respectively. Due to the low probability of inlier test set selection, the recall ratios of the other four RANSAC algorithms begin to decrease rapidly when the outlier rate is larger than 30%. As the fundamental matrix estimation is solved with several linear equations, the constraint condition is much weaker than the homography relationship. The average recall ratios of fundamental matrix estimation are a little better than the homography estimation. Table 3 illustrates the statistics of iterations when IPGSAC is converged with 50 repeat experiments. It shows that it only needs 26.5 iterations in average to deal with the outlier rate 80%, and it is much more efficient than other four algorithms.
(a) 10 iterations
(b) 20 iterations
4.1.4. The Time Cost and Accuracy of IPGSAC
In the previous section, some results are given to illustrate the high performance of the proposed algorithm. In some application of RANSAC, the time cost and the accuracy of the proposed algorithm are important issue for the real time requirement. As a typical application, the fundamental matrix estimation simulation was used for evaluating the time cost and the accuracy of the proposed algorithm. To ensure the convergence of each RANSAC algorithm, the maximum number of iterations was set to 100 and the other parameters were set the same as those in Section 4.1.3. All timing results were obtained on a PC with Intel i5 760 CPU (2.80 GHz) and 4 GB RAM.
The time cost, mean absolute error, and recall ratio of five RANSAC algorithms are illustrated in Tables 4–6. As shown in the tables, the recall ratios of those five RANSAC algorithms are very similar to each other when the outlier rate is lower than 40%. Generally, a lot of iterations are needed to ensure that an inlier set is found in the standard RANSAC, MSAC, MLESAC, and NAPSAC. Therefore, the time cost of those algorithms is much higher than the proposed algorithm. The recall ratios of those algorithms are decreased rapidly with the outlier rate increasing. The time cost of the proposed algorithm is the lowest with the smallest mean absolute error and highest recall ratio during the simulation. And the time cost has a small increase with the raising of the outlier rate. The results further demonstrate the low time cost and the high accuracy of the proposed algorithm.
4.2. Real Experimental Results
4.2.1. Image Matching Experiments
During the experiment, two images taken from different perspective in a real scene [34] were used for image matching. Firstly, the SIFT algorithm was used for feature detection and matching, as shown in Figure 8. There were 1244 feature pairs which were matched by using the distance of feature descriptor. We used 8-point method and 4-point method for the fundamental matrix and homography matrix estimation, respectively. We also replicated all the matching experiment 50 times; the average outlier rate was about 52% which was achieved by using the proposed algorithm.
During the homography estimation experiment, and were set to 0.05 and 4, respectively. The image matching result is shown in Figure 8. The probabilities of test points have obvious boundaries and the probability of inlier points is near to 1. Table 7 illustrates the number of inlier points achieved by different RANSAC algorithms with different iterations. The experimental result is highly consistent with the simulations. This proposed IPGSAC algorithm needs no more than 20 iterations to get convergence in average.
In fundamental matrix estimation, and were set to 0.05 and 1, respectively. The probabilities of test points are quite similar with homography for foundation estimation. Table 8 shows the number of inlier points achieved by different RANSAC algorithms with different iterations. This experimental result is also similar with the simulation of fundamental matrix estimation. This proposed IPGSAC algorithm also needs no more than 20 iterations to achieve a convergent solution in average. While the homography matrix is a plane constraint of features and the fundamental matrix is a common constraint in image matching, the result has some tiny difference with homography estimation. All those model estimation results demonstrate the feasibility and effectiveness of the proposed IPGSAC algorithm.
4.2.2. The 3D Map Building Experiment
The 3D mapping experiment was taken in an indoor environment shown in Figure 9. The experimental platform consisted of a Pioneer3-DX mobile robot and a Microsoft Kinect. The P3-DX mobile robot is an intelligent mobile platform equipped with an embedded PC. The Kinect provides the depth and color data captured in the experimental scene. The maximum field of view of Kinect is 52° and the image resolution is 640 × 480 with the refresh frequency 30 Hz. In the experiment, the Kinect was fixed on the mobile robot and captured the depth and color data in the scene. The distance threshold and angle threshold were set to 0.2 and 0.3, respectively. The tolerance threshold was equal to 1. The pseudo minimum inlier rate was 0.05. To localize the robot, four points were used in the IPGSAC-based pose estimation procedure.
(a) Mobile robot platform
(b) Experimental environment
To evaluate the proposed algorithm, we compare our method with standard ICP-based tracking algorithm and a dense image-based tracking algorithm [9] using the RGBD SLAM benchmark provided by Technical University of Münich [35]. Table 9 shows the evaluations of the absolute translational error by using those three algorithms with two typical indoor data sets. In the experiment, ICP-based tracking method almost failed with the largest evaluation of the absolute translational error. Due to the blur and occlusion regions of the input data, the dense image-based method easily falls into local optimum. In our proposed method, the data association was established by using the feature matching procedure and it sometimes noised by the mismatched features. And the noise would be removed in our IGPSAC-based pose estimation procedure. Although the IGPSAC-based pose estimation procedure is limited by the accuracy of feature detection, the smallest RMSE (Root Mean Square Error) of the absolute translational error is achieved by using our proposed method.
In the indoor experiment, the robot was instructed to traverse around in the environment. And the translational speed of mobile robot was about 0.1 m/s. The origin of coordinate of robot was set at the location where robot started to move. While the mobile robot was moving, the depth and color image were captured by the Kinect and the robot’s pose was estimated by using the algorithm detailed in Section 3. When a keyframe was captured, the global map was updated with the new coming keyframe and the loop detection procedure was performed to estimate the relation pose between this new coming keyframe and previous nonadjacent keyframes. When a loop closure was detected, the TORO optimization algorithm would be performed to achieve a global optimization of generated map. Table 10 illustrates the number of iterations when the proposed IPGSAC is converged during the mapping experiment. According to (1), the minimum iteration is in the range of 60~7725 when the confidence level is 98%. The iteration when the proposed IPGSAC-based algorithm is converged is much littler than the estimated minimum iterations. It is highly consistent with our previous simulations and verifies the high efficiency of the proposed algorithm.
Moreover, the ICP-based pose estimation, the dense image-based pose estimation, and the standard RANSAC-based pose estimation were also employed for map building. Figure 10 shows the generated mapping before the loop optimization by using those methods. During the map building, the ICP-based tracking algorithm failed due to the similar spatial structure of the input point clouds. Although the dense image-based method tracked the mobile robot well, the generated map sank seriously. In the RANSAC-based method, it was sometimes easy to converge to local optima and tracking procedure would fail as indicated by the red circle in Figure 10(c). In the IPGSAC-based pose estimation method, the pose estimation procedure worked well and the generated map had a very slightly subsidence during the experiment. With those estimated poses, the loop closure was easily detected and it was much easier to perform loop optimization procedure and achieve a global consistent map. The final mapping experimental result of our method is shown in Figure 11. All the experimental results demonstrate the feasibility and effectiveness of this proposed algorithm.
(a) ICP-based method
(b) Dense image-based method
(c) RANSAC-based method
(d) The proposed IPGSAC-based method
5. Conclusions
In this paper, we present a novel RANSAC algorithm for robot 3D map building. Firstly, two types of the probabilities are updated and evaluated for test points by using a total probability formula and the maximum number of inlier points. Then, a more reliable evaluation of test points is achieved by using the DS evidence theory. To reduce the influence of incorrect judgment, the theories of forgetting are utilized to filter out the unstable inlier points. Finally, an inverse mapping sample algorithm is adopted to sample the points based on the updated probabilities of points. All simulation and real experimental results show that the proposed algorithm can quickly and efficiently achieve a stable convergence in a few iterations and demonstrate the feasibility and effectiveness of the proposed algorithm.
As is illustrated in the experiment results, the proposed algorithm is much more efficient than some other RANSAC algorithms when the outlier rate is lower than 80%. In fact, the utmost outlier rate of this proposed algorithm is 85%. When the outlier rate rises to 90%, this proposed algorithm becomes unstable and unviable. In some extreme cases, it needs more than 2,000 iterations to achieve a correct solution. In future work, we will further improve the robustness and efficiency of our proposed algorithm to achieve a stable solution for the higher outlier rate. And an adapt threshold scheme will be proposed to enhance the robust of our method.
Competing Interests
The authors declare that they have no competing interests.
Acknowledgments
The research work is financially supported by Promotion Project on Intelligent Robot in Beijing University of Technology “Key Technologies on Modularization and Practicalization of Intelligent Service Robot” and the Natural Science Foundation of China (61175087).