CN112798010A - Initialization method and device for VIO system of visual inertial odometer - Google Patents
Initialization method and device for VIO system of visual inertial odometer Download PDFInfo
- Publication number
- CN112798010A CN112798010A CN201911107857.2A CN201911107857A CN112798010A CN 112798010 A CN112798010 A CN 112798010A CN 201911107857 A CN201911107857 A CN 201911107857A CN 112798010 A CN112798010 A CN 112798010A
- Authority
- CN
- China
- Prior art keywords
- initialization
- imu
- image data
- static
- zero
- 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.)
- Granted
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Image Analysis (AREA)
- Navigation (AREA)
Abstract
The application discloses an initialization method and device of a visual inertial odometer VIO system. The method comprises the following steps: recording IMU information reported by the IMU and image data reported by the camera according to a time sequence; performing zero-speed detection on the system according to the IMU information and the image data; judging whether the system is suitable for static initialization or dynamic initialization according to the result of the zero-speed detection of the system; and according to the result that the system is suitable for static initialization or dynamic initialization, realizing the static initialization or the dynamic initialization of the system based on the IMU information and the image data. The scheme enables the initialization of the VIO system to cover more scenes, and the initialization of the VIO system can be completed no matter the initial state is a dynamic state or a static state, so that the robustness of the initialization link of the system is better.
Description
Technical Field
The application relates to the technical field of visual positioning navigation, in particular to an initialization method and device of a Visual Inertial Odometer (VIO) system.
Background
Currently, the visual positioning navigation technology is beginning to be applied to unmanned aerial vehicles or automatic driving, robots and other specific technologies. In the visual positioning navigation technology, the visual inertial odometer VIO is an important technology. In use, the visual inertial odometer VIO needs to be initialized. The related initialization method further comprises an initialization scheme based on nonlinear optimization and an initialization scheme based on filtering. The initialization scheme based on the nonlinear optimization requires that the equipment is in a motion state in an initialization link and requires enough motion excitation, and if the system is in a static state, the initialization cannot be completed. In the filtering scheme-based VIO system (frames such as MSCKF, Mono-SLAM, etc.), the equipment needs to be in a static state in the initialization link. When the system is in a static state, the IMU accelerometer output value can be used in the filtering scheme to directly solve the system attitude, at the moment, the speed and the displacement can be set to be zero, and the covariance can be set to be a default value. And if the system is in a motion state, the initialization scheme in the filtering scheme cannot be performed.
For example, the VIOSDK scheme of the high traffic company uses a filtering scheme, if initialization is required when the device is in motion, for example, when the VIO system needs to be initialized again after diverging during the motion, the device must be forced to be in a static state to complete the initialization, otherwise, the system fails.
Disclosure of Invention
In view of the above, the present application has been developed to provide a method and apparatus for initializing a visual inertial odometer, VIO, system that overcomes or at least partially solves the above-mentioned problems.
In accordance with one aspect of the present application, there is provided a method of initializing a visual inertial odometer, VIO, system, the system comprising a single inertial measurement unit, IMU, and a single camera, the method comprising:
recording IMU information reported by the IMU and image data reported by the camera according to a time sequence;
performing zero-speed detection on the system according to the IMU information and the image data;
judging whether the system is suitable for static initialization or dynamic initialization according to the result of the zero-speed detection of the system;
and according to the result that the system is suitable for static initialization or dynamic initialization, realizing the static initialization or the dynamic initialization of the system based on the IMU information and the image data.
Optionally, the performing zero-speed detection on the system according to the IMU information and the image data includes:
and sliding a time sequence sliding window with a first length according to the time sequence by a first sliding step length until the IMU information and the image data in the time sequence sliding window with the first length meet the zero-speed detection condition.
Optionally, the determining that the system is suitable for static initialization or dynamic initialization according to the result of the zero-speed detection on the system includes:
and judging whether the system is in a static state or not according to the IMU information and the image data meeting the zero-speed detection condition, if so, the system is suitable for static initialization, and otherwise, the system is suitable for dynamic initialization.
Optionally, the determining whether the system is in a static state according to the IMU information and the image data meeting the zero-speed detection condition includes any one or more of the following conditions:
counting the variation upper and lower limit amplitude of the x value and/or y value of each characteristic point in the image according to the IMU and the image data, calculating the average amplitude of all the characteristic points according to the variation upper and lower limit amplitude, and if the average amplitude is larger than a preset threshold value, enabling the system to be in a motion state;
calculating a mean value of a three-axis accelerometer and/or a three-axis modular length of the IMU, wherein if the mean value is larger than or smaller than a preset threshold value, the system is in a motion state;
calculating a three-axis accelerometer and/or a three-axis modular length variance of the IMU, wherein if any axis variance is larger than a preset threshold value of the axis variance, the system is in a motion state;
and if the above conditions cannot be met, the system is in a static state.
Optionally, the implementing static initialization of the system includes:
the system performs a zero-speed update, closes the dynamic initialization logic, and begins outputting positioning information.
Optionally, the implementing of the dynamic initialization of the system includes:
if the system is in a motion state during the time sequence sliding window with the first length, sliding the time sequence sliding window with the second length according to the time sequence by a second sliding step length until the IMU information and the image data in the time sequence sliding window with the second length meet the dynamic initialization condition, wherein the second length is larger than the first length, and then calculating the data for the dynamic initialization of the system according to the IMU information and the image data in the time sequence sliding window with the second length.
Optionally, the calculating data for the dynamic initialization of the system according to the IMU information and the image data in the time series sliding window of the second length includes:
modeling multi-frame common-view feature points in the collected multi-frame images, and obtaining rotation and translation among the frames of images by using SFM;
and performing pre-integration on the IMU information in each interval of the time interval according to the image time interval, aligning the rotation and translation of the IMU in each interval with the rotation and translation corresponding to the image, and acquiring state data at each moment through a fitting algorithm for initializing the system.
Optionally, the data for dynamically initializing the system includes any one or more of the following items: three-dimensional displacement, three-dimensional speed, three-dimensional attitude, IMU zero offset, accelerometer zero offset, roll angle, pitch angle, scale factor of the initial reference frame in a world coordinate system, and coordinates of the characteristic points relative to the initial reference frame.
According to another aspect of the present application, there is provided an apparatus for initializing a visual inertial odometer, VIO, system, the system comprising a single inertial measurement unit, IMU, and a single camera, the apparatus comprising:
the recording unit is suitable for recording IMU information reported by the IMU and image data reported by the camera according to a time sequence;
the zero-speed detection unit is suitable for carrying out zero-speed detection on the system according to the IMU information and the image data;
the judging unit is suitable for judging that the system is suitable for static initialization or dynamic initialization according to the result of the zero-speed detection of the system;
and the initialization unit is suitable for realizing the static initialization of the system or realizing the dynamic initialization of the system based on the IMU information and the image data according to the result that the system is suitable for the static initialization or the dynamic initialization.
In accordance with yet another aspect of the present application, there is provided an electronic device including: a processor; and a memory arranged to store computer executable instructions that, when executed, cause the processor to perform a method as any one of the above.
According to a further aspect of the application, there is provided a computer readable storage medium, wherein the computer readable storage medium stores one or more programs which, when executed by a processor, implement a method as in any above.
From the above, the initialization scheme of the visual inertial odometer VIO system disclosed in the present application includes: recording IMU information reported by the IMU and image data reported by the camera according to a time sequence; performing zero-speed detection on the system according to the IMU information and the image data; judging whether the system is suitable for static initialization or dynamic initialization according to the result of the zero-speed detection of the system; and according to the result that the system is suitable for static initialization or dynamic initialization, realizing the static initialization or the dynamic initialization of the system based on the IMU information and the image data. The scheme enables the initialization of the VIO system to cover more scenes, and the initialization of the VIO system can be completed no matter the initial state is a dynamic state or a static state, so that the robustness of the initialization link of the system is better.
The foregoing description is only an overview of the technical solutions of the present application, and the present application can be implemented according to the content of the description in order to make the technical means of the present application more clearly understood, and the following detailed description of the present application is given in order to make the above and other objects, features, and advantages of the present application more clearly understandable.
Drawings
Various other advantages and benefits will become apparent to those of ordinary skill in the art upon reading the following detailed description of the preferred embodiments. The drawings are only for purposes of illustrating the preferred embodiments and are not to be construed as limiting the application. Also, like reference numerals are used to refer to like parts throughout the drawings. In the drawings:
FIG. 1 shows a schematic flow diagram of a method of initializing a visual-inertial odometer VIO system according to one embodiment of the present application;
FIG. 2 shows a schematic structural diagram of an initialization apparatus of a visual-inertial odometer VIO system according to an embodiment of the present application;
FIG. 3 shows a schematic structural diagram of an electronic device according to an embodiment of the present application;
FIG. 4 shows a schematic structural diagram of a computer-readable storage medium according to an embodiment of the present application;
FIG. 5 illustrates a VIO system initialization framework diagram according to one embodiment of the present application;
FIG. 6 shows a time-sequential sliding window schematic according to an embodiment of the present application;
fig. 7 illustrates a timing diagram of a VIO system initialization process according to one embodiment of the present application.
Detailed Description
Exemplary embodiments of the present application will be described in more detail below with reference to the accompanying drawings. While exemplary embodiments of the present application are shown in the drawings, it should be understood that the present application may be embodied in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the scope of the disclosure to those skilled in the art.
Some keywords in this application are defined as follows:
VIO: visual inertia odometer.
VISLAM: visual Inertial positioning And Mapping.
SFM: structure From Motion, Motion recovery Structure.
An IMU: inertial Measurement Unit, generally, an IMU contains three single-axis accelerometers and three single-axis gyroscopes.
MSCKF: Multi-State Constraint Kalman Filter.
To some extent, VIO and VISLAM may be considered to be the same thing: the visual inertial odometer is hereinafter referred to as VIO.
FIG. 1 shows a schematic flow diagram of a method of initializing a visual-inertial odometer VIO system according to one embodiment of the present application; the VIO system comprises an Inertial Measurement Unit (IMU) and a camera, and the method comprises the following steps:
and step S110, recording IMU information reported by the IMU and image data reported by the camera according to the time sequence.
The initialization of the system needs to acquire IMU information and image data of initial time, and relevant state information of the system is analyzed according to the time data so as to realize initialization.
And step S120, carrying out zero-speed detection on the system according to the IMU information and the image data.
Referring to fig. 5, in order to implement the initialization scheme of this embodiment, zero-speed detection needs to be introduced before the original framework static initialization module, and it is recognized whether the current system is suitable for dynamic initialization or static initialization.
Step S130, according to the result of the zero-speed detection of the system, judging that the system is suitable for static initialization or dynamic initialization.
In a specific implementation, the IMU information and image data recorded in a time series sliding window of a certain length may be collected in time sequence, so as to determine whether the system is suitable for static initialization or dynamic initialization according to the data or information. For example, a dynamic initialization module in a nonlinear optimization scheme can be introduced on the basis of an original static initialization module of the MSCKF based on a filtering scheme, so that the initialization mode of the system is judged and switched according to the time sequence window sliding mode.
Step S140, implementing static initialization of the system or implementing dynamic initialization of the system based on the IMU information and the image data according to the result of judging that the system is suitable for static initialization or dynamic initialization.
According to the steps, static initialization or dynamic initialization can be adopted according to the self-adaptive identification of the zero-speed detection unit. And finally, realizing the initialization operation of the system according to the judgment or identification result.
In short, the steps disclosed in this embodiment implement switching between dynamic initialization and static initialization by adding zero-speed determination in the system and implementing dynamic initialization and static initialization at the same time, thereby meeting the requirements of different scenes of the system and enhancing the robustness of the system.
In one embodiment, step S120 includes: and sliding a time sequence sliding window with a first length according to the time sequence by a first sliding step length until the IMU information and the image data in the time sequence sliding window with the first length meet the zero-speed detection condition.
In order to be able to implement the adaptive function of initialization, in this embodiment, a timing selection or adjustment is implemented by means of a timing window sliding, as shown in fig. 6.
In a specific implementation, according to the initialization flow timing chart disclosed in fig. 7, the time sequence of the first length may be preset to be t0-t4, that is, zero-speed detection is performed when five frames of images are acquired, and dynamic or static initialization processing is performed according to the result of the zero-speed detection. For example, at time t0, at the initial stage of power-on, or at the beginning of system restart, the static initialization may be performed by the single-frame IMU to complete the static initialization logic, and then the system starts the normal state estimation of the VIO, at which time no positioning information is output, because the initialization is not completed at this time. When the time goes to t4, the system can perform zero speed detection. Of course, if the data or information recorded at t0-t4 does not satisfy the requirements, the window of the above length can be slipped one time series at a time to obtain the appropriate data.
In one embodiment, the step S130 includes: and judging whether the system is in a static state or not according to the IMU information and the image data meeting the zero-speed detection condition, if so, the system is suitable for static initialization, and otherwise, the system is suitable for dynamic initialization.
Namely, the system is judged to be in a static state according to the IMU information and the image in the first time sequence length, static initialization is started, and otherwise, dynamic initialization is started.
In an embodiment, the condition for determining in step S130 includes any one or more of the following:
counting the variation upper and lower limit amplitude of the x value and/or y value of each characteristic point in the image according to the IMU and the image data, calculating the average amplitude of all the characteristic points according to the variation upper and lower limit amplitude, and if the average amplitude is larger than a preset threshold value, enabling the system to be in a motion state;
calculating the mean value of the three-axis accelerometer and/or the three-axis module length of the IMU, and if the mean value is larger than or smaller than a preset threshold value, enabling the system to be in a motion state;
calculating a three-axis accelerometer and/or a three-axis modular length variance of the IMU, and if any axis variance is larger than a preset threshold value of the axis variance, enabling the system to be in a motion state;
and if the above conditions cannot be met, the system is in a static state.
In a specific implementation, a zero-speed detector may be set, and if the calculated value is greater than or less than a preset threshold, the value of the zero-speed detector is added by 1, otherwise, the value of the zero-speed detector is unchanged, and if the final zero-speed detector is not 0, the system is in a motion state, otherwise, the system is in a static state.
In a specific embodiment, the step S140 includes: the system performs a zero-speed update, closes the dynamic initialization logic, and begins outputting positioning information.
In order to obtain an accurate calculation result, if the system is judged to be in a static state, the data recorded in the time sequence window is subjected to zero-speed updating, meanwhile, the dynamic initialization logic is closed, the static initialization is completed, and the positioning information is started to be output.
In a specific embodiment, the step S140 further includes: if the system is in a motion state during the time sequence sliding window with the first length, sliding the time sequence sliding window with the second length according to the time sequence by a second sliding step length until the IMU information and the image data in the time sequence sliding window with the second length meet the dynamic initialization condition, wherein the second length is larger than the first length, and then calculating the data for the dynamic initialization of the system according to the IMU information and the image data in the time sequence sliding window with the second length.
According to fig. 7, if it is determined at time t4 that the system is in a dynamic state, it may be determined that the static initialization procedure performed before cannot work normally, at this time, the static initialization is exited, and when the time reaches t9, the number of the sliding windows for dynamic initialization has met the requirement, at this time, the dynamic initialization logic is executed. And (3) jointly resolving the system state at the time t9 by using the IMU data and the visual information, wherein the resolved initial state can be used for initializing the MSCKF system state, and the covariance is set as a preset default value. At this point dynamic initialization is complete.
According to experiments, the first length may range from 4 to 6 and the second length may range from 8 to 12, the first length preferably being 5 according to fig. 7, typically from t0 to t 4; the second length is preferably 10, and is typically t0-t 9. If the data recorded in the initial window cannot meet the calculation requirement, the sliding can be performed in a manner of moving by one time unit each time, wherein the first step length and the second step length can be the same or different.
In a specific embodiment, the implementing dynamic initialization in step S140 further includes:
modeling multi-frame common-view feature points in the collected multi-frame images, and obtaining rotation and translation among the frames of images by using SFM;
and performing pre-integration on IMU information in each interval of the time interval according to the image time interval, aligning the rotation and translation of the IMU in each interval with the rotation and translation corresponding to the image, and acquiring state data at each moment through a fitting algorithm for initializing the system.
In a specific implementation, in the initialization based on the non-linear optimization scheme, the VIO system must have enough motion excitation to enable the VIO algorithm to be successfully initialized, and the specific steps include:
the image data and IMU information acquisition is realized in a segmented manner, namely, each frame of image is acquired along with multi-frame IMU data, because the frequency of the IMU data is far higher than that of the image, preferably, the acquisition moments are t0-t9 respectively; and then modeling the multi-frame common-view feature points of the acquired images through a pure visual scheme, and obtaining rotation and translation among the images by using the SFM, wherein the translation obtained by the SFM has no scale information because the SFM is data acquired by a monocular camera.
Cutting the IMU data into 9 parts according to the image time interval, pre-integrating the IMU data in the interval, aligning the rotation of the IMU in the 9 parts with the rotation corresponding to the image after pre-integrating, and fitting the zero offset of the gyroscope in the time period by using a least square technology; and finally, aligning the displacement integral of the IMU in 9 parts with the non-scale displacement of the image, and fitting the three-dimensional displacement, the three-dimensional speed (m/s), the roll angle, the pitch angle and the scale factor of the initial reference frame relative to the world coordinate system and the coordinate of the characteristic point relative to the initial reference frame at 10 image acquisition moments. Accordingly, initial state information required to perform dynamic initialization is obtained.
In a specific embodiment, the IMU information and image data within the time-series sliding window of the second length includes any one or more of: three-dimensional displacement, three-dimensional speed, three-dimensional attitude, IMU zero offset, accelerometer zero offset, roll angle, pitch angle, scale factor of the initial reference frame in a world coordinate system, and coordinates of the characteristic points relative to the initial reference frame.
FIG. 2 shows a schematic structural diagram of an initialization apparatus of a visual-inertial odometer VIO system according to an embodiment of the present application; the VIO system includes an inertial measurement unit IMU and a camera, the apparatus 200 including:
the recording unit 210 is adapted to record IMU information reported by the IMU and image data reported by the camera in time series.
The initialization of the system needs to acquire IMU information and image data of initial time, and relevant state information of the system is analyzed according to the time data so as to realize initialization.
A zero-speed detection unit 220 adapted to perform a zero-speed detection on the system according to the IMU information and the image data.
Referring to fig. 5, in order to implement the initialization scheme of this embodiment, zero-speed detection needs to be introduced before the original framework static initialization module, and it is recognized whether the current system is suitable for dynamic initialization or static initialization.
The judging unit 230 is adapted to judge that the system is suitable for static initialization or dynamic initialization according to the result of the zero-speed detection on the system.
In a specific implementation, the IMU information and image data recorded in a time series sliding window of a certain length may be collected in time sequence, so as to determine whether the system is suitable for static initialization or dynamic initialization according to the data or information. For example, a dynamic initialization module in a nonlinear optimization scheme can be introduced on the basis of an original static initialization module of the MSCKF based on a filtering scheme, so that the initialization mode of the system is judged and switched according to the time sequence window sliding mode.
An initialization unit 240 adapted to implement static initialization of the system or implement dynamic initialization of the system based on the IMU information and the image data according to the result of the determination that the system is suitable for static initialization or dynamic initialization.
The unit can adaptively identify whether to adopt static initialization or dynamic initialization according to the zero-speed detection unit. And finally, realizing the initialization operation of the system according to the judgment or identification result.
In short, the device disclosed in this embodiment realizes the switching between the dynamic initialization and the static initialization by adding the zero-speed judgment in the system and simultaneously realizing the dynamic initialization and the static initialization, thereby meeting the requirements of different scenes of the system and enhancing the robustness of the system.
In a particular embodiment, the zero-speed detection unit 220 is adapted to: and sliding a time sequence sliding window with a first length according to the time sequence by a first sliding step length until the IMU information and the image data in the time sequence sliding window with the first length meet the zero-speed detection condition.
In one embodiment, the determination unit is adapted to: and judging whether the system is in a static state or not according to the IMU information and the image data meeting the zero-speed detection condition, if so, the system is suitable for static initialization, and otherwise, the system is suitable for dynamic initialization.
In one embodiment, the conditions for the zero-speed detection unit 220 to determine include any one or more of the following:
counting the variation upper and lower limit amplitude of the x value and/or y value of each characteristic point in the image according to the IMU and the image data, calculating the average amplitude of all the characteristic points according to the variation upper and lower limit amplitude, and if the average amplitude is larger than a preset threshold value, enabling the system to be in a motion state;
calculating the mean value of the three-axis accelerometer and/or the three-axis module length of the IMU, and if the mean value is larger than or smaller than a preset threshold value, enabling the system to be in a motion state;
calculating a three-axis accelerometer and/or a three-axis modular length variance of the IMU, and if any axis variance is larger than a preset threshold value of the axis variance, enabling the system to be in a motion state;
and if the above conditions cannot be met, the system is in a static state.
In a specific embodiment, the initialization unit 240 is adapted to: the system performs a zero-speed update, closes the dynamic initialization logic, and begins outputting positioning information.
In a specific embodiment, the initialization unit 240 is adapted to: if the system is in a motion state during the time sequence sliding window with the first length, sliding the time sequence sliding window with the second length according to the time sequence by a second sliding step length until the IMU information and the image data in the time sequence sliding window with the second length meet the dynamic initialization condition, wherein the second length is larger than the first length, and then calculating the data for the dynamic initialization of the system according to the IMU information and the image data in the time sequence sliding window with the second length.
In a specific embodiment, the initialization unit 240 is further adapted to:
modeling multi-frame common-view feature points in the collected multi-frame images, and obtaining rotation and translation among the frames of images by using SFM;
pre-integrating IMU information in intervals according to image time intervals, aligning rotation and displacement of the IMU in each interval with rotation and displacement corresponding to an image, and acquiring state data at each moment through a fitting algorithm for initializing the system.
In a specific embodiment, the IMU information and image data within the time-series sliding window of the second length includes any one or more of: three-dimensional displacement, three-dimensional speed, three-dimensional attitude, IMU zero offset, accelerometer zero offset, roll angle, pitch angle, scale factor of the initial reference frame in a world coordinate system, and coordinates of the characteristic points relative to the initial reference frame.
In summary, the initialization scheme of the visual inertial odometer VIO system disclosed in the present application specifically includes: recording IMU information reported by the IMU and image data reported by the camera according to a time sequence; performing zero-speed detection on the system according to the IMU information and the image data; judging whether the system is suitable for static initialization or dynamic initialization according to the result of the zero-speed detection of the system; and according to the result that the system is suitable for static initialization or dynamic initialization, realizing the static initialization or the dynamic initialization of the system based on the IMU information and the image data. The scheme enables the initialization of the VIO system to cover more scenes, and the initialization of the VIO system can be completed no matter the initial state is a dynamic state or a static state, so that the robustness of the initialization link of the system is better.
It should be noted that:
the algorithms and displays presented herein are not inherently related to any particular computer, virtual machine, or other apparatus. Various general purpose devices may be used with the teachings herein. The required structure for constructing such a device will be apparent from the description above. In addition, this application is not directed to any particular programming language. It will be appreciated that a variety of programming languages may be used to implement the teachings of the present application as described herein, and any descriptions of specific languages are provided above to disclose the best modes of the present application.
In the description provided herein, numerous specific details are set forth. However, it is understood that embodiments of the application may be practiced without these specific details. In some instances, well-known methods, structures and techniques have not been shown in detail in order not to obscure an understanding of this description.
Similarly, it should be appreciated that in the foregoing description of exemplary embodiments of the application, various features of the application are sometimes grouped together in a single embodiment, figure, or description thereof for the purpose of streamlining the application and aiding in the understanding of one or more of the various application aspects. However, the disclosed method should not be interpreted as reflecting an intention that: this application is intended to cover such departures from the present disclosure as come within known or customary practice in the art to which this invention pertains. Rather, as the following claims reflect, application is directed to less than all features of a single foregoing disclosed embodiment. Thus, the claims following the detailed description are hereby expressly incorporated into this detailed description, with each claim standing on its own as a separate embodiment of this application.
Those skilled in the art will appreciate that the modules in the device in an embodiment may be adaptively changed and disposed in one or more devices different from the embodiment. The modules or units or components of the embodiments may be combined into one module or unit or component, and furthermore they may be divided into a plurality of sub-modules or sub-units or sub-components. All of the features disclosed in this specification (including any accompanying claims, abstract and drawings), and all of the processes or elements of any method or apparatus so disclosed, may be combined in any combination, except combinations where at least some of such features and/or processes or elements are mutually exclusive. Each feature disclosed in this specification (including any accompanying claims, abstract and drawings) may be replaced by alternative features serving the same, equivalent or similar purpose, unless expressly stated otherwise.
Furthermore, those skilled in the art will appreciate that while some embodiments described herein include some features included in other embodiments, rather than other features, combinations of features of different embodiments are meant to be within the scope of the application and form different embodiments. For example, in the following claims, any of the claimed embodiments may be used in any combination.
The various component embodiments of the present application may be implemented in hardware, or in software modules running on one or more processors, or in a combination thereof. Those skilled in the art will appreciate that a microprocessor or Digital Signal Processor (DSP) may be used in practice to implement some or all of the functions of some or all of the components of the initialization apparatus of the visual inertial odometer VIO system according to embodiments of the present application. The present application may also be embodied as apparatus or device programs (e.g., computer programs and computer program products) for performing a portion or all of the methods described herein. Such programs implementing the present application may be stored on a computer readable medium or may be in the form of one or more signals. Such a signal may be downloaded from an internet website or provided on a carrier signal or in any other form.
For example, fig. 3 shows a schematic structural diagram of an electronic device according to an embodiment of the present application. The electronic device 300 comprises a processor 310 and a memory 320 arranged to store computer executable instructions (computer readable program code). The memory 320 may be an electronic memory such as a flash memory, an EEPROM (electrically erasable programmable read only memory), an EPROM, a hard disk, or a ROM. The memory 320 has a storage space 330 storing computer readable program code 331 for performing any of the method steps described above. For example, the storage space 330 for storing the computer readable program code may comprise respective computer readable program codes 331 for respectively implementing various steps in the above method. The computer readable program code 331 may be read from or written to one or more computer program products. These computer program products comprise a program code carrier such as a hard disk, a Compact Disc (CD), a memory card or a floppy disk. Such a computer program product is typically a computer readable storage medium such as described in fig. 4. FIG. 4 shows a schematic structural diagram of a computer-readable storage medium according to an embodiment of the present application. The computer readable storage medium 400 has stored thereon a computer readable program code 331 for performing the steps of the method according to the application, readable by a processor 310 of an electronic device 300, which computer readable program code 331, when executed by the electronic device 300, causes the electronic device 300 to perform the steps of the method described above, in particular the computer readable program code 331 stored on the computer readable storage medium may perform the method shown in any of the embodiments described above. The computer readable program code 331 may be compressed in a suitable form.
It should be noted that the above-mentioned embodiments illustrate rather than limit the application, and that those skilled in the art will be able to design alternative embodiments without departing from the scope of the appended claims. In the claims, any reference signs placed between parentheses shall not be construed as limiting the claim. The word "comprising" does not exclude the presence of elements or steps not listed in a claim. The word "a" or "an" preceding an element does not exclude the presence of a plurality of such elements. The application may be implemented by means of hardware comprising several distinct elements, and by means of a suitably programmed computer. In the unit claims enumerating several means, several of these means may be embodied by one and the same item of hardware. The usage of the words first, second and third, etcetera do not indicate any ordering. These words may be interpreted as names.
Claims (11)
1. A method of initializing a visual inertial odometer, VIO, system, the system including a single inertial measurement unit, IMU, and a single camera, the method comprising:
recording IMU information reported by the IMU and image data reported by the camera according to a time sequence;
performing zero-speed detection on the system according to the IMU information and the image data;
judging whether the system is suitable for static initialization or dynamic initialization according to the result of the zero-speed detection of the system;
and according to the result that the system is suitable for static initialization or dynamic initialization, realizing the static initialization or the dynamic initialization of the system based on the IMU information and the image data.
2. The method of claim 1, wherein the stall detecting the system from the IMU information and the image data comprises:
and sliding a time sequence sliding window with a first length according to the time sequence by a first sliding step length until the IMU information and the image data in the time sequence sliding window with the first length meet the zero-speed detection condition.
3. The method of claim 2, wherein said determining that the system is suitable for static initialization or dynamic initialization based on the result of the stall detection on the system comprises:
and judging whether the system is in a static state or not according to the IMU information and the image data meeting the zero-speed detection condition, if so, the system is suitable for static initialization, and otherwise, the system is suitable for dynamic initialization.
4. The method as claimed in claim 3, wherein said determining whether the system is in a still state according to the IMU information and the image data satisfying a zero-velocity detection condition comprises any one or more of the following conditions:
counting the variation upper and lower limit amplitude of the x value and/or y value of each characteristic point in the image according to the IMU and the image data, calculating the average amplitude of all the characteristic points according to the variation upper and lower limit amplitude, and if the average amplitude is larger than a preset threshold value, enabling the system to be in a motion state;
calculating a mean value of a three-axis accelerometer and/or a three-axis modular length of the IMU, wherein if the mean value is larger than or smaller than a preset threshold value, the system is in a motion state;
calculating a three-axis accelerometer and/or a three-axis modular length variance of the IMU, wherein if any axis variance is larger than a preset threshold value of the axis variance, the system is in a motion state;
and if the above conditions cannot be met, the system is in a static state.
5. The method of claim 1, wherein the implementing the static initialization of the system comprises:
the system performs a zero-speed update, closes the dynamic initialization logic, and begins outputting positioning information.
6. The method of claim 3, wherein the implementing the dynamic initialization of the system comprises:
if the system is in a motion state during the time sequence sliding window with the first length, sliding the time sequence sliding window with the second length according to the time sequence by a second sliding step length until the IMU information and the image data in the time sequence sliding window with the second length meet the dynamic initialization condition, wherein the second length is larger than the first length, and then calculating the data for the dynamic initialization of the system according to the IMU information and the image data in the time sequence sliding window with the second length.
7. The method of claim 6, wherein the resolving data for dynamic initialization of the system from the IMU information and the image data within the time-series sliding window of the second length comprises:
modeling multi-frame common-view feature points in the collected multi-frame images, and obtaining rotation and translation among the frames of images by using SFM;
and performing pre-integration on the IMU information in each interval of the time interval according to the image time interval, aligning the rotation and translation of the IMU in each interval with the rotation and translation corresponding to the image, and acquiring state data at each moment through a fitting algorithm for initializing the system.
8. A method according to claim 6 or 7, wherein the data for dynamic initialization of the system comprises any one or more of:
three-dimensional displacement, three-dimensional speed, three-dimensional attitude, IMU zero offset, accelerometer zero offset, roll angle, pitch angle, scale factor of the initial reference frame in a world coordinate system, and coordinates of the characteristic points relative to the initial reference frame.
9. An apparatus for initializing a visual inertial odometer, VIO, system, the system comprising a single inertial measurement unit, IMU, and a single camera, the apparatus comprising:
the recording unit is suitable for recording IMU information reported by the IMU and image data reported by the camera according to a time sequence;
the zero-speed detection unit is suitable for carrying out zero-speed detection on the system according to the IMU information and the image data;
the judging unit is suitable for judging that the system is suitable for static initialization or dynamic initialization according to the result of the zero-speed detection of the system;
and the initialization unit is suitable for realizing the static initialization of the system or realizing the dynamic initialization of the system based on the IMU information and the image data according to the result that the system is suitable for the static initialization or the dynamic initialization.
10. An electronic device, wherein the electronic device comprises: a processor; and a memory arranged to store computer-executable instructions that, when executed, cause the processor to perform the method of any one of claims 1-8.
11. A computer readable storage medium, wherein the computer readable storage medium stores one or more programs which, when executed by a processor, implement the method of any of claims 1-8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911107857.2A CN112798010B (en) | 2019-11-13 | 2019-11-13 | Initializing method and device of VIO system of visual inertial odometer |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911107857.2A CN112798010B (en) | 2019-11-13 | 2019-11-13 | Initializing method and device of VIO system of visual inertial odometer |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112798010A true CN112798010A (en) | 2021-05-14 |
| CN112798010B CN112798010B (en) | 2023-05-09 |
Family
ID=75803330
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911107857.2A Active CN112798010B (en) | 2019-11-13 | 2019-11-13 | Initializing method and device of VIO system of visual inertial odometer |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112798010B (en) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113670327A (en) * | 2021-08-11 | 2021-11-19 | 影石创新科技股份有限公司 | Visual inertial odometer initialization method, device, equipment and storage medium |
| CN114964230A (en) * | 2022-05-12 | 2022-08-30 | 北京自动化控制设备研究所 | Vehicle-mounted integrated navigation gyro drift correction method |
| WO2023087681A1 (en) * | 2021-11-16 | 2023-05-25 | 上海商汤智能科技有限公司 | Positioning initialization method and apparatus, and computer-readable storage medium and computer program product |
| CN116429094A (en) * | 2023-06-15 | 2023-07-14 | 小米汽车科技有限公司 | Positioning method, positioning device, electronic equipment and storage medium |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102865881A (en) * | 2012-03-06 | 2013-01-09 | 武汉大学 | Quick calibration method for inertial measurement unit |
| CN103616030A (en) * | 2013-11-15 | 2014-03-05 | 哈尔滨工程大学 | Autonomous navigation system positioning method based on strapdown inertial navigation resolving and zero-speed correction |
| US20150204674A1 (en) * | 2012-09-27 | 2015-07-23 | Rafael Advanced Defense Systems Ltd. | Inertial Navigation System and Method |
| CN104977001A (en) * | 2014-04-02 | 2015-10-14 | 北京自动化控制设备研究所 | Relative navigation method applied to individual indoor navigation system |
| CN106153069A (en) * | 2015-03-31 | 2016-11-23 | 日本电气株式会社 | Attitude rectification apparatus and method in autonomous navigation system |
| US20180274940A1 (en) * | 2015-10-13 | 2018-09-27 | Shanghai Huace Navigation Technology Ltd | A method for initial alignment of an inertial navigation apparatus |
| CN109238277A (en) * | 2018-08-01 | 2019-01-18 | 清华大学 | The localization method and device of vision inertial data depth integration |
| CN110057381A (en) * | 2019-03-20 | 2019-07-26 | 深圳市元征科技股份有限公司 | A kind of zero velocity update method and system of navigation system |
-
2019
- 2019-11-13 CN CN201911107857.2A patent/CN112798010B/en active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102865881A (en) * | 2012-03-06 | 2013-01-09 | 武汉大学 | Quick calibration method for inertial measurement unit |
| US20140372063A1 (en) * | 2012-03-06 | 2014-12-18 | Wuhan University | Quick calibration method for inertial measurement unit |
| US20150204674A1 (en) * | 2012-09-27 | 2015-07-23 | Rafael Advanced Defense Systems Ltd. | Inertial Navigation System and Method |
| CN103616030A (en) * | 2013-11-15 | 2014-03-05 | 哈尔滨工程大学 | Autonomous navigation system positioning method based on strapdown inertial navigation resolving and zero-speed correction |
| CN104977001A (en) * | 2014-04-02 | 2015-10-14 | 北京自动化控制设备研究所 | Relative navigation method applied to individual indoor navigation system |
| CN106153069A (en) * | 2015-03-31 | 2016-11-23 | 日本电气株式会社 | Attitude rectification apparatus and method in autonomous navigation system |
| US20180274940A1 (en) * | 2015-10-13 | 2018-09-27 | Shanghai Huace Navigation Technology Ltd | A method for initial alignment of an inertial navigation apparatus |
| CN109238277A (en) * | 2018-08-01 | 2019-01-18 | 清华大学 | The localization method and device of vision inertial data depth integration |
| CN110057381A (en) * | 2019-03-20 | 2019-07-26 | 深圳市元征科技股份有限公司 | A kind of zero velocity update method and system of navigation system |
Non-Patent Citations (1)
| Title |
|---|
| 贾亮等: "基于MEMS惯性传感器零速检测算法的研究", 《电脑与信息技术》 * |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113670327A (en) * | 2021-08-11 | 2021-11-19 | 影石创新科技股份有限公司 | Visual inertial odometer initialization method, device, equipment and storage medium |
| WO2023087681A1 (en) * | 2021-11-16 | 2023-05-25 | 上海商汤智能科技有限公司 | Positioning initialization method and apparatus, and computer-readable storage medium and computer program product |
| CN114964230A (en) * | 2022-05-12 | 2022-08-30 | 北京自动化控制设备研究所 | Vehicle-mounted integrated navigation gyro drift correction method |
| CN114964230B (en) * | 2022-05-12 | 2023-11-03 | 北京自动化控制设备研究所 | Vehicle-mounted integrated navigation gyro drift correction method |
| CN116429094A (en) * | 2023-06-15 | 2023-07-14 | 小米汽车科技有限公司 | Positioning method, positioning device, electronic equipment and storage medium |
| CN116429094B (en) * | 2023-06-15 | 2023-09-26 | 小米汽车科技有限公司 | Positioning method, positioning device, electronic equipment and storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112798010B (en) | 2023-05-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112798010B (en) | Initializing method and device of VIO system of visual inertial odometer | |
| CN110246147B (en) | Visual inertial odometer method, visual inertial odometer device and mobile equipment | |
| CN109084732B (en) | Positioning and navigation method, device and processing equipment | |
| CN113554698B (en) | Vehicle pose information generation method and device, electronic equipment and storage medium | |
| CN109443348B (en) | Underground garage position tracking method based on fusion of look-around vision and inertial navigation | |
| CN112734852A (en) | Robot mapping method and device and computing equipment | |
| CN111210477A (en) | Method and system for positioning moving target | |
| US20220051031A1 (en) | Moving object tracking method and apparatus | |
| CN111145251B (en) | Robot and synchronous positioning and mapping method thereof and computer storage device | |
| CN105989581A (en) | Image stabilizing apparatus and method | |
| EP2851868A1 (en) | 3D Reconstruction | |
| CN112729294B (en) | Pose estimation method and system suitable for vision and inertia fusion of robot | |
| US20180033162A1 (en) | Method and device for camera pose estimation | |
| CN109669533A (en) | A kind of motion capture method, the apparatus and system of view-based access control model and inertia | |
| CN112118537B (en) | Method and related device for estimating movement track by using picture | |
| KR20200037502A (en) | Method and apparatus of outputting pose information | |
| CN113112524A (en) | Method and device for predicting track of moving object in automatic driving and computing equipment | |
| CN109035308A (en) | Image compensation method and device, electronic equipment and computer readable storage medium | |
| CN112435278B (en) | Visual SLAM method and device based on dynamic target detection | |
| CN112967393A (en) | Method and device for correcting vehicle movement track, electronic equipment and storage medium | |
| CN109816588B (en) | Method, device and equipment for recording driving trajectory | |
| CN115560744A (en) | Robot, multi-sensor-based three-dimensional mapping method and storage medium | |
| CN108322698A (en) | The system and method merged based on multiple-camera and Inertial Measurement Unit | |
| CN111199179B (en) | Target object tracking method, terminal equipment and medium | |
| CN113532455B (en) | Visual fusion positioning method and device, electronic equipment and readable storage medium |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |