CN112304311B - Method for solving BA problem based on differential evolution algorithm for SLAM process - Google Patents
Method for solving BA problem based on differential evolution algorithm for SLAM process Download PDFInfo
- Publication number
- CN112304311B CN112304311B CN201910690395.5A CN201910690395A CN112304311B CN 112304311 B CN112304311 B CN 112304311B CN 201910690395 A CN201910690395 A CN 201910690395A CN 112304311 B CN112304311 B CN 112304311B
- Authority
- CN
- China
- Prior art keywords
- individual
- value
- space
- image
- slam
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Image Analysis (AREA)
- Manipulator (AREA)
Abstract
The invention discloses a method for solving a BA problem based on a differential evolution algorithm for a SLAM process, which belongs to the field of mobile robot synchronous positioning and map construction (Simultaneous Localization and Mapping, SLAM), and comprises the following steps: reading a visual image of a robot, extracting characteristic points of a current frame and a previous frame image, and acquiring all matched characteristic point pairs; calculating the space coordinates corresponding to all the feature points matched with the current frame image in the previous frame image; calculating an error function according to the reprojection error of the space coordinates; and solving the pose of the camera corresponding to the optimal solution of the error function by utilizing a differential evolution algorithm to finish the solution of the BA problem. The method can avoid the problem of overlarge operand caused by the need of derivative operation in the existing method, can reduce the calculated amount of the BA process and ensures the instantaneity and rapidity of the SLAM process.
Description
Technical Field
The invention relates to the field of mobile robot synchronous positioning and map construction (Simultaneous Localization and Mapping, SLAM), in particular to a method for solving a BA problem based on a differential evolution algorithm for SLAM process.
Background
With the development of indoor mobile service robots, key technologies related to indoor mobile service robots become research hotspots, and a navigation positioning system is a core of the indoor mobile service robots, so that SLAM problems are key to the robots to realize autonomous navigation and are important points of research of people in the present and future.
The most widely known method in the field of SLAM problems is a smoothing-based method, which is a batch type SLAM method and divides SLAM into two parts of graph construction and graph optimization. In the visual SLAM algorithm of the graph optimization framework, a beam adjustment method (Bundle Adjustment, BA) plays a core role, and a nonlinear least square method is mainly adopted to optimally estimate the global pose and map of the robot, so that the robot can stably run for a long time.
In the visual odometer section of SLAM, a PnP (transparent-n-Point) problem is involved, i.e. the problem of solving the camera pose by means of known n 3D spatial points and their projection positions on the camera plane. After estimating the pose of the camera by using P3P or EPnP, the estimated value needs to be adjusted by a beam adjustment method (BA).
Conventional solutions of BA are currently divided into a linear search method represented by gauss newton's method and a trust region method represented by the levenberg-marquardt method. The two methods have the problems of large calculated amount, low speed, insufficient precision and the like.
The differential evolution algorithm (Differential Evolution Algorithm, DE) is provided for parameter optimization design on the basis of evolution ideas such as genetic algorithm, and the essence of the differential evolution algorithm is a multi-objective optimization algorithm for continuous variables, which is used for solving the overall optimal solution in a multidimensional space.
Disclosure of Invention
The invention aims to provide a resolving method which can be integrated into any SLAM scheme optimized by BA and can quickly solve the pose of a camera and optimize the back end in a visual odometer part by the SLAM program.
The technical solution for realizing the purpose of the invention is as follows: a method for BA problem solving based on differential evolution algorithm for SLAM process, comprising the steps of:
step 1, reading a visual image of a robot, extracting characteristic points of a current frame and a previous frame image, and acquiring all matched characteristic point pairs (a) i ，b i )；a i B, as the characteristic point in the previous frame of image i Characteristic points in the current frame image; wherein 0 isM is more than or equal to m and is the number of feature point pairs;
step 2, calculating all feature points a in the previous frame of image i Respectively corresponding space coordinates P i ；
Step 3, each feature point a obtained according to the step 2 i Is defined by the spatial coordinates P of i Corresponding feature point b of current frame i Calculating an error function;
and 4, solving the pose xi of the camera corresponding to the optimal solution of the error function by utilizing a differential evolution algorithm to finish the solution of the BA problem.
Compared with the prior art, the invention has the remarkable advantages that: 1) The nonlinear least square problem constructed by a beam adjustment method is solved by utilizing a differential evolution algorithm (DE), the derivative problems of technologies such as Gaussian Newton method, levenberg-Marquardt method and the like are avoided, the execution efficiency of an SLAM program can be improved, the solving speed is improved, and the precision is higher; 2) The method of the invention can be seamlessly accessed into the existing SLAM scheme without modifying other parts of the SLAM scheme, thereby being convenient for application.
Drawings
FIG. 1 is a flowchart of a method for BA problem solving based on a differential evolution algorithm for SLAM process.
Detailed Description
Referring to fig. 1, the method for solving the BA problem based on the differential evolution algorithm for SLAM process of the present invention comprises the following steps:
step 1, reading a visual image of a robot, extracting characteristic points of a current frame and a previous frame image, and acquiring all matched characteristic point pairs (a) i ，b i )；a i B, as the characteristic point in the previous frame of image i Characteristic points in the current frame image; wherein i is more than 0 and less than or equal to m M is the number of feature point pairs;
step 2, calculating all feature points a in the previous frame of image i Respectively corresponding space coordinates P i ；
Step 3, each feature point a obtained according to the step 2 i Is defined by the spatial coordinates P of i Corresponding feature point b of current frame i Calculating an error function;
and 4, solving the pose xi of the camera corresponding to the optimal solution of the error function by utilizing a differential evolution algorithm to finish the solution of the BA problem.
Further preferably, step 2 calculates all feature points a in the previous frame image i Respectively corresponding space coordinates P i The formula used is:
wherein K is an internal reference matrix of the camera, f x Is the product of the u-axis scaling factor of the image coordinates and the focal length, and is expressed in units of pixels, f y The unit is pixel, which is the product of the v-axis scaling factor of the image coordinates and the focal length; c x 、c y For the translation of the origin of the image, [ u, v ]] T The pixel coordinates of the feature points are Z, the depth of the feature points and P, the calculated space coordinates of the feature points.
Further, step 3 is to obtain each feature point a according to step 2 i Is defined by the spatial coordinates P of i Corresponding feature point b of current frame i Calculating an error function, specifically:
the feature point a i Is marked as P by the space sitting sign i ＝[X i ，Y i ，Z i ] T Feature point b i Pixel sitting sign b i ＝[u bi ，v bi ]；
Step 3-1, the feature points a obtained in step 2 are selected i Is defined by the spatial coordinates P of i Projecting to the current frame to obtain a pixel coordinate u i ＝[u i ，v i ] T ；
Step 3-2 according to u i ＝[u i ，v i ] T And b i ＝[u bi ，v bi ]Solving for the reprojection error e i The formula used is:
step 3-3, the actual depth of the feature point under the current camera pose is s i Let lie algebra of camera pose be ζ, ζ is defined as:
wherein, the front three-dimensional is translation, marked as rho, the rear three-dimensional is rotation, marked as phi, and according to the pinhole model of the camera, there are:
writing in a matrix form:
s i u i ＝K exp(ξ^)P i
the above equation s due to the error of the initial pose and the noise of the observation point i u i ＝K exp(ξ^)P i There is an error in the presence of the error,
according to the principle of a beam adjustment method, the re-projection errors of each pair of characteristic points are summed to construct a nonlinear least square problem, and an error function f (ζ) is obtained:
in the formula, xi is a lie algebra of the pose of the camera, and exp (ζ) represents a transformation matrix corresponding to xi;
the above problems translate into:
further, step 4 utilizes a differential evolution algorithm to obtain the pose ζ of the camera corresponding to the optimal solution of the error function, namely completing the solution of the BA problem, and specifically comprises the following steps:
step 4-1, initializing a population, and randomly and uniformly generating M individuals in a solution space, wherein each individual is a 6-dimensional vector;
and 4-2, performing mutation operation, specifically: during the evolution of the ith individual in the g-th iteration, 3 individuals are randomly selected from the population: x is x p1 (g)，x p2 (g)，x p3 (g) And p1 not equal to p2 not equal to p3 not equal to i, and obtaining a variation vector v i (g)；
Step 4-3, performing cross operation, specifically: during the evolution of the ith individual in the g-th iteration, v i (g) And the ith individual v in the current population i (g) The data of each dimension of (2) is crossed with the crossover probability Cr to generate a new individual u i (g)；
Step 4-4, selecting operation, specifically: during the evolution of the ith individual in the g-th iteration, a new individual u i (g) And the ith individual x in the current population i (g) Respectively substituting the fitness values into error functions to obtain respective corresponding fitness values, and selecting an individual with low fitness value as a next generation individual x i (g+1)；
Step 4-5, repeating the steps 4-2 to 4-4 until all individuals in the g-th iteration finish updating, and executing the next step;
and 4-6, judging termination conditions, wherein the termination conditions are as follows: let the fitness of each individual of the g generation be f (x i (g) Judging whether the difference e between the maximum fitness and the minimum fitness is smaller than a set threshold value, if so, ending the whole process, and outputting a value xi corresponding to the minimum fitness min And (3) finishing the solution of the BA problem, otherwise, entering the g+1st generation of circulation, and executing the steps 4-2 to 4-5.
Further, each of the 4-1M individuals is a 6-dimensional vector, specifically: each 6-dimensional individual of the M individuals is noted as x i I=1, 2,3, where, M, wherein, the solution space is a 6-dimensional space, the front three-dimensional value range is the size of the robot running space, and the rear three-dimensional value range is (-pi, pi)]M has a value of [5n,10n]Where n is the dimension of the solution space, i.e. [30,60]The j-th dimension of the i-th individual takes the value x i，j (0) The method comprises the following steps:
x i，j (0)＝L j_min +rand(0，1)·(L j_max -L j_min )
wherein L is j_min Represents the minimum value of the j-th dimension of the solution space, L j_max Representing the maximum value of the j-th dimension of the solution space, rand (0, 1) represents any value between 0 and 1.
Further, step 4-2 obtains a variance vector v i (g) The formula of (2) is:
v i (g)＝x p1 (g)+F·[x p2 (g)-x p3 (g)]
wherein F is a scaling factor, F.epsilon.0, 2.
Further, step 4-3 will v i (g) And the ith individual x in the current population i (g) The data of each dimension of (2) is crossed with the crossover probability Cr to generate a new individual u i (g) The method specifically comprises the following steps:
wherein j represents a j-th dimension element, v i，j (g) Represents the ith variation vector v i (g) Is x i，j (g) Represents the ith individual x i (g) Rand (0, 1) represents any value between 0 and 1, cr represents the crossover probability, cr ε [0,1 ]]。
The method for solving the BA problem based on the differential evolution algorithm for the SLAM process can avoid the problem of overlarge operand caused by the need of derivative operation in the existing method, can reduce the calculated amount of the BA process, and ensures the instantaneity and rapidity of the SLAM process.
Claims (6)
1. A method for BA problem solving based on differential evolution algorithm for SLAM process, characterized by comprising the steps of:
step 1, reading a visual image of a robot, extracting characteristic points of a current frame and a previous frame image, and acquiring all matched characteristic point pairs (a) i ,b i )；a i For the previous frameFeature points in the image, b i Characteristic points in the current frame image; wherein i is more than 0 and less than or equal to m, and m is the number of feature point pairs;
step 2, calculating all feature points a in the previous frame of image i Respectively corresponding space coordinates P i ；
Step 3, each feature point a obtained according to the step 2 i Is defined by the spatial coordinates P of i Corresponding feature point b of current frame i Calculating an error function;
step 4, solving the pose xi of the camera corresponding to the optimal solution of the error function by utilizing a differential evolution algorithm, namely completing the solution of the BA problem; the method comprises the following steps:
step 4-1, initializing a population, and randomly and uniformly generating M individuals in a solution space, wherein each individual is a 6-dimensional vector;
and 4-2, performing mutation operation, specifically: during the evolution of the ith individual in the g-th iteration, 3 individuals are randomly selected from the population: x is x p1 (g)，x p2 (g)，x p3 (g) And p1 not equal to p2 not equal to p3 not equal to i, and obtaining a variation vector v i (g)；
Step 4-3, performing cross operation, specifically: during the evolution of the ith individual in the g-th iteration, v i (g) And the ith individual v in the current population i (g) The data of each dimension of (2) is crossed with the crossover probability Cr to generate a new individual u i (g)；
Step 4-4, selecting operation, specifically: during the evolution of the ith individual in the g-th iteration, a new individual u i (g) And the ith individual x in the current population i (g) Respectively substituting the fitness values into error functions to obtain respective corresponding fitness values, and selecting an individual with low fitness value as a next generation individual x i (g+1)；
Step 4-5, repeating the steps 4-2 to 4-4 until all individuals in the g-th iteration finish updating, and executing the next step;
and 4-6, judging termination conditions, wherein the termination conditions are as follows: let the fitness of each individual of the g generation be f (x i (g) Determining whether the difference e between the maximum fitness and the minimum fitness is smaller than the set valueIf the threshold value is smaller than the threshold value, ending the whole process and outputting a value xi corresponding to the minimum adaptability min And (3) finishing the solution of the BA problem, otherwise, entering the g+1st generation of circulation, and executing the steps 4-2 to 4-5.
2. The method for BA problem solving by differential evolution algorithm based on SLAM procedure according to claim 1, wherein step 2 said calculating all feature points a in previous frame image i Respectively corresponding space coordinates P i The formula used is:
wherein K is an internal reference matrix of the camera, f x Is the product of the u-axis scaling factor of the image coordinates and the focal length, and is expressed in units of pixels, f y The unit is pixel, which is the product of the v-axis scaling factor of the image coordinates and the focal length; c x 、c y For the translation of the origin of the image, [ u, v ]] T The pixel coordinates of the feature points are Z, the depth of the feature points and P, the calculated space coordinates of the feature points.
3. The method for BA problem solving based on differential evolutionary algorithm for SLAM process according to claim 1 or 2, wherein the feature points a according to step 2 are the feature points a according to step 3 i Is defined by the spatial coordinates P of i Corresponding feature point b of current frame i Calculating an error function, specifically:
the feature point a i Is marked as P by the space sitting sign i ＝[X i ,Y i ,Z i ] T Feature point b i Pixel sitting sign b i ＝[u bi ,v bi ]；
Step 3-1, the feature points a obtained in step 2 are selected i Is defined by the spatial coordinates P of i Projecting to the current frame to obtain a pixel coordinate u i ＝[u i ,v i ] T ；
Step 3-2, according to u i ＝[u i ,v i ] T And b i ＝[u bi ,v bi ]Solving for the reprojection error e i The formula used is:
step 3-3, summing the re-projection errors of each pair of characteristic points according to the principle of a beam adjustment method, constructing a nonlinear least square problem, and obtaining an error function f (ζ):
in the formula, xi is a lie algebra of the pose of the camera, and exp (ζ) represents a transformation matrix corresponding to xi.
4. The method for BA problem solving based on differential evolution algorithm for SLAM process according to claim 1, wherein each of the M individuals in step 4-1 is a 6-dimensional vector, specifically:
each 6-dimensional individual of the M individuals is noted as x i I=1, 2,3, …, M, wherein the solution space is a 6-dimensional space, the value range of the front three-dimensional space is the size of the robot running space, and the value range of the rear three-dimensional space is (-pi, pi)]M has a value of [5n,10n]Where n is the dimension of the solution space, i.e. [30,60 ]]The j-th dimension of the i-th individual takes the value x i,j (0) The method comprises the following steps:
x i,j (0)＝L j_min +rand(0,1)·(L j_max -L j_min )
wherein L is j_min Represents the minimum value of the j-th dimension of the solution space, L j_max Representing the maximum value of the j-th dimension of the solution space, rand (0, 1) represents any value between 0 and 1.
5. The method for BA problem solving based on differential evolutionary algorithm for SLAM process of claim 1, which isCharacterized in that the variation vector v is obtained in the step 4-2 i (g) The formula of (2) is:
v i (g)＝x p1 (g)+F·[x p2 (g)-x p3 (g)]
wherein F is a scaling factor, F.epsilon.0, 2.
6. The method for BA problem solving by differential evolution algorithm based on SLAM procedure according to claim 1, wherein step 4-3 is characterized by combining v i (g) And the ith individual x in the current population i (g) The data of each dimension of (2) is crossed with the crossover probability Cr to generate a new individual u i (g) The method specifically comprises the following steps:
wherein j represents a j-th dimension element, v i,j (g) Represents the ith variation vector v i (g) Is x i,j (g) Represents the ith individual x i (g) Rand (0, 1) represents any value between 0 and 1, ce represents the crossover probability, ce ε [0,1 ]]。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910690395.5A CN112304311B (en) | 2019-07-29 | 2019-07-29 | Method for solving BA problem based on differential evolution algorithm for SLAM process |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910690395.5A CN112304311B (en) | 2019-07-29 | 2019-07-29 | Method for solving BA problem based on differential evolution algorithm for SLAM process |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112304311A CN112304311A (en) | 2021-02-02 |
| CN112304311B true CN112304311B (en) | 2023-08-22 |
Family
ID=74328924
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910690395.5A Active CN112304311B (en) | 2019-07-29 | 2019-07-29 | Method for solving BA problem based on differential evolution algorithm for SLAM process |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112304311B (en) |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106952341A (en) * | 2017-03-27 | 2017-07-14 | 中国人民解放军国防科学技术大学 | The underwater scene three-dimensional point cloud method for reconstructing and its system of a kind of view-based access control model |
| CN107680133A (en) * | 2017-09-15 | 2018-02-09 | 重庆邮电大学 | A kind of mobile robot visual SLAM methods based on improvement closed loop detection algorithm |
-
2019
- 2019-07-29 CN CN201910690395.5A patent/CN112304311B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106952341A (en) * | 2017-03-27 | 2017-07-14 | 中国人民解放军国防科学技术大学 | The underwater scene three-dimensional point cloud method for reconstructing and its system of a kind of view-based access control model |
| CN107680133A (en) * | 2017-09-15 | 2018-02-09 | 重庆邮电大学 | A kind of mobile robot visual SLAM methods based on improvement closed loop detection algorithm |
Non-Patent Citations (1)
| Title |
|---|
| 移动机器人FastSLAM算法的对比研究;汤文俊 等;《计算机工程与设计》(第3期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112304311A (en) | 2021-02-02 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108416840B (en) | Three-dimensional scene dense reconstruction method based on monocular camera | |
| CN110503680B (en) | Unsupervised convolutional neural network-based monocular scene depth estimation method | |
| CN101996420B (en) | Information processing device, information processing method and program | |
| CN108133496B (en) | Dense map creation method based on g2o and random fern algorithm | |
| CN111645065A (en) | Mechanical arm motion planning method based on deep reinforcement learning | |
| CN105469110B (en) | Non-rigid transformation Image Feature Matching method and system based on local linear migration | |
| CN109544687B (en) | Directional layered household graph expansion method | |
| CN113450396A (en) | Three-dimensional/two-dimensional image registration method and device based on bone features | |
| CN113077497A (en) | Tunneling roadway point cloud registration method based on 3D NDT-ICP algorithm | |
| CN112686952A (en) | Image optical flow computing system, method and application | |
| CN111709984A (en) | Pose depth prediction method, visual odometer method, device, equipment and medium | |
| Li et al. | NR-MVSNet: Learning multi-view stereo based on normal consistency and depth refinement | |
| CN112304311B (en) | Method for solving BA problem based on differential evolution algorithm for SLAM process | |
| CN112991421A (en) | Robot vision stereo matching method | |
| CN100590658C (en) | Method for matching two dimensional object point and image point with bilateral constraints | |
| CN116844004A (en) | Point cloud automatic semantic modeling method for digital twin scene | |
| CN103198510B (en) | The model gradual deformation method of data-driven | |
| CN114399547B (en) | Monocular SLAM robust initialization method based on multiframe | |
| CN115861563A (en) | Three-dimensional reconstruction method for registration of topological rigid point cloud of graph | |
| CN116030335A (en) | Visual positioning method and system based on indoor building framework constraint | |
| CN113592021B (en) | Stereo matching method based on deformable and depth separable convolution | |
| CN112116640B (en) | Binocular stereo matching method based on OpenCL | |
| CN105469112B (en) | Image Feature Matching method and system based on local linear migration and rigid model | |
| CN111080517B (en) | Three-dimensional point cloud splicing method based on improved butterfly optimization algorithm | |
| CN114419255A (en) | Three-dimensional human head model generation method and device fusing real human faces, electronic equipment and storage medium |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |