Relative posture estimation method, device, electronic equipment and medium

Abstract

The present application discloses a relative pose estimation method, device, electronic device and medium. The method includes the following steps: obtaining measurement data collected by a sensor and an estimated relative pose between a first image and a second image, the measurement data including first color information and first depth information of the first image, second color information and second depth information of the second image; determining the projection point of the first image on the second image based on the first depth information and the estimated relative pose; calculating the color difference between the first image and the second image based on the first color information, the second color information and the projection point; calculating the depth difference between the first image and the second image based on the first depth information, the second depth information and the projection point; optimizing the estimated relative pose based on the color difference and the depth difference to obtain the target relative pose. In the above manner, the present application can reduce the error of relative pose estimation of depth images.

Description

Technical Field

[0001] The present application relates to the field of computer vision technology, and in particular to a relative posture estimation method, device, electronic device and computer storage medium. Background Art

[0002] Using depth cameras to obtain depth images has very wide applications. Pose estimation, map construction, and 3D reconstruction based on depth images are current research hotspots in computer vision.

[0003] The iterative closest point (ICP) algorithm, as a classic pose estimation algorithm, only uses depth information, resulting in low pose estimation accuracy. Summary of the invention

[0004] The present application provides a relative posture estimation method, device, electronic device and medium to solve the problem of large error in posture estimation based on depth images in related technologies.

[0005] In order to solve the above technical problems, the present application provides a relative pose estimation method. The method includes: obtaining measurement data collected by a sensor and an estimated relative pose between a first image and a second image, the measurement data including first color information and first depth information of the first image, second color information and second depth information of the second image; determining the projection point of the first image on the second image based on the first depth information and the estimated relative pose; calculating the color difference between the first image and the second image based on the first color information, the second color information and the projection point; calculating the depth difference between the first image and the second image based on the first depth information, the second depth information and the projection point; optimizing the estimated relative pose based on the color difference and the depth difference to obtain the target relative pose.

[0006] In a possible implementation, the measurement data also includes first inertial information collected at the same time as the first image, and second inertial information collected at the same time as the second image. The method also includes: calculating the inertial difference between the first image and the second image based on the first inertial information, the second inertial information and the estimated relative pose; optimizing the estimated relative pose based on color difference and depth difference to obtain the target relative pose, including: optimizing the estimated relative pose based on color difference, depth difference and inertial difference to obtain the target relative pose.

[0007] In a possible implementation, the estimated relative pose is optimized based on the color difference, the depth difference and the inertia difference to obtain the target relative pose, including: obtaining weights of the color difference, the depth difference and the inertia difference; performing weighted summation of the color difference, the depth difference and the inertia difference according to the weights to obtain a total difference; in response to the total difference not meeting a preset condition, adjusting the estimated relative pose until the total difference meets the preset condition; and using the adjusted estimated relative pose that makes the total difference meet the preset condition as the target relative pose.

[0008] In one possible implementation, the estimated relative pose is optimized based on color difference, depth difference and inertia difference to obtain the target relative pose, including: adjusting the estimated relative pose to obtain a first target relative pose that minimizes the color difference; adjusting the estimated relative pose to obtain a second target relative pose that minimizes the depth difference; adjusting the estimated relative pose to obtain a third target relative pose that minimizes the inertia difference; and fusing the first target relative pose, the second target relative pose and the third target relative pose to obtain the target relative pose.

[0009] In one possible implementation, the estimated relative posture includes an estimated rotation matrix, the first inertial information includes first gravity information, the second inertial information includes second gravity information, and the inertial difference between the first image and the second image under the estimated relative posture is calculated based on the first inertial information, the second inertial information and the estimated relative posture, including: rotating the first gravity information according to the estimated rotation matrix; calculating the square of the difference between the second gravity information and the rotated first gravity information to obtain the inertial difference.

[0010] In a possible implementation, it also includes: the estimated relative posture includes an estimated rotation matrix, the first inertial information includes first angular velocity information, the second inertial information includes second angular velocity information, and the inertial difference between the first image and the second image under the estimated relative posture is calculated according to the first inertial information, the second inertial information and the estimated relative posture, including: determining the timestamps of the first angular velocity information and the second angular velocity information, and calculating the integration time between the first angular velocity information and the second angular velocity information; integrating the first angular velocity information according to the integration time to obtain the rotation amount; and calculating the inertial difference according to the difference between the estimated rotation matrix and the rotation amount.

[0011] In a possible implementation, the first image includes multiple first pixel points, and based on the first depth information and the estimated relative posture, the projection point of the first image on the second image is determined, including: according to the pixel coordinates of each first pixel point, obtaining the first depth value corresponding to each first pixel from the first depth information; according to the first depth value, back-projecting each first pixel point to the first camera coordinate system to obtain a first three-dimensional point; according to the estimated relative posture, converting each first three-dimensional point to the second camera coordinate system to obtain a second three-dimensional point; projecting each second three-dimensional point to the second pixel coordinate system of the second image to obtain the projection point corresponding to the first pixel point in the second image.

[0012] In a possible implementation, the first color information, the second color information and the projection point calculate the color difference between the first image and the second image, including: pairing each first pixel point with the projection point corresponding to the first pixel point to obtain multiple matching point pairs; obtaining the first color value of each first pixel point according to the first color information, and obtaining the second color value of each projection point according to the second color information; respectively calculating the color difference between the first color value and the second color value of each matching point pair; squaring the color differences of multiple matching point pairs and then summing them to obtain the color difference.

[0013] In one possible implementation, the first depth information includes first depth values ​​of multiple first pixel points in the first image, and based on the first depth information, the second depth information and the estimated relative pose, the depth difference between the first image and the second image in the estimated relative pose is calculated, including: pairing each first pixel point with a projection point corresponding to the first pixel point to obtain multiple matching point pairs; calculating the second depth value of each second three-dimensional point; obtaining the third depth value of the projection point according to the second depth information; respectively calculating the depth difference between the second depth value and the third depth value of each matching point pair; squaring the depth differences of multiple matching point pairs and then summing them to obtain the depth difference.

[0014] In a possible implementation, before determining the target relative pose of the first image and the second image, the method further includes: performing the same image pyramid downsampling on the first image and the second image to obtain a first image pyramid and a second image pyramid of a target number of layers, and recording the topmost layer of the first image pyramid and the second image pyramid as the target layer; determining the target relative pose of the first image and the second image includes: determining the target relative pose of the first image and the second image of the target layer; judging whether the target layer is the bottommost layer of the first image pyramid and the second image pyramid; in response to the target layer being the bottommost layer, determining the target relative pose of the target layer as the final target relative pose; in response to the target layer not being the bottommost layer, using the next layer of the target layer as a new target layer, recording the target relative pose of the target layer as the estimated relative pose of the new target layer, and returning to execute "determining the target relative pose of the first image and the second image of the target layer".

[0015] In order to solve the above technical problems, the present application provides a relative pose estimation device. The device includes an acquisition module, a reprojection module and an optimization module; the acquisition module is used to acquire measurement data collected by a sensor, the measurement data including first color information and first depth information of a first image, second color information and second depth information of a second image, and an estimated relative pose between the first image and the second image; the reprojection module is used to determine the projection point of the first image on the second image based on the first depth information and the estimated relative pose; the pose optimization module is used to calculate the color difference between the first image and the second image based on the first color information, the second color information and the projection point; calculate the depth difference between the first image and the second image based on the first depth information, the second depth information and the projection point; optimize the estimated relative pose based on the color difference and the depth difference to obtain the target relative pose.

[0016] To solve the above technical problems, the present application provides an electronic device. The electronic device includes a processor and a memory; the processor is coupled to the memory and executes instructions when working to cooperate with the memory to implement the above relative posture estimation method.

[0017] To solve the above technical problem, the present application provides a computer storage medium. The computer storage medium stores a computer program, and the computer program is executed to implement the steps of the relative pose estimation method.

[0018] The present application calculates the color difference and depth difference between the first image and the second image respectively, and iteratively updates the estimated relative pose based on the color difference and depth difference, making full use of the color information and depth information of the pixels in the image, so that the obtained relative pose of the target is more accurate. Furthermore, the present application also optimizes the estimated relative pose in combination with inertial information, which can further reduce the error of target pose estimation caused by weak texture features in the depth image. The relative pose of the first image and the second image is optimized layer by layer using an image pyramid, which reduces the requirements for the initial estimated relative pose and can further improve the accuracy of the target relative pose. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 It is a flowchart of the first embodiment of the relative posture estimation method provided by the present application;

[0020] Figure 2 It is a flowchart of an implementation method of determining the projection point of the first image on the second image based on the first depth information and the estimated relative posture provided by the present application;

[0021] Figure 3 is a flowchart of a second embodiment of a relative posture estimation method provided by the present application;

[0022] Figure 4 It is a flowchart of the third embodiment of the relative posture estimation method provided by the present application;

[0023] Figure 5 It is a structural schematic diagram of an embodiment of a relative posture estimation device provided by the present application;

[0024] Figure 6 It is a structural schematic diagram of an embodiment of an electronic device provided by the present application;

[0025] Figure 7 It is a structural diagram of an embodiment of a computer storage medium provided by the present application. DETAILED DESCRIPTION

[0026] In order to enable those skilled in the art to better understand the technical solution of the present invention, the relative posture estimation method, device, electronic device and medium provided in the present application are further described in detail below in combination with the accompanying drawings and specific implementation methods.

[0027] See also Figure 1 , Figure 1 1 is a flow chart of the first embodiment of the relative posture estimation method provided by the present application. This embodiment includes the following steps:

[0028] S110: Acquire measurement data collected by the sensor and an estimated relative pose between the first image and the second image.

[0029] The sensor includes, for example, an image sensor, and the measurement data includes, for example, first color information and first depth information of a first image, and second color information and second depth information of a second image.

[0030] In this embodiment, the first image is a depth image captured by the depth camera at the current moment, and the second image is a depth image acquired by the depth camera at the previous moment. Of course, the first image may also refer to the depth image acquired by the depth camera at the previous moment, and the second image may refer to the depth image acquired by the depth camera at the current moment, and this application does not limit this.

[0031] Compared with traditional cameras, the depth camera has a depth measurement function added to it, which can obtain the distance between the depth camera and the surface of objects in the environment, that is, the depth data. In this embodiment, the depth camera can be a RGB-D camera. When the RGB-D camera outputs a depth image, the RGB data of each pixel in the depth image is aligned with the depth data, that is, the data corresponding to the same pixel coordinates in the RGB data and the depth data are collected from the same point in the three-dimensional space.

[0032] The first color information and the second color information may refer to the grayscale value of each pixel in the first image and the second image. The grayscale value is obtained by grayscale conversion of the RGB data of the pixel, and the grayscale conversion method may be a floating point algorithm, an integer method, an average method or a shift method.

[0033] The estimated relative pose represents the amount of pose change between the initially estimated first image and the second image. The estimated relative pose can be obtained by roughly matching the first image and the second image, for example, by calculating it through a sparse direct method. Specifically, a small number (tens or hundreds) of feature points are extracted from the first image and the second image respectively, and the estimated relative pose is obtained by minimizing the brightness error between the feature points in the first image and the second image. The sparse direct method uses fewer feature points and does not need to calculate the descriptors of the feature points, so that the estimated relative pose can be calculated quickly, improving the efficiency of pose estimation.

[0034] For an electronic device equipped with an odometer, a magnetometer, or an accelerometer, an estimated relative posture may be calculated based on the odometer data, the magnetometer data, or the acceleration data.

[0035] Of course, when the interval between capturing the first image and the second image is short, the relative posture at the moment of capturing the second image can also be used as the estimated relative posture at the current moment.

[0036] S120: Determine a projection point of the first image on the second image based on the first depth information and the estimated relative pose.

[0037] According to the first depth information, the first image can be back-projected into a three-dimensional space to obtain a first three-dimensional point cloud of the first image, and according to the estimated relative posture, the first three-dimensional point cloud can be transformed and projected into a second pixel coordinate system to obtain a reprojected first image. In this way, the first color information and the second color information can be further used to calculate the color difference of the same pixel coordinates in the reprojected first image and the second image, and the first depth information and the second depth information can be used to calculate the depth difference of the same pixel coordinates in the reprojected first image and the second image, so as to make full use of the pixel information in the first image and the second image, so as to make the posture estimation more accurate.

[0038] Among them, the first image includes multiple first pixel points. The first pixel points can be all the pixel points in the first image, or they can be some representative pixel points (feature points). When the first pixel point is a feature point, it can be extracted using an algorithm such as the FAST (Features from Accelerated Segments Test) algorithm or the ORB (Oriented Fast and Rotated Brief) algorithm. The number of first pixel points can be hundreds, thousands, tens of thousands or even hundreds of thousands. If the number of first pixel points is small, the calculation speed is faster and the requirements for the processor are lower; the larger the number of first pixel points, the more accurate the evaluation of the estimated posture, but the processing speed will decrease and the requirements for the processor will be higher. The number of first pixel points can be set according to actual needs, and this application does not impose any restrictions on this.

[0039] See also Figure 2 , Figure 2 This is a flow chart of an implementation method of determining the projection point of the first image on the second image based on the first depth information and the estimated relative posture provided by the present application. The specific steps are as follows:

[0040] S121: Acquire a first depth value corresponding to each first pixel from first depth information according to the pixel coordinates of each first pixel point.

[0041] In this embodiment, it is assumed that the position of the feature point in the three-dimensional space is fixed, and the brightness of the same feature point in the first image and the second image remains unchanged.

[0042] The first pixel point includes the pixel coordinates of the feature point in the first image. According to the pixel coordinates of the first pixel point, a first depth value of the first pixel point can be read from the first depth information.

[0043] S122: According to the first depth value, back-project the first pixel point into a first camera coordinate system to obtain a first three-dimensional point.

[0044] According to the internal parameters of the depth camera and the first depth value of the first pixel, each first pixel can be back-projected into the three-dimensional space using a back-projection function to obtain a plurality of first three-dimensional points in the first camera coordinate system. The first camera coordinate system is a three-dimensional coordinate system in the camera pose when the first image is acquired.

[0045] S123: Convert the first three-dimensional point to a second camera coordinate system according to the estimated relative pose to obtain a second three-dimensional point.

[0046] Using the estimated relative pose, each first three-dimensional point is transformed into a second camera coordinate system (ie, the second camera coordinate system) to obtain a plurality of second three-dimensional points.

[0047] S124: Project the second three-dimensional point to a second pixel coordinate system of the second image to obtain a projection point corresponding to the first pixel point in the second image.

[0048] In this embodiment, the pixel coordinate system where the first image is located is the first pixel coordinate system, and the pixel coordinate system where the second image is located is the second pixel coordinate system.

[0049] According to the internal parameters of the depth camera, each second three-dimensional point is projected to the second pixel coordinate system using a projection function to obtain a projection point corresponding to each first pixel point on the second image. The internal parameters of the depth camera include focal length and pixel size.

[0050] The first pixel point is paired with the projection point corresponding to the first pixel point to obtain a plurality of matching point pairs. The projection point paired with the first pixel point is determined on the second image using a reprojection method, so that the matching point pairs can be obtained quickly, which can improve the efficiency of pose estimation.

[0051] If the estimated relative pose is consistent with the actual relative pose, the coordinates of the projection point are consistent with the coordinates of the feature point corresponding to the projection point in the second image. However, in general, there is a certain deviation between the estimated relative pose and the actual relative pose, resulting in the inconsistency between the coordinates of the projection point and the feature point corresponding to the projection point in the second image. Therefore, it is necessary to optimize the estimated relative pose so that the coordinates of the projection point and the feature point corresponding to the projection point in the second image are consistent or nearly consistent.

[0052] S130: Calculate a color difference between the first image and the second image based on the first color information, the second color information, and the projection point.

[0053] According to the pixel coordinates of each first pixel, the gray value of each first pixel in the first color information is read as the first color value, and the gray value of each projection point in the second color information is read as the second color value.

[0054] The color difference between the first color value and the second color value of each matching point pair is calculated, and the color difference between the first image and the second image is obtained by squared and summed color differences of all matching point pairs.

[0055] The formula for calculating color difference is as follows:

[0056]

[0057] P=π -1 (x,D c [x])

[0058] Among them, E color represents the color difference, x represents the pixel coordinate of the first pixel, I c [x] represents the first color value of the first pixel, Represents the second color value of the projection point, P represents the first depth value of the coordinate x according to the camera internal parameter K c The first pixel point of [x] is back-projected to the first three-dimensional point in three-dimensional space. π(·) represents the projection point obtained by projecting the second three-dimensional point to the second pixel coordinate system according to the camera internal parameter K. T is the estimated relative pose, Lie algebra representing the change of posture. ξ=[α β γ t x t y t z ], is the optimization variable. The first three dimensions of the optimization variable ξ [αβγ] represent the rotation angles of the first three-dimensional point along the x, y, and z axes. The last three dimensions of the optimization variable ξ [t x t y t z ] represents the translation of the first three-dimensional point along the x, y, and z axes.

[0059] S140: Calculate a depth difference between the first image and the second image based on the first depth information, the second depth information, and the projection point.

[0060] Specifically, the depth difference between the first image and the second image in this embodiment refers to the difference between the depth value of the first image after back-projection and posture transformation and the depth value of the projection point. That is, the depth difference is obtained by comparing the second depth value of the second three-dimensional point with the third depth value of the projection point.

[0061] According to the pixel coordinates of the projection point, the second depth value of each projection point can be read from the second depth information.

[0062] Calculate the depth difference of each matching point pair, square each depth difference and sum them to get the depth difference. The depth difference formula is as follows:

[0063]

[0064] Among them, E depth represents the depth difference, represents a second depth value of a second three-dimensional point, The third depth value of the projection point is indicated. The meanings of other symbols are the same as those in S130, so they are not repeated here.

[0065] S150: Optimize and estimate the relative pose based on the color difference and the depth difference to obtain the target relative pose.

[0066] There are many methods to solve the relative pose of the target based on color difference and depth difference.

[0067] For example, the color difference and the depth difference may be weightedly summed to obtain a total difference, and the estimated relative pose may be iteratively updated to obtain an adjusted estimated relative pose that minimizes the total difference as the target relative pose.

[0068] The total difference formula is as follows:

[0069] E = argmin ξ (w1E rgb +w2E depth )

[0070] Where E represents the total difference, w1 is the weight of the color difference, and w2 is the weight of the depth difference. The values ​​of w1 and w2 can be set according to the accuracy of the depth sensor or photosensitive element of the depth camera. The higher the sensor accuracy, the greater the corresponding weight. The smaller the value of the total difference, the higher the accuracy of the estimated relative pose after the iteration update, and the closer it is to the actual relative pose.

[0071] Solve the Lie algebra that minimizes the total difference in the current iteration, use the Lie algebra to update the estimated relative pose, and obtain the updated estimated relative pose. Among them, the optimization algorithm for solving the minimum total difference can be the LM (Levenberg-Marquardt) algorithm or the GN (Gauss-Newton) algorithm, or a combination of the two. Lie algebra to Lie group satisfies the exponential mapping relationship, and Lie group to Lie algebra satisfies the logarithmic mapping relationship. The Lie group includes the above-mentioned six-dimensional optimization variables. By minimizing the total difference, the optimization variable that minimizes the total difference value in the current iteration is obtained, so that the Lie algebra of the current iteration can be obtained according to the mapping relationship between the Lie group and the Lie algebra.

[0072] Lie algebra is used to update the estimated relative pose, the formula is as follows:

[0073]

[0074] Determine whether the number of iteration updates or the total difference satisfies the preset conditions. If the preset conditions are met, stop the iteration and output the updated estimated relative pose as the target relative pose. If the preset conditions are not met, use the updated estimated relative pose as the estimated relative pose for the next iteration, and execute step S120 to continue iterating to optimize the estimated relative pose.

[0075] Among them, the number of iterative updates meeting the preset condition means, for example, that the number of iterative updates is greater than the iteration threshold. The iteration threshold may be 4 times, 5 times, 6 times, or more than 6 times. The iteration threshold may be determined according to the resolution of the depth image. The higher the resolution of the depth image, the larger the iteration threshold, such as 6 times, 8 times, or 10 times; the higher the resolution of the depth image, the smaller the iteration threshold may be, such as 3 times or 4 times, etc., and this application does not limit this.

[0076] The total difference degree meeting the preset condition may mean that the total difference degree value is less than the error threshold, or the change between the total difference degree value of the current iteration and the total difference degree value of the previous iteration is less than the change threshold, etc.

[0077] The target relative pose is solved according to the color difference and the depth difference. The target relative pose can also be calculated using the color difference and the depth difference respectively, and finally the two target relative poses are fused to obtain the final output target relative pose.

[0078] Specifically, the estimated relative pose is iteratively updated, and the relative pose that minimizes the color difference is solved as the first estimated relative pose. The estimated relative pose is iteratively updated, and the relative pose that minimizes the depth difference is solved as the second estimated relative pose.

[0079] The first estimated relative pose and the second estimated relative pose are fused and calculated using a fusion algorithm, and the relative pose is output. The fusion algorithm is, for example, a Kalman filter algorithm.

[0080] In this embodiment, in addition to using the color information of the first image and the second image, the depth information is further combined for constraint, which can improve the accuracy and robustness of relative pose estimation. In addition, the matching points of the first image and the second image are determined by projection, which can reduce the amount of calculation for feature point matching, making the process of optimizing and estimating relative pose faster.

[0081] The accuracy of pose estimation often depends on the number of feature points in two adjacent frames. When facing weakly textured rotating bodies such as spheres or cylinders, fewer feature points are extracted from the image, which reduces the accuracy of pose estimation. In order to effectively process weakly textured rotating bodies, this application also adds inertial data as a constraint in pose estimation to reduce the error of pose estimation.

[0082] See also Figure 3 , Figure 3 1 is a flow chart of the second embodiment of the relative posture estimation method provided by the present application. The difference between this embodiment and the first embodiment of the relative posture estimation method is that this embodiment further uses inertial information for constraints. This embodiment includes the following steps:

[0083] S210: Acquire measurement data collected by the sensor and an estimated relative pose between the first image and the second image.

[0084] The measurement data may also include first inertial information collected at the same time as the first image, and second inertial information collected at the same time as the second image.

[0085] The inertial measurement unit is a device that measures the three-axis attitude angle (or angular rate) and acceleration of an object. In this embodiment, the inertial measurement unit measures the inertial information of the depth camera, which can be used to estimate the position of the depth camera. The inertial measurement unit can specifically include one or more of a gyroscope, an acceleration sensor, or a gravity sensor.

[0086] The first inertial information may include first angular velocity information collected by the gyroscope, and the second inertial information may include second angular velocity information collected by the gyroscope.

[0087] The first inertial information may further include first gravity information, and the second inertial information may further include second gravity information. The gravity information is acceleration caused by gravity measured by a gravity sensor, and the tilt angle of the device relative to the horizontal plane may be calculated.

[0088] S220: Determine a projection point of the first image on the second image based on the first depth information and the estimated relative pose.

[0089] S230: Calculate a color difference between the first image and the second image based on the first color information, the second color information, and the projection point.

[0090] S240: Calculate a depth difference between the first image and the second image based on the first depth information, the second depth information, and the projection point.

[0091] S250: Calculate the inertia difference between the first image and the second image according to the first inertia information, the second inertia information, and the estimated relative position and posture.

[0092] Differences in inertia include differences in rotation.

[0093] By determining the timestamps of the first angular velocity information and the second angular velocity information, the integration time between the first angular velocity information and the second angular velocity information can be calculated. The first angular velocity information is integrated using the integration time to obtain the rotation amount between the first image and the second image.

[0094] Calculate the difference between the estimated rotation matrix and the rotation amount in the estimated relative pose, that is, the value obtained by taking the logarithm of the ratio of the estimated rotation matrix to the rotation amount, and square the difference to get the rotation difference. The rotation difference formula is as follows:

[0095]

[0096] Among them, E gyr represents the difference in rotation, ω = [α β γ], which is the first three dimensions of the optimization variable ξ. R represents the estimated rotation matrix between the first image and the second image, which is part of the estimated relative pose T. Used to update the rotation matrix. gyr Represents the rotation amount obtained by integrating the angular velocity information.

[0097] Differences in inertia also include differences in gravity.

[0098] The first gravity information is rotated according to the estimated rotation matrix in the estimated relative posture to obtain the rotated first gravity information. The square of the difference between the second gravity information and the rotated first gravity information is calculated to obtain the gravity difference. The gravity difference formula is as follows:

[0099]

[0100] Among them, E G Represents the gravity difference, G c Represents the first gravity information in the first camera coordinate system, Represents the first gravity information after rotation, G r Indicates the second gravity information.

[0101] By constraining the rotation difference and the gravity difference, the rotation matrix between the first image and the second image can be accurately estimated, reducing the error of the estimated relative pose, making the estimation of the relative pose more robust.

[0102] S260: Optimize the estimated relative pose based on the color difference, the depth difference and the inertia difference to obtain the target relative pose.

[0103] There are many methods to solve the relative pose of the target based on color difference, depth difference and inertia difference.

[0104] For example, the color difference, depth difference and inertia difference may be weightedly summed to obtain a total difference, and the estimated relative pose may be iteratively updated to obtain an adjusted estimated relative pose that minimizes the total difference as the target relative pose.

[0105] The total difference formula is as follows:

[0106] E = argminξ (w1E rgb +w2E depth +w3E G +w4E 9yr )

[0107] Where E represents the total difference, and w1, w2, w3, and w4 are weights of different differences. The values ​​of w1, w2, w3, and w4 can be set according to the accuracy of the depth sensor, photosensitive element, or inertial measurement sensor of the depth camera. The higher the sensor accuracy, the greater the corresponding weight. The smaller the value of the total difference, the higher the accuracy of the estimated relative pose after the iteration update, and the closer it is to the actual relative pose.

[0108] In some implementations, if the inertial information only includes gravity information, the value of w4 is 0.

[0109] In other implementations, if the inertial information only includes angular velocity information, the value of w3 is 0.

[0110] The iterative process of posture is the same as that of S150 and will not be repeated here.

[0111] Of course, the target relative pose can be solved according to the color difference, depth difference and inertia difference. The target relative pose can also be calculated using the color difference, depth difference and inertia difference respectively, and finally the target relative poses are fused in pairs to obtain the final output target relative pose. For example, the target relative poses calculated based on the color difference and the depth difference are first fused to obtain the fused target relative pose, and then the fused target relative pose is fused with the target relative pose obtained based on the inertia difference to obtain the final output target relative pose.

[0112] The first estimated relative pose and the second estimated relative pose are fused and calculated using a fusion algorithm, and the relative pose is output. The fusion algorithm is, for example, a Kalman filter algorithm.

[0113] In this embodiment, in addition to using the depth information and color information of the first image and the second image, inertial information is further constrained to reduce the impact of weak texture objects such as rotating bodies in the image on the accuracy of pose estimation, thereby improving the accuracy and robustness of relative pose estimation. In the process of pose estimation, the initial pose, that is, the accuracy of the estimated relative pose, is often required to be high. When the accuracy of the estimated relative pose is low, the number of iterations of pose estimation will increase, resulting in slower pose estimation. In addition, the low-precision estimated relative pose will also result in low accuracy of the target relative pose output in the final output. Therefore, please refer to Figure 4 , Figure 4This is a flow chart of the third embodiment of the relative pose estimation method provided by the present application. Based on the first and second embodiments of the relative pose estimation method, this embodiment further performs pyramid downsampling on the first image and the second image, optimizes the estimated relative pose from the top layer of low resolution, and reduces the impact of the low accuracy of the estimated relative pose on the accuracy of the target relative pose. This embodiment includes the following steps:

[0114] S310: Perform the same image pyramid downsampling on the first image and the second image to obtain a first image pyramid and a second image pyramid with a target number of layers, and record the top layers of the first image pyramid and the second image pyramid as target layers.

[0115] An image pyramid is a combination of a series of images. The image size at the bottom layer is the largest, and the image size at the top layer is the smallest. From the bottom layer to the top layer, the image size decreases in sequence. Looking down from the space, it looks like a pyramid. Using image pyramid downsampling, a series of images with different resolutions are generated. The bottom layer has the highest resolution, and the top layer has the lowest resolution. From the bottom layer to the top layer, the image resolution decreases in sequence. Then, the corresponding features of the images are found in different scale spaces. The image pyramid can ensure the existence of image features.

[0116] The image pyramid downsampling may be mean downsampling, that is, the color values ​​and depth values ​​of four adjacent pixels on the first image and the second image are averaged into one value.

[0117] The top layer of the first image pyramid and the second image pyramid is recorded as the target layer, and the transition is made to the bottom layer by layer. The purpose of constructing the image pyramid is to calculate the camera position and posture from coarse to fine, which can accelerate the calculation of the target relative posture and reduce the requirements for the initial value of the estimated relative posture.

[0118] The same image pyramid downsampling is performed on the first image and the second image of the original resolution to obtain the first image and the second image of the target number of layers.

[0119] Furthermore, in order to ensure data smoothness during the downsampling process, the first image and the second image may be filtered before downsampling each layer of the depth image. The filtering algorithm may be Gaussian filtering, bilateral filtering, or median filtering.

[0120] Furthermore, due to the pyramid downsampling of the first image and the second image of the lower layer, the image resolution of the first image and the second image of the upper layer decreases and the size becomes smaller, so that the internal parameters (focal length and pixel center) of the depth camera of the first image and the second image of the upper layer change. Therefore, the internal parameters can be proportionally reduced according to the size ratio of the first image and the second image of the upper layer to the first image and the second image of the lower layer. The resolution of each layer of the first image and the second image corresponds to the internal parameters one by one, and when the feature points of the first image are subsequently back-projected into the three-dimensional space, and the feature points of the three-dimensional space are projected into the second pixel coordinate system, accurate pixel coordinates can be obtained. The upper layer and the lower layer described in this embodiment are relative to the two adjacent layers of the first image and the second image.

[0121] When the estimated relative pose between the first image and the second image of each layer is iterated, the first inertial information and the second inertial information remain unchanged.

[0122] S320: Acquire first color information, first depth information, and first inertial information of a first image of a target layer collected at the same time as the first image, second color information, second depth information, and second inertial information of a second image collected at the same time as the second image, and an estimated relative pose between the first image and the second image.

[0123] In this embodiment, the estimated relative pose is optimized starting from the topmost image of the first image pyramid and the second image pyramid, the topmost image is recorded as the target layer, and the target relative pose of the target layer is sequentially used as the estimated relative pose of the next layer until the target layer becomes the bottom layer of the first image pyramid and the second image pyramid, and the target relative pose calculated based on the bottom layer is calculated as the target relative pose of the final output.

[0124] This step is similar to S210, so it will not be repeated here.

[0125] S330: Determine a projection point of the first image on the second image based on the first depth information and the estimated relative pose.

[0126] S340: Calculate a color difference between the first image and the second image based on the first color information, the second color information, and the projection point.

[0127] S350: Calculate a depth difference between the first image and the second image based on the first depth information, the second depth information, and the projection point.

[0128] S360: Calculate the inertia difference between the first image and the second image according to the first inertia information, the second inertia information, and the estimated relative position and posture.

[0129] S370: Optimize the estimated relative pose based on the color difference, the depth difference and the inertia difference to obtain the target relative pose.

[0130] S380: Determine whether the target layer is the bottom layer of the first image pyramid and the second image pyramid.

[0131] In response to the target layer being the lowest layer, S490 is executed.

[0132] In response to the situation that the target layer is not the lowest layer, the next layer of the target layer is used as the new target layer, the target relative pose of the target layer is recorded as the estimated relative pose of the new target layer, and the process returns to execute "determining the target relative pose of the first image and the second image of the target layer", that is, executing step S320.

[0133] S390: Stop iteration, and output the target relative pose of the target layer as the final target relative pose of the first image and the second image.

[0134] In this embodiment, in addition to using the depth information of the first image and the second image, the color information and inertial information are further combined for constraints, thereby improving the accuracy and robustness of the relative pose estimation. In addition, the use of projection to determine the matching points of the first image and the second image can reduce the amount of calculation for feature point matching, making the process of optimizing the estimated relative pose faster. Furthermore, the relative pose of the first image and the second image is optimized layer by layer using an image pyramid, which reduces the requirements for the initial estimated relative pose and can further improve the accuracy of the target relative pose.

[0135] See also Figure 5 , Figure 5 : is a schematic diagram of the structure of an embodiment of a relative posture estimation device provided by the present application. The relative posture estimation device 500 includes:

[0136] The acquisition module 501 is used to acquire measurement data collected by the sensor, the measurement data including first color information and first depth information of the first image, second color information and second depth information of the second image, and an estimated relative pose between the first image and the second image.

[0137] The reprojection module 502 is used to determine a projection point of the first image on the second image based on the first depth information and the estimated relative pose.

[0138] The posture optimization module 503 is used to calculate the color difference between the first image and the second image based on the first color information, the second color information and the projection point; calculate the depth difference between the first image and the second image based on the first depth information, the second depth information and the projection point; and optimize the estimated relative posture based on the color difference and the depth difference to obtain the target relative posture.

[0139] In some embodiments, the relative posture estimation device can be used to execute the relative posture estimation method described in the above embodiments. Of course, it may include units or modules for executing any process and / or steps in the relative posture estimation method described in the above embodiments. For the sake of brevity, they will not be repeated.

[0140] The description of the above device embodiment is similar to the description of the above method embodiment, and has similar beneficial effects as the method embodiment. For technical details not disclosed in the device embodiment of the present invention, please refer to the description of the method embodiment of the present invention for understanding.

[0141] See also Figure 6 , Figure 6 600 is a schematic diagram of a structure of an electronic device according to an embodiment of the present application. The electronic device 600 includes:

[0142] Processor 601 and memory 602. The processor 601 is coupled to the memory 602, and executes instructions when working, so as to cooperate with the memory 602 to implement the above relative posture estimation method.

[0143] Optionally, the electronic device is a mobile phone, a camera, a tablet computer or a mobile robot.

[0144] The memory 602 is used to store measurement data, which includes first color information and first depth information of the first image, second color information and second depth information of the second image, and an estimated relative pose between the first image and the second image.

[0145] The processor 601 is used to optimize the estimated relative pose of the first image and the second image using the color information and depth information of the depth image. Specifically, the processor 601 determines the projection point of the first image on the second image based on the first depth information and the estimated relative pose; calculates the color difference between the first image and the second image based on the first color information, the second color information and the projection point; calculates the depth difference between the first image and the second image based on the first depth information, the second depth information and the projection point; optimizes the estimated relative pose based on the color difference and the depth difference to obtain the target relative pose.

[0146] The processor 601 may be an integrated circuit chip with signal processing capability. The processor 601 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. The general-purpose processor may be a microprocessor or the processor may also be any conventional processor, etc.

[0147] The method of the above embodiment may exist in the form of a computer program, so the present application proposes a computer storage medium, see Figure 7 , Figure 7 The computer storage medium 700 of this embodiment stores a computer program 701, which can be executed to implement the method in the above embodiment.

[0148] The computer storage medium 700 of this embodiment can be a medium that can store program instructions, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a disk or an optical disk, or it can also be a server that stores the program instructions. The server can send the stored program instructions to other devices for execution, or it can also execute the stored program instructions by itself.

[0149] In the several embodiments provided in the present application, it should be understood that the disclosed methods and devices can be implemented in other ways. For example, the device implementation described above is only schematic. For example, the division of modules or units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms.

[0150] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the present embodiment.

[0151] In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0152] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application is essentially or the part that contributes to the prior art or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions to enable a computer device (which can be a personal computer, server, or network device, etc.) or a processor (processor) to perform all or part of the steps of each implementation method of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk and other media that can store program code.

[0153] 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 relative pose estimation method, characterized in that: The method includes: determining a relative pose of an object in a first image and a second image; Determining the relative position of the target in the first image and the second image includes: Acquire measurement data collected by a sensor and an estimated relative pose between the first image and the second image, the measurement data including first color information and first depth information of the first image, second color information and second depth information of the second image; Determining a projection point of the first image on the second image based on the first depth information and the estimated relative pose; Calculating a color difference between the first image and the second image based on the first color information, the second color information, and the projection point; Calculating a depth difference between the first image and the second image based on the first depth information, the second depth information, and the projection point; Optimizing the estimated relative pose based on the color difference and the depth difference to obtain the target relative pose; The measurement data further includes first inertial information collected at the same time as the first image, and second inertial information collected at the same time as the second image, and the method further includes: Calculating an inertial difference between the first image and the second image according to the first inertial information, the second inertial information, and the estimated relative pose; The optimizing the estimated relative pose based on the color difference and the depth difference to obtain the target relative pose includes: The estimated relative pose is optimized based on the color difference, the depth difference and the inertia difference to obtain the target relative pose.

2. The method according to claim 1, characterized in that The optimizing the estimated relative pose based on the color difference, the depth difference and the inertia difference to obtain the target relative pose includes: Obtaining weights of the color difference, the depth difference, and the inertia difference; Performing weighted summation on the color difference, the depth difference, and the inertia difference according to the weight to obtain a total difference degree; In response to the total difference not satisfying a preset condition, adjusting the estimated relative posture until the total difference satisfies the preset condition; The estimated relative posture adjusted so that the total difference satisfies the preset condition is used as the target relative posture.

3. The method according to claim 1, characterized in that The optimizing the estimated relative pose based on the color difference, the depth difference and the inertia difference to obtain the target relative pose includes: Adjusting the estimated relative pose to obtain a first target relative pose that minimizes the color difference; Adjusting the estimated relative pose to obtain a second target relative pose that minimizes the depth difference; Adjusting the estimated relative posture to obtain a third target relative posture that minimizes the inertia difference; The first target relative posture, the second target relative posture and the third target relative posture are fused to obtain the target relative posture.

4. The method according to claim 1, characterized in that The estimated relative pose includes an estimated rotation matrix, the first inertial information includes first gravity information, the second inertial information includes second gravity information, and the calculating, according to the first inertial information, the second inertial information and the estimated relative pose, an inertial difference between the first image and the second image under the estimated relative pose includes: Rotating the first gravity information according to the estimated rotation matrix; The square of the difference between the second gravity information and the rotated first gravity information is calculated to obtain the inertia difference.

5. The method according to claim 1, characterized in that: The estimated relative pose includes an estimated rotation matrix, the first inertial information includes first angular velocity information, the second inertial information includes second angular velocity information, and the calculating, according to the first inertial information, the second inertial information and the estimated relative pose, an inertial difference between the first image and the second image under the estimated relative pose includes: Determine timestamps of the first angular velocity information and the second angular velocity information, and calculate an integration time between the first angular velocity information and the second angular velocity information; Integrate the first angular velocity information according to the integration time to obtain a rotation amount; The inertia difference is calculated based on the difference between the estimated rotation matrix and the rotation amount.

6. The method according to claim 1, characterized in that The first image includes a plurality of first pixel points, and determining a projection point of the first image on the second image based on the first depth information and the estimated relative pose includes: Acquire, from the first depth information, the first depth value corresponding to each of the first pixels according to the pixel coordinates of each of the first pixels; According to the first depth value, each of the first pixel points is back-projected into a first camera coordinate system to obtain a first three-dimensional point; Convert each of the first three-dimensional points into a second camera coordinate system according to the estimated relative pose to obtain a second three-dimensional point; Each of the second three-dimensional points is projected onto a second pixel coordinate system of the second image to obtain the projection point corresponding to each of the first pixel points in the second image.

7. The method according to claim 6, characterized in that The first color information, the second color information and the projection point, and calculating a color difference between the first image and the second image, comprising: Pairing each of the first pixel points with the projection point corresponding to the first pixel point to obtain a plurality of matching point pairs; Obtain a first color value of each of the first pixel points according to the first color information, and obtain a second color value of each of the projection points according to the second color information; Calculate the color difference between the first color value and the second color value of each matching point pair respectively; The color difference values ​​of the multiple matching point pairs are squared and then summed to obtain the color difference.

8. The method according to claim 6, characterized in that The first depth information includes first depth values ​​of the plurality of first pixels in the first image, and the calculating, based on the first depth information, the second depth information, and the estimated relative pose, a depth difference between the first image and the second image at the estimated relative pose includes: Pairing each of the first pixel points with the projection point corresponding to the first pixel point to obtain a plurality of matching point pairs; Calculate a second depth value of each of the second three-dimensional points; Acquire a third depth value of each of the projection points according to the second depth information; Calculate the depth difference between the second depth value and the third depth value of each matching point pair respectively; The depth differences of the multiple matching point pairs are squared and then summed to obtain the depth difference.

9. The method according to claim 1, characterized in that: Before determining the relative position of the target in the first image and the second image, the method further includes: Performing the same image pyramid downsampling on the first image and the second image to obtain a first image pyramid and a second image pyramid with a target number of layers, and recording the topmost layers of the first image pyramid and the second image pyramid as target layers; Determining the relative position of the target of the first image and the second image includes: Determine a relative position of a target between the first image and the second image of the target layer; Determine whether the target layer is the bottom layer of the first image pyramid and the second image pyramid; In response to the target layer being the bottom layer, determining the target relative posture of the target layer as the final target relative posture; In response to the situation that the target layer is not the bottom layer, the next layer of the target layer is used as the new target layer, the target relative pose of the target layer is recorded as the estimated relative pose of the new target layer, and the process of "determining the target relative pose of the first image and the second image of the target layer" is returned.

10. A relative pose estimation device, characterized in that: The device comprises: an acquisition module, configured to acquire measurement data collected by a sensor, wherein the measurement data includes first color information and first depth information of a first image, second color information and second depth information of a second image, and an estimated relative position between the first image and the second image; a reprojection module, configured to determine a projection point of the first image on the second image based on the first depth information and the estimated relative pose; a posture optimization module, configured to calculate a color difference between the first image and the second image based on the first color information, the second color information and the projection point; calculate a depth difference between the first image and the second image based on the first depth information, the second depth information and the projection point; and optimize the estimated relative posture based on the color difference and the depth difference to obtain the target relative posture; The measurement data also includes first inertial information collected at the same time as the first image, and second inertial information collected at the same time as the second image, The posture optimization module is further used to calculate the inertia difference between the first image and the second image according to the first inertia information, the second inertia information and the estimated relative posture; The posture optimization module is further used to optimize the estimated relative posture based on the color difference, the depth difference and the inertia difference to obtain the target relative posture.

11. An electronic device, characterized in that: The electronic device includes a processor and a memory; the processor is coupled to the memory and executes instructions when working to cooperate with the memory to implement the relative posture estimation method as described in any one of claims 1 to 9.

12. A computer storage medium, characterized in that: The computer storage medium stores a computer program, and the computer program is executed by a processor to implement the steps of the method according to any one of claims 1 to 9.

Images