Pose estimation method, apparatus, and control device

Abstract

Embodiments of the present application disclose a pose estimation method and device and a control device. The method comprises: obtaining vehicle motion information and first IMU information of a control device; converting the first IMU information into second IMU information in a vehicle coordinate system based on the vehicle motion information; and obtaining pose information of the control device relative to the vehicle based on the second IMU information. In this way, the first IMU information can be converted into second IMU information in a vehicle coordinate system based on vehicle motion information, so that the first IMU information and the vehicle motion information can be effectively fused to obtain accurate second IMU information, and then the pose information of the control device on the vehicle can be obtained based on the second IMU information, thereby improving the accuracy of the pose information.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of computers, and more particularly, to a pose estimation method and device and a control device. BACKGROUND

[0002] As a second living space for people, vehicles have a growing position in daily life, making the demand for using MR (Mixed Reality), VR (Virtual Reality), XR (Extended Reality), AR (Augmented Reality) devices and the like in the vehicle cabin gradually increasing. In order to improve the user experience, the above devices can also be configured with a control device (such as a handle, etc.). In related manners, the pose information of the control device can be determined based on a VIO (Visual-Inertial Odometry) algorithm, but the related manner still has the problem that the obtained pose information is inaccurate. SUMMARY

[0003] In view of the above problems, the present application provides a pose estimation method, device and control device to improve the above problems.

[0004] In a first aspect, the present application provides a pose estimation method, which comprises: obtaining vehicle motion information and first IMU information of a control device; converting the first IMU information into second IMU information in a vehicle coordinate system based on the vehicle motion information; and obtaining pose information of the control device relative to the vehicle based on the second IMU information.

[0005] In a second aspect, the present application provides a pose estimation device, which comprises: an IMU information acquisition unit configured to obtain vehicle motion information and first IMU information of a control device; convert the first IMU information into second IMU information in a vehicle coordinate system based on the vehicle motion information; and a pose information acquisition unit configured to obtain pose information of the control device relative to the vehicle based on the second IMU information.

[0006] In a third aspect, the present application provides a control device comprising one or more processors and a memory; one or more programs stored in the memory and configured to be executed by the one or more processors, the one or more programs being configured to perform the above method.

[0007] In a fourth aspect, the present application provides a computer-readable storage medium, wherein the computer-readable storage medium stores program codes, and the program codes perform the above method when running.

[0008] The pose estimation method, device, control device and storage medium provided by the present application are as follows: after obtaining vehicle motion information and first IMU information of a control device, the first IMU information is converted into second IMU information in a vehicle coordinate system based on the vehicle motion information, and the pose information of the control device relative to the vehicle is obtained based on the second IMU information. In this way, the first IMU information can be converted into second IMU information in the vehicle coordinate system based on the vehicle motion information, so that the first IMU information and the vehicle motion information can be effectively fused to obtain accurate second IMU information, and then the pose information of the control device on the vehicle is obtained based on the second IMU information, thereby improving the accuracy of the pose information. BRIEF DESCRIPTION OF DRAWINGS

[0009] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings needed in the embodiment description will be briefly introduced. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor.

[0010] Figure 1 A flowchart of a pose estimation method according to an embodiment of the present application is shown;

[0011] Figure 2 A schematic diagram of a time difference between a vehicle and a control device according to the present application is shown;

[0012] Figure 3 A flowchart of a pose estimation method according to another embodiment of the present application is shown;

[0013] Figure 4 A structural block diagram of a pose estimation device according to an embodiment of the present application is shown;

[0014] Figure 5 A structural block diagram of a control device according to the present application is shown. DETAILED DESCRIPTION

[0015] The technical solutions in the embodiments of the present application will be described clearly and completely with reference to the drawings in the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor fall within the scope of protection of the present application.

[0016] In the embodiments of the present application, the inventors provide a pose estimation method, device and control equipment. After obtaining vehicle motion information and first IMU information of the control equipment, the first IMU information is converted into second IMU information in a vehicle coordinate system based on the vehicle motion information, and the pose information of the control equipment relative to the vehicle is obtained based on the second IMU information. In this way, the first IMU information can be converted into second IMU information in the vehicle coordinate system based on the vehicle motion information, so that the first IMU information and the vehicle motion information can be effectively fused to obtain accurate second IMU information, and then the pose information of the control equipment on the vehicle is obtained based on the second IMU information, thereby improving the accuracy of the pose information.

[0017] Please refer to Figure 1 The embodiments of the present application provide a pose estimation method, which comprises:

[0018] S110: Obtain vehicle motion information and first IMU information of the control equipment.

[0019] The vehicle motion information can refer to information for describing the motion of the vehicle in a world coordinate system (such as a UTM (Universal Transverse Mercator Grid System) coordinate system), and can include vehicle position information, vehicle speed information and vehicle rotation information. The vehicle rotation information can be used to represent the orientation of the vehicle, that is, the attitude in the world coordinate system. The first IMU information can refer to information for describing the motion of the control equipment in the world coordinate system, and can include first position information and first rotation information. The first rotation information can represent the attitude of the control equipment in the world coordinate system.

[0020] In the embodiments of the present application, the control equipment can refer to a handle with a physical entity matched with an MR (Mixed Reality) device, a VR (Virtual Reality) device, an XR (Extended Reality) device, an AR (Augmented Reality) device, etc. The control equipment can also refer to an electronic device (such as a mobile phone, a tablet, etc.) loaded with a virtual handle. The virtual handle can refer to an application loaded on the control equipment, which can present images of all physical handle buttons on the screen of the control equipment, so that the user can simulate the operation of the real physical handle by operating the button images on the touch screen.

[0021] The control device can establish a network connection with the vehicle via technologies such as Bluetooth and WiFi (Wireless-Fidelity) to obtain vehicle motion information; in addition, the control device can be equipped with an IMU (Inertial Measurement Unit), which can be used to detect and measure the acceleration and rotational motion of the control device in the world coordinate system.

[0022] In one approach, the control device can acquire vehicle motion information and the control device's first IMU information in real time.

[0023] Optionally, when acquiring vehicle motion information and first IMU information, the NTP (Network Time Protocol) service provided by the vehicle can be used to synchronize the acquisition time of the vehicle motion information acquired by the control device with that of the first IMU information. In other words, vehicle motion information and first IMU information acquired at the same acquisition time can be obtained.

[0024] Optionally, if the vehicle does not provide NTP service, the time delay between the vehicle motion information and the first IMU information received by the control device at the same acquisition time can be calculated by averaging the error. The calculated time delay can then be used to time-align the vehicle motion information and the first IMU information, thereby obtaining the vehicle motion information and the first IMU information at the same acquisition time.

[0025] For example, such as Figure 2 As shown, the vehicle and the control equipment use different times, and the delay can be calculated by the data transmission rate and the amount of data transmitted between them.

[0026] In this embodiment of the application, by using NTP service or averaging error, the vehicle motion information acquired by the control device is synchronized with the acquisition time of the first IMU information, which can improve the accuracy and reliability of the pose information.

[0027] S120: Based on the vehicle motion information, convert the first IMU information into second IMU information in the vehicle coordinate system.

[0028] One approach is to obtain the acceleration function in the second IMU information based on vehicle position information, vehicle speed information, and first position information. The acceleration function can characterize the relationship between the acceleration of the control device in the vehicle coordinate system and the vehicle acceleration. Based on vehicle rotation information and first rotation information, the angular velocity function in the second IMU information can be obtained. The angular velocity function can characterize the relationship between the angular velocity of the control device in the vehicle coordinate system and the vehicle angular velocity.

[0029] Optionally, a position function can be obtained based on the vehicle position information and the first position information, the position function can represent a relationship between position information of the control device in the vehicle coordinate system and the vehicle position information and the first position information; and an acceleration function can be obtained based on the position function and the vehicle speed information.

[0030] The position function can be:

[0031] P car,imu = P car，world + R car，world P world,imu

[0032] The position function can be: car，imu P car，world may be an inverse of P world，car P car，world and P world，car may be matrices, P world，car may represent vehicle position information, P car，world may represent world position information in the vehicle coordinate system; R car，world may represent an inverse of R world，car R car，world and R world，car may be matrices, R world，car may represent vehicle rotation information, R car，world may represent world rotation information in the vehicle coordinate system; and P world,imu may represent the first position information.

[0033] Optionally, the process of obtaining the acceleration function based on the position function and the vehicle speed information can be: first, performing partial derivation on the position function to obtain a velocity function; and then obtaining the acceleration function based on the velocity function and the vehicle speed information.

[0034] The vehicle motion information can further include angular velocity information, angular acceleration information, etc., and the first IMU information can further include first velocity information, first acceleration information, etc.

[0035] The velocity function can be:

[0036]

[0037] The velocity function can be: car，imu P car，imu may represent position information of the control device in the vehicle coordinate system; P car，world may be an inverse of P world，car P world，car may represent vehicle position information; R car，world may represent an inverse of R world，carinverse of R world，car may represent vehicle rotation information; P world,imu may represent first position information; v world，car may represent vehicle speed information; v world,min may represent speed information of the control device in the world coordinate system, that is, first speed information in the first IMU information; w car，world may be w world，car inverse of w world，car may represent angular velocity information of the vehicle in the world coordinate system, that is, angular velocity information in the vehicle motion information.

[0038] wherein the acceleration function can be:

[0039]

[0040] wherein a car，imu may represent speed information of the control device in the vehicle coordinate system; v car，imu may represent speed information of the control device in the vehicle coordinate system; R car，world may represent R world，car inverse of R world，car may represent vehicle rotation information; v world,min may represent speed information of the control device in the world coordinate system, that is, first speed information in the first IMU information; w car，world may be w world，car inverse of w world，car may represent angular velocity information of the vehicle in the world coordinate system, that is, angular velocity information in the vehicle motion information; a world,min may represent acceleration information of the control device in the vehicle coordinate system, that is, first acceleration information in the first IMU information; w′ car，world may represent w′ world，car inverse of w′ world，car may represent acceleration information of the vehicle angular velocity in the world coordinate system, that is, angular acceleration information in the vehicle motion information.

[0041] Optionally, based on the vehicle rotation information and the first rotation information, a process of obtaining angular velocity function in the second IMU information is as follows:

[0042]

[0043]

[0044] wherein R car，imu may represent speed information of the control device in the vehicle coordinate system; R world，car may represent vehicle rotation information; R world,mminThe rotation information of the control device in the world coordinate system, that is, the first rotation information in the first IMU information.

[0045] S130: Obtain the pose information of the control device relative to the vehicle based on the second IMU information.

[0046] The pose information of the control device relative to the vehicle can be understood as the pose information of the control device in the vehicle coordinate system.

[0047] As a manner, the pose information of the control device relative to the vehicle can be obtained based on the second IMU information and an EKF (Extended Kalman Filter) algorithm.

[0048] The pose estimation method provided in the embodiment comprises the following steps: obtaining vehicle motion information and first IMU information of a control device; converting the first IMU information into second IMU information in a vehicle coordinate system based on the vehicle motion information; and obtaining the pose information of the control device relative to the vehicle based on the second IMU information. In this way, the first IMU information can be converted into the second IMU information in the vehicle coordinate system based on the vehicle motion information, so that the first IMU information and the vehicle motion information can be effectively fused to obtain accurate second IMU information, and then the pose information of the control device on the vehicle can be obtained based on the second IMU information, thereby improving the accuracy of the pose information.

[0049] Please refer to Figure 3 The pose estimation method provided in the embodiment comprises the following steps:

[0050] S210: Obtain vehicle motion information and first IMU information of a control device.

[0051] S220: Convert the first IMU information into second IMU information in a vehicle coordinate system based on the vehicle motion information.

[0052] S230: Obtain a photometric error function based on multiple images collected by a multi-camera in the control device, wherein the photometric error function represents photometric errors between multiple feature point pairs in the multiple images, and two feature points in each feature point pair come from different images.

[0053] The photometric error can represent the brightness error of two feature points.

[0054] As a manner, the feature point pair position relation function can be obtained based on multiple images and a direct method, and the feature point pair position relation function can represent a position relation between two feature points in each feature point pair; and the photometric error function can be obtained based on the feature point pair position relation function and a photometric calibration algorithm.

[0055] The multi-view camera can refer to a camera including at least two cameras, and all cameras in the multi-view camera can continuously capture images at the same capture frequency. At the same capture moment, each camera in the multi-view camera can capture an image. Therefore, one feature point can be selected from two images captured by the same camera at adjacent capture moments (adjacent frames) according to a direct method (Direct Method) to form a feature point pair; and one feature point can also be selected from two images captured by different cameras at the same capture moment according to the direct method to form a feature point pair, and each feature point corresponds to a pixel point in the corresponding image. The direct method can refer to an algorithm for calculating the motion of the feature point.

[0056] In the embodiments of the present application, the feature point pairs are extracted from the images captured by the same camera at adjacent frames, and the feature point pairs are also extracted from the images captured by different cameras at the same capture moment, that is, the motion change of the same camera in the vehicle can be tracked, and the real-time pose relation between different cameras can also be tracked, so that the plurality of feature point pairs can represent at least two kinds of features related to the pose information of the control device, thereby improving the density and effectiveness of the feature information in the plurality of feature point pairs, and further improving the accuracy of the finally obtained pose information.

[0057] Optionally, in order to reduce the amount of calculation, a sparse direct method (Sparse Direct Method) in the direct method can be used to select the pixel points with gradient changes exceeding a preset value as the feature points.

[0058] For example, the position relation of the two feature points in the feature point pair obtained based on the sparse direct method is:

[0059]

[0060] wherein (u1, v1) can represent the position information of one feature point (feature point 1) in the corresponding image (image 1) in the feature point pair, (u2, v2) can represent the position information of the other feature point (feature point 2) in the corresponding image (image 2) in the feature point pair, ∏1(.) can represent the projection function of the camera model (camera) corresponding to image 1, ∏2(.) can represent the projection function of the camera model (camera) corresponding to image 2, which can represent the relative position and attitude of image 1 and image 2.

[0061] Optionally, the photometric error function can be:

[0062]

[0063] wherein I1(u1, v1) can represent the pixel value of the feature point 1 in the image 1, I2(u2, v2) can represent the pixel value of the feature point pair (feature point 2) in the image 2, G(.) can represent a response function in the photometric calibration algorithm, which is an inherent attribute of the fresh egg, and V(.) can represent an attenuation factor in the photometric calibration algorithm, which can be used to describe the vignetting phenomenon.

[0064] In the embodiments of the present application, by introducing photometric calibration, the pose estimation algorithm proposed in the present application can better cope with the huge changes of light and shadow inside the vehicle cabin, and improve the accuracy and robustness of pose estimation.

[0065] S240: obtaining the pose information of the control device relative to the vehicle based on the second IMU information, the photometric error function and an extended Kalman filtering algorithm.

[0066] wherein the pose information of the control device relative to the vehicle can be:

[0067]

[0068] wherein P car,imu may represent the position information of the control device in the vehicle coordinate system (relative to the vehicle); v car,imu may represent the speed of the control device in the vehicle coordinate system; R car,imu may represent the rotation information of the control device in the vehicle coordinate system; bias acc may represent the bias (offset) of the IMU measured acceleration in the control device; bias acc may represent the bias (offset) of the gyroscope in the IMU in the control device; d i may represent the depth value of the i-th feature point.

[0069] It should be noted that the above x can be the state of the system corresponding to the extended Kalman filtering algorithm, and an initial value containing the same information as x can be input when obtaining the pose information by the extended Kalman filtering algorithm. The pose information of the control device relative to the vehicle can be 6DoF (Degree of Freedom) information.

[0070] As a manner, the predicted state information and the predicted covariance matrix can be obtained based on the second IMU information, the state prediction equation and the covariance matrix prediction equation; and the pose information of the control device relative to the vehicle can be obtained based on the predicted state information, the predicted covariance matrix and the photometric error function.

[0071] The state prediction equation can be used to make a priori estimation on the pose information of the control device relative to the vehicle, that is, to obtain the predicted state information, and then the a priori estimation result can be corrected through the predicted state information, the predicted covariance matrix and the photometric error function, so as to obtain the pose information of the control device relative to the vehicle.

[0072] The state prediction equation can be:

[0073]

[0074] The f(.) can represent the second IMU information, X k|k The x can represent the pose information of the control device relative to the vehicle at the last moment, The ^ can represent the exclusive or operation.

[0075] The predicted covariance matrix can be:

[0076]

[0077] The F can represent the first-order linearization matrix of the f(.); the Q k The P can represent the error of system update, which is determined by the internal parameters of the IMU and obtained through the calibration of the IMU; and the P k|k The P can represent the covariance matrix at the last moment.

[0078] After the predicted state information and the predicted covariance matrix are obtained, the observation can be updated through the following formula:

[0079]

[0080] The r(.) can represent the residual, that is, the photometric error function in the application, which can be usually represented by the matrix of Gaussian error; the j can represent the iteration number of the algorithm; the v(.) can represent the first-order derivative of the system state x; the D(.) can represent the linearization matrix of the first-order derivative of the h(.) function relative to the v(.); and the H(.) can represent the linearization matrix of the first-order derivative of the h(.) function relative to the system state x.

[0081] After the observation is updated, the predicted state information and the predicted covariance matrix can be corrected based on the updated observation, so that the iteration can be continued based on the corrected predicted covariance matrix.

[0082]

[0083] wherein I, J, L can represent unit matrix, K can represent Kalman gain matrix, X k+1|k may represent the pose information of the control device relative to the vehicle.

[0084] Optionally, in the iteration process of the extended Kalman filtering algorithm, the conjugate gradient method can be used to reduce the running time of the system on the hardware and increase the accuracy of the system under the same number of iterations.

[0085] The pose estimation method provided in this embodiment, through the above-mentioned manner, can convert the first IMU information into the second IMU information in the vehicle coordinate system based on the vehicle motion information, so as to effectively fuse the first IMU information and the vehicle motion information, obtain accurate second IMU information, and then obtain the pose information of the control device on the vehicle based on the second IMU information, thereby improving the accuracy of the pose information. In this embodiment, the accuracy of the observation results of the multi-camera of the control device is improved by direct method, photometric calibration method, and the image data obtained by the multi-camera and the multi-sensor data (vehicle motion information, first IMU information, etc.) are fused by Kalman filtering algorithm, thereby improving the accuracy and stability of the pose information of the control device in the vehicle cabin environment, and then the user can not be disturbed by the change of light or the motion of the vehicle when using the MR device, VR device, XR device and AR device, can more accurately perceive and operate the virtual environment, and more smoothly interact, thereby improving the realism of the virtual reality experience. In addition, through the direct method and data fusion in this application, the accuracy and stability of the obtained control device pose information can be improved without adding additional hardware, thereby reducing the use cost of the MR device, VR device, XR device and AR device, and attracting more users.

[0086] Please refer to Figure 4 The pose estimation device 400 provided in this application comprises:

[0087] The IMU information acquisition unit 410 is configured to acquire vehicle motion information and first IMU information of a control device, and convert the first IMU information into second IMU information in a vehicle coordinate system based on the vehicle motion information.

[0088] The pose information acquisition unit 420 is configured to obtain the pose information of the control device relative to the vehicle based on the second IMU information.

[0089] As a manner, the vehicle motion information comprises vehicle position information, vehicle speed information and vehicle rotation information, the IMU information obtaining unit 410 is specifically configured to obtain an acceleration function in the second IMU information based on the vehicle position information, the vehicle speed information and the first position information, the acceleration function representing a relationship between an acceleration of the control device in a vehicle coordinate system and an acceleration of the vehicle; obtain an angular velocity function in the second IMU information based on the vehicle rotation information and the first rotation information, the angular velocity function representing a relationship between an angular velocity of the control device in the vehicle coordinate system and an angular velocity of the vehicle.

[0090] Optionally, the IMU information obtaining unit 410 is specifically configured to obtain a position function based on the vehicle position information and the first position information, the position function representing a relationship between position information of the control device in the vehicle coordinate system and the vehicle position information and the first position information; obtain the acceleration function based on the position function and the vehicle speed information.

[0091] As a manner, the pose information obtaining unit 420 is specifically configured to obtain a photometric error function based on a plurality of images collected by a multi-camera in the control device, wherein the photometric error function represents photometric errors between a plurality of feature point pairs in the plurality of images, and each feature point pair is from different images; obtain the pose information of the control device relative to the vehicle based on the second IMU information, the photometric error function and an extended Kalman filtering algorithm.

[0092] Optionally, the pose information obtaining unit 420 is specifically configured to obtain a feature point pair position relationship function based on the plurality of images and a direct method, the feature point pair position relationship function representing a position relationship between two feature points in each feature point pair; obtain the photometric error function based on the feature point pair position relationship function and a photometric calibration algorithm.

[0093] Optionally, the pose information obtaining unit 420 is specifically configured to obtain predicted state information and a predicted covariance matrix based on the second IMU information, a state prediction equation and a covariance matrix prediction equation; obtain the pose information of the control device relative to the vehicle based on the predicted state information, the predicted covariance matrix and the photometric error function.

[0094] The following will be combined with the specific embodiments of the present application to further illustrate the present application. Figure 5 A control device provided by the present application will be described.

[0095] Please refer to Figure 5, based on the above-mentioned pose estimation method and device, the embodiment of the application further provides another control device 100 which can execute the aforementioned pose estimation method. The control device 100 comprises a processor 102, a memory 104, a data acquisition module 106 and a network module 108. The memory 104 stores programs which can execute the contents of the aforementioned embodiments, and the backup processor 102 can execute the programs stored in the memory 104.

[0096] The processor 102 can include one or more processing cores. The processor 102 connects various parts in the control device 100 through various interfaces and lines, executes various functions of the control device 100 and processes data by running or executing instructions, programs, code sets or instruction sets stored in the memory 104, and calling data stored in the memory 104. Alternatively, the processor 102 can be implemented in at least one of the following hardware forms: a neural network processing unit (NPU), a digital signal processing (DSP), a field-programmable gate array (FPGA), and a programmable logic array (PLA). The processor 102 can be integrated with a combination of one or more of the following: a central processing unit (CPU), a graphics processing unit (GPU), a neural network processing unit (NPU), and a modem. Among them, the CPU mainly processes the operating system, user interface and application programs; the GPU is responsible for rendering and drawing display content; the NPU is responsible for processing multimedia data such as video and image; and the modem is used for processing wireless communication. It can be understood that the aforementioned modem can also not be integrated into the processor 102, but can be realized by a separate communication chip.

[0097] The memory 104 can include a random access memory (RAM), and can also include a read-only memory (ROM) and a double data rate (DDR). The memory 104 can be used to store instructions, programs, codes, code sets or instruction sets. The memory 104 can include a program storage area and a data storage area, where the program storage area can store instructions for implementing an operating system, instructions for implementing at least one function (such as a touch function, a sound playing function, an image playing function, etc.), instructions for implementing each of the following method embodiments, etc. The data storage area can also store data created by the control device 100 in use (such as a phone book, audio and video data, chat record data), etc.

[0098] The data acquisition module 106 can include, but is not limited to, an IMU (inertial measurement unit), a multi-view camera, a level, a light sensor, a motion sensor, a pressure sensor, an infrared thermal sensor, a distance sensor, an acceleration sensor, and other sensors. Among them, the IMU can obtain first IMU information of the control device 100. The multi-view camera can acquire environmental information around the control device 100, such as the seat and window of a vehicle, etc.

[0099] Among them, the pressure sensor can detect the pressure generated by pressing on the control device 100. That is, the pressure sensor detects the pressure generated by the contact or pressing between the user and the control device 100, for example, the pressure generated by the contact or pressing between the user's hand and the control device 100. Therefore, the pressure sensor can be used to determine whether contact or pressing occurs between the user and the control device 100, and the size of the pressure.

[0100] Among them, the acceleration sensor can detect the size of acceleration in each direction (generally three axes), and can detect the size and direction of gravity when at rest, which can be used for applications such as magnetometer attitude calibration, vibration recognition related functions (such as pedometer, tapping), etc. In addition, the control device 100 can also be configured with a gyroscope, a barometer, a hygrometer, a thermometer and other sensors, which will not be described here.

[0101] The network module 108 can be used to realize information interaction between the control device 100 and other devices through a network, for example, transmitting device control instructions, manipulation request instructions and state information acquisition instructions, etc. When the other device is a different device, the corresponding network module 108 of the other device can be different.

[0102] The embodiment of the present application provides a computer readable storage medium. The computer readable storage medium stores program codes, and the program codes can be invoked by a processor to execute the method described in the above method embodiment.

[0103] The computer readable storage medium can be an electronic storage such as a flash memory, an EEPROM (electrically erasable programmable read-only memory), an EPROM, a hard disk or a ROM. Alternatively, the computer readable storage medium comprises a non-volatile computer readable storage medium. The computer readable storage medium has a storage space for storing program codes for executing any method steps in the above method. The program codes can be read from or written into one or more computer program products. The program codes can be compressed in a suitable form.

[0104] To sum up, the pose estimation method, device and control equipment provided by the present application, after obtaining the vehicle motion information and the first IMU information of the control equipment, the first IMU information is converted into the second IMU information in the vehicle coordinate system based on the vehicle motion information, and the pose information of the control equipment relative to the vehicle is obtained based on the second IMU information. Through the above-mentioned manner, the first IMU information can be converted into the second IMU information in the vehicle coordinate system based on the vehicle motion information, so that the first IMU information and the vehicle motion information can be effectively fused to obtain accurate second IMU information, and then the pose information of the control equipment on the vehicle is obtained based on the second IMU information, thereby improving the accuracy of the pose information.

[0105] Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present application, but not to limit them; although the present application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that: it can still modify the technical solutions recorded in the foregoing embodiments, or make equivalent replacement for part of the technical features; and these modifications or replacements do not drive the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present application.

Claims

1. A pose estimation method, characterized by, The method comprises: acquiring vehicle motion information and first IMU information of a control device; converting the first IMU information into second IMU information in a vehicle coordinate system based on the vehicle motion information; obtaining a photometric error function based on multiple images captured by a multi-camera in the control device, wherein the photometric error function represents photometric errors between multiple feature point pairs in the multiple images, and each feature point pair is from different images; obtaining pose information of the control device relative to the vehicle based on the second IMU information, the photometric error function and an extended Kalman filtering algorithm.

2. The method of claim 1, wherein, The vehicle motion information comprises vehicle position information, vehicle speed information and vehicle rotation information, the first IMU information comprises first position information and first rotation information, and the conversion of the first IMU information into second IMU information in a vehicle coordinate system based on the vehicle motion information comprises: obtaining an acceleration function in the second IMU information based on the vehicle position information, the vehicle speed information and the first position information, wherein the acceleration function represents a relationship between acceleration of the control device in the vehicle coordinate system and acceleration of the vehicle; obtaining an angular velocity function in the second IMU information based on the vehicle rotation information and the first rotation information, wherein the angular velocity function represents a relationship between angular velocity of the control device in the vehicle coordinate system and angular velocity of the vehicle.

3. The method of claim 2, wherein, The obtaining of the acceleration function in the second IMU information based on the vehicle position information, the vehicle speed information and the first position information comprises: obtaining a position function based on the vehicle position information and the first position information, wherein the position function represents a relationship between position information of the control device in the vehicle coordinate system and the vehicle position information and the first position information; obtaining the acceleration function based on the position function and the vehicle speed information.

4. The method of claim 1, wherein, The obtaining of the photometric error function based on the multiple images captured by the multi-camera in the control device comprises: obtaining a feature point pair position relationship function based on the multiple images and a direct method, wherein the feature point pair position relationship function represents a position relationship between two feature points in each feature point pair; obtaining the photometric error function based on the feature point pair position relationship function and a photometric calibration algorithm.

5. The method of claim 1, wherein, The obtaining of the pose information of the control device relative to the vehicle based on the second IMU information, the photometric error function and the extended Kalman filtering algorithm comprises: obtaining predicted state information and a predicted covariance matrix based on the second IMU information, a state prediction equation and a covariance matrix prediction equation; obtaining the pose information of the control device relative to the vehicle based on the predicted state information, the predicted covariance matrix and the photometric error function.

6. The method according to any one of claims 1 to 5, characterized in that, The acquisition time of the vehicle motion information and the first IMU information is synchronized.

7. A pose estimation apparatus, characterized by comprising: The device comprises: An IMU information acquisition unit is configured to acquire vehicle motion information and first IMU information of a control device, and convert the first IMU information into second IMU information in a vehicle coordinate system based on the vehicle motion information; A pose information acquisition unit is configured to obtain a photometric error function based on multiple images captured by a multi-camera in the control device, and obtain pose information of the control device relative to the vehicle based on the second IMU information, the photometric error function and an extended Kalman filtering algorithm, wherein the photometric error function represents photometric errors between multiple feature point pairs in the multiple images, and each feature point pair includes two feature points from different images.

8. A control device, characterized by one or more processors and memory; one or more programs stored in the memory and configured for execution by the one or more processors, the one or more programs configured for performing the method of any one of claims 1-6.

9. A computer-readable storage medium, characterized in that, The computer readable storage medium has program code stored therein, wherein the method of any one of claims 1-6 is performed when the program code is executed.

Images