CN107240133A - Stereoscopic vision mapping model building method - Google Patents

Abstract

The present invention discloses a method for establishing a stereoscopic vision mapping model. Compared with the prior art, the present invention proposes a method for calibrating key distortion factors based on the Tsai improved algorithm, and performs distortion correction on the image in steps, thereby avoiding the increase in the amount of calculation caused by the nonlinear mapping of the lens, reducing the number of algorithm iterations, and is an efficient and simple algorithm. In order to further improve the calibration accuracy, a nonlinear optimization algorithm of particle swarm optimization particle filtering is adopted. Through particle swarm optimization, the particle set is moved toward the area with a larger value of the posterior probability density distribution, thereby overcoming the particle poverty problem, greatly reducing the number of particles required for accurate estimation, and improving the estimation accuracy. At the same time, the projection pixel error is used as a constraint condition during particle swarm optimization to improve the accuracy and robustness of the calibration, and has a good application prospect in stereoscopic vision navigation.

Description

A method for building stereo vision mapping model

technical field

The invention relates to the technical field of machine vision, in particular to a method for establishing a stereo vision mapping model.

Background technique

The current types of vision systems mainly include monocular vision systems and stereo vision systems. The simple monocular vision system imaging method is based on linear lens imaging. After considering the structure of the vision system, its imaging is extended to the spatial imaging coordinate system, and The global coordinate system, camera coordinate system, imaging plane coordinate system and pixel coordinate system are constructed respectively. The purpose of object space point conversion is to extract the three-dimensional space coordinate value of the object from the imaging picture. The stereo vision system adds the depth information of the object point on the basis of the monocular imaging principle, which enables the mobile robot to recognize The attitude and spatial orientation of the target. However, the distortion factor during camera lens processing will inevitably affect the vision system's judgment on the imaging position of the spatial coordinate point. Therefore, the use of mathematical algorithms to reasonably solve the distortion factor is also the key to improving the cognitive information accuracy of the mobile robot vision system.

Contents of the invention

The object of the present invention is to provide a stereo vision mapping model building method in order to solve the above problems.

The present invention achieves the above object through the following technical solutions:

1, the present invention comprises the following steps:

Step 1: Particle sampling; a total of 28 internal and external parameters of the stereo vision calibrated by the improved Tsai algorithm are sampled, so the sampled particles have a total of 28 dimensions, and the random sampling method is used for particle sampling. The importance density function is a Gaussian distribution, and the variance can be obtained through experience Obtained; a particle swarm H={h ₁ , h ₂ ,…,h _N } of size N is generated in the space, h is the 28-dimensional vector of each particle in the space, h=(α _x1 ,α _y1 ,u _0l ,v _0l ,k _1l ,k _2l ,p _1l ,p _2l ,α _l ,β _l ,γ _l ,t _xl ,t _yl ,t _zl ,α _xr ,α _yr ,u _0r ,v _0r ,k _1r , k _2r ,p _1r ,p _2r ,α _r ,β _r ,γ _r ,t _xr t _yr ,t _zr )

Step 2: Use the distortion parameters of the sampled particles to correct the image, and use other parameters of the sampled particles to generate a projection matrix, so that each particle space (28-dimensional vector) generates two corresponding distortion-corrected images and two projection matrices;

Step 3: Determine the fitness function;

P _i is the pixel coordinate of the actual projection of the k-space point, Indicates that the current particle h is used as the pixel coordinate value estimated by the stereo vision nonlinear model parameter, and is solved by using the projection matrix generated by each particle;

Step 4: Particle swarm update;

In order to effectively control the particle migration speed and make the algorithm have fine search ability, the idea of simulated annealing algorithm is used to introduce the inertia factor in the particle swarm update strategy; in each migration, each particle updates its position and speed according to the following criteria

In formula (2), is the d-dimensional velocity of the i-th particle when it migrates for the k-th time; c ₁ is the cognitive factor, c ₂ is the social factor, which adjust the step size of the particle’s migration to the individual extremum and the global extremum respectively; r ₁ and r ₂ is a random number distributed between (0, 1); is the individual extremum position of the i-th particle in the d-dimension, is the global extremum position of the particle swarm in the d-th dimension, with In the iterative process, it is updated in real time according to the fitness of the particles; the inertia factor ω=b–k(b–a)/K, b=0.9, a=0.4, and K is the maximum number of migrations;

Step 5: Particle swarm update ends;

When the particle swarm reaches the set cut-off migration times K or the particle fitness value reaches the expected value ζ, the particle swarm optimization stops; the particles after the update are subjected to particle filtering, and the particle filtering uses the 3D coordinates of the checkerboard feature points as the optimization condition, which is also in line with Practical application of stereo vision;

Step 6: Get observations of feature points

Take the first position of the checkerboard as the world coordinate system, and obtain the coordinate values of each feature point of the checkerboard as the observation value. The checkerboard adopts a 30cm*30cm black and white square, and it is easy to obtain the observation value of each feature point;

Step 7: Predicted value of feature points Solve; extract the pixel coordinates of the feature points in the left and right camera images, and use the current particle h as the nonlinear parameter of stereo vision to calculate the predicted value of the corresponding feature points; the calculation method is as follows;

Among them, (X _W , Y _W , Z _W , 1) is the homogeneous coordinate of point P in the world coordinate system, (u ₁ , v ₁ , 1) and (u ₂ , v ₂ , 1) are point P The homogeneous coordinates projected on the image coordinate system of the left and right cameras; four linear equations about X _W , Y _W , and Z _W can be obtained from formulas (3) and (4):

Formula (5) has 3 unknowns and 4 equations. In order to improve the calculation accuracy and reduce the noise influence, the least square method is used to solve it;

Step 8: Particle weight calculation and normalization processing;

Particle weight calculation is to calculate the probability of each particle _hi ;

Since the error distribution between each calibration feature point P _i is independent, the calculation formula of the probability of calibration parameter sampling particles is as follows:

It can be seen from formula (6) that if the influence of each feature point P _i on the calibration parameter sampling particles can be obtained, that is, p(P _i |h _i ), it is easy to obtain the calibration parameter sampling particle through formula (6). The probability of particles under the influence of all feature points; the observed value of each feature point The predicted value is For simplicity of calculation, let p(P _i |h _i ) obey the Gaussian distribution, where the mean value u is with The difference, the variance is the observation variance and predicted values The sum of the variances;

Among them, the observed value is obtained by checkerboard, the error is negligible, and the predicted value is obtained by stereo vision;

The landmark error is calculated as follows:

In formula (8), r and c are the projected coordinates of feature points on the left image, and r ₀ and c ₀ are the center coordinates of the left image. The variables c, r, and d are considered to be Gaussian random distribution with zero mean, according to the covariance before to the propagation theorem,

J is the Jacobian matrix of formula (9), are the covariances of the corresponding variables, and generally take Formula (9) is calculated to get:

Normalized weights:

Step 9: Find the accurate estimated value of the calibration parameter and its covariance. The accurate estimated value of the calibration parameter is to multiply each group of sampled particles by the normalized weight and then sum as in formula (12). The covariance of the accurate estimated value of the calibration parameter is as follows: Formula (13):

The particle filter used for stereo vision calibration optimization algorithm is better combined with the specific application of stereo vision navigation, and the overall optimization of all parameters minimizes the 3D coordinate reprojection error.

The beneficial effects of the present invention are:

The invention is a method for establishing a stereo vision mapping model. Compared with the prior art, the invention proposes an improved algorithm based on Tsai for the calibration of key distortion factors. The implementation method based on OpenCV is introduced in detail. The algorithm fully considers the characteristics of the least square method of the OpenCV function library, and corrects the distortion of the image step by step, avoiding the increase in calculation caused by the nonlinear mapping of the lens, and reducing the number of iterations of the algorithm. Efficient and simple algorithm. In order to further improve the calibration accuracy, the nonlinear optimization algorithm of particle swarm optimization and particle filter is adopted. Through particle swarm optimization, the particle set moves towards the area with a larger value of the posterior probability density distribution, thus overcoming the problem of particle poverty and greatly improving the calibration accuracy. Reduced the number of particles required for accurate estimation and improved estimation accuracy. At the same time, the particle swarm optimization uses the projected pixel error as the constraint condition, while the particle filter calculates the particle weight with the 3D coordinate error of the feature point of the checkerboard grid, fully considering the 2D and 3D errors, greatly improving the calibration accuracy and robustness sex. Finally, the experimental analysis is carried out, and the 3D projection error of the calibration board feature points is used as the accuracy evaluation index. The results show that the algorithm has high precision and good robustness, and has a good application prospect in stereo vision navigation.

Description of drawings

Fig. 1 is a 3D landmark uncertainty figure of the present invention;

Fig. 2 is a checkerboard plane calibration mold of the present invention;

Fig. 3 is a comparison diagram of successfully detected feature points of the present invention;

Fig. 4 is a distortion correction target of the present invention.

detailed description

The present invention will be further described below in conjunction with accompanying drawing:

The present invention comprises the following steps:

Step 1: Particle sampling; a total of 28 internal and external parameters of the stereo vision calibrated by the improved Tsai algorithm are sampled, so the sampled particles have a total of 28 dimensions, and the random sampling method is used for particle sampling. The importance density function is a Gaussian distribution, and the variance can be obtained through experience Obtained; a particle swarm H={h ₁ , h ₂ ,…,h _N } of size N is generated in the space, h is the 28-dimensional vector of each particle in the space, h=(α _x1 ,α _y1 ,u _0l ,v _0l ,k _1l ,k _2l ,p _1l ,p _2l ,α _l ,β _l ,γ _l ,t _xl ,t _yl ,t _zl ,α _xr ,α _yr ,u _0r ,v _0r ,k _1r , k _2r ,p _1r ,p _2r ,α _r ,β _r ,γ _r ,t _xr t _yr ,t _zr )

Step 2: Use the distortion parameters of the sampled particles to correct the image, and use other parameters of the sampled particles to generate a projection matrix, so that each particle space (28-dimensional vector) generates two corresponding distortion-corrected images and two projection matrices;

Step 3: Determine the fitness function;

P _i is the pixel coordinate of the actual projection of the k-space point, Indicates that the current particle h is used as the pixel coordinate value estimated by the stereo vision nonlinear model parameter, and is solved by using the projection matrix generated by each particle;

Step 4: Particle swarm update;

In order to effectively control the particle migration speed and make the algorithm have fine search ability, the idea of simulated annealing algorithm is used to introduce the inertia factor in the particle swarm update strategy; in each migration, each particle updates its position and speed according to the following criteria

In formula (2), is the d-dimensional velocity of the i-th particle when it migrates for the k-th time; c ₁ is the cognitive factor, c ₂ is the social factor, which adjust the step size of the particle’s migration to the individual extremum and the global extremum respectively; r ₁ and r ₂ is a random number distributed between (0, 1); is the individual extremum position of the i-th particle in the d-dimension, is the global extremum position of the particle swarm in the d-th dimension, with In the iterative process, it is updated in real time according to the fitness of the particles; the inertia factor ω=b–k(b–a)/K, b=0.9, a=0.4, and K is the maximum number of migrations;

Step 5: Particle swarm update ends;

When the particle swarm reaches the set cut-off migration times K or the particle fitness value reaches the expected value ζ, the particle swarm optimization stops; the particles after the update are subjected to particle filtering, and the particle filtering uses the 3D coordinates of the checkerboard feature points as the optimization condition, which is also in line with Practical application of stereo vision;

Step 6: Get observations of feature points

Take the first position of the checkerboard as the world coordinate system, and obtain the coordinate values of each feature point of the checkerboard as the observation value. The checkerboard adopts a 30cm*30cm black and white square, and it is easy to obtain the observation value of each feature point;

Step 7: Predicted value of feature points Solve; extract the pixel coordinates of the feature points in the left and right camera images, and use the current particle h as the nonlinear parameter of stereo vision to calculate the predicted value of the corresponding feature points; the calculation method is as follows;

Among them, (X _W , Y _W , Z _W , 1) is the homogeneous coordinate of point P in the world coordinate system, (u ₁ , v ₁ , 1) and (u ₂ , v ₂ , 1) are point P The homogeneous coordinates projected on the image coordinate system of the left and right cameras; four linear equations about X _W , Y _W , and Z _W can be obtained from formulas (3) and (4):

Formula (5) has 3 unknowns and 4 equations. In order to improve the calculation accuracy and reduce the noise influence, the least square method is used to solve it;

Step 8: Particle weight calculation and normalization processing;

Particle weight calculation is to calculate the probability of each particle _hi ;

Since the error distribution between each calibration feature point P _i is independent, the calculation formula of the probability of calibration parameter sampling particles is as follows:

It can be seen from formula (6) that if the influence of each feature point P _i on the calibration parameter sampling particles can be obtained, that is, p(P _i |h _i ), it is easy to obtain the calibration parameter sampling particle through formula (6). The probability of particles under the influence of all feature points; the observed value of each feature point The predicted value is For simplicity of calculation, let p(P _i |h _i ) obey the Gaussian distribution, where the mean value u is with The difference, the variance is the observation variance and predicted values The sum of the variances;

Among them, the observed value is obtained by checkerboard, the error is negligible, and the predicted value is obtained by stereo vision;

The road sign error is formed as shown in Figure 1, and is calculated as follows:

In formula (8), r and c are the projected coordinates of feature points on the left image, and r ₀ and c ₀ are the center coordinates of the left image. The variables c, r, and d are considered to be Gaussian random distribution with zero mean, according to the covariance before to the propagation theorem,

J is the Jacobian matrix of formula (9), are the covariances of the corresponding variables, and generally take Formula (9) is calculated to get:

Normalized weights:

Step 9: Find the accurate estimated value of the calibration parameter and its covariance. The accurate estimated value of the calibration parameter is to multiply each group of sampled particles by the normalized weight and then sum as in formula (12). The covariance of the accurate estimated value of the calibration parameter is as follows: Formula (13):

The particle filter used for stereo vision calibration optimization algorithm is better combined with the specific application of stereo vision navigation, and the overall optimization of all parameters minimizes the 3D coordinate reprojection error.

Experiment and Result Analysis

As shown in Figure 2: Since the accuracy of the 3D calibration object is difficult to guarantee, a checkerboard plane calibration template with different spatial orientations and different black and white alternate arrangements is used here. The Bumblebee2Camera produced by Point Gray Research is used as the image acquisition device. The baseline length of the binocular vision system is 120mm, the maximum resolution is 1024×768, the pixel area is 4.65um×4.65um, the frame rate is 30FPS, the focal length at 70°horizontal viewing angle is 3.8mm, and the focal length at 50°horizontal viewing angle is 6mm , the digital image bit depth is set to 24bit by the supported data format, and the image signal-to-noise ratio is greater than 60dB when the signal-to-noise ratio is 0dB.

The size of the target checkerboard calibration board is 270mm×210mm, the size of each checkerboard is 30mm×30mm, the center distance is 30mm, the array is 9×7, the pixel ratio of each image is 640×480, the bit depth is 8bit, and the flatness error of the calibration surface The range is within -0.05mm, and the angle between the lens imaging plane and the calibration target surface is kept within 50°.

During the experiment, the binocular camera collected a total of 3 groups of 10 images of different spatial orientations, and it should be ensured that the acquired checkerboard occupies most of the imaging plane. When the algorithm extracts feature points, it first draws the feature point image within the range of the center point of the image to avoid the actual calibration error caused by the excessive distortion of the image edge. The number of rows and columns of feature points. For images that are successfully extracted, the last feature point of the previous row is connected with the first feature point of the next row by a colored straight line to determine whether all the feature points are found. Each row of feature points is distinguished by a circle of a different color. The three checkerboard images in the first row in the figure are the feature point extraction results of the left camera, and the three images in the second row are the feature point extraction results of the right camera under the same spatial imaging mapping.

As shown in Figure 3: In order to verify the feasibility of the calibration method proposed in this paper, the particle swarm optimization particle filter optimization algorithm (method 1) proposed in this paper is respectively combined with the stereo vision two-step calibration method of nonlinear optimization calibration of genetic algorithm ^[81] (method 2) Compared with the stereo vision two-step calibration method ^[77] (method 3) that only uses the particle swarm optimization algorithm for nonlinear optimization calibration, the number of particles in method 1 is 500, and the number of cut-off migration is 500. In method 2, the genetic cut-off evolution algebra is 10000, the number of particle swarm cut-off migration in method 3 is 3000, the specific parameters after optimization are shown in Table 3.2.

Table 3.2 Comparison of parameters

After particle swarm optimization and particle filter optimization algorithm, it is closer to the real value. In order to further evaluate the pros and cons of the algorithm, combined with the application environment of stereo vision navigation, the root mean square of the real coordinate point in space and the corresponding coordinate value after reconstruction is used as the evaluation index. Such as formula (3-30), its parameters are shown in Table 3.3.

Table 3.3 Performance evaluation results

The mutual position parameters of the stereo vision calibrated by the three algorithms are shown in Table 3.4, and the parameters of the particle swarm optimization particle filter are closer to the factory parameters.

Table 3.4 Comparison of mutual position parameters between left and right cameras

Finally, the calibrated distortion parameters are used to correct the distortion of the image. The result of the distortion correction of the checkerboard image in Figure 3 is shown in Figure 4. It can be seen that the algorithm can significantly correct the radial distortion and tangential distortion that occur at the edge of the image during lens imaging. distortion.

summary

The content of the present invention introduces the calibration of stereo vision. Through the analysis of the calibration methods of several commonly used vision systems at present, a multi-distortion model stereo camera calibration method based on 2D plane targets is proposed. The algorithm linearly solves the scale focal length factor, Ten camera parameters including the center point coordinates of the pixel plane, rotation matrix parameters, and translation matrix parameters. For the calibration of key distortion factors, an improved algorithm based on Tsai is proposed. The implementation method based on OpenCV is introduced in detail. The algorithm fully considers the characteristics of the least square method of the OpenCV function library, and corrects the distortion of the image step by step, avoiding the increase in calculation caused by the nonlinear mapping of the lens, and reducing the number of iterations of the algorithm. Efficient and simple algorithm. In order to further improve the calibration accuracy, the nonlinear optimization algorithm of particle swarm optimization and particle filter is adopted. Through particle swarm optimization, the particle set moves towards the area with a larger value of the posterior probability density distribution, thus overcoming the problem of particle poverty and greatly improving the calibration accuracy. Reduced the number of particles required for accurate estimation and improved estimation accuracy. At the same time, the particle swarm optimization uses the projected pixel error as the constraint condition, and the particle filter calculates the particle weight with the 3D coordinate error of the feature point of the checkerboard. It fully considers the 2D and 3D errors and greatly improves the calibration accuracy and robustness. sex. Finally, the experimental analysis is carried out, and the 3D projection error of the calibration board feature points is used as the accuracy evaluation index. The results show that the algorithm has high precision and good robustness, and has a good application prospect in stereo vision navigation.

The basic principles and main features of the present invention and the advantages of the present invention have been shown and described above. Those skilled in the industry should understand that the present invention is not limited by the above-mentioned embodiments. What are described in the above-mentioned embodiments and the description only illustrate the principle of the present invention. Without departing from the spirit and scope of the present invention, the present invention will also have Variations and improvements are possible, which fall within the scope of the claimed invention. The protection scope of the present invention is defined by the appended claims and their equivalents.

Claims (1)

1. a kind of stereoscopic vision mapping model method for building up, it is characterised in that：Comprise the following steps：

Step 1：Particle sampler；The stereoscopic vision inside and outside parameter demarcated by improving Tsai algorithms is sampled for 28 totally, because This sampling particle has 28 dimensions, carries out particle sampler using stochastical sampling method, the importance density function is Gaussian Profile, and variance can To be obtained by experience；Population H={ the h that scale is N are produced in space₁,h₂,…,h_N, h is each particle in space 28 n dimensional vector ns,

H=(α_x1,α_y1,u_0l,v_0l,k_1l,k_2l,p_1l,p_2l,α_l,β_l,γ_l,t_xl,t_yl,t_zl,α_xr,α_yr,u_0r,v_0r,k_1r,k_2r,p_1r, p_2r,α_r,β_r,γ_r,t_xr t_yr,t_zr)

Step 2：Image is corrected using the distortion parameter for particle of sampling, is generated and projected using the other specification for particle of sampling Matrix, so each particles spatial (28 n dimensional vector n) generates corresponding 2 width distortion correction image and 2 projection matrixes；

Step 3：Determine fitness function；

<mrow> <mi>E</mi> <mrow> <mo>(</mo> <mi>h</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mn>1</mn> <msqrt> <mrow> <mfrac> <mn>1</mn> <mi>n</mi> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <mo>|</mo> <mo>|</mo> <msub> <mi>p</mi> <mi>i</mi> </msub> <mo>-</mo> <mover> <msub> <mi>p</mi> <mrow> <mi>i</mi> <mi>h</mi> </mrow> </msub> <mo>^</mo> </mover> <mo>|</mo> <msup> <mo>|</mo> <mn>2</mn> </msup> </mrow> </msqrt> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow>

P_iFor the pixel coordinate of the actual projection of k-space point,Represent that current particle h estimates as stereoscopic vision nonlinear model shape parameter The pixel coordinate value of meter, the projection matrix generated using each particle is solved；

Step 4：Population updates；

In order to efficiently control particle migration speed, make algorithm that there is fine search ability, use for reference simulated annealing thought and exist Inertial factor is introduced in population more new strategy；In each migration, each particle is according to following criterion more new position and speed

<mrow> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>v</mi> <mrow> <mi>i</mi> <mi>d</mi> </mrow> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msubsup> <mo>=</mo> <msubsup> <mi>&amp;omega;v</mi> <mrow> <mi>i</mi> <mi>d</mi> </mrow> <mi>k</mi> </msubsup> <mo>+</mo> <msub> <mi>c</mi> <mn>1</mn> </msub> <msub> <mi>r</mi> <mn>1</mn> </msub> <mrow> <mo>(</mo> <msubsup> <mi>P</mi> <mrow> <mi>i</mi> <mi>d</mi> </mrow> <mi>k</mi> </msubsup> <mo>-</mo> <msubsup> <mi>h</mi> <mrow> <mi>i</mi> <mi>d</mi> </mrow> <mi>k</mi> </msubsup> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>c</mi> <mn>2</mn> </msub> <msub> <mi>r</mi> <mn>2</mn> </msub> <mrow> <mo>(</mo> <msubsup> <mi>P</mi> <mrow> <mi>h</mi> <mi>d</mi> </mrow> <mi>k</mi> </msubsup> <mo>-</mo> <msubsup> <mi>h</mi> <mrow> <mi>i</mi> <mi>d</mi> </mrow> <mi>k</mi> </msubsup> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>h</mi> <mrow> <mi>i</mi> <mi>d</mi> </mrow> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msubsup> <mo>=</mo> <msubsup> <mi>h</mi> <mrow> <mi>i</mi> <mi>d</mi> </mrow> <mi>k</mi> </msubsup> <mo>+</mo> <msubsup> <mi>v</mi> <mrow> <mi>i</mi> <mi>d</mi> </mrow> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msubsup> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow>

In formula (2),For i-th of particle, in kth time migration, d ties up speed；c₁For perception factor, c₂For the social factor, divide Tiao Jie not the step-length that is migrated to individual extreme value and global extremum direction of particle；r₁And r₂For the random number being distributed between (0,1)； The individual extreme value place tieed up for i-th of particle in d,The global extremum position tieed up for population in d,WithRepeatedly According to particle fitness real-time update during generation；Inertial factor ω=b-k (b-a)/K, b=0.9, a=0.4, K move for maximum Move number of times；

Step 5：Population, which updates, to be terminated；

When population reaches that the cutoff number of times K or particle adaptive value of setting reach desired value ζ, population stops optimization；Will Update the particle after terminating and carry out particle filter, particle filter is also using the 3D coordinates of gridiron pattern characteristic point as optimal conditions Meet stereoscopic vision practical application；

Step 6：Obtain characteristic point observation

Using first position of gridiron pattern as world coordinate system, the coordinate value of each characteristic point of gridiron pattern is asked for as observation, chess The black and white grid for the 30cm*30cm that disk lattice are used, it is easy to obtain the observation of a characteristic point；

Step 7：Characteristic point predicted valueSolve；Pixel coordinate of the characteristic point in left and right cameras image is extracted, with current particle h The predicted value of character pair point is calculated as the nonlinear parameter of stereoscopic vision；Computational methods are as follows；

<mrow> <msub> <mi>Z</mi> <mrow> <mi>C</mi> <mn>1</mn> </mrow> </msub> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>u</mi> <mn>1</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>v</mi> <mn>1</mn> </msub> </mtd> </mtr> <mtr> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msup> <msub> <mi>m</mi> <mn>11</mn> </msub> <mn>1</mn> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msub> <mi>m</mi> <mn>12</mn> </msub> <mn>1</mn> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msub> <mi>m</mi> <mn>13</mn> </msub> <mn>1</mn> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msub> <mi>m</mi> <mn>14</mn> </msub> <mn>1</mn> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msup> <msub> <mi>m</mi> <mn>21</mn> </msub> <mn>1</mn> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msub> <mi>m</mi> <mn>22</mn> </msub> <mn>1</mn> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msub> <mi>m</mi> <mn>23</mn> </msub> <mn>1</mn> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msub> <mi>m</mi> <mn>24</mn> </msub> <mn>1</mn> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msup> <msub> <mi>m</mi> <mn>31</mn> </msub> <mn>1</mn> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msub> <mi>m</mi> <mn>32</mn> </msub> <mn>1</mn> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msub> <mi>m</mi> <mn>33</mn> </msub> <mn>1</mn> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msub> <mi>m</mi> <mn>34</mn> </msub> <mn>1</mn> </msup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>X</mi> <mi>W</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>Y</mi> <mi>W</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>Z</mi> <mi>W</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow>

<mrow> <msub> <mi>Z</mi> <mrow> <mi>C</mi> <mn>2</mn> </mrow> </msub> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>u</mi> <mn>2</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>v</mi> <mn>2</mn> </msub> </mtd> </mtr> <mtr> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msup> <msub> <mi>m</mi> <mn>11</mn> </msub> <mn>2</mn> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msub> <mi>m</mi> <mn>12</mn> </msub> <mn>2</mn> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msub> <mi>m</mi> <mn>13</mn> </msub> <mn>2</mn> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msub> <mi>m</mi> <mn>14</mn> </msub> <mn>2</mn> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msup> <msub> <mi>m</mi> <mn>21</mn> </msub> <mn>2</mn> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msub> <mi>m</mi> <mn>22</mn> </msub> <mn>2</mn> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msub> <mi>m</mi> <mn>23</mn> </msub> <mn>2</mn> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msub> <mi>m</mi> <mn>24</mn> </msub> <mn>2</mn> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msup> <msub> <mi>m</mi> <mn>31</mn> </msub> <mn>2</mn> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msub> <mi>m</mi> <mn>32</mn> </msub> <mn>2</mn> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msub> <mi>m</mi> <mn>33</mn> </msub> <mn>2</mn> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msub> <mi>m</mi> <mn>34</mn> </msub> <mn>2</mn> </msup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>X</mi> <mi>W</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>Y</mi> <mi>W</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>Z</mi> <mi>W</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> </mrow>

Wherein, (X_W, Y_W, Z_W, 1) and it is homogeneous coordinates of the point P under world coordinate system, (u₁, v₁, 1) and (u₂, v₂, 1) and it is respectively point P The homogeneous coordinates of the image coordinate system projection of camera in left and right；It can be obtained on X by formula (3) and (4)_W, Y_W, Z_WFour lines Property equation：

<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mo>(</mo> <msub> <mi>u</mi> <mn>1</mn> </msub> <msubsup> <mi>m</mi> <mn>31</mn> <mn>1</mn> </msubsup> <mo>-</mo> <msubsup> <mi>m</mi> <mn>11</mn> <mn>1</mn> </msubsup> <mo>)</mo> <msub> <mi>X</mi> <mi>W</mi> </msub> <mo>+</mo> <mo>(</mo> <msub> <mi>u</mi> <mn>1</mn> </msub> <msubsup> <mi>m</mi> <mn>32</mn> <mn>1</mn> </msubsup> <mo>-</mo> <msubsup> <mi>m</mi> <mn>12</mn> <mn>1</mn> </msubsup> <mo>)</mo> <msub> <mi>Y</mi> <mi>W</mi> </msub> <mo>+</mo> <mo>(</mo> <msub> <mi>u</mi> <mn>1</mn> </msub> <msubsup> <mi>m</mi> <mn>33</mn> <mn>1</mn> </msubsup> <mo>-</mo> <msubsup> <mi>m</mi> <mn>13</mn> <mn>1</mn> </msubsup> <mo>)</mo> <msub> <mi>Z</mi> <mi>W</mi> </msub> <mo>=</mo> <msubsup> <mi>m</mi> <mn>14</mn> <mn>1</mn> </msubsup> <mo>-</mo> <msub> <mi>u</mi> <mn>1</mn> </msub> <msubsup> <mi>m</mi> <mn>34</mn> <mn>1</mn> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>(</mo> <msub> <mi>v</mi> <mn>1</mn> </msub> <msubsup> <mi>m</mi> <mn>31</mn> <mn>1</mn> </msubsup> <mo>-</mo> <msubsup> <mi>m</mi> <mn>21</mn> <mn>1</mn> </msubsup> <mo>)</mo> <msub> <mi>X</mi> <mi>W</mi> </msub> <mo>+</mo> <mo>(</mo> <msub> <mi>v</mi> <mn>1</mn> </msub> <msubsup> <mi>m</mi> <mn>32</mn> <mn>1</mn> </msubsup> <mo>-</mo> <msubsup> <mi>m</mi> <mn>22</mn> <mn>1</mn> </msubsup> <mo>)</mo> <msub> <mi>Y</mi> <mi>W</mi> </msub> <mo>+</mo> <mo>(</mo> <msub> <mi>v</mi> <mn>1</mn> </msub> <msubsup> <mi>m</mi> <mn>33</mn> <mn>1</mn> </msubsup> <mo>-</mo> <msubsup> <mi>m</mi> <mn>23</mn> <mn>1</mn> </msubsup> <mo>)</mo> <msub> <mi>Z</mi> <mi>W</mi> </msub> <mo>=</mo> <msubsup> <mi>m</mi> <mn>24</mn> <mn>1</mn> </msubsup> <mo>-</mo> <msub> <mi>v</mi> <mn>1</mn> </msub> <msubsup> <mi>m</mi> <mn>34</mn> <mn>1</mn> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>(</mo> <msub> <mi>u</mi> <mn>2</mn> </msub> <msubsup> <mi>m</mi> <mn>31</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>m</mi> <mn>11</mn> <mn>2</mn> </msubsup> <mo>)</mo> <msub> <mi>X</mi> <mi>W</mi> </msub> <mo>+</mo> <mo>(</mo> <msub> <mi>u</mi> <mn>2</mn> </msub> <msubsup> <mi>m</mi> <mn>32</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>m</mi> <mn>12</mn> <mn>2</mn> </msubsup> <mo>)</mo> <msub> <mi>Y</mi> <mi>W</mi> </msub> <mo>+</mo> <mo>(</mo> <msub> <mi>u</mi> <mn>2</mn> </msub> <msubsup> <mi>m</mi> <mn>33</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>m</mi> <mn>13</mn> <mn>2</mn> </msubsup> <mo>)</mo> <msub> <mi>Z</mi> <mi>W</mi> </msub> <mo>=</mo> <msubsup> <mi>m</mi> <mn>14</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msub> <mi>u</mi> <mn>2</mn> </msub> <msubsup> <mi>m</mi> <mn>34</mn> <mn>2</mn> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>(</mo> <msub> <mi>v</mi> <mn>2</mn> </msub> <msubsup> <mi>m</mi> <mn>31</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>m</mi> <mn>21</mn> <mn>2</mn> </msubsup> <mo>)</mo> <msub> <mi>X</mi> <mi>W</mi> </msub> <mo>+</mo> <mo>(</mo> <msub> <mi>v</mi> <mn>2</mn> </msub> <msubsup> <mi>m</mi> <mn>32</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>m</mi> <mn>22</mn> <mn>2</mn> </msubsup> <mo>)</mo> <msub> <mi>Y</mi> <mi>W</mi> </msub> <mo>+</mo> <mo>(</mo> <msub> <mi>v</mi> <mn>2</mn> </msub> <msubsup> <mi>m</mi> <mn>33</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>m</mi> <mn>23</mn> <mn>2</mn> </msubsup> <mo>)</mo> <msub> <mi>Z</mi> <mi>W</mi> </msub> <mo>=</mo> <msubsup> <mi>m</mi> <mn>24</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msub> <mi>v</mi> <mn>2</mn> </msub> <msubsup> <mi>m</mi> <mn>34</mn> <mn>2</mn> </msubsup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>5</mn> <mo>)</mo> </mrow> </mrow>

Formula 4 equations of (5) 3 unknown numbers, are reduced influence of noise to improve computational accuracy, are solved using least square method；

Step 8：Granular Weights Computing, and do normalized；

Granular Weights Computing is exactly to calculate each particle h_iProbability size；

Due to each feature point for calibration P_iBetween error distribution be independent, calibrating parameters are sampled the probability size calculation formula of particle It is as follows：

<mrow> <mi>p</mi> <mrow> <mo>(</mo> <mi>P</mi> <mo>|</mo> <msub> <mi>h</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <munder> <mo>&amp;Pi;</mo> <mi>i</mi> </munder> <mi>p</mi> <mrow> <mo>(</mo> <msub> <mi>P</mi> <mi>i</mi> </msub> <mo>|</mo> <msub> <mi>h</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>6</mn> <mo>)</mo> </mrow> </mrow>

From formula (6), if it is possible to obtain each characteristic point P_iInfluence to calibrating parameters sampling particle, namely obtain p (P_i| h_i), it is easy to ask for probability size of the calibrating parameters sampling particle under the influence of all characteristic points by formula (6)；If each special Levy observation a littlePredicted value isIt is simple in order to calculate, if p (P_i|h_i) Gaussian distributed, wherein average u isWithDifference, variance be observed value varianceAnd predicted valueVariance sum；

<mrow> <mtable> <mtr> <mtd> <mrow> <mi>p</mi> <mrow> <mo>(</mo> <msub> <mi>P</mi> <mi>i</mi> </msub> <mo>|</mo> <msub> <mi>h</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mi>N</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>P</mi> <mo>&amp;OverBar;</mo> </mover> <mi>i</mi> </msub> <mo>-</mo> <msup> <msub> <mover> <mi>m</mi> <mo>&amp;OverBar;</mo> </mover> <mi>i</mi> </msub> <mi>i</mi> </msup> <mo>,</mo> <msub> <mo>&amp;Sigma;</mo> <msub> <mover> <mi>P</mi> <mo>&amp;OverBar;</mo> </mover> <mi>i</mi> </msub> </msub> <mo>+</mo> <msub> <mo>&amp;Sigma;</mo> <msub> <mover> <mi>m</mi> <mo>&amp;OverBar;</mo> </mover> <mi>i</mi> </msub> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <msup> <mrow> <mo>(</mo> <mn>2</mn> <mi>&amp;pi;</mi> <mo>|</mo> <msub> <mo>&amp;Sigma;</mo> <msup> <mover> <mi>P</mi> <mo>&amp;OverBar;</mo> </mover> <mi>i</mi> </msup> </msub> <mo>+</mo> <msub> <mo>&amp;Sigma;</mo> <msup> <mover> <mi>m</mi> <mo>&amp;OverBar;</mo> </mover> <mi>i</mi> </msup> </msub> <mo>|</mo> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> </mrow> </msup> <mo>&amp;times;</mo> <mi>exp</mi> <mo>{</mo> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msup> <mrow> <mo>(</mo> <msub> <mover> <mi>P</mi> <mo>&amp;OverBar;</mo> </mover> <mi>i</mi> </msub> <mo>-</mo> <msub> <mover> <mi>m</mi> <mo>&amp;OverBar;</mo> </mover> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <msup> <mrow> <mo>(</mo> <msub> <mo>&amp;Sigma;</mo> <msub> <mover> <mi>P</mi> <mo>&amp;OverBar;</mo> </mover> <mi>i</mi> </msub> </msub> <mo>+</mo> <msub> <mo>&amp;Sigma;</mo> <msub> <mover> <mi>m</mi> <mo>&amp;OverBar;</mo> </mover> <mi>i</mi> </msub> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>(</mo> <msub> <mover> <mi>P</mi> <mo>&amp;OverBar;</mo> </mover> <mi>i</mi> </msub> <mo>-</mo> <msub> <mover> <mi>m</mi> <mo>&amp;OverBar;</mo> </mover> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>}</mo> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>7</mn> <mo>)</mo> </mrow> </mrow>

Wherein observation is obtained by gridiron pattern, and error can be ignored, and predicted value is obtained by stereoscopic vision；

Road sign error calculation is as follows：

<mrow> <mi>X</mi> <mo>=</mo> <mrow> <mo>(</mo> <mi>c</mi> <mo>-</mo> <msub> <mi>c</mi> <mn>0</mn> </msub> <mo>)</mo> </mrow> <mfrac> <mi>b</mi> <mi>d</mi> </mfrac> <mo>;</mo> <mi>Y</mi> <mo>=</mo> <mrow> <mo>(</mo> <mi>r</mi> <mo>-</mo> <msub> <mi>r</mi> <mn>0</mn> </msub> <mo>)</mo> </mrow> <mfrac> <mi>b</mi> <mi>d</mi> </mfrac> <mo>;</mo> <mi>Z</mi> <mo>=</mo> <mi>f</mi> <mfrac> <mi>b</mi> <mi>d</mi> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> </mrow>

R in formula (8), c are projection coordinate of the characteristic point in left image, r₀,c₀It is that centre coordinate the variables c, r, d of left image is recognized The gaussian random distribution for being zero for average, according to covariance propagated forward theorem,

<mrow> <msub> <mi>&amp;Sigma;</mi> <mi>X</mi> </msub> <mo>&amp;ap;</mo> <mi>J</mi> <mi>d</mi> <mi>i</mi> <mi>a</mi> <mi>g</mi> <mrow> <mo>(</mo> <msubsup> <mi>&amp;sigma;</mi> <mi>c</mi> <mn>2</mn> </msubsup> <mo>,</mo> <msubsup> <mi>&amp;sigma;</mi> <mi>r</mi> <mn>2</mn> </msubsup> <mo>,</mo> <msubsup> <mi>&amp;sigma;</mi> <mi>d</mi> <mn>2</mn> </msubsup> <mo>)</mo> </mrow> <msup> <mi>J</mi> <mi>T</mi> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow> 2

J is the Jacobian matrix of formula (9),Respectively to the covariance of dependent variable, typically take Formula (9) is calculated：

<mrow> <msub> <mo>&amp;Sigma;</mo> <mi>X</mi> </msub> <mo>=</mo> <msup> <mrow> <mo>(</mo> <mfrac> <mi>b</mi> <mi>d</mi> </mfrac> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>&amp;times;</mo> <mfenced open = "(" close = ")"> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>&amp;sigma;</mi> <mi>c</mi> <mn>2</mn> </msubsup> <mo>+</mo> <mfrac> <mrow> <msubsup> <mi>&amp;sigma;</mi> <mi>d</mi> <mn>2</mn> </msubsup> <msup> <mrow> <mo>(</mo> <mi>c</mi> <mo>-</mo> <msub> <mi>c</mi> <mn>0</mn> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> <msup> <mi>d</mi> <mn>2</mn> </msup> </mfrac> </mrow> </mtd> <mtd> <mfrac> <mrow> <msubsup> <mi>&amp;sigma;</mi> <mi>d</mi> <mn>2</mn> </msubsup> <mrow> <mo>(</mo> <mi>c</mi> <mo>-</mo> <msub> <mi>c</mi> <mn>0</mn> </msub> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <mi>r</mi> <mo>-</mo> <msub> <mi>r</mi> <mn>0</mn> </msub> <mo>)</mo> </mrow> </mrow> <msup> <mi>d</mi> <mn>2</mn> </msup> </mfrac> </mtd> <mtd> <mfrac> <mrow> <msubsup> <mi>&amp;sigma;</mi> <mi>d</mi> <mn>2</mn> </msubsup> <mrow> <mo>(</mo> <mi>c</mi> <mo>-</mo> <msub> <mi>c</mi> <mn>0</mn> </msub> <mo>)</mo> </mrow> <mi>f</mi> </mrow> <msup> <mi>d</mi> <mn>2</mn> </msup> </mfrac> </mtd> </mtr> <mtr> <mtd> <mfrac> <mrow> <msubsup> <mi>&amp;sigma;</mi> <mi>d</mi> <mn>2</mn> </msubsup> <mrow> <mo>(</mo> <mi>c</mi> <mo>-</mo> <msub> <mi>c</mi> <mn>0</mn> </msub> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <mi>r</mi> <mo>-</mo> <msub> <mi>r</mi> <mn>0</mn> </msub> <mo>)</mo> </mrow> </mrow> <msup> <mi>d</mi> <mn>2</mn> </msup> </mfrac> </mtd> <mtd> <mrow> <msubsup> <mi>&amp;sigma;</mi> <mi>r</mi> <mn>2</mn> </msubsup> <mo>+</mo> <mfrac> <mrow> <msubsup> <mi>&amp;sigma;</mi> <mi>r</mi> <mn>2</mn> </msubsup> <msup> <mrow> <mo>(</mo> <mi>r</mi> <mo>-</mo> <msub> <mi>r</mi> <mn>0</mn> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> <msup> <mi>d</mi> <mn>2</mn> </msup> </mfrac> </mrow> </mtd> <mtd> <mfrac> <mrow> <msubsup> <mi>&amp;sigma;</mi> <mi>d</mi> <mn>2</mn> </msubsup> <mrow> <mo>(</mo> <mi>r</mi> <mo>-</mo> <msub> <mi>r</mi> <mn>0</mn> </msub> <mo>)</mo> </mrow> <mi>f</mi> </mrow> <msup> <mi>d</mi> <mn>2</mn> </msup> </mfrac> </mtd> </mtr> <mtr> <mtd> <mfrac> <mrow> <msubsup> <mi>&amp;sigma;</mi> <mi>d</mi> <mn>2</mn> </msubsup> <mrow> <mo>(</mo> <mi>c</mi> <mo>-</mo> <msub> <mi>c</mi> <mn>0</mn> </msub> <mo>)</mo> </mrow> <mi>f</mi> </mrow> <msup> <mi>d</mi> <mn>2</mn> </msup> </mfrac> </mtd> <mtd> <mfrac> <mrow> <msubsup> <mi>&amp;sigma;</mi> <mi>d</mi> <mn>2</mn> </msubsup> <msup> <mrow> <mo>(</mo> <mi>r</mi> <mo>-</mo> <msub> <mi>r</mi> <mn>0</mn> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> <msup> <mi>d</mi> <mn>2</mn> </msup> </mfrac> </mtd> <mtd> <mfrac> <mrow> <msubsup> <mi>&amp;sigma;</mi> <mi>d</mi> <mn>2</mn> </msubsup> <msup> <mi>f</mi> <mn>2</mn> </msup> </mrow> <msup> <mi>d</mi> <mn>2</mn> </msup> </mfrac> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>10</mn> <mo>)</mo> </mrow> </mrow>

Normalized weight：

<mrow> <msup> <mover> <mi>&amp;omega;</mi> <mo>&amp;OverBar;</mo> </mover> <mi>i</mi> </msup> <mo>=</mo> <mfrac> <msup> <mi>&amp;omega;</mi> <mi>i</mi> </msup> <mrow> <munder> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> </munder> <msup> <mi>&amp;omega;</mi> <mi>i</mi> </msup> </mrow> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>11</mn> <mo>)</mo> </mrow> </mrow>

Step 9：Calibrating parameters fine estimation and its covariance are asked, calibrating parameters fine estimation is to multiply every group of sampling particle With normalized weight and then summation such as formula (12), the covariance such as formula (13) of its calibrating parameters fine estimation：

<mrow> <mi>P</mi> <mo>=</mo> <mfrac> <mn>1</mn> <mi>N</mi> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <mrow> <mo>(</mo> <mover> <mi>h</mi> <mo>^</mo> </mover> <mo>-</mo> <msub> <mi>h</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <mover> <mi>h</mi> <mo>^</mo> </mover> <mo>-</mo> <msub> <mi>h</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>13</mn> <mo>)</mo> </mrow> </mrow>

The particle filter of use carries out stereoscopic vision calibration optimization algorithm, preferably combines specifically should for stereoscopic vision navigation With carrying out global optimization to all parameters minimizes 3-dimensional coordinate re-projection error.