CN114943741A - Visual SLAM method based on target detection and geometric probability in dynamic scene - Google Patents

Abstract

The application provides a visual SLAM method based on target detection and geometric probability in a dynamic scene, which comprises the following steps: 1) collecting an image to be processed, and dividing the image to be processed into a static area and a dynamic area by using a target detection algorithm; 2) calculating a basic matrix F of the static region matching points by using a VSAC algorithm; 3) calculating the geometric probability of the dynamic region matching points by using the basic matrix F and epipolar constraint; 4) fusing the geometric probabilities of two continuous frames of matching points in the dynamic region to serve as the final geometric probability of the matching points of the current frame; 5) and carrying out probability expansion on unmatched points in the dynamic region to obtain the geometric probability of all the characteristic points in the dynamic region, eliminating the characteristic points which do not meet the probability requirement, and transmitting the rest points to the subsequent tracking and drawing steps. According to the method and the device, the target detection network is combined with the traditional geometric method, the requirements of the visual SLAM on precision and real-time performance can be met, the real-time performance is guaranteed, and the positioning precision and the drawing construction quality are improved.

Description

Visual SLAM method based on target detection and geometric probability in dynamic scene

Technical Field

The invention relates to a visual SLAM method, in particular to a visual SLAM method based on target detection and geometric probability in a dynamic scene.

Background

The SLAM (Simultaneous Localization and Mapping) technology is a core technology of intelligent robot application, and can estimate self position and construct an environment map under an unknown environment. The visual SLAM uses a camera as a perception sensor, and has become an important research subject in recent years because of its low cost, high accuracy, and rich information.

Over the past decades, many excellent visual SLAMs have emerged, such as ORB-SALM2, KinectFusion, SVO, DSO; however, conventional visual SLAM systems generally work well in static environments, but only use geometric constraints to cull dynamic points when there are dynamic objects in the environment, so the effect is poor and even fails. With the development of deep learning technology, a dynamic object can be marked by semantic segmentation or target detection network at present, and then all features of the dynamic object are removed. However, the semantic segmentation network cannot guarantee accurate semantic segmentation on the premise of guaranteeing real-time performance; the target detection network can meet the real-time requirement, but can cause that part of static objects are removed to reduce the positioning precision and the image construction quality.

Disclosure of Invention

The invention aims to provide a visual SLAM method based on target detection and geometric probability in a dynamic scene, which can effectively improve the positioning accuracy of an SLAM system in the dynamic scene.

The invention is realized by the technical scheme, and the method comprises the following specific steps:

1) collecting an image to be processed, and dividing the image to be processed into a static area and a dynamic area by using a target detection algorithm;

2) calculating a basic matrix F of the static region matching points by using a VSAC algorithm;

3) calculating the geometric probability of the dynamic region matching points by using the basic matrix F and epipolar constraint;

4) fusing the geometric probabilities of two continuous frames of matching points in the dynamic region to serve as the final geometric probability of the matching points of the current frame;

5) and carrying out probability expansion on unmatched points in the dynamic region to obtain the geometric probability of all the characteristic points in the dynamic region, eliminating the characteristic points which do not meet the probability requirement, and transmitting the rest points to the subsequent tracking and drawing steps.

Further, the specific steps of using the target detection algorithm to divide the image to be processed into the static area and the dynamic area in the step 1) are as follows:

1-1) training a Yolov3 tiny target detection network by using a Crowdumman data set to obtain a trained Yolov3 tiny network;

1-2) dividing the image to be processed into a dynamic area and a static area by using a trained Yolov3 tiny network, and extracting the characteristic points of the image to be processed.

Further, the specific step of calculating the static region matching point basis matrix F by using the VSAC algorithm in the step 2) is as follows:

2-1) carrying out feature matching on the feature points of the static area, recording the logarithm of the matching points as N, sorting according to the quality of the matching points, randomly sampling a minimum sample set from the high-quality matching points, and calculating a basic matrix F of the minimum sample set by using a numerical method _i ；

2-2) judging the basis matrix F by using the self-adaptive SPRT algorithm _i Whether or not to satisfy becoming the optimal basis matrix F _best If yes, the step 2-3) is carried out, and if not, the step 2-1) is returned;

2-3) judging whether the minimum sample set of the current basic matrix meets the judgment condition that Q points are coplanar, if not, judging that the current basic matrix is not degraded, and turning to the step 2-4);

if the judgment bar is satisfiedRandomly sampling the sample outside the minimum sample set, and applying the current basic matrix F _i Verifying through epipolar constraint to judge whether the limiting conditions of P points outside the non-dominant plane are met, if the limiting conditions are met, the current basic matrix is not degraded, if the limiting conditions are not met, the step 2-4) is carried out, the current basic matrix is degraded, the current basic matrix is discarded, and the step 2-1 is returned;

2-4) calculating the basis matrix F _i The number of corresponding inner points is F if the current optimal value is larger than the current optimal value _best If the number of the independent interior points is more than the threshold value, the number of the independent interior points is calculated

F is to be _i Is recorded as optimal F _best And turning to the step 2-5), otherwise, returning to the step 2-1);

2-5) judging the current optimal F _best Whether a local optimization condition is satisfied, if so, using F _best Inner point set pair F _best Performing local optimization, and returning to the step 2-1 if the local optimization condition is not met);

the judging conditions of the local optimization are as follows: if the number of the independent inner points is larger than the threshold value

Only if the minimum sample set of the current optimal basic matrix and the last optimal basic matrix is lower than 0.95Jaccard index, the condition for carrying out local optimization is met, otherwise, the condition for carrying out local optimization is not met;

the local optimization process comprises the following steps: randomly selecting points with the number larger than the minimum sample set number from the inner points corresponding to the optimal basic matrix, and iteratively calculating a better basic matrix by using an analytical method;

2-6) judging whether the algorithm reaches a cycle termination condition, if so, turning to the step 2-7), and if not, returning to the step 2-1), wherein the judgment condition of the cycle termination is as follows: eta is greater than a preset threshold value

Wherein η represents the probability that all the randomly extracted minimum sample sets at present contain at least one minimum sample set which is all inliers, and is calculated as follows:

η＝(1-P _g (1-α)) ^k

where k is the current iteration number, P _g Represents the probability that m points in the minimum sample set are all inliers, and α represents the probability of rejecting a good model, calculated as follows:

P _g ＝ε ^m

α≈1/A

2-7) to optimal F _best Using iterative least square method to make final optimization to obtain final optimal F _best And the final optimal F is _best As the basis matrix F for the static area.

Further, judging the basic matrix F by using the self-adaptive SPRT algorithm in the step 2-2) _i Whether or not to satisfy being the optimal basis matrix F _best The conditions of (a) are as follows:

2-2-1) constructing a basis matrix F _i Using the adaptive SPRT algorithm:

in the formula (I), the compound is shown in the specification,

and t _v Representing testing whether a point is the current basis matrix F with and without adaptive SPRT, respectively _i Time of inliers, α is the probability of rejecting a good model, E ^w (T) is the average number of tested points;

if yes, turning to the step 2-2-2), and if not, turning to the step 2-3);

2-2-2) calculating the initial value of the parameter of the adaptive SPRT: probability of bad model interior points delta ₀ And the probability epsilon of points in the good model ₀ ：

In the formula, T is the total number of the matching points, and the average value of the number of the inner points corresponding to the non-optimal basic matrix in the previous n iterations

Is the number of corresponding inner points, I, of the current optimal basic matrix F _δ Is passing through delta ₀ And

estimating the maximum number of inner points corresponding to the obtained non-optimal basis matrix F;

let initial time j equal to 1, randomly extract a matching point outside the minimum sample set, and calculate the splt likelihood ratio lambda based on Wald _j ：

In the formula, H _b And H _g Respectively representing a bad model and a good model, p (x) _r |H _b ) Is that a point is the probability of a point in the bad model, p (x) _r |H _g ) The probability that a point is a good intra-model point; if the data point r is the interior point of the model, then x _r 1, otherwise x _r ＝0；

If λ _j >A, returning the current basic matrix as a bad modelStep 2-1), wherein A is a decision threshold;

if λ _j Is less than or equal to A, and j>N, if the current basic matrix is a good model, namely the condition of becoming the optimal basic matrix is met, and the step 2-3) is carried out;

if λ _j A is less than or equal to A, and j is less than or equal to N, then a point is extracted again, j is increased by 1, and the calculation of the SPRT likelihood ratio based on Wald is continued until j is less than or equal to N>N。

Further, the specific steps of calculating the geometric probability of the dynamic region matching points by using the basis matrix F and the epipolar constraint in the step 3) are as follows:

computing epipolar lines for matching points in a dynamic region using a basis matrix F

Then calculating the distance from the matching point to the epipolar line in the dynamic region

In formula (II) p' _i And p _i Is a pair of matching points, x, y, z are vector parametric representations of epipolar lines;

distance from matching point to polar line in dynamic region

Conversion to geometric probabilities using a binary Sigmod function

Where Dth is the threshold for static and dynamic point-to-epipolar distance.

Further, the specific step of calculating the final geometric probability of the current frame matching point in step 4) is as follows:

according to the last frame point p' _i Probability value of state probability

Predicting current frame point p _i Probability value of state probability

According to the last frame point p' _i Variance of state probability

Predicting current frame point p _i Variance of state probability

In the formula, delta ₀ Is the standard deviation of the state probability transition between the matching points;

calculating a Kalman gain k:

in the formula, delta ₁ The probability standard deviation of the observation state of the key point is;

according to point p _i Probability of observation state of

Update point p _i State probability of (2):

in the formula (I), the compound is shown in the specification,

represents a point p _i The state probability of (b) in the prediction of the current frame,

represents a point p _i The state probability of (2) is passed through the final result after Kalman filtering;

update point p _i Variance of state probability:

in the formula (I), the compound is shown in the specification,

for the current frame point p _i A predicted value of the state probability variance.

Further, the specific steps of expanding the probability of unmatched points in the dynamic region in the step 5) to obtain the geometric probability of all the feature points in the dynamic region and eliminating the feature points which do not meet the probability requirement are as follows:

searching for neighboring feature points p around the high-confidence feature point in the dynamic region that do not match the previous frame _i Updating neighboring feature points p _i State probability of

In the formula, p _j Is and point p _i The neighboring high-confidence feature points are,

is a point p _j The probability of the state of (a) is,

is a point p _i Observing state probabilities; c is a constant, λ (d) represents a distance factor;

if it is

If the value is less than the preset threshold value, p is added _i Dividing into dynamic points;

if it is

If the static point is larger than the threshold value, the static point is divided into static points, the dynamic points are removed, and the static points in the dynamic area are added into the point set of the static area for subsequent positioning and drawing.

Due to the adoption of the technical scheme, the invention has the following advantages:

1. according to the method and the device, the target detection network is combined with the traditional geometric method, the requirements of the visual SLAM on precision and real-time performance can be met, the real-time performance is guaranteed, and the positioning precision and the drawing construction quality are improved.

2. According to the method, the dynamic region and the static region of the image are marked by using a target detection algorithm, the VSAC algorithm is combined with epipolar constraint, the geometric probability of the matching points of the dynamic region is calculated, the matching precision of the image feature points in a dynamic scene is improved, and the positioning precision is improved.

Additional advantages, objects, and features of the invention will be set forth in part in the description which follows and in part will become apparent to those having ordinary skill in the art upon examination of the following or may be learned from practice of the invention. The objectives and other advantages of the invention will be realized and attained by the structure particularly pointed out in the written description and claims hereof.

Drawings

The drawings of the present invention are described below.

FIG. 1 is a flow chart of the method of the present invention.

FIG. 2 is a flowchart of a method for computing a basis matrix of static region matching points using a VSAC algorithm in accordance with the present invention.

Detailed Description

The invention is further illustrated by the following figures and examples.

Example 1:

1. a visual SLAM method based on target detection and geometric probability in a dynamic scene comprises the following specific steps:

1) collecting an image to be processed, and dividing the image to be processed into a static area and a dynamic area by using a target detection algorithm; the method comprises the following specific steps:

1-1) training a Yolov3 tiny target detection network by using a Crowdumman data set to obtain a trained Yolov3 tiny network;

1-2) dividing the image to be processed into a dynamic area and a static area by using a trained Yolov3 tiny network, and extracting the characteristic points of the image to be processed.

2) The method comprises the following steps of calculating a basic matrix F of static region matching points by using a VSAC algorithm, and specifically comprises the following steps:

2-1) carrying out feature matching on the feature points of the static area, recording the logarithm of the matching points as N, sorting according to the quality of the matching points, randomly sampling a minimum sample set from the high-quality matching points, and calculating a basic matrix F of the minimum sample set by using a numerical method _i 。

In the embodiment of the invention, the quality of the matching point is the distance from the characteristic point to the epipolar line, and the quality of the first time is the result of the matching point after the ratio test.

2-2) judging the basis matrix F by using an adaptive SPRT algorithm _i Whether or not to satisfy being the optimal basis matrix F _best If the condition is met, the step 2-3) is carried out, if the condition is not met, the step 2-1) is returned to,the method comprises the following specific steps:

2-2-1) constructing a basis matrix F _i Using the adaptive SPRT algorithm:

in the formula (I), the compound is shown in the specification,

and t _v Representing testing whether a point is the current basis matrix F with and without adaptive SPRT, respectively _i Time of inliers, α is the probability of rejecting a good model, E ^w (T) is the average number of tested points;

if yes, turning to the step 2-2-2), and if not, turning to the step 2-3);

2-2-2) calculating the initial value of the parameter of the adaptive SPRT: bad model interior point probability delta ₀ And the probability epsilon of points in the good model ₀ ：

In the formula, T is the total number of the matching points, and the average value of the number of the inner points corresponding to the non-optimal basic matrix in the previous n iterations

Is the number of corresponding inner points, I, of the current optimal basic matrix F _δ Is passing through delta ₀ And

estimating the maximum number of inner points corresponding to the obtained non-optimal basis matrix F;

let initial time j equal to 1, randomly extract a matching point outside the minimum sample set, and calculate the splt likelihood ratio lambda based on Wald _j ：

In the formula, H _b And H _g Respectively representing a bad model and a good model, p (x) _r |H _b ) Is that a point is a probability of a point in the bad model, p (x) _r |H _g ) The probability that a point is a good intra-model point; if the data point r is the interior point of the model, then x _r 1, otherwise x _r ＝0；

If λ _j >A, if the current basic matrix is a bad model, returning to the step 2-1), wherein A is a decision threshold;

if λ _j Is less than or equal to A, and j>N, if the current basic matrix is a good model, namely the condition of becoming the optimal basic matrix is met, the step 2-3) is carried out;

if λ _j A is less than or equal to A, and j is less than or equal to N, then a point is extracted again, j is increased by 1, and the calculation of the SPRT likelihood ratio based on Wald is continued until j is less than or equal to N>N。

In the present example, the model estimates the time t _M Calculating the time average value of the basic matrix and the corresponding inner point number in the previous n iterations to obtain the average number of the models

And (3) passing the average number of the effective basic matrix corresponding to each feature point in the previous N iterations, wherein N & lt N. The decision threshold a is calculated in the following manner:

A _n+1 ＝K ₁ /K ₂ +1+log(A _n )

wherein the initial value A ₀ Is K ₁ /K ₂ +1

K ₁ ＝t _M /P _g

2-3) judging whether the minimum sample set of the current basic matrix meets the judgment condition that Q points are coplanar, if not, judging that the current basic matrix is not degraded, and turning to the step 2-4);

if the minimum sample set meets the judgment condition, randomly sampling samples outside the minimum sample set, and carrying out comparison on the current basic matrix F _i Verifying through epipolar constraint to judge whether the limiting conditions of P points outside the non-dominant plane are met, if the limiting conditions are met, the current basic matrix is not degraded, if the limiting conditions are not met, the step 2-4) is carried out, the current basic matrix is degraded, the current basic matrix is discarded, and the step 2-1 is returned;

in the present embodiment, Q is a threshold of the minimum number of coplanar points in the sample set, and Q is taken to be 5.

2-4) calculating the basis matrix F _i The number of corresponding inner points is F if the current optimal value is larger than the current optimal value _best If the number of the independent interior points is more than the threshold value, the number of the independent interior points is calculated

F is to be _i Is recorded as optimal F _best And the step 2-5) is carried out, otherwise, the step 2-1) is carried out.

In the present example, the definition of independent interior points is: the points that are removed from the minimum sample set, the points that are near the independent inliers, the points that are near a pair of matching point epipolar lines, and that satisfy a point-to-epipolar distance less than a threshold are called independent inliers.

The threshold calculation mode of the independent interior point is as follows: calculating median of independent interior point number (excluding the optimal basis matrix and the basis matrix with higher overlap degree of the minimum sample set and the optimal matrix) corresponding to the effective basis matrix obtained in the first N (N < N) iterations, and recording as

Will be provided with

Calculating 95% percentile of Poisson distribution as λ of Poisson distribution, calculating average value of independent interior points corresponding to effective basis matrix obtained in first N (N < N) iterations and recorded as independent interior point threshold value

2-5) judging the current optimal F _best Whether a local optimization condition is satisfied, if so, using F _best Inner point set pair F _best Performing local optimization, and returning to the step 2-1 if the local optimization condition is not met);

the judging conditions of the local optimization are as follows: if the number of the independent inner points is larger than the threshold value

Only if the minimum sample set of the current optimal basic matrix and the last optimal basic matrix is lower than 0.95Jaccard index, the condition for carrying out local optimization is met, otherwise, the condition for carrying out local optimization is not met;

the local optimization process comprises the following steps: randomly selecting points with the number larger than the minimum sample set number from the inner points corresponding to the optimal basic matrix, and iteratively calculating a better basic matrix by using an analytical method;

2-6) judging whether the algorithm reaches a cycle termination condition, if so, turning to the step 2-7), and if not, returning to the step 2-1), wherein the judgment condition of the cycle termination is as follows: eta is greater than a preset threshold value

Wherein eta represents the probability that all randomly-drawn minimum sample sets at present contain at least one minimum sample set which is all inliers, andthe calculation method is as follows:

η＝(1-P _g (1-α)) ^k

where k is the current iteration number, P _g Represents the probability that m points in the minimum sample set are all inliers, and α represents the probability of rejecting a good model, calculated as follows:

P _g ＝ε ^m

α≈1/A

2-7) to optimal F _best Using iterative least square method to make final optimization to obtain final optimal F _best And the final optimum F _best As the basis matrix F for the static area.

3) Calculating the geometric probability of the dynamic region matching points by using the basic matrix F and epipolar constraint, and specifically comprising the following steps of:

computing epipolar lines for matching points in a dynamic region using a basis matrix F

Then calculating the distance from the matching point to the epipolar line in the dynamic region

In the formula (II), p' _i And p _i Is a pair of matching points, x, y, z are vector parametric representations of epipolar lines;

distance from matching point to polar line in dynamic region

Conversion to geometric probabilities using a binary Sigmod function

In the formula, Dth is the threshold value of the distance from the static point and the dynamic point to the epipolar line.

4) Fusing the geometric probability of two continuous frame matching points in the dynamic region to be used as the final geometric probability of the current frame matching point, and specifically comprising the following steps:

according to the last frame point p' _i Probability value of state probability

Predicting current frame point p _i Probability value of state probability

According to the last frame point p' _i Variance of state probability

Predicting the current frame point p _i Variance of state probability

In the formula, delta ₀ Is the standard deviation of the state probability transition between the matching points;

calculating a Kalman gain k:

in the formula, delta ₁ The probability standard deviation of the observation state of the key point is taken as the probability standard deviation of the observation state of the key point;

according to point p _i Probability of observation state of

Update point p _i State probability of (2):

in the formula (I), the compound is shown in the specification,

representing point p _i The state probability of (b) in the prediction of the current frame,

represents a point p _i The state probability of (2) is passed through the final result after Kalman filtering;

update point p _i Variance of state probability:

in the formula (I), the compound is shown in the specification,

for the current frame point p _i A predicted value of the state probability variance.

In the embodiment of the invention, the probability transfer of the matching point between two continuous frames is defined to satisfy Markov, namely, the current state is only related to the last state, and the state probability after filtering can be obtained based on Kalman filtering.

5) Carrying out probability expansion on unmatched points in the dynamic region to obtain the geometric probability of all feature points in the dynamic region, eliminating feature points which do not meet the probability requirement, and transmitting the rest points to the subsequent tracking and drawing establishment steps, wherein the specific steps are as follows:

searching for neighboring feature points p around the high-confidence feature point in the dynamic region that do not match the previous frame based on the idea that the states of the neighboring feature points are similar in most cases _i Updating neighboring feature points p _i State probability of

In the formula, p _j Is and point p _i The neighboring high-confidence feature points are,

is a point p _j The probability of the state of (a) is,

is a point p _i Observing state probabilities; c is a constant, λ (d) represents a distance factor;

if it is

If the value is less than the preset threshold value, p is added _i Dividing into dynamic points;

if it is

And if the value is larger than the threshold value, dividing the static points into static points, eliminating the dynamic points, and adding the static points in the dynamic area into the point set of the static area for subsequent positioning and drawing construction.

As will be appreciated by one skilled in the art, embodiments of the present application may be provided as a method, system, or computer program product. Accordingly, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment or an embodiment combining software and hardware aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, and the like) having computer-usable program code embodied therein.

The present application is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the application. It will be understood that each flow and/or block of the flow diagrams and/or block diagrams, and combinations of flows and/or blocks in the flow diagrams and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.

These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.

These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.

Finally, it should be noted that: although the present invention has been described in detail with reference to the above embodiments, it should be understood by those skilled in the art that: modifications and equivalents may be made to the embodiments of the invention without departing from the spirit and scope of the invention, which is to be covered by the claims.

Claims (7)

1. A visual SLAM method based on target detection and geometric probability in a dynamic scene is characterized by comprising the following specific steps:

1) collecting an image to be processed, and dividing the image to be processed into a static area and a dynamic area by using a target detection algorithm;

2) calculating a basic matrix F of the static region matching points by using a VSAC algorithm;

3) calculating the geometric probability of the matching points of the dynamic region by using the basic matrix F and epipolar constraint;

4) fusing the geometric probabilities of two continuous frames of matching points in the dynamic region to serve as the final geometric probability of the matching points of the current frame;

5) and carrying out probability expansion on unmatched points in the dynamic region to obtain the geometric probability of all the characteristic points in the dynamic region, eliminating the characteristic points which do not meet the probability requirement, and transmitting the rest points to the subsequent tracking and drawing steps.

2. The visual SLAM method based on object detection and geometric probability in a dynamic scene as claimed in claim 1, wherein the specific steps of using the object detection algorithm to divide the image to be processed into the static region and the dynamic region in step 1) are:

1-1) training a Yolov3 tiny target detection network by using a Crowdumman data set to obtain a trained Yolov3 tiny network;

1-2) dividing the image to be processed into a dynamic area and a static area by using a trained Yolov3 tiny network, and extracting characteristic points of the image to be processed.

3. The visual SLAM method based on object detection and geometric probability in a dynamic scene as claimed in claim 1 wherein the step 2) of using VSAC algorithm to calculate the static region matching point basis matrix F comprises the following specific steps:

2-1) carrying out feature matching on the feature points of the static area, recording the logarithm of the matching points as N, sorting according to the quality of the matching points, randomly sampling a minimum sample set from the high-quality matching points, and calculating a basic matrix F of the minimum sample set by using a numerical method _i ；

2-2) judging the basis matrix F by using an adaptive SPRT algorithm _i Whether or not to satisfy being the optimal basis matrix F _best If yes, the step 2-3) is carried out, and if not, the step 2-1) is returned;

2-3) judging whether the minimum sample set of the current basic matrix meets the judgment condition that Q points are coplanar, if not, judging that the current basic matrix is not degraded, and turning to the step 2-4);

if the judgment condition is met, sampling samples outside the minimum sample set randomly, and carrying out comparison on the current basic matrix F _i Verifying through epipolar constraint to judge whether the limiting conditions of P points outside the non-dominant plane are met, if the limiting conditions are met, the current basic matrix is not degraded, if the limiting conditions are not met, the step 2-4) is carried out, the current basic matrix is degraded, the current basic matrix is discarded, and the step 2-1 is returned;

2-4) calculating the basis matrix F _i The number of corresponding inner points is F if being more than the current optimal _best If the number of the independent interior points is more than the threshold value, the number of the independent interior points is calculated

F is to be _i Is recorded as optimal F _best And turning to the step 2-5), otherwise, returning to the step 2-1);

2-5) judging the current optimal F _best Whether a local optimization condition is satisfied, if the local optimization condition is satisfied, F is used _best Inner point set pair F _best Performing local optimization, and returning to the step 2-1 if the local optimization condition is not met);

the judging conditions of the local optimization are as follows: if the number of the independent inner points is larger than the threshold value

Only if the minimum sample set of the current optimal basic matrix and the last optimal basic matrix is lower than 0.95Jaccard index, the condition for carrying out local optimization is met, otherwise, the condition for carrying out local optimization is not met;

the local optimization process comprises the following steps: randomly selecting points with the number larger than the minimum sample set number from the inner points corresponding to the optimal basic matrix, and iteratively calculating a better basic matrix by using an analytical method;

2-6) judging whether the algorithm reaches a cycle termination condition, if so, turning to the step 2-7), and if not, returning to the step 2-1), wherein the judgment condition of the cycle termination is as follows: eta is greater than a preset threshold value

Wherein η represents the probability that all the randomly extracted minimum sample sets at present contain at least one minimum sample set which is all inliers, and is calculated as follows:

η＝(1-P _g (1-α)) ^k

where k is the current iteration number, P _g Representing the probability that m points in the minimum sample set are all inliers, and α represents the probability of rejecting a good model, calculated as follows:

P _g ＝ε ^m

α≈1/A

2-7) to optimum F _best Using iterative least square method to make final optimization to obtain final optimal F _best And the final optimal F is _best As the basis matrix F for the static area.

4. The visual SLAM method based on target detection and geometric probability in a dynamic scene as claimed in claim 3 wherein step 2-2) uses adaptive SPRT algorithm to judge the basis matrix F _i Whether or not to satisfy being the optimal basis matrix F _best The specific steps of the conditions of (a) are as follows:

2-2-1) constructing a basis matrix F _i Using the adaptive SPRT algorithm:

in the formula (I), the compound is shown in the specification,

and t _v Representing testing whether a point is the current basis matrix F with and without adaptive SPRT, respectively _i Time of inliers, α is the probability of rejecting a good model, E ^w (T) is the average number of tested points;

if yes, turning to the step 2-2-2), and if not, turning to the step 2-3);

2-2-2) calculating the initial value of the parameter of the adaptive SPRT: probability of bad model interior points delta ₀ And the probability epsilon of points in the good model ₀ ：

In the formula, T is the total number of the matching points, and the average of the number of the inner points corresponding to the non-optimal basic matrix in the previous n iterationsValue of

Is the number of corresponding inner points, I, of the current optimal basic matrix F _δ Is passing through delta ₀ And

estimating the number of the maximum inner points corresponding to the obtained non-optimal basic matrix F;

let initial time j equal to 1, randomly extract a matching point outside the minimum sample set, and calculate the splt likelihood ratio lambda based on Wald _j ：

In the formula, H _b And H _g Respectively representing a bad model and a good model, p (x) _r |H _b ) Is that a point is the probability of a point in the bad model, p (x) _r |H _g ) The probability that a point is a good intra-model point; if the data point r is the interior point of the model, then x _r 1, otherwise x _r ＝0；

If λ _j >A, if the current basic matrix is a bad model, returning to the step 2-1), wherein A is a decision threshold;

if λ _j Is less than or equal to A, and j>N, if the current basic matrix is a good model, namely the condition of becoming the optimal basic matrix is met, the step 2-3) is carried out;

if λ _j A is less than or equal to A, and j is less than or equal to N, then a point is extracted again, j is increased by 1, and the calculation of the SPRT likelihood ratio based on Wald is continued until j is less than or equal to N>N。

5. The visual SLAM method based on target detection and geometric probability in a dynamic scene as claimed in claim 1 wherein, the specific steps of calculating the geometric probability of the dynamic region matching point using the basis matrix F and epipolar constraint in step 3) are:

computing the poles of matching points within a dynamic region using a basis matrix FThread

Then calculating the distance from the matching point to the epipolar line in the dynamic region

In formula (II) p' _i And p _i Is a pair of matching points, x, y, z are vector parametric representations of epipolar lines;

distance from matching point to polar line in dynamic region

Conversion to geometric probabilities using a binary Sigmod function

Where Dth is the threshold for static and dynamic point-to-epipolar distance.

6. The visual SLAM method based on target detection and geometric probability in a dynamic scene as claimed in claim 1, wherein the specific step of calculating the final geometric probability of the current frame matching point in step 4) is:

according to the last frame point p' _i Probability value of state probability

Predicting current frame point p _i Probability value of state probability

According to the last frame point p' _i Variance of state probability

Predicting current frame point p _i Variance of state probability

In the formula, delta ₀ Is the standard deviation of the state probability transition between the matching points;

calculating a Kalman gain k:

in the formula, delta ₁ The probability standard deviation of the observation state of the key point is;

according to point p _i Probability of observation state of

Update point p _i State probability of (2):

in the formula (I), the compound is shown in the specification,

represents a point p _i The state probability of (b) in the prediction of the current frame,

represents a point p _i The state probability of (2) is passed through the final result after Kalman filtering;

update point p _i Variance of state probability:

in the formula (I), the compound is shown in the specification,

for the current frame point p _i A predicted value of the state probability variance.

7. The visual SLAM method based on target detection and geometric probability under a dynamic scene as claimed in claim 1, wherein the specific steps of performing probability expansion on unmatched points of the dynamic region in step 5) to obtain the geometric probability of all feature points of the dynamic region and eliminating feature points not meeting the probability requirement are as follows:

searching for neighboring feature points p around the high-confidence feature point in the dynamic region that do not match the previous frame _i Updating neighboring feature points p _i State probability of

In the formula, p _j Is and point p _i The neighboring high-confidence feature points are,

is a point p _j The probability of the state of (a) is,

is a point p _i Observing state probabilities; c is a constant, λ (d) represents a distance factor;

if it is

If the value is less than the preset threshold value, p is added _i Dividing into dynamic points;

if it is

And if the value is larger than the threshold value, dividing the static points into static points, eliminating the dynamic points, and adding the static points in the dynamic area into the point set of the static area for subsequent positioning and drawing construction.

Images