Position optimization device and method, electronic device and computer-readable storage medium
By obtaining the camera's previous frame pose information and current pose information, and using prior constraints for iterative optimization, the problem of high computational complexity and insufficient robustness of visual odometer technology is solved, and weak calculations or low sensors are realized Efficient pose optimization and continuous tracking in quality scenarios.
Patent Information
- Application Number
- CN202110320659.5
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2021-03-25
- Publication Date
- 2024-11-26
- Estimated Expiration
- 2041-03-25
AI Technical Summary
The existing visual odometer technology has high computational complexity, relies on hardware performance and sensor data quality affects the algorithm effect, making it difficult to apply smoothly in scenarios with weak computing power or poor sensor data quality.
By obtaining the camera's previous frame pose information and current frame pose information, a prior value is used to generate a prior constraint, and iteratively optimize the current frame pose information, including position, height, orientation and reprojection error constraints, reducing calculations and improve robustness.
In scenarios with weak computing power or poor sensor data quality, efficient optimization and continuous tracking of camera position information are achieved, improving computing efficiency and robustness.
Smart Images

Figure CN113034582B_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the field of positioning and navigation technology, and in particular to a posture optimization device and method, an electronic device and a computer-readable storage medium. Background Art
[0002] Visual odometry technology is a technology that determines and optimizes the robot's position by analyzing image-related sequences. On the one hand, the visual odometry method requires real-time calculation of a large amount of data and is highly dependent on hardware performance. If the algorithm cannot be calculated in real time, it will affect whether it can be successfully applied in certain scenarios. At the same time, the quality of sensor data will also affect the effect of the algorithm and have a great impact on the experience. However, many existing visual odometry methods for smart terminals can only run on terminals with strong computing power and good sensor data quality.
[0003] Therefore, obtaining a visual odometry calculation method with low computational complexity and strong robustness, namely, a pose optimization method, has become an urgent problem to be solved in positioning and navigation technology. Summary of the invention
[0004] The present application provides a posture optimization device and method, an electronic device and a computer-readable storage medium, which can reduce the computational efficiency of posture optimization and improve its robustness.
[0005] In order to solve the above technical problems, the first aspect of the present application provides a posture optimization method. The posture optimization method includes: obtaining the current frame posture information and at least the previous frame posture information of the camera, and using at least the previous frame posture information and / or the current frame posture information to obtain the prior value of the current frame posture information of the camera; using the prior value and the current estimated value of the current frame posture information of the camera to generate a prior constraint on the current frame posture information; and optimizing the current frame posture information using the prior constraint.
[0006] Among them, the above-mentioned optimization of the current frame pose information using prior constraints includes: iteratively optimizing the current frame pose information using prior constraints, wherein the current estimated value in the prior constraints used for this optimization is the pose information of the current frame after the last optimization, and the current estimated value in the prior constraints used for the first optimization is the prior value.
[0007] The present application can implement iterative optimization of the current frame pose information through the above method, so that the accuracy of the pose information after iterative optimization meets the requirements.
[0008] Among them, the posture information includes the position coordinates of the camera, the prior constraints include motion constraints, and the prior values include position coordinate prior values. The above-mentioned steps of obtaining at least the previous frame posture information and the current frame posture information of the camera, and using at least the previous frame posture information and / or the current frame posture information to obtain the prior value of the camera's current frame posture information include: obtaining the first two frames of posture information of the camera; respectively obtaining the first position coordinates and the second position coordinates of the camera in a preset coordinate system from the first two frames of posture information; calculating the first position coordinates and the second position coordinates to obtain the position coordinate prior value; the above-mentioned step of using the prior value and the current estimated value of the camera's current frame posture information to generate the prior constraint on the current frame posture information includes: obtaining the current estimated value of the camera's current frame position coordinates; calculating the difference between the current estimated value of the current frame position coordinates and the position coordinate prior value to obtain the motion constraint.
[0009] The present application obtains a priori value of position coordinates through the first position coordinates and the second position coordinates of the first two frames of the camera, and obtains the difference between the current estimated value of the position coordinates of the current frame of the camera and the priori value of the position coordinates as the camera motion constraint to realize the motion constraint of the camera with small amount of calculation.
[0010] The step of calculating the first position and the second position to obtain a position coordinate prior value includes: obtaining a coordinate difference between the second position coordinate and the first position coordinate; and obtaining the sum of the coordinate difference and the second position coordinate to obtain a position coordinate prior value.
[0011] The present application first obtains the coordinate difference between the second position coordinate and the first position coordinate, and then obtains the sum of the coordinate difference and the second position coordinate as the position coordinate prior value, which can realize the uniform motion constraint of the camera.
[0012] Among them, the posture information includes the height of the camera, the prior constraint includes the height constraint, and the prior value includes the height prior value. The step of obtaining the prior value of the camera's current frame posture information using at least the previous frame posture information and / or the current frame posture information includes: obtaining the previous frame height of the camera from the previous frame posture information as the height prior value; the step of using the prior value to generate and the current estimated value of the camera's current frame posture information to impose a prior constraint on the current frame posture information includes: obtaining the current estimated value of the camera's current frame height; calculating the height difference between the current estimated value of the current frame height and the height prior value to obtain the height constraint.
[0013] Therefore, this application uses the height of the previous frame of the camera as the height prior value of the current frame of the camera, and obtains the height difference between the current estimated value of the current frame height and the height prior value as the height constraint of the current frame of the camera, which can realize the height constraint of the camera with small calculation amount.
[0014] Among them, the posture information includes the orientation information of the camera, the prior constraints include the orientation constraints, and the prior values include the orientation prior values. The step of obtaining the prior value of the current frame posture information of the camera using at least the previous frame posture information and / or the current frame posture information includes: obtaining the orientation information of the camera from the current frame posture information as the orientation prior value; the step of generating the prior constraint on the current frame posture information using the prior value and the current estimated value of the current frame posture information of the camera includes: obtaining the current estimated value of the current frame orientation information of the camera; calculating the logarithm of the product between the conjugate value of the orientation prior value and the current estimated value of the current frame orientation information to obtain the orientation constraint.
[0015] Therefore, the present application uses the orientation information of the previous frame of the camera as the orientation prior value of the orientation information of the current frame of the camera, and obtains the logarithm of the product between the conjugate value of the orientation prior value and the current estimated value of the current frame orientation information as the orientation constraint of the current frame of the camera, which can realize the orientation constraint of the camera with small amount of calculation.
[0016] Among them, the prior constraint includes a reprojection error constraint, the prior value includes a reprojection prior value, and the above-mentioned step of obtaining at least the previous frame pose information and the current frame pose information of the camera, and using at least the previous frame pose information and / or the current frame pose information to obtain the prior value of the camera's current frame pose information includes: obtaining the camera's previous frame pose information, previous frame image information, and current estimated value of the current frame pose information; obtaining the first feature point of the previous frame image information; using the camera's intrinsic parameter and the previous frame pose information to project the first feature point onto the Z plane of the world coordinate system to obtain the second feature point; using the camera's intrinsic parameter and the current estimated value of the camera's current frame pose information to project the second feature point into the current frame pixel coordinate system to obtain the reprojection prior value; the above-mentioned step of obtaining at least the current frame pose information of the camera, and using the prior value to generate the prior constraint on the current frame pose information includes: obtaining the camera's current frame image information; using the optical flow algorithm to obtain the third feature point in the current frame image corresponding to the first feature point; calculating the difference between the reprojection prior value and the third feature point to obtain the reprojection error constraint.
[0017] Therefore, the present application first projects the first feature point in the previous frame image of the camera onto the Z plane of the world coordinate system to obtain the second feature point; then projects the second feature point to the current frame pixel coordinate system to obtain a reprojection prior value; finally, the difference between the reprojection prior value and the third feature point corresponding to the first feature point in the current frame image of the camera is obtained as the reprojection error constraint, which can realize the reprojection error constraint of the camera with small amount of calculation.
[0018] Among them, the prior constraints include motion constraints, height constraints, orientation constraints and reprojection error constraints. The above steps of iteratively optimizing the current frame pose information using the prior constraints include: setting weights for the position coordinate constraints, height constraints, orientation constraints and reprojection error constraints respectively; obtaining the weighted sum of the position coordinate constraints, height constraints, orientation constraints and reprojection error constraints based on the weights; performing nonlinear optimization on the weighted sum, and outputting the optimized camera pose information.
[0019] Therefore, the present application first obtains the weighted sum of the position coordinate constraint, height constraint, orientation constraint and reprojection error constraint of the camera's current frame, and then performs nonlinear constraints on the weighted sum to obtain the final posture information of the camera's current frame.
[0020] In order to solve the above technical problems, the second aspect of the present application provides a posture optimization device. The posture optimization device includes: an acquisition module, which is used to acquire the current frame posture information and at least the previous frame posture information of the camera, and use at least the previous frame posture information and / or the current frame posture information to obtain the prior value of the current frame posture information of the camera; a generation module is coupled to the acquisition module, which is used to generate a priori constraints on the current frame posture information using the prior value and the current estimated value of the current frame posture information of the camera; and an optimization module is coupled to the generation module, which is used to optimize the current frame posture information using the prior constraints.
[0021] In order to solve the above-mentioned technical problems, the third aspect of the present application provides an electronic device, which includes: a memory and a processor coupled to each other, and the processor is used to execute program data stored in the memory to implement any of the above-mentioned posture optimization methods.
[0022] In order to solve the above-mentioned technical problems, the fourth aspect of the present application provides a computer-readable storage medium, which stores program data, and the program data can be executed to implement any of the above-mentioned posture optimization methods.
[0023] In the above scheme, the present application obtains the prior value of the current frame pose information of the camera through at least the previous frame pose information and / or the current frame pose information of the camera, and optimizes the current frame pose information using the prior constraint generated by the prior value and the current estimated value of the current frame pose information of the camera, so as to obtain the optimized current frame pose information. Therefore, the present application can optimize the current frame pose information of the camera only through at least the previous frame pose information and / or the current frame pose information of the camera, which can reduce the amount of calculation, thereby shortening the calculation time of the pose optimization method, and further improving its calculation efficiency; and each frame pose information of the camera of the present application can be optimized using at least the previous frame pose information and / or the current frame pose information, so that each frame pose information of the camera can be optimized to achieve continuous tracking of the camera pose information, which can improve the robustness of the pose optimization method. BRIEF DESCRIPTION OF THE DRAWINGS
[0024] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.
[0025] Figure 1 It is a flow chart of an embodiment of the posture optimization method of the present application;
[0026] Figure 2 yes Figure 1 A schematic diagram of a specific flow chart of step S101 in the posture optimization method of the embodiment;
[0027] Figure 3 yes Figure 2 A schematic diagram of a specific process of step S203 in the embodiment;
[0028] Figure 4 yes Figure 1 A schematic diagram of a specific process of step S101 in the posture optimization method of the embodiment;
[0029] Figure 5 yes Figure 1 A schematic diagram of a specific process of step S102 in the posture optimization method of the embodiment;
[0030] Figure 6 yes Figure 1 A schematic diagram of a specific flow chart of step S102 in the posture optimization method of the embodiment;
[0031] Figure 7 yes Figure 1 A schematic diagram of a specific flow chart of step S102 in the posture optimization method of the embodiment;
[0032] Figure 8 yes Figure 1 A schematic diagram of a specific flow chart of step S102 in the posture optimization method of the embodiment;
[0033] Fig. 9 yes Figure 1 A schematic diagram of a specific flow chart of step S103 in the posture optimization method of the embodiment;
[0034] Fig.10 It is a structural schematic diagram of an embodiment of the posture optimization device of the present application;
[0035] Fig.11 It is a structural schematic diagram of an embodiment of the electronic device of the present application;
[0036] Fig.12It is a structural diagram of an embodiment of a computer-readable storage medium of the present application. DETAILED DESCRIPTION
[0037] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.
[0038] The existing visual odometer calculation method has a high computational complexity and is highly dependent on hardware performance when performing real-time calculations on large amounts of data. The quality of sensor data will also affect the effectiveness of the algorithm, greatly affecting the experience.
[0039] For example, in web application scenarios, the time taken to run web programs is often more than 10 times that of native applications, so the visual mileage calculation method for web pages needs to put time control first. At the same time, the images, accelerometers, gyroscopes, gravimeters, heading meters and other data obtained on the web page do not have accurate timestamps, so there are errors in the data source. Web pages are basically single-threaded, and the frequency of the above data is affected by the performance of the algorithm itself and other parts of the web page (such as web page loading, rendering, etc.), so it is difficult to be effectively guaranteed.
[0040] To solve the above problems, the present application proposes a posture optimization method. The posture optimization method of the embodiment of the present application will be introduced based on the web application scenario. Of course, the web application scenario of the embodiment of the present application can also be used in other application scenarios.
[0041] See also Figure 1 , Figure 1 1 is a flow chart of an embodiment of a posture optimization method of the present application. The posture optimization method of this embodiment includes the following steps:
[0042] Step S101: obtaining current frame pose information and at least previous frame pose information of a camera, and obtaining a priori value of current frame pose information of the camera using at least previous frame pose information and / or current frame pose information.
[0043] Each frame of information from the camera includes a timestamp, position information, and image information. The position information includes the camera's position coordinates, height, and orientation information.
[0044] This embodiment can obtain the previous frame or previous N frames of the camera's posture information, or can obtain the previous frame of the camera's posture information and the previous frame of image information, etc., or obtain the previous N frames of the camera's posture information and the previous N frames of image information, etc.; wherein N is a natural number greater than or equal to 2.
[0045] In this embodiment, the position coordinates of the camera in the i-th frame include the two-dimensional coordinates of the camera in the world coordinate system (p ix , p iy ), the two-dimensional coordinate (p ix , p iy ) and the camera height p iz is the three-dimensional coordinate p of the camera in the world coordinate system i =(p ix , p iy , p iz );The camera's orientation information is the camera's orientation information in the world coordinate system q i =(q iw ,q ix ,q iy ,q iz ), qi is a unit quaternion; the camera’s position information for the i-th frame is {p i ,q i}.
[0046] To simplify the measurement method, the world coordinate system can be set as the camera coordinate system when the system is initialized.
[0047] Among them, the two-dimensional coordinates (p ix , p iy ) is the coordinate of the camera in the i-th frame on the Z plane, and the height of the camera p iz is the height of the camera from the Z plane in the i-th frame.
[0048] In this embodiment, the position information of the camera can be obtained through positioning technology, and the orientation information of the camera can be obtained through an orientation meter or a gyroscope.
[0049] Assuming that the current frame of the camera is the i-th frame, this embodiment obtains at least the previous frame pose information of the camera {p (i-1) ,q (i-1)}, and use at least the previous frame pose information {p (i-1) ,q (i-1)}Get the camera's current frame pose information i ,q i} prior value.
[0050] The prior values of this embodiment include any one or any combination of a position coordinate prior value, a height prior value, a direction prior value and a reprojection prior value.
[0051] Optionally, the a priori value of this embodiment includes a coordinate position a priori value, and this embodiment can be performed as follows: Figure 2 The method shown implements step S101. The method of this embodiment specifically includes steps S201 to S203.
[0052] Step S201: Obtain the first two frames of camera position information.
[0053] Assuming that the current frame of the camera is the i-th frame, the first two frames of the camera are the (i-2)-th frame and the (i-1)-th frame. Get the pose information of the first two frames of the camera {p (i-2) ,q (i-2)} and {p (i-1) ,q (i-1)}.
[0054] Step S202: respectively obtaining the first position coordinates and the second position coordinates of the camera in the preset coordinate system from the first two frames of posture information.
[0055] The preset coordinate system in this embodiment may be a camera coordinate system; in other embodiments, the preset coordinate system may also be a world coordinate system.
[0056] From the camera's (i-2) frame pose information {p (i-2) ,q (i-2)} to obtain the first position coordinates of the camera (p (i-2)x , p (i-2)y ); From the camera's (i-1)th frame pose information {p (i-1) ,q (i-1)} to obtain the second position coordinates (p (i-1)x , p (i-1)y ).
[0057] Step S203: Calculate the first position coordinate and the second position coordinate to obtain a priori value of the position coordinate.
[0058] Specifically, this embodiment can be implemented by Figure 3 The method shown implements step S203. The method of this embodiment specifically includes step S301 and step S302.
[0059] Step S301: Obtain the coordinate difference between the second position coordinates and the first position coordinates.
[0060] Specifically, obtain the second position coordinates (p (i-1)x , p (i-1)y ) coordinate value p (i-1)x With the first position coordinate (p (i-2)x , p (i-2)y ) coordinate value p (i-2)x The first coordinate difference of the second position is obtained (p (i-1)x , p (i-1)y ) coordinate value p (i-1)y With the first position coordinate (p (i-2)x , p (i-2)y ) coordinate value p (i-2)y The second coordinate difference.
[0061] The first coordinate difference represents the movement distance of the camera from the (i-2)th frame to the (i-1)th frame along the coordinate axis X; the second coordinate difference represents the movement distance of the camera from the (i-2)th frame to the (i-1)th frame along the coordinate axis Y.
[0062] Step S302: Obtain the sum of the coordinate difference and the second position coordinate to obtain a position coordinate priori value.
[0063] Specifically, the first coordinate difference and the second position coordinate information (p (i-1)x , p (i-1)y ) coordinate value p (i-1)x The sum is the first coordinate position priori value, and the second coordinate difference and the second position coordinate information (p (i-1)x , p (i-1)y ) coordinate value p (i-1)y The sum is the prior value of the second coordinate position.
[0064] The first coordinate position prior value is the prior value of the camera on the coordinate axis X, so as to constrain the movement of the camera along the coordinate axis X; the second coordinate position prior value is the prior value of the camera on the coordinate axis Y, so as to constrain the movement of the camera along the coordinate axis Y.
[0065] In the web application scenario, when a user uses a mobile phone, the mobile phone is usually stationary or moves slowly relative to the ground, so it can be assumed that the movement of the camera on the mobile phone is uniform motion; therefore, based on the above steps S301 and S302, the coordinate position prior value (i.e., the initial value of the coordinate position) of the camera's i-th frame can be obtained according to the uniform motion between the camera's (i-2)-th frame, (i-1)-th frame, and i-th frame. (i-1) +(p (i-1) -p (i-2) )] xy .
[0066] Optionally, the prior value of this embodiment includes a height prior value. This embodiment may obtain the height of the previous frame of the camera as the height prior value from the previous frame posture information of the camera.
[0067] The camera height can be obtained from cloud-based visual positioning, or through internal Wi-Fi positioning, Bluetooth positioning, etc., and is not limited here.
[0068] In this embodiment, the camera coordinate system is set to the world coordinate system during initialization, so the height of the camera in the world coordinate system is the height in the camera coordinate system, which can be unified with the two-dimensional coordinates of the camera.
[0069] Specifically, from the camera's (i-1)th frame pose information {(p (i-1)x , p (i-1)y , p (i-1z), q (i-1)} to get the camera's (i-1) frame height p (i-1)z is a high prior value.
[0070] In the web application scenario, when the user uses the mobile phone, the height of the mobile phone and the ground is usually constant (or the change is small and can be ignored). Therefore, this embodiment can assume that the user always puts the mobile phone at a fixed height, and does not consider the scenario where the height of the mobile phone fluctuates due to crouching, jumping, etc. Based on the above method, the height of the camera's (i-1)th frame can be used as the height prior value (i.e., the initial height value) p of the camera's i-th frame. (i-1)z .
[0071] Optionally, the priori value of this embodiment includes a direction priori value, and this embodiment can obtain the direction priori value from a direction meter provided by the smart phone system.
[0072] Read the current reading of the smartphone system heading meter (q i_atti_w ,q i_atti_x ,q i_atti_y ,q i_atti_z ) is used as the orientation prior value and also as the initial current estimate of the orientation information to be optimized later.
[0073] Optionally, the prior value of this embodiment includes a reprojection prior value, and this embodiment can be performed as follows: Figure 4 The method shown implements step S101. The method of this embodiment includes steps S401 to S404.
[0074] Step S401: Obtain the camera's previous frame pose information, previous frame image information, and current estimated value of the current frame pose information.
[0075] Get the camera's (i-1)th frame pose information {p (i-1) ,q (i-1)} and the (i-1)th frame image information I (i-1) , the current estimated value of the camera's pose information in the i-th frame {p i ,q i}.
[0076] The image information of the camera can be obtained by capturing the image area of the camera.
[0077] Step S402: Acquire the first feature point of the previous frame image information.
[0078] In this embodiment, an algorithm such as ORB can be used to obtain the image information I of the (i-1)th frame. (i-1) Extract the first feature point Y i-1,n Y i-1,n (1≤n≤N), N is the number of feature points extracted from this image.
[0079] Step S403: Project the first feature point onto the Z plane of the world coordinate system using the camera's intrinsic parameters and the previous frame's pose information to obtain a second feature point.
[0080] The camera intrinsic parameter is used to realize the projection transformation between the point in the camera coordinate system and the point in the pixel coordinate system; the camera intrinsic parameter K can be a 3*3 matrix; for a point p in the camera coordinate system c , the coordinate K·p in the pixel coordinate system c On the contrary, for a point in the pixel coordinate system, it can be projected to the camera coordinate system through the inverse of K.
[0081] Use the camera's internal parameters and the previous frame's pose information to convert the first feature point Y i-1,n Project it into the camera coordinate system, and find the intersection of the ray formed by the origin of the camera coordinate system and the projection point with the Z plane of the world coordinate system. All the intersection points constitute the second feature point x i-1,n (1≤n≤N).
[0082] The second feature point corresponds to the first feature point one by one, the second feature point is located in the world coordinate system, and the first feature point is located in the pixel coordinate system of the previous frame image.
[0083] Step S404: Project the second feature point to the pixel coordinate system of the current frame using the camera's internal parameters and the current estimated value of the camera's current frame pose information to obtain a reprojection prior value. i ,q i} and the second feature point (xi-1,n) in the world coordinate system, project the second feature point (xi-1,n) into the pixel coordinate system of the current frame to obtain the reprojection prior value π(q i * (x i-1,n -p i )).
[0084] Step S102: Generate a priori constraints on the current frame pose information using the priori value and the current estimated value of the camera's current frame pose information.
[0085] The current estimated value of the camera's current frame pose information is the optimization of the last optimization of the current frame pose information. The current estimated value used in the first optimization is the priori value generated in step S101.
[0086] Optionally, in one embodiment, the Figure 5 The method shown implements step S102. The priori value of this embodiment includes a coordinate position priori value, and the method of this embodiment specifically includes step S501 and step S502.
[0087] Step S501: Obtain the current estimated value of the current frame position coordinate of the camera.
[0088] Get the current estimated value of the camera's i-th frame pose information {(p ix , p iy , p iz ), (q iw ,q ix ,q iy ,q iz )}, and from the current estimated value of the pose information of the i-th frame {(p ix , p iy , p iz ), (q iw ,q ix ,q iy ,q iz )}Get the current estimated value of the position coordinates of the i-th frame (p ix , p iy ).
[0089] Step S502: Calculate the difference between the current estimated value of the position coordinate of the current frame and the priori value of the position coordinate to obtain the motion constraint.
[0090] Get the current estimated value of the position coordinates of the i-th frame [p i ] xy With the position coordinate prior value [2p (i-1) -p (i-2) ] xy The difference [p i -2p (i-1) +p (i-2) ] xy as a motion constraint.
[0091] Specifically, obtain the current estimated value p of the position coordinate of the i-th frame ix With the first prior value [2p (i-1) -p (i-2) ] x The difference is used as the motion constraint of the coordinate axis X; get the current estimated value p of the position coordinate of the i-th frame iy p iy With the first prior value [2p (i-1) -p (i-2) ] y The difference is used as the motion constraint of the camera along the coordinate axis Y.
[0092] This embodiment can realize the uniform motion constraint of the camera, and the method has a small amount of calculation, which can improve the calculation efficiency. In other embodiments, the speed change or static constraint of the camera can be realized according to the actual application scene of the camera.
[0093] Alternatively, in another embodiment, the Figure 6 The method shown implements step S102. The priori value of this embodiment includes a height priori value, and the method of this embodiment specifically includes step S601 and step S602.
[0094] Step S601: Obtain the current estimated value of the current frame height of the camera.
[0095] Get the current estimated value of the camera's i-th frame pose information {(p ix , p iy , p iz ), (q iw ,q ix ,q iy ,q iz )}, and from the current estimated value of the pose information of the i-th frame {(p ix , p iy , p iz ), (q iw ,q ix ,q iy ,q iz )} Get the current estimated value p of the height of the i-th frame iz .
[0096] Step S602: Calculate the height difference between the current estimated value of the current frame height and the height prior value to obtain the height constraint.
[0097] Get the current estimate p of the height of the i-th frame iz With the height prior value p (i-1)z The height difference (p iz -p (i-1)z ) as a height constraint.
[0098] The camera height can be obtained from cloud-based visual positioning, or through internal Wi-Fi positioning, Bluetooth positioning, etc., and is not limited here.
[0099] This embodiment can realize the height constraint of the camera, and the method has a small amount of calculation, which can improve the calculation efficiency.
[0100] Optionally, in one embodiment, the Figure 7 The method shown implements step S102. The priori value of this embodiment includes a direction priori value, and the method of this embodiment specifically includes step S701 and step S702.
[0101] Step S701: Obtain the current estimated value of the current frame orientation information of the camera.
[0102] Get the current estimated value of the camera's i-th frame pose information {(p ix , p iy , p iz ), (qiw ,q ix ,q iy ,q iz )}, and from the current estimated value of the pose information of the i-th frame {(p ix , p iy , p iz ), (q iw ,q ix ,q iy ,q iz )} Get the current estimated value q of the orientation information of the i-th frame i =(q iw ,q ix ,q iy ,q iz ).
[0103] Step S702: Calculate the logarithm of the product of the conjugate value of the orientation prior value and the current estimated value of the orientation information of the current frame to obtain the orientation constraint.
[0104] Specifically, first obtain the orientation prior value q i_atti =(q iatti_w ,q i_atti_x ,q i_atti_y ,q i_atti_z )'s conjugate value q i_atti * ; Then first obtain the conjugate value q of the prior value i_atti * and the current estimated value q of the orientation information of the i-th frame i =(q iw ,q ix ,q iy ,q iz ) is the logarithm of the product of i_atti * ·q i ) is the orientation constraint.
[0105] This embodiment can realize the orientation constraint of the camera, and the method has a small amount of calculation and can improve the calculation efficiency.
[0106] Optionally, in one embodiment, the Figure 8 The method shown implements step S102. The priori value of this embodiment includes a reprojection error, and the method of this embodiment specifically includes steps S801 to S803.
[0107] Step S801: Acquire the current frame image information of the camera.
[0108] The acquisition method is similar to the above-mentioned acquisition of the previous frame image information of the camera; because the image acquisition area corresponding to the previous frame of the camera is different from the image acquisition area corresponding to the current frame, the previous frame image information is also different from the current frame image information.
[0109] Step S802: using an optical flow algorithm to obtain a third feature point corresponding to the first feature point in the current frame image.
[0110] Use the optical flow algorithm to obtain the first feature point (Y i-1,n ) corresponds to the third feature point X i,n (1≤n≤N).
[0111] Step S803: Calculate the difference between the reprojection prior value and the third feature point to obtain a reprojection error constraint.
[0112] Get the third feature point (X i,n ) and the reprojection prior value π(q i * (x i-1,n -p i ). Assuming that there are N feature points in the image, the reprojection error constraint is
[0113] This embodiment can realize the reprojection error constraint of the camera, and the method has a small amount of calculation and can improve the calculation efficiency.
[0114] Step S103: Optimize the current frame pose information using prior constraints.
[0115] Specifically, the current frame pose information is iteratively optimized using the prior constraints, where the current estimated value in the prior constraints used in this optimization is the pose information of the current frame after the last optimization, and the current estimated value in the prior constraints used in the first optimization is the prior value
[0116] This embodiment uses prior constraints to iteratively optimize the current frame pose information, and the optimization result of the current frame pose information after each optimization is used as the current estimated value for the next optimization; the prior value is used as the current estimated value for the first optimization.
[0117] The previous frame pose information of the camera refers to the previous frame pose information after iterative optimization.
[0118] From the above analysis, it can be seen that the prior constraints include any one or any combination of motion constraints, height constraints, orientation constraints and reprojection error constraints. Therefore, any one or any combination of these prior constraints can be used to optimize the camera's current frame pose information according to the actual application scenario to output the optimized pose information.
[0119] This embodiment can be implemented by Fig. 9 The method shown implements step S103. The method of this embodiment specifically includes steps S901 to S903.
[0120] Step S901: setting weights for the position coordinate constraint, height constraint, orientation constraint and reprojection error constraint respectively.
[0121] They are motion constraints [p i -2p (i-1) +p (i-2) ] xy Setting weights For height constraints (p iz -p (i-1)z )Set weight is the orientation constraint log(q i_atti * ·q i )Set weight is the reprojection error constraint Setting weights
[0122] Step S902: Obtaining the weighted sum of the position coordinate constraint, the height constraint, the orientation constraint, and the reprojection error constraint based on the weights.
[0123] Get the weighted sum of the above constraints
[0124] Step S903: Perform nonlinear optimization on the weighted sum and output the optimized camera position information.
[0125] In this embodiment, a nonlinear least square method can be used to obtain the weighted sum of the position coordinate constraint, height constraint, orientation constraint and reprojection error constraint as the minimum value. The corresponding camera pose information is used as the optimized current frame pose information.
[0126] In other embodiments, the Levenberg-Marquardt nonlinear fitting method or the like may also be used.
[0127] Different from the prior art, the present embodiment obtains the prior value of the current frame pose information of the camera through at least the previous frame pose information and / or the current frame pose information of the camera, and optimizes the current frame pose information using the prior constraint generated by the prior value and the current estimated value of the current frame pose information of the camera, so as to obtain the optimized current frame pose information. Therefore, the present embodiment can optimize the current frame pose information of the camera only through at least the previous frame pose information and / or the current frame pose information of the camera, which can reduce the amount of calculation, thereby shortening the calculation time of the pose optimization method, and further improving its calculation efficiency; and each frame pose information of the camera of the present embodiment can be optimized using at least the previous frame pose information and / or the current frame pose information, so that each frame pose information of the camera can be optimized to achieve continuous tracking of the camera pose information, which can improve the robustness of the pose optimization method.
[0128] Furthermore, in response to i being 1, i.e., the first frame of the camera, the camera position is {(0, 0, h, q1}; in response to i being 2, the initial value (i.e., the prior value) of the camera position information of the second frame is set to {(0, 0, h, q1}, and the weight of the motion constraint is set is zero, that is, no motion constraint is applied to the second frame pose information of the camera; in response to i being greater than or equal to 3, the weight of the motion constraint is set Highly constrained weights Weight of the orientation constraint and the weight of the reprojection error constraint bit is non-zero.
[0129] This application further proposes a posture optimization device, such as Fig.10 As shown, Fig.10 It is a structural diagram of an embodiment of the posture optimization device of the present application. The posture optimization device 100 of this embodiment includes: an acquisition module 111, a generation module 112 and an optimization module 113; wherein the acquisition module 111 is used to acquire the current frame posture information and at least the previous frame posture information of the camera, and obtain the prior value of the current frame posture information of the camera using at least the previous frame posture information and / or the current frame posture information; the generation module 112 is coupled to the acquisition module 111, and is used to generate a priori constraints on the current frame posture information using the priori value and the current estimated value of the current frame posture information of the camera; the optimization module 113 is coupled to the generation module 112, and is used to optimize the current frame posture information using the priori constraints.
[0130] Different from the prior art, the posture optimization device 100 of this embodiment can optimize the current frame posture information of the camera only through at least the previous frame posture information and / or the current frame posture information of the camera, which can reduce the amount of calculation, thereby shortening the calculation time of the posture optimization method, and further improving its calculation efficiency; and each frame posture information of the camera can be optimized using at least the previous frame posture information and / or the current frame posture information, so that each frame posture information of the camera can be optimized to achieve continuous tracking of the camera posture information, which can improve the robustness of the posture optimization method.
[0131] The posture optimization device 100 of this embodiment is also used to implement the above-mentioned posture optimization method, which will not be repeated here.
[0132] The present application further proposes an electronic device, such as Fig.11 As shown, Fig.11 The electronic device 80 of this embodiment includes a processor 81 , a memory 82 , an input / output device 83 and a bus 84 .
[0133] The processor 81, memory 82, and input / output device 83 are respectively connected to a bus 84. The memory 82 stores program data, and the processor 81 is used to execute the program data to implement the posture optimization method of the above embodiment.
[0134] In this embodiment, the processor 81 may also be referred to as a CPU (Central Processing Unit). The processor 81 may be an integrated circuit chip having signal processing capabilities. The processor 81 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 devices, discrete gate or transistor logic devices, discrete hardware components. A general-purpose processor may be a microprocessor or the processor 81 may also be any conventional processor, etc.
[0135] The present application further proposes a computer-readable storage medium, such as Fig.12 As shown, Fig.12 The computer-readable storage medium 121 stores program instructions 122 thereon, and when the program instructions 122 are executed by a processor (not shown), the above-mentioned posture optimization method is implemented.
[0136] The computer-readable storage medium 121 of this embodiment may be, but is not limited to, a USB flash drive, an SD card, a PD optical drive, a mobile hard disk, a large-capacity floppy drive, a flash memory, a multimedia memory card, a server, and the like.
[0137] Different from the prior art, the posture optimization method of the embodiment of the present application includes: obtaining the current frame posture information and at least the previous frame posture information of the camera, and using at least the previous frame posture information and / or the current frame posture information to obtain the prior value of the current frame posture information of the camera; using the prior value and the current estimated value of the current frame posture information of the camera to generate the prior constraint on the current frame posture information; and optimizing the current frame posture information using the prior constraint. The present application obtains the prior value of the current frame posture information of the camera through at least the previous frame posture information and / or the current frame posture information of the camera, and optimizes the current frame posture information using the prior value and the prior constraint generated by the current estimated value of the current frame posture information of the camera, so as to obtain the optimized current frame posture information. Therefore, the present application can optimize the current frame pose information of the camera only through at least the previous frame pose information and / or the current frame pose information of the camera, which can reduce the amount of calculation, thereby shortening the calculation time of the pose optimization method, and further improving its calculation efficiency; and each frame pose information of the camera of the present application can be optimized using at least the previous frame pose information and / or the current frame pose information, so that each frame pose information of the camera can be optimized to achieve continuous tracking of the camera pose information, which can improve the robustness of the pose optimization method.
[0138] In addition, if the above functions are implemented in the form of software functions and sold or used as independent products, they can be stored in a storage medium readable by a mobile terminal, that is, the present application also provides a storage device storing program data, the program data can be executed to implement the method of the above embodiment, and the storage device can be, for example, a USB flash drive, an optical disk, a server, etc. In other words, the present application can be embodied in the form of a software product, which includes a number of instructions for enabling a smart terminal to execute all or part of the steps of the method described in each embodiment.
[0139] In the description of the present application, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present application. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art may combine and combine the different embodiments or examples described in this specification and the features of the different embodiments or examples, without contradicting each other.
[0140] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include at least one of the features. In the description of this application, the meaning of "plurality" is at least two, such as two, three, etc., unless otherwise clearly and specifically defined.
[0141] Any process or method description in a flowchart or otherwise described herein may be understood to represent a module, segment or portion of code that includes one or more executable instructions for implementing the steps of a specific logical function or process, and the scope of the preferred embodiments of the present application includes alternative implementations in which functions may not be performed in the order shown or discussed, including performing functions in a substantially simultaneous manner or in the reverse order depending on the functions involved, which should be understood by technicians in the technical field to which the embodiments of the present application belong.
[0142] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as an ordered list of executable instructions for implementing the logical functions, and can be embodied in any computer-readable medium for use by an instruction execution system, device or apparatus (which can be a personal computer, server, network device or other system that can fetch instructions from the instruction execution system, device or apparatus and execute the instructions), or in combination with these instruction execution systems, devices or apparatuses. For the purpose of this specification, "computer-readable medium" can be any device that can contain, store, communicate, propagate or transmit a program for use by an instruction execution system, device or apparatus or in combination with these instruction execution systems, devices or apparatuses. More specific examples of computer-readable media (a non-exhaustive list) include the following: an electrical connection with one or more wires (electronic device), a portable computer disk box (magnetic device), a random access memory (RAM), a read-only memory (ROM), an erasable and programmable read-only memory (EPROM or flash memory), a fiber optic device, and a portable compact disk read-only memory (CDROM). In addition, the computer-readable medium may even be paper or other suitable medium on which the program is printed, since the program may be obtained electronically, for example, by optically scanning the paper or other medium and then editing, interpreting or processing in other suitable ways if necessary, and then stored in a computer memory.
[0143] The above description is only an implementation method of the present application, and does not limit the patent scope of the present application. Any equivalent structure or equivalent process transformation made using the contents of the present application specification and drawings, or directly or indirectly used in other related technical fields, are also included in the patent protection scope of the present application.
Claims
1. A posture optimization method, characterized in that: The posture optimization method comprises: Acquire current frame pose information and at least previous frame pose information of the camera, and use the at least previous frame pose information and / or the current frame pose information to obtain a priori value of the current frame pose information of the camera; wherein the priori value includes at least one of a position coordinate prior value, a height prior value, a direction prior value, and a reprojection prior value; Generate a priori constraints on the current frame pose information using the priori value and a current estimated value of the current frame pose information of the camera; Optimizing the current frame pose information using the prior constraints; The optimizing the current frame pose information by using the prior constraints comprises: The current frame pose information is iteratively optimized using the prior constraints, wherein the current estimated value in the prior constraints used for this optimization is the pose information of the current frame after the last optimization, and the current estimated value in the prior constraints used for the first optimization is the prior value.
2. The posture optimization method according to claim 1, characterized in that: The pose information includes the position coordinates of the camera, the prior constraints include motion constraints, the prior values include the position coordinate prior values, and the steps of obtaining at least the previous frame pose information and the current frame pose information of the camera, and obtaining the prior value of the current frame pose information of the camera using at least the previous frame pose information and / or the current frame pose information include: Obtaining the first two frames of pose information of the camera; Acquire the first position coordinates and the second position coordinates of the camera in a preset coordinate system respectively from the first two frames of posture information; Calculating the first position coordinate and the second position coordinate to obtain the position coordinate prior value; The step of generating a priori constraints on the current frame pose information by using the priori value and the current estimated value of the current frame pose information of the camera comprises: Obtaining a current estimate of the current frame position coordinates of the camera; The difference between the current estimated value of the current frame position coordinate and the priori value of the position coordinate is calculated to obtain the motion constraint.
3. The posture optimization method according to claim 2, characterized in that: The step of calculating the first position and the second position to obtain the position coordinate prior value comprises: Obtaining a coordinate difference between the second position coordinate and the first position coordinate; The sum of the coordinate difference and the second position coordinate is obtained to obtain the position coordinate prior value.
4. The posture optimization method according to claim 1, characterized in that: The pose information includes the height of the camera, the prior constraint includes the height constraint, the prior value includes the height prior value, and the step of obtaining the prior value of the current frame pose information of the camera using at least the previous frame pose information and / or the current frame pose information includes: Acquire the previous frame height of the camera from the previous frame posture information as the height prior value; The step of generating a priori constraints on the current frame pose information by using the priori value and the current estimated value of the current frame pose information of the camera comprises: Obtaining a current estimate of the camera's current frame height; The height difference between the current estimated value of the current frame height and the height prior value is calculated to obtain the height constraint.
5. The posture optimization method according to claim 1, characterized in that: The pose information includes the orientation information of the camera, the prior constraint includes the orientation constraint, the prior value includes the orientation prior value, and the step of obtaining the prior value of the current frame pose information of the camera using at least the previous frame pose information and / or the current frame pose information includes: Acquiring the orientation information of the camera from the current frame pose information as the orientation prior value; The step of generating a priori constraints on the current frame pose information by using the priori value and the current estimated value of the current frame pose information of the camera comprises: Obtaining a current estimate of the current frame orientation information of the camera; The logarithm of the product of the conjugate value of the orientation prior value and the current estimated value of the current frame orientation information is calculated to obtain the orientation constraint.
6. The posture optimization method according to claim 1, characterized in that: The prior constraint includes a reprojection error constraint, the prior value includes the reprojection prior value, and the step of obtaining at least a previous frame pose information and a current frame pose information of the camera, and obtaining the prior value of the current frame pose information of the camera using at least the previous frame pose information and / or the current frame pose information includes: Obtaining the current estimated value of the previous frame pose information, the previous frame image information, and the current frame pose information of the camera; Acquire a first feature point of the previous frame image information; Projecting the first feature point onto the Z plane of the world coordinate system using the intrinsic parameters of the camera and the previous frame pose information to obtain a second feature point; Projecting the second feature point into a pixel coordinate system of the current frame using the intrinsic parameters of the camera and the current estimated value of the current frame pose information of the camera to obtain a reprojection prior value; The step of obtaining at least current frame pose information of the camera and generating a priori constraints on the current frame pose information using the priori value comprises: Obtaining current frame image information of the camera; Acquire a third feature point corresponding to the first feature point in the current frame image by using an optical flow algorithm; A difference between the reprojection prior value and the third feature point is calculated to obtain the reprojection error constraint.
7. The posture optimization method according to any one of claims 1 to 6, characterized in that: The prior constraints include motion constraints, height constraints, orientation constraints and reprojection error constraints. The step of iteratively optimizing the current frame pose information using the prior constraints comprises: Set weights for the motion constraint, the height constraint, the orientation constraint, and the reprojection error constraint, respectively; Obtaining a weighted sum of the motion constraint, the height constraint, the orientation constraint, and the reprojection error constraint based on the weights; The weighted sum is nonlinearly optimized, and the optimized position and posture information of the camera is output.
8. A posture optimization device, characterized in that: The posture optimization device comprises: An acquisition module, used to acquire current frame pose information and at least previous frame pose information of a camera, and to obtain a priori value of the current frame pose information of the camera using the at least previous frame pose information and / or the current frame pose information; A generating module, coupled to the acquiring module, for generating a priori constraints on the current frame pose information using the priori value and a current estimated value of the current frame pose information of the camera; An optimization module, coupled to the generation module, for optimizing the current frame pose information using the prior constraints; The optimization module uses the prior constraints to iteratively optimize the current frame pose information; the current estimated value in the prior constraints used in this optimization is the pose information of the current frame after the last optimization, and the current estimated value in the prior constraints used in the first optimization is the prior value; The prior value includes at least one of a position coordinate prior value, a height prior value, a direction prior value, and a reprojection prior value.
9. An electronic device, characterized in that: The electronic device comprises: a memory and a processor coupled to each other, wherein the processor is used to execute program data stored in the memory to implement the posture optimization method described in any one of claims 1 to 7.
10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores program data, and the program data can be executed to implement the posture optimization method as described in any one of claims 1-7.
