CN109960402B - Virtual and real registration method based on point cloud and visual feature fusion - Google Patents

Abstract

The invention discloses a virtual and real registration method based on point cloud and visual feature fusion, which is completed in two stages of off-line and on-line, and specifically comprises the following steps: in an off-line stage, a three-dimensional CAD model of a reference object part is generated into a three-dimensional point cloud model in a CAD environment, preparation is made for establishment of an assembly absolute coordinate system and subsequent virtual-real registration, and in order to accelerate the point cloud registration and the assembly coordinate system establishment process, the point cloud model of the reference object part needs to be simplified in the stage; in an online stage, collecting assembly field video stream and generating point cloud, completing the definition of an assembly absolute coordinate system by utilizing the registration between reference object point cloud and assembly field point cloud, setting the relative position of the initial point cloud in an iterative registration process based on the virtual and real registration of the point cloud, and obtaining an accurate tracking registration matrix under the absolute coordinate system. The invention not only avoids the frame loss caused by the over-fast moving speed of the camera, but also improves the registration precision of the virtual and the real.

Description

Virtual and real registration method based on point cloud and visual feature fusion

Technical Field

The invention belongs to the technical field of augmented reality, and particularly relates to a virtual and real registration method based on point cloud and visual feature fusion.

Background

The virtual-real registration (i.e. three-dimensional tracking registration) technology is to correctly match or organically combine computer-generated virtual objects or other information with a real scene, and the key is the tracking and positioning of a camera. Many scholars have proposed a large number of methods for tracking and positioning cameras, mainly including tracking and positioning based on artificial identification and natural features. However, manual identification is strictly prohibited due to strict assembly requirements of mechanical products. In addition, the assembly environment mostly takes smooth and texture-lacking parts as main components, and tracking registration based on natural features is difficult to effectively apply. The tracking registration method based on the dense point cloud generates the point cloud by using the scene depth image, realizes the tracking and positioning of the camera through the point cloud registration, can show better robustness in the mechanical product assembly environment with lower illumination intensity and lacking texture on the surface, and is therefore interested and concerned by vast researchers.

The point cloud-based virtual and real registration method can show good robustness in a mechanical product assembly environment with low illumination intensity and a lack of texture on the surface. However, the existing virtual and real registration method based on dense point cloud can only estimate the relative pose of the sensor, and cannot be directly applied to the assembly environment of mechanical products. Meanwhile, the dense point cloud-based virtual and real registration method mainly adopts an iterative closest point method to estimate the pose of the camera. However, when the camera moves rapidly, the Iterative Closest Point (ICP) registration algorithm cannot acquire a sufficient number of correct initial corresponding data point sets, so that the iterative process is prone to fall into local optimality, convergence speed and registration accuracy are difficult to guarantee, and even a frame loss situation is caused, which causes interruption of a virtual-real registration process. Therefore, how to acquire a sufficient number of correct initial corresponding data point sets by utilizing redundant color image information of the depth sensor at the same time and through feature point matching becomes a difficult point for improving the convergence speed of the iterative process and the calculation precision of the camera pose. And because the surface of the object on the assembly site is mostly smooth and non-textured, how to obtain a sufficient number of matching feature point pairs is also a problem to be solved.

The virtual-real registration is the primary condition for virtual-real fusion of the AR assembly auxiliary system. How to register the virtual assembly guide information into the assembly environment stably and accurately becomes a difficult point of research. The dense point cloud-based virtual and real registration method can show better robustness in a mechanical product assembly environment with lower illumination intensity and lacking texture on the surface. The method uses all or most points in the point cloud to participate in the calculation process, and uses an Iterative Closest Point (ICP) registration algorithm to carry out iterative operation on all corresponding points, so as to obtain the camera pose and greatly improve the false and true registration accuracy. However, an iterative closest point strategy adopted by the existing dense point cloud-based virtual and real registration method can only estimate the pose of the sensor relative to the initial position (unfixed) of the sensor, and cannot be directly applied to an assembly scene needing absolute assembly position information. Meanwhile, when the moving speed of the depth sensor is too high, so that a correct initial corresponding data point set cannot be obtained, the iterative process of the ICP algorithm is easy to fall into local optimization, and the convergence speed and the registration accuracy are difficult to guarantee. And when the working range of the depth sensor is exceeded (0.15-4 m), the condition of frame loss is easily caused, and the interruption of the virtual and real registration process is caused. Continuous interframe registration adopted in the tracking process also reduces the tracking precision greatly along with the increase of the movement distance of the sensor; the virtual and real registration method based on feature point matching extracts feature points in a color image, then maps the feature points to a three-dimensional space, and calculates the transformation between corresponding feature point pairs through an RANSAC algorithm, thereby finishing the estimation of the pose of the camera. The virtual and real registration method based on feature point matching has the advantages of high response speed, low requirements on the surfaces of parts and components, wide working range and capability of well finishing camera pose estimation when the depth sensor exceeds the working range. But this method has high requirements on ambient lighting conditions. Therefore, the combination of the algorithm based on the feature point matching and the point cloud registration is one of effective methods for solving the failure of the virtual and real registration process of the mechanical product assembly environment. However, due to the lack of sufficient texture features on the mechanical product assembly site or the surface of the tool, the transformation matrix estimation is performed by using the conventional SIFT or SURF as a feature detector or descriptor, and the false and real registration is prone to fail due to the lack of sufficient matching feature points.

Disclosure of Invention

The present invention is directed to solving the above problems of the prior art. The virtual and real registration method based on the point cloud and visual feature fusion not only avoids the frame loss caused by the over-high moving speed of the camera, but also improves the virtual and real registration accuracy. The technical scheme of the invention is as follows:

a virtual and real registration method based on point cloud and visual feature fusion is completed in two stages of off-line and on-line, and specifically comprises the following steps:

in the off-line stage, a three-dimensional point cloud model is generated by a three-dimensional CAD model of the reference object part in a CAD environment, preparation is made for the establishment of an assembly absolute coordinate system and the subsequent virtual-real registration, and in order to accelerate the processes of point cloud registration and assembly coordinate system establishment, the point cloud model of the reference object part needs to be simplified in the stage;

in an online stage, collecting assembly field video stream and generating point cloud, completing the definition of an assembly absolute coordinate system by utilizing the registration between reference object point cloud and assembly field point cloud, setting the relative position of the initial point cloud in an iterative registration process based on the virtual and real registration of the point cloud, and obtaining an accurate tracking registration matrix under the absolute coordinate system.

Further, in the online stage, the assembly environment point cloud generation and preprocessing specifically include:

1) point cloud generation: for each pixel point u in the depth image_DPerforming parallel back projection to a camera coordinate space to obtain a vertex and a three-dimensional vertex V_t(u) and the normal vector N corresponding to the vertex_t(u) forming a three-dimensional assembly environment point cloud;

2) point cloud space index establishing: establishing a point cloud data structure by adopting a spatial index Kd-tree;

3) Kd-Tree nearest neighbor search step.

Further, in the point cloud generating step in the step 1), after the depth image of the assembly environment is subjected to noise reduction and restoration, each pixel point u at the moment i in the image is subjected to noise reduction and restoration_D＝(x_d,y_d) Back-projecting to a depth camera space coordinate system by the formula (1), and accelerating the process to each pixel point u in the depth image_DPerforming parallel back projection to a camera coordinate space to obtain a vertex, wherein the back projection result is as follows:

V_t(u)＝D_i(u)M_{int_D} ^-1[u_D,1] (1)

in the formula: d_i(u) -the depth image at time i; m_{int_D}-depth camera internal parameters.

Meanwhile, each vertex corresponds to a normal vector N_t(u) using the neighboring proxels by the following operation:

N_t(u)＝(V_t(x+1,y)-V_t(x,y))×(V_t(x,y+1)-V_t(x,y)) (2)

V_t(x +1, y) corresponds to a vertex. x and y represent the horizontal and vertical coordinates of the pixel points;

normalizing (2) to a unit length N_t/||N_t||₂，||N_t||₂、N_tRepresenting the corresponding time normal vector three-dimensional vertex V_t(u) and the normal vector N corresponding to the vertex_t(u) forming a three-dimensional assembly environment point cloud.

Further, the step 2) of establishing a point cloud spatial index: establishing a point cloud data structure by adopting a spatial index Kd-tree, which specifically comprises the following steps:

step1: determining a Split point Split threshold value;

step2: determining a Node-data domain value of a root Node;

step3: determining a left subspace and a right subspace;

step4: and (5) recursion step.

Further, the basic idea of the nearest neighbor search in step 3) is as follows:

step1, perform binary search and generate search path. The point to be inquired is quickly searched along the search path to find the nearest approximate point, namely the leaf node;

step2, backtracking search, the found leaf node is not necessarily the nearest, the nearest point should be located in the circle domain which uses the query point as the center and passes through the leaf node, and in order to find the true nearest neighbor, it is also necessary to search backward along the search path whether there is a data point closer to the query point.

Further, the assembly coordinate system establishing process specifically includes the steps of:

step1, generating point cloud by taking water pump shell as object

Step2, acquiring depth image R of assembly site at t moment on line_t(u) (where u is (x, y)), and for depth image R_t(u) performing noise reduction restoration to obtain a noise-reduced and restored depth image, and then generating a three-dimensional environment point cloud, wherein X ═ { X ═ X₀,x₁,x₂...x_n,N₀,N₁,N₂...N_n}

Step3, the three-dimensional environment point cloud and the parameters are mixedPoint cloud generated by the test object model P ═ { P₀,p₁,p₂...p_m,n₀,n₁,n₂...n_mAnd (5) performing registration by using an Iterative Closest Point (ICP), and performing parallel operation by adopting multiple threads of a Graphics Processing Unit (GPU) to improve the registration speed. In order to obtain the corresponding registration points, Kd-tree is used to accelerate the matching search, Euclidean distance (Euclidean distance) is used as the matching metric (see formula 3), and in order to reduce the number of mismatching points as much as possible, normal vector features are added as an additional constraint condition. Using dot product d_ij＝<m_i,m_j>Measure the normal vector for the candidate point, if d_ijIf the weight is greater than a certain threshold psi, the point is considered as a corresponding point, and a weight value of 1 is given, which is expressed as formula 4

Step4, obtaining the number N of corresponding points and the weight w of each pair of corresponding points by adopting characteristic measurement methods such as Euclidean distance, normal vector and the like_i. And solving the position relation error matrix by adopting a least square method, and determining a transformation matrix between the point cloud X and the point cloud P, wherein the transformation matrix is shown in a formula 5.

Step5, the definition of the initial absolute coordinate system of the assembly can be completed through the solution of the rotation matrix R and the translational vector t, and the preparation is made for the solution of the virtual and real registration matrix M under the subsequent absolute coordinate system

Further, the tracking registration matrix fused by the point cloud and the visual features specifically comprises the following steps:

firstly, collecting an assembly environment color image and a depth image, and then converting a depth measurement value from a two-dimensional image coordinate to a global coordinate space by adopting formulas (1) and (2) to generate an environment point cloud; meanwhile, extracting feature points in the color image by using an improved feature description operator ORB and performing continuous inter-frame matching; mapping the matched ORB characteristic point pair to a three-dimensional coordinate space by using depth information, removing outliers by adopting random sampling consistency, keeping the characteristic point pair when the distance between the two points is less than an error value, and removing the outliers to obtain an initial transformation matrix; using the transformation matrix and the correct matching point pair obtained by calculation as an initial data set for ICP point cloud registration, and using loop detection and pose optimization strategy to register point cloudOptimizing the transformation matrix obtained by the segments, and finally combining the matrix M_initAnd obtaining an accurate tracking registration matrix under an absolute coordinate system.

The invention has the following advantages and beneficial effects:

aiming at the characteristics of the assembly environment of mechanical products, the invention firstly defines a virtual and real registration absolute coordinate system by using reference model point cloud, then designs a visual feature matching strategy based on direction vector consistency, designs an RGBD-ICP virtual and real registration method combining dense point cloud and visual features based on the visual feature matching strategy, and adds a loop detection and global optimization strategy on the basis, thereby avoiding the frame loss caused by the over-high moving speed of a camera and improving the virtual and real registration precision.

The invention establishes an assembly environment point cloud based on the depth image. According to the characteristics of light reflection and lack of textures of an assembly environment, an RGBD-ICP virtual-real registration method combining point cloud and visual characteristics is designed, and a primary condition is provided for generation of a virtual-real fusion scene.

Drawings

FIG. 1 is a flow chart of a virtual-real registration method based on point cloud and visual feature fusion according to a preferred embodiment of the present invention;

FIG. 2 shows a planar cut corresponding to a two-dimensional Kd-tree;

FIG. 3 is a schematic diagram illustrating an assembly environment tracking registration matrix solution;

fig. 4 represents the error metric: (a) the euclidean distance between points (Point-to-Point) (b) the distance between points and Tangent Plane (Point-to-finger Plane);

FIG. 5 illustrates a camera pose estimation loop problem during a tracking registration process;

fig. 6 shows a pose graph structure of inter-frame constraint.

Detailed Description

The technical solutions in the embodiments of the present invention will be described in detail and clearly with reference to the accompanying drawings. The described embodiments are only some of the embodiments of the present invention.

The technical scheme for solving the technical problems is as follows:

the invention establishes an assembly environment point cloud based on the depth image. According to the characteristics of light reflection and lack of textures of an assembly environment, an RGBD-ICP virtual-real registration method combining point cloud and visual characteristics is designed, and a primary condition is provided for generation of a virtual-real fusion scene.

Basic idea and workflow of algorithm

The basic idea and workflow of the algorithm are shown in fig. 1. The whole process is completed in an off-line stage and an on-line stage, and in the off-line stage, a three-dimensional point cloud model is generated by a three-dimensional CAD model of the reference object part in a CAD environment, so that preparation is made for establishing an assembly absolute coordinate system and subsequent virtual and real registration. In order to accelerate the process of point cloud registration and coordinate system building, the point cloud model of the reference object part needs to be simplified at this stage. In the online stage, an assembly site video stream is collected and a point cloud is generated, and the definition of an assembly absolute coordinate system is completed by utilizing the registration between a reference object point cloud and the assembly site point cloud. The virtual and real registration based on the point cloud needs an initial registration stage in the iterative registration process, the relative position of the initial point cloud is reasonable, the iteration times can be reduced, the registration time can be reduced, and the algorithm is prevented from falling into local optimization. The RGBD-ICP registration process is initialized by matching based on the characteristic points, so that virtual and real registration matrix calculation is completed. And according to the virtual and real registration matrix, superposing the three-dimensional model of the part to the assembly environment, and realizing registration of the virtual guide information in the assembly environment.

The invention provides a virtual and real registration technology based on point cloud. The invention mainly comprises the following steps:

1. generating and preprocessing an assembly environment point cloud:

1) point cloud generation:

after the depth image of the assembly environment is subjected to noise reduction and restoration, each pixel point u at the moment i in the image_D＝(x_d,y_d) The back projection to the depth camera spatial coordinate system can be done by equation (1). In order to accelerate the process, each pixel point u in the depth image is calculated by using a CUDA parallel computing platform of NVIDIA company_DParallel back-projection is performed to the camera coordinate space,the vertex (vertex map) is obtained, and the back projection result is:

V_t(u)＝D_i(u)M_{int_D} ^-1[u_D,1] (3)

in the formula:

D_i(u) -the depth image at time i;

M_{int_D}-depth camera internal parameters.

Meanwhile, each vertex corresponds to a normal vector N_t(u) using the neighboring proxels by the following operation:

N_t(u)＝(V_t(x+1,y)-V_t(x,y))×(V_t(x,y+1)-V_t(x,y)) (4)

normalizing (2) to a unit length N_t/||N_t||₂. Three-dimensional vertex V_t(u) and the normal vector N corresponding to the vertex_t(u) forming a three-dimensional assembly environment point cloud.

2) Establishing a point cloud space index:

the point cloud is only a collection of surface points of the target object and does not contain geometric topological relation between the points. In order to more efficiently realize the following operation operations such as midpoint cloud data registration, a spatial relationship between point clouds needs to be established. The common point cloud spatial organization relations include octree, regular grid, K-dimensional tree (Kd-tree), etc., and the invention adopts a spatial index Kd-tree to establish the point cloud data structure.

The Kd-Tree is a binary Tree in which some K-dimensional data is stored. Constructing a Kd-Tree on a K-dimensional data set represents the partitioning of the K-dimensional space formed by the K-dimensional data set. To illustrate the Kd-Tree construction process, assume that there are six two-dimensional data points { (2,3), (5,4), (9,6), (4,7), (8,1), (7,2) }, which are located in two-dimensional space, using two-dimensional data points as an example. In order to effectively find the nearest point, the Kd-tree adopts the concept of divide and conquer method, so that the axes of X and Y can be simply numbered as 0,1, i.e. Split ═ 0, 1.

Step1: a cut point (Split) threshold is determined. Respectively calculating the maximum value of the variance of the data in the X and Y directions of the six two-dimensional data points, wherein the corresponding dimension is the value of a Split domain, so that the value of the Split domain is 0 at first, namely the X-axis direction;

step2: a root Node (Node-data) domain value is determined. Sorting is performed according to data 2,5,9,4,8,7 in the X dimension, wherein the value is 7, so the Node-data field value is (7, 2). The splitting hyperplane of the node is thus the straight line passing through the (7,2) point and perpendicular to Split-0 (X-axis), i.e. X-7;

step3: a left subspace and a right subspace are determined. Dividing the whole space into two parts by dividing a hyperplane X to 7, wherein the part with X less than or equal to 7 is a left subspace and comprises 3 nodes (2,3), (5,4) and (4, 7); the part where X > 7 is the right subspace, containing 2 nodes (9,6) and (8, 1).

Step4: recursion. The Kd-tree is constructed in a recursive process, and the process of repeating the root node for the data in the left subspace and the right subspace can obtain the next level of sub-nodes (5,4) and (9,6) (namely, the root nodes of the left subspace and the right subspace), and further subdivide the space and the data set. This is repeated until only one data point is contained in the space. Kd-trees generated from the six two-dimensional data points are shown in FIG. 2;

the three-dimensional space point cloud Kd-Tree construction is the extension of a two-dimensional situation, for the three-dimensional Kd-Tree, the X axis is selected as a root Node, the Y axis is selected as a child Node of the root Node (Node-data), and the Z axis is selected as a child Node of the child Node, so that the circulation is carried out. The median of the corresponding instance is selected as a dividing point (Split) every time, the dividing point is used as a father node, and the left side and the right side are two sub-trees which are divided.

3) Kd-Tree nearest neighbor search:

the searching of data in the Kd-Tree is also an important link of point cloud registration, and the aim is to retrieve point cloud data which is closest to the query point in the Kd-Tree. In three-dimensional space, two data points p (x)₁,x₂,x₃) And P (P)₁,p₂,p₃) The Euclidean distance (Euclidian distance) between them can be represented by formula (3):

the basic idea of nearest neighbor search is as follows:

step1, perform binary search and generate search path. The point to be queried is quickly searched along the search path to find the nearest approximate point, namely the leaf node.

Step2 backtracking lookup. The leaf node found is not necessarily the closest, and the closest point should be within a circle centered at the query point and passing through the leaf node. In order to find the true nearest neighbor, it is also necessary to search backward along the search path whether there is a data point closer to the query point, and the specific process is described in the literature.

2. And establishing an absolute coordinate system.

Generating a point cloud model P ═ { P) under an absolute coordinate system by using a reference object model₀,p₁,p₂...p_m,n₀,n₁,n₂...n_mIn which p is₀,p₁,...p_mAs the coordinates of the vertices of the model surface, n₀,n₁,...n_mThe normal vector corresponding to the vertex. The shell of the water pump is taken as an object,

in the assembly process, an assembly site depth image R at t moment is acquired on line_t(u) (where u is (x, y)), and for depth image R_t(u) performing noise reduction restoration by adopting a second chapter method to obtain a depth image D subjected to noise reduction restoration_t(u) then generating a three-dimensional environment point cloud X according to the point cloud generation method, wherein X ═ { X ═ X₀,x₁,x₂...x_n,N₀,N₁,N₂...N_n}. Generating point cloud P ═ { P) by using three-dimensional environment point cloud and reference object model₀,p₁,p₂...p_m,n₀,n₁,n₂...n_mAnd (5) performing registration by using an Iterative Closest Point (ICP), and performing parallel operation by adopting multiple threads of a Graphics Processing Unit (GPU) to improve the registration speed. In order to obtain the corresponding registration points, Kd-tree is used to accelerate the matching search, and Euclidean distance (Euclidean distance) is used as the matching metric (see formula 3), and in order to reduce the number of mismatching points as much as possible, normal vector features are added as an additional constraint condition. MiningBy dot product d_ij＝<m_i,m_j>Measure the normal vector for the candidate point, if d_ijIf the value is greater than a certain threshold psi, the corresponding point is considered, and a weight value of 1 is given, which is expressed as:

by adopting the characteristic measurement methods such as Euclidean distance and normal vector, the number N of corresponding points and the weight w of each pair of corresponding points can be obtained_i. Solving the position relation error matrix by adopting a least square method to determine a transformation matrix between the point cloud X and the point cloud P

And the definition of an initial absolute coordinate system can be completed by solving the rotation matrix R and the translational vector t, so as to prepare for solving the virtual and real registration matrix M under the subsequent absolute coordinate system.

3. A tracking registration method fusing point cloud and visual features.

A schematic diagram of the tracking registration of the point cloud and visual feature fusion is shown in fig. 3. Firstly, collecting an assembly environment color image and a depth image, and then converting a depth measurement value from a two-dimensional image coordinate to a global coordinate space by adopting formulas (1) and (2) to generate an environment point cloud. Meanwhile, the improved ORB (ordered FAST and Rotated BRIEF) operator is used for extracting the feature points in the color image and carrying out continuous frame matching. And mapping the matched ORB characteristic point pairs to a three-dimensional coordinate space by using depth information, and removing outliers by adopting random sample consensus (RANSAC). When the distance between the two points is smaller than the error value, the characteristic point pair is reserved, otherwise, outlier elimination is carried out, and therefore an initial transformation matrix is obtained. Using the transformation matrix and correct matching point pairs obtained by calculation as an initial data set for ICP point cloud registrationThe ICP iterative process is prevented from falling into local optimization, and meanwhile, the convergence rate and the registration accuracy are improved. Optimizing a transformation matrix obtained in a point cloud registration stage by utilizing a loop detection and pose optimization strategy, and finally combining the matrix M_initAnd obtaining an accurate tracking registration matrix under an absolute coordinate system.

1) Visual feature extraction and matching:

the key step of tracking and registering of point cloud and visual feature fusion is to solve the transformation relation between environmental point cloud data of the depth sensor under different poses. The iterative closest point algorithm (ICP) is one of the stable and efficient methods that can transform a point cloud data matching problem into an exact solution to solve the minimization of a distance function between data sets. The ICP algorithm is mainly divided into two steps, wherein the first step is to determine a corresponding data point set, and the second step is to solve the transformation relation among point cloud data according to the corresponding point set. The acquisition of the correct initial corresponding data point set is the primary condition of the ICP algorithm, and is the key for avoiding the iterative process of the algorithm from falling into local optimization and ensuring the convergence speed and the registration accuracy. However, when the camera moves rapidly, the ICP registration algorithm is prone to fall into local optimization due to the inability to obtain the initial correspondence between the two frames of point cloud data before and after the ICP registration algorithm. And when the working range of the depth sensor is exceeded (0.15-4 m), the condition of frame loss is easily caused, and the interruption of the tracking registration process is caused. Considering that the depth sensor color image data and the depth image data have a one-to-one correspondence relationship, the initial matching problem of the point cloud can be converted into the matching problem of the color image feature points. At present, many local features such as SIFT, SURF, ORB, BRISK, FREAK and the like are widely applied to the fields of image matching, object recognition and the like. Based on consideration of rapidity and stability of feature point matching, the ORB feature point pairs are adopted as an initial matching data point set. However, due to the lack of sufficient texture features in the mechanical product assembly environment or the surface of the tool, it is difficult for the original ORB matching process to obtain a sufficient number of matching point pairs, and the feature points are relatively concentrated, which is not favorable for ensuring the reliability of the calculation of the rotational translation matrix.

In order to obtain a sufficient number of correct ORB feature point pairs as the ICP initial matching data point set, the present invention needs to improve the original ORB matching process. The original ORB feature matching algorithm adopts FAST as a feature point detector, the improved BRIEF as a feature descriptor, and adopts BF (Brute force) pattern matching algorithm to match the feature descriptors. And (5) adopting the original ORB to extract the features and adopting BF to match the descriptors. It can be seen that the original ORB can obtain a sufficient number of feature points (9466), but there are fewer pairs of matching points (252) and there are more mismatches.

In order to eliminate the mismatch, random sample consensus (RANSAC) algorithm is often adopted to eliminate the mismatch. After RANSAC is adopted, only 87 matching point pairs can be obtained finally, the number of the matching point pairs is further reduced, and the ICP initial registration requirement is difficult to meet.

In order to obtain a sufficient number of correct matching point pairs, the invention is inspired by the idea of a grid-based motion statistics (GMS) method, namely matching points with smooth consistency of motion are unlikely to occur randomly, and the discrimination of the matching point pairs to be true or false can be realized by simply counting the surrounding matching points. The OBR feature matching process is improved based on the thought, the invention is called OGMS, the algorithm thought is based on the following assumptions:

suppose that: matching point pairs within the neighborhood around the correct matching point pair should have the same unit direction vector.

Setting M as a matrix formed by the unit direction vectors of connecting lines of matching point pairs in a neighborhood of a certain correct matching point pair:

M＝[m₁ m₂ m₃...m_n]^T (8)

direction vector m_iAnd m_jSimilarity of (D) is expressed by inner product d_ijRepresents:

d_ij＝<m_i,m_j> (9)

direction vector m_i(1×2)And matching point pair matrix

The similarity of the vectors in all directions is represented by G:

G＝|m_iM^T|/(n-1) (10)

and arranging all elements in the G in a descending order, if a certain element in the G is smaller than a threshold value delta, indicating that the connecting line direction of the corresponding matching point pair deviates from the connecting line direction of most matching point pairs, taking the matching point pair as mismatching and removing the mismatching, and accelerating the matching process by adopting grid division in the GMS.

And performing feature matching results by using the improved ORB. By carrying out feature extraction and matching on two adjacent video frames in an assembly environment through the improved ORB, the number of correct matching points is changed from 87 to 3478, and the process is less time-consuming. The visual characteristic matching pseudo code is shown in table 1.

TABLE 1 feature matching Algorithm pseudocode

2) And (3) ICP point cloud registration of fused visual features:

in order to accurately overlay the virtual assembly guide information in the assembly environment, a tracking registration matrix in an absolute coordinate system needs to be acquired in real time. The part is used for solving the position and the posture of the current visual angle through ICP registration of point cloud data among different depth images, namely a rotation translation transformation relation matrix among corresponding point cloud data. In order to avoid that an ICP iterative process is trapped in local optimization, improve convergence speed and registration accuracy and simultaneously avoid the problem of tracking failure caused by rapid movement of a camera, the initial matching problem of point cloud is converted into the matching of color image feature points by fusing visual features in a color image and utilizing the one-to-one correspondence relationship between color image data and depth image data, and an initial transformation matrix and a correct matching point pair obtained by calculation are used as an initial data set for ICP point cloud registration, the process is called RGBD-ICP point cloud registration, and pseudo codes of an RGBD-ICP point cloud registration algorithm are shown in a table 2.

TABLE 2 RGBD-ICP Point cloud registration algorithm pseudo code

Firstly, respectively extracting ORB (organized FAST and Rotated BRIEF) feature point set F from two adjacent video frame color images in assembly environment_a,F_bObtaining a feature point matching point pair set C by an improved ORB feature point matching algorithm_s,C_t. Since the ORB features are extracted from the image, it indicates that there is similarity between the feature vectors in two-dimensional space, but this does not indicate that there are still matching point pairs in three-dimensional space, because the change of the viewing angle causes the change of the spatial position. If the mismatched point pairs are not removed in advance but participate in the calculation of the transformation matrix, the point cloud transformation matrix is updated iteratively by using the initial transformation matrix obtained by calculating the mismatched point pairs, so that an error accumulation is formed. Therefore, the invention firstly maps the matching point pairs to the three-dimensional space by the internal parameters of the depth sensor through the depth images corresponding to the two frames of color images to form a spatial point cloud set P_s,P_t. Then, three-dimensional mismatching point pairs are removed by using a random sample consensus (RANSAC) method to respectively form an interior point set P_{s_inliner},P_{t_inliner}And simultaneously obtaining an initial point cloud transformation matrix M ' [ [ R ' | t ']。

The ICP main loop is entered from step 5. In the first iteration, the initial transformation array M ' calculated by matching the ORB characteristic point pairs is [ R ' | t ' ], so that nearest neighbor initial registration of the point cloud between two adjacent frames without any prior pose knowledge is realized. In order to realize further accurate registration of the point clouds, the point cloud registration problem is converted into an accurate solution problem for solving minimization of an error function between point cloud sets. And taking the matching point pair which satisfies the minimum error function among the assembly environment data point clouds as the input quantity of the algorithm. The data among the images with different visual angles can be regarded as rotational and translational motion among a plurality of coordinate systems, so that the data can be updated once the rotational and translational motion is performed, the input data is updated through repeated iteration transformation arrays, the offset error function among the environmental point cloud data is minimized, and iteration is continuously updated until the error meets certain precision requirements. The invention divides an error function into two parts, wherein the first part calculates the average Euclidean distance between ORB visual characteristic matching Point pairs, and the second part calculates the distance from a current Point in Point cloud to a target matching Point Tangent Plane (Point-to-finger Plane). Compared with the classical point-to-point matching method, the point cloud matching method from the point to the tangent plane can generate more accurate registration effect, and the iterative convergence speed is obviously accelerated. The error function of the present invention is defined as:

in the formula:

α -is the weight value;

-as target points in a dense point cloud

The normal vector of (a) is obtained by cross-multiplying its peripheral neighborhood points.

To accelerate the ICP registration process, the nearest point search is accelerated using a Kd-tree, while employing a Graphics Processor (GPU) in parallel. And when the error change of the point cloud transformation matrix is smaller than a threshold value delta or the maximum iteration number is reached, the ICP main loop stops iteration, otherwise, the point cloud is re-registered by using the transformation matrix calculated at the last time.

3) Loop detection and global camera pose optimization:

the above-adopted continuous adjacent frame-to-frame point cloud registration is a better short-distance tracking registration method. However, registration errors between two adjacent frames can cause the estimation of the camera pose to drift with the moving distance, thereby causing the tracking registration result to be inaccurate. This drift phenomenon is most pronounced, particularly when the camera moves a long distance, eventually returning to a previously visited position, which is known as the loop back problem. In order to solve the above problem, loop back detection is required to recognize whether the camera returns to a previously visited position.

The above examples are to be construed as merely illustrative and not limitative of the remainder of the disclosure. After reading the description of the invention, the skilled person can make various changes or modifications to the invention, and these equivalent changes and modifications also fall into the scope of the invention defined by the claims.

Claims (4)

1. A virtual and real registration method based on point cloud and visual feature fusion is characterized in that the whole process is completed in an off-line stage and an on-line stage, and specifically comprises the following steps:

in an off-line stage, generating a three-dimensional point cloud model of a reference object part three-dimensional CAD model in a CAD environment to prepare for establishing an assembly absolute coordinate system and subsequent virtual and real registration;

in an online stage, collecting assembly field video stream and generating point cloud, completing the definition of an assembly absolute coordinate system by utilizing the registration between reference object point cloud and assembly field point cloud, setting the relative position of the initial point cloud in an iterative registration process based on the virtual and real registration of the point cloud, and obtaining an accurate tracking registration matrix under the assembly absolute coordinate system;

in the online stage, the assembly site point cloud generation and pretreatment specifically comprise:

1) point cloud generation: for each pixel point u in the depth image_DPerforming parallel back projection to a camera coordinate space to obtain a vertex and a three-dimensional vertex V_t(u) and the normal vector N corresponding to the vertex_t(u) forming an assembly site point cloud;

2) point cloud space index establishing: establishing a point cloud data structure by adopting a spatial index Kd-tree;

3) a Kd-tree nearest neighbor searching step;

the assembly absolute coordinate system establishing process specifically comprises the following steps:

step1, generating a point cloud by taking the water pump shell as an object;

step2, acquiring depth image R of assembly site at t moment on line_t(u), where u is (x, y), and for depth image R_t(u) performing noise reduction restoration to obtain a noise-reduced and restored depth image, and then generating an assembly site point cloud, wherein X is { X ═ X }₀,x₁,x₂...x_n,N₀,N₁,N₂...N_n}

Step3, the point cloud P of the assembly site and the point cloud generated by the reference object model is { P }₀,p₁,p₂...p_m,n₀,n₁,n₂...n_mUsing iterative closest point to make registration, adopting multithread of graphic processor to make parallel operation to raise registration speed, in order to obtain correspondent registration point utilizing Kd-tree to accelerate matching search, using Euclidean distance as matching measure and adding normal vector feature as additional limiting condition to reduce number of error matching points as far as possible, adopting dot product d_ij＝<m_i,m_j>Measure the normal vector for the candidate point, if d_ijIf the value is larger than a certain threshold psi, the point is regarded as a corresponding point, and a weight value of 1 is given;

step4, obtaining the number N of corresponding points and the weight w of each pair of corresponding points by adopting a characteristic measurement method including Euclidean distance and normal vectors_iSolving the position relation error matrix by adopting a least square method, and determining a transformation matrix between the point cloud X and the point cloud P;

step5, the definition of an initial absolute coordinate system of assembly can be completed through the solution of the rotation matrix R and the translational vector t, and preparation is made for the solution of a virtual and real registration matrix M under a subsequent absolute coordinate system;

the tracking registration matrix fused by the point cloud and the visual features specifically comprises the following steps:

firstly, collecting a color image and a depth image of an assembly environment, and then converting a depth measurement value from a two-dimensional image coordinate to a global coordinate space to generate an assembly site point cloud; meanwhile, extracting feature points in the color image by using an improved feature description operator ORB and performing continuous inter-frame matching; mapping the matched ORB characteristic point pair to a three-dimensional coordinate space by using depth information, removing outliers by adopting random sampling consistency, keeping the characteristic point pair when the distance between the two points is less than an error value, and removing the outliers to obtain an initial transformation matrix; and taking the transformation matrix and the correct matching point pair obtained by calculation as an initial data set for ICP point cloud registration, optimizing the transformation matrix obtained in the point cloud registration stage by using loop detection and pose optimization strategies, and finally obtaining an accurate tracking registration matrix in an absolute coordinate system by combining the matrix M.

2. The virtual-real registration method based on point cloud and visual feature fusion of claim 1, wherein in the step 1) of point cloud generation, each pixel point u at time i in the image is subjected to noise reduction and restoration in the depth image of the assembly environment_D＝(x_d,y_d) Back-projecting to a depth camera space coordinate system by the formula (1), and accelerating the process to each pixel point u in the depth image_DPerforming parallel back projection to a camera coordinate space to obtain a vertex, wherein the back projection result is as follows:

the formula (1) is: v_t(u)＝D_i(u)M_{int_D} ^-1[u_D,1]

In the formula: d_i(u) -the depth image at time i; m_{int_D}-depth camera internal parameters;

meanwhile, each vertex corresponds to a normal vector N_t(u) using the neighboring proxels by the following operation:

the formula (2) is: n is a radical of_t(u)＝(V_t(x+1,y)-V_t(x,y))×(V_t(x,y+1)-V_t(x,y))

V_t(x +1, y) is the vertex corresponding to the pixel, and x and y represent the horizontal and vertical coordinates of the pixel;

normalizing formula (2) to unit length N_t/||N_t||₂，||N_t||₂、N_tRepresenting the corresponding time normal vector three-dimensional vertex V_t(u) and the normal vector N corresponding to the vertex_t(u) forming an assembly field point cloud.

3. The virtual-real registration method based on point cloud and visual feature fusion of claim 1, wherein the 2) point cloud spatial index establishing step comprises: establishing a point cloud data structure by adopting a spatial index Kd-tree, which specifically comprises the following steps:

step1: determining a Split point Split threshold value;

step2: determining a Node-data domain value of a root Node;

step3: determining a left subspace and a right subspace;

step4: and (5) recursion step.

4. The virtual-real registration method based on point cloud and visual feature fusion as claimed in claim 1, wherein the 3) Kd-tree nearest neighbor search step has the basic idea:

step1, carrying out binary search to generate a search path, and carrying out quick search on the point to be inquired along the search path to find the nearest approximate point, namely the leaf node;

step2, backtracking search, the found leaf node is not necessarily the nearest, the nearest point should be located in the circle domain which uses the query point as the center and passes through the leaf node, and in order to find the true nearest neighbor, it is also necessary to search backward along the search path whether there is a data point closer to the query point.

Images