An indoor pose estimation method and device

Abstract

This application relates to the technical field of attitude estimation, and particularly to an indoor attitude estimation method and device. The indoor attitude estimation method is applied to a carrier, and the carrier includes a depth camera. The indoor attitude estimation method includes: obtaining a target depth image of a target scene through the depth camera; segmenting the target depth image into multiple sub-images and calculating the unit normal vector of each sub-image; extracting the unit normal vector and matching it with the normal vector of the Manhattan world to estimate the absolute attitude of the depth camera relative to the Manhattan world. Since the embodiments of this application do not make assumptions about the distribution form of the absolute attitude, they are more robust. In addition, compared with parameter estimation methods, the calculation complexity can be reduced, saving computing power.

Description

Technical Field

[0001] This application relates to the technical field of attitude estimation, and in particular, to an indoor attitude estimation method and device. Background Art

[0002] Attitude estimation is the intelligent foundation for applications such as mobile robots, virtual reality, and mobile intelligent devices. It estimates the orientation of a carrier relative to a reference coordinate system through the input carrier sensor data.

[0003] Generally speaking, an attitude estimation system requires two main types of sensors: the first is a high-speed angular rate sensor, i.e., a gyroscope, which can measure the instantaneous angular velocity of the carrier; the second is a reference vector sensor, which can measure a set of known direction vectors of the reference coordinate system. Since directly integrating the angular velocity to estimate the attitude will cause cumulative errors due to the existence of measurement noise, in order to reduce the cumulative errors, it is necessary to fuse this angular velocity information with the absolute attitude readings of the reference vector sensor.

[0004] With the progress of Micro-Electro-Mechanical System (MEMS) technology, MEMS-based Inertial Measurement Unit (IMU) has become the most critical sensor for attitude measurement. The existing IMU-based attitude estimation technology is called Attitude and Heading Reference System (AHRS). AHRS usually includes a three-axis gyroscope, a three-axis accelerometer, and a three-axis magnetometer. AHRS estimates the reference directions of the earth's gravity field and magnetic field through these two types of reference vector sensors, namely the accelerometer and the magnetometer, so as to determine the absolute attitude, and then fuse it with the gyroscope information to correct the cumulative errors. Summary of the Invention

[0005] In view of this, embodiments of this application provide an indoor attitude estimation method and device, which can solve at least one technical problem in the related art.

[0006] In a first aspect, an embodiment of this application provides an indoor attitude estimation method, which is applied to a carrier. The carrier includes a depth camera. The indoor attitude estimation method includes: obtaining a target depth image of a target scene through the depth camera; segmenting the target depth image into multiple sub-images, and calculating the unit normal vector of each sub-image; extracting the unit normal vector and matching it with the normal vector of the Manhattan world to estimate the absolute attitude of the depth camera relative to the Manhattan world.

[0007] In this embodiment, by using the measurement information of the depth camera and based on the assumption that the indoor environment is a Manhattan world, the absolute pose of the depth camera relative to the Manhattan world is estimated. Since no assumption is made about the distribution form of the absolute pose, it is more robust. In addition, compared with the parameter estimation method, the computational complexity can be reduced, saving computing power.

[0008] In a second aspect, an embodiment of the present application provides an indoor pose estimation device configured on a carrier, where the carrier includes a depth camera. The indoor pose estimation device includes: a depth image acquisition execution module for acquiring a target depth image of a target scene through the depth camera; a segmentation and calculation module for segmenting the target depth image into multiple sub-images and calculating the unit normal vector of each sub-image; an estimation module for extracting the unit normal vector and matching it with the normal vector of the Manhattan world to estimate the absolute pose of the depth camera relative to the Manhattan world.

[0009] In a third aspect, an embodiment of the present application provides a carrier, including: a depth camera, a processor, and a computer program stored in a memory and executable on the processor. When the processor executes the computer program, the indoor pose estimation method described in any embodiment of the first aspect is implemented.

[0010] In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium storing a computer program, and when the computer program is executed by a processor, the indoor pose estimation method described in any embodiment of the first aspect is implemented.

[0011] In a fifth aspect, an embodiment of the present application provides a computer program product, and when the computer program product runs on an electronic device, the electronic device is caused to execute the indoor pose estimation method described in any embodiment of the first aspect.

[0012] It should be understood that the beneficial effects of the second to fifth aspects can refer to the relevant descriptions of the embodiments of the first aspect, which will not be elaborated here. Description of the Drawings

[0013] To more clearly illustrate the technical solutions in the embodiments of the present application, the following will briefly introduce the drawings required for use in the embodiments or the description of the prior art. Obviously, the following drawings are only some embodiments of the present application. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0014] Figure 1 It is a schematic structural diagram of a carrier provided by an embodiment of the present application;

[0015] Figure 2It is a schematic diagram of the implementation process of an indoor pose estimation method provided by an embodiment of the present application;

[0016] Figure 3 It is a schematic diagram of the implementation process of step S120 in an indoor pose estimation method provided by an embodiment of the present application;

[0017] Figure 4 It is a schematic diagram of the implementation process of step S130 in an indoor pose estimation method provided by an embodiment of the present application;

[0018] Figure 5 It is a schematic diagram of the implementation process of an indoor pose estimation method provided by another embodiment of the present application;

[0019] Figure 6 It is a schematic diagram of the implementation process of step S260 in an indoor pose estimation method provided by another embodiment of the present application;

[0020] Figure 7 It is a schematic diagram of the structure of an indoor pose estimation device provided by an embodiment of the present application;

[0021] Figure 8 It is a schematic diagram of the structure of an indoor pose estimation device provided by another embodiment of the present application. Detailed implementation manners

[0022] In the following description, for the purpose of illustration rather than limitation, specific details such as specific system structures and technologies are presented to thoroughly understand the embodiments of the present application. However, those skilled in the art should clearly understand that the present application can also be implemented in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted to avoid unnecessary details from interfering with the description of the present application.

[0023] The term "and / or" used in the specification and appended claims of the present application refers to any combination and all possible combinations of one or more of the associated listed items, and includes these combinations.

[0024] Describing "an embodiment" or "some embodiments" etc. in the specification of the present application means that specific features, structures, or characteristics described in connection with the embodiment are included in one or more embodiments of the present application. Thus, statements such as "in an embodiment", "in some embodiments", "in other some embodiments", "in still other embodiments" etc. that appear in different places in this specification do not necessarily refer to the same embodiment, but mean "one or more but not all embodiments", unless otherwise specifically emphasized in other ways. The terms "include", "comprise", "have" and their variants all mean "including but not limited to", unless otherwise specifically emphasized in other ways.

[0025] In addition, in the description of the present application, "a plurality of" means two or more. Terms such as "first" and "second" are only used for distinguishing descriptions and cannot be understood as indicating or implying relative importance.

[0026] Currently, considering that there is usually more electromagnetic interference in the indoor environment, the AHRS needs to perform complex electromagnetic field calibration in advance, and the electromagnetic field may change with time, resulting in a large systematic error in the attitude estimation of the AHRS. In addition, the high-dynamic motion scenario of the carrier will limit the application of the AHRS because it will bring additional acceleration other than gravity, affecting the estimation of the gravity direction.

[0027] To solve the problem of limited accuracy of attitude estimation in the indoor environment, the embodiments of the present application introduce a depth camera and fuse the depth camera and the gyroscope to ensure the robustness and accuracy of attitude estimation.

[0028] In some embodiments of the present application, similar to the accelerometer and magnetometer, the depth camera also serves as a reference vector sensor. Using the measurement information of the depth camera and based on the assumption that the indoor environment is a Manhattan world, the absolute direction of the Manhattan world is estimated as the absolute attitude of the carrier, that is, the absolute attitude of the depth camera relative to the Manhattan world.

[0029] In some embodiments of the present application, the cumulative error of the gyroscope can also be corrected with the absolute attitude, which can make the result of the carrier attitude estimation more accurate and more robust.

[0030] In some embodiments of the present application, the embodiments of the present application can be, for example, application fields such as AR / VR and handheld scanners. These application fields need to obtain the attitude information of the carrier, and the technical solutions provided by the embodiments of the present application can solve this technical problem.

[0031] To illustrate the technical solutions described in the present application, the following will be described through specific embodiments.

[0032] An embodiment of the present application provides an indoor attitude estimation method. The indoor attitude estimation method can be executed by a processor, which is usually configured in a carrier. The indoor attitude estimation method is applicable to the situation where the attitude information of the carrier needs to be estimated indoors. The carrier includes but is not limited to: a computing device including a processor, or an electronic device including a processor, etc. The processor can be a Central Processing Unit (CPU), or other general-purpose processors, Digital Signal Processors (DSPs), Application Specific Integrated Circuits (ASICs), Field-Programmable Gate Arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor can be a microprocessor or the processor can also be any conventional processor, etc. The electronic device includes but is not limited to a computer, a tablet computer, a server, a mobile phone, a camera, or a wearable device, etc.

[0033] An embodiment of the present application provides a carrier, which may include but is not limited to a processor and a depth camera, and the depth camera is connected to the processor. An embodiment of the present application provides a carrier, which may include but is not limited to a processor, a depth camera, and a gyroscope, and both the depth camera and the gyroscope are connected to the processor.

[0034] In a possible implementation, the processor can be a single-chip microcomputer, and the depth camera and the gyroscope are both integrated on the single-chip microcomputer. In another possible implementation, the carrier is an electronic device, which includes a housing, and the processor, the depth camera, and the gyroscope are all arranged inside the housing of the electronic device. It should be noted that a shooting window for the depth camera to collect the environmental depth image is provided at the position corresponding to the depth camera on the housing.

[0035] Figure 1 The following shows a schematic structural diagram of a carrier provided by an embodiment of the present application. As Figure 1 shown, the carrier 1 may include one or more processors 10 ( Figure 1 only one is shown here), a memory 11, and a computer program 12 stored in the memory 11 and executable on the one or more processors 10. For example, a program for indoor attitude estimation, a depth camera 12, and a gyroscope 13. When the one or more processors 10 execute the computer program 12, each step in the embodiment of the indoor attitude estimation method can be implemented. Alternatively, when the one or more processors 10 execute the computer program 12, the functions of each module / unit in the embodiment of the indoor attitude estimation device can be implemented, which is not limited here.

[0036] Those skilled in the art can understand that Figure 1 These are merely examples of the carrier and do not constitute a limitation on the carrier. The carrier may include more or fewer components than shown in the figure, or combine certain components, or different components. For example, the carrier may also include input / output devices, network access devices, buses, etc.

[0037] In one embodiment, the memory 11 may be an internal storage unit of the carrier, such as the hard disk or memory of the carrier. The memory 11 may also be an external storage device of the carrier, such as a plug-in hard disk equipped on the carrier, a smart media card (SMC), a secure digital (SD) card, a flash card, etc. Further, the memory 11 may also include both the internal storage unit and the external storage device of the carrier. The memory 11 is used to store computer programs and other programs and data required by the carrier. The memory 11 may also be used to temporarily store data that has been output or will be output.

[0038] Figure 2 is a schematic flowchart of the implementation of an indoor pose estimation method provided by an embodiment of the present application. The indoor pose estimation method is applied to a carrier. As Figure 2 shown, the indoor pose estimation method may include steps S110 to S130.

[0039] S110, obtain a target depth image of the target scene through a depth camera.

[0040] In the embodiment of the present application, the depth camera may be a depth camera based on the structured light triangulation method, the time-of-flight method, or the binocular vision principle, and obtain the target depth image of the target scene through the depth camera. The target scene is an indoor scene, and the target scene includes at least two vertical planes. Preferably, it may include three vertical planes.

[0041] S120, segment the target depth image into multiple sub-images, and calculate the unit normal vector n of each sub-image i , where i is the serial number of the sub-image.

[0042] Among them, the target depth image is segmented into multiple sub-images, the multiple sub-images are sequentially numbered to form the serial number i of the sub-image, and then the unit normal vector n of each sub-image is calculated i .

[0043] In one embodiment, as Figure 3 shown, step S120 may include steps S121 to S124.

[0044] S121, perform bilateral filtering on the target depth image to obtain a first depth image.

[0045] Among them, bilateral filtering is performed on the depth image to remove large noises and reduce the adverse effects of the noises on the accuracy of the result.

[0046] S122, undistort the first depth image based on the internal parameters of the depth camera to obtain a second depth image.

[0047] Among them, the internal parameters of the depth camera are, for example, the internal parameter matrix and distortion coefficients, etc.

[0048] S123, divide the second depth image into multiple sub-images, and convert the depth information of each sub-image into 3D point clouds in the camera coordinate system.

[0049] Among them, the second depth image can be divided into multiple image blocks, that is, multiple sub-images, according to a preset size, where i is the serial number of the sub-image; then convert the depth information of each sub-image into 3D point clouds in the camera coordinate system. The purpose of dividing the entire image into small image blocks is to speed up the operation speed. The selection of the size of the image blocks is related to the resolution of the depth camera, and this application does not make specific limitations on this.

[0050] In one embodiment, in order to balance data processing efficiency and extract a sufficient number of normal vectors to ensure the accuracy of the result, the second depth image is divided into image blocks of size 10*10, that is, sub-images of size 10*10.

[0051] S124, calculate the unit normal vector n of the 3D point cloud of each sub-image i .

[0052] Among them, for the 3D point cloud of each sub-image, perform plane fitting based on the least squares method, and extract the unit normal vector n corresponding to the fitted plane i , where i is the serial number of the sub-image.

[0053] S130, extract the unit normal vector and match it with the normal vector of the Manhattan world to estimate the absolute pose of the depth camera relative to the Manhattan world.

[0054] It should be noted that the Manhattan world hypothesis believes that in an artificial indoor environment, buildings and objects are usually composed of mutually orthogonal or parallel planes. In this case, the plane normal vectors are mutually orthogonal or opposite. Usually, three mutually perpendicular normal vectors are selected to form a Cartesian coordinate system to uniquely represent this Manhattan world.

[0055] In the embodiment of this application, in the Manhattan indoor environment, considering the depth camera measurement error, the unit normal vector n in the depth camera measurement value iThey will be distributed around the unit vectors in the Manhattan world according to a certain probability distribution model. Therefore, by extracting the unit normal vectors from the point cloud of the depth camera, the normal vectors in the Manhattan world can be matched, and thus the absolute pose of the depth camera in the Manhattan world can be estimated.

[0056] That is to say, the Manhattan world is hypothetical. It can be considered that the origin of the camera coordinate system of the Manhattan world coincides with that of the depth camera, and the three coordinate axes point to the three positive directions. The extracted unit normal vectors represent the unit normal vectors of several vertical planes, which should be associated with the normal vectors in the Manhattan world. If the pose of the depth camera is the same as that of the Manhattan world coordinate system, the unit normal vectors are respectively the unit vectors of the coordinate axes in the Manhattan world.

[0057] In one embodiment, as Figure 4 shown, step S130 may include steps S131 to S132.

[0058] S131, using the mean shift algorithm to cluster the unit normal vector n i measured by the depth camera, and search for the normal vector modes.

[0059] In one embodiment, using the mean shift algorithm to cluster the unit normal vector n i measured by the depth camera, and search for the normal vector modes.

[0060] First, cluster the unit normal vector n i to determine the set of normal vectors for the normal vector modes to be searched. Specifically, define the normal vector mode to be searched as r j , where j = 1, 2, 3, and r1, r2, and r3 respectively represent the unit vectors of the positive x-axis, positive y-axis, and positive z-axis of the Manhattan world coordinate system expressed in the depth camera coordinate system. When the algorithm is first started, three normal vector modes r j can be randomly selected and a search threshold is preset. In this case, θ th is set to 180°, and the initial value of θ th can also be set to other values, and each normal vector mode is searched separately. If the startup fails, that is, step S131 is determined to be a failure, then re-enter step S131 to continue randomly selecting three normal vector modes and search for each normal vector mode separately. Finally, find the set of normal vectors N i near each normal vector mode r j in the unit normal vector n j , where j = 1, 2, 3. In one embodiment, the unit normal vectors n i in the unit normal vector n i,j that satisfy the following calculation formula are grouped into the normal vector mode r jSet of normal vectors N near j In:

[0061] ‖n i,j ×r j ‖ < sin(θ th ) ,

[0062] where n i,j represents the unit normal vector n i in the normal vector mode r j nearby, and θ th can be a fixed threshold. It can be understood that when the algorithm is successfully started for the first time and the absolute pose of the depth camera relative to the Manhattan world is calculated, in subsequent iterative operations, the search threshold can be set smaller. In a possible implementation, θ th can be set to 10°.

[0063] It should be noted that in the actual implementation process, in order to ensure the stability and robustness of the algorithm, some other screening thresholds can be set. For example, in a possible implementation, a threshold for the number of normal vectors in the search set of normal vectors can be set. If the number of normal vectors in the set of normal vectors N j in step S131 is less than a certain threshold, which can be set to 100, it can be determined that step S131 fails, that is, the initialization fails. Then, re-enter step S131 to continue randomly selecting three normal vector modes, and search for the set of normal vectors near each normal vector mode respectively to find the set of normal vectors N j . If the number of normal vectors in the set of normal vectors N j in step S131 is greater than or equal to a certain threshold, it can be determined that step S131 is successful, that is, the initialization is successful, and then continue with the subsequent steps. Another example is that in a possible implementation, if the number of consecutive failures in step S131 accumulates to a certain number, that is, the cumulative number of initialization failures reaches a preset number, it can be determined that the quality of the current depth image is not good enough, and it is necessary to re-enter step S110 to obtain a new frame of the target depth image of the target scene through the depth camera, and continue with the subsequent steps based on the new frame of the target depth image.

[0064] The sets of normal vector modes obtained by searching each mode respectively correspond to normal vector modes r1, r2, and r3. At this time, the specific quantity of the normal vector mode cannot be directly determined based on the set of normal vector modes, because the normal vectors in the three sets of normal vector modes do not satisfy strict mutual perpendicularity. Therefore, new normal vector modes can be calculated for the sets of normal vector modes obtained after the search respectively.

[0065] In one embodiment, all the unit normal vectors in each set of normal vectors are transformed into the tangent plane coordinate system of the corresponding normal vector mode at the unit sphere centered at the origin of the camera coordinates. Since they are unit normal vectors, the normal vectors can be mapped to the unit sphere centered at the origin of the camera coordinates through the Gauss mapping. Thus, one end of each unit normal vector lies on the unit sphere. Then, calculate the normalized two-dimensional coordinates of the unit normal vectors in the set of normal vectors in the tangent plane coordinate system, and calculate the mean shift vector according to the normalized two-dimensional coordinates; then transform the mean shift vector onto the unit sphere to obtain the updated normal vector mode.

[0066] The specific calculation process is as follows: First, use the rotation matrix Q to rotate n i,j to the tangent plane coordinate system of the unit sphere with r j as the positive direction of the z-axis, and obtain the rotated n i,j , where the origin of the tangent plane coordinate system is at the end point of the vector r j , the origin of the spherical coordinate system is at the center of the sphere, and the coordinate axis directions of the two coordinate systems are the same. Specifically, for j = 1, 2, 3, Q corresponds to Q1 = [r2, r3, r1], Q2 = [r3, r1, r2], and Q3 = [r1, r2, r3] respectively. The rotated n i,j is calculated as: n i,j ←Q j n i,j , where the arrow "←" represents an update, that is, the product of Q j and n i,j results in the rotated n i,j , that is, the new n i,j . For the sake of distinction, the rotated n i,j or the new n i,j can be denoted as

[0067] Furthermore, calculate the normalized two-dimensional coordinate m of i,j in the tangent plane coordinate system. In a possible implementation, the calculation formula for the normalized two-dimensional coordinate m i,j is:

[0068]

[0069] where is the sign function: when when when

[0070] After obtaining the normalized two-dimensional coordinates of all the unit normal vectors in the set of normal vectors N j in the tangent plane coordinate system, calculate the mean shift vector sj 。In a possible implementation, s j The calculation formula of is:

[0071]

[0072] where c is a constant, which can be an empirical value or a better value obtained through experiments.

[0073] Since the mean shift vector s j is expressed in the tangent plane coordinate system at the unit sphere with the positive z-axis direction, and it is re-converted to the unit sphere. The conversion formula is: At this time, the unit sphere takes r j as the positive z-axis direction. Further, is normalized and converted to the camera coordinate system. The conversion formula is: where the arrow "←" represents the update. In this way, the updated normal vector modes are obtained, which are respectively

[0074] S132. According to the normal vector modes, the absolute pose of the depth camera relative to the Manhattan world is estimated using the singular value decomposition algorithm (SVD decomposition)

[0075] The absolute pose of the depth camera relative to the Manhattan world corresponds to the three normal vectors of the Manhattan world.

[0076] In one embodiment, according to the updated normal vector modes, the absolute pose of the depth camera relative to the Manhattan world is estimated using the singular value decomposition algorithm (SVD decomposition) The calculation formula of is:

[0077]

[0078]

[0079] where j = 1, 2, 3, λ j is the weight factor used to indicate the credibility of this normal vector mode. λ j The calculation formula of is:

[0080]

[0081] where Card(N j ) represents the number of unit normal vectors in the normal vector set N j , that is, the number of elements in the set.

[0082] It should be noted that when the algorithm runs normally once, when the algorithm is started for the second time, the normal vector mode r to be searchedj The j-th column of the estimated last time can be selected.

[0083] In this embodiment, the mean shift vector is used to estimate the absolute pose of the depth camera relative to the Manhattan world. Since no assumption is made about the distribution form of the absolute pose, it is more robust. In addition, compared with the parameter estimation method, the computational complexity can be reduced and the computing power can be saved.

[0084] Another embodiment of the present application provides an indoor pose estimation method. On the basis of the foregoing embodiment, the cumulative error of the gyroscope is corrected by using the absolute pose, which can make the result of pose estimation more accurate and more robust. As Figure 5 FIG. is a schematic flowchart of the implementation of an indoor pose estimation method provided by an embodiment of the present application. The indoor pose estimation method in this embodiment can be executed by a carrier. As Figure 5 shown, the indoor pose estimation method may include steps S210 to S260. It should be noted that for the same parts as those in the foregoing embodiment, please refer to the foregoing, and details will not be described herein again.

[0085] S210, obtaining a target depth image of a target scene through a depth camera.

[0086] S220, segmenting the target depth image into a plurality of sub-images, and calculating the unit normal vector n i of each sub-image, where i is the serial number of the sub-image.

[0087] S230, extracting the unit normal vector and matching it with the normal vector of the Manhattan world to estimate the absolute pose of the depth camera relative to the Manhattan world.

[0088] S240, obtaining a real-time angular velocity measurement value w m through a gyroscope.

[0089] S250, calculating the pose of the carrier and the gyroscope zero drift according to the real-time angular velocity measurement value w m

[0090] In the embodiments of the present application, in order to ensure higher accuracy, an error-state Kalman filter is used for data fusion. Since the gyroscope has angular velocity zero drift, it is necessary to correct the angular velocity zero drift. Among them, the state to be estimated of the carrier is set as x = [q, b], where q is the quaternion attitude of the carrier relative to the Manhattan world, and b is the gyroscope zero drift. Before starting the calculation, initialization settings need to be performed to initialize the state to be estimated. At this time, q = q0, b = 0, q0 is the quaternion attitude of the carrier relative to the Manhattan world obtained after the algorithm is successfully started for the first time in step S230, and the error-state covariance matrix P = I, indicating that the matrix is an identity matrix of size 6 by 6 at the beginning. The error state corresponding to the state to be estimated is δx = [δθ, δb], where δθ is the attitude error state, represented by three-dimensional Euler angles, and δb is the gyroscope zero-drift error state. Step S250 is based on the gyroscope measurement value w m , and predicts the state to be estimated and the error-state covariance matrix.

[0091] In one embodiment, the real-time state prediction includes the following process: First, obtain the angular velocity measurement value w at the current moment m and predict the current state based on the state at the previous moment.

[0092] As a possible implementation, the prediction formula is:

[0093]

[0094] b ← b,

[0095] where Δt is the time difference between the current moment and the previous moment; q{v} represents converting the rotation vector v into the corresponding quaternion, and the conversion formula is: v = (w m - b)Δt = φu, φ is the modulus of the rotation vector v, and u is the rotation vector v divided by the modulus, that is, the corresponding unit vector. According to the prediction formula, the state at the current moment can be obtained as x = [q, b].

[0096] In one embodiment, predicting the error-state covariance matrix P includes the following process: First, obtain the angular velocity measurement value w at the current moment m and predict the current error-state covariance matrix based on the state at the previous moment and the error-state covariance matrix at the previous moment.

[0097] As a possible implementation, the prediction formula is:

[0098] P ← FPF T + Q i ,

[0099] where, R is the rotation matrix corresponding to the attitude at the previous moment, Qi is a fixed value, related to the covariance of the gyroscope Gaussian white noise and the zero-drift random walk noise. The characteristic parameters of the gyroscope Gaussian white noise and the zero-drift random walk noise can be directly obtained from the IMU data sheet, and then Q can be calculated. i , that is to say, the Q of each gyroscope i is determined.

[0100] It should be noted that when the algorithm is executed, after step S230 is initially started, step S240 is immediately carried out. After that, step S240 is carried out synchronously with step S230. In the specific implementation process, the two steps S230 and S240 can run as two independent threads or processes in the processor.

[0101] It should be noted that during the execution of the algorithm, when the number of vertical planes of the target scene within the current frame field of view is less than two during the running of step S230, resulting in the failure of the absolute pose estimation of the depth camera relative to the Manhattan world, step S240 still proceeds as normal, without affecting the normal operation of the overall system. This fusion strategy makes the system more robust.

[0102] S260, according to the absolute pose, correct the pose of the carrier and the gyroscope zero-drift to obtain the corrected pose of the carrier and the gyroscope zero-drift.

[0103] The absolute pose of the Manhattan world is obtained by executing steps S210 to S230 above The pose of the carrier and the gyroscope zero-drift are obtained by executing steps S240 to S250. Step S260 is the process of completing the pose correction. Specifically, after the Manhattan world pose estimation value is output by step S230 , use to correct each predicted value in step S250.

[0104] In some embodiments, an error-state Kalman filter can be used for correction. In one embodiment, as Figure 6 shown, step S260 may include: step S261 and S262.

[0105] S261, according to the absolute pose, correct the error state and the error state covariance matrix. The correction formula is:

[0106] K = PH T (HPH T + V) -1

[0107]

[0108] P ← (I - KH)P

[0109] Where V is a constant value, is the Manhattan world attitude quaternion form in the carrier coordinate system, is the observation equation, H is the Jacobian matrix of h() with respect to the error state. is the transpose matrix of. represents the rotation transformation matrix from the carrier coordinate system (the carrier coordinate system coincides with the coordinate system of the IMU or the gyroscope) to the depth camera coordinate system. In actual implementation, this rotation matrix can be obtained through external parameter calibration or from the 3D drawing of the sensor installation structure.

[0110] S262, according to the corrected error state, correct the attitude of the carrier.

[0111] After obtaining the error state in step S261, correct the estimated state x obtained in step S250. The correction formula is:

[0112]

[0113]

[0114] At this point, q is the corrected attitude estimate value. In actual application, q can be output to the backend application for subsequent processing.

[0115] The embodiment of the present application estimates the attitude of a mobile carrier in an indoor environment by fusing the measurement values of a depth camera and a gyroscope. First, based on the Manhattan world hypothesis, the absolute attitude of the depth camera relative to the Manhattan world is estimated. Then, based on the error state Kalman filter, the gyroscope measurement values are fused to make the attitude estimation more accurate and robust.

[0116] It should be understood that the magnitudes of the sequence numbers of the steps in the above embodiments do not mean the order of execution. The execution order of each process should be determined according to its function and internal logic, and should not constitute any limitation to the implementation process of the embodiments of the present application.

[0117] An embodiment of the present application also provides an indoor attitude estimation device. For details not described in this indoor attitude estimation device, please refer to the description in the foregoing embodiment of the indoor attitude estimation method.

[0118] See Figure 7 , Figure 7 which is a schematic structural diagram of an indoor attitude estimation device provided by an embodiment of the present application. The indoor attitude estimation device is configured on a carrier, and the carrier includes a depth camera. As Figure 7 shown, the indoor attitude estimation device includes: a depth image acquisition execution module 71, a segmentation and calculation module 72, and an estimation module 73.

[0119] Among them, the depth image acquisition execution module 71 is used to acquire the target depth image of the target scene through a depth camera.

[0120] The segmentation and calculation module 72 is used to segment the target depth image into multiple sub-images and calculate the unit normal vector of each sub-image.

[0121] The estimation module 73 is used to extract the unit normal vector and match it with the normal vector of the Manhattan world, and estimate the absolute pose of the depth camera relative to the Manhattan world.

[0122] See Figure 8 , Figure 8 is a schematic structural diagram of an indoor pose estimation device provided by another embodiment of the present application. The indoor pose estimation device is configured on a carrier, and the carrier includes a depth camera and a gyroscope. As Figure 8 shown, the indoor pose estimation device includes: a depth image acquisition execution module 81, a segmentation and calculation module 82, an estimation module 83, an angular velocity measurement execution module 84, a calculation module 85, and a calibration module 86.

[0123] Among them, the depth image acquisition execution module 81 is used to acquire the target depth image of the target scene through a depth camera.

[0124] The segmentation and calculation module 82 is used to segment the target depth image into multiple sub-images and calculate the unit normal vector of each sub-image.

[0125] The estimation module 83 is used to extract the unit normal vector and match it with the normal vector of the Manhattan world, and estimate the absolute pose of the depth camera relative to the Manhattan world.

[0126] The angular velocity measurement execution module 84 is used to acquire the real-time angular velocity measurement value through a gyroscope.

[0127] The calculation module 85 is used to calculate the pose of the carrier and the gyroscope zero drift according to the real-time angular velocity measurement value.

[0128] The calibration module 86 is used to calibrate the pose of the carrier and the gyroscope zero drift according to the absolute pose to obtain the calibrated pose of the carrier and the gyroscope zero drift.

[0129] Those skilled in the art can clearly understand that, for the convenience and brevity of description, only the above-mentioned division of each functional unit and module is used as an example. In actual applications, the above-mentioned functions can be allocated to different functional units and modules according to needs, that is, the internal structure of the device is divided into different functional units or modules to complete all or part of the functions described above. Each functional unit and module in the embodiment can be integrated into a processing unit, or each unit can exist physically alone, or two or more units can be integrated into one unit. The above-mentioned integrated unit can be implemented in the form of hardware or in the form of a software functional unit. In addition, the specific names of each functional unit and module are only for the convenience of mutual distinction and do not limit the protection scope of this application. The specific working process of the units and modules in the above system can refer to the corresponding process in the foregoing method embodiment and will not be elaborated here.

[0130] An embodiment of this application also provides a computer-readable storage medium. The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps in the embodiment of the indoor pose estimation method can be implemented.

[0131] An embodiment of this application provides a computer program product. When the computer program product runs on an electronic device, the electronic device can implement the steps in the embodiment of the indoor pose estimation method.

[0132] In the above embodiments, the descriptions of each embodiment have their own emphases. For the parts not detailed or recorded in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0133] Those of ordinary skill in the art can realize that the units and algorithm steps of each example described in combination with the embodiments disclosed in this article can be implemented by electronic hardware, or by a combination of computer software and electronic hardware. Whether these functions are executed in a hardware or software manner depends on the specific application and design constraints of the technical solution. Professionals can use different methods to implement the described functions for each specific application, but such implementation should not be considered to exceed the scope of this application.

[0134] In the embodiments provided in the present application, it should be understood that the disclosed device / electronic device and method can be implemented in other ways. For example, the device / electronic device embodiments described above are merely illustrative. For example, the division of modules or units is only a logical function division. In actual implementation, there may be other division methods. For example, 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 displayed or discussed couplings or direct couplings or communication connections between each other can be through some interfaces. The indirect couplings or communication connections of devices or units can be in electrical, mechanical or other forms.

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

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

[0137] If the integrated module / 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, to implement all or part of the processes in the above method embodiments of the present application, it can also be completed by a computer program instructing relevant hardware. The computer program can be stored in a computer-readable storage medium. When the computer program is executed by a processor, the steps of the above various method embodiments can be implemented. Among them, the computer program includes computer program code, and the computer program code can be in source code form, object code form, executable file or some intermediate form, etc. The computer-readable medium can include: any entity or device capable of carrying the computer program code, recording medium, USB flash drive, mobile hard disk, magnetic disk, optical disc, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signal, telecommunication signal, and software distribution medium, etc. It should be noted that the content included in the computer-readable medium can be appropriately increased or decreased according to the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, the computer-readable medium does not include electrical carrier signals and telecommunication signals.

[0138] The above embodiments are only used to illustrate the technical solutions of the present application, rather than to limit it; although the present application has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: they can still modify the technical solutions recorded in the foregoing embodiments, or perform equivalent replacements on some of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the various embodiments of the present application, and should all be included within the protection scope of the present application.

Claims

1. An indoor pose estimation method, characterized in that, Applied to a carrier, the carrier includes a depth camera, and the indoor pose estimation method includes: Obtaining a target depth image of a target scene through the depth camera; Segmenting the target depth image into multiple sub-images and calculating the unit normal vector of each sub-image; Randomly selecting three normal vector modes to be searched; Searching each of the normal vector modes to be searched separately, and finding a set of normal vectors in the unit normal vectors near each of the normal vector modes to be searched; If the number of unit normal vectors in the set of normal vectors is greater than or equal to a preset threshold, determining the set of normal vectors of the normal vector mode to be searched; If the number of unit normal vectors in the set of normal vectors is less than the preset threshold, returning to the step of randomly selecting three normal vector modes to be searched; Converting all the unit normal vectors in the set of normal vectors to the tangent coordinate system at the tangent plane of the normal vector mode to be searched on the unit sphere centered at the origin of the camera coordinate system, and calculating the normalized two-dimensional coordinates of the unit normal vectors in the set of normal vectors in the tangent coordinate system; Calculating a mean shift vector according to the normalized two-dimensional coordinates, and converting the mean shift vector to the unit sphere to obtain an updated normal vector mode; Estimating the absolute pose of the depth camera relative to the Manhattan world according to the normal vector mode by using a singular value decomposition algorithm; The carrier further includes a gyroscope: Obtaining a real-time angular velocity measurement value through the gyroscope; Calculating the pose of the carrier and the gyroscope zero drift according to the real-time angular velocity measurement value; Correcting the pose of the carrier and the gyroscope zero drift according to the absolute pose to obtain a corrected carrier pose and gyroscope zero drift.

2. The indoor attitude estimation method according to claim 1, wherein The segmenting the target depth image into multiple sub-images and calculating the unit normal vector of each sub-image includes: Performing bilateral filtering on the target depth image to obtain a first depth image; Undistorting the first depth image based on the internal parameters of the depth camera to obtain a second depth image; Segmenting the second depth image into multiple sub-images, and converting the depth information of each sub-image into 3D point clouds in the camera coordinate system; Calculating the unit normal vector of the 3D point cloud of each sub-image.

3. The indoor pose estimation method according to claim 1, wherein, The correcting the pose of the carrier and the gyroscope zero drift according to the absolute pose to obtain a corrected carrier pose and gyroscope zero drift includes: Correcting the pose of the carrier and the gyroscope zero drift according to the absolute pose by using an error state Kalman filter to obtain a corrected carrier pose and gyroscope zero drift.

4. An indoor attitude estimation device, characterized in that, Configured on a carrier, the carrier includes a depth camera, and the indoor pose estimation device includes: A depth image acquisition execution module for obtaining a target depth image of a target scene through the depth camera; A segmentation and calculation module for segmenting the target depth image into multiple sub-images and calculating the unit normal vector of each sub-image; An estimation module for: Randomly selecting three normal vector modes to be searched; Searching each of the normal vector modes to be searched separately, and finding a set of normal vectors in the unit normal vectors near each of the normal vector modes to be searched; If the number of unit normal vectors in the set of normal vectors is greater than or equal to a preset threshold, determine the set of normal vectors of the to-be-searched normal vector mode; If the number of unit normal vectors in the set of normal vectors is less than the preset threshold, return to the step of randomly selecting three to-be-searched normal vector modes; Convert all the unit normal vectors in the set of normal vectors to the tangent plane coordinate system at the tangent plane of the to-be-searched normal vector mode on the unit sphere centered at the origin of the camera coordinate system, and calculate the normalized two-dimensional coordinates of the unit normal vectors in the set of normal vectors in the tangent plane coordinate system; Calculate the mean shift vector according to the normalized two-dimensional coordinates, and convert the mean shift vector to the unit sphere to obtain the updated normal vector mode; According to the normal vector mode, use the singular value decomposition algorithm to estimate the absolute pose of the depth camera relative to the Manhattan world; The carrier further includes a gyroscope: Obtain the real-time angular velocity measurement value through the gyroscope; Calculate the pose of the carrier and the gyroscope zero drift according to the real-time angular velocity measurement value; According to the absolute pose, correct the pose of the carrier and the gyroscope zero drift to obtain the corrected pose of the carrier and the gyroscope zero drift.

5. A carrier for indoor attitude estimation, comprising a processor and a computer program stored in a memory and executable on the processor, characterized in that, The carrier further includes a depth camera, and when the processor executes the computer program, it implements the indoor pose estimation method according to any one of claims 1 to 3.

6. A computer storage medium storing a computer program, characterized in that, When the computer program is executed by a processor, it implements the indoor pose estimation method according to any one of claims 1 to 3.

Images