A method and system for fusion of visual positioning and visual odometer posture

Abstract

A method and system for fusing visual positioning with visual odometer pose, relying on a real-time high-frame-rate visual odometer and visual positioning pose running on a device, and eliminating poses with large positioning errors through RANSAC's Sim3 estimation. By solving the loose coupling of the transformation matrix parameters, a stable similarity transformation from visual odometer to visual positioning is obtained. The stable similarity transformation is converted into a smooth similarity transformation through linear interpolation to avoid pose jumping, and the high-frame-rate visual odometer is converted to a visual positioning coordinate system in real time through a smooth similarity transformation to obtain a pose with the same frame rate as the visual odometer. Compared with the pose obtained by direct positioning, the pose obtained by the method and system for fusing visual positioning with visual odometer pose of the present invention has higher accuracy, better stability, and higher frame rate, and has higher practical value.

Description

Technical Field

[0001] The present invention belongs to the field of computer vision, and in particular relates to a method and system for fusing visual positioning with visual odometer posture. Background Art

[0002] Vision-based spatial positioning technology is one of the key technologies in the fields of augmented reality, robotics, and unmanned driving. It is the core of technologies such as automatic recognition of the surrounding environment using images and visual positioning, such as VPS launched by Google, Cyberverse launched by Huawei, SpatialMap launched by Vision+, and Mega released by EasyAR. In AR applications, cameras are often used to shoot, and the current device's posture is calculated by identifying environmental feature points. Common ones include plane image tracking, 3D model tracking, and visual positioning (VPS).

[0003] Visual positioning uses a camera to acquire images, extract image features and match them with known features, and calculate the position and posture of the camera in a map or known coordinate system in space. Visual positioning includes image tracking, 3D model tracking, visual positioning VPS, etc. In the AR field, image tracking, 3D model tracking, and visual positioning VPS are resource-consuming computing processes, and are affected by factors such as user operation methods and environmental changes, which may lead to a series of problems such as tracking failure, tracking jitter, positioning errors, large positioning deviations, and low frame rates.

[0004] Visual odometry is a method of calculating the relative position and attitude of a camera through a continuous image sequence, including VO, VIO, SLAM, and pure inertial odometry.

[0005] Some AR devices run an odometer (VO / VIO / SLAM) locally. Since visual pose positioning is a low frame rate (for example, one frame per second) and the odometer is a high frame rate (above 30fps) pose positioning, the odometer pose is fused into the visual positioning pose coordinate system with the assistance of the odometer, thereby achieving a subjective experience of high frame rate smooth tracking and positioning. Currently, some AR SDKs have implemented the pose fusion of visual positioning (image tracking, 3D model tracking, visual positioning VPS) and odometer (VO / VIO / SLAM / pure inertial navigation odometer), using image tracking + odometer, 3D model tracking + odometer, visual positioning VPS + odometer, etc., such as Vufario, EasyAR, Huawei AREngine. Currently, the fusion of visual positioning and visual odometer pose generally adopts the following steps:

[0006] 1. Single frame positioning calculates the conversion relationship

[0007] The conversion relationship from the visual odometry pose to the visual positioning pose is calculated using the single-frame positioning results. Due to the lack of single-frame accuracy, unstable errors occur, and repeated changes in the conversion relationship cause obvious jumps in the pose, resulting in a poor subjective experience.

[0008] 2. Smoothing the results of continuous single-frame positioning

[0009] N consecutive single-frame positioning results are used to calculate N transformation relationships and then take the average or median of the transformation relationships; smoothing the results of consecutive single-frame positioning can alleviate the problem of posture jump, but it is not robust enough to erroneous positioning. When multiple erroneous positioning occurs, it is difficult to eliminate them all.

[0010] 3. Multi-frame positioning

[0011] Continuously perform visual positioning joint optimization on multiple frames to solve the coordinate transformation result; multi-frame joint positioning can alleviate the problems of posture jump and wrong positioning, but it brings pressure on the data transmission volume, calculation amount and algorithm complexity. For example, the cloud positioning request of VPS with tens of thousands times per second is a high-cost pressure.

[0012] In addition, the prior art still has the following problems:

[0013] 1. Visual recognition errors, recognition deviations, and recognition failures may occur in scenes with weak textures, similar textures, symmetrical objects, and the same specifications;

[0014] 2. In challenging angles and positions, occlusion or unfriendly operation methods, visual recognition may jump, jitter, have errors or fail, affecting the user's subjective experience;

[0015] 3. Due to the difference between the visual scale and the terminal odometer (VO / VIO / SLAM) scale, there is an error when the terminal uses the odometer to convert to the visual coordinate system, and the positioning posture is inaccurate;

[0016] 4. When there is a large change in visual positioning, the sudden change in posture causes the observed virtual object to jump, affecting the subjective experience.

[0017] In summary, due to the errors in visual positioning (image tracking, 3D model tracking, visual positioning VPS), and the differences between the coordinate system of visual positioning (image tracking, 3D model tracking, visual positioning VPS) and the local odometer, it is difficult to fuse the pose, and there will be problems such as jumping, jittering, and offset. How to achieve high-precision fusion of visual positioning (image tracking, 3D model tracking, visual positioning VPS) and odometer (VO / VIO / SLAM / pure inertial odometer) pose is an urgent problem to be solved. Summary of the invention

[0018] In order to overcome the defects of the prior art, one object of the present invention is to provide a method for fusing visual positioning with visual odometer posture, and another object of the present invention is to provide a system for fusing visual positioning with visual odometer posture. In order to achieve the above objects, the technical solutions adopted by the present invention are as follows:

[0019] One aspect of the present invention provides a method for fusing visual positioning and visual odometer posture, comprising:

[0020] Step 1: Obtain the odometer pose and visual positioning pose of the image at the current moment, and the odometer pose and the visual positioning pose form a fusion frame;

[0021] Step 2: Save a fixed number of the fused frames in the most recent time, where the fixed number is greater than three;

[0022] Step 3: solving the scale parameters and a fixed number of transformation matrices of the similarity transformation between the visual odometer pose and the visual positioning pose through the fixed number of fusion frames;

[0023] Step 4: performing loose coupling solution on the parameters of the fixed number of transformation matrices to obtain a stable similarity transformation;

[0024] Step 5: Smoothly transform the stable similarity transformation to obtain a smooth similarity transformation, and transform the visual odometer pose to the visual positioning pose coordinate system according to the smooth similarity transformation.

[0025] Step 6: Output the visual positioning pose at the same frame rate as the visual odometry.

[0026] Preferably, the visual odometer pose and the visual positioning pose both include 6Dof poses, and the fused frame includes a timestamp, a visual odometer pose and a visual positioning pose of an image.

[0027] Preferably, saving a fixed number of the fused frames in the most recent time includes:

[0028] Continuously store the current fused frame into the sliding window;

[0029] When the number of fused frames in the sliding window is greater than the fixed number, the fused frame with the earliest timestamp is deleted.

[0030] Preferably, solving the scale parameter and a fixed number of transformation matrices of the similarity transformation between the visual odometer pose and the visual positioning pose through the fixed number of fusion frames includes:

[0031] Reading a fixed number of fused frames within the sliding window at the current moment;

[0032] Aligning the visual odometry pose in the fused frame to the coordinate system of the visual positioning pose by using the Sim3 algorithm of RANSAC;

[0033] Eliminating the external points in the visual positioning pose to obtain the internal points in the visual positioning pose, wherein the internal points in the visual positioning pose and the corresponding three-dimensional points in the visual odometer pose form an internal point pose pair;

[0034] Calculate the scale parameter and a fixed number of transformation matrices of the similarity transformation from the visual odometer pose to the visual positioning pose at the current moment through the interior point pose pair;

[0035] The parameters of each transformation matrix in the fixed number of transformation matrices at the current moment include a rotation matrix and a translation matrix, and the parameters of the similarity transformation include a scale parameter, a rotation matrix and a translation matrix.

[0036] Preferably, the parameters of the transformation matrix are loosely coupled and solved to obtain a stable similarity transformation, including:

[0037] Calculating the median of the rotation matrix and the median of the translation matrix of the fixed number of transformation matrices;

[0038] Comparing all rotation matrices in the fixed number of transformation matrices with a median value of the rotation matrices, and comparing all translation matrices in the fixed number of transformation matrices with a median value of the translation matrices;

[0039] Eliminate the rotation matrices whose absolute value of the median difference between the rotation matrix and the rotation matrix is ​​greater than a threshold or whose absolute value of the median difference between the translation matrix and the translation matrix is ​​greater than a threshold, to obtain an updated transformation matrix;

[0040] The translation matrix of the similarity transformation is calculated by the pose of the three-dimensional point pair corresponding to the updated transformation matrix, the scale parameter of the similarity transformation and the median of the rotation matrix to obtain a stable similarity transformation.

[0041] Preferably, the stable similarity transformation is smoothly converted to obtain a smooth similarity transformation, comprising:

[0042] Keep the stable similarity transformation of the previous moment and the stable similarity transformation of the current moment;

[0043] The scale parameters, rotation matrix and translation matrix of the stable similarity transformation at the previous moment and the stable similarity transformation at the current moment are calculated respectively by interpolation method to obtain a smooth similarity transformation.

[0044] Another aspect of the present invention provides a system for fusion of visual positioning and visual odometer posture, the system comprising a visual positioning module, a visual odometer module, a fusion module, a sliding window module, a posture alignment module, a solution module, a coordinate system conversion module, and an output posture module, wherein:

[0045] The visual positioning module is used to obtain the visual positioning posture of the image at the current moment;

[0046] The visual odometer module is used to obtain the visual odometer pose of the image at the current moment;

[0047] The fusion module is used to combine the visual odometer pose and the visual positioning pose into a fusion frame;

[0048] The sliding window module is used to store a fixed number of the fused frames in the most recent time, and the fixed number is greater than three;

[0049] The pose alignment module is used to solve the scale parameters and a fixed number of transformation matrices of the similarity transformation between the visual odometer pose and the visual positioning pose through the fixed number of fusion frames;

[0050] The solution module is used to perform loose coupling solution on the fixed number of transformation matrices to obtain a stable similarity transformation;

[0051] The coordinate system conversion module is used to smoothly convert the stable similarity transformation to obtain a smooth similarity transformation;

[0052] The output pose module is used to convert the visual odometer pose to the visual positioning pose coordinate system according to the smooth similarity transformation.

[0053] Preferably, the visual positioning module and the visual odometry module respectively obtain the 6Dof pose of the visual positioning of the current image and the 6Dof pose of the visual odometry, the fusion module obtains the current timestamp, and the fused frame includes the timestamp, the visual odometry pose of the image, and the visual positioning pose.

[0054] Preferably, the sliding window module continuously stores the fused frame at the current moment, and when the number of fused frames in the sliding window module is greater than the fixed number, the fused frame with the earliest timestamp is deleted.

[0055] Preferably, the posture alignment module aligns the visual odometry posture in the fusion frame in the sliding window module to the coordinate system of the visual positioning posture through the Sim3 algorithm of RANSAC, and obtains the inner points in the visual positioning posture after eliminating the outer points in the visual positioning posture. The inner points in the visual positioning posture and the corresponding three-dimensional points in the visual odometry posture form an inner point posture pair, and the scale parameters and a fixed number of transformation matrices of the similarity transformation from the visual odometry posture to the visual positioning posture at the current moment are calculated through the inner point posture pair. The parameters of the similarity transformation at the current moment include the scale parameter, the rotation matrix and the translation matrix, and the parameters of the fixed number of transformation matrices include the rotation matrix and the translation matrix.

[0056] Preferably, the solution module eliminates the transformation matrices in the transformation matrix whose absolute value of the median difference between the rotation matrix and the rotation matrix is ​​greater than a threshold or whose absolute value of the median difference between the translation matrix and the translation matrix is ​​greater than a threshold to obtain an updated transformation matrix, and calculates the translation matrix of the similarity transformation by using the pose of the three-dimensional point pairs corresponding to the updated transformation matrix, the scale parameter of the similarity transformation and the median of the rotation matrix to obtain a stable similarity transformation.

[0057] Preferably, the coordinate system conversion module calculates the scale parameter, rotation matrix and translation matrix of the stable similarity transformation at the previous moment and the stable similarity transformation at the current moment respectively by interpolation method to obtain a smooth similarity transformation.

[0058] Another aspect of the present invention provides a system for fusing visual positioning and visual odometer posture, including a memory and a processor, wherein:

[0059] The memory is used to store codes and related data;

[0060] The processor is used to call the data in the memory and execute the code in the memory to implement any of the methods and steps described above.

[0061] The method and system for fusing visual positioning with visual odometer pose of the present invention rely on the real-time high frame rate visual odometer and visual positioning pose running on the device side, and eliminate the pose with large positioning error through RANSAC's sim3 estimation. By solving the loose coupling of the transformation matrix parameters, a stable similarity transformation from visual odometer to visual positioning is obtained. The stable similarity transformation is converted into a smooth similarity transformation through linear interpolation to avoid the jump of the pose, and the high frame rate visual odometer is converted to the visual positioning coordinate system in real time through a smooth similarity transformation to obtain the pose with the same frame rate as the visual odometer. Compared with the pose obtained by direct positioning, the pose obtained by the method and system for fusing visual positioning with visual odometer pose of the present invention has higher accuracy, better stability, and higher frame rate, and has higher practical value. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] After reading the specific embodiments of the present invention with reference to the accompanying drawings, readers will have a clearer understanding of various aspects of the present invention.

[0063] Figure 1 The overall flow chart of the visual positioning and visual odometer posture fusion method according to an embodiment of the present invention;

[0064] Figure 2 A system structure diagram of a visual positioning and visual odometer posture fusion system according to an embodiment of the present invention;

[0065] Figure 3 This is a system structure diagram of a visual positioning and visual odometer posture fusion system according to another embodiment of the present invention.

[0066] Reference numerals:

[0067] 11: Visual positioning module; 12: Visual odometer module; 13: Fusion module; 14: Sliding window module;

[0068] 15: pose alignment module; 16: solution module; 17: coordinate system conversion module; 18: output pose module.

[0069] 21: memory; 22: processor. DETAILED DESCRIPTION

[0070] In order to make the technical content disclosed in this application more detailed and complete, reference may be made to the accompanying drawings and the following specific embodiments of the present invention. However, it should be understood by those skilled in the art that the embodiments provided below are not intended to limit the scope of the present invention. In addition, the accompanying drawings are only used for schematic illustration and are not drawn according to their original size.

[0071] The specific implementation modes of various aspects of the present invention are further described in detail below with reference to the accompanying drawings.

[0072] Embodiment 1:

[0073] This embodiment provides a method for fusing visual positioning and visual odometer posture. Figure 1 , the method comprising:

[0074] S1: Obtain the odometer pose and visual positioning pose of the image at the current moment, and the odometer pose and the visual positioning pose form a fusion frame. In this embodiment, the visual odometer and visual positioning pose of the image at the current moment are obtained by relying on the real-time high frame rate visual odometer / odometer / pure inertial navigation running on the device side and the visual positioning. The visual odometer pose and the visual positioning pose both include 6Dof poses, and the fusion frame includes a timestamp, the visual odometer pose and the visual positioning pose of the image. Specifically, the pose P1 of the local visual odometer of N consecutive frames is obtained (P1 contains N 6Dof poses [R|t]), and the pose P2 of the visual positioning of M consecutive frames is obtained (P2 contains M 6Dof poses [R|t], and the fusion frame fusionFrame includes the timestamp of time1, the visual odometer pose P1 (6Dof pose [R|t]) and the visual positioning pose P2 (6Dof pose [R|t]) of the image.

[0075] S2: Saving a fixed number of the fused frames in the most recent time, where the fixed number is greater than three, including:

[0076] S21: continuously storing the current fused frame into the sliding window;

[0077] S22: When the number of fused frames in the sliding window is greater than the fixed number, deleting the fused frame with the earliest timestamp.

[0078] S3: solving the scale parameters and a fixed number of transformation matrices of the similarity transformation between the visual odometer pose and the visual positioning pose through the fixed number of fusion frames, including:

[0079] S31: reading a fixed number of fused frames in the sliding window at the current moment;

[0080] S32: Align the visual odometry pose P1 in the fusion frame fusionFrame to the coordinate system of the visual positioning pose P2 through the Sim3 algorithm of RANSAC, remove the external points in the visual positioning pose P2, and obtain the internal point P2i in the visual positioning pose (P2i contains X 6Dof poses [R|t]). The internal point P2i in the visual positioning pose and the corresponding three-dimensional point in the visual odometry pose constitute an internal point pose pair.

[0081] Specifically, in this embodiment, the RANSAC (Random Sample Consensus) algorithm is used to remove the outliers. The RANSAC algorithm is an iterative algorithm for correctly estimating the parameters of a mathematical model from a set of data containing "outliers". "Outliers" generally refer to noise in the data, such as mismatches in matching and outliers in the estimated curve. Therefore, RANSAC is also an "outlier" detection algorithm. Specifically, the following steps are included:

[0082] S321: Randomly sample X fusion frames fusionFrame (including P1 pose and P2 pose, that is, X pose pairs) from the sliding window;

[0083] S322: Fitting a Sim3 model to the X pose pairs;

[0084] S323: Calculate the distance from other fused frames in the sliding window to the fitted Sim3 model. If the distance is less than a certain threshold, determine that the point is an inlier, and count the number of inliers.

[0085] S324: Repeat steps S321-323 M times, and select the Sim3 model with the largest number of internal points as the best result;

[0086] S325: Re-estimate the Sim3 model using the inliers output by S324. Specifically, since the Sim3 model output by step S324 corresponds to a fused frame randomly sampled from the sliding window, including both inliers and outliers, it is necessary to re-fit the Sim3 model using the inliers output by S324 to further improve the accuracy;

[0087] S33: Calculate the similarity transformation and a fixed number of transformation matrices from the visual odometer pose to the visual positioning pose at the current moment through the interior point pose pair and the Sim3 model refitted in S325;

[0088] Specifically, the process of solving the Sim3 transformation in step S32 can be understood as solving the similarity transformation of the two coordinate systems. Specifically, the similarity transformation parameters at the current moment include the scale parameter s, the rotation matrix R and the translation matrix t; specifically, in the X fusion frames fusionFrame sampled in S321, the camera positions of the X P1 poses are p1(t10, t11, t12, ...), where t10, t11, t12, ... are three-dimensional coordinate points, and the camera positions of the X P2 poses at the same moment are p2(t20, t21, t22, ...), where t20, t21, t22, ... are three-dimensional coordinate points. X positioning data constitute X three-dimensional point pairs, where p2 = sR (p1) + t. After removing the outliers through RANSAC in S32, only more than three sets of three-dimensional point pairs are needed to solve the scale parameter s, 3*3 rotation matrix R and 3*1 translation matrix t of the similarity transformation from the visual odometer coordinate system to the visual positioning coordinate system through the Sim algorithm. This is why the sliding window saves more than three fusion frames. Among them, the X 6Dof poses in P1 and the corresponding X 6Dof poses in P2i constitute the inner point pose pairs. The Sim3 model is generated by fitting the X 6Dof poses in P1 and the corresponding X 6Dof poses in P2i, and the X 6Dof transformation matrices Tx (Tx contains X [R|t]) are calculated. The Tx is the fixed number of transformation matrices.

[0089] S4: performing loose coupling to solve the parameters of the transformation matrix to obtain a stable similarity transformation; including:

[0090] S41: Calculate the median value of the rotation matrix and the median value of the translation matrix of the transformation matrix Tx; specifically, the median value of the X rotation matrices R in Tx is Rm, and the median value of the X translation matrices t in Tx is tm;

[0091] S42: Compare all rotation matrices R in the transformation matrix Tx with the median Rm of the rotation matrix, and compare all translation matrices t in the transformation matrix Tx with the median tm of the translation matrix, and eliminate the transformation matrices whose absolute value of the difference between the rotation matrix R and the median Rm of the rotation matrix is ​​greater than a threshold or whose absolute value of the difference between the translation matrix t and the median tm of the translation matrix is ​​greater than a threshold to obtain an updated transformation matrix. Specifically, eliminate the transformation matrices with abs(R-Rm)>Ω or abs(t-tm) / tm>c in Tx to obtain an updated transformation matrix, which is Y 6Dof transformation matrices Ty (Ty contains y [R|t]), and the postures with large positioning errors are eliminated by the above method of eliminating point pairs with large differences from the median. Specifically, the median value of the rotation matrix R of the Y 6Dof transformation matrices Ty is Rm2. The visual odometer poses corresponding to the Y 6Dof transformation matrices Ty are the Y poses P1y in P1, and the corresponding visual positioning poses are the Y poses P2y in P2.

[0092] S43: Calculate the translation matrix of the similarity transformation through the pose of the three-dimensional point pair corresponding to the updated transformation matrix, the scale parameter s of the similarity transformation and the median Rm2 of the rotation matrix to obtain a stable similarity transformation.

[0093] Specifically, the camera positions of the X P1y poses are p1(t10, t11, t12, ...), t10, t11, etc. are three-dimensional coordinate points, and the camera positions of the X P2y poses at the same time are p2(t20, t21, t22, ...), t20, t21, etc. are three-dimensional coordinate points; the transformation relationship from p1 to p2 is calculated by p1 in P1y (including Y 3D coordinate points (x, y, z)) and p2 in P2y (including Y 3D coordinate points (x, y, z)), and the translation matrix t12 is calculated according to the scale parameter s of the similarity transformation calculated in step S33 and Rm2 calculated in step S42, specifically, t12 = p1-s*Rm2*t12. The transformation matrix [Rm2|t12 s] is the stable similarity transformation between the visual odometer coordinate system and the visual positioning coordinate system. Since the transformation matrix estimated by Sim3 often has an angular error, the rotation matrix of the similarity transformation does not use the calculation result of Sim3, but uses Rm2; and the translation matrix estimated by Sim3 does not have an error problem, so the translation matrix of the similarity transformation can use the result estimated by Sim3. Using the above method, the rotation matrix R, translation matrix t, and scale parameter s of the similarity transformation are solved separately, and do not rely entirely on a single Sim3 alignment, so this method is a loosely coupled solution. The transformation matrix estimated by Sim3 often has an angular error. Solving it separately uses the posture of the inner point pose pair to calculate a more accurate rotation matrix R, which has higher accuracy.

[0094] S5: Smoothly transform the stable similarity transformation to obtain a smooth similarity transformation. The purpose of this step is to smoothly transform the high-frame-rate visual odometer pose to the visual positioning coordinate system. Specifically, the stable similarity transformation Trans1 ([R1|t1 s1]) of the previous moment and the stable similarity transformation Trans2 ([R2|t2 s2]) of the current moment are retained. Specifically, a similarity transformation matrix can be solved each time using N internal points. For example, a sliding window is used to calculate once every one second. Trans1 ([R1|t1 s1]) is calculated at the previous moment, and Trans2 ([R2|t2 s2]) is calculated again after 1s using a new sliding window. There may be continuous changes between the two stable transformation matrices. In order to achieve a visual pose smoothing effect, the smooth change of the stable transformation matrix within n seconds is achieved by weighting, and s, R and t are interpolated respectively. Timestamp1 is the time when Trans1 is solved, and timestamp2 is the time when Trans2 is solved. The specific steps are as follows:

[0095] S51: Calculate the interpolation ratio ratio according to the timestamp, ratio = min(n, (timestamp1-timestamp2)) / n;

[0096] S52: Calculate the interpolated scale parameter s according to the scale parameter s1 calculated by the last Sim3 transformation and the scale parameter s2 calculated by the current Sim3 transformation according to the interpolation ratio ratio, s=s1*(1-ratio)+s2*ratio;

[0097] S53: Calculate the interpolated displacement matrix t according to the displacement matrix t1 calculated by the last Sim3 transformation and the displacement matrix t2 calculated by the current Sim3 transformation according to the interpolation ratio ratio, t=t1*(1-ratio)+t2*ratio;

[0098] S54: Calculate the interpolated rotation matrix R according to the rotation matrix R1 calculated by the last Sim3 transformation and the angle conversion matrix R2 calculated by the current Sim3 transformation according to the interpolation ratio ratio, R=R1*(1-ratio)+R2*ratio;

[0099] S55: The visual odometry pose P1 is aligned to the visual positioning pose P2 coordinate system through a smooth similarity transformation T([R|ts]) to obtain pose P3.

[0100] S6: Output the visual positioning pose P3 with the same frame rate as the visual odometry.

[0101] Embodiment 2:

[0102] This embodiment provides a system that integrates visual positioning and visual odometer posture. Figure 2 The system includes a visual positioning module 11, a visual odometer module 12, a fusion module 13, a sliding window module 14, a posture alignment module 15, a solution module 16, a coordinate system conversion module 17, and an output posture module 18, wherein:

[0103] The visual positioning module 11 is used to obtain the visual positioning posture of the image at the current moment. Specifically, the image positioning of device A obtains the posture (position and posture) P2 of the image at time 1 in the visual positioning. Including but not limited to image tracking positioning, 3D model tracking positioning, three-dimensional space positioning (VPS), automatic driving navigation positioning, mobile phone AR scanning QR code positioning and a series of methods for calculating the current device posture through the camera.

[0104] In this embodiment, the visual odometer module 12 is used to obtain the visual odometer pose of the image at the current moment. Specifically, the odometer of device A obtains the pose (position and attitude) P1 at time 1. The odometer here includes but is not limited to odometer (VIO, VISLAM), pure visual odometer (VO, SLAM), pure inertial navigation (IMU, GPS, RTK) and other methods for calculating odometer.

[0105] In this embodiment, the fusion module 13 is used to form a fusion frame with the visual odometer pose and the visual positioning pose. Specifically, the visual positioning module 11 and the visual odometer module 12 respectively obtain the 6Dof pose P1 of the visual positioning of the current image and the 6Dof pose P2 of the visual odometer, and the fusion module 13 obtains the current timestamp time1, and the fusion frame fusionFrame includes the timestamp time1, the odometer pose P1 (6Dof pose [R|t]) of the image, and the visual positioning pose P2 (6Dof pose [R|t]).

[0106] In this embodiment, the sliding window module 14 is used to save a fixed number of fused frames fusionFrame in the recent time, and the fixed number is greater than three; specifically, the sliding window module 14 continuously stores the fused frame fusionFrame at the current moment, and when the number of fused frames of the sliding window module 14 is greater than the fixed number, the fused frame with the earliest timestamp is deleted.

[0107] In this embodiment, the pose alignment module 15 is used to solve the scale parameters and a fixed number of transformation matrices of the similarity transformation between the visual odometer pose and the visual positioning pose through the fixed number of fusion frames; specifically, the pose alignment module 15 reads N fusion frames fusionFrame in the cache module of the sliding window module 14, and the odometer pose P1 and the visual positioning pose P2 in each fusion frame fusionFrame form a pose pair. N fusion frames fusionFrame provide N pose pairs. Specifically, the pose alignment module 15 aligns the visual odometer pose P1 in the fusion frame fusionFrame to the coordinate system of the visual positioning pose P2 through the Sim3 algorithm of RANSAC. The Sim3 algorithm uses at least three pairs of matching points to solve the similarity transformation, and then solves the rotation matrix R, translation matrix t and scale parameter s between the two coordinate systems. Specifically, the N fusion frames fusionFrame include N odometer poses P1, and the corresponding poses P1 include N rotation matrices and translation matrices ([R10|t10], [R11|t11], [R12|t12], ...), where R1n is the 3*3 rotation matrix of the nth odometer pose, and t1n is the 3*1 translation matrix of the nth odometer pose. The N fusion frames fusionFrame also include N visual positioning poses, and the corresponding poses P2 include N rotation matrices and translation matrices ([R20|t20], [R21|t21], [R22|t22], ...), where R2n is the 3*3 rotation matrix of the nth positioning pose, and t2n is the 3*1 translation matrix of the nth positioning pose. The camera position of the visual odometer pose P1 is p1(t10, t11, t12, ...), and the camera position of the visual positioning pose P2 is p2(t20, t21, t22, ...), and N data constitute N three-dimensional point pairs. Specifically, the process of solving the Sim3 transformation can be understood as solving the similarity transformation of two coordinate systems. Among them, p2 = sR (p1) + t, after removing the outliers through RANSAC, X fusion frames fusionFrame are obtained as internal points. Only if there are more than three images with three-dimensional point pairs can the scale parameter s, 3*3 rotation matrix R, and 3*1 translation matrix t of the coordinate system transformation matrix be solved. X fusion frames fusionFrame calculate X 6Dof transformation matrices Tx (T contains X [R|t]), and each transformation matrix [R|t] represents the transformation relationship between the visual odometer pose and the visual positioning pose.

[0108] In this embodiment, the solution module 16 is used to perform loose coupling solution on the parameters of the transformation matrix at the current moment to obtain a stable similarity transformation; specifically, the solution module removes the transformation matrix in which the absolute value of the median difference between the rotation matrix and the rotation matrix is ​​greater than a threshold or the absolute value of the median difference between the translation matrix and the translation matrix is ​​greater than a threshold to obtain an updated transformation matrix, and calculates the translation matrix of the similarity transformation through the pose of the three-dimensional point pair corresponding to the updated transformation matrix, the scale parameter of the similarity matrix and the median of the rotation matrix to obtain a stable similarity transformation. Specifically, the median of the X rotation matrices R in Tx is Rm, the median of the X displacement matrices t in Tx is tm, and the transformation matrices in Tx with abs(R-Rm)>Ω or abs(t-tm) / tm>c are removed to obtain Y 6Dof transformation matrices Ty (Ty contains y [R|t]), and the median of the rotation matrix R of the Y Dof transformation matrices Ty is Rm2. The poses corresponding to the y transformation matrices are the Y poses P1y in P1 and the Y poses P2y in P2. The transformation relationship from p1 to p2 is calculated based on p1 (including Y 3D coordinate points (x, y, z)) in P1y and p2 (including Y 3D coordinate points (x, y, z)) in P2y. The translation matrix t12 is calculated based on the scale parameter s and Rm2 of the similarity transformation, thereby obtaining a stable similarity transformation [Rm2|t12 s], that is, the transformation relationship between the visual odometer coordinate system and the visual positioning coordinate system.

[0109] In this embodiment, the coordinate system conversion module 17 is used to smoothly convert the stable similarity transformation to obtain a smooth similarity transformation, and convert the visual odometer posture to the visual positioning posture coordinate system according to the smooth similarity transformation; specifically, the coordinate system conversion module 17 calculates the scale parameters, rotation matrix and translation matrix of the stable similarity transformation of the previous moment and the stable similarity transformation of the current moment by interpolation method to obtain a smooth similarity transformation. Specifically, the coordinate system conversion module 17 retains two similarity transformations, one is the stable similarity transformation Trans1 ([R1|t1 s1]) output by the solution module 16 last time, and the latest stable similarity transformation Trans2 ([R2|t2 s2]) output by the solution module 16. There may be continuous changes between the two stable similarity transformations. In order to achieve a visual posture smoothing effect, the smooth change of the two stable similarity transformations within n seconds is achieved by weighting. Interpolate s, R and t respectively, timestamp1 is the time when Trans1 is obtained, timestamp2 is the time when Trans2 is obtained, the coordinate system conversion module 17 calculates the interpolation ratio ratio according to the timestamp, ratio=min(n,(timestamp1-timestamp2)) / n; calculates the interpolated scale parameter s according to the interpolation ratio ratio according to the scale parameter s1 calculated by the last Sim3 transformation and the scale parameter s2 calculated by the current Sim3 transformation, s=s1*(1-ratio)+s2*ratio; calculates the interpolated scale parameter s according to the interpolation ratio ratio according to the scale parameter s1 calculated by the last Sim3 transformation and the scale parameter s2 calculated by the current Sim3 transformation, s=s1*(1-ratio)+s2*ratio; calculates the interpolated scale parameter s according to the interpolation ratio ratio according to the last Sim3 transformation. The displacement matrix t1 calculated by the previous Sim3 transformation and the displacement matrix t2 calculated by the current Sim3 transformation are used to calculate the interpolated displacement matrix t according to the interpolation ratio ratio, t=t1*(1-ratio)+t2*ratio; the rotation matrix R after interpolation is calculated according to the interpolation ratio ratio, R=R1*(1-ratio)+R2*ratio according to the rotation matrix R1 calculated by the previous Sim3 transformation and the rotation matrix R2 calculated by the current Sim3 transformation; the coordinate system conversion module 17 aligns the visual odometer posture P1 to the visual positioning posture P2 coordinate system through the smooth similarity transformation T([R|ts]) to obtain the posture P3.

[0110] In this embodiment, the output pose module 18 is used to output the visual positioning pose P3 with the same frame rate as the visual odometer.

[0111] Embodiment three:

[0112] This embodiment provides another system that integrates visual positioning and visual odometer posture. Figure 3 , the system comprises a memory 21 and a processor 22, wherein:

[0113] The memory 21 is used to store codes and related data;

[0114] The processor 22 is used to call the data in the memory 21 and execute the code in the memory 21 to implement the aforementioned method and steps of visual positioning and visual odometer posture fusion. The specific methods and steps refer to the description of the aforementioned embodiment and will not be repeated here.

[0115] In this embodiment, the memory 21 may include a volatile memory, such as a random access memory (RAM), which may include a static RAM or a dynamic RAM. The memory 21 may also include a non-volatile memory, such as a read-only memory (PROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM), or a flash memory. The memory 21 may also be an external flash memory, at least one disk memory, or a cache.

[0116] In this embodiment, the processor 22 may be a central processing unit (CPU), or other general control processors, digital signal processors (DSP), application specific integrated circuits (ASIC), field programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general control processor may be a microcontroller or any conventional control processor, such as a single chip microcomputer.

[0117] The method and system for fusing visual positioning with visual odometer pose of the present invention rely on the real-time high frame rate visual odometer and visual positioning pose running on the device side, and eliminate the pose with large positioning error through RANSAC's sim3 estimation. By solving the loose coupling of the transformation matrix parameters, a stable similarity transformation from visual odometer to visual positioning is obtained. The stable similarity transformation is converted into a smooth similarity transformation through linear interpolation to avoid the jump of the pose, and the high frame rate visual odometer is converted to the visual positioning coordinate system in real time through a smooth similarity transformation to obtain the pose with the same frame rate as the visual odometer. Compared with the pose obtained by direct positioning, the pose obtained by the method and system for fusing visual positioning with visual odometer pose of the present invention has higher accuracy, better stability, and higher frame rate, and has higher practical value.

[0118] Obviously, the above embodiments of the present invention are only examples for clearly explaining the present invention, and are not intended to limit the implementation methods of the present invention. For ordinary technicians in the relevant field, other different forms of changes or modifications can be made based on the above description. The embodiments of the present invention cannot exhaust all implementation methods. All obvious changes or modifications derived from the technical solutions of the present invention are still within the scope of protection of the present invention. All documents mentioned in the present invention are cited as references in this application, just as if a document is cited as a reference alone.

Claims

1. A method for fusing visual positioning and visual odometer posture, characterized in that: The method comprises: Step 1: Obtain the visual odometer pose and visual positioning pose of the image at the current moment, and the visual odometer pose and the visual positioning pose form a fusion frame; Step 2: Save a fixed number of the fused frames in the most recent time, where the fixed number is greater than three; Step 3: solving the scale parameters and a fixed number of transformation matrices of the similarity transformation between the visual odometer pose and the visual positioning pose through the fixed number of fusion frames; Step 4: performing loose coupling solution on the parameters of the fixed number of transformation matrices to obtain a stable similarity transformation; calculating the median value of the rotation matrix and the median value of the translation matrix of the fixed number of transformation matrices; Comparing all rotation matrices in the fixed number of transformation matrices with a median value of the rotation matrices, and comparing all translation matrices in the fixed number of transformation matrices with a median value of the translation matrices; Eliminate the rotation matrices whose absolute value of the median difference between the rotation matrix and the rotation matrix is ​​greater than a threshold or whose absolute value of the median difference between the translation matrix and the translation matrix is ​​greater than a threshold, to obtain an updated transformation matrix; Calculating the translation matrix of the similarity transformation by the pose of the three-dimensional point pair corresponding to the updated transformation matrix, the scale parameter of the similarity transformation and the median value of the rotation matrix to obtain a stable similarity transformation; Step 5: Smoothly transforming the stable similarity transformation to obtain a smooth similarity transformation, and transforming the visual odometer pose to the visual positioning pose coordinate system according to the smooth similarity transformation; the smoothing transforming the stable similarity transformation to obtain a smooth similarity transformation includes: Keep the stable similarity transformation of the previous moment and the stable similarity transformation of the current moment; The scale parameter, the rotation matrix and the translation matrix of the stable similarity transformation at the previous moment and the stable similarity transformation at the current moment are calculated respectively by an interpolation method to obtain a smooth similarity transformation; Step 6: Output the visual positioning pose at the same frame rate as the visual odometry.

2. The method for fusion of visual positioning and visual odometer posture according to claim 1, characterized in that: The visual odometer pose and the visual positioning pose both include 6Dof poses, and the fused frame includes a timestamp, a visual odometer pose, and a visual positioning pose of an image.

3. The method for fusion of visual positioning and visual odometer posture according to claim 2, characterized in that: Saving a fixed number of the fused frames in the most recent time, including: Continuously store the current fused frame into the sliding window; When the number of fused frames in the sliding window is greater than the fixed number, the fused frame with the earliest timestamp is deleted.

4. The method for fusing visual positioning and visual odometer posture according to claim 1, characterized in that: Solving the scale parameters and a fixed number of transformation matrices of the similarity transformation between the visual odometer pose and the visual positioning pose through the fixed number of fusion frames includes: Read a fixed number of fused frames in the sliding window at the current moment; Aligning the visual odometry pose in the fused frame to the coordinate system of the visual positioning pose by using the Sim3 algorithm of RANSAC; Eliminating the external points in the visual positioning pose to obtain the internal points in the visual positioning pose, wherein the internal points in the visual positioning pose and the corresponding three-dimensional points in the visual odometer pose form an internal point pose pair; Calculate the scale parameter and a fixed number of transformation matrices of the similarity transformation from the visual odometer pose to the visual positioning pose at the current moment through the interior point pose pair; The parameters of each transformation matrix in the fixed number of transformation matrices at the current moment include a rotation matrix and a translation matrix, and the parameters of the similarity transformation include a scale parameter, a rotation matrix and a translation matrix.

5. A system for fusion of visual positioning and visual odometer posture, characterized in that: The system includes a visual positioning module, a visual odometer module, a fusion module, a sliding window module, a posture alignment module, a solution module, a coordinate system conversion module, and an output posture module, wherein: The visual positioning module is used to obtain the visual positioning posture of the image at the current moment; The visual odometer module is used to obtain the visual odometer pose of the image at the current moment; The fusion module is used to combine the visual odometer pose and the visual positioning pose into a fusion frame; The sliding window module is used to store a fixed number of the fused frames in the most recent time, and the fixed number is greater than three; The pose alignment module is used to solve the scale parameters and a fixed number of transformation matrices of the similarity transformation between the visual odometer pose and the visual positioning pose through the fixed number of fusion frames; The solution module is used to perform loose coupling solution on the fixed number of transformation matrices to obtain a stable similarity transformation; the solution module eliminates the transformation matrices in which the absolute value of the median difference between the rotation matrix and the rotation matrix is ​​greater than a threshold or the absolute value of the median difference between the translation matrix and the translation matrix is ​​greater than a threshold to obtain an updated transformation matrix, and calculates the translation matrix of the similarity transformation through the pose of the three-dimensional point pair corresponding to the updated transformation matrix, the scale parameter of the similarity transformation and the median of the rotation matrix to obtain a stable similarity transformation; The coordinate system conversion module is used to smoothly convert the stable similarity transformation to obtain a smooth similarity transformation; the coordinate system conversion module retains the stable similarity transformation of the previous moment and the stable similarity transformation of the current moment; the scale parameters, rotation matrix and translation matrix of the stable similarity transformation of the previous moment and the stable similarity transformation of the current moment are calculated by interpolation method to obtain a smooth similarity transformation; The output pose module is used to convert the visual odometer pose to the visual positioning pose coordinate system according to the smooth similarity transformation.

6. The system for fusion of visual positioning and visual odometer posture according to claim 5, characterized in that: The visual positioning module and the visual odometer module respectively obtain the 6Dof pose of the visual positioning of the current image and the 6Dof pose of the visual odometer, the fusion module obtains the current timestamp, and the fusion frame includes the timestamp, the visual odometer pose of the image, and the visual positioning pose.

7. The system for fusion of visual positioning and visual odometer posture according to claim 6, characterized in that: The sliding window module continuously stores the fused frame at the current moment, and when the number of fused frames in the sliding window module is greater than the fixed number, the fused frame with the earliest timestamp is deleted.

8. The system for fusion of visual positioning and visual odometer posture according to claim 7, characterized in that: The pose alignment module aligns the visual odometer pose in the fused frame in the sliding window module to the coordinate system of the visual positioning pose through the Sim3 algorithm of RANSAC, and obtains the inner points in the visual positioning pose after eliminating the outer points in the visual positioning pose. The inner points in the visual positioning pose and the corresponding three-dimensional points in the visual odometer pose form an inner point pose pair. The scale parameters and a fixed number of transformation matrices of the similarity transformation from the visual odometer pose to the visual positioning pose at the current moment are calculated through the inner point pose pair. The parameters of the similarity transformation at the current moment include the scale parameter, the rotation matrix and the translation matrix, and the parameters of the fixed number of transformation matrices include the rotation matrix and the translation matrix.

9. The system for fusion of visual positioning and visual odometer posture according to claim 8, characterized in that: The coordinate system conversion module calculates the scale parameters, rotation matrix and translation matrix of the stable similarity transformation at the previous moment and the stable similarity transformation at the current moment respectively by interpolation method to obtain a smooth similarity transformation.

10. A system for fusion of visual positioning and visual odometer posture, characterized in that: The system comprises a memory and a processor, wherein: The memory is used to store codes and related data; The processor is used to call the data in the memory and execute the code in the memory to implement the method and steps according to any one of claims 1 to 4.

Images