CN116309883B - 3D target 6DoF accurate positioning method and system - Google Patents

3D target 6DoF accurate positioning method and system Download PDF

Info

Publication number
CN116309883B
CN116309883B CN202310551212.8A CN202310551212A CN116309883B CN 116309883 B CN116309883 B CN 116309883B CN 202310551212 A CN202310551212 A CN 202310551212A CN 116309883 B CN116309883 B CN 116309883B
Authority
CN
China
Prior art keywords
target
image
point
6dof
target model
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202310551212.8A
Other languages
Chinese (zh)
Other versions
CN116309883A (en
Inventor
邓辉
赵越
李华伟
王益亮
沈锴
陈忠伟
石岩
陈丁
陆蕴凡
李虎
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Xiangong Intelligent Technology Co ltd
Original Assignee
Shanghai Xiangong Intelligent Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shanghai Xiangong Intelligent Technology Co ltd filed Critical Shanghai Xiangong Intelligent Technology Co ltd
Priority to CN202310551212.8A priority Critical patent/CN116309883B/en
Publication of CN116309883A publication Critical patent/CN116309883A/en
Application granted granted Critical
Publication of CN116309883B publication Critical patent/CN116309883B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/10—Segmentation; Edge detection
    • G06T7/13—Edge detection
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10024—Color image
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

The application provides a 3D target 6DoF accurate positioning method and a system, wherein the method comprises the following steps: based on the image M, the depth map D and the camera internal reference K, calculating initial pose of the scan scene point cloud data P and the target model in the scan sceneThe method comprises the steps of carrying out a first treatment on the surface of the Sum of squares of vertical distances of target edge feature points in statistical image M to projection lines of sharp edges ME in corresponding target model, line feature matching errorThe method comprises the steps of carrying out a first treatment on the surface of the Calculating the distance from the target feature point in the point cloud data P to the corresponding target model plane as a face feature matching error E f The method comprises the steps of carrying out a first treatment on the surface of the Calculating the difference of the texture of the target model and the color of the image M at the same pixel point as a texture feature matching error E t The method comprises the steps of carrying out a first treatment on the surface of the Will E e 、E f And E is t And (3) performing nonlinear iterative optimization, setting an optimization variable as the 6DoF pose of the target model under a camera coordinate system, and solving the pose of the target model with the minimum accumulated error to obtain the accurate 6DoF pose of the target object.

Description

3D target 6DoF accurate positioning method and system
Technical Field
The application relates to a space positioning technology, in particular to a method and a system for realizing accurate positioning of a 3D target 6DoF by using a computer vision technology.
Background
The 6DoF six-degree-of-freedom target recognition (Six Degrees of Freedom Object Recognition) is to identify the characteristics of an object in a three-dimensional space and determine the position and the posture of the object, so that tracking and positioning of the object are realized. In addition, the RGBD camera can also extract and analyze the characteristics of textures, colors and the like on the surface of the object, so that the accurate classification and identification of the object are realized. Specifically, 6DoF object recognition is capable of recognizing and determining translation of an object in three directions and rotation in three directions, and thus is widely used in the fields of computer vision, robotics, augmented reality, virtual reality, and the like.
In the field of computer vision, the 6DoF target recognition is mainly applied to scenes such as automatic driving, intelligent home, security monitoring and the like, and an intelligent automobile or a sweeping robot can realize automatic navigation according to the three-dimensional pose of the recognized object.
In the field of robots, the 6DoF technology can help the robots to quickly and accurately identify and position objects in the environment, so that tasks such as picking, packaging, logistics and the like can be completed better. In the field of augmented reality and virtual reality, accurate alignment of the virtual object and the actual object can be achieved, so that more real augmented reality and virtual reality experience are achieved. For example, displaying the virtual object in a real environment enables the user to more realistically perceive the presence of the virtual object.
Currently, 6DoF mesh identification based on an RGBD camera refers to target detection and identification by using the RGBD camera (simultaneously acquiring a color image and a depth image), so as to accurately measure and track the position and the posture of an object in a three-dimensional space. Compared with the traditional RGB camera, the RGBD camera can provide more abundant information, including three-dimensional shape and depth information of an object, so that target recognition is more accurate and stable.
Since RGBD cameras work on the principle that the distance of an object to the camera is measured by combining infrared light with a color image. The depth image thus provides coordinate information of the object in three-dimensional space, enabling more accurate localization and tracking of the 6DoF target identification.
However, subject to technical constraints, RGBD cameras currently on the market are classified into: high precision but low imaging efficiency and low precision and high efficiency. However, the requirements of each application field on the accuracy and efficiency of 6DoF target identification are concentrated on high accuracy and high efficiency.
Therefore, before no revolutionary RGBD camera appears, how to acquire the position and the gesture of a high-precision target object in a low-precision and high-efficiency RGBD camera is a main research direction of the current visual recognition algorithm.
Disclosure of Invention
The application mainly aims to provide a 3D target 6DoF accurate positioning method and system, so as to obtain the accurate 6DoF three-dimensional position and posture of a target object on the basis of a low-precision RGBD camera sensor.
In order to achieve the above object, according to a first aspect of the present application, there is provided a 3D object 6DoF accurate positioning method, comprising the steps of:
step S100 is based on RGBD camera capturing image M, depth map D and camera internal reference K of scan scene, calculating point cloud data P of scan scene for determining initial pose of target model in scan scene；
Step S200 statistics of sum of squares of vertical distances from the set of target edge feature points in the image M to projection lines of the set of sharp edges ME in the corresponding target model as a match error of line features；
Step S300, calculating the distance from the target feature point in the point cloud data P to the plane of the corresponding target model as the matching error of the surface feature；
Step S400 calculates the color difference between the texture in the observable area of the target model and the image M at the same pixel point as the matching error of the texture feature；
Step S500 will、/> and />And performing nonlinear iterative optimization, setting an optimization variable as the 6DoF pose of the target model under a camera coordinate system, and calculating the pose of the target model with the minimum accumulated error.
In a possibly preferred embodiment, the step of obtaining the target edge feature point set in step S200 includes:
according to the camera internal parameters K and the initial poseAnd projecting the sharp edge ME set of the target model onto an image plane, and calculating the gradient value and the gradient direction of the image M near the projection line segment so as to find a target edge characteristic point set matched with the projection line segment.
In a possibly preferred embodiment, the step of projecting the set of target model sharp edges ME onto the image plane comprises:
with a sharp edge of one lineThe starting point of the line segment is->The end point isCoordinate values transformed into the camera coordinate system are +.>The starting point of the lower line segment of the camera coordinate system can be obtained from the camera reference K>Endpoint->The projection points on the image are:
；
wherein ,is->Respectively->Is->Is to establish the Z-axis coordinate value of (2) in the form of polar coordinates on the image plane>Andstraight line equation of>The method comprises the following steps:
；
wherein ,for origin to straight line->Is>Is->And an included angle between a perpendicular line passing through the origin and the positive direction of the X axis. (x, y) is the position of the straight line +.>And a coordinate point thereon.
In a possibly preferred embodiment, the step of calculating the gradient value and gradient direction of the image M in the vicinity of the projected line segment, thereby finding a set of target edge feature points matching the projected line segment, comprises:
to be used for and />The connecting line is an axis, and a rectangular frame with +/-n pixel areas perpendicular to the axis direction is a template area ROI; gradient image M to pixels in the region of the ROI, and calculate the gradient direction and straight line +.>Is included in the plane of the first part; extracting pixel points with included angles meeting a threshold as target edge feature points +.>And establishing a set.
In a possibly preferred embodiment, the step of obtaining the set of sharp edges ME in the target model in step S200 includes:
judging normal angles of any triangular surface in the target model and a plurality of connected triangular surfaces, and if the angles of the two connected triangular surfaces are preset acute angles, considering that the intersecting edges of the two triangular surfaces are sharp edges ME; and continuing to judge until all triangular surfaces are judged completely, and acquiring the set of all sharp edges ME.
In a possibly preferred embodiment, the step of calculating the distance from the target feature point in the point cloud data P to the plane of the corresponding target model in step S300 includes:
step S310 according to the initial poseAnd point cloud data P, transforming the triangular surface of the target model into a camera coordinate system;
step S320 calculates the angle between the normal line of the triangular surface and the optical axis direction of the camera, and takes the triangular surface with the angle larger than 120 degrees as the visible triangular surface set in the visual field；
Step S330 traversalMiddle triangle face->Projecting the depth map D onto an image plane, calculating the area of a triangle on the image plane, counting the proportion of the effective pixel area of the depth map D in the triangle to the total area of the triangle, marking the triangle with the proportion reaching the standard as available, and marking the target characteristic point cloud data corresponding to the pixels in the triangle>And triangle face->Establishing a matching relationship;
step S340 statisticsIs +.>Is a vertical distance of (c).
In a possibly preferred embodiment, the step of calculating in step S400 the color difference between the texture in the observable area of the target model and the image M at the same pixel point includes:
step S410 traverses all triangular facetsProjecting the pixel point to an image plane, and calculating the pixel point and the triangular plane of the pixel point according to projection transformation, wherein the pixel point is positioned in the projection triangle of the image plane>Taking texture information at the intersection point as the texture value of the pixel point to obtain a rendering map of +.>；
Step S420 calculates normalized rendering mapIn the same pixel point as image M +.>Color differences in three channel pixel values.
In a possibly preferred embodiment, wherein the Levenberg-Marquarelt algorithm pair is employed in step S500、/> and />And (5) performing nonlinear iterative optimization.
In a possibly preferred embodiment, wherein the initial pose in step S100The acquisition step of (a) comprises:
performing point sampling on the surface of the target model to obtain a model point cloud in a point cloud format; extracting feature points and corresponding feature descriptors from the model point cloud and the point cloud data P respectively, and searching target feature points matched with the model point cloud and the point cloud data P according to the similarity of the feature descriptors;according to the target characteristic points, computing rigid transformation from the model point cloud to the corresponding point cloud data P to obtain the initial pose of the target model in the scanned scene。
In order to achieve the above object, corresponding to the above method, a second aspect of the present application further provides a 3D object 6DoF accurate positioning system, which includes:
the storage unit is used for storing a program comprising the steps of the 3D object 6DoF accurate positioning method, so that the control unit, the processing unit and the transmission unit can timely adjust and execute the program;
the control unit is used for controlling the RGBD camera to acquire an image M and a depth map D of a scanned scene;
the processing unit is used for calculating point cloud data P of the scanned scene based on the camera internal parameter K, the image M and the depth map D so as to determine the initial pose of the target model in the scanned sceneThe method comprises the steps of carrying out a first treatment on the surface of the The sum of squares of the vertical distances from the set of target edge feature points in the statistical image M to the projection line of the set of sharp edges ME in the corresponding target model is taken as the matching error of the line feature +.>The method comprises the steps of carrying out a first treatment on the surface of the Calculating the distance from the target feature point in the point cloud data P to the plane of the corresponding target model as the matching error of the surface feature +.>The method comprises the steps of carrying out a first treatment on the surface of the Calculating the color difference between the texture in the observable area of the target model and the image M at the same pixel point as the matching error of the texture feature>The method comprises the steps of carrying out a first treatment on the surface of the Will then->、/> and />Performing nonlinear iterative optimization, setting an optimization variable as the 6DoF pose of the target model under a camera coordinate system, and calculating the pose of the target model with the minimum accumulated error;
and the transmission unit is used for transmitting the pose of the target model.
The 3D target 6DoF accurate positioning method and the 3D target 6DoF accurate positioning system provided by the application skillfully utilize the point cloud and the texture to rapidly distinguish the pose of the object positioning target model, and simultaneously utilize the high-precision edge definition potential capability of the image to complement the problem of fuzzy boundary of the point cloud and the texture, thereby creating a 6DoF pose recognition scheme for fusing the image and the point cloud so as to support the high-precision pose recognition of the three-dimensional object on the basis of a low-precision RGBD camera sensor.
Drawings
The accompanying drawings, which are included to provide a further understanding of the application and are incorporated in and constitute a part of this specification, illustrate embodiments of the application and together with the description serve to explain the application. In the drawings:
FIG. 1 is a schematic diagram of steps of a 3D object 6DoF accurate positioning method of the present application;
FIG. 2 is a schematic logic diagram of key steps in the 3D target 6DoF accurate positioning method of the present application;
FIG. 3 is a schematic view of the point-to-line vertical distance in polar form in the 3D object 6DoF precise positioning method of the present application;
FIG. 4 is a schematic diagram of the mapping between the gradient direction of the feature point of the target edge in the image and the sharp edge of the target model in the 3D target 6DoF accurate positioning method of the present application;
FIG. 5 is a schematic view of projection of a triangular surface of a target model on an RGBD camera depth map in the 3D target 6DoF accurate positioning method of the present application;
FIG. 6 is a schematic view of a normal direction of a target model and a surface formed by triangular patches in the 3D target 6DoF accurate positioning method of the present application;
fig. 7 is a schematic structural diagram of the 3D target 6DoF precise positioning system of the present application.
Detailed Description
In order that those skilled in the art can better understand the technical solutions of the present application, the following description will clearly and completely describe the specific technical solutions of the present application in conjunction with the embodiments to help those skilled in the art to further understand the present application. It will be apparent that the embodiments described herein are merely some, but not all embodiments of the application. It should be noted that embodiments of the present application and features of embodiments may be combined with each other by those of ordinary skill in the art without departing from the spirit of the present application and without conflicting with each other. All other embodiments, which are derived from the embodiments herein without creative effort for a person skilled in the art, shall fall within the disclosure and the protection scope of the present application.
Furthermore, the terms "first," "second," "S100," "S200," and the like in the description and in the claims and drawings are used for distinguishing between similar objects and not necessarily for describing a particular sequential or chronological order. It is to be understood that the data so used may be interchanged where appropriate such that the embodiments of the application described herein may be implemented in sequences other than those described herein. Also, the terms "comprising" and "having" and any variations thereof herein are intended to cover a non-exclusive inclusion. Unless specifically stated or limited otherwise, the terms "disposed," "configured," "mounted," "connected," "coupled" and "connected" are to be construed broadly, e.g., as being either permanently connected, removably connected, or integrally connected; can be mechanically or electrically connected; can be directly connected or indirectly connected through an intermediate medium, and can be communication between two elements. The specific meaning of the terms in this case will be understood by those skilled in the art in view of the specific circumstances and in combination with the prior art.
In order to achieve the purpose of obtaining accurate three-dimensional positions and postures of target objects on the basis of low-precision RGBD camera sensors. As shown in fig. 1 to 6, the present application provides a 3D target 6DoF accurate positioning method, which includes the steps of:
step S100 is based on RGBD camera capturing image M, depth map D and camera internal reference K of scan scene, calculating point cloud data P of scan scene for determining initial pose of target model in scan scene。
Specifically, the conversion relationship between the depth map D and the camera internal reference K and the point cloud data P is as follows:
；
wherein ,，Zis depth map D->Depth value at position ∈>Is the coordinate value corresponding to the point cloud data P.
Further, in step S100, the initial poseThe calculation steps of (1) comprise:
and step S110, performing point sampling on the surface of the target model to obtain a model point cloud in a point cloud format.
In step S120, feature points and corresponding feature descriptors are extracted from the model point cloud and the point cloud data P, and target feature points matched with the model point cloud and the point cloud data P are found according to the similarity of the feature descriptors.
Step S130 calculates the rigid transformation from the model point cloud to the corresponding point cloud data P according to the target feature points to obtain the initial pose of the target model in the scanned scene。
Step S200 statistics of sum of squares of vertical distances from the set of target edge feature points in the image M to projection lines of the set of sharp edges ME in the corresponding target model as a match error of line features。
Specifically, the method for obtaining the target edge feature point set mainly includes: according to the camera internal parameters K and the initial poseAnd projecting the sharp edge ME set of the target model onto an image plane, and calculating the gradient value and the gradient direction of the image M near the projection line segment so as to find a target edge characteristic point set matched with the projection line segment.
In this example, the calculating step of the target edge feature point set includes:
step S210 uses the initial pose calculated in step S100The sharp edge segment me in the object model is transformed into the camera coordinate system.
Assuming a sharp edge as one line segmentThe starting point of the line segment is->The end point is. Coordinate values transformed under the camera coordinate system are +.>The starting point of the lower line segment of the camera coordinate system can be obtained from the camera reference K>Endpoint->The projection points on the image are:
；
wherein ,is->Respectively->Is->Is a Z-axis coordinate value of (a).
Step S220 as shown in FIG. 3, establishing an image plane in polar coordinates and />Straight line equation of>The method comprises the following steps:
；
wherein ,for origin to straight line->Is>Is->And an included angle between a perpendicular line passing through the origin and the positive direction of the x axis.For being located in straight line under the image coordinate system>And a coordinate point thereon.
Step S230 to and />The connecting line is an axis, and a rectangular frame with + -n pixel areas perpendicular to the axis direction is a template area RIO. Wherein the size of the n pixel areas is set to be +.>With respect to accuracy, the smaller the initial pose accuracy the smaller the pixel area can be set and thus the higher the computational efficiency, and vice versa, where n=6 is preferred in this example.
Further, the image M gradients pixels in the RIO region, and calculates the gradient direction and the straight lineIncluded angle->Extracting pixel points with included angles larger than 60 degrees as target edge feature points +.>And establishing a set.
As shown in FIG. 4, the black line segment identifying the outline of the model is a straight lineThe arrow pointing direction is the gradient direction of the target edge feature point of the region.
The image gradient calculation formula is as follows:
gradient vector；
Gradient amplitude；
Gradient direction；
WhileRepresenting the pixel value of the corresponding coordinate.
On the other hand, in this example, the calculation step of the set of sharp edges ME comprises:
step S211 is to start judging the normal included angle between any triangular surface and the connected 3 triangular surfaces. If the angle between the two triangular faces connected is greater than 45 °, the edge where the two triangular faces meet is considered to be a sharp edge ME.
Step S212 continues to judge the 3 triangular faces and the adjacent triangular faces until all the triangular faces are judged to be finished. ME is the set of all sharp edges.
As shown in fig. 6, the object model file for identifying the object includes a texture image and a patch structure, wherein a black line segment is an edge of a triangular patch, and a white line segment is a normal direction of the triangular patch.
Finally, calculating the characteristic points of the target edge in the template areaTo the corresponding straight line +.>Is a vertical distance of:
；
with the sum of squares of cumulative vertical distances as the match error of line featuresThe specific formula is as follows:
；
step S300, calculating the distance from the target feature point in the point cloud data P to the plane of the corresponding target model as the matching error of the surface feature。
Specifically, the step of calculating the distance from the target feature point in the point cloud data P to the plane where the corresponding target model is located includes:
step S310 according to the initial poseAnd point cloud data P, transforming the triangular surface of the target model into a camera coordinate system. Suppose that one triangular surface F contains three vertices +.>. The specific formula is as follows:
；
step S320 calculates the angle between the normal line of the triangular surface and the optical axis direction of the camera, and takes the triangular surface with the angle larger than 120 degrees as the visible triangular surface set in the visual field。
Step S330 traversalMiddle triangle face->Projecting the depth map D onto an image plane, calculating the area of a triangle on the image plane, counting the proportion of the effective pixel area of the depth map D in the triangle to the total area of the triangle, and taking the proportion to reach the standardIs marked as available, and the target characteristic point data corresponding to the pixels in the triangle are marked +.>And triangle face->Establishing a matching relationship, which is regarded as->And->Matching each other.
For example, the triangular face is marked as usable when the duty ratio is greater than 60%. As shown in fig. 5, the depth map represents depth information at each pixel by gray values, and black indicates that no depth information at the pixel is an invalid pixel. In the depth map enclosed by triangle 1, the area of the black invalid pixel is larger than 40%, so that the area of the effective pixel is smaller than 60%, and the triangle is judged to be unavailable.
Step S340 statisticsIs +.>Is a vertical distance of (c).
For example, assume triangular surfacesThe plane equation is +.>Point cloud data->The sum of the accumulated vertical distances from each point to the plane is:
；
the error accumulation of all available triangles and the corresponding matching point cloud is the face feature matching errorThe specific formula is as follows:
；
step S400 calculates the color difference between the texture in the observable area of the target model and the image M at the same pixel point as the matching error of the texture feature。
The step of calculating the color difference between the texture in the observable area of the target model and the image M at the same pixel point includes:
step S410 traverses all triangular facetsProjecting the pixel point to an image plane, and calculating the pixel point and the triangular plane of the pixel point according to projection transformation, wherein the pixel point is positioned in the projection triangle of the image plane>By using texture information at the intersection point as the texture value of the pixel point, gathering all visible triangular faces ++>The projected rendering on the image plane is regarded as +.>。
Step S420 calculates normalized rendering mapR, g, b three channel pixel values at the same pixel point as image M.
Specifically, the normalization operation is performed on the image M first:
；
where (R, G, B) is the color information of the original color image and (R, G, B) is the normalized image M. The same will render the graphThe normalization operation is performed on the colors of (3).
Then, a rendering graph is calculatedThe r, g, b three-channel color difference at the same pixel point as the color image M, to obtain texture feature matching error ∈>. The specific formula is as follows:
；
step S500 adopts a Levenberg-Marquarelt algorithm pair、/> and />And performing nonlinear iterative optimization, setting an optimization variable as the 6DoF pose of the target model under a camera coordinate system, and calculating the pose of the target model with the minimum accumulated error.
Specifically, the calculation step of step S500 includes:
step S510 first synthesizes line feature matching errorsFace feature matching error->And texture feature match error->As a cost function->. The Taylor expansion form of the cost function is as follows:
；
wherein ,is a jacobian matrix that is the first derivative of the cost function to the model pose. The method comprises the following steps:
；
h is a Heisen matrix, and represents the second derivative of the cost function on the pose of the model. The method comprises the following steps:
；
wherein x in the equation represents the current model pose, h represents the pose adjustment amount,for the cost value of x at the current pose, < +.>Is a very small amount approximately equal to 0. The Taylor expansion is to fit the change trend of the cost function at the current pose x through a quadratic equation, calculate the h-added offspring value relative to the current pose x, and find a proper adjustment amount h by observing the change of the cost function value so as to enable +.>Minimum.
From the equation can be seenThe minimum is +.>Minimum value. Since the maximum of the quadratic equation is at 0 for the first derivative, the equation can be established:
；
therefore, the most suitable adjustment amount is。
Further, in step S520, since the calculation process of the hessian matrix in the high-dimensional space is complex, we use the jacobian matrix to approximate the hessian matrix to increase the calculation efficiency of the algorithm. The specific formula is as follows:
；
wherein ,the damping is generally set to 0.01 #>Is an identity matrix. Damping amount->The larger the set adjustment amount h, the larger the change range of the loss function, but the optimal solution may be skipped. On the contrary, let(s)>The smaller the variation amplitude of the loss function is, the smaller the variation amplitude is, and therefore the damping amount can be appropriately selected according to the actual situation.
Step S530 is further to update the initial pose of the target model according to the adjustment amount h. Recalculating the cost function according to the updated model pose>The iteration steps S510-S530 are continued until the iteration is stopped when the two adjacent cost function value changes do not exceed a certain threshold. For example, a distance of 1mm, an angle of 0.5 deg., may be provided, indicating that the resulting pose differs from the desired value by less than this range. The final model pose with the high precision and the distance error smaller than 1mm and the angle error smaller than 0.5 degree is obtained.
It follows that the method of the present example is capable of virtualizing color images and depth information under a 3D object model camera from the intrinsic model K of a real RGBD camera. Obtaining edge characteristic differences by calculating edge point of object in real camera on image and corresponding model edge distance differences under virtual camera visionThe distance difference between the scanning point cloud positioned on the object in the real camera and the corresponding surface of the model under the visual field of the virtual camera is calculated to obtain the surface characteristic difference +.>Calculating the color difference between the surface color of the object in the real camera and the corresponding position of the model under the virtual camera to obtain texture feature difference +.>. Therefore, the model pose can be continuously adjusted to reduce the three differences by utilizing the LM algorithm, and the 6DoF model pose meeting the precision requirement can be obtained by setting the iteration stop condition.
On the other hand, as shown in fig. 7, the present application also provides a 3D target 6DoF accurate positioning system, corresponding to the above method, which includes:
the storage unit is used for storing a program comprising the steps of the 3D object 6DoF accurate positioning method, so that the control unit, the processing unit and the transmission unit can timely adjust and execute the program;
the control unit is used for controlling the RGBD camera to acquire an image M and a depth map D of a scanned scene;
the processing unit is used for calculating point cloud data P of the scanned scene based on the camera internal parameter K, the image M and the depth map D so as to determine the initial pose of the target model in the scanned sceneThe method comprises the steps of carrying out a first treatment on the surface of the The sum of squares of the vertical distances from the set of target edge feature points in the statistical image M to the projection line of the set of sharp edges ME in the corresponding target model is taken as the matching error of the line feature +.>The method comprises the steps of carrying out a first treatment on the surface of the Calculating the distance from the target feature point in the point cloud data P to the plane of the corresponding target model as the matching error of the surface feature +.>The method comprises the steps of carrying out a first treatment on the surface of the Calculating the color difference between the texture in the observable area of the target model and the image M at the same pixel point as the matching error of the texture feature>The method comprises the steps of carrying out a first treatment on the surface of the Will then->、/> and />Performing nonlinear iterative optimization, setting an optimization variable as the 6DoF pose of the target model under a camera coordinate system, and calculating the pose of the target model with the minimum accumulated error;
and the transmission unit is used for transmitting the pose of the target model.
In summary, the 3D target 6DoF accurate positioning method and system provided by the application skillfully utilizes the point cloud and the texture to rapidly distinguish the pose of the object positioning target model, and simultaneously utilizes the high-precision edge definition capability of the image to complement the problem of fuzzy boundary of the point cloud and the texture, thereby creating a 6DoF pose recognition scheme for fusing the image and the point cloud so as to support the high-precision pose recognition of the three-dimensional object on the basis of a low-precision RGBD camera sensor.
The preferred embodiments of the application disclosed above are intended only to assist in the explanation of the application. The preferred embodiments are not exhaustive or to limit the application to the precise form disclosed. Obviously, many modifications and variations are possible in light of the above teaching. The embodiments were chosen and described in order to best explain the principles of the application and the practical application, to thereby enable others skilled in the art to best understand and utilize the application. The application is to be limited only by the following claims and their full scope and equivalents, and any modifications, equivalents, improvements, etc., which fall within the spirit and principles of the application are intended to be included within the scope of the application.
It will be appreciated by those skilled in the art that the system, apparatus and their respective modules provided by the present application may be implemented entirely by logic programming method steps, in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers, embedded microcontrollers, etc., except for implementing the system, apparatus and their respective modules provided by the present application in a purely computer readable program code. Therefore, the system, the apparatus, and the respective modules thereof provided by the present application may be regarded as one hardware component, and the modules included therein for implementing various programs may also be regarded as structures within the hardware component; modules for implementing various functions may also be regarded as being either software programs for implementing the methods or structures within hardware components.
Furthermore, all or part of the steps in implementing the methods of the embodiments described above may be implemented by a program, where the program is stored in a storage medium and includes several instructions for causing a single-chip microcomputer, chip or processor (processor) to execute all or part of the steps in the methods of the embodiments of the application. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a random access Memory (RAM, random Access Memory), a magnetic disk, or an optical disk, or other various media capable of storing program codes.

Claims (10)

1. A3D target 6DoF accurate positioning method comprises the following steps:
step S100 is based on RGBD camera capturing image M, depth map D and camera internal reference K of scan scene, calculating point cloud data P of scan scene for determining initial pose of target model in scan scene；
Step S200 statistics of sum of squares of vertical distances from the set of target edge feature points in the image M to projection lines of the set of sharp edges ME in the corresponding target model as a match error of line features；
Step S300, calculating the distance from the target feature point in the point cloud data P to the plane of the corresponding target model as the matching error of the surface feature；
Step S400 calculates the color difference between the texture in the observable area of the target model and the image M at the same pixel point as the matching error of the texture feature；
Step S500 will、/> and />And performing nonlinear iterative optimization, setting an optimization variable as the 6DoF pose of the target model under a camera coordinate system, and calculating the pose of the target model with the minimum accumulated error.
2. The 3D object 6DoF accurate positioning method according to claim 1, wherein the acquiring of the object edge feature point set in step S200 includes:
according to the camera internal parameters K and the initial poseAnd projecting the sharp edge ME set of the target model onto an image plane, and calculating the gradient value and the gradient direction of the image M near the projection line segment so as to find a target edge characteristic point set matched with the projection line segment.
3. The 3D object 6DoF accurate positioning method according to claim 2, wherein the step of projecting the set of object model sharp edges ME onto the image plane comprises:
with a sharp edge of one lineThe starting point of the line segment is->The end point isCoordinate values transformed into the camera coordinate system are +.>The starting point of the lower line segment of the camera coordinate system can be obtained from the camera reference K>Endpoint->The projection points on the image are:
；
wherein ,is->Respectively->Is->Is a Z-axis coordinate value of (2); establishing the cross-section in the image plane in the form of polar coordinates> and />Straight line equation of>The method comprises the following steps:
；
wherein ,for origin to straight line->Is>Is->An included angle between a vertical line passing through the origin and the positive direction of the X axis,for being located in straight line under the image coordinate system>And a coordinate point thereon.
4. A 3D object 6DoF accurate positioning method according to claim 3, wherein the step of calculating the gradient value and gradient direction of the image M in the vicinity of the projected line segment so as to find a set of object edge feature points matching the projected line segment comprises:
to be used for and />The connecting line is an axis, and a rectangular frame which is perpendicular to the axis direction and is provided with n pixel areas is a template area RIO; gradient the image M to pixels in RIO region, and calculate gradient direction and straight line +.>Is included in the plane of the first part; extracting pixel points with included angles meeting a threshold as target edge feature points +.>And establishing a set.
5. The 3D object 6DoF accurate positioning method according to claim 1, wherein the step of obtaining the set of sharp edges ME in the object model in step S200 comprises:
judging normal angles of any triangular surface in the target model and a plurality of connected triangular surfaces, and if the angles of the two connected triangular surfaces are preset acute angles, considering that the intersecting edges of the two triangular surfaces are sharp edges ME; and continuing to judge until all triangular surfaces are judged completely, and acquiring the set of all sharp edges ME.
6. The method for precisely positioning a 3D object 6DoF according to claim 1, wherein the step of calculating the distance from the object feature point in the point cloud data P to the plane of the corresponding object model in step S300 includes:
step S310 according to the initial poseAnd point cloud data P, transforming the triangular surface of the target model into a camera coordinate system;
step S320 calculates the angle between the normal line of the triangular surface and the optical axis direction of the camera, and takes the triangular surface with the angle larger than 120 degrees as the visible triangular surface set in the visual field；
Step S330 traversalMiddle triangle face->Projecting the depth map D onto an image plane, calculating the area of a triangle on the image plane, counting the proportion of the effective pixel area of the depth map D in the triangle to the total area of the triangle, marking the triangle with the proportion reaching the standard as available, and marking the target characteristic point cloud data corresponding to the pixels in the triangle>And triangle face->Establishing a matching relationship;
step S340 statisticsIs +.>Is a vertical distance of (c).
7. The 3D object 6DoF accurate positioning method according to claim 6, wherein the step of calculating the color difference between the texture in the observable area of the object model and the image M at the same pixel point in step S400 includes:
step S410 traverses all triangular facetsProjecting the pixel point to an image plane, and calculating the pixel point and the triangular plane of the pixel point according to projection transformation, wherein the pixel point is positioned in the projection triangle of the image plane>Taking texture information at the intersection point as the texture value of the pixel point to obtain a rendering map of +.>；
Step S420 calculates normalized rendering mapR, g, b three channel pixel values at the same pixel point as image M.
8. The method for precisely locating a 3D object 6DoF according to claim 1, wherein in step S500, a Levenberg-Marquarelt algorithm pair is adopted、/> and />And (5) performing nonlinear iterative optimization.
9. According to claimThe 3D object 6DoF precise positioning method as claimed in claim 1, wherein the initial pose in step S100The acquisition step of (a) comprises:
performing point sampling on the surface of the target model to obtain a model point cloud in a point cloud format;
extracting feature points and corresponding feature descriptors from the model point cloud and the point cloud data P respectively, and searching target feature points matched with the model point cloud and the point cloud data P according to the similarity of the feature descriptors;
according to the target characteristic points, computing rigid transformation from the model point cloud to the corresponding point cloud data P to obtain the initial pose of the target model in the scanned scene。
10. A 3D target 6DoF precision positioning system, comprising:
a storage unit, configured to store a program including the steps of the 3D object 6DoF accurate positioning method according to any one of claims 1 to 9, for the control unit, the processing unit, and the transmission unit to timely call and execute;
the control unit is used for controlling the RGBD camera to acquire an image M and a depth map D of a scanned scene;
the processing unit is used for calculating point cloud data P of the scanned scene based on the camera internal parameter K, the image M and the depth map D so as to determine the initial pose of the target model in the scanned sceneThe method comprises the steps of carrying out a first treatment on the surface of the The sum of squares of the vertical distances from the set of target edge feature points in the statistical image M to the projection line of the set of sharp edges ME in the corresponding target model is taken as the matching error of the line feature +.>The method comprises the steps of carrying out a first treatment on the surface of the Calculating the distance from the target feature point in the point cloud data P to the plane of the corresponding target model asMatching error of surface features->The method comprises the steps of carrying out a first treatment on the surface of the Calculating the color difference between the texture in the observable area of the target model and the image M at the same pixel point as the matching error of the texture featuresThe method comprises the steps of carrying out a first treatment on the surface of the Will then->、/> and />Performing nonlinear iterative optimization, setting an optimization variable as the 6DoF pose of the target model under a camera coordinate system, and calculating the pose of the target model with the minimum accumulated error;
and the transmission unit is used for outputting the pose of the target model.
CN202310551212.8A 2023-05-16 2023-05-16 3D target 6DoF accurate positioning method and system Active CN116309883B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310551212.8A CN116309883B (en) 2023-05-16 2023-05-16 3D target 6DoF accurate positioning method and system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310551212.8A CN116309883B (en) 2023-05-16 2023-05-16 3D target 6DoF accurate positioning method and system

Publications (2)

Publication Number Publication Date
CN116309883A CN116309883A (en) 2023-06-23
CN116309883B true CN116309883B (en) 2023-08-18

Family

ID=86781805

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310551212.8A Active CN116309883B (en) 2023-05-16 2023-05-16 3D target 6DoF accurate positioning method and system

Country Status (1)

Country Link
CN (1) CN116309883B (en)

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112907610A (en) * 2021-03-25 2021-06-04 东南大学 LeGO-LOAM-based step-by-step interframe pose estimation algorithm
CN115272596A (en) * 2022-07-08 2022-11-01 同济大学 Multi-sensor fusion SLAM method oriented to monotonous texture-free large scene

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7844105B2 (en) * 2007-04-23 2010-11-30 Mitsubishi Electric Research Laboratories, Inc. Method and system for determining objects poses from range images
EP2904417A1 (en) * 2012-09-27 2015-08-12 Metaio GmbH Method of determining a position and orientation of a device associated with a capturing device for capturing at least one image

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112907610A (en) * 2021-03-25 2021-06-04 东南大学 LeGO-LOAM-based step-by-step interframe pose estimation algorithm
CN115272596A (en) * 2022-07-08 2022-11-01 同济大学 Multi-sensor fusion SLAM method oriented to monotonous texture-free large scene

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
Xiangxu Meng，et al.Six-degree-of-freedom posture measurement technologies using position sensitive detectors（PSDs）：state of the art.micromachines.2022,第13卷(第11期),第1-21页. *

Also Published As

Publication number Publication date
CN116309883A (en) 2023-06-23

Similar Documents

Publication Publication Date Title
WO2021233029A1 (en) Simultaneous localization and mapping method, device, system and storage medium
Azad et al. Stereo-based 6d object localization for grasping with humanoid robot systems
CN104463108B (en) A kind of monocular real time target recognitio and pose measuring method
CN110176032B (en) Three-dimensional reconstruction method and device
Zhao et al. Reconstruction of textured urban 3D model by fusing ground-based laser range and CCD images
CN111476841A (en) Point cloud and image-based identification and positioning method and system
CN111998862A (en) Dense binocular SLAM method based on BNN
Yan et al. Joint camera intrinsic and lidar-camera extrinsic calibration
Wietrzykowski et al. PlaneLoc: Probabilistic global localization in 3-D using local planar features
Streiff et al. 3D3L: Deep learned 3D keypoint detection and description for lidars
Nirmal et al. Homing with stereovision
Ramisa et al. Mobile robot localization using panoramic vision and combinations of feature region detectors
CN111198563B (en) Terrain identification method and system for dynamic motion of foot type robot
Cociaş et al. Multiple-superquadrics based object surface estimation for grasping in service robotics
CN116309883B (en) 3D target 6DoF accurate positioning method and system
CN116402904A (en) Combined calibration method based on laser radar inter-camera and monocular camera
Azad et al. Accurate shape-based 6-dof pose estimation of single-colored objects
CN114972491A (en) Visual SLAM method, electronic device, storage medium and product
Wang et al. Automatic measurement based on stereo vision system using a single PTZ camera
Hachiuma et al. Recognition and pose estimation of primitive shapes from depth images for spatial augmented reality
Peng et al. Real time and robust 6D pose estimation of RGBD data for robotic bin picking
CN114004900A (en) Indoor binocular vision odometer method based on point-line-surface characteristics
Su Vanishing points in road recognition: A review
Chen et al. Object recognition and localization based on kinect camera in complex environment
Holz et al. Approximate surface reconstruction and registration for RGB-D SLAM

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant