CN109798889A - Optimization method, device, storage medium and electronic equipment based on monocular VINS system - Google Patents
Optimization method, device, storage medium and electronic equipment based on monocular VINS system Download PDFInfo
- Publication number
- CN109798889A CN109798889A CN201811642112.1A CN201811642112A CN109798889A CN 109798889 A CN109798889 A CN 109798889A CN 201811642112 A CN201811642112 A CN 201811642112A CN 109798889 A CN109798889 A CN 109798889A
- Authority
- CN
- China
- Prior art keywords
- monocular
- imu
- vins
- vins system
- correlated variables
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Landscapes
- Navigation (AREA)
Abstract
This disclosure relates to a kind of optimization method based on monocular VINS system, device, storage medium and electronic equipment.This method comprises: firstly, estimate the correlated variables of inertial navigation sensors IMU, and using the estimated value as the initial value of monocular VINS system.Then, according to figure optimization method, joint IMU motion model is established in motion tracking thread, and state estimation is carried out to monocular VINS system.In this way, the correlated variables of IMU, which is introduced into monocular VINS system, can determine scale factor, in addition, carrying out state estimation by establishing joint IMU motion model to monocular VINS system, the accuracy and robustness of monocular VINS system can be improved.
Description
Technical field
This disclosure relates to computer vision technique and information fusion technology, and in particular, to one kind is based on monocular VINS system
Optimization method, device, storage medium and the electronic equipment of system.
Background technique
In recent years, with the fast development of computer hardware and computer vision technique, mobile robot while, is positioned
Very big breakthrough is obtained with diagram technology (Simultaneous Localization and Mapping, SLAM) is built, due to list
Mesh camera has many advantages, such as small in size and easy to install, and monocular vision SLAM technology becomes the active positioning field of mobile robot
In research emphasis.
However, monocular camera itself, there are scale uncertain problem, system can not correctly measure environment, so as to cause
It can not be applied in actual navigation application, and pass through fusion inertial navigation sensors (Inertial Measurement
Unit, IMU) metrical information, monocular SLAM system is improved to monocular VINS (Visual Inertial Navigation
System) system can correctly recover scale factor, while obtain higher accuracy and stronger robustness.
Summary of the invention
In order to overcome problems of the prior art, the embodiment of the present disclosure provides a kind of based on the excellent of monocular VINS system
Change method, apparatus, storage medium and electronic equipment.
To achieve the goals above, disclosure first aspect provides a kind of optimization method based on monocular VINS system, packet
It includes:
The correlated variables of inertial navigation sensors IMU is estimated, and resulting value will be estimated as monocular VINS system
Initial value, where the correlated variables of the IMU includes the gyroscope zero bias of IMU, accelerometer bias and monocular VINS system
The movement speed of platform；
According to figure optimization method, joint IMU motion model is established in motion tracking thread, to the monocular VINS
System carries out state estimation.
Optionally, described according to figure optimization method, joint IMU motion model is established in motion tracking thread, it is right
The monocular VINS system carries out state estimation, comprising:
In motion tracking thread, determine whether monocular VINS system map updates；
Whether updated according to the monocular VINS system map, selects corresponding factor graph；
According to the factor graph, state estimation is carried out to the monocular VINS system.
Optionally, the correlated variables to inertial navigation sensors IMU is estimated, and regard estimation resulting value as list
The initial value of mesh VINS system, comprising:
Pre-integration processing is carried out to the correlated variables of inertial navigation sensors IMU by way of decoupling, and will processing gained
It is worth the initial value as monocular VINS system.
Optionally, described that pre-integration processing, packet are carried out to the correlated variables of inertial navigation sensors IMU by way of decoupling
It includes:
Pre-integration processing is carried out to the gyroscope zero bias of inertial navigation sensors IMU；
Pre-integration processing tentatively is carried out to scale factor and gravity direction in the case where not considering accelerometer bias；
Pre-integration processing is carried out to the accelerometer bias, and correction updates the scale factor and the gravity side
To；
Pre-integration processing is carried out to linear speed.
Optionally, the method also includes:
According to the pre-integration value of the correlated variables of the IMU, the state of the IMU is designed in local map building thread
The local window of variable, and corresponding local optimum factor graph is constructed, carry out state estimation；
According to the scale factor, pose optimization is carried out in closure winding thread, and constructs global nonlinear optimization
Factor graph carries out state estimation.
Optionally, before the correlated variables to inertial navigation sensors IMU is estimated, the method is also wrapped
It includes:
Determine the time interval between adjacent two key frame；
If the time interval is greater than preset duration, adjacent two key frame corresponding with the time interval is deleted.
The second aspect of the disclosure provides a kind of optimization device based on monocular VINS system, comprising:
The correlated variables estimation module of IMU, estimates for the correlated variables to inertial navigation sensors IMU, and will
Estimate that initial value of the resulting value as monocular VINS system, the correlated variables of the IMU include the gyroscope zero bias of IMU, accelerate
The movement speed of platform where degree meter zero bias and monocular VINS system；
First state estimation module, for establishing joint IMU fortune in motion tracking thread according to figure optimization method
Movable model carries out state estimation to the monocular VINS system.
Optionally, the first state estimation module, comprising:
Submodule is determined, for determining whether monocular VINS system map updates in motion tracking thread；
Select submodule selects corresponding factor graph for whether updating according to the monocular VINS system map；
State estimation submodule, for carrying out state estimation to the monocular VINS system according to the factor graph.
Optionally, the correlated variables estimation module of the IMU is also used to through decoupling mode to inertial navigation sensors IMU
Correlated variables carry out pre-integration processing, and will processing resulting value as the initial value of monocular VINS system.
Optionally, the correlated variables estimation module of the IMU includes:
First pre-integration handles submodule, carries out at pre-integration for the gyroscope zero bias to inertial navigation sensors IMU
Reason；
Second pre-integration handles submodule, in the case where not considering accelerometer bias tentatively to scale factor and
Gravity direction carries out pre-integration processing；
Third pre-integration handles submodule, is used to carry out the accelerometer bias pre-integration processing, and correction is more
The new scale factor and the gravity direction；
4th pre-integration handles submodule, for carrying out pre-integration processing to linear speed.
Optionally, described device further include:
Second state estimation module constructs line in local map for the pre-integration value according to the correlated variables of the IMU
The local window of the state variable of the IMU is designed in journey, and constructs corresponding local optimum factor graph, carries out state estimation；
Third state estimation module, for carrying out pose optimization in closure winding thread according to the scale factor, and
The factor graph for constructing global nonlinear optimization, carries out state estimation.
Optionally, described device further include:
Determining module, for determining the time interval between adjacent two key frame；
Removing module deletes phase corresponding with the time interval if being greater than preset duration for the time interval
Adjacent two key frames.
The disclosure third aspect provides a kind of computer readable storage medium, is stored thereon with computer program, the program
The step of the method provided by disclosure first aspect is realized when being executed by processor.
Disclosure fourth aspect provides a kind of electronic equipment, comprising:
Memory is stored thereon with computer program；
Processor, for executing the computer program in the memory, to realize that disclosure first aspect is mentioned
The step of the method for confession.
Through the above technical solutions, firstly, estimate the correlated variables of inertial navigation sensors IMU, and this is estimated
Initial value of the evaluation as monocular VINS system.Then, according to figure optimization method, joint is established in motion tracking thread
IMU motion model carries out state estimation to monocular VINS system.In this way, in this way, the correlated variables of IMU is introduced into monocular
Scale factor can be determined in VINS system, in addition, carrying out by establishing joint IMU motion model to monocular VINS system
The accuracy and robustness of monocular VINS system has can be improved in state estimation.
Other feature and advantage of the disclosure will the following detailed description will be given in the detailed implementation section.
Detailed description of the invention
Attached drawing is and to constitute part of specification for providing further understanding of the disclosure, with following tool
Body embodiment is used to explain the disclosure together, but does not constitute the limitation to the disclosure.In the accompanying drawings:
Fig. 1 is a kind of flow chart of optimization method based on monocular VINS system shown according to an exemplary embodiment.
Fig. 2 is a kind of process of the optimization method based on monocular VINS system shown according to another exemplary embodiment
Figure.
Fig. 3 is a kind of factor in the monocular VINS system map not more new stage shown according to an exemplary embodiment
The schematic diagram of figure.
Fig. 4 is a kind of factor graph at the monocular VINS system map rejuvenation stage shown according to an exemplary embodiment
Schematic diagram.
Fig. 5 is a kind of local optimum factor graph in local map building thread shown according to an exemplary embodiment
Schematic diagram.
Fig. 6 is a kind of progress pose Optimization Factor figure in closure winding thread shown according to an exemplary embodiment
Schematic diagram.
Fig. 7 is a kind of progress global optimization factor graph in closure winding thread shown according to an exemplary embodiment
Schematic diagram.
Fig. 8 is a kind of block diagram of optimization device based on monocular VINS system shown according to an exemplary embodiment.
Fig. 9 is the block diagram of a kind of electronic equipment shown according to an exemplary embodiment.
Specific embodiment
It is described in detail below in conjunction with specific embodiment of the attached drawing to the disclosure.It should be understood that this place is retouched
The specific embodiment stated is only used for describing and explaining the disclosure, is not limited to the disclosure.
Referring to FIG. 1, Fig. 1 is a kind of optimization method based on monocular VINS system shown according to an exemplary embodiment
Flow chart.As shown in Figure 1, this method may comprise steps of.
In a step 11, the correlated variables of inertial navigation sensors IMU is estimated, and regard estimation resulting value as list
The initial value of mesh VINS system.
Under normal conditions, any VINS system is required to initialize.The main mesh that VINS system is initialized
Be obtain optimize necessary to parameter and state initial value.Wherein, in the disclosure, the correlated variables packet of the IMU
The movement speed of platform where including gyroscope zero bias, accelerometer bias and the monocular VINS system of IMU.
It is more sensitive to some initial values since VINS system is the very high system of nonlinear degree, initialization it is good
The bad robustness and positioning accuracy that will have a direct impact on whole system.Therefore, during initialization, the correlation to IMU is needed
Variable is solved.
In addition, in order to which the measured value of the IMU data that allow between two continuous frames picture frame as a whole in the disclosure can
To use pre-integration method, in data processing, pre-integration is solved to the correlated variables of IMU.Wherein, two frame of IMU data
IMU data between continuous picture frame refer to the acceleration that accelerometer is measured and the angular speed that gyroscope is measured.Pass through IMU
Pre-integration method, can merge the state variable of the IMU between two neighboring vision key frame i and j becomes a compound item,
Constitute the kinematic constraint of two adjacent vision key frames.Once they are not only in monocular in addition, the correlated variables of IMU is introduced into
Constraint is generated between the continuous motion pose of camera, and in the accelerometer of continuous time and the speed of gyroscope and IMU zero bias
Also there is constraint between estimation, to increase the system state variables to be estimated.
Specifically, the six-freedom degree pose (pose) of platform where monocular camera SLAM system can solve system: x,
Y, z, roll, pitch, yaw indicate with English alphabet P, can be by measuring to accelerometer due to IMU kinematics model
The angular speed that acceleration and gyroscope are measured is integrated, so IMU motion model can also find out x, y, z, roll, pitch,
Yaw, while movement speed (Velocity) V can also be found outx,Vy,Vz, indicated with English alphabet V.And due to cheap IMU's
The zero bias of accelerometer and gyroscope can change over time, so also to solve zero bias (Bias), be indicated with English alphabet B.It will
The state variable that IMU and monocular camera respectively solve is unified by the relative pose (outer ginseng) between two sensors, can will
The state variable to be estimated of monocular VINS system is write as the form of P, V, B.
In order to guarantee that the correlated variables of the IMU in initialization is observable, monocular VINS system when entire initial phase
Platform where system will have abundant exercise to motivate and measure enough data.Further, since the degree of coupling of the correlated variables of IMU
Higher, the especially component of gravity vector and acceleration bias in z-axis directly disposably estimates all variables to be very tired
Difficult something, so, pre-integration processing is carried out to the correlated variables of IMU by the way of a kind of decoupling in the disclosure,
And the correlated variables of the IMU by solving to all key frames in a period of time carries out successive ignition update, receives each variable
It holds back to a stable value.
It specifically, is to become the correlation of IMU by the essence that correlated variables of the decoupling mode to IMU carries out pre-integration processing
The estimation problem of amount is divided into 4 independent subproblems.Wherein, 4 independent subproblems are as follows: (1) to inertial navigation sensors
The gyroscope zero bias of IMU carry out pre-integration processing；(2) in the case where not considering accelerometer bias tentatively to scale factor and
Gravity direction carries out pre-integration processing；(3) to accelerometer bias carry out pre-integration processing, and correction update scale factor and
Gravity direction；(4) pre-integration processing is carried out to linear speed.
In addition, tentatively being carried out at pre-integration to scale factor and gravity direction in the case where not considering accelerometer bias
Reason, and, during carrying out pre-integration processing, and correction update scale factor and gravity direction to accelerometer bias,
It can determine that scale factor.
As described above, the x of monocular camera SLAM system estimation, y, z do not have scale, i.e., no unit, it is not known that be cm
Or m or other, it is scale base 1 that result only simply sets a moving distance when system initialization,
Subsequent estimation is all based on value progress ratio and expands or shrinks.The correlated variables of IMU is being fused to monocular camera SLAM
In system, and after determining scale factor, it is true the scale base 1 can be reduced to physical world according to the scale factor
Moving distance, all x of such monocular VINS system estimation, y, z are true physical values.That is, making monocular VINS system
Scale factor can correctly be recovered.
By adopting the above technical scheme, pre-integration processing is carried out to the correlated variables of IMU by way of decoupling, and institute will be handled
The initial value as monocular VINS system must be worth, so that the initial value of monocular VINS system is more accurate, and then list can be improved
The accuracy and robustness of mesh VINS system.
In addition, it is contemplated that the gyroscope zero bias and accelerometer bias (can be collectively referred to as IMU zero bias) of IMU are to become at any time
Change, so the time interval between adjacent two frames key frame is bigger, between IMU correlated variables pre-integration go out P, V,
B is more inaccurate, and the precision of monocular VINS system will decline.Therefore, in the disclosure, estimate in the correlated variables to IMU
Before, redundancy key frames need to be rejected.Specifically, the strategy for rejecting redundancy key frames can be with are as follows: determines between adjacent two key frame
Time interval；If the time interval is greater than preset duration, adjacent two key frame corresponding with the time interval is rejected.
Wherein, which can be the numerical value of default, be also possible to user's self-setting, can be, for example,
0.5s.It should be noted that how to judge whether a certain picture frame is that key frame belongs to the prior art, details are not described herein again.
In this way, by rejecting redundancy key frames, so that the time interval between adjacent key frame is shorter, so that the phase of IMU
The estimated value for closing variable is more accurate.
In step 12, right in movement according to foundation joint IMU motion model in thread according to figure optimization method
Monocular VINS system carries out state estimation.
Joint IMU motion model is the system model that finger vision and IMU are combined.State estimation is carried out to monocular VINS system
Refer to the state variable that monocular VINS system is solved by some algorithms, wherein the state variable be six-freedom degree pose P,
Movement speed V and zero bias B, wherein P includes: x, y, z, roll, pitch, yaw.In the disclosure, pass through figure Optimization Solution side
Method carries out state estimation to monocular VINS system, that is, P, V, B of monocular VINS system are solved, to obtain more accurate position
Appearance.
Specifically, as shown in Fig. 2, the step 12 can specifically include following steps.
In step 121, in motion tracking thread, determine whether monocular VINS system map updates.
VINS system is nonlinear system, and the state estimation problem for solving a nonlinear system has many methods, herein
It is to be solved in the way of " figure optimization ", figure optimization is the form that nonlinear optimal problem is abstracted into " factor graph ",
When system model is converted into Mathematical framework, more intuitively.
Wherein, a factor graph includes the side between multiple vertex and vertex, and figure optimization refers to that the state that will be solved becomes
The vertex of (namely six-freedom degree pose P, movement speed V and IMU zero bias B) as factor graph is measured, (re-projection misses observation error
Difference, closed loop constraint etc.) side as factor graph, it is then solved using the solver (Gauss-Newton etc.) schemed in optimizing, i.e.,
State variable P, V, B so that when observation error minimum can be acquired, which is optimal solution.In the disclosure, monocular
VINS system is to set different strategies according to the solution demand of different phase to solve the state variable in different stages (top
Point), and this has also corresponded to different observation errors (side).Therefore, in the disclosure, can according to monocular VINS system map whether
It updates, selects different factor graphs.Wherein, building map thread often does a local map optimization and just completes primary update.
In step 122, whether updated according to monocular VINS system map, select corresponding factor graph.
In step 123, according to factor graph, state estimation is carried out to monocular VINS system.
As described above, the different strategy of monocular VINS default solves the state variable in different stages, therefore,
In monocular VINS system map rejuvenation stage and not more new stage, different factor graphs need to be selected to carry out solving state variable.
Specifically, referring to FIG. 3, Fig. 3 is one kind shown according to an exemplary embodiment in monocular VINS system map
The not schematic diagram of factor graph when the more new stage.As shown in figure 3, the vertex of the factor graph is current image frame j's to be optimized
State variable, the state variable of previous key frame i and the point map being matched to previous key frame i, the Bian Weiqian of factor graph
The point map that one key frame i is matched to is missed to the IMU between the re-projection error of monocular camera, picture frame j and previous key frame i
Difference, the imu error refer to the V between two field picturesiAnd VjError, ViAnd VjError, PiAnd PjError.Due to monocular
VINS system map does not update, the state variable and point map of previous key frame i immobilizes, therefore, shown in Fig. 3 because
In subgraph, state variable and point map to previous key frame i are without optimization.That is, to the void in the factor graph shown in Fig. 3
Content in wire frame is without optimization.In addition, it should be noted that, optimizing rear resulting state variable can be used as next shape
The prior information of state estimation.
Fig. 4 is a kind of factor graph at the monocular VINS system map rejuvenation stage shown according to an exemplary embodiment
Schematic diagram.As shown in figure 4, the vertex of the factor graph is the state variable of current image frame j to be optimized, previous image frame j-
1 state variable and with the point map of previous picture frame j-1, the point map that the side of factor graph is previous image frame j-1 is to singly
Imu error and previous image frame j-1 between the re-projection error of mesh camera, previous image frame j-1 and current image frame j
Prior information.Wherein, which refers to the V between two field picturesj-1And VjError, Vj-1And VjError, Pj-1And Pj
Error.
Due to monocular VINS system map rejuvenation, the state variable of previous image frame will no longer immobilize, but previous
The point map of picture frame is still to immobilize, therefore, in the factor graph shown in Fig. 4, to the state variable of previous key frame j-1
It is optimized together with the state variable of current image frame j.And after this suboptimization, previous image frame j-1 is dissolved by edge
It goes, does not then use the state variable of previous image frame j-1 in estimated state variable next time, but will the previous figure
As frame j-1 is retained in map.It is optimized in this way, monocular VINS system sets different factor graphs in the different stages
State variable P, V, B obtain more accurate pose, meet system accuracy and real-time in practical application.
By adopting the above technical scheme, firstly, estimating the correlated variables of inertial navigation sensors IMU, and this is estimated
Initial value of the evaluation as monocular VINS system.Then, according to figure optimization method, joint is established in motion tracking thread
IMU motion model carries out state estimation to monocular VINS system.In this way, the correlated variables of IMU is introduced into monocular VINS system
In can determine scale factor, in addition, by establish joint IMU motion model, to monocular VINS system progress state estimation,
The accuracy and robustness of monocular VINS system can be improved.
In addition, it is contemplated that in ORBSLAM project other than including motion tracking thread, further includes: local map structure
Build thread and closure winding thread.In the disclosure, in addition to optimizing it to monocular VINS system in motion tracking thread
Outside, it can also be constructed in thread and closure winding thread in local map and monocular VINS system is optimized.
Specifically, in local map building thread, according to the pre-integration value of the correlated variables of IMU, the state of IMU is designed
The local window of variable, and corresponding local optimum factor graph is constructed, carry out state estimation.As described above, pass through the pre- product of IMU
Divide method, can merge the state variable of the IMU between two neighboring vision key frame i and j becomes a compound item, constitutes
The kinematic constraint of two adjacent vision key frames.Therefore, the monocular VINS system progress part for having merged the correlated variables of IMU is excellent
When change, since the introducing of the correlated variables of IMU has stronger the constraint relationship between nearest some key frames, part
Optimize the state variable to the N frame key frame being newly inserted and all point maps that can be observed by these key frames carry out
Optimization.
Illustratively, Fig. 5 is a kind of local optimum in local map building thread shown according to an exemplary embodiment
The schematic diagram of factor graph.As shown in figure 5, defining the local window comprising this N frame key frame and one includes those energy
Enough observe the fixation window of other key frames of these point maps.The vertex of factor graph is the position of each key frame in local window
Appearance, movement speed and IMU zero bias, key frame in all point maps that these key frames can observe, and fixed window
The state variable of pose and N+1 frame, while being the imu error between re-projection error and adjacent two frame.Wherein, in fixed window
Key frame also provide the constraint condition of nonlinear optimization and the re-projection error of vision be only provided, but local optimum is not right
The pose of these key frames optimizes.That is, the content in dotted line frame in factor graph shown in Fig. 5 is without optimization.In addition,
In order to ensure the real-time of monocular VINS system, the scale of the local window in Fig. 5 cannot be too big, that is, the value of above-mentioned N cannot
It is excessive.
In addition, carrying out pose optimization according to scale factor, and construct global nonlinear optimization in closure winding thread
Factor graph carries out state estimation.
It is finished in order to rapid Optimum and realizes the closed-loop corrected of VINS system, pose figure optimal way can be used, only
The pose of monocular camera is optimized, is optimized without to map point.In the disclosure, according to the scale having been observed that because
Son only optimizes the pose of six degree of freedom.Wherein, which is to become above by correlation of the decoupling mode to IMU
Amount carries out the scale factor determined in pre-integration treatment process.Referring to FIG. 6, Fig. 6 is shown according to an exemplary embodiment
A kind of schematic diagram carrying out pose Optimization Factor figure in closure winding thread.As shown in fig. 6, the vertex of factor graph is all
Key frame pose, while being the error of the transformation under the relative pose and world coordinate system between picture frame between pose.
The although available position and posture estimation of above-mentioned part, but since the nonlinear optimization of progress is only in local window
Middle operation, there is no optimizing to global pose and map, inevitably there is error accumulation in system, that is, generates drift
Phenomenon.Therefore, to the influence of reduction accumulated error, the factor graph for needing to construct globalization carries out global optimization.Illustratively, it asks
It is a kind of progress global optimization factor graph in closure winding thread shown according to an exemplary embodiment with reference to Fig. 7, Fig. 7
Schematic diagram.As shown in fig. 7, the variable all to monocular VINS system optimizes, the position including all point maps, key frame
Appearance and movement speed and IMU zero bias.The vertex of factor graph is the above-mentioned variable to be optimized, and side is that re-projection error and IMU are missed
Difference.
By adopting the above technical scheme, three primary thread motion tracking threads, local maps for including in ORBSLAM project
It is optimized respectively in building thread and closure winding thread, further just improves the accuracy and robust of monocular VINS system
Property.
Based on the same inventive concept, the disclosure also provides a kind of optimization device based on monocular VINS system.Fig. 8 is basis
A kind of block diagram of optimization device based on monocular VINS system shown in one exemplary embodiment.As shown in figure 8, the device packet
It includes:
The correlated variables estimation module 81 of IMU, estimates for the correlated variables to inertial navigation sensors IMU, and
Using estimation resulting value as the initial value of monocular VINS system, the correlated variables of the IMU includes the gyroscope zero bias of IMU, adds
The movement speed of platform where speedometer zero bias and monocular VINS system；
First state estimation module 82, for establishing joint IMU in motion tracking thread according to figure optimization method
Motion model carries out state estimation to the monocular VINS system.
Optionally, the first state estimation module, comprising:
Submodule is determined, for determining whether monocular VINS system map updates in motion tracking thread；
Select submodule selects corresponding factor graph for whether updating according to the monocular VINS system map；
State estimation submodule, for carrying out state estimation to the monocular VINS system according to the factor graph.
Optionally, the correlated variables estimation module of the IMU is also used to through decoupling mode to inertial navigation sensors IMU
Correlated variables carry out pre-integration processing, and will processing resulting value as the initial value of monocular VINS system.
Optionally, the correlated variables estimation module of the IMU includes:
First pre-integration handles submodule, carries out pre-integration processing for the gyroscope zero bias to IMU；
Second pre-integration handles submodule, in the case where not considering accelerometer bias tentatively to scale factor and
Gravity direction carries out pre-integration processing；
Third pre-integration handles submodule, is used to carry out the accelerometer bias pre-integration processing, and correction is more
The new scale factor and the gravity direction；
4th pre-integration handles submodule, for carrying out pre-integration processing to linear speed.
Optionally, described device further include:
Second state estimation module constructs line in local map for the pre-integration value according to the correlated variables of the IMU
The local window of the state variable of the IMU is designed in journey, and constructs corresponding local optimum factor graph, carries out state estimation；
Third state estimation module, for carrying out pose optimization in closure winding thread according to the scale factor, and
The factor graph for constructing global nonlinear optimization, carries out state estimation.
Optionally, described device further include:
Determining module, for determining the time interval between adjacent two key frame；
Removing module deletes phase corresponding with the time interval if being greater than preset duration for the time interval
Adjacent two key frames.
About the device in above-described embodiment, wherein modules execute the concrete mode of operation in related this method
Embodiment in be described in detail, no detailed explanation will be given here.
Fig. 9 is the block diagram of a kind of electronic equipment 900 shown according to an exemplary embodiment.As shown in figure 9, the electronics is set
Standby 900 may include: processor 901, memory 902.The electronic equipment 900 can also include multimedia component 903, input/
Export one or more of (I/O) interface 904 and communication component 905.
Wherein, processor 901 is used to control the integrated operation of the electronic equipment 900, above-mentioned based on monocular to complete
All or part of the steps in the optimization method of VINS system.Memory 902 is for storing various types of data to support
The operation of the electronic equipment 900, these data for example may include any using journey for what is operated on the electronic equipment 900
The instruction of sequence or method and the relevant data of application program, such as contact data, the message of transmitting-receiving, picture, audio, view
Frequency etc..The memory 902 can realize by any kind of volatibility or non-volatile memory device or their combination,
Such as static random access memory (Static Random Access Memory, abbreviation SRAM), electrically erasable is only
It reads memory (Electrically Erasable Programmable Read-Only Memory, abbreviation EEPROM), it is erasable
Except programmable read only memory (Erasable Programmable Read-Only Memory, abbreviation EPROM), may be programmed only
It reads memory (Programmable Read-Only Memory, abbreviation PROM), read-only memory (Read-Only Memory,
Abbreviation ROM), magnetic memory, flash memory, disk or CD.Multimedia component 903 may include screen and audio component.
Wherein screen for example can be touch screen, and audio component is used for output and/or input audio signal.For example, audio component can be with
Including a microphone, microphone is for receiving external audio signal.The received audio signal can be further stored in
Memory 902 is sent by communication component 905.Audio component further includes at least one loudspeaker, is used for output audio signal.
I/O interface 904 provides interface between processor 901 and other interface modules, other above-mentioned interface modules can be keyboard, mouse
Mark, button etc..These buttons can be virtual push button or entity button.Communication component 905 is for the electronic equipment 900 and its
Wired or wireless communication is carried out between his equipment.Wireless communication, such as Wi-Fi, bluetooth, near-field communication (Near Field
Communication, abbreviation NFC), 2G, 3G, 4G, NB-IOT, eMTC or other 5G etc. or they one or more of
Combination, it is not limited here.Therefore the corresponding communication component 905 may include: Wi-Fi module, bluetooth module, NFC mould
Block etc..
In one exemplary embodiment, electronic equipment 900 can be by one or more application specific integrated circuit
(Application Specific Integrated Circuit, abbreviation ASIC), digital signal processor (Digital
Signal Processor, abbreviation DSP), digital signal processing appts (Digital Signal Processing Device,
Abbreviation DSPD), programmable logic device (Programmable Logic Device, abbreviation PLD), field programmable gate array
(Field Programmable Gate Array, abbreviation FPGA), controller, microcontroller, microprocessor or other electronics member
Part is realized, for executing the above-mentioned optimization method based on monocular VINS system.
In a further exemplary embodiment, a kind of computer readable storage medium including program instruction is additionally provided, it should
The step of above-mentioned optimization method based on monocular VINS system is realized when program instruction is executed by processor.For example, the calculating
Machine readable storage medium storing program for executing can be the above-mentioned memory 902 including program instruction, and above procedure instruction can be by electronic equipment 900
Processor 901 is executed to complete the above-mentioned optimization method based on monocular VINS system.
The preferred embodiment of the disclosure is described in detail in conjunction with attached drawing above, still, the disclosure is not limited to above-mentioned reality
The detail in mode is applied, in the range of the technology design of the disclosure, a variety of letters can be carried out to the technical solution of the disclosure
Monotropic type, these simple variants belong to the protection scope of the disclosure.
It is further to note that specific technical features described in the above specific embodiments, in not lance
In the case where shield, it can be combined in any appropriate way.In order to avoid unnecessary repetition, the disclosure to it is various can
No further explanation will be given for the combination of energy.
In addition, any combination can also be carried out between a variety of different embodiments of the disclosure, as long as it is without prejudice to originally
Disclosed thought equally should be considered as disclosure disclosure of that.
Claims (10)
1. a kind of optimization method based on monocular VINS system characterized by comprising
The correlated variables of inertial navigation sensors IMU is estimated, and resulting value will be estimated as the first of monocular VINS system
Initial value, the correlated variables of the IMU include platform where gyroscope zero bias, accelerometer bias and the monocular VINS system of IMU
Movement speed；
According to figure optimization method, joint IMU motion model is established in motion tracking thread, to the monocular VINS system
Carry out state estimation.
2. the method according to claim 1, wherein described according to figure optimization method, in motion tracking line
Joint IMU motion model is established in journey, and state estimation is carried out to the monocular VINS system, comprising:
In motion tracking thread, determine whether monocular VINS system map updates；
Whether updated according to the monocular VINS system map, selects corresponding factor graph；
According to the factor graph, state estimation is carried out to the monocular VINS system.
3. the method according to claim 1, wherein the correlated variables to inertial navigation sensors IMU into
Row estimation, and using estimation resulting value as the initial value of monocular VINS system, comprising:
Pre-integration processing is carried out to the correlated variables of inertial navigation sensors IMU by way of decoupling, and processing resulting value is made
For the initial value of monocular VINS system.
4. according to the method described in claim 3, it is characterized in that, it is described by way of decoupling to inertial navigation sensors IMU
Correlated variables carry out pre-integration processing, comprising:
Pre-integration processing is carried out to the gyroscope zero bias of inertial navigation sensors IMU；
Pre-integration processing tentatively is carried out to scale factor and gravity direction in the case where not considering accelerometer bias；
Pre-integration processing is carried out to the accelerometer bias, and correction updates the scale factor and the gravity direction；
Pre-integration processing is carried out to linear speed.
5. according to the method described in claim 4, it is characterized in that, the method also includes:
According to the pre-integration value of the correlated variables of the IMU, the state variable of the IMU is designed in local map building thread
Local window, and construct corresponding local optimum factor graph, carry out state estimation；
According to the scale factor, pose optimization is carried out in closure winding thread, and constructs the factor of global nonlinear optimization
Figure carries out state estimation.
6. the method according to claim 1, wherein in the correlated variables to inertial navigation sensors IMU
Before being estimated, the method also includes:
Determine the time interval between adjacent two key frame；
If the time interval is greater than preset duration, adjacent two key frame corresponding with the time interval is deleted.
7. a kind of optimization device based on monocular VINS system characterized by comprising
The correlated variables estimation module of IMU, estimates for the correlated variables to inertial navigation sensors IMU, and will estimation
Initial value of the resulting value as monocular VINS system, the correlated variables of the IMU include the gyroscope zero bias of IMU, accelerometer
The movement speed of platform where zero bias and monocular VINS system；
First state estimation module, for establishing joint IMU in motion tracking thread and moving mould according to figure optimization method
Type carries out state estimation to the monocular VINS system.
8. device according to claim 7, which is characterized in that the first state estimation module, comprising:
First determines submodule, for determining whether monocular VINS system map updates in motion tracking thread；
Select submodule selects corresponding factor graph for whether updating according to the monocular VINS system map；
State estimation submodule, for carrying out state estimation to the monocular VINS system according to the factor graph.
9. a kind of computer readable storage medium, is stored thereon with computer program, which is characterized in that the program is held by processor
The step of any one of claim 1-6 the method is realized when row.
10. a kind of electronic equipment characterized by comprising
Memory is stored thereon with computer program；
Processor, for executing the computer program in the memory, to realize described in any one of claim 1-6
The step of method.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811642112.1A CN109798889A (en) | 2018-12-29 | 2018-12-29 | Optimization method, device, storage medium and electronic equipment based on monocular VINS system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811642112.1A CN109798889A (en) | 2018-12-29 | 2018-12-29 | Optimization method, device, storage medium and electronic equipment based on monocular VINS system |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN109798889A true CN109798889A (en) | 2019-05-24 |
Family
ID=66558290
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811642112.1A Pending CN109798889A (en) | 2018-12-29 | 2018-12-29 | Optimization method, device, storage medium and electronic equipment based on monocular VINS system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109798889A (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110717927A (en) * | 2019-10-10 | 2020-01-21 | 桂林电子科技大学 | Indoor robot motion estimation method based on deep learning and visual inertial fusion |
| CN112669196A (en) * | 2021-03-16 | 2021-04-16 | 浙江欣奕华智能科技有限公司 | Method and equipment for optimizing data by factor graph in hardware acceleration engine |
| CN112697158A (en) * | 2020-12-03 | 2021-04-23 | 南京工业大学 | Man-made loop-back instant positioning and picture building method and system for indoor and outdoor scenes |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20140333741A1 (en) * | 2013-05-08 | 2014-11-13 | Regents Of The University Of Minnesota | Constrained key frame localization and mapping for vision-aided inertial navigation |
| CN106197408A (en) * | 2016-06-23 | 2016-12-07 | 南京航空航天大学 | A kind of multi-source navigation data fusion method based on factor graph |
| US20170261324A1 (en) * | 2014-07-11 | 2017-09-14 | Regents Of The University Of Minnesota | Inverse sliding-window filters for vision-aided inertial navigation systems |
| CN107193279A (en) * | 2017-05-09 | 2017-09-22 | 复旦大学 | Robot localization and map structuring system based on monocular vision and IMU information |
| CN107869989A (en) * | 2017-11-06 | 2018-04-03 | 东北大学 | A kind of localization method and system of the fusion of view-based access control model inertial navigation information |
| CN108489482A (en) * | 2018-02-13 | 2018-09-04 | 视辰信息科技(上海)有限公司 | The realization method and system of vision inertia odometer |
| CN108827315A (en) * | 2018-08-17 | 2018-11-16 | 华南理工大学 | Vision inertia odometer position and orientation estimation method and device based on manifold pre-integration |
-
2018
- 2018-12-29 CN CN201811642112.1A patent/CN109798889A/en active Pending
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20140333741A1 (en) * | 2013-05-08 | 2014-11-13 | Regents Of The University Of Minnesota | Constrained key frame localization and mapping for vision-aided inertial navigation |
| US20170261324A1 (en) * | 2014-07-11 | 2017-09-14 | Regents Of The University Of Minnesota | Inverse sliding-window filters for vision-aided inertial navigation systems |
| CN106197408A (en) * | 2016-06-23 | 2016-12-07 | 南京航空航天大学 | A kind of multi-source navigation data fusion method based on factor graph |
| CN107193279A (en) * | 2017-05-09 | 2017-09-22 | 复旦大学 | Robot localization and map structuring system based on monocular vision and IMU information |
| CN107869989A (en) * | 2017-11-06 | 2018-04-03 | 东北大学 | A kind of localization method and system of the fusion of view-based access control model inertial navigation information |
| CN108489482A (en) * | 2018-02-13 | 2018-09-04 | 视辰信息科技(上海)有限公司 | The realization method and system of vision inertia odometer |
| CN108827315A (en) * | 2018-08-17 | 2018-11-16 | 华南理工大学 | Vision inertia odometer position and orientation estimation method and device based on manifold pre-integration |
Non-Patent Citations (5)
| Title |
|---|
| RAÚL MUR-ARTAL 等: "Visual-Inertial Monocular SLAM With Map Reuse", 《IEEE ROBOTICS AND AUTOMATION LETTERS》 * |
| 李庆峰: ""基于非线性优化的单目视觉与IMU融合的SLAM算法研究"", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
| 王京: ""基于传感器数据融合的单目视觉SLAM方法研究"", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
| 王琪: ""基于非线性优化的单目VINS系统的研究"", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
| 高翔 等: "《视觉SLAM十四讲 从理论到实践》", 31 March 2017, 电子工业出版社 * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110717927A (en) * | 2019-10-10 | 2020-01-21 | 桂林电子科技大学 | Indoor robot motion estimation method based on deep learning and visual inertial fusion |
| CN112697158A (en) * | 2020-12-03 | 2021-04-23 | 南京工业大学 | Man-made loop-back instant positioning and picture building method and system for indoor and outdoor scenes |
| CN112669196A (en) * | 2021-03-16 | 2021-04-16 | 浙江欣奕华智能科技有限公司 | Method and equipment for optimizing data by factor graph in hardware acceleration engine |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110118554A (en) | SLAM method, apparatus, storage medium and device based on visual inertia | |
| CN108549863B (en) | Human body gesture prediction method, apparatus, equipment and storage medium | |
| CN110246182A (en) | Vision-based global map positioning method and device, storage medium and equipment | |
| Pei et al. | Optimal heading estimation based multidimensional particle filter for pedestrian indoor positioning | |
| CN109798889A (en) | Optimization method, device, storage medium and electronic equipment based on monocular VINS system | |
| CN107888828A (en) | Space-location method and device, electronic equipment and storage medium | |
| US20130127904A1 (en) | Automatically Displaying Measurement Data Acquired by a Measurement System on a Mobile Device | |
| CN107478223A (en) | A kind of human body attitude calculation method based on quaternary number and Kalman filtering | |
| CN110118556A (en) | A kind of robot localization method and device based on covariance mixing together SLAM | |
| CN108235735A (en) | Positioning method and device, electronic equipment and computer program product | |
| CN107976193A (en) | A kind of pedestrian's flight path estimating method, device, flight path infer equipment and storage medium | |
| RU2708027C1 (en) | Method of transmitting motion of a subject from a video to an animated character | |
| CN108957512A (en) | Positioning device and method and automatic running device | |
| CN109211277A (en) | The state of vision inertia odometer determines method, apparatus and electronic equipment | |
| CN109461208A (en) | Three-dimensional map processing method, device, medium and calculating equipment | |
| CN107462260A (en) | A kind of trace generator method, apparatus and wearable device | |
| CN111680747B (en) | Method and apparatus for closed loop detection of occupancy grid subgraphs | |
| CN110310304A (en) | Monocular vision builds figure and localization method, device, storage medium and mobile device | |
| CN108332758A (en) | A kind of corridor recognition method and device of mobile robot | |
| CN108235809A (en) | End cloud combination positioning method and device, electronic equipment and computer program product | |
| CN108020813A (en) | Localization method, positioner and electronic equipment | |
| Dong et al. | Robust mobile computing framework for visualization of simulated processes in augmented reality | |
| CN107782304A (en) | The localization method and device of mobile robot, mobile robot and storage medium | |
| US11245763B2 (en) | Data processing method, computer device and storage medium | |
| CN108829996A (en) | Obtain the method and device of vehicle location information |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20190524 |