A Novel Improved Probability-Guided RANSAC Algorithm for Robot 3 D Map Building

1College of Electronic and Control Engineering, Beijing University of Technology, Beijing 100124, China 2Beijing Key Laboratory of Computational Intelligence and Intelligent System, Beijing 100124, China 3Engineering Research Center of Digital Community, Ministry of Education, Beijing 100124, China 4School of Mechanical and Electrical Engineering, Henan Institute of Science and Technology, Xinxiang 453003, China


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][2][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 Mestimation [10], LMedS (Least Median of Squares) [11], or RANSAC (Random Sample Consensus) [12] algorithm.Mestimation 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.

Journal of Sensors
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 probabilityguided 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.

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  0 .When the residual error   of point   is larger than  0 , 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  max ensures that a correct hypothesis set is achieved with a determined confidence level at least once and it can be estimated by where  is the confidence level,  indicates the inlier rate, and  indicates the minimum number of test points for model estimation.In the proposed IPGSAC algorithm, x n 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:

Probability distribution Cumulative density function
where Set inlier is the inlier set which is acquired in the current iteration,  inlier is the number of points in Set inlier , Set  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.

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   , V determines the probability of outlier point, and  indicates whether   is an inlier or not.When   is an inlier point, then  = 1; otherwise  = 0. 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 2,  and  are solved by using a standard normal distribution with several iterations.In this proposed algorithm, V =  and  = 0.1.

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 as where (  ∈  |  ̸ ⊂ ) 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  inlier and  outlier are the number of inlier and outlier points, respectively. =  inlier +  outlier is the total number of test points.Moreover,  inlier 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 as where   is the maximum size of inlier set which contains the test point   and ñ = ∑  =1 (  ∈ ) indicates the current predicted size of inlier set.

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 { 1 ,  2 , . . .,   , . . .,   }.   is a really defined focal element of discernment framework Θ;  is the number of elements in Θ.The power set of a discernment framework 2 Θ contains all subsets of the discernment framework.There are 2  elements in 2 Θ and each of the elements corresponds to a solution for .The power set 2 Θ is composed of the following subsets: (1) It contains the empty set  and all of the elements in Θ; (2) If subset ,  ∈ 2 Θ , then  ∪  ⊆ 2 Θ ; (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 where  is an empty set,  is a probability belief function in 2 Θ , 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  1 and  2 are the corresponding probability belief functions which are mutually independent in the same discernment set Θ.For the focal elements  1 ,  2 and  1 ,  2 , the synthesis operation is described as where  is conflict coefficient and 1/(1 − ) is regularization factor.
In this proposed IPGSAC algorithm, the status of test samples is assigned as inlier() or outlier() 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 (11)

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 as where  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

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 (). −1 () is an inverse function of () where  ∈ [0, 1]. −1 () 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, [0, 1] is divided into  subsections which is corresponding to sample points.The th subsection is expressed as [∑ −1 =1 (  ), ∑  =1 (  )).When a uniform sampled variable   ∈ [0, 1] 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   = (  )/ ∑  =1 (  ).And a sampling cumulative density function (CDF)  is defined by (  ) = ∑  =1   .The expected output is   when the random variable   is in the subsection [(  ), ( +1 )).
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.

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 Set test 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  0 , the model estimation fails and the probabilities of test points are updated by using (4) with ( ⊂ ) = 0.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 Set inlier is refreshed by current inlier size  inlier .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.

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 as where (  ,  V ) and (  ,  V ) 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 as where  = (, , ) 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 as where   ∈ SE(3); SE(3) is a special rigid transformation group.  is the rotation matrix;   is the translation vector.
According to the  algebra,   is expressed as where  = (, , ,   ,   ,   ), and ζ is define as

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 as (  (24) end for (25)  where   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 expansion where  = (  /)| =0 .
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.

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  0 and an angle threshold  0 are employed for the keyframes selection.In the procedure of keyframe selection, if the pose estimation between current frame  +1 and keyframe  −1  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 icp ( 1 ,  2 ) evaluates the procedure of pose estimation and icp = 0 when pose estimation fails or  ≥  0 or  ≥  0 ; for others, icp = 1.

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 as where   = { 1  , . . .,    , . . .,    } 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   .

Line Fitting Simulation.
The principle of line fitting is as follows: assume that there exists a point set {( 1 ,  1 ), ( 2 ,  2 ), . . ., ( −1 ,  −1 ), (  ,   )}, the fitting line is expressed as  +  +  = 0, 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 ,  0 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).
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.

Homography Estimation
where  is a scaling factor and  is a homography matrix.
According to  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  ( ≥ 4) 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.,  0 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.

Fundament Matrix Estimation Simulation.
In this paper, 8-point method is used for fundamental matrix estimation.Assume that  = (, V, 1) and   = (  , V  , 1) are an image matching pair; the fundamental matrix is limited with following equation: where  is a 3 × 3 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  0 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.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.

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  0 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  0 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.

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  0 and angle threshold  0 were set to 0.2 and 0.3, respectively.The tolerance threshold  0 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.
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 RANSACbased 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.

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

Figure 1 :
Figure 1: The illustration of the proposed IPGSAC algorithm.

Figure 2 :
Figure 2: The estimated parameters during the fundament estimation simulation.(a) shows the results by using the total probability formula, (b) illustrates the result with DS evidence theory, and (c) demonstrates the result of the procedure of the proposed algorithm.

Figures 2 (
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.

Figure 3 :
Figure 3: The recall ratios with 30 and 50 iterations in line fitting simulation.

Figure 4 :
Figure 4: The simulation result of line fitting when the outlier rate is 80%.

Figure 5 :
Figure 5: The line fitting results of the proposed algorithm when the outlier rates are 70% and 80%.

Figure 6 :
Figure 6: The recall ratios with 10 and 20 iterations in homography estimation simulation.

Figure 7 :
Figure 7: The recall ratios with 10 and 20 iterations in fundamental matrix estimation simulation.

4. 1 . 4 .
The Time Cost and Accuracy of IPGSAC.In the previous section, some results are given to illustrate the high test points The maximum inlier size The probability of test points The maximum inlier size Constrained by u i = Hu ref i Probabilities Probabilities Constrained by m  Fm = 0

Figure 8 :
Figure 8: The image matching experimental results.

Figure 9 :
Figure 9: The platform and experimental environment.

Figure 10 :
Figure 10: The generated mapping before the loop optimization.

Table 1 :
The statistics of iterations when IPGSAC algorithm is converged in line fitting.

Table 2 :
The statistics of iterations when IPGSAC is converged in homography estimation simulation.

Table 3 :
The statistics of iterations when IPGSAC is converged in fundamental estimation simulation.

Table 4 :
The time cost of different RANSAC algorithms.

Table 5 :
The mean absolute error of different RANSAC algorithms.

Table 6 :
The recall ratio of different RANSAC algorithms.

Table 7 :
The inlier size with different iterations in homography estimation.

Table 8 :
The inlier size with different iterations in fundament estimation.

Table 9 :
The evaluation of absolute translational error ().
(a) Mobile robot platform (b) Experimental environment

Table 10 :
The statistics of iterations when IPGSAC is converged in 3D map building.