CN105953796A - Stable motion tracking method and stable motion tracking device based on integration of simple camera and IMU (inertial measurement unit) of smart cellphone - Google Patents

Abstract

The invention discloses a stable motion tracking method and a stable motion tracking device based on integration of a simple camera and an IMU (inertial measurement unit) of a smart cellphone, and belongs to the technical field of AR (augmented reality)/VR (virtual reality) motion tracking. The method includes processing an acquired image according to an ORB (object request broker) algorithm, performing 3D (three-dimensional) reconstruction to obtain initial map points, and completing map initialization; performing visual tracking through ORB algorithm real-time matching and parallel partial keyframe mapping to obtain a visual pose; acquiring accelerated velocity and angular velocity, both generated in a three-dimensional space, of the IMU, and performing integral operation on the accelerated velocity and the angular velocity to obtain an IMU pose prediction result; performing Kalman fusion on the visual pose and the IMU pose prediction result, and performing motion tracking according to pose information acquired after fusion. Compared with the prior art, the stable motion tracking method and the stable motion tracking device have the advantages that a stable motion tracking mode can be acquired and real-time online dimension estimation can be achieved.

Description

Stable motion tracking that smart mobile phone monocular and IMU merge and device

Technical field

The present invention relates to moving communicating field, particularly relate to a kind of smart mobile phone monocular and IMU merges Stable motion tracking and device.

Background technology

Along with the development of VR technology, utilizing advanced motion tracking technology is its prerequisite technology applied One of condition, can realize preferably mutual and more preferably feeling of immersion in this technical foundation.Work as reach Dynamic VR mainly uses handle to interact, and simply uses the gyroscope of mobile phone in interaction Carry out rotating and follow the tracks of, due to deviation and the impact of noise of mobile phone gyroscope itself, cause rotation to be estimated Meter is inaccurate, and repeatable accuracy is poor；When user be seated stand up move forward time, if not using handle If Jiao Hu, virtual scene remains stationary, and as not occurring whatever, interactive experience is bad； And be immersed in virtual environment when user is seated, subconsciousness is stood up and is attempted moving, and virtual scene does not has Any change occurs, and feeling of immersion disappears.

Motion tracking technology is intended to measure, follows the tracks of, records object movement locus in three dimensions, It mainly obtains the information of moving scene by sensor technology, and is calculated tracked thing in real time Body, in the attitude in space, is widely used in robot navigation, Navigation of Pilotless Aircraft and unmanned vehicle and automatically drives Sail the fields such as navigation.Within 2004, Nister proposes visual odometry (Visual Odometry, VO) first Concept since, the method for view-based access control model speedometer has become the master of real-time Attitude estimation and motion tracking Stream.It, by estimating the camera incremental motion in space, determines the movement locus of camera at time and space. And vision inertia speedometer (Visual IMU Odometry, VIO) has merged camera and inertia sensing The information of device, mainly gyroscope and accelerometer, give a scheme having complementary advantages.Such as, One single camera can estimate relative position, but it is not provided that absolute measure, it is impossible to obtains thing Actual range between body size or two objects, and photographic head sampling frame per second is the most relatively low and image passes The noise of sensor is relatively large so that it is poor to the adaptability of environment during motion tracking. Inertial sensor can provide absolute measure, and measures with higher sample frequency, thus improve and set For robustness when moving quick.But, the low cost inertial sensor carried, compare based on camera Location estimation bigger drift easily occurs, it is impossible to realize stable motion tracking.

Summary of the invention

The technical problem to be solved in the present invention is to provide and a kind of can obtain more stable motion tracking Mode, and realize smart mobile phone monocular and the stable fortune of IMU fusion of the real-time online estimation of yardstick Motion tracking method and apparatus.

For solving above-mentioned technical problem, the present invention provides technical scheme as follows:

The stable motion tracking that a kind of smart mobile phone monocular and IMU merge, including:

Utilize ORB algorithm that the image obtained is processed, carry out 3D reconstruct afterwards, obtain initial Point map, completes map initialization；

The mode using ORB algorithm real-time matching and parallel local key frame to build figure carry out vision with Track, obtains vision pose；

Obtain acceleration and magnitude of angular velocity that IMU produces at three dimensions, and fast to acceleration and angle Angle value is integrated computing, obtains IMU pose and predicts the outcome；

Vision pose and IMU pose are predicted the outcome and carries out Kalman's fusion, obtain according to after merging Posture information carry out motion tracking.

Further, the image obtained is processed by the described ORB of utilization algorithm, carries out 3D afterwards Reconstruct, obtains initial point map, completes map initialization and include:

The first two field picture obtained use ORB algorithm extract characteristic point and calculate description, by the One frame is designated as key frame, the absolute pose of labelling camera；

After camera translates one end distance, then ORB algorithm is used to extract characteristic point to the image obtained And calculate description, mate with the first frame image features point, the second frame is designated as key frame, and Calculate under the second frame camera relative to the relative pose of the first frame；

The feature point set that the match is successful is carried out 3D reconstruct, obtains initial point map.

Further, calculate camera under the second frame described in include relative to the relative pose of the first frame:

According to the Corresponding matching feature point set on the first frame and the second two field picture, calculate between two two field pictures Basis matrix；

According to basis matrix and the intrinsic parameter of camera, it is calculated essential matrix；

Essential matrix is used singular value decomposition, obtains under described second frame camera relative to the first frame Relative pose.

Further, described use ORB algorithm real-time matching and parallel local key frame build the side of figure Formula carries out visual tracking, obtains vision pose and includes:

ORB algorithm rasterizing is used to extract image characteristic point and calculate description the present frame of image Son；

Use constant speed movement model, estimate present frame correspondence camera pose, by previous frame image institute There is point map to project in current image frame, carry out Feature Points Matching, and by the previous frame that the match is successful Point map is assigned to the corresponding characteristic point of present frame；

LM algorithm and Huber is used to estimate to update present frame pose and present frame point map；

According to the pose after updating, all for local key frame point maps are projected in current image frame, And carry out Feature Points Matching, after the match is successful, all point maps that the match is successful are assigned to present frame Corresponding characteristic point, and renewal present frame pose is with current again to use LM algorithm and Huber to estimate Frame point map.

Further, described use ORB algorithm real-time matching and parallel local key frame build the side of figure Formula carries out visual tracking, obtains vision pose and also includes:

Judge whether to need to increase key according to time interval situation and/or present frame point map number Frame, if increasing after key frame exceedes certain time from last time or the map of present frame is counted less than threshold Value, then increase new key frame；

Judge whether present frame is new key frame, if it is, increase new point map, by new Key frame carries out characteristic point without all characteristic points of point map with all characteristic points in the key frame of local Coupling, after the match is successful, 3D reconstruct obtains new point map；

Locally bundle adjustment optimization, revises cumulative error, the pose after being optimized and point map.

Device is followed the tracks of in the stable motion that a kind of smart mobile phone monocular and IMU merge, including:

Map initialization module, for utilizing ORB algorithm to process the image obtained, afterwards Carry out 3D reconstruct, obtain initial point map, complete map initialization；

Visual tracking module, is used for using ORB algorithm real-time matching and parallel local key frame to build figure Mode carry out visual tracking, obtain vision pose；

IMU pose computing module: fast for obtaining acceleration that IMU produces at three dimensions and angle Angle value, and acceleration and magnitude of angular velocity are integrated computing, obtain IMU pose and predict the outcome；

Fusion Module: carry out Kalman's fusion for vision pose and IMU pose are predicted the outcome, Motion tracking is carried out according to the posture information obtained after merging.

Further, described map initialization module is additionally operable to:

The first two field picture obtained use ORB algorithm extract characteristic point and calculate description, by the One frame is designated as key frame, the absolute pose of labelling camera；

After camera translates one end distance, then ORB algorithm is used to extract characteristic point to the image obtained And calculate description, mate with the first frame image features point, the second frame is designated as key frame, and Calculate under the second frame camera relative to the relative pose of the first frame；

The feature point set that the match is successful is carried out 3D reconstruct, obtains initial point map.

Further, calculate camera under the second frame described in include relative to the relative pose of the first frame:

According to the Corresponding matching feature point set on the first frame and the second two field picture, calculate between two two field pictures Basis matrix；

According to basis matrix and the intrinsic parameter of camera, it is calculated essential matrix；

Essential matrix is used singular value decomposition, obtains under described second frame camera relative to the first frame Relative pose.

Further, described visual tracking module is additionally operable to:

ORB algorithm rasterizing is used to extract image characteristic point and calculate description the present frame of image Son；

Use constant speed movement model, estimate present frame correspondence camera pose, by previous frame image institute There is point map to project in current image frame, carry out Feature Points Matching, and by the previous frame that the match is successful Point map is assigned to the corresponding characteristic point of present frame；

LM algorithm and Huber is used to estimate to update present frame pose and present frame point map；

According to the pose after updating, all for local key frame point maps are projected in current image frame, And carry out Feature Points Matching, after the match is successful, all point maps that the match is successful are assigned to present frame Corresponding characteristic point, and renewal present frame pose is with current again to use LM algorithm and Huber to estimate Frame point map.

Further, described visual tracking module is additionally operable to:

Judge whether to need to increase key according to time interval situation and/or present frame point map number Frame, if increasing after key frame exceedes certain time from last time or the map of present frame is counted less than threshold Value, then increase new key frame；

Judge whether present frame is new key frame, if it is, increase new point map, by new Key frame carries out characteristic point without all characteristic points of point map with all characteristic points in the key frame of local Coupling, after the match is successful, 3D reconstruct obtains new point map；

Locally bundle adjustment optimization, revises cumulative error, the pose after being optimized and point map.

The method have the advantages that

In the present invention, first to map initialization, obtain image after initializing successfully and follow the tracks of continuously also Carry out pose estimation；Meanwhile, obtain IMU data to be integrated predicting pose；At spreading kalman Carry out data fusion under filtering (Extended Kalman Filter, EKF) framework and obtain stable position Appearance is estimated.For the motion tracking problem of current mobile VR, the camera that the present invention carries with mobile terminal And IMU, by using VIO to combine vision measurement and inertial sensor measurement under EKF framework, Pose and absolute measure can be estimated exactly.Realize the quick and stable motion tracking of mobile VR Method.Compared with prior art, the present invention has can obtain more stable motion tracking mode, And the feature that the real-time online realizing yardstick is estimated.

Accompanying drawing explanation

The stream of the stable motion tracking that Fig. 1 is the smart mobile phone monocular of the present invention and IMU merges Journey schematic diagram；

Regarding of the stable motion tracking that Fig. 2 is the smart mobile phone monocular of the present invention and IMU merges Feel that pose estimates schematic flow sheet；

Regarding of the stable motion tracking that Fig. 3 is the smart mobile phone monocular of the present invention and IMU merges Feel that pose and IMU pose Kalman merge principle schematic；

The seat of the stable motion tracking that Fig. 4 is the smart mobile phone monocular of the present invention and IMU merges Mark system schematic diagram；

The list of the stable motion tracking that Fig. 5 is the smart mobile phone monocular of the present invention and IMU merges Visually feel and IMU system schematic；

The skill of the stable motion tracking that Fig. 6 is the smart mobile phone monocular of the present invention and IMU merges Art scheme overall block flow diagram；

Fig. 7 is the knot that device is followed the tracks of in the smart mobile phone monocular of the present invention and the stable motion of IMU fusion Structure schematic diagram.

Detailed description of the invention

For making the technical problem to be solved in the present invention, technical scheme and advantage clearer, below will knot Conjunction drawings and the specific embodiments are described in detail.

On the one hand, the stable motion that the present invention provides a kind of smart mobile phone monocular and IMU to merge is followed the tracks of Method, as it is shown in figure 1, include:

Step S101: utilize ORB algorithm to process the image obtained, carries out 3D weight afterwards Structure, obtains initial point map, completes map initialization；

In this step, the purpose carrying out map initialization is to build initial three-dimensional point cloud.Due to can not Only obtain depth information from single frame, it is therefore desirable to from image sequence, choose more than two frames or two frames Image, estimate that camera attitude also reconstructs initial three-dimensional point cloud.In this step, use two passes Key frame, one is initial key frame (initial frame), and another is the key after motion certain angle Frame (end frame), to carrying out the coupling of key point, then to mating between initial frame to end frame The feature point set of merit carries out 3D reconstruct, finally completes map initialization.

Step S102: the mode using ORB algorithm real-time matching and parallel local key frame to build figure is entered Row visual tracking, obtains vision pose；

In this step, after map initialization success, view-based access control model carries out motion tracking.In view of mobile terminal More weak computing capability, uses the real-time matching of ORB algorithm and pose to estimate, and parallel local The maintenance of key frame and build the mode of figure and carry out visual tracking, and then obtain vision pose.Wherein, ORB Algorithm real-time matching and pose are estimated as track thread, locally key frame maintenance and build figure be local close Joint main feed line journey.

Step S103: obtain acceleration and magnitude of angular velocity that IMU produces at three dimensions, and to adding Speed and magnitude of angular velocity are integrated computing, obtain IMU pose and predict the outcome；

In this step, (Inertial measurement unit is called for short IMU, inertia to the IMU related to Measuring unit) it is Measuring Object three-axis attitude angular velocity (or angular speed) and the device of acceleration.Typically , an IMU contains accelerometer and the gyro of three single shafts, the accelerometer of three single shafts Found the acceleration signal of three axles in carrier coordinate system unification and independence for detecting object, gyro is used for detecting carrier Relative to the angular velocity signal of navigational coordinate system, in this step, IMU can produce 3 vertical axis Raw acceleration and magnitude of angular velocity, be integrated predicting pose, and the monocular vision in mobile device pass Sensor can provide 3D position and the measured value of pose not having yardstick.Between front and back's consecutive frame Obtaining IMU data and carry out pose prediction, a later frame vision pose is estimated to be updated as measured value.

Step S104: vision pose and IMU pose are predicted the outcome and carries out Kalman's fusion, foundation The posture information obtained after fusion carries out motion tracking；

In this step, for obtaining stable tracking pose, make full use of the sensor of vision and IMU The information obtained, by the present invention in that and use Kalman's fusion method, the vision position obtained by visual pattern The pose that appearance and IMU integration obtain predicts the outcome and merges, to realize the letter of two Dissimilar sensors Breath complementation and Target state estimator, thus pose the most accurate after obtaining fusion, reliable.And then, Motion tracking is carried out according to the posture information after merging.

In the present invention, first to map initialization, obtain image after initializing successfully and follow the tracks of continuously also Carry out pose estimation；Meanwhile, obtain IMU data to be integrated predicting pose；At spreading kalman Carry out data fusion under filtering (Extended Kalman Filter, EKF) framework and obtain stable position Appearance is estimated.For the motion tracking problem of current mobile VR, the camera that the present invention carries with mobile terminal And IMU, by using VIO to combine vision measurement and inertial sensor measurement under EKF framework, Pose and absolute measure can be estimated exactly.Realize the quick and stable motion tracking of mobile VR Method.Compared with prior art, the present invention has can obtain more stable motion tracking mode, And the feature that the real-time online realizing yardstick is estimated.

As a modification of the present invention, utilize ORB algorithm that the image obtained is processed, it After carry out 3D reconstruct, obtain initial point map, complete map initialization and include:

The first two field picture obtained use ORB algorithm extract characteristic point and calculate description, by the One frame is designated as key frame, the absolute pose of labelling camera；

After camera translates one end distance, then ORB algorithm is used to extract characteristic point to the image obtained And calculate description, mate with the first frame image features point, the second frame is designated as key frame, and Calculate under the second frame camera relative to the relative pose of the first frame；

The feature point set that the match is successful is carried out 3D reconstruct, obtains initial point map.

For this improvement of the present invention, the present invention provides a kind of complete specific embodiment as follows:

1. gather the first two field picture and use characteristics algorithm (the Oriented FAST with local invariant And Rotated BRIEF, ORB) extract characteristic point and calculate description, the first frame is key frame, The absolute pose of labelling camera is [R_(0,k)|t_(0,k)], subscript (0, k) represent the absolute pose of kth frame, then [R_(0,0)|t_(0,0)]=[I | 0]；

2. after translation one segment distance, then gather image and use ORB algorithm to extract characteristic point calculating to retouch State son.After the first frame image features Point matching success, this frame is also denoted as key frame.And calculate Under two frames, camera is [R relative to the relative pose of the first frame_(0,1)|t_(0,1)]=[R | t]；

3. the match is successful, and feature point set carries out 3D reconstruct, obtains initial point map.

In the present embodiment, ORB algorithm is used to extract feature directly coupling estimation pose, ORB calculation Method is that FAST Corner Detection is combined a kind of algorithm improvement with BRIEF feature description, has taken into account monocular Efficiency during visual tracking and precision.

As a further improvement on the present invention, relative relative to the first frame of camera under the second frame is calculated Pose includes:

According to the Corresponding matching feature point set on the first frame and the second two field picture, calculate between two two field pictures Basis matrix；

According to basis matrix and the intrinsic parameter of camera, it is calculated essential matrix；

Essential matrix is used singular value decomposition, obtains relative relative to the first frame of camera under the second frame Pose.

For further improvement of the present invention, the complete specific embodiment that the present invention provides is as follows:

1., after translating a segment distance, ORB algorithm is used to extract characteristic point and calculate the second two field picture Describe son, after the first frame image features Point matching success, obtain the Corresponding matching on two key frames special Levy point set and be designated as (X_L,X_R)；

2. according to X_L ^TFX_R=0, calculate basis matrix F；

3. by basis matrix F and the mutual relation of essential matrix E: E=K_L ^TFK_R, wherein (K_L,K_R) The intrinsic parameter of camera respectively, this intrinsic parameter can be demarcated and K in advance_L=K_R.Obtain essential matrix E, this Stromal matrix is the most relevant with the outer ginseng of camera, and unrelated with camera internal reference；

4. according to E=[t]_×R, wherein [t]_×For translational movement t=(t_x,t_y,t_z)^TAntisymmetric matrix, R for rotation Torque battle array.Matrix E is utilized singular value decomposition (Singular Value Decomposition, SVD), Can calculate R, t, then under the second frame, camera relative to the relative pose of the first frame is [R_(0,1)|t_(0,1)]=[R | t].

The present embodiment is at camera in motor process, and can obtain corresponding to every frame picture one successively is Row relative pose.

As the further improvement of the present invention, use ORB algorithm real-time matching and parallel local Key frame is built the mode of figure and is carried out visual tracking, obtains vision pose and includes:

ORB algorithm rasterizing is used to extract image characteristic point and calculate description the present frame of image Son；

Use constant speed movement model, estimate present frame correspondence camera pose, by previous frame image institute There is point map to project in current image frame, carry out Feature Points Matching, and by the previous frame that the match is successful Point map is assigned to the corresponding characteristic point of present frame；

LM algorithm and Huber is used to estimate to update present frame pose and present frame point map；

According to the pose after updating, all for local key frame point maps are projected in current image frame, And carry out Feature Points Matching, after the match is successful, all point maps that the match is successful are assigned to present frame Corresponding characteristic point, and renewal present frame pose is with current again to use LM algorithm and Huber to estimate Frame point map.

For the further improvement of the invention described above, as in figure 2 it is shown, for the present frame (of image I_kTwo field picture), the specific embodiment of tracking step is as follows:

(1) ORB algorithm rasterizing is used (piece image to be divided into the grid that a series of size is identical Lattice) extracted region images characteristic point and calculate describe son, rasterizing extract can ensure that the spy on image Levy an extraction to be evenly distributed, improve stability and the precision of follow-up tracking；

(2) use constant speed movement model, estimate present frame correspondence camera pose.By previous frame Image I_k-1All point maps project in current image frame.Feature Points Matching, and upper by what the match is successful One frame point map is assigned to the corresponding characteristic point of present frame；

(3) LM (Levenberg-Marquardt) algorithm and Huber is used to estimate to update present frame Pose and present frame point map；

(4) according to the pose after updating, by the local all point maps of key frame, (this point map does not wraps Point map containing in (2)) project in current image frame, and carry out Feature Points Matching.Mate into After merit, all point maps that the match is successful are assigned to the corresponding characteristic point of present frame.And use LM Algorithm and Huber estimate again to update present frame pose [R_(0,k)|t_(0,k)] and present frame point map.

In the present embodiment, ORB algorithm real-time matching and parallel local key frame is used to build the mode of figure Carry out visual tracking, and then obtain vision pose.Wherein, ORB algorithm real-time matching and pose are estimated It is calculated as track thread, locally the maintenance of key frame and to build figure be local joint main feed line journey.In the present embodiment, Track thread processes with local key frame thread parallel, and efficiency high energy realizes real-time tracking.

As a modification of the present invention, use ORB algorithm real-time matching and parallel local key frame The mode building figure carries out visual tracking, and obtaining vision pose can also include:

Judge whether to need to increase key according to time interval situation and/or present frame point map number Frame, if increasing after key frame exceedes certain time from last time or the map of present frame is counted less than threshold Value, then increase new key frame；

Judge whether present frame is new key frame, if it is, increase new point map, by new Key frame carries out characteristic point without all characteristic points of point map with all characteristic points in the key frame of local Coupling, after the match is successful, 3D reconstruct obtains new point map；

Locally bundle adjustment optimization, revises cumulative error, the pose after being optimized and point map.

For this improvement, the complete specific embodiment that the present invention provides is as follows:

1) increase new key frame, judge whether to need from time dimension and present frame point map number Key frame to be strengthened.Exceed after certain time then or the map of present frame when increasing key frame from last time New key frame is increased when counting less than threshold value；

2) if present frame is new key frame, new point map is increased.By new key frame without ground All characteristic points of figure point carry out Feature Points Matching with all characteristic points in the key frame of local, mate into After merit, 3D reconstruct obtains new point map；

3) in order to ensure efficiency and the seriality of tracking followed the tracks of, the quantity of local key frame is controlled, When key frame quantity is more than threshold value, delete the key frame added the earliest in the key frame of local；

4) locally bundle adjustment (Bundle Adjustment) optimizes, and revises cumulative error.Obtain excellent Pose after change and point map.

In the present embodiment, step 1) to 4) local key frame thread (locally key frame can be placed on Thread is (4) in above-described embodiment) in parallel processing, improve efficiency.Repeat in above-described embodiment (1) to (4), and 1) to 4) be capable of following the tracks of continuously.

In the present embodiment, i.e. can guarantee that the seriality of tracking can reduce again and need crucial frame number to be processed Amount, decreases the process time, improves the efficiency of motion tracking.

In the present invention, vision pose and IMU pose carry out Kalman's fusion process can use ability Accomplished in many ways known to field technique personnel, it is preferred that be referred to below embodiment and carry out:

Vision and IMU merge schematic flow sheet, as it is shown on figure 3, describe for convenience, under definition Mark w, i, v, c represent world coordinate system, IMU coordinate, visual coordinate system and camera coordinates system respectively.Sit The definition of mark system, as shown in Figure 4；

Step 1: assume that inertia measurement includes specific deviation b and white Gaussian noise n is then actual Angular velocity omega and actual acceleration a as follows:

ω=ω_m-b_ω-n_ωA=a_m-b_a-n_a

Wherein subscript m represents measured value, and dynamic deviation b can be represented as a stochastic process:

${\stackrel{\·}{b}}_{\mathrm{\ω}}=n_{b\mathrm{\ω}}{\stackrel{\·}{b}}_a=n_{ba}$

The state of wave filter includes IMU position in world coordinate systemAnd world coordinate system The speed of IMU coordinate system relativelyWith attitude four elementMeanwhile, also gyroscope and acceleration Deviation b of meter_ω, b_aAnd Ocular measure factor lambda.And demarcate the rotation between the IMU of gained and camera Transfer the registration of Party membership, etc. from one unit to anotherTranslation relationIt is hereby achieved that one comprises 24 elementary state vector X, As shown in accompanying drawing 5 prediction module.

$X=\left\{\begin{array}{cccccccc}{p_w^i}^T& {v_w^i}^T& {q_w^i}^T& {b_{\mathrm{\ω}}}^T& {b_a}^T& \mathrm{\λ}& p_i^c& q_i^c\end{array}\right\}$

Step 2: in above-mentioned state is expressed and described, we use four elements to be described attitude. In this case, we use four element errors to represent error and its covariance, so can increase Numerical stability and minimum express.So, we define the error condition vector of 22 elements.

$\tilde{x}=\left\{\begin{array}{cccccccc}{{\mathrm{\Δp}}_w^i}^T& {{\mathrm{\Δv}}_w^i}^T& {{\mathrm{\δ\θ}}_w^i}^T& {{\mathrm{\Δb}}_{\mathrm{\ω}}}^T& {{\mathrm{\Δb}}_a}^T& \mathrm{\Δ}\mathrm{\λ}& {{\mathrm{\Δp}}_i^c}^T& {{\mathrm{\δ\θ}}_i^c}^T\end{array}\right\}$

In view of estimated valueWith its true value x, such asIn addition to four element errors, we All state variables are used the method, and wherein four element errors are defined as:

${\mathrm{\δq}}_w^i=q_w^i\⊗{{\hat{q}}_w^i}^{-1}\≈{\left[\begin{array}{cc}\frac{1}{2}{{\mathrm{\δ\θ}}_w^i}^T& 1\end{array}\right]}^T,{\mathrm{\δq}}_i^c=q_i^c\⊗{{\hat{q}}_i^c}^{-1}\≈{\left[\begin{array}{cc}\frac{1}{2}{{\mathrm{\δ\θ}}_i^c}^T& 1\end{array}\right]}^T$

Thus, it is possible to obtain the lienarized equation of continuous error condition:

$\stackrel{\·}{\tilde{x}}=F_c\tilde{x}+G_{c}n$

Wherein,It it is noise vector.In current solution, we are to calculation The speed of method is especially of interest, to this end, within the time of integration of two adjacent states, it will be assumed that F_cAnd G_c It it is steady state value.In order to it is carried out discretization expression:

$F_d=\exp \left(F_c\mathrm{\Δ}t\right)=I_d+F_c\mathrm{\Δ}t+\frac{1}{2}{F}_c^2{\mathrm{\Δt}}^2+...$

Meanwhile, the covariance matrix Q of discrete time can be obtained by integration_d:

$Q_d={\∫}_{\mathrm{\Δ}t}{F}_d\left(\mathrm{\τ}\right)G_c{Q}_c{G}_C^T{F}_d{\left(\mathrm{\τ}\right)}^{T}d\mathrm{\τ}$

By calculating gained F_dAnd Q_d, according to Kalman filtering, it is calculated state covariance matrix:

P_k+1|k=F_dP_k|kF_d ^T+Q_d

Step 3: for the position measurement of cameraWe obtain according to the visual tracking of one camera Pose [R_(0,k)|t_(0,k)], (With) be camera pose position vector and rotate quaternion representation.Enter And obtain the measurement position of its correspondence.Thus obtain following measurement model:

$z_p=p_v^c=C_{\left(q_v^w\right)}^T(p_w^i+C_{\left(q_w^i\right)}^T{p}_i^c)\mathrm{\λ}+n_p$

Wherein,It is IMU attitude under world coordinate system,It is that visual coordinate system is relative to generation The rotation of boundary's coordinate system.

Step 4: definition errors in position measurement model

${\tilde{z}}_p=z_p-{\hat{z}}_p=C_{\left(q_v^w\right)}^T(p_w^i+C_{\left(q_w^i\right)}^T{p}_i^c)\mathrm{\λ}+n_p-C_{\left(q_v^w\right)}^T({\hat{p}}_w^i+C_{\left({\hat{q}}_w^i\right)}^T{\hat{p}}_i^c)\widehat{\mathrm{\λ}}$

Definition wheel measuring error model

$\begin{array}{c}{\tilde{z}}_q=z_q-{\hat{z}}_q=q_i^c\⊗q_w^i\⊗q_v^w\⊗{(q_i^c\⊗q_w^i\⊗q_v^w)}^{-1}\\ ={\mathrm{\δq}}_i^c\⊗{\hat{q}}_i^c\⊗{\mathrm{\δq}}_w^i\⊗{q_i^c}^{-1}\\ =H_q^{wi}{\mathrm{\δq}}_w^i=H_q^{ic}{\mathrm{\δq}}_i^c\end{array}$

Wherein,WithIt is to be error state amount respectivelyWithWrong calculation matrix.Finally, Calculation matrix can be accumulated as:

$\left[\begin{array}{c}{\tilde{z}}_p\\ {\tilde{z}}_q\end{array}\right]=\left[\begin{array}{c}{H}_p\\ \begin{array}{cccc}{0}_{3\×6}& {\tilde{H}}_q^{wi}& 0_{3\×10}& {\tilde{H}}_q^{ic}\end{array}\end{array}\right]\tilde{x}$

Step 5: when we get calculation matrix H, we can be according to Kalman filter Step is updated, as shown in the more new module in Fig. 5.

Calculating residual vector:

Calculate with new amount: S=HPH^T+R；

Calculate Kalman gain: K=PH^TS^-1；

Calculating correct amount:According to correct amountWe can calculate the renewal amount of X state. Error state four element can be updated by such as following formula:

P_k+1|k+1=(I_d-KH)P_k+1|k(I_d-KH)^T+KRK^T

Followed the tracks of by above-mentioned monocular and IMU merges, obtain the attitude output that mobile terminal is stable, and then Realize stable motion tracking, the technical scheme overall block flow diagram of the embodiment of the present invention, as shown in Figure 6.

Vision pose and the IMU pose of above-described embodiment only present invention carry out Kalman's fusion One citing, in addition to this embodiment, it is also possible to uses and well known to a person skilled in the art other method, It also is able to realize the technique effect of the present invention.

In each method embodiment of the present invention, the sequence number of each step can not be used for limiting the elder generation of each step Rear order, for those of ordinary skill in the art, on the premise of not paying creative work, The priority of each step is changed also within protection scope of the present invention.

On the other hand, with said method accordingly, the present invention also provide for a kind of smart mobile phone monocular and Device is followed the tracks of in the stable motion that IMU merges, as it is shown in fig. 7, comprises:

Map initialization module 11, for utilizing ORB algorithm that the image obtained is processed, it After carry out 3D reconstruct, obtain initial point map, complete map initialization；

Visual tracking module 12, is used for using ORB algorithm real-time matching and parallel local key frame to build The mode of figure carries out visual tracking, obtains vision pose；

IMU pose computing module 13: for obtaining acceleration and the angle that IMU produces at three dimensions Velocity amplitude, and acceleration and magnitude of angular velocity are integrated computing, obtain IMU pose and predict the outcome；

Fusion Module 14: carry out Kalman's fusion for vision pose and IMU pose are predicted the outcome, Motion tracking is carried out according to the posture information obtained after merging.

Compared with prior art, the present invention has the more stable motion tracking mode that can obtain, and Realize the feature that the real-time online of yardstick is estimated.

As a modification of the present invention, map initialization module 11 is additionally operable to:

The first two field picture obtained use ORB algorithm extract characteristic point and calculate description, by the One frame is designated as key frame, the absolute pose of labelling camera；

After camera translates one end distance, then ORB algorithm is used to extract characteristic point to the image obtained And calculate description, mate with the first frame image features point, the second frame is designated as key frame, and Calculate under the second frame camera relative to the relative pose of the first frame；

The feature point set that the match is successful is carried out 3D reconstruct, obtains initial point map.

In the present invention, ORB algorithm is used to extract feature and directly mate estimation pose, ORB algorithm It is that FAST Corner Detection is combined with BRIEF feature description a kind of algorithm improvement, has taken into account monocular and regarded Efficiency during feel tracking and precision.

As a further improvement on the present invention, relative relative to the first frame of camera under the second frame is calculated Pose includes:

According to the Corresponding matching feature point set on the first frame and the second two field picture, calculate between two two field pictures Basis matrix；

According to basis matrix and the intrinsic parameter of camera, it is calculated essential matrix；

Essential matrix is used singular value decomposition, obtains relative relative to the first frame of camera under the second frame Pose.

The present invention is at camera in motor process, and can obtain corresponding to every frame picture successively is a series of Relative pose.

As the further improvement of the present invention, visual tracking module 12 is additionally operable to:

ORB algorithm rasterizing is used to extract image characteristic point and calculate description the present frame of image Son；

Use constant speed movement model, estimate present frame correspondence camera pose, by previous frame image institute There is point map to project in current image frame, carry out Feature Points Matching, and by the previous frame that the match is successful Point map is assigned to the corresponding characteristic point of present frame；

LM algorithm and Huber is used to estimate to update present frame pose and present frame point map；

According to the pose after updating, all for local key frame point maps are projected in current image frame, And carry out Feature Points Matching, after the match is successful, all point maps that the match is successful are assigned to present frame Corresponding characteristic point, and renewal present frame pose is with current again to use LM algorithm and Huber to estimate Frame point map.

In the present invention, the mode using ORB algorithm real-time matching and parallel local key frame to build figure is entered Row visual tracking, and then obtain vision pose.Wherein, ORB algorithm real-time matching and pose are estimated For track thread, the locally maintenance of key frame and to build figure be joint, local main feed line journey.In the present invention, with Track thread processes with local key frame thread parallel, and efficiency high energy realizes real-time tracking.

As a modification of the present invention, visual tracking module 12 is additionally operable to:

Judge whether to need to increase key according to time interval situation and/or present frame point map number Frame, if increasing after key frame exceedes certain time from last time or the map of present frame is counted less than threshold Value, then increase new key frame；

Judge whether present frame is new key frame, if it is, increase new point map, by new Key frame carries out characteristic point without all characteristic points of point map with all characteristic points in the key frame of local Coupling, after the match is successful, 3D reconstruct obtains new point map；

Locally bundle adjustment optimization, revises cumulative error, the pose after being optimized and point map.

In the present invention, i.e. can guarantee that the seriality of tracking can reduce again and need key frame quantity to be processed, Decrease the process time, improve the efficiency of motion tracking.

The above is the preferred embodiment of the present invention, it is noted that general for the art For logical technical staff, on the premise of without departing from principle of the present invention, it is also possible to make some changing Entering and retouch, these improvements and modifications also should be regarded as protection scope of the present invention.

Claims (10)

1. the stable motion tracking that a smart mobile phone monocular and IMU merge, it is characterised in that Including:

Utilize ORB algorithm that the image obtained is processed, carry out 3D reconstruct afterwards, obtain initial Point map, completes map initialization；

The mode using ORB algorithm real-time matching and parallel local key frame to build figure carry out vision with Track, obtains vision pose；

Obtain acceleration and magnitude of angular velocity that IMU produces at three dimensions, and fast to acceleration and angle Angle value is integrated computing, obtains IMU pose and predicts the outcome；

Vision pose and IMU pose are predicted the outcome and carries out Kalman's fusion, obtain according to after merging Posture information carry out motion tracking.

The stable motion that smart mobile phone monocular the most according to claim 1 and IMU merge is followed the tracks of Method, it is characterised in that the described ORB of utilization algorithm to obtain image process, laggard Row 3D reconstructs, and obtains initial point map, completes map initialization and include:

The first two field picture obtained use ORB algorithm extract characteristic point and calculate description, by the One frame is designated as key frame, the absolute pose of labelling camera；

After camera translates one end distance, then ORB algorithm is used to extract characteristic point to the image obtained And calculate description, mate with the first frame image features point, the second frame is designated as key frame, and Calculate under the second frame camera relative to the relative pose of the first frame；

The feature point set that the match is successful is carried out 3D reconstruct, obtains initial point map.

The stable motion that smart mobile phone monocular the most according to claim 2 and IMU merge is followed the tracks of Method, it is characterised in that described in calculate under the second frame camera relative to the relative pose bag of the first frame Include:

According to the Corresponding matching feature point set on the first frame and the second two field picture, calculate between two two field pictures Basis matrix；

According to basis matrix and the intrinsic parameter of camera, it is calculated essential matrix；

Essential matrix is used singular value decomposition, obtains under described second frame camera relative to the first frame Relative pose.

4. according to stablizing that described smart mobile phone monocular arbitrary in claim 1-3 and IMU merge Motion tracking method, it is characterised in that described use ORB algorithm real-time matching and parallel local are closed Key frame is built the mode of figure and is carried out visual tracking, obtains vision pose and includes:

ORB algorithm rasterizing is used to extract image characteristic point and calculate description the present frame of image Son；

Use constant speed movement model, estimate present frame correspondence camera pose, by previous frame image institute There is point map to project in current image frame, carry out Feature Points Matching, and by the previous frame that the match is successful Point map is assigned to the corresponding characteristic point of present frame；

LM algorithm and Huber is used to estimate to update present frame pose and present frame point map；

According to the pose after updating, all for local key frame point maps are projected in current image frame, And carry out Feature Points Matching, after the match is successful, all point maps that the match is successful are assigned to present frame Corresponding characteristic point, and renewal present frame pose is with current again to use LM algorithm and Huber to estimate Frame point map.

The stable motion that smart mobile phone monocular the most according to claim 4 and IMU merge is followed the tracks of Method, it is characterised in that described use ORB algorithm real-time matching and parallel local key frame build figure Mode carry out visual tracking, obtain vision pose and also include:

Judge whether to need to increase key according to time interval situation and/or present frame point map number Frame, if increasing after key frame exceedes certain time from last time or the map of present frame is counted less than threshold Value, then increase new key frame；

Judge whether present frame is new key frame, if it is, increase new point map, by new Key frame carries out characteristic point without all characteristic points of point map with all characteristic points in the key frame of local Coupling, after the match is successful, 3D reconstruct obtains new point map；

Locally bundle adjustment optimization, revises cumulative error, the pose after being optimized and point map.

6. device is followed the tracks of in the stable motion that a smart mobile phone monocular and IMU merge, it is characterised in that Including:

Map initialization module, for utilizing ORB algorithm to process the image obtained, afterwards Carry out 3D reconstruct, obtain initial point map, complete map initialization；

Visual tracking module, is used for using ORB algorithm real-time matching and parallel local key frame to build figure Mode carry out visual tracking, obtain vision pose；

IMU pose computing module: fast for obtaining acceleration that IMU produces at three dimensions and angle Angle value, and acceleration and magnitude of angular velocity are integrated computing, obtain IMU pose and predict the outcome；

Fusion Module: carry out Kalman's fusion for vision pose and IMU pose are predicted the outcome, Motion tracking is carried out according to the posture information obtained after merging.

The stable motion that smart mobile phone monocular the most according to claim 6 and IMU merge is followed the tracks of Device, it is characterised in that described map initialization module is additionally operable to:

The first two field picture obtained use ORB algorithm extract characteristic point and calculate description, by the One frame is designated as key frame, the absolute pose of labelling camera；

After camera translates one end distance, then ORB algorithm is used to extract characteristic point to the image obtained And calculate description, mate with the first frame image features point, the second frame is designated as key frame, and Calculate under the second frame camera relative to the relative pose of the first frame；

The feature point set that the match is successful is carried out 3D reconstruct, obtains initial point map.

The stable motion that smart mobile phone monocular the most according to claim 7 and IMU merge is followed the tracks of Device, it is characterised in that described in calculate under the second frame camera relative to the relative pose bag of the first frame Include:

According to the Corresponding matching feature point set on the first frame and the second two field picture, calculate between two two field pictures Basis matrix；

According to basis matrix and the intrinsic parameter of camera, it is calculated essential matrix；

Essential matrix is used singular value decomposition, obtains under described second frame camera relative to the first frame Relative pose.

9. according to stablizing that described smart mobile phone monocular arbitrary in claim 6-8 and IMU merge Motion tracer, it is characterised in that described visual tracking module is additionally operable to:

ORB algorithm rasterizing is used to extract image characteristic point and calculate description the present frame of image Son；

Use constant speed movement model, estimate present frame correspondence camera pose, by previous frame image institute There is point map to project in current image frame, carry out Feature Points Matching, and by the previous frame that the match is successful Point map is assigned to the corresponding characteristic point of present frame；

LM algorithm and Huber is used to estimate to update present frame pose and present frame point map；

According to the pose after updating, all for local key frame point maps are projected in current image frame, And carry out Feature Points Matching, after the match is successful, all point maps that the match is successful are assigned to present frame Corresponding characteristic point, and renewal present frame pose is with current again to use LM algorithm and Huber to estimate Frame point map.

The stable motion that smart mobile phone monocular the most according to claim 9 and IMU merge with Track device, it is characterised in that described visual tracking module is additionally operable to:

Judge whether to need to increase key according to time interval situation and/or present frame point map number Frame, if increasing after key frame exceedes certain time from last time or the map of present frame is counted less than threshold Value, then increase new key frame；

Judge whether present frame is new key frame, if it is, increase new point map, by new Key frame carries out characteristic point without all characteristic points of point map with all characteristic points in the key frame of local Coupling, after the match is successful, 3D reconstruct obtains new point map；

Locally bundle adjustment optimization, revises cumulative error, the pose after being optimized and point map.