Positioning method, device and equipment based on multi-state color consistency constraints

Abstract

The present invention provides a positioning method, device and equipment based on multi-state color consistency constraints, which can be applied to the field of computer technology. The method includes: based on the three-dimensional position of the positioning point in the three-dimensional point cloud, performing imaging simulation on the positioning point to determine the imaging position of the positioning point in the current frame image; based on the projection position and imaging position of the positioning point in the current frame image, correcting the initial object state corresponding to the current frame image to obtain the corrected object state; based on multiple historical postures of the inertial measurement tool, multiple observed colors of the positioning point, the current color of the positioning point in the three-dimensional point cloud and color noise distribution parameters, correcting the corrected object state to obtain the target object state; based on the target object state, determining the target posture of the positioning object, so as to at least partially solve the technical problem of poor positioning effect existing in the related technology, and achieve the technical effect of improving positioning robustness and accuracy.

Description

Technical Field

[0001] The present invention relates to the field of computer technology, and in particular to a positioning method, device, equipment, medium and program product based on multi-state color consistency constraints. Background Art

[0002] In satellite-denied environments such as severe occlusion, underground, and indoors, the system can only rely on local sensors such as vision and laser for positioning. However, due to the inherent defects and limitations of local sensor algorithms in related technologies, in complex scenes such as rapid lighting changes, weak textures, high occlusion, scene structure degradation, and intense motion, there are still problems such as system performance degradation or even operation failure.

[0003] In the process of realizing the concept of the present invention, the inventors found that there are at least the following problems in the related art: due to the inherent defects and limitations of the local sensor algorithm in the related art, there is a problem of poor positioning effect in complex scenes. Summary of the invention

[0004] In view of the above problems, the present invention provides a positioning method, apparatus, device, medium and program product based on multi-state color consistency constraints.

[0005] According to a first aspect of the present invention, a positioning method based on multi-state color consistency constraints is provided, comprising:

[0006] Based on the three-dimensional position of the positioning point in the three-dimensional point cloud, an imaging simulation is performed on the positioning point to determine the imaging position of the positioning point in the current frame image, wherein the positioning point is a point determined from the three-dimensional point cloud, and the three-dimensional point cloud is obtained by collecting information about the environment carried by the laser radar carried by the positioning object; based on the projection position and imaging position of the positioning point in the current frame image, the initial object state corresponding to the current frame image is corrected to obtain a corrected object state, wherein the corrected object state includes a corrected posture of an inertial measurement tool carried by the positioning object; based on multiple historical postures of the inertial measurement tool, multiple observed colors of the positioning point, the current color of the positioning point in the three-dimensional point cloud, and color noise distribution parameters, the corrected object state is corrected to obtain a target object state, wherein the observed color represents the color of the positioning point observed in the observed posture determined from the corrected posture and multiple historical postures; based on the target object state, a target posture of the positioning object is determined.

[0007] The second aspect of the present invention provides a positioning device based on multi-state color consistency constraints, including: an imaging simulation module, which is used to perform imaging simulation on the positioning point based on the three-dimensional position of the positioning point in the three-dimensional point cloud, and determine the imaging position of the positioning point in the current frame image, wherein the positioning point is a point determined from the three-dimensional point cloud, and the three-dimensional point cloud is obtained by the laser radar carried by the positioning object to collect information about the environment; a first correction module, which is used to correct the initial object state corresponding to the current frame image based on the projection position and imaging position of the positioning point in the current frame image, and obtain the corrected object state, wherein the corrected object state includes the corrected posture of the inertial measurement tool carried by the positioning object; a second correction module, which is used to correct the corrected object state based on multiple historical postures of the inertial measurement tool, multiple observed colors of the positioning point, the current color of the positioning point in the three-dimensional point cloud, and color noise distribution parameters, and obtain the target object state, wherein the observed color represents the color of the positioning point observed under the observed posture determined from the corrected posture and multiple historical postures; a position determination module, which is used to determine the target posture of the positioning object based on the target object state.

[0008] A third aspect of the present invention provides an electronic device, comprising: one or more processors; a memory for storing one or more computer programs, wherein the one or more processors execute the one or more computer programs to implement the steps of the above method.

[0009] The fourth aspect of the present invention further provides a computer-readable storage medium on which a computer program or instruction is stored, and the steps of the above method are implemented when the above computer program or instruction is executed by a processor.

[0010] The fifth aspect of the present invention also provides a computer program product, including a computer program or instructions, which implement the steps of the above method when executed by a processor.

[0011] According to the positioning method based on multi-state color consistency constraints of the present invention, the positioning point is imaged and simulated by the three-dimensional position of the positioning point in the three-dimensional point cloud, so as to obtain the imaging position of the positioning point in the current frame image, and combined with the projection position of the positioning point in the current frame image, based on the position invariance of the same positioning point in the same frame image, the initial object state is corrected to obtain the corrected object state, and the observed color of the positioning point observed in multiple historical postures or corrected object states is determined, and combined with the current color of the positioning point in the three-dimensional point cloud, multiple historical positions, color noise distribution parameters, etc., the correction of the corrected object state is achieved based on the invariance of the color of the same positioning point in a posture sliding window composed of multiple historical postures and corrected postures, so as to obtain the target object state, and then obtain the target posture of the positioning object. By adopting the invariance of the position of the same positioning point in the same frame image and the invariance of the color of the same positioning point, multiple corrections are made to the initial object state, and the posture sliding window is taken into account in the correction process, thereby considering the constraints between multiple camera states, thereby at least partially solving the technical problem of poor positioning effect existing in the related technology, and achieving the technical effect of improving positioning robustness and accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0012] The above contents and other objects, features and advantages of the present invention will become more apparent through the following description of the embodiments of the present invention with reference to the accompanying drawings, in which:

[0013] Figure 1 An application scenario diagram of a positioning method, apparatus, device, medium, and program product based on multi-state color consistency constraints according to an embodiment of the present invention is shown.

[0014] Figure 2 A flow chart of a positioning method based on multi-state color consistency constraints according to an embodiment of the present invention is shown.

[0015] Figure 3 A data flow diagram of a positioning method based on multi-state color consistency constraints according to an embodiment of the present invention is shown.

[0016] Figure 4 A schematic diagram of implementing a positioning method based on multi-state color consistency constraints according to an embodiment of the present invention is shown.

[0017] Figure 5 A structural block diagram of a positioning device based on multi-state color consistency constraints according to an embodiment of the present invention is shown.

[0018] Figure 6 A block diagram of an electronic device suitable for implementing a positioning method based on multi-state color consistency constraints according to an embodiment of the present invention is shown. DETAILED DESCRIPTION

[0019] Below, embodiments of the present invention will be described with reference to the accompanying drawings. However, it should be understood that these descriptions are exemplary only and are not intended to limit the scope of the present invention. In the following detailed description, for ease of explanation, many specific details are set forth to provide a comprehensive understanding of embodiments of the present invention. However, it is apparent that one or more embodiments may also be implemented without these specific details. In addition, in the following description, descriptions of known structures and technologies are omitted to avoid unnecessary confusion of concepts of the present invention.

[0020] The terms used herein are only for describing specific embodiments and are not intended to limit the present invention. The terms "include", "comprises", etc. used herein indicate the existence of the features, steps, operations and / or components, but do not exclude the existence or addition of one or more other features, steps, operations or components.

[0021] All terms (including technical and scientific terms) used herein have the meanings commonly understood by those skilled in the art unless otherwise defined. It should be noted that the terms used herein should be interpreted as having a meaning consistent with the context of this specification and should not be interpreted in an idealized or overly rigid manner.

[0022] When using expressions such as "at least one of A, B, and C, etc.", they should generally be interpreted according to the meaning of the expression commonly understood by those skilled in the art (for example, "a system having at least one of A, B, and C" should include but is not limited to a system having A alone, B alone, C alone, A and B, A and C, B and C, and / or A, B, C, etc.).

[0023] It should be noted that the positioning method based on multi-state color consistency constraints of the present invention can be used in the field of computer technology, and can also be used in any field outside the field of computer technology, such as the positioning field, etc. The present invention does not limit the application field of the positioning method based on multi-state color consistency constraints.

[0024] In the technical solution of the present invention, the user information (including but not limited to user personal information, user image information, user device information, such as location information, etc.) and data (including but not limited to data used for analysis, stored data, displayed data, etc.) involved are all information and data authorized by the user or fully authorized by all parties, and the collection, storage, use, processing, transmission, provision, invention and application of the relevant data comply with relevant laws, regulations and standards, take necessary confidentiality measures, do not violate public order and good morals, and provide corresponding operation entrances for users to choose to authorize or refuse.

[0025] In the scenario of using personal information for automated decision-making, the methods, devices, and systems provided by the embodiments of the present invention provide users with corresponding operation portals for users to choose to agree or reject the automated decision-making results; if the user chooses to reject, the expert decision-making process will be entered. The expression "automated decision-making" here refers to the activity of automatically analyzing and evaluating an individual's behavioral habits, interests and hobbies, or economic, health, credit status, etc. through computer programs, and making decisions. The expression "expert decision-making" here refers to the activity of making decisions by people who specialize in a certain field, have specialized experience, knowledge and skills, and have reached a certain level of professionalism.

[0026] An embodiment of the present invention provides a positioning method based on multi-state color consistency constraints, including: based on the three-dimensional position of the positioning point in a three-dimensional point cloud, performing imaging simulation on the positioning point to determine the imaging position of the positioning point in the current frame image, wherein the positioning point is a point determined from the three-dimensional point cloud, and the three-dimensional point cloud is obtained by collecting information about the environment carried by a laser radar carried by the positioning object; based on the projection position and imaging position of the positioning point in the current frame image, correcting the initial object state corresponding to the current frame image to obtain a corrected object state, wherein the corrected object state includes a corrected posture of an inertial measurement tool carried by the positioning object; based on multiple historical postures of the inertial measurement tool, multiple observed colors of the positioning point, the current color of the positioning point in the three-dimensional point cloud, and color noise distribution parameters, correcting the corrected object state to obtain a target object state, wherein the observed color represents the color of the positioning point observed in the observed posture determined from the corrected posture and multiple historical postures; based on the target object state, determining the target posture of the positioning object.

[0027] Figure 1 The application scenario diagram of the positioning method, apparatus, device, medium and program product based on multi-state color consistency constraint according to an embodiment of the present invention is schematically shown.

[0028] like Figure 1 As shown, the application scenario 100 according to this embodiment may include a first terminal device 101, a second terminal device 102, a third terminal device 103, a network 104, and a server 105. The network 104 is used to provide a medium for a communication link between the first terminal device 101, the second terminal device 102, the third terminal device 103, and the server 105. The network 104 may include various connection types, such as wired, wireless communication links, or optical fiber cables, etc.

[0029] The user can use the first terminal device 101, the second terminal device 102, and the third terminal device 103 to interact with the server 105 through the network 104 to receive or send messages, etc. Various communication client applications can be installed on the first terminal device 101, the second terminal device 102, and the third terminal device 103, such as shopping applications, web browser applications, search applications, instant messaging tools, email clients, social platform software, etc. (only for example).

[0030] The first terminal device 101, the second terminal device 102, and the third terminal device 103 may be various electronic devices having display screens and supporting web browsing, including but not limited to smart phones, tablet computers, laptop computers, desktop computers, and the like.

[0031] The server 105 may be a server that provides various services, such as a background management server (only as an example) that provides support for websites browsed by users using the first terminal device 101, the second terminal device 102, and the third terminal device 103. The background management server may analyze and process the received data such as user requests, and feed back the processing results (such as web pages, information, or data obtained or generated according to user requests) to the terminal device.

[0032] It should be noted that the positioning method based on multi-state color consistency constraints provided in the embodiment of the present invention can generally be executed by the server 105. Accordingly, the positioning device based on multi-state color consistency constraints provided in the embodiment of the present invention can generally be set in the server 105. The positioning method based on multi-state color consistency constraints provided in the embodiment of the present invention can also be executed by a server or server cluster that is different from the server 105 and can communicate with the first terminal device 101, the second terminal device 102, the third terminal device 103 and / or the server 105. Accordingly, the positioning device based on multi-state color consistency constraints provided in the embodiment of the present invention can also be set in a server or server cluster that is different from the server 105 and can communicate with the first terminal device 101, the second terminal device 102, the third terminal device 103 and / or the server 105.

[0033] It should be understood that Figure 1 The number of terminal devices, networks and servers in the embodiment is only for illustration. Any number of terminal devices, networks and servers may be provided according to the implementation requirements.

[0034] The following will be based on Figure 1 The scene described by Figure 2~Figure 4 The positioning method based on multi-state color consistency constraint according to an embodiment of the present invention is described in detail.

[0035] Figure 2A flow chart of a positioning method based on multi-state color consistency constraints according to an embodiment of the present invention is shown.

[0036] like Figure 2 As shown, the method includes operations S210 to S240.

[0037] In operation S210, based on the three-dimensional position of the positioning point in the three-dimensional point cloud, an imaging simulation is performed on the positioning point to determine the imaging position of the positioning point in the current frame image, wherein the positioning point is a point determined from the three-dimensional point cloud, and the three-dimensional point cloud is obtained by collecting information about the environment carried by a laser radar carried by the positioning object.

[0038] In operation S220, based on the projection position and imaging position of the positioning point in the current frame image, the initial object state corresponding to the current frame image is corrected to obtain a corrected object state, wherein the corrected object state includes a corrected posture of an inertial measurement tool carried by the positioning object.

[0039] In operation S230, based on multiple historical poses of the inertial measurement tool, multiple observed colors of the positioning point, the current color of the positioning point in the three-dimensional point cloud, and color noise distribution parameters, the corrected object state is corrected to obtain the target object state, wherein the observed color represents the color of the positioning point observed in the observed pose determined from the corrected pose and multiple historical poses.

[0040] In operation S240 , a target pose of the positioning object is determined based on the state of the target object.

[0041] According to an embodiment of the present invention, the projection position may be obtained by projecting a three-dimensional positioning point onto a two-dimensional current frame image.

[0042] According to an embodiment of the present invention, there may be one or more positioning points, which may be determined according to actual conditions.

[0043] According to the embodiment of the present invention, the specific implementation of the inertial measurement tool is not limited, and it may be an inertial measurement unit (Inertial Measurement Unit, IMU).

[0044] According to an embodiment of the present invention, the initial object state is the object state closest to the acquisition time of the current frame image, and the initial object state is determined by combining the historical target object state corresponding to the previous frame image with the acceleration and velocity measured by the IMU.

[0045] According to the embodiment of the present invention, the positioning object is not limited and can be any movable object, such as a robot, a drone, etc.

[0046] According to an embodiment of the present invention, the three-dimensional point cloud may be obtained through a global voxel map in a LiDAR-Inertial Odometry (LIO), or may be obtained by any laser radar. The three-dimensional point cloud may include multiple positioning points, and each positioning point may include position information and color information, as defined in the following formula (1).

[0047] ; (1)

[0048] Among them, the subvector Indicates the position of the anchor point in the world coordinate system. , , are the x, y, and z coordinates of the positioning point in the world coordinate system, Represents the three-dimensional coordinates of the positioning point in the world coordinate system, Characterize the color of the positioning point, ,in, They are the values ​​of three independent channels: red, green, and blue. Represents a set of n-dimensional vectors, n=3, 6, etc.

[0049] According to an embodiment of the present invention, a map management module for managing a global voxel map may be included, wherein a map management module for managing a global voxel map is recorded. and Covariance matrix of the estimation error and , and the timestamp when the point was created and rendered.

[0050] According to an embodiment of the present invention, in order to efficiently query map points, all points can be stored in fixed-size voxels. If a voxel has a point added within a target time period, it is marked as activated, and the target time period can be 1S, otherwise it is inactivated.

[0051] According to an embodiment of the present invention, the color of each point in the three-dimensional point cloud may be determined by an image captured in each frame.

[0052] According to an embodiment of the present invention, the radar-inertial odometer can add points in the current radar data frame to the global voxel map. If there are neighbors with a fixed resolution distance, they will be discarded to maintain the fixed spatial resolution. Otherwise, a new voxel structure will be created in the global voxel map. , as shown in formula (2):

[0053] ; (2)

[0054] Among them, the color vector is set to zero, initialized when it is first observed in the current frame, and will contain The voxels are marked as activated.

[0055] According to an embodiment of the present invention, the positioning method based on multi-state color consistency constraints of the present invention can be used in Visual-Inertial Odometry (VIO), so as to conveniently determine the position and pose of the positioning object by using VIO and LIO.

[0056] According to an embodiment of the present invention, the positioning point may be a tracking point continuously determined in a plurality of consecutive frames, and the method for determining the positioning point is not limited. For example, the positioning point in the current frame image may be determined based on the positioning point in the previous frame image by an optical flow method. Specifically, assuming that the previous frame image There are Tracked positions , and they are The projection on , use the optical flow method to find them in the current frame image The position in is represented by .

[0057] According to an embodiment of the present invention, the manner of performing imaging simulation on the positioning point is not limited and may be implemented through a preset imaging model. By performing imaging simulation on the positioning point, it is possible to simulate the position where the positioning point may exist in the current frame image.

[0058] According to an embodiment of the present invention, the initial system state It may include information characterizing the system state, such as the initial position of the inertial measurement tool, the external parameters between the inertial measurement tool and the camera, and the bias of the inertial measurement tool. Specifically, it may be shown in the following formulas (3) to (6).

[0059] ; (3)

[0060] ; (4)

[0061] ; (5)

[0062] ; (6)

[0063] Among them, the position and posture of the inertial measurement unit includes , when the kth frame image is the current frame, it represents the initial pose, Represents the initial rotation matrix and initial position of the IMU relative to the world coordinate system corresponding to the k-th frame image, is the velocity of the IMU corresponding to the k-th frame in the world frame, represents the coordinates of the i-th positioning point corresponding to the k-th frame image in the world coordinate system, and Respectively represent the bias of the accelerometer and gyroscope in the IMU, represents the external parameters between the camera and IMU corresponding to the k-th frame image, , Respectively represent the rotation and translation vectors of the camera corresponding to the k-th frame image relative to the IMU.

[0064] According to an embodiment of the present invention, multiple observation colors can be used to determine the position of the positioning point in the current frame image under multiple IMU posture states such as different historical postures, corrected postures, etc., and determine the observation color using the position and the color corresponding to the position.

[0065] According to an embodiment of the present invention, the color noise distribution parameters are not limited and may include a noise covariance matrix including at least one noise corresponding to the current frame image, and may also include a covariance matrix of at least one color noise corresponding to the generation time of multiple historical postures. The covariance matrix is ​​used to determine the variance of the distribution to which the color noise belongs.

[0066] According to an embodiment of the present invention, the target object state includes a corrected corrected posture and a corrected historical posture, and the corrected corrected posture can be used as the target posture of the positioning object.

[0067] According to the positioning method based on multi-state color consistency constraint of the present invention, the positioning point is imaged and simulated by the three-dimensional position of the positioning point in the three-dimensional point cloud, so as to obtain the imaging position of the positioning point in the current frame image, and the initial object state is corrected based on the position invariance of the same positioning point in the same frame image in combination with the projection position of the positioning point in the current frame image, so as to obtain the corrected object state, and determine the observed color of the positioning point observed in multiple historical postures or corrected object states, and consider the current color of the positioning point in the three-dimensional point cloud, multiple historical positions, color noise distribution parameters, etc., and consider the position sliding window composed of multiple historical postures and corrected postures, so as to achieve the correction of the corrected object state based on the invariance of the color of the same positioning point, so as to obtain the target object state, and then obtain the target posture of the positioning object. Since the initial object state is multiple-corrected by adopting the invariance of the position of the same positioning point in the same frame image and the invariance of the color of the same positioning point, and the position sliding window is considered in the correction process, so as to realize the consideration of the constraints between multiple camera states, at least partially solve the problem of poor positioning effect existing in the related art, and achieve the effect of improving the positioning robustness and accuracy.

[0068] According to an embodiment of the present invention, multiple observation colors are determined in the following manner: based on multiple historical postures, corrected postures and the three-dimensional positions of the positioning point in the three-dimensional point cloud, multiple observation positions of the positioning point in the current frame image are determined; for each observation position, the observation color of the positioning point is determined based on the first color of the observation position and the second color of the adjacent area of ​​the observation position in the current frame image.

[0069] According to an embodiment of the present invention, the observation position of the positioning point in the current frame image that may be observed under the historical posture can be determined through the historical posture and the three-dimensional position.

[0070] According to an embodiment of the present invention, by correcting the posture and the three-dimensional posture, the observation position of the current frame image that may be observed by the positioning point in the current corrected posture can be determined.

[0071] According to an embodiment of the present invention, the first color may be a color displayed at the observation position in the current frame image.

[0072] According to an embodiment of the present invention, the adjacent area is not limited and can be determined based on a preset adjacent range, and the second color can be determined based on the colors of adjacent positions included in the adjacent area. For example, when the preset adjacent range is 6 adjacent points adjacent to the positioning point, the 6 adjacent positions closest to the observation position can be determined, and the color of each adjacent position can be determined. The second color is determined by performing correlation calculations on the colors of the 6 adjacent positions.

[0073] According to the embodiment of the present invention, there is no limitation on the related calculation, which may be addition and averaging, addition, weighted averaging, etc.

[0074] According to an embodiment of the present invention, by determining the observation position and determining the observation color by the color of the observation position and the color of the neighborhood of the observation position, it is possible to improve the accuracy of the observed color, thereby making the subsequently determined photometric error value more accurate, thereby determining a more accurate state of the target object.

[0075] According to an embodiment of the present invention, the historical posture includes a first historical rotation matrix and a first historical position; the corrected posture includes a corrected rotation matrix and a corrected position; based on multiple historical postures, corrected postures and the three-dimensional positions of the positioning point in the three-dimensional point cloud, determining multiple observation positions of the positioning point in the current frame image may include the following operations.

[0076] For each historical posture, based on the first historical rotation matrix, the first historical position and the three-dimensional position, determine the first imaging three-dimensional position of the positioning point in the camera coordinate system, wherein the camera coordinate system is a coordinate system based on the camera carried by the positioning object, and the camera is a tool for acquiring the current frame image; the first imaging three-dimensional position and the historical posture are input into a preset imaging model to obtain the first imaging two-dimensional position of the positioning point in the current frame image; for the corrected posture, based on the corrected rotation matrix, the corrected position and the three-dimensional position, determine the second imaging three-dimensional position of the positioning point; input the second imaging three-dimensional position and the corrected posture into the preset imaging model to obtain the second imaging two-dimensional position of the positioning point in the current frame image; based on multiple first imaging two-dimensional positions and second imaging two-dimensional positions, determine multiple observation positions.

[0077] According to an embodiment of the present invention, when it is believed that the positioning point can be observed in the historical posture, the first imaging three-dimensional position of the positioning point in the camera coordinate system is determined, and the first imaging three-dimensional position and the historical posture are input into the preset imaging model to realize the mapping of the positioning point in the current frame image, thereby realizing the imaging simulation of the positioning point in the historical posture.

[0078] According to an embodiment of the present invention, since there are multiple historical postures, each historical posture may correspond to a first imaging two-dimensional position.

[0079] According to an embodiment of the present invention, when it is believed that the positioning point can be observed under the corrected posture, the second imaging three-dimensional position of the positioning point in the camera coordinate system is determined, and the second imaging three-dimensional position and the corrected posture are input into the preset imaging model, thereby realizing the imaging simulation of the positioning point in the corrected posture.

[0080] According to an embodiment of the present invention, determining a first imaging three-dimensional position of a positioning point in a camera coordinate system based on a first historical rotation matrix, a first historical position, and a three-dimensional position may include the following operations.

[0081] Based on the first historical rotation matrix and the historical external parameters corresponding to the historical posture, the second historical rotation matrix of the camera relative to the inertial measurement tool is determined, wherein the historical external parameters are external parameters between the camera and the inertial measurement tool; based on the first historical position and the historical external parameters, the second historical position of the camera relative to the inertial measurement tool is determined; based on the first historical rotation matrix, the first historical position, the second historical rotation matrix, the second historical position and the three-dimensional position, the first imaging three-dimensional position is determined.

[0082] According to an embodiment of the present invention, the historical external parameters may correspond to the historical posture and represent the external parameters between the camera and the inertial measurement tool obtained at the historical moment or the most recent moment of the historical posture.

[0083] According to the embodiment of the present invention, the specific implementation method for determining the first imaging three-dimensional position is not limited, and may be as shown in the following formula (7).

[0084] ; (7)

[0085] Among them, k represents the number of frames of the image obtained, T represents the inverse of the matrix, I represents IMU, G is the world coordinate system, C is the camera, R is the rotation matrix, and p is the position. Represents the first imaging 3D position of the sth positioning point corresponding to the ith historical pose in the camera coordinate system, Represents the second historical rotation matrix relative to the inertial measurement tool corresponding to the k-th frame image and the i-th historical pose, The first historical rotation matrix representing the i-th historical pose, represents the three-dimensional position of the sth positioning point, represents the first historical position of the i-th historical pose, Represents the second historical position of the camera corresponding to the k-th frame image and the i-th historical pose relative to the inertial measurement tool.

[0086] According to an embodiment of the present invention, the second imaging three-dimensional position can be determined in the following manner: based on the corrected rotation matrix and the corrected external parameters included in the modified rotation matrix, determining the relative rotation matrix of the camera relative to the inertial measurement tool, wherein the corrected external parameters are external parameters between the camera and the inertial measurement tool; based on the corrected position and the corrected external parameters, determining the relative position of the camera relative to the inertial measurement tool; based on the corrected rotation matrix, the relative rotation matrix, the corrected position, the relative position and the three-dimensional position, determining the second imaging three-dimensional position.

[0087] According to the embodiment of the present invention, the specific implementation method for determining the second imaging three-dimensional position is not limited, and may be as shown in the following formula (8).

[0088] ; (8)

[0089] in, Represents the second imaging three-dimensional position of the s-th positioning point in the camera coordinate system, Characterizes the relative rotation matrix of the camera corresponding to the k-th frame image relative to the inertial measurement tool, Characterize the corrected rotation matrix, Characterize the corrected position, Characterizes the relative position of the camera corresponding to the k-th frame image relative to the inertial measurement tool.

[0090] According to the embodiment of the present invention, the preset imaging model is not limited and may be a pinhole imaging model or the like. The preset imaging model is used to simulate the imaging process of a camera.

[0091] According to an embodiment of the present invention, the observation position is obtained by using a preset imaging model. In some embodiments, it can be as shown in the following formula (9).

[0092] ; (9)

[0093] According to an embodiment of the present invention, For the preset imaging model, Represents the three-dimensional position of the positioning point in the camera coordinate system, It can be a fused object state, which includes multiple historical poses and corrected poses. In the process of determining the observation position, the corresponding historical pose or corrected pose can be selected from the fused object state for calculation.

[0094] According to an embodiment of the present invention, by determining the rotation matrix and position of the camera relative to the IMU, and combining it with the historical pose or the current pose to determine the position of the positioning point in the camera coordinate system under different poses, and using this position to perform imaging simulation, it is possible to determine multiple possible positions of the positioning point, thereby improving the possibility of the position of the positioning point in the current frame image.

[0095] According to an embodiment of the present invention, based on multiple historical postures of the inertial measurement tool, multiple observed colors of the positioning point, the current color of the positioning point in the three-dimensional point cloud and the color noise distribution parameters, the correction object state is corrected to obtain the target object state, which may include the following operations.

[0096] Based on multiple historical postures, multiple observed colors of each positioning point, the current color of the positioning point in the three-dimensional point cloud, color noise distribution parameters, the estimated object state of the inertial measurement tool and the corrected object state, a first state error determination function is constructed, wherein the color noise distribution parameters include at least one representation parameter of a statistical distribution conforming to the color noise, and the estimated object state is an object state estimated based on the measurement data of the inertial measurement tool; the first state error determination function is solved to obtain the target object state.

[0097] According to an embodiment of the present invention, constructing a first state error determination function based on multiple observed colors of each positioning point, the current color of the positioning point in the three-dimensional point cloud, color noise distribution parameters and the corrected object state may include the following operations.

[0098] A plurality of historical postures and a corrected object state are fused to obtain a fused object state; a plurality of photometric error values ​​are determined based on a plurality of observed colors and a current color; a first state error determination function for the fused object state is constructed based on the plurality of photometric error values, photometric error correction parameters and color noise distribution parameters, the estimated object state and the fused object state, wherein the goal of the first state error determination function is to minimize a target error of the fused object state, and the target error includes an error of the corrected object state and errors of a plurality of historical postures.

[0099] According to an embodiment of the present invention, the specific method of fusing multiple historical postures and the corrected object state is not limited, and the two may be spliced ​​together, which may be specifically as shown in the following formula (10).

[0100] ; (10)

[0101] in, is the fusion object state, Including d historical poses , It can be a parameter included in the state of the corrected object, and its specific expansion is the same as formula (3) to formula (6).

[0102] According to an embodiment of the present invention, there is no limitation on the method for determining the photometric error value. The current color and multiple observed colors may be subtracted from each other to obtain multiple photometric error values. Specifically, the photometric error values ​​may be obtained by the following formula (11).

[0103] ; (11)

[0104] in, is the photometric error value of the sth positioning point, which is an estimated value. is the current color of the sth positioning point in the 3D point cloud, is the observed color of the sth positioning point.

[0105] According to an embodiment of the present invention, the photometric error correction parameter may be determined based on the historical posture, the corrected posture and the three-dimensional position of the positioning point in the three-dimensional point cloud. The photometric error correction parameter may be predetermined and stored in a database, and the photometric error correction parameter corresponding to each positioning point may include a historical error sub-parameter corresponding to each of the multiple historical postures and a current error sub-parameter corresponding to the corrected posture. The historical error sub-parameter is determined by the three-dimensional position and historical posture of the positioning point, and the error sub-parameter is determined by the corrected posture and the three-dimensional position of the positioning point.

[0106] According to an embodiment of the present invention, the first state error determination function can be obtained by combining the observations of the corrected posture and multiple historical postures for the positioning point. Therefore, the target error includes the error of the corrected object state and the errors of multiple historical postures. Therefore, in the process of solving the first state error determination function to obtain the error of the corrected object state, it will be affected by the errors of multiple historical postures, thereby obtaining a more accurate error of the corrected object state.

[0107] According to an embodiment of the present invention, the first state error determination function can be obtained by the following formulas (12) to (18).

[0108] ; (12)

[0109] ; (13)

[0110] in, , is the preset true value of an observed color at the Sth positioning point, is the preset true value of the current color of the s-th positioning point in the 3D point cloud, is the color observation noise of the Sth positioning point, is the temporal rendering estimation error of the Sth positioning point, To consider the random walk process noise of ambient light changes, are the representation parameters of the statistical distributions to which the above noises belong, including the color noise distribution parameters. Represents the interval between the current frame image and the last 3D point cloud rendering time.

[0111] According to an embodiment of the present invention, formula (11), formula (12), and formula (13) are combined to obtain an error determination function corresponding to any one of the multiple historical postures and corrected postures and the positioning point. A first-order Taylor expansion is performed on the function to obtain formulas (14) to (15).

[0112] ; (14)

[0113] in,

[0114] ; (15)

[0115] in, , and The photometric error determination function corresponding to the k-th frame image and the s-th positioning point is and The Jacobian matrix of is the remainder of the first-order Taylor expansion, is the true value of the photometric error, is the object state truth value, is the true value of the three-dimensional coordinate of the sth positioning point in the world coordinate system, is the three-dimensional coordinate of the Sth positioning point relative to the world coordinate system, which is an estimated value, is the true color value of the sth map point, is the target error.

[0116] According to an embodiment of the present invention, the first-order Taylor expansion of the error determination function of each of the multiple historical postures and the corrected postures, i.e., the above formula (14), is merged to realize the posture sliding window obtained by using the multiple historical postures and the corrected postures; and the merged function is mapped to the null space, wherein for each positioning point, the merged formula can be as shown in formula (16), and the formula mapped to the null space can be as shown in formula (18), and formula (17) schematically shows the formula of the sub-error in the target error.

[0117] ; (16)

[0118] in,

[0119] ; (17)

[0120] ; (18)

[0121] in, Represents the photometric error value corresponding to the s-th positioning point, The photometric error value representing the sth positioning point corresponding to the kth frame image can be considered as the photometric error value determined by the corrected posture, that is, the current photometric error value. Represents the photometric error value corresponding to the i-th historical pose, i=1, 2, ... N, After null space projection , are the photometric error determination functions corresponding to the i-th historical pose. The Jacobian matrix of as well as The target error The parameters included are: Represents the error of the corrected rotation matrix corresponding to the k-th frame image in the world coordinate system, Represents the error of the corrected position corresponding to the k-th frame image in the world coordinate system, Represents the error of the first historical rotation matrix corresponding to the i-th historical pose in the world coordinate system, Represents the error of the first historical position corresponding to the i-th historical pose in the world coordinate system, Characterizes the error of the rotation matrix of the camera corresponding to the k-th frame image relative to the inertial measurement tool, Characterizes the error of the position of the camera corresponding to the k-th frame image relative to the inertial measurement tool, Characterizes the error of the 3D position of the s-th map point in the 3D point cloud relative to the world coordinate system.

[0122] .

[0123] in, For characterization or , o=k or i, For characterization or , Characterize the null space projection, represents the null space projection , Characterize the null space after projection .

[0124] According to an embodiment of the present invention, the position uncertainty of the positioning point can be incorporated into the determination of the target error through the null space projection.

[0125] According to an embodiment of the present invention, the photometric error correction parameters may include and .

[0126] According to an embodiment of the present invention, based on the above formula (18) combined with the prior distribution from the IMU forward propagation, a maximum a posteriori estimate of the target error can be obtained. In some embodiments, the first state error determination function can be as shown in the following formula (19).

[0127] ; (19)

[0128] in, To estimate the state of the object from The tangent space is projected onto The Jacobian matrix of the tangent space, is the estimated object state of the inertial measurement tool, s represents the sth positioning point, s=1, 2..., m, Characterizes the error in estimating the object state, The representation is subtracted in each iteration.

[0129] According to an embodiment of the present invention, in the process of solving the first state error determination function, it can be sorted out, as shown in the following formulas (20) to (21). In some embodiments, a first state error determination function of the form of formula (20) can be generated.

[0130] ; (20)

[0131] in,

[0132] ;(twenty one)

[0133] Among them, I is the identity matrix, -1 is the reciprocal of the matrix, and T is the inverse of the matrix.

[0134] According to an embodiment of the present invention, formula (19) and formula (20) can be combined with the following formula (22) to implement an iterative solution to the target error.

[0135] ;(twenty two)

[0136] According to an embodiment of the present invention, The addition operation is performed in each iteration, and formula (22) represents that the intermediate object state obtained in the previous iteration is corrected by the target error, thereby obtaining the intermediate object state of this iteration. In some embodiments, the intermediate object state of this iteration is input into the first state error determination function, replacing the intermediate object state obtained in the previous iteration in the first state error determination function, thereby obtaining the first state error determination function containing the intermediate object state of this iteration.

[0137] According to the embodiment of the present invention, there is no limitation on the solution method of the first state error determination function. In some embodiments, an iterative Kalman filter may be used for the solution.

[0138] According to an embodiment of the present invention, solving the first state error determination function to obtain the target object state may include the following operations.

[0139] A first state error determination function including the i-1th intermediate object state is solved to obtain the i-th target error, wherein i is a positive integer greater than or equal to 1, and when i=1, the i-1th intermediate object state is a fused object state; error correction is performed on the i-1th intermediate object state based on the i-th target error to obtain the i-th intermediate object state; when it is determined that the i-th target error does not fall within the preset error range, the i-1th intermediate object state is replaced by the i-th intermediate object state to obtain a first state error determination function including the i-th intermediate object state; the above operations are repeated until the first state error falls within the preset error range; when it is determined that the i-th target error falls within the preset error range, the i-th intermediate object state is the target object state.

[0140] According to the embodiment of the present invention, the preset error range is not limited and can be determined according to actual conditions.

[0141] According to an embodiment of the present invention, in the process of solving the first state error determination function, if the output first state error does not fall within the preset error range, the first state error output each time can be corrected for the intermediate object state determined last time, so as to obtain the current intermediate object state, and the first state error determination function determined by the current intermediate object state can be used to repeatedly solve until the first state error data falls within the preset error range.

[0142] According to the embodiment of the present invention, the first state error is iteratively determined and the object state is continuously corrected, so that the first state error gradually decreases, thereby achieving a more accurate correction of the corrected object state.

[0143] According to an embodiment of the present invention, based on the projection position and imaging position of the positioning point in the current frame image, the initial object state corresponding to the current frame image is corrected to obtain the corrected object state, which may include the following operations.

[0144] A reprojection error value is determined based on the projection position and the imaging position; a second state error determination function is determined based on the reprojection error value, the reprojection error correction parameter, the position error distribution parameter corresponding to the current frame image, the estimated object state of the inertial measurement tool, and the initial object state, wherein the goal of the second state error determination function is to minimize the error of the initial object state, and the position error distribution parameter includes at least one representation parameter of a statistical distribution that the position error conforms to; the second state error determination function is solved to obtain a corrected object state.

[0145] According to an embodiment of the present invention, there is no limitation on the position error distribution parameters, which may include at least one covariance matrix of the position error corresponding to the acquisition time of the current frame image. The covariance matrix may be used to determine the variance of the distribution to which the position error belongs. The position error may be a pixel tracking error and a three-dimensional position error of the positioning point.

[0146] According to an embodiment of the present invention, the color noise distribution parameters and the position error distribution parameters may be obtained according to respective preset covariance update functions.

[0147] According to an embodiment of the present invention, the reprojection error correction parameter may be obtained by locating the three-dimensional position and initial posture of the point in the three-dimensional point cloud.

[0148] According to an embodiment of the present invention, the estimated object state may be obtained by combining the target object state corresponding to the previous frame image with real-time measurement data of an inertial measurement tool, such as velocity and acceleration.

[0149] According to an embodiment of the present invention, the imaging position can be obtained by determining the three-dimensional position of the positioning point in the camera coordinate system, and inputting the three-dimensional position and the initial object state into a preset imaging model. The three-dimensional position in the camera coordinate system can be determined by a formula as shown in formula (8), and the corrected posture is replaced by the initial posture.

[0150] According to an embodiment of the present invention, the reprojection error value It can be obtained by the following formula (23).

[0151] ;(twenty three)

[0152] in, The projection position of the sth positioning point corresponding to the kth frame camera, Represents the imaging position of the sth positioning point corresponding to the kth frame camera, is the initial object state and is an estimated value.

[0153] According to an embodiment of the present invention, the second state error determination function can be obtained by the following formulas (24) to (30).

[0154] ;(twenty four)

[0155] ; (25)

[0156] in, , and They are and The preset truth value parameter of , are position errors, Pixel tracking error, The three-dimensional position error of the positioning point.

[0157] According to an embodiment of the present invention, formula (23) to formula (25) are combined to determine a reprojection error determination function, and a first-order Taylor expansion is performed on the reprojection error determination function to obtain formula (26) to formula (27).

[0158] ; (26)

[0159] in,

[0160] ; (27)

[0161] in, is the reprojection error value, which is an estimated value, is the true value of the reprojection error, is the truth value of the initial object state, , Determine the remainder of the Taylor expansion corresponding to the reprojection error function corresponding to the s-th positioning point, and The reprojection error corresponding to the sth positioning point is determined by and The Jacobian matrix of is the initial error of the initial object state, is the reprojection error correction parameter.

[0162] According to an embodiment of the present invention, by combining formula (27) with the prior distribution from the IMU forward propagation, a maximum a posteriori estimate of the initial error can be obtained, and it is used as the second state error determination function, which is specifically shown in formula (28).

[0163] ; (28)

[0164] in, is the initial error, It is the estimated object state and the state forward propagated by IMU. is the propagated covariance, is to change the error state from The tangent space is projected onto The Jacobian matrix of the tangent space.

[0165] According to an embodiment of the present invention, the forward propagation process is as follows: assuming that the current state, i.e., the initial system state and the covariance estimate Having been corrected by the k-th frame image measurement, the system propagates the current state through the discrete-time kinematic model until the k+1-th frame image measurement is received.

[0166] According to an embodiment of the present invention, before solving the second state error determination function, it can be sorted out first to obtain formulas (29) to (30). In some embodiments, a second state error determination function such as formula (30) can also be constructed.

[0167] ; (29)

[0168] in,

[0169] ; (30)

[0170] According to an embodiment of the present invention, formula (28) and formula (30) can be combined with the following formula (31) to implement an iterative solution to the target error.

[0171] ; (31)

[0172] According to an embodiment of the present invention, formula (31) represents that the initial object state obtained in the previous iteration process is corrected by the initial error obtained in the current iteration process to obtain the initial object state in the current iteration process. In some embodiments, the initial object state of this iteration is input into the second state error determination function, and the initial object state obtained in the previous iteration process in the second state error determination function is replaced, so as to obtain the first state error determination function including the initial object state of this iteration, and repeat the above-mentioned solution process of the second state error determination function until the initial error is less than the preset initial error, and the initial object state obtained in the current iteration process is considered to be the corrected object state.

[0173] According to an embodiment of the present invention, in the process of solving the second state error determination function, an iterative Kalman filter may be used for solving.

[0174] According to an embodiment of the present invention, the reprojection error can be determined by the position of the same positioning point in the current frame image obtained at different angles, and a second state error determination function can be constructed, so that the initial object state can be iteratively corrected by iteratively solving the function, thereby achieving a rough correction of the initial object state, and minimizing the photometric error of the same positioning point observed by multiple camera frames for the corrected object state obtained after the above-mentioned inter-frame reprojection error update, so as to further reduce the state error, achieve a more refined object state correction, and obtain a more accurate target object state.

[0175] According to an embodiment of the present invention, the three-dimensional position of the positioning point in the three-dimensional point cloud, the first historical position in the historical pose, the first historical rotation matrix, the corrected position in the corrected pose, the corrected rotation matrix, the initial position in the initial pose, and the initial rotation matrix mentioned in the above method are all information relative to the world coordinate system.

[0176] Figure 3 A data flow diagram of a positioning method based on multi-state color consistency constraints according to an embodiment of the present invention is shown.

[0177] like Figure 3 As shown, the imaging position 302 of the positioning point in the current frame image is determined by the three-dimensional position 301 of the positioning point in the three-dimensional point cloud, and the positioning point is projected in the current frame image to obtain the projection position 303 of the positioning point in the current frame image. The initial object state 304 is corrected by the imaging position 302 and the projection position 303 to obtain the corrected object state 305.

[0178] According to an embodiment of the present invention, multiple observed colors 307 of the positioning point are determined by correcting the corrected posture included in the corrected object state 305 and multiple historical postures 306 of the inertial measurement unit. The corrected object state is corrected by the multiple observed colors 307, the current color 308 of the positioning point in the three-dimensional point cloud, the color noise distribution parameter 309, and the multiple historical postures 306 to obtain the target object state 310.

[0179] Figure 4 A schematic diagram of implementing a positioning method based on multi-state color consistency constraints according to an embodiment of the present invention is shown.

[0180] like Figure 4 As shown, the three-dimensional point cloud can be a global voxel map, which is constructed by a radar-inertial odometry. The positioning method based on multi-state color consistency constraints of the present invention can be constructed as a visual-inertial odometry, and interact with the radar-inertial odometry to transmit positioning point information or color information, etc. The visual-inertial odometry of the present invention can include at least two parts, respectively using inter-frame reprojection errors to roughly estimate the system state and using photometric errors to accurately estimate the system state, and finally output the position and posture of the positioned object.

[0181] According to an embodiment of the present invention, the laser-inertial odometry incrementally builds the environment geometry by adding the information collected by the laser radar to the global map. The visual-inertial odometry uses the color information of the input image to perform pose estimation and render the global voxel map.

[0182] Based on the above positioning method based on multi-state color consistency constraints, the present invention also provides a positioning device based on multi-state color consistency constraints. Figure 5The device is described in detail.

[0183] Figure 5 A structural block diagram of a positioning device based on multi-state color consistency constraints according to an embodiment of the present invention is shown.

[0184] like Figure 5 As shown, the positioning device 500 based on multi-state color consistency constraints includes an imaging simulation module 510 , a first correction module 520 , a second correction module 530 and a position determination module 540 .

[0185] The imaging simulation module 510 is used to perform imaging simulation on the positioning point based on the three-dimensional position of the positioning point in the three-dimensional point cloud, and determine the imaging position of the positioning point in the current frame image, wherein the positioning point is a point determined from the three-dimensional point cloud, and the three-dimensional point cloud is obtained by the laser radar carried by the positioning object to collect information about the environment in which it is located.

[0186] The first correction module 520 is used to correct the initial object state corresponding to the current frame image based on the projection position and imaging position of the positioning point in the current frame image to obtain a corrected object state, wherein the corrected object state includes a corrected posture of the inertial measurement tool carried by the positioning object.

[0187] The second correction module 530 is used to correct the state of the correction object based on multiple historical postures of the inertial measurement tool, multiple observed colors of the positioning point, the current color of the positioning point in the three-dimensional point cloud and the color noise distribution parameter to obtain the target object state, wherein the observed color represents the color of the positioning point observed in the observed posture determined from the correction posture and multiple historical postures.

[0188] The position determination module 540 is used to determine the target position of the positioning object based on the state of the target object.

[0189] According to an embodiment of the present invention, any multiple modules of the imaging simulation module 510, the first correction module 520, the second correction module 530 and the position determination module 540 can be combined into one module for implementation, or any one of the modules can be split into multiple modules. Alternatively, at least part of the functions of one or more of these modules can be combined with at least part of the functions of other modules and implemented in one module. According to an embodiment of the present invention, at least one of the imaging simulation module 510, the first correction module 520, the second correction module 530 and the position determination module 540 can be at least partially implemented as a hardware circuit, such as a field programmable gate array (FPGA), a programmable logic array (PLA), a system on a chip, a system on a substrate, a system on a package, an application specific integrated circuit (ASIC), or can be implemented by hardware or firmware such as any other reasonable way of integrating or packaging the circuit, or implemented in any one of the three implementation methods of software, hardware and firmware or in a suitable combination of any of them. Alternatively, at least one of the imaging simulation module 510, the first correction module 520, the second correction module 530 and the position determination module 540 may be at least partially implemented as a computer program module, and when the computer program module is executed, a corresponding function may be performed.

[0190] Figure 6 A block diagram of an electronic device suitable for implementing a positioning method based on multi-state color consistency constraints according to an embodiment of the present invention is shown.

[0191] like Figure 6 As shown, the electronic device 600 according to an embodiment of the present invention includes a processor 601, which can perform various appropriate actions and processes according to a program stored in a read-only memory (ROM) 602 or a program loaded from a storage part 608 to a random access memory (RAM) 603. The processor 601 may include, for example, a general-purpose microprocessor (e.g., a CPU), an instruction set processor and / or a related chipset and / or a special-purpose microprocessor (e.g., an application-specific integrated circuit (ASIC)), etc. The processor 601 may also include an onboard memory for caching purposes. The processor 601 may include a single processing unit or multiple processing units for performing different actions of the method flow according to an embodiment of the present invention.

[0192] In RAM 603, various programs and data required for the operation of electronic device 600 are stored. Processor 601, ROM 602 and RAM 603 are connected to each other via bus 604. Processor 601 performs various operations of the method flow according to the embodiment of the present invention by executing the program in ROM 602 and / or RAM 603. It should be noted that the program can also be stored in one or more memories other than ROM 602 and RAM 603. Processor 601 can also perform various operations of the method flow according to the embodiment of the present invention by executing the program stored in the one or more memories.

[0193] According to an embodiment of the present invention, the electronic device 600 may further include an input / output (I / O) interface 605, which is also connected to the bus 604. The electronic device 600 may further include one or more of the following components connected to the input / output (I / O) interface 605: an input portion 606 including a keyboard, a mouse, etc.; an output portion 607 including a cathode ray tube (CRT), a liquid crystal display (LCD), etc., and a speaker, etc.; a storage portion 608 including a hard disk, etc.; and a communication portion 609 including a network interface card such as a LAN card, a modem, etc. The communication portion 609 performs communication processing via a network such as the Internet. A drive 610 is also connected to the input / output (I / O) interface 605 as needed. A removable medium 611, such as a magnetic disk, an optical disk, a magneto-optical disk, a semiconductor memory, etc., is installed on the drive 610 as needed, so that a computer program read therefrom is installed into the storage portion 608 as needed.

[0194] The present invention also provides a computer-readable storage medium, which may be included in the device / apparatus / system described in the above embodiment; or may exist independently without being assembled into the device / apparatus / system. The above computer-readable storage medium carries one or more programs, and when the above one or more programs are executed, the method according to the embodiment of the present invention is implemented.

[0195] According to an embodiment of the present invention, the computer-readable storage medium may be a non-volatile computer-readable storage medium, for example, it may include but is not limited to: a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination thereof. In the present invention, the computer-readable storage medium may be any tangible medium containing or storing a program, which may be used by or in combination with an instruction execution system, an apparatus or a device. For example, according to an embodiment of the present invention, the computer-readable storage medium may include the ROM 602 and / or RAM 603 described above and / or one or more memories other than ROM 602 and RAM 603.

[0196] The embodiment of the present invention also includes a computer program product, which includes a computer program, and the computer program contains program code for executing the method shown in the flowchart. When the computer program product is run in a computer system, the program code is used to enable the computer system to implement the positioning method based on multi-state color consistency constraints provided by the embodiment of the present invention.

[0197] The computer program executes the above functions defined in the system / device of the embodiment of the present invention when it is executed by the processor 601. According to the embodiment of the present invention, the system, device, module, unit, etc. described above can be implemented by a computer program module.

[0198] In one embodiment, the computer program may rely on tangible storage media such as optical storage devices, magnetic storage devices, etc. In another embodiment, the computer program may also be transmitted and distributed in the form of signals on a network medium, and downloaded and installed through the communication part 609, and / or installed from a removable medium 611. The program code contained in the computer program may be transmitted using any appropriate network medium, including but not limited to: wireless, wired, etc., or any suitable combination of the above.

[0199] In such an embodiment, the computer program can be downloaded and installed from the network through the communication part 609, and / or installed from the removable medium 611. When the computer program is executed by the processor 601, the above functions defined in the system of the embodiment of the present invention are performed. According to the embodiment of the present invention, the system, device, means, module, unit, etc. described above can be implemented by a computer program module.

[0200] According to an embodiment of the present invention, the program code for executing the computer program provided by the embodiment of the present invention can be written in any combination of one or more programming languages. Specifically, these computing programs can be implemented using high-level process and / or object-oriented programming languages, and / or assembly / machine languages. Programming languages ​​include, but are not limited to, Java, C++, python, "C" language or similar programming languages. The program code can be executed entirely on the user computing device, partially on the user device, partially on the remote computing device, or entirely on the remote computing device or server. In the case of a remote computing device, the remote computing device can be connected to the user computing device through any type of network, including a local area network (LAN) or a wide area network (WAN), or can be connected to an external computing device (e.g., using an Internet service provider to connect through the Internet).

[0201] The flow chart and block diagram in the accompanying drawings illustrate the possible architecture, function and operation of the system, method and computer program product according to various embodiments of the present invention. In this regard, each box in the flow chart or block diagram can represent a module, a program segment, or a part of a code, and the above-mentioned module, program segment, or a part of a code contains one or more executable instructions for realizing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order from the order marked in the accompanying drawings. For example, two boxes represented in succession can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram or flow chart, and the combination of the boxes in the block diagram or flow chart can be implemented with a dedicated hardware-based system that performs a specified function or operation, or can be implemented with a combination of dedicated hardware and computer instructions.

[0202] It will be appreciated by those skilled in the art that the features described in the various embodiments of the present invention may be combined and / or combined in various ways, even if such combinations or combinations are not explicitly described in the present invention. In particular, without departing from the spirit and teachings of the present invention, the features described in the various embodiments of the present invention may be combined and / or combined in various ways. All of these combinations and / or combinations fall within the scope of the present invention.

[0203] The embodiments of the present invention are described above. However, these embodiments are only for the purpose of illustration, and are not intended to limit the scope of the present invention. Although each embodiment is described above, it does not mean that the measures in each embodiment cannot be used in combination advantageously. Without departing from the scope of the present invention, those skilled in the art may make various substitutions and modifications, which should all fall within the scope of the present invention.

Claims

1. A positioning method based on multi-state color consistency constraints, characterized in that: The method comprises: Based on the three-dimensional position of the positioning point in the three-dimensional point cloud, the positioning point is imaged by simulating the positioning point to determine the imaging position of the positioning point in the current frame image, wherein the positioning point is a point determined from the three-dimensional point cloud, and the three-dimensional point cloud is obtained by the laser radar carried by the positioning object to collect information about the environment; Based on the projection position of the positioning point on the current frame image and the imaging position, an initial object state corresponding to the current frame image is corrected to obtain a corrected object state, wherein the corrected object state includes a corrected position and posture of an inertial measurement tool carried by the positioning object; Based on a plurality of historical postures of the inertial measurement tool, a plurality of observed colors of the positioning point, a current color of the positioning point in the three-dimensional point cloud, and a color noise distribution parameter, the corrected object state is corrected to obtain a target object state, wherein the observed color represents the color of the positioning point observed in an observed posture determined from the corrected posture and the plurality of historical postures; Based on the state of the target object, determining the target posture of the positioning object; The plurality of observed colors is determined by: Based on the multiple historical postures, the revised postures and the three-dimensional position of the positioning point in the three-dimensional point cloud, determine multiple observation positions of the positioning point in the current frame image, where the observation position is the position of the positioning point observed in the current frame image under the historical posture or the revised posture; For each of the observation positions, the observation color of the positioning point is determined based on the first color of the observation position and the second color of the adjacent area of ​​the observation position in the current frame image, and the observation color is used to determine the photometric error value to construct a first state error determination function that minimizes the error of the corrected object state.

2. The method according to claim 1, characterized in that: The historical posture includes a first historical rotation matrix and a first historical position; the corrected posture includes a corrected rotation matrix and a corrected position; Wherein, determining a plurality of observation positions of the positioning point in the current frame image based on the plurality of historical postures, the corrected postures and the three-dimensional position of the positioning point in the three-dimensional point cloud comprises: For each of the historical poses, Based on the first historical rotation matrix, the first historical position and the three-dimensional position, determine a first imaging three-dimensional position of the positioning point in a camera coordinate system, wherein the camera coordinate system is a coordinate system based on a camera carried by the positioning object, and the camera is a tool for acquiring the current frame image; Inputting the first imaging three-dimensional position and the historical posture into a preset imaging model to obtain a first imaging two-dimensional position of the positioning point in the current frame image; For the corrected pose, Determine a second imaging three-dimensional position of the positioning point based on the corrected rotation matrix, the corrected position and the three-dimensional position; Inputting the second imaging three-dimensional position and the corrected posture into the preset imaging model to obtain the second imaging two-dimensional position of the positioning point in the current frame image; Based on a plurality of the first imaged two-dimensional positions and the second imaged two-dimensional positions, a plurality of the observation positions are determined.

3. The method according to claim 2, characterized in that The determining, based on the first historical rotation matrix, the first historical position, and the three-dimensional position, a first imaging three-dimensional position of the positioning point in a camera coordinate system includes: Determining a second historical rotation matrix of the camera relative to the inertial measurement tool based on the first historical rotation matrix and historical external parameters corresponding to the historical posture, wherein the historical external parameters are external parameters between the camera and the inertial measurement tool; Determining a second historical position of the camera relative to the inertial measurement tool based on the first historical position and the historical external parameter; The first imaging three-dimensional position is determined based on the first historical rotation matrix, the first historical position, the second historical rotation matrix, the second historical position, and the three-dimensional position.

4. The method according to claim 1, characterized in that: The method of correcting the corrected object state based on the multiple historical postures of the inertial measurement tool, the multiple observed colors of the positioning point, the current color of the positioning point in the three-dimensional point cloud, and the color noise distribution parameter to obtain the target object state includes: constructing a first state error determination function based on the plurality of historical postures, the plurality of observed colors of each of the positioning points, the current color of the positioning point in the three-dimensional point cloud, color noise distribution parameters, the estimated object state of the inertial measurement tool, and the corrected object state, wherein the color noise distribution parameters include at least one representation parameter of a statistical distribution conforming to color noise, and the estimated object state is an object state estimated based on measurement data of the inertial measurement tool; The first state error determination function is solved to obtain the target object state.

5. The method according to claim 4, characterized in that The constructing a first state error determination function based on the plurality of historical poses, the plurality of observed colors of each of the positioning points, the current color of the positioning point in the three-dimensional point cloud, a color noise distribution parameter, the estimated object state of the inertial measurement tool, and the corrected object state comprises: Fusing the plurality of historical postures and the corrected object state to obtain a fused object state; Determining a plurality of photometric error values ​​based on the plurality of observed colors and the current color; constructing a first state error determination function for the fused object state based on the plurality of photometric error values, the photometric error correction parameters and the color noise distribution parameters, the estimated object state and the fused object state; The goal of the first state error determination function is to minimize the target error of the fused object state, and the target error includes the error of the corrected object state and the errors of the plurality of historical postures.

6. The method according to claim 5, characterized in that The step of solving the first state error determination function to obtain the target object state includes: Solving a first state error determination function including an i-1th intermediate object state to obtain an i-th target error, wherein i is a positive integer greater than or equal to 1, and when i=1, the i-1th intermediate object state is the fused object state; Performing error correction on the (i-1)th intermediate object state based on the (i)th target error to obtain the (i)th intermediate object state; When it is determined that the i-th target error does not fall within the preset error range, replacing the (i-1)-th intermediate object state with the i-th intermediate object state to obtain a first state error determination function including the i-th intermediate object state; Repeat the above operation until the target error falls within the preset error range; When it is determined that the i-th target error belongs to the preset error range, the i-th intermediate object state is the target object state.

7. The method according to claim 1, characterized in that The step of correcting the initial object state corresponding to the current frame image based on the projection position of the positioning point on the current frame image and the imaging position to obtain the corrected object state includes: Determining a reprojection error value based on the projection position and the imaging position; Determine a second state error determination function based on the reprojection error value, the reprojection error correction parameter, the position error distribution parameter corresponding to the current frame image, the estimated object state of the inertial measurement tool, and the initial object state, wherein the goal of the second state error determination function is to minimize the error of the initial object state, and the position error distribution parameter includes at least one representation parameter of a statistical distribution that the position error conforms to; The second state error determination function is solved to obtain the corrected object state.

8. A positioning device based on multi-state color consistency constraints, characterized in that: The device comprises: An imaging simulation module, used to perform imaging simulation on the positioning point based on the three-dimensional position of the positioning point in the three-dimensional point cloud, and determine the imaging position of the positioning point in the current frame image, wherein the positioning point is a point determined from the three-dimensional point cloud, and the three-dimensional point cloud is obtained by the laser radar carried by the positioning object to collect information about the environment; A first correction module is used to correct the initial object state corresponding to the current frame image based on the projection position of the positioning point on the current frame image and the imaging position to obtain a corrected object state, wherein the corrected object state includes a corrected posture of an inertial measurement tool carried by the positioning object; A second correction module is used to correct the correction object state based on multiple historical postures of the inertial measurement tool, multiple observed colors of the positioning point, the current color of the positioning point in the three-dimensional point cloud, and color noise distribution parameters to obtain a target object state, wherein the observed color represents the color of the positioning point observed in the observed posture determined from the correction posture and the multiple historical postures; A position determination module, used to determine the target position of the positioning object based on the state of the target object; Wherein, the plurality of observed colors are determined in the following manner: Based on the multiple historical postures, the revised postures and the three-dimensional position of the positioning point in the three-dimensional point cloud, determine multiple observation positions of the positioning point in the current frame image, where the observation position is the position of the positioning point observed in the current frame image under the historical posture or the revised posture; For each of the observation positions, the observation color of the positioning point is determined based on the first color of the observation position and the second color of the adjacent area of ​​the observation position in the current frame image, and the observation color is used to determine the photometric error value to construct a first state error determination function that minimizes the error of the corrected object state.

9. An electronic device, comprising: one or more processors; a memory for storing one or more computer programs, It is characterized in that the one or more processors execute the one or more computer programs to implement the steps of the method according to any one of claims 1 to 7.

Images