CN113012224B

CN113012224B - Positioning initialization method and related device, equipment and storage medium

Info

Publication number: CN113012224B
Application number: CN202110272183.2A
Authority: CN
Inventors: 翟尚进; 章国锋
Original assignee: Zhejiang Shangtang Technology Development Co Ltd
Current assignee: Zhejiang Shangtang Technology Development Co Ltd
Priority date: 2021-03-12
Filing date: 2021-03-12
Publication date: 2022-06-03
Anticipated expiration: 2041-03-12
Also published as: WO2022188334A1; CN113012224A

Abstract

The application discloses a positioning initialization method, a related device, equipment and a storage medium, wherein the positioning initialization method comprises the following steps: after initializing the positioning parameters of the equipment to be positioned, acquiring the motion information of the equipment; determining a current motion state of the device based on the motion information; and optimizing the positioning parameters according to the current motion state of the equipment to obtain the optimized positioning parameters. According to the scheme, after the positioning parameters are initialized, the positioning parameters are optimized according to the motion state of the equipment to obtain the optimized positioning parameters, so that the subsequent positioning of the equipment is more accurate.

Description

Positioning initialization method and related device, equipment and storage medium

Technical Field

The present application relates to the field of positioning technologies, and in particular, to a positioning initialization method, and a related apparatus, device, and storage medium.

Background

At present, a visual tracking and positioning method plays an important role in the fields of computer vision, robots, unmanned aerial vehicles, three-dimensional reconstruction first-level augmented reality and the like. Generally, after initializing parameters required for visual tracking and positioning, directly using a result obtained by the initialization to track and position the device, which still has problems, for example, when the motion amplitude of the device is not large, the parameters solved in the initialization process may have large errors, such as parameters such as dimensions, and if the parameters are directly used to track and position the device, the accuracy of finally tracking and positioning the device may be low.

Disclosure of Invention

The application at least provides a positioning initialization method, a related device, equipment and a storage medium.

A first aspect of the present application provides a positioning initialization method, including: after initializing the positioning parameters of the equipment to be positioned, acquiring the motion information of the equipment; determining a current motion state of the device based on the motion information; and optimizing the positioning parameters by using the current motion state of the equipment to obtain the optimized positioning parameters.

Therefore, after the positioning parameters are initialized, the motion state of the equipment is further obtained, and then the positioning parameters are optimized according to the motion state, so that the subsequent movement of the equipment can be positioned according to the optimized parameters, and the positioning accuracy is improved.

The positioning parameters are divided into a first positioning parameter and a second positioning parameter; optimizing the positioning parameters according to the current motion state of the equipment to obtain the optimized positioning parameters, wherein the optimizing method comprises the following steps of: under the condition that the current motion state belongs to a non-static state, optimizing a first positioning parameter by using a first optimization mode, wherein the first positioning parameter comprises a scale; or, under the condition that the current motion state belongs to the static state, optimizing a second positioning parameter by using a second optimization mode, wherein the second positioning parameter is different from the first positioning parameter.

Therefore, different optimization modes are adopted when the equipment is in a static state or a non-static state, the positioning parameters are optimized, and different motion scenes can be suitable.

Wherein the non-stationary state comprises a first non-stationary state and a second non-stationary state; under the condition that the current motion state belongs to the non-static state, optimizing the first positioning parameter by using a first optimization mode, wherein the optimization mode comprises the following steps: under the condition that the current motion state belongs to a first non-static state, the first positioning parameter is optimized by utilizing the reprojection errors of all three-dimensional points on the current image frame and a plurality of historical image frames; the current image frame and the historical image frame are obtained by shooting of equipment, the current image frame is not used for tracking and positioning of the equipment, and the historical image frame is used for tracking and positioning of the equipment; or, under the condition that the current motion state belongs to the second non-static state, optimizing the first positioning parameter by utilizing a preset kinematic equation set.

Therefore, the non-static state is subdivided into a plurality of non-static states, so that when the equipment is in the non-static state, a proper optimization mode can be selected according to a specific motion condition, and the optimization effect on the parameters is improved.

Wherein determining the current motion state of the device based on the motion information comprises at least one of: taking one historical image frame as a motion starting frame; under the condition that the characteristics between a motion starting frame and a current image frame do not meet preset conditions, determining that the current motion state is a first non-static state; wherein the features include one or more of a number of image frames, time differences, disparities, and sensory data; and under the condition that the characteristics from the motion starting frame to the current image frame meet the preset conditions, determining that the current motion state is a second non-static state.

Therefore, the motion state of the equipment is determined through the historical image frame and the current image frame after the motion starting frame, the interference caused by the static image frame before the motion starting frame is reduced, and the motion state of the equipment is determined more accurately.

Wherein the preset condition comprises at least one of: a first time difference between the motion start frame and the current image frame is greater than or equal to a first time threshold; the number of historical image frames between the motion starting frame and the current image frame is greater than or equal to a first number threshold value; the number of the preset two-dimensional point pairs in the motion starting frame and the current image frame is greater than or equal to a second number threshold, wherein the parallax of the preset two-dimensional point pairs is greater than or equal to a first parallax threshold; the discrete degree of the accelerometer sensing data between the motion starting frame and the current image frame is greater than or equal to a preset degree threshold value.

Therefore, the larger the parallax and the dispersion degree of the sensing data, the larger the movement amplitude of the equipment is, the larger the number of the historical image frames and the larger the time difference are, and the sufficient sensing data are used for optimizing the positioning parameters, so that the optimization result of the positioning parameters is more accurate.

Wherein, optimizing the first positioning parameter by using a preset kinematic equation set comprises: taking one historical image frame as a motion starting frame, and acquiring the position relation and pre-integral information of adjacent image frames between the motion starting frame and a current image frame; substituting the position relation and the pre-integral information into a preset kinematic equation set, and solving to obtain a scale and the speed of the equipment; and taking the scale obtained by solving and the speed of the equipment as the optimized first positioning parameter.

Therefore, under the condition of fixing the rest positioning parameters which are not coupled with the scale much, the scale and the speed are solved, and the difficulty in solving the scale and the speed is reduced.

Wherein the solved scale and speed are a set of solutions that minimize the error of the preset kinematic equation set.

Therefore, a set of solution which enables the error of the preset kinematic equation set to be minimum is used as the solved scale and speed, so that the solved scale and speed are more accurate compared with other solutions, and the subsequent tracking and positioning results are more accurate.

Before the solved scale and the speed of the device are used as the optimized first positioning parameters, the method further comprises the following steps: substituting the scale and the speed obtained by the solution into a preset kinematic equation set to obtain the error of each kinematic equation in the preset kinematic equation set; and if the errors of the kinematic equations are less than or equal to a preset error threshold, determining that the scale and the speed are successfully solved, and executing that the scale obtained by solving and the speed of the equipment are used as the optimized first positioning parameters.

Therefore, the scale and the speed are considered to be solved successfully only under the condition that the errors of all the kinematic equations are smaller than or equal to the preset error threshold, so that the solving process of the scale and the speed is strict, and the solved scale and speed are more accurate.

Wherein, taking one of the historical image frames as a motion start frame comprises: sorting according to the second time difference between the historical image frames and the current image frame from less to more, and sequentially judging whether the third time difference between every two adjacent historical image frames is greater than a second time threshold value; and under the condition that the third time difference between two adjacent historical image frames is judged to be larger than the second time threshold, the historical image frame with smaller second time difference in the two adjacent historical image frames is taken as the motion starting frame.

Therefore, the motion start frame is determined according to the time difference between the two adjacent historical image frames, so that the identification process of the motion start frame is simple and convenient.

Wherein after optimizing the first positioning parameter using the set of preset kinematic equations, the method further comprises: and optimizing the position of each historical image frame and/or the depth of each three-dimensional point after the scale optimization.

Therefore, the depth of each history image frame and the depth of each three-dimensional point after the scale optimization are optimized, so that more accurate prior information can be obtained in the subsequent tracking and positioning process, and the tracking and positioning result is more accurate.

Optimizing the position of each history image frame after the scale optimization, wherein the optimizing the position of each history image frame after the scale optimization comprises the following steps: selecting one of the historical image frames as a first reference frame; determining the distance between the first reference frame and the adjacent historical image frame by using the optimized scale; preliminarily adjusting the positions of the adjacent historical image frames of the first reference frame according to the distance; and taking the preliminarily adjusted adjacent historical image frame as a new first reference frame, and re-executing the steps of determining the distance between the first reference frame and the adjacent historical image frame and subsequent steps thereof by using the optimized scale until the historical image frames in the preset time period are preliminarily adjusted.

Therefore, the distance relation between the historical image frames is adjusted according to the calculated scale, and the position relation between the historical image frames is optimized, so that the follow-up tracking and positioning result of the equipment is more accurate.

After the historical image frames in the preset time period are all adjusted preliminarily, the method further comprises the following steps: taking a historical image frame with the minimum time difference with the current image frame in a preset time period as a second reference frame; finally adjusting the position of the second reference frame to the position where the second reference frame is located before the initial adjustment; and adjusting the positions of the rest historical image frames except the second reference frame in the preset time period based on the position change of the second reference frame in the final adjustment process.

Therefore, after the preliminary adjustment is completed, the position of each historical image frame before and after the preliminary adjustment is changed too much, namely, the phenomenon of discontinuity between the historical image frames before and after the preliminary adjustment is generated, and the phenomenon of discontinuity can be alleviated by moving the preliminarily adjusted position of the historical image frame with the smallest time difference between the historical image frame and the current image frame to the position before the preliminary adjustment, so that the subsequent tracking and positioning effects are better.

Wherein, optimizing the depth of each three-dimensional point after the scale optimization comprises: and for each three-dimensional point, taking a historical image frame which observes the three-dimensional point for the first time as a reference, and adjusting the depth of the three-dimensional point according to the scale.

Therefore, the depth of the three-dimensional point is optimized through the calculated scale, so that the equipment can be tracked and positioned subsequently by using more accurate coordinates of the three-dimensional point, and the accuracy of tracking and positioning is improved.

Before optimizing the position of each history image frame and/or the depth of each three-dimensional point after the scale optimization, the method further comprises the following steps: deleting a historical image frame positioned before the motion starting frame in a preset time period; and/or after optimizing the position of each history image frame and/or the depth of each three-dimensional point after the scale optimization, the method comprises the following steps: an uncertainty of at least one of a position, a velocity, and an accelerometer bias of the device is increased.

Therefore, the influence of the history image frame having an excessively long photographing time on the optimization result can be reduced by deleting the history image frame before the motion start frame. Improving the uncertainty of the position, the speed and the bias amount of the accelerometer of the equipment can reduce the occurrence of inaccurate results of later positioning results due to over convergence.

Wherein determining the current motion state of the device based on the motion information comprises: determining that the current motion state of the equipment is a static state under the condition that the parallaxes between the plurality of historical image frames and the current image frame are all smaller than a second parallax threshold value and/or the discrete degree of at least one piece of sensing data is smaller than a corresponding preset degree value; wherein the second disparity threshold is fixed or dynamically adjusted based on a temporal differential between the historical image frame and the current image frame; at least one sensory data is collected during a first time period prior to a current motion state, including gyroscope sensory data and/or accelerometer sensory data.

Therefore, the recognition of the static state by the combination of the parallax and the sensing data is more accurate than the recognition of the single parallax or the sensing data.

Wherein, optimizing the second positioning parameter by using a second optimization mode comprises: and taking the speed as zero as a constraint, and constraining the second positioning parameter.

Therefore, the positioning parameters cannot be optimized through the reprojection error of the three-dimensional point in a static state, and the pose of the current image frame is enabled not to be changed greatly compared with the pose of the previous historical image frame by increasing the constraint with the speed of zero, so that the stability of the positioning parameters is improved.

Wherein, the method further comprises: judging whether a plurality of historical image frames before the current image frame have a preset number of three-dimensional points or not; under the condition that a plurality of historical image frames do not have a preset number of three-dimensional points, increasing the constraint weight with the speed being zero; and/or, in the case that a preset number of three-dimensional points exist in a plurality of historical image frames, reducing the constraint weight with the speed being zero.

Therefore, by reducing or increasing the weight at which the velocity is zero according to the number of three-dimensional points, it is made possible to optimize the positioning parameters well in various states.

A second aspect of the present application provides a positioning initialization apparatus, including: the device comprises an acquisition module, a positioning module and a processing module, wherein the acquisition module is used for acquiring the motion information of the device after the initialization of the positioning parameters of the device to be positioned; a determination module for determining a current motion state of the device based on the motion information; and the optimization module is used for optimizing the positioning parameters according to the current motion state of the equipment to obtain the optimized positioning parameters.

A third aspect of the present application provides an electronic device, comprising a memory and a processor, wherein the processor is configured to execute program instructions stored in the memory to implement the above positioning initialization method.

A fourth aspect of the present application provides a computer-readable storage medium having stored thereon program instructions that, when executed by a processor, implement the above-described positioning initialization method.

According to the scheme, after the positioning parameters are initialized, the motion state of the equipment is further acquired, and then the positioning parameters are optimized according to the motion state, so that the subsequent movement of the equipment can be positioned according to the optimized positioning parameters, and the positioning accuracy is improved.

It is to be understood that both the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the application.

Drawings

The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the present application and, together with the description, serve to explain the principles of the application.

FIG. 1 is a first flowchart of an embodiment of a positioning initialization method of the present application;

FIG. 2 is a second flowchart of an embodiment of a positioning initialization method of the present application;

FIG. 3 is a schematic structural diagram of an embodiment of a positioning initialization apparatus according to the present application;

FIG. 4 is a schematic structural diagram of an embodiment of an electronic device of the present application;

FIG. 5 is a schematic structural diagram of an embodiment of a computer-readable storage medium of the present application.

Detailed Description

The following describes in detail the embodiments of the present application with reference to the drawings attached hereto.

In the following description, for purposes of explanation and not limitation, specific details are set forth such as particular system structures, interfaces, techniques, etc. in order to provide a thorough understanding of the present application.

The term "and/or" herein is merely an association describing an associated object, meaning that three relationships may exist, e.g., a and/or B, may mean: a exists alone, A and B exist simultaneously, and B exists alone. In addition, the character "/" herein generally indicates that the former and latter related objects are in an "or" relationship. Further, the term "plurality" herein means two or more than two. In addition, the term "at least one" herein means any one of a plurality or any combination of at least two of a plurality, for example, including at least one of A, B, C, and may mean including any one or more elements selected from the group consisting of A, B and C.

Referring to fig. 1, fig. 1 is a first flowchart illustrating a positioning initialization method according to an embodiment of the present application. Specifically, the following steps may be included:

step S11: after the positioning parameters of the equipment to be positioned are initialized, the motion information of the equipment is obtained.

The method for initializing the positioning parameters of the device to be positioned may include a motion initialization and a stationary initialization. The motion initialization may be performed by separating the visual initialization from the inertial sensor initialization, that is, performing the visual initialization first, and after the visual initialization is completed, aligning the initialization result with the inertial sensor initialization result, thereby completing the initialization of the positioning parameters. The static initialization applies when the device is in a relatively static state. Because the device is in a relatively static state, the corresponding three-dimensional point coordinates cannot be obtained from the two-dimensional points observed from each frame of image in a triangularization mode or other modes, and accurate scale information cannot be obtained.

The motion information of the device may be acquired by using parallax, time difference, or the like between image frames captured by the device, or may be acquired by using sensing data between image frames. The image frames acquired in the process of tracking and positioning the device can be obtained by shooting by any device with a camera assembly, can also be obtained by obtaining images from other devices, and can also be images subjected to frame selection, brightness adjustment, resolution adjustment and the like. The device may be the device itself that captures the image frames. The other devices are devices that can be operated by using different central processing units, respectively. For example, in the course of unmanned aerial vehicle driving, the unmanned aerial vehicle is the device described in the embodiments of the present disclosure. Or the sweeping robot is also the equipment in the embodiment of the disclosure when the sweeping robot works. I.e. the device is in an environment enabling positioning and/or navigation of its own position in the environment.

Step S12: based on the motion information, a current motion state of the device is determined.

The motion state may be a static state or a non-static state. The motion state may be determined according to the motion amplitude of the device, for example, according to the disparity between two-dimensional point pairs in each image frame obtained by shooting from front and back, or of course, according to the sensing data. For example, when the disparity between two image frames is greater than a preset value, it is determined that the current motion state of the device is a non-stationary state.

Step S13: and optimizing the positioning parameters according to the current motion state of the equipment to obtain the optimized positioning parameters.

If the preset motion states of the device are multiple, each motion state may correspond to a corresponding optimization mode to optimize the positioning parameter, for example, the preset motion states of the device include a static state and a non-static state, and the optimization mode corresponding to the static state or the non-static state may be used to optimize the positioning parameter. The optimized parameters can be directly tracked and positioned, and certainly, the optimized parameters can be used for tracking and positioning the equipment after one or more times of optimization.

According to the scheme, after the positioning parameters are initialized, the motion state of the equipment is further acquired, and then the positioning parameters are optimized according to the motion state, so that the subsequent movement of the equipment can be positioned according to the optimized parameters, and the positioning accuracy is improved.

In some disclosed embodiments, the positioning parameters are divided into a first positioning parameter and a second positioning parameter. Wherein the first positioning parameter comprises a scale and the second positioning parameter is different from the first positioning parameter. The second positioning parameter is different from the first positioning parameter, and it is understood that the second positioning parameter is not identical to the parameter included in the first positioning parameter, for example, the second positioning parameter may not include a scale. Referring to fig. 2, fig. 2 is a second flowchart of an embodiment of the positioning initialization method of the present application. As shown in fig. 2: step S13 includes step S131 and step S132. Wherein, step S131: when the current motion state belongs to the static state, optimizing the second positioning parameter by using a second optimization mode, and step S132: and under the condition that the current motion state belongs to the non-static state, optimizing the first positioning parameter by using a first optimization mode, wherein the first optimization mode is different from the second optimization mode. The positioning parameters are optimized by adopting different optimization modes when the equipment is in a static state or a non-static state, so that the method can be suitable for different motion scenes. That is, whether to perform step S131 or step S132 is determined according to the motion state of the apparatus.

Specifically, the method for determining whether the device is in the static state or the non-static state includes determining that the current motion state of the device is the static state when the parallaxes between the plurality of history image frames and the current image frame are all smaller than a second parallax threshold value and/or the dispersion degree of the at least one sensing data is smaller than a corresponding preset degree value. The historical image frames used in the still state determination may be all image frames captured within a first time period before the current image frame. Further, the current image frame and the historical image frame are obtained by shooting the device, the current image frame is not used for tracking and positioning the device, and the historical image frame is used for tracking and positioning the device. That is, the current image frame is only subjected to image processing, i.e., feature extraction and matching, and tracking and positioning are not performed temporarily, whereas the historical image frame is not only subjected to image processing but also subjected to tracking and positioning. Tracking the location includes determining the location of the device in the world coordinate system. The first time period can be set according to the requirement without limitation. And respectively judging the parallax between all the historical image frames in the first time period and the current image frame, and if the parallax between a certain historical image frame and the current image frame is larger than a second parallax threshold, determining that the current motion state of the equipment does not belong to a static state. Optionally, the number of historical image frames in the first time period is greater than or equal to 2 frames. In the embodiment of the present disclosure, the number of the history image frames is 3 frames or more. The judgment of the parallax can be carried out by setting the multi-frame historical image frames, so that the misjudgment caused by the too small number of the historical image frames can be reduced, and the accuracy of the judgment of the static state is improved. Meanwhile, the first time period may not be too long, and if the first time period is too long, the information corresponding to the historical image frame with too long shooting time may bring an error to the determination of the current motion state. Whether the first time period is too long can be determined by whether a time threshold value is exceeded, and if the first time period exceeds the time threshold value, the first time period is determined to be too long. The time threshold may be determined according to the frequency of capturing images by the device, for example, in the case that the device is capable of capturing 200 images within 1s, when the first time period is more than 1s, the information between more than 200 historical image frames may be caused to be inaccurate, and an error is brought to the determination of the current motion state, so the time threshold may be 1s, and the first time period is less than 1s, for example, 200ms, and so on. That is, all the history image frames captured within 1 second before the current image frame is captured are used for the determination of the still state. Of course, in some disclosed embodiments, the quality of the history image frame captured in the first time period may also be selected, and if the quality of the history image frame is too low, the history image frame does not participate in the determination of the still state. For example, if two-dimensional points cannot be extracted from the history image frame, the quality of the history image frame is considered to be too low.

Alternatively, the second disparity threshold may be fixed or dynamically adjusted based on the temporal differential between the historical image frame and the current image frame. The dynamic adjustment based on the time difference between the history image and the current image frame may be performed according to a time difference between the history image frame and the current image frame which are farthest away from the current image frame within a preset time period. For example, the second parallax threshold is adjusted higher as the time difference is larger, and is adjusted lower as the time difference is smaller. Wherein the preset time period is longer than the first time period. All the history image frames within the preset time period are partial frames selected from all the history image frames captured by the device. That is, a part of the history image frames in the first period may be simultaneously located within the preset period. The specific selection manner may be a manner of specifically selecting the history image frames within a preset time period from all the history images according to that the number of two-dimensional points extracted from the history image frames is greater than a certain threshold or the disparity between the two-dimensional points and the previous history image frame is greater than a corresponding threshold, and the like, which is not specifically specified herein. It should be noted that, except that the history image frame used in the still state determination is the history image frame in the first time period, the history image frames in the remaining steps are all history image frames in the preset time period, and therefore, the following does not excessively explain each appearing history image frame. Of course, in other embodiments, the history image frame used in the still state determination may also be a history image frame within a preset time period. In some embodiments, the preset time period described herein may also be referred to as a sliding window.

Optionally, the at least one sensory data is collected within a first time period prior to the current motion state. The sensed data includes gyroscope sensed data and/or accelerometer sensed data. That is, the sensing data refers to sensing data between the history image frames in the first period. The discrete degree of the sensing data can be standard deviation of the gyroscope sensing data and/or the accelerometer sensing data within a preset time period. Of course, in other embodiments, other parameters may be used to characterize the degree of dispersion of the sensing data, such as variance, etc., and the specific determination of the degree of dispersion is not specifically defined herein.

In the embodiment of the disclosure, if one standard deviation of the gyroscope sensing data and the accelerometer sensing data is greater than the corresponding preset degree value, the current motion state of the device is determined not to belong to a static state. Further, only when the parallax determinations are all smaller than the second parallax threshold and the discrete degrees are all smaller than the corresponding preset degree values, the current motion state of the equipment is determined to be a static state.

The combination of the parallax and the sensing data is used for identifying the static state, and compared with the identification of the parallax or the sensing data alone, the identification result of the static state is more accurate.

And under the condition that the current motion state belongs to the static state, optimizing the second positioning parameter by using a second optimization mode, wherein the second positioning parameter does not comprise a scale. The difference between the second positioning parameter and the first positioning parameter includes that the first positioning parameter needs to optimize the scale obtained by initialization, and the second positioning parameter does not need to optimize the scale obtained by initialization. The second positioning parameters comprise other first positioning parameters except for the scale. When the equipment is in a static state, the coordinates of the three-dimensional points cannot be obtained according to triangulation, only the orientation of the three-dimensional points relative to the equipment can be determined, and the depth information of the three-dimensional points is unknown. Therefore, the positioning parameters cannot be optimized by means of reprojection errors and the like. Therefore, the positioning parameters are optimized by adopting a second optimization mode, namely the second positioning parameters are constrained by taking the speed as zero. In this case, the history image frame within the preset time period does not change. Of course, the embodiment of the present disclosure uses a speed of zero as a constraint, and does not mean that only the speed of zero is used to optimize the positioning parameters, but a constraint that the speed of the current image frame is zero is additionally added in the general visual optimization process.

The positioning parameters can not be optimized through the reprojection error of the three-dimensional point in a static state, and the pose of the current image frame can not be changed greatly compared with the pose of the previous historical image frame by increasing the constraint with the speed of zero, so that the stability of the positioning parameters is improved.

Optionally, in the process of optimizing the second positioning parameter by using the second optimization mode, determining whether a preset number of three-dimensional points exist in a plurality of historical image frames before the current image frame, and increasing the constraint weight with the speed being zero under the condition that the preset number of three-dimensional points do not exist in the plurality of historical image frames; and/or reducing the constraint weight with the speed of zero under the condition that a preset number of three-dimensional points exist in a plurality of historical image frames. The higher the weight is, the higher the confidence coefficient is, the more the influence of the weight on the tracking and positioning of the equipment is, and the lower the weight is, the lower the confidence coefficient is, the less the influence of the weight on the tracking and positioning of the equipment is. That is, if a plurality of historical image frames before the current image frame have a preset number of three-dimensional points, the positioning parameters can be optimized by directly using the reprojection errors of the three-dimensional points on each frame, so as to improve the optimization effect of the positioning parameters, that is, the weight with the speed of zero is reduced.

By reducing or increasing the weight with the speed of zero according to the number of the three-dimensional points, the positioning parameters can be well optimized in various states.

The method for optimizing the positioning parameter by using the current motion state of the device may be to optimize the first positioning parameter by using a first optimization method when the current motion state belongs to a non-stationary state, where the first positioning parameter includes a scale. The first positioning parameters may also include a velocity of the device, an offset of the inertial sensor, a velocity, a position of the device, and so forth. In the embodiment of the present disclosure, the non-stationary state includes a first non-stationary state and a second non-stationary state. Wherein the motion amplitude of the device in the first non-stationary state and the second non-stationary state is different. Optionally, the non-stationary state is subdivided into a first non-stationary state and a second non-stationary state, depending on, among other factors, the magnitude of the motion of the device. For example, if the motion amplitude of the device is large enough, the device is considered to belong to the second non-stationary state, and if the motion amplitude is too small, the device is considered to belong to the first non-stationary state. The relationship among the motion amplitudes of the device in the first non-static state, the second non-static state and the static state is as follows: the amplitude of motion of the device at rest < the amplitude of motion of the device at first non-rest < the amplitude of motion of the device at second non-rest.

The manner of determining that the motion state of the device belongs to the first or second non-stationary motion state comprises:

one of the historical image frames is used as a motion start frame. And under the condition that the characteristics from the motion starting frame to the current image frame do not meet the preset conditions, determining that the current motion state is a first non-static state. And under the condition that the characteristics from the motion starting frame to the current image frame meet the preset conditions, determining that the current motion state is a second non-static state. Here, in the determination of the non-stationary state, the history image frame used may be a history image frame within a preset time period. Wherein the features include one or more of a number of image frames, time differences, disparities, and sensory data. The second non-stationary state refers to a state in which the motion amplitude of the device is large, and the first non-stationary state is a motion state between the stationary state and the second non-stationary state. Because the depth information of the three-dimensional point cannot be obtained in the historical image frame before the motion starting frame, the method does not help to solve the scale, and may cause errors in the process of solving the positioning parameters such as the scale due to the fact that the shooting time is too long, and therefore, only the historical image frame after the motion starting frame is considered in the embodiment of the disclosure.

The motion state of the equipment is determined through the historical image frame and the current image frame after the motion starting frame, so that the interference caused by the static image frame before the motion starting frame is reduced, and the motion state of the equipment is determined more accurately.

The determination method of the motion start frame comprises the following steps: and sequentially judging whether a third time difference between every two adjacent historical image frames is greater than a second time threshold or not according to a sequence from a few second time differences between the historical image frames and the current image frame to a plurality of rows. And under the condition that the third time difference between two adjacent historical image frames is judged to be larger than the second time threshold, the historical image frame with smaller second time difference in the two adjacent historical image frames is taken as the motion starting frame. That is, the second time difference is a time difference between each of the history image frames and the current image frame, respectively, and the third time difference refers to a time difference between every adjacent two history image frames. At this time, the third time difference does not include the time difference between the current image frame and its neighboring history image frame.

Wherein, the historical image frame in the determination of the motion starting frame is the historical image frame in the preset time period. In the embodiment of the disclosure, if the device is in a static state, the history image frames in the preset time period meet a last-in first-out principle, where the last-in first-out principle means that the history image frames shot later are removed from the preset time period first, and of course, the removal from the preset time period does not mean that the history image frames are deleted from the storage space, and the storage space still retains the pose of the key frame and the sensing data between the key frame and the previous history image frame. When the equipment is in a non-static state, if the parallax between two last-shot historical image frames is larger than a certain threshold value, one historical image frame shot earliest in a preset time period is removed from the preset time period, and if the parallax is smaller than or equal to the threshold value, the last-to-last-shot historical image frame in the preset time period is removed from the preset time period. In this way, there is a significant time difference between the motion start frame and the historical image frame that is adjacent to and captured before the motion start frame within the preset time period, and thus, the embodiment of the present disclosure determines the motion start frame by the time difference between the adjacent image frames. The motion starting frame is determined according to the time difference between two adjacent historical image frames, so that the process of identifying the motion starting frame is simple and convenient.

The preset conditions specifically include at least one of the following conditions: first, a first time difference between the motion start frame and the current image frame is greater than or equal to a first time difference threshold. By ensuring that there is sufficient time difference between the motion start frame and the current image frame, enough sensing data can be used for integration, and the accuracy of optimization of the positioning parameters is improved. And secondly, the number of the historical image frames between the motion starting frame and the current image frame is larger than or equal to a first number threshold value. And ensuring that enough historical image frames exist between the motion starting frame and the current image frame so that enough sensing data can participate in integration to enable the optimized positioning parameters to be more accurate. And thirdly, the number of the preset two-dimensional point pairs in the motion starting frame and the current image frame is greater than or equal to a second number threshold. The preset two-dimensional point pair refers to a two-dimensional point pair obtained by two-dimensional point matching between different historical image frames of two-dimensional points extracted from the historical image frames. There are various ways to perform two-dimensional point matching, for example, feature matching, that is, calculating the distance between descriptors of two-dimensional points, that is, keypoints obtained by extraction, and if the distance is smaller than a certain threshold, it is determined that matching is successful, and the way of two-dimensional point matching may be block matching by an optical flow method, and the like, and the manner of obtaining the preset two-dimensional point pair is not specifically specified here. And the parallax of the preset two-dimensional point pair is greater than or equal to a first parallax threshold value. That is, it is determined whether the number of preset two-dimensional point pairs having a disparity greater than or equal to the first disparity threshold is greater than or equal to the second number threshold. Of course, in other embodiments, the number of pairs of preset two-dimensional points in any historical image frame between the motion start frame and the current image frame may be greater than or equal to the second number threshold, which is not specifically specified herein. And fourthly, the discrete degree of the accelerometer sensing data between the motion starting frame and the current image frame is greater than or equal to a preset degree threshold value. Of course, if the preset condition is strictly set according to the actual requirement, a fifth judgment condition may be added, that is, the discrete degree of the gyroscope sensing data between the motion start frame and the current image frame is greater than or equal to the preset degree threshold corresponding to the discrete degree. In the embodiment of the present disclosure, if the feature simultaneously satisfies the first four conditions, the current motion state of the device is determined to be the second non-stationary state, that is, if the feature does not satisfy any of the first four conditions, the current motion state of the device is determined not to belong to the second non-stationary state. In the embodiment of the present disclosure, the determination of whether the motion state of the device belongs to the static state or the non-static state may be performed simultaneously. And if the motion state of the equipment is not considered to belong to the static state and does not meet any one of the first four preset conditions, the motion state of the equipment is considered to be a first non-static state.

The larger the degree of dispersion of the parallax and the sensing data is, the larger the movement amplitude of the equipment is, the larger the number of the historical image frames is and the larger the time difference is, and the sufficient sensing data is used for optimizing the positioning parameters, so that the optimization result of the positioning parameters is more accurate.

Optionally, in a case that the current motion state belongs to the first non-static state, the first positioning parameter may be optimized by using a reprojection error of each three-dimensional point on the current image frame and the historical image frame. That is, when the device is in a non-stationary state, according to the motion of the device, the coordinates of three-dimensional points can be obtained by triangulating two-dimensional points observed in a current image frame and each historical image frame taken by the device, and then the obtained three-dimensional points are re-projected onto the current image frame and each historical image frame to obtain projected points, wherein the distance between the projected point corresponding to the three-dimensional point in each image frame and the observed two-dimensional point corresponding to the three-dimensional point is a re-projection error. Generally, the reprojection error can be optimized for three-dimensional points and the pose of each image frame simultaneously. By changing the pose of the camera, the reprojection error is minimized, and the effects of optimizing the pose of the camera and optimizing the coordinates of the three-dimensional points are achieved. Because the coordinates of the three-dimensional points are determined, the pose of the camera is also determined, namely the distance and the angle between the adjacent frames are determined, and other positioning parameters can be determined by combining the sensing data between the adjacent image frames.

By subdividing the non-static state into multiple non-static states, the device can select a proper optimization mode according to specific motion conditions when in the non-static state, and the optimization effect on the parameters is improved.

And optimizing the first positioning parameter by using a preset kinematic equation set under the condition that the current motion state belongs to a second non-static state. One historical image frame is used as a motion starting frame, and the position relation between the motion starting frame and an adjacent image frame between current image frames and pre-integration information are obtained. As described above, if the motion start frame is already determined in the motion state determination process, the step of determining the motion start frame may not be performed again here. The adjacent image frame between the motion start frame and the current image frame is a historical image frame within a preset time period. Wherein the positional relationship includes a distance relationship and an angle relationship. The pre-integration information includes pre-integration and acceleration of data measured by the gyroscopeThe meter measures a pre-integration of the resulting data. And substituting the position relation and the pre-integral information into a preset kinematic equation set, and solving to obtain the scale and the speed of the equipment. The angles in the positioning parameters, the gyroscope offset in the sensor and the accelerometer offset are still obtained by initialization, and the results, the position relation and the pre-integration information are substituted into a kinematic equation set to solve the scale and the speed. Generally, upon detecting that the device is in a non-stationary state, the positioning parameters include

Wherein X_kDenotes all positioning parameters, X, at time k_CIndicating the currently used positioning parameter, X_SAnd representing the corresponding positioning parameters of each historical key frame in a preset time period. Wherein the content of the first and second substances,

wherein G represents a world coordinate system, C represents a camera coordinate system,

a rotation matrix representing the camera coordinate system from the world coordinate system to time k,

representing the position of the camera at time k in the world coordinate system,

representing the speed of movement at time k in the world coordinate system, b_wkRepresents the bias of the gyroscope at time k, b_akIndicating the offset of the accelerometer at time k and T indicates the transposition. Wherein, assuming that n frames of historical image frames are in total in the preset time period, the method comprises the following steps

Namely, the position, the angle and the speed of the historical image frame in the world coordinate system are reserved in the preset time period.

First, a set of requirements is definedThe location parameters of the solution are calculated,

where s represents the dimension, g represents the direction of gravity,

representing the speed of movement of the device in the world coordinate system at time k.

Wherein the preset kinematic equation is as follows:

wherein r is_Ii，jRepresenting the error between the pre-integration value and the prior value from i frame to j frame.

To represent

The corresponding pre-integrated covariance matrix. The preset kinematic equation set consists of a plurality of preset kinematic equations.

In some disclosed embodiments, the method for solving the scale and the speed can adopt linear solution of pre-integration information of the sensing data and initialized positioning parameters, wherein the initialized positioning parameters comprise three-dimensional points and reprojection errors thereof, and can also adopt nonlinear optimization of the two types of information. Therefore, the preset kinematic equation is not limited to the above kinematic equation in the manner of construction. Under the condition of fixing the rest positioning parameters which are not coupled with the scale much, the scale and the speed are solved, and the difficulty in solving the scale and the speed is reduced.

And solving the obtained scale and speed to obtain a set of solutions with the minimum error of the preset kinematic equation set. I.e. a set of solutions that minimizes the sum of the errors of the various preset kinematic equations. By taking a group of solutions which enable the error of the preset kinematic equation set to be minimum as the scale and the speed obtained by solving, the scale and the speed obtained by solving are more accurate compared with other solutions, and the result of subsequent tracking and positioning is more accurate. And further substituting the solved scale and speed into a preset kinematic equation set to obtain the error of each kinematic equation in the preset kinematic equation set. And if the error of one equation is greater than a preset error threshold value, determining that the scale and speed solution fails. And after the scale and speed solution fails, tracking and positioning the current image frame according to the initialized positioning parameters, wherein if the current image frame meets the selection requirement of the historical image frame in a preset time period after tracking and positioning, the current image frame can enter the preset time period and be used as the latest historical image frame in the preset time period. A new image frame is then acquired, the motion state of the device is determined, and the subsequent steps are performed. And if the errors of the kinematic equation sets are less than or equal to a preset error threshold value, determining the scale and the speed and successfully solving. And executing the solved scale and the speed of the equipment as the optimized first positioning parameters. Namely, the scale and the speed obtained by the original initialization are updated by the scale and the speed obtained by the solution. The other positioning parameters including the angle, the gyroscope and the accelerometer offset may be directly used as the convergence values initialized before, or of course, preset values may be used, such as factory calibration values or values used in the last tracking positioning. Of course, the calculated values of the scale, the velocity and the gyroscope offset may be substituted into the pre-integration equation of the sensing data to find a set of solutions that minimizes the error of the pre-integration equation.

The scale and the speed are considered to be solved successfully under the condition that the errors of all the kinematic equations are smaller than or equal to the preset error threshold, so that the solving process of the scale and the speed is strict, and the scale and the speed obtained through solving are more accurate.

And optimizing the position of each history image frame after the scale optimization. Optionally, after the scale and velocity solution is successful, one of the historical image frames is selected as the first reference frame. For example, the history image frame having the largest time difference from the current image frame within the preset time period is selected as the reference frame. And determining the distance between the first reference frame and the adjacent historical image frame by using the optimized scale. Namely, the optimized scale is used for replacing the original scale to solve the distance between two adjacent historical image frames. According to the distance, the positions of the adjacent image frames of the first reference frame are preliminarily adjusted. For example, if the distance between the first reference frame obtained by solving according to the original scale and the adjacent image frame thereof is 1 meter, and the distance between the first reference frame obtained by solving according to the optimized scale and the adjacent historical image frame thereof is 2 meters, the adjacent image frame of the first reference frame is moved, so that the distance between the first reference frame and the adjacent frame thereof is 2 meters, and of course, only the distance is moved in the whole process without changing the angle between the first reference frame and the adjacent frame thereof. And then, taking the adjacent image frame after the initial adjustment as a new first reference frame, and re-executing the steps of determining the distance between the first reference frame and the adjacent historical image frame thereof and the subsequent steps thereof by using the optimized scale. And finishing the preliminary adjustment of the historical image frames within the preset time period. Of course, after the adjacent image frame is used as a new first reference frame, the positions of the remaining history image frames which are not preliminarily adjusted may be continuously adjusted without continuously adjusting the position of the last first reference frame adjacent to the new first reference frame.

And after the historical image frames in the preset time period are all adjusted preliminarily, taking the historical image frame with the minimum time difference with the current image frame in the preset time period as a second reference frame. I.e. the previous history image frame to the current image frame. And finally adjusting the position of the second reference frame to the position of the second reference frame before the initial adjustment. That is, for the second reference frame, the position after the initial adjustment and the final adjustment is unchanged from the position before the initial adjustment. Then, based on the position change of the second reference frame in the final adjustment process, the positions of the rest of the historical image frames except the second reference frame in the preset time period are adjusted. In the final adjustment process, the scale between adjacent historical image frames does not change, and simply speaking, the process is a translation process of the whole historical image frame within a preset time period. After the preliminary adjustment is completed, the position of each historical image frame before and after the preliminary adjustment is changed too much, namely, the phenomenon of discontinuity occurs between the historical image frames before and after the preliminary adjustment, and the phenomenon of discontinuity can be alleviated by moving the position of the historical image frame after the preliminary adjustment, which has the smallest time difference with the current image frame, to the position before the preliminary adjustment, so that the subsequent tracking and positioning effects are better.

And optimizing the depth of each three-dimensional point after the scale optimization. Of course, after the scale optimization, the positions of the respective historical image frames and the depths of the respective three-dimensional points may be both optimized. For example, the history image frames after position optimization are used to triangulate the two-dimensional point pairs matched in each frame again to obtain new coordinates of the three-dimensional point, and the three-dimensional points originally corresponding to the new three-dimensional point are updated by the new three-dimensional point, so that the three-dimensional points are updated. Of course, the depth of the three-dimensional point may be optimized by adjusting the depth of the three-dimensional point according to the scale based on the historical image frame of the three-dimensional point observed for the first time. Because the orientation of the three-dimensional point relative to the three-dimensional point observed for the first time is known, and the depth is obtained by solving according to the scale before optimization, the orientation relation between the historical image frames observing the three-dimensional point for the first time after final adjustment can be solved according to the orientation relation between the original three-dimensional point and the historical image frame observing the three-dimensional point for the first time, and the depth of the three-dimensional point is solved according to the new scale to obtain the coordinates of the new three-dimensional point, so that the coordinates of each three-dimensional point are updated.

By optimizing the depth of each history image frame and three-dimensional point after the scale optimization, more accurate prior information can be obtained in the subsequent tracking and positioning process, so that the tracking and positioning result is more accurate.

And deleting the historical image frames positioned before the motion starting frame in a preset time period before optimizing the position of each historical image frame and/or the depth of each three-dimensional point after the scale optimization. The deletion means to remove the history image frame before the motion start frame from the second preset time period, but may also save the pose of the removed history image frame and the sensory data in the storage unit.

In some disclosed embodiments, the uncertainty of at least one of the device's position, velocity, and accelerometer bias is increased after optimizing the position of each history image frame and/or the depth of each three-dimensional point after scale optimization. Because the position, velocity and accelerometer bias of the device may converge with excessive confidence in the previous initialization process, i.e. the errors of the position, velocity and accelerometer bias may be large, it is necessary to increase their uncertainty to reduce the influence subsequently in the tracking and positioning process, and then continuously update the position, velocity and accelerometer according to the motion information acquired in the subsequent tracking and positioning process.

According to the scheme, after the positioning parameters are initialized, the motion state of the equipment is further obtained, and then the positioning parameters are optimized according to the motion state, so that the subsequent movement of the equipment can be positioned according to the optimized parameters, and the positioning accuracy is improved.

The main body of the positioning initialization method may be a positioning initialization apparatus, for example, the positioning initialization method may be executed by a terminal device or a server or other processing devices, where the terminal device may be: virtual reality helmets, augmented reality glasses, unmanned vehicles, mobile robots, sweeping robots, flying devices, User Equipment (UE), mobile devices, User terminals, cellular phones, cordless phones, Personal Digital Assistants (PDA), handheld devices, computing devices, onboard devices, wearable devices, and the like, having both image sensors and inertial sensors. In some possible implementations, the location initialization method may be implemented by a processor calling computer readable instructions stored in a memory.

Referring to fig. 3, fig. 3 is a schematic structural diagram of an embodiment of a positioning initialization apparatus according to the present application. The positioning initialization device 30 includes: an acquisition module 31, a determination module 32 and an optimization module 33. The obtaining module 31 is configured to obtain motion information of a device to be located after a positioning parameter of the device to be located is initialized; the determination module 32 is configured to determine a current motion state of the device based on the motion information; the optimization module 33 is configured to optimize the positioning parameters according to the current motion state of the device, so as to obtain optimized positioning parameters.

In some disclosed embodiments, the positioning parameters are divided into a first positioning parameter and a second positioning parameter; the optimizing module 33 optimizes the positioning parameters by using the current motion state of the device to obtain optimized positioning parameters, including: under the condition that the current motion state belongs to a non-static state, optimizing a first positioning parameter by using a first optimization mode, wherein the first positioning parameter comprises a scale; or, under the condition that the current motion state belongs to the static state, optimizing a second positioning parameter by using a second optimization mode, wherein the second positioning parameter is different from the first positioning parameter.

According to the scheme, different optimization modes are adopted when the equipment is in a static state or a non-static state, the positioning parameters are optimized, and the method and the device can be suitable for different motion scenes.

In some disclosed embodiments, the non-stationary state includes a first non-stationary state and a second non-stationary state; the optimization module 33 optimizes the first positioning parameter by using a first optimization method when the current motion state belongs to the non-stationary state, including: under the condition that the current motion state belongs to a first non-static state, the first positioning parameters are optimized by utilizing the reprojection errors of all three-dimensional points on the current image frame and a plurality of historical image frames; the current image frame and the historical image frame are obtained by shooting equipment, the current image frame is not used for tracking and positioning the equipment, and the historical image frame is used for tracking and positioning the equipment; or, under the condition that the current motion state belongs to the second non-static state, optimizing the first positioning parameter by utilizing a preset kinematic equation set.

According to the scheme, the non-static state is subdivided into multiple non-static states, so that when the equipment is in the non-static state, a proper optimization mode can be selected according to a specific motion condition, and the optimization effect on parameters is improved.

In some disclosed embodiments, the determination module 32 determines the current motion state of the device based on the motion information, including at least one of: taking one historical image frame as a motion starting frame; under the condition that the characteristics between a motion starting frame and a current image frame do not meet preset conditions, determining that the current motion state is a first non-static state; wherein the features include one or more of a number of image frames, time differences, disparities, and sensory data; and under the condition that the characteristics from the motion starting frame to the current image frame meet the preset conditions, determining that the current motion state is a second non-static state.

According to the scheme, the motion state of the equipment is determined through the motion starting frame, the subsequent historical image frame and the current image frame, so that the interference caused by the static image frame before the motion starting frame is reduced, and the motion state of the equipment is determined more accurately.

In some disclosed embodiments, the preset condition includes at least one of: a first time difference between the motion start frame and the current image frame is greater than or equal to a first time threshold; the number of historical image frames between the motion starting frame and the current image frame is greater than or equal to a first number threshold value; the number of the preset two-dimensional point pairs in the motion starting frame and the current image frame is greater than or equal to a second number threshold, wherein the parallax of the preset two-dimensional point pairs is greater than or equal to a first parallax threshold; the discrete degree of the accelerometer sensing data between the motion starting frame and the current image frame is greater than or equal to a preset degree threshold value.

According to the scheme, the larger the parallax and the discrete degree of the sensing data are, the larger the motion amplitude of the equipment is, the larger the number of the historical image frames is and the larger the time difference is, and the sufficient sensing data are used for optimizing the positioning parameters, so that the optimization result of the positioning parameters is more accurate.

In some disclosed embodiments, the optimization module 33 optimizes the first positioning parameter using a set of predetermined kinematic equations, including: taking one historical image frame as a motion starting frame, and acquiring the position relation and pre-integral information of adjacent image frames between the motion starting frame and a current image frame; substituting the position relation and the pre-integral information into a preset kinematic equation set, and solving to obtain a scale and the speed of the equipment; and taking the scale obtained by solving and the speed of the equipment as the optimized first positioning parameter.

According to the scheme, under the condition that the rest positioning parameters which are not coupled with the scale are fixed, the scale and the speed are solved, and the difficulty in solving the scale and the speed is reduced.

In some disclosed embodiments, the solved scale and velocity are a set of solutions that minimize the error of the predetermined set of kinematic equations.

According to the scheme, a group of solutions which enable the error of the preset kinematic equation set to be minimum is used as the scale and the speed obtained through solving, so that the scale and the speed obtained through solving are more accurate compared with other solutions, and the result of subsequent tracking and positioning is more accurate.

In some disclosed embodiments, before using the solved dimension and the speed of the device as the optimized first positioning parameter, the optimization module 33 is further configured to: substituting the scale and the speed obtained by solving into a preset kinematics equation set to obtain the error of each kinematics equation in the preset kinematics equation set; and if the errors of the kinematic equations are less than or equal to a preset error threshold, determining that the scale and the speed are successfully solved, and executing that the scale obtained by solving and the speed of the equipment are used as the optimized first positioning parameters.

According to the scheme, the scale and the speed are considered to be solved successfully only under the condition that the errors of all the kinematic equations are smaller than or equal to the preset error threshold, so that the solving process of the scale and the speed is strict, and the scale and the speed obtained through solving are more accurate.

In some disclosed embodiments, taking one of the historical image frames as the motion start frame includes: the determining module 32 sorts the image frames according to the second time difference between the historical image frames and the current image frame from less to more, and sequentially judges whether the third time difference between every two adjacent historical image frames is greater than a second time threshold; and under the condition that the third time difference between two adjacent historical image frames is judged to be larger than the second time threshold, the historical image frame with smaller second time difference in the two adjacent historical image frames is taken as the motion starting frame.

According to the scheme, the motion starting frame is determined according to the time difference between two adjacent historical image frames, so that the process of determining the motion starting frame is simpler and more convenient.

In some disclosed embodiments, after optimizing the first positioning parameter using the preset set of kinematic equations, the optimization module 33 is further configured to: and optimizing the position of each history image frame and/or the depth of each three-dimensional point after the scale optimization.

According to the scheme, the depth of each history image frame and the depth of each three-dimensional point after the scale optimization are optimized, so that more accurate prior information can be obtained in the subsequent tracking and positioning process, and the tracking and positioning result is more accurate.

In some disclosed embodiments, the optimizing module 33 optimizes the position of each history image frame after the scale optimization, including: selecting one of the historical image frames as a first reference frame; determining the distance between the first reference frame and the adjacent historical image frame by using the optimized scale; preliminarily adjusting the positions of the adjacent historical image frames of the first reference frame according to the distance; and taking the preliminarily adjusted adjacent historical image frame as a new first reference frame, and re-executing the steps of determining the distance between the first reference frame and the adjacent historical image frame and subsequent steps thereof by using the optimized scale until the historical image frames in the preset time period are preliminarily adjusted.

According to the technical scheme, the distance relation between the historical image frames is adjusted according to the scale obtained through calculation, and the position relation between the historical image frames is optimized, so that the follow-up tracking and positioning result of the equipment is more accurate.

In some disclosed embodiments, after the historical image frames within the preset time period have been preliminarily adjusted, the optimization module 33 is further configured to: taking a historical image frame with the minimum time difference with the current image frame in a preset time period as a second reference frame; finally adjusting the position of the second reference frame to the position where the second reference frame is located before the initial adjustment; and adjusting the positions of the rest historical image frames except the second reference frame in the preset time period based on the position change of the second reference frame in the final adjustment process.

According to the scheme, after the preliminary adjustment is completed, the position change of each historical image frame before and after the preliminary adjustment is too large, namely, the phenomenon of discontinuity occurs between the historical image frames before and after the preliminary adjustment, and the phenomenon of discontinuity can be alleviated by moving the position of the historical image frame after the preliminary adjustment with the minimum time difference between the current image frames to the position before the preliminary adjustment, so that the subsequent tracking and positioning effects are better.

In some disclosed embodiments, the optimizing module 33 optimizes the depth of each three-dimensional point after the scale optimization, including: and for each three-dimensional point, taking a historical image frame of the three-dimensional point observed for the first time as a reference, and adjusting the depth of the three-dimensional point according to the scale.

According to the technical scheme, the depth of the three-dimensional point is optimized through the scale obtained through calculation, so that the equipment can be tracked and positioned subsequently by using the more accurate coordinate of the three-dimensional point, and the accuracy of tracking and positioning is improved.

In some disclosed embodiments, before optimizing the position of each history image frame and/or the depth of each three-dimensional point after the scale optimization, the optimization module 33 is further configured to: deleting a historical image frame positioned before the motion starting frame in a preset time period; and/or after optimizing the position of each history image frame and/or the depth of each three-dimensional point after the scale optimization, the method comprises the following steps: an uncertainty of at least one of a position, a velocity, and an accelerometer bias of the device is increased.

According to the scheme, the influence of the history image frame with overlong shooting time on the optimization result can be reduced by deleting the history image frame before the motion starting frame. Improving the uncertainty of the position, the speed and the bias amount of the accelerometer of the equipment can reduce the occurrence of inaccurate results of later positioning results due to over convergence.

In some disclosed embodiments, the determination module 32 determines the current motion state of the device based on the motion information, including: determining that the current motion state of the equipment is a static state under the condition that the parallaxes between the plurality of historical image frames and the current image frame are smaller than a second parallax threshold value and/or the dispersion degree of at least one sensing data is smaller than a corresponding preset degree value; wherein the second disparity threshold is fixed or dynamically adjusted based on a temporal differential between the historical image frame and the current image frame; at least one sensory data is collected during a first time period prior to a current motion state, including gyroscope sensory data and/or accelerometer sensory data.

According to the scheme, the static state is identified through the combination of the parallax and the sensing data, and compared with the identification of single parallax or sensing data, the identification result of the static state is more accurate.

In some disclosed embodiments, the optimizing module 33 optimizes the second positioning parameter by using a second optimizing method, which includes: and taking the speed as zero as a constraint, and constraining the second positioning parameter.

According to the scheme, the positioning parameters cannot be optimized through the reprojection errors of the three-dimensional points in the static state, and the pose of the current image frame is enabled not to be changed greatly compared with the pose of the previous historical image frame by increasing the constraint with the speed of zero, so that the stability of the positioning parameters is improved.

In some disclosed embodiments, the optimization module 33 is further configured to: judging whether a plurality of historical image frames before the current image frame have a preset number of three-dimensional points or not; under the condition that a plurality of historical image frames do not have a preset number of three-dimensional points, increasing the constraint weight with the speed being zero; and/or, in the case that a preset number of three-dimensional points exist in a plurality of historical image frames, reducing the constraint weight with the speed being zero.

According to the scheme, the weight with the speed being zero is reduced or increased according to the number of the three-dimensional points, so that the positioning parameters can be well optimized in various states.

Referring to fig. 4, fig. 4 is a schematic structural diagram of an embodiment of an electronic device according to the present application. The electronic device 40 comprises a memory 41 and a processor 42, the processor 42 being configured to execute program instructions stored in the memory 41 to implement the steps in any of the above-described embodiments of the location initialization method. In one particular implementation scenario, electronic device 40 may include, but is not limited to: the electronic device 40 may further include a mobile device such as a laptop or a tablet computer, which is not limited herein.

In particular, the processor 42 is configured to control itself and the memory 41 to implement the steps of any of the above-described embodiments of the location initialization method. Processor 42 may also be referred to as a CPU (Central Processing Unit). The processor 42 may be an integrated circuit chip having signal processing capabilities. The Processor 42 may also be a general purpose Processor, a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), a Field Programmable Gate Array (FPGA) or other Programmable logic device, discrete Gate or transistor logic, discrete hardware components. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like. In addition, the processor 42 may be commonly implemented by an integrated circuit chip.

Referring to fig. 5, fig. 5 is a schematic structural diagram of an embodiment of a computer-readable storage medium according to the present application. The computer readable storage medium 50 stores program instructions 51 executable by the processor, the program instructions 51 being for implementing the steps in any of the above-described embodiments of the location initialization method.

In some embodiments, functions of or modules included in the apparatus provided in the embodiments of the present disclosure may be used to execute the method described in the above method embodiments, and specific implementation thereof may refer to the description of the above method embodiments, and for brevity, will not be described again here.

The foregoing description of the various embodiments is intended to highlight various differences between the embodiments, and the same or similar parts may be referred to each other, and for brevity, will not be described again herein.

In the several embodiments provided in the present application, it should be understood that the disclosed method and apparatus may be implemented in other manners. For example, the above-described apparatus embodiments are merely illustrative, and for example, a division of a module or a unit is merely one type of logical division, and an actual implementation may have another division, for example, a unit or a component may be combined or integrated with another system, or some features may be omitted, or not implemented. In addition, the shown or discussed mutual coupling or direct coupling or communication connection may be an indirect coupling or communication connection of devices or units through some interfaces, and may be in an electrical, mechanical or other form.

In addition, functional units in the embodiments of the present application may be integrated into one processing unit, or each unit may exist alone physically, or two or more units are integrated into one unit. The integrated unit can be realized in a form of hardware, and can also be realized in a form of a software functional unit. The integrated unit, if implemented in the form of a software functional unit and sold or used as a stand-alone product, may be stored in a computer readable storage medium. Based on such understanding, the technical solution of the present application may be substantially implemented or contributed to by the prior art, or all or part of the technical solution may be embodied in a software product, which is stored in a storage medium and includes instructions for causing a computer device (which may be a personal computer, a server, a network device, or the like) or a processor (processor) to execute all or part of the steps of the method according to the embodiments of the present application. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk or an optical disk, and other various media capable of storing program codes.

Claims

1. A method for location initialization, comprising:

after initializing the positioning parameters of the equipment to be positioned, acquiring the motion information of the equipment;

determining a current motion state of the device based on the motion information;

optimizing the positioning parameters according to the current motion state of the equipment to obtain optimized positioning parameters, wherein the positioning parameters are divided into a first positioning parameter and a second positioning parameter;

wherein, the optimizing the positioning parameter according to the current motion state of the device to obtain an optimized positioning parameter includes:

under the condition that the current motion state belongs to a non-static state, optimizing a first positioning parameter by using a first optimization mode, wherein the first positioning parameter comprises a scale;

or, when the current motion state belongs to a static state, optimizing a second positioning parameter by using a second optimization mode, wherein the second positioning parameter is different from the first positioning parameter.

2. The method of claim 1, wherein the non-stationary state comprises a first non-stationary state and a second non-stationary state; the optimizing a first positioning parameter by using a first optimization mode under the condition that the current motion state belongs to a non-static state includes:

under the condition that the current motion state belongs to a first non-static state, utilizing reprojection errors of three-dimensional points on a current image frame and a plurality of historical image frames to optimize the first positioning parameter, wherein the three-dimensional points are space points obtained by triangularization of two-dimensional points in the current image frame and the historical image frames; the current image frame and the historical image frame are obtained by shooting of the equipment, the current image frame is not used for tracking and positioning of the equipment, and the historical image frame is used for tracking and positioning of the equipment;

or, under the condition that the current motion state belongs to the second non-static state, optimizing the first positioning parameter by using a preset kinematic equation set.

3. The method of claim 2, wherein determining the current motion state of the device based on the motion information comprises at least one of:

taking one of the historical image frames as a motion starting frame;

determining the current motion state as a first non-static state under the condition that the characteristics between the motion starting frame and the current image frame do not meet preset conditions; wherein the features include one or more of a number of image frames, time differences, disparities, and sensory data;

determining that the current motion state is the second non-static state when the feature between the motion start frame and the current image frame satisfies a preset condition.

4. The method of claim 3, wherein the preset condition comprises at least one of:

a first time difference between the motion start frame and the current image frame is greater than or equal to a first time threshold;

the number of historical image frames between the motion starting frame and the current image frame is greater than or equal to a first number threshold;

the number of preset two-dimensional point pairs in the motion starting frame and the current image frame is greater than or equal to a second number threshold, wherein the parallax of the preset two-dimensional point pairs is greater than or equal to a first parallax threshold;

the discrete degree of the accelerometer sensing data between the motion starting frame and the current image frame is greater than or equal to a preset degree threshold value.

5. The method according to any one of claims 2 to 4, wherein the optimizing the first positioning parameter using the set of predetermined kinematic equations comprises:

taking one historical image frame as a motion starting frame, and acquiring the position relation between the positions of the equipment corresponding to adjacent image frames between the motion starting frame and a current image frame and pre-integration information;

substituting the position relation and the pre-integral information into a preset kinematics equation set, and solving to obtain the scale and the speed of the equipment;

and taking the scale obtained by solving and the speed of the equipment as the optimized first positioning parameter.

6. The method of claim 5, wherein the scale and the velocity obtained by the solving is a set of solutions that minimizes the error of the preset set of kinematic equations.

7. The method according to claim 5 or 6, wherein before the solving the scale and the speed of the device as the optimized first positioning parameters, the method further comprises:

substituting the scale and the speed obtained by solving into the preset kinematic equation set to obtain the error of each kinematic equation in the preset kinematic equation set;

and if the error of each kinematic equation is less than or equal to a preset error threshold, determining that the scale and the speed are solved successfully, and executing the scale obtained by solving and the speed of the equipment as optimized first positioning parameters.

8. The method according to claim 3 or 5, wherein said taking one of said historical image frames as a motion start frame comprises:

removing at least one frame of history image frame from history image frames in a preset time period before a current image frame based on a parallax between two last-shot history image frames of the device when the device is in a non-static state;

sorting according to the second time difference between the historical image frames and the current image frame from less to more, and sequentially judging whether a third time difference between every two adjacent historical image frames is greater than a second time threshold value or not;

and taking the history image frame with smaller second time difference as the motion starting frame when judging that the third time difference between the two adjacent history image frames is larger than a second time threshold.

9. The method according to any one of claims 2 to 8, wherein after said optimizing the first positioning parameter using the set of preset kinematic equations, the method further comprises:

and optimizing the position of the equipment and/or the depth of each three-dimensional point corresponding to each history image frame after the scale optimization.

10. The method according to claim 9, wherein optimizing the position of the device corresponding to each history image frame after the scale optimization comprises:

selecting one of the historical image frames as a first reference frame;

determining the distance between the position of the equipment corresponding to the first reference frame and the position of the equipment corresponding to the adjacent historical image frame by using the optimized scale;

preliminarily adjusting the position of the equipment corresponding to the adjacent historical image frame of the first reference frame according to the distance;

and taking the preliminarily adjusted adjacent historical image frame as a new first reference frame, and re-executing the optimized scale to determine the distance between the position of the equipment corresponding to the first reference frame and the position of the equipment corresponding to the adjacent historical image frame and subsequent steps thereof until the historical image frames in a preset time period are preliminarily adjusted.

11. The method of claim 10, wherein after the preliminary adjustment of the historical image frames within the preset time period is completed, the method further comprises:

taking a historical image frame with the minimum time difference with the current image frame in a preset time period as a second reference frame;

finally adjusting the position of the equipment corresponding to the second reference frame to the position where the equipment is located before the preliminary adjustment;

and adjusting the positions of the devices corresponding to the rest historical image frames except the second reference frame in the preset time period based on the position change of the second reference frame in the final adjustment process.

12. The method according to any one of claims 9 to 11, wherein the optimizing the depth of each three-dimensional point after the scale optimization comprises:

and for each three-dimensional point, taking a historical image frame observing the three-dimensional point for the first time as a reference, and adjusting the depth of the three-dimensional point according to the scale, wherein the depth before the three-dimensional point is adjusted is obtained by solving the scale before optimization, and the depth after the three-dimensional point is adjusted is obtained by solving the scale after optimization.

13. The method according to claim 9, wherein before the optimizing the position of the device and/or the depth of each three-dimensional point corresponding to each history image frame after the scale optimization, the method further comprises:

deleting a historical image frame positioned before the motion starting frame in a preset time period;

and/or after the optimization of the position of the device and/or the depth of each three-dimensional point corresponding to each history image frame after the scale optimization, the method comprises the following steps:

increasing an uncertainty of at least one of a position, a velocity, and an accelerometer bias of the device.

14. The method of any of claims 1 to 13, wherein determining the current motion state of the device based on the motion information comprises:

determining the current motion state of the equipment as the static state under the condition that the parallaxes between a plurality of historical image frames and the current image frame are all smaller than a second parallax threshold value and/or the discrete degree of at least one piece of sensing data is smaller than a corresponding preset degree value;

wherein the second disparity threshold is fixed or dynamically adjusted based on a temporal differential between the historical image frame and the current image frame; the at least one sensory data is collected during a first time period prior to a current motion state, including gyroscope sensory data and/or accelerometer sensory data.

15. The method according to any one of claims 1 to 14, wherein the optimizing the second positioning parameter by the second optimization method comprises:

and constraining the second positioning parameter by taking the speed as zero as constraint.

16. The method of claim 15, further comprising:

judging whether a plurality of historical image frames before the current image frame have a preset number of three-dimensional points or not;

under the condition that a plurality of historical image frames do not have a preset number of three-dimensional points, increasing the constraint weight with the speed being zero; and/or, in the case that a preset number of three-dimensional points exist in a plurality of historical image frames, reducing the constraint weight with the speed being zero.

17. A positioning initialization apparatus, comprising:

the device comprises an acquisition module, a processing module and a processing module, wherein the acquisition module is used for acquiring the motion information of the device after the positioning parameters of the device to be positioned are initialized;

a determination module to determine a current motion state of the device based on the motion information;

the optimization module is used for optimizing the positioning parameters according to the current motion state of the equipment to obtain optimized positioning parameters, and the positioning parameters are divided into first positioning parameters and second positioning parameters;

wherein, according to the current motion state of the device, optimizing the positioning parameters to obtain optimized positioning parameters includes:

18. An electronic device comprising a memory and a processor configured to execute program instructions stored in the memory to implement the method of any of claims 1 to 16.

19. A computer readable storage medium having stored thereon program instructions, which when executed by a processor implement the method of any of claims 1 to 16.