An RGBD Camera Coordinate Transformation and Visual Localization Method Based on Stereo Matrix Spheres

Abstract

The present invention discloses an RGBD camera coordinate conversion and visual positioning method based on a three-dimensional matrix of small balls, belonging to the technical field of visual positioning; this method is based on the three-dimensional matrix of small balls to convert the camera coordinate system and the world coordinate system of the RGBD camera, and further realizes the positioning of the target point. The RGB camera is used to encode small balls of different colors, and the depth camera is used to obtain the spherical point cloud of the small balls to determine the position of the ball center, generating a one-to-one mapping relationship of the small balls under the RGBD camera coordinate system and the world coordinate system. According to the position of the target point, the transformation matrix of the local area is solved, or interpolation is performed using the position of the target point in the camera coordinate system for accurate positioning of the target point. The present invention can achieve coordinate conversion of the RGBD camera and accurate positioning of the target point through the matrix of small balls, and has the characteristics of simple operation, high accuracy, and fast speed.

Description

Technical Field

[0001] The present invention relates to a method for coordinate transformation and visual positioning of an RGBD camera based on a three-dimensional matrix of small balls, belonging to the technical field of visual positioning. Background Art

[0002] (1) An RGBD camera is a composite visual sensor that can collect the color image of an object and the depth image corresponding to each pixel. When performing the coordinate transformation from the RGBD camera coordinate system to the world coordinate system, the method of checkerboard calibration is usually used. However, this coordinate transformation method only utilizes color information, and since the homography matrix needs to be used for transformation during the calibration process, the accuracy of the solved external parameter matrix is relatively low.

[0003] (2) When an RGBD camera is used to locate a target point, the global external parameter matrix is usually used for transformation to obtain the two-dimensional coordinates of the target point in the world coordinate system, and then the depth value is combined for positioning. The positioning accuracy obtained by this positioning method is relatively low.

[0004] (3) Therefore, in view of the problems of the coordinate transformation of the current RGBD camera and the target point positioning in the field of visual positioning. The present invention designs a method for coordinate transformation and target point positioning of an RGBD camera based on a three-dimensional matrix of small balls. By encoding the small balls through the color information of the small balls, and solving the position of the ball center through the point cloud on the spherical surface of the small balls, the position of the small balls in the camera coordinate system is obtained. Combining the position of the small balls in the world coordinate system, the coordinate transformation of the RGBD camera and the visual positioning of the target point are realized. Summary of the Invention

[0005] In order to overcome the deficiencies of the prior art, the present invention proposes a method for coordinate transformation and visual positioning of an RGBD camera based on a three-dimensional matrix of small balls. For the case of a single camera, a three-dimensional matrix of small balls is used to realize the transformation between the camera coordinate system and the world coordinate system, and further realize the precise positioning of the target point in the world coordinate system in space and the pose solution of the target object on the constrained plane. For the case of multiple cameras, a three-dimensional matrix of small balls is used to realize the pose transformation solution of multiple cameras. At the same time, the coordinate transformation relationship can be used to optimize the external parameter matrix obtained by the traditional checkerboard calibration method, and correct the relative pose parameters of the RGB camera and the depth camera provided by the RGBD camera.

[0006] The technical solution adopted by the present invention is a method for coordinate transformation and visual positioning of an RGBD camera based on a three-dimensional matrix of small balls, including the following steps:

[0007] Step 1, fabricate a three-dimensional matrix model containing multiple small balls.

[0008] Make a three-dimensional matrix model containing multiple small balls. This model is similar to a lattice model, and small balls of different colors are provided at the corners and centers of each three-dimensional matrix; or the three-dimensional matrix model is made by combining multiple planar small ball models with a single-degree-of-freedom moving platform, and the moving platform moves to generate a three-dimensional matrix model from each planar small ball model; or the three-dimensional matrix model is made by combining multiple linear small ball models with a two-degree-of-freedom moving platform; or the three-dimensional matrix model is made by combining a single small ball model with a three-degree-of-freedom moving platform. The target point is included in the three-dimensional matrix model, and the moving platform can be combined to increase the positioning field of view.

[0009] Step 2, generate the conversion relationship between the camera coordinate system and the world coordinate system.

[0010] Use an RGBD camera to image the three-dimensional matrix model. After processing, obtain the positions of the centers of the small balls in the camera coordinate system. Through the known parameters of the three-dimensional matrix model, obtain the one-to-one mapping relationship between the camera coordinate system and the world coordinate system of the centers of the small balls. The steps to obtain the positions of the centers of each small ball in the camera coordinate system are as follows:

[0011] Step 2.1, use the RGB camera to encode different small balls in the three-dimensional matrix model using color information.

[0012] Step 2.2, use the depth camera to obtain the point cloud of the small balls using depth information, and further calculate the positions of the centers of the small balls.

[0013] Step 3, locate the position of the target point in the world coordinate system in space.

[0014] The steps for locating the target point using the interpolation method include the following:

[0015] Step 3.1.1, use an RGBD camera to image the three-dimensional matrix model and the target point in space.

[0016] Step 3.1.2, obtain the position of the target point in the camera coordinate system, and perform linear interpolation, quadratic interpolation, or cubic interpolation in combination with the positions of the centers of the small balls of the three-dimensional matrix model in the camera coordinate system obtained in Step 2 to obtain the position of the target point in the world coordinate system.

[0017] The steps for locating the target point using the local transformation matrix method include the following:

[0018] Step 3.2.1, use an RGBD camera to image the three-dimensional matrix model and the target point in space.

[0019] Step 3.2.2, divide the three-dimensional matrix model into several regions, and each local region contains several small balls.

[0020] Step 3.2.3: Select the corresponding local area according to the position of the target point.

[0021] Step 3.2.4: Solve the external parameter conversion matrix of the local area according to the corresponding relationship between the camera coordinate system and the world coordinate system of the small balls in the local area obtained in Step 2, and realize the solution of the position of the target point in the world coordinate system.

[0022] Step 4: Locate the target points with constrained planes in space.

[0023] When the target point is constrained to a certain plane in space, the position of the target point in the world coordinate system is solved by arranging plane matrix small balls on the constrained plane.

[0024] Step 4.1: Use the RGBD camera to image the plane matrix small ball model and the target point on the constrained plane.

[0025] Step 4.2: Encode the small balls using color information, solve the position of the center of the small ball in the camera coordinate system using depth information, and obtain the one-to-one mapping relationship between the camera coordinate system and the world coordinate system of the center of the small ball according to the plane small ball model parameters.

[0026] Step 4.3: Solve the position of the target point by interpolation or local external parameter matrix.

[0027] Step 5: Solve the pose of the target object with a constrained plane in space.

[0028] When the target object is constrained to a certain plane in space, the pose of the target object is solved by arranging plane model matrix small balls on the constrained plane to obtain the positions of several target points on the target object.

[0029] Step 5.1: Select several target points on the target object.

[0030] Step 5.2: Obtain the positions of several target points in the world coordinate system according to Step 4.

[0031] Step 5.3: Determine the pose of the target object based on the positions of the target points on the target object and the positions of the target points in the world coordinate system.

[0032] Step 6: Optimize the parameters of the external parameter matrix and the pose parameters of the RGB camera and the depth camera.

[0033] The coordinate conversion relationship obtained through the stereo matrix small ball model combined with the optimization method can optimize the external parameter matrix obtained by the traditional checkerboard calibration method.

[0034] Step 6.1.1: Use the traditional checkerboard calibration method to calibrate the same world coordinate system corresponding to the stereo matrix model to obtain the external parameter conversion matrix.

[0035] Step 6.1.2: Using the correspondence obtained in Step 2 as the optimization objective, the external parameter matrix obtained by the traditional checkerboard calibration method is optimized using the particle swarm optimization algorithm.

[0036] The pose parameters of the RGB camera and the depth camera provided by the RGBD camera are corrected through the correspondence of the same small ball center position under the RGB camera and the depth camera.

[0037] Step 6.2.1: The RGBD camera images the small ball model, and the center coordinates of the ball are obtained by using the Hough circle transform in the RGB image.

[0038] Step 6.2.2: The center coordinates of the ball are solved by using the generated small ball spherical point cloud in the depth image.

[0039] Step 6.2.3: The pose relationship between the RGB camera and the depth camera is solved according to the correspondence of the same small ball center position.

[0040] Step 7: Solve the multi-camera pose transformation matrix.

[0041] For multiple RGBD cameras, by imaging the same small ball model, the pose conversion relationship between multiple cameras is obtained.

[0042] Step 7.1: Multiple RGBD cameras image the same small ball model.

[0043] Step 7.2: The position of the small ball center in the camera coordinate system of each camera is obtained by the method described in Step 2.

[0044] Step 7.3: According to the small ball model, a unified world coordinate system is defined for conversion to obtain the pose conversion relationship between multiple cameras.

[0045] The positive effects of the present invention are as follows:

[0046] 1. The present invention uses a three-dimensional matrix small ball model to realize the coordinate conversion of the RGBD camera. Compared with the traditional checkerboard calibration method, this method is relatively simple to operate, does not require multiple shootings, and can obtain a more accurate conversion relationship.

[0047] 2. When the present invention obtains the position of the target point, the position of the target point in the world coordinate system is interpolated according to the position of the target point in the camera coordinate system combined with the known position of the small ball. This method is more accurate than the method of obtaining the position of the target point through the external parameter conversion matrix.

[0048] 3. The present invention obtains the position of the target point based on the coordinate conversion matrix of the local area near the target point, which is more accurate than the method of solving through the global conversion matrix.

[0049] 4. By using the correspondence relationship of the same small ball model in multiple camera coordinate systems, the pose relationship between multiple cameras is solved without the need for multiple calibrations and external parameter conversions. This method is more convenient, faster, and has higher accuracy.

[0050] 5. For target points with plane constraints, the present invention can achieve dimensionality reduction calculation; for target objects with plane constraints, the pose relationship of the target object is obtained through the positions of multiple target points on the target object. This method is simple to operate and fast.

[0051] 6. The present invention can optimize the external parameter matrix obtained by the traditional checkerboard calibration method in combination with an optimization method. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] The drawings are only for the purpose of showing specific embodiments and are not considered to be a limitation of the present invention. Throughout the drawings, the same reference signs denote the same components.

[0053] Figure 1 is a schematic diagram of a three-dimensional matrix small ball model, which is composed of 27 small balls. Among them, 1 is the center of the small ball 1, which is defined as the origin of the world coordinate system, and 2 is the target point inside the model

[0054] Figure 2 is a schematic diagram of the target point positioning process in the case of a single camera of the present invention.

[0055] Figure 3 is a schematic diagram of the pose solution process in the case of multiple cameras of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0056] To make the objectives, technical solutions, and advantages of the present invention clearer, the preferred embodiments of the present invention will be specifically described below with reference to the drawings. The drawings form a part of this application and are used together with the embodiments of the present invention to explain the principles of the present invention, rather than to limit the scope of the present invention.

[0057] This embodiment discloses an RGBD camera coordinate conversion and visual positioning method based on a three-dimensional matrix small ball, including the following steps:

[0058] For target point positioning in the case of a single camera:

[0059] 1) The RGBD camera captures an image of the model small ball and encodes the small ball using RGB information.

[0060] 2) The depth camera is used to capture the spherical point cloud of the small ball. Optionally, three spherical point cloud points A, B, and C are selected, and the coordinates are (x A , y A , z A ), (x B , y B , zB ), (x C , y C , z C ), the three - dimensional coordinates (x O , y O , z O ) of the center of the small ball are obtained through formula (1).

[0061]

[0062] 3) Take the center of the small ball 1 in the model as the origin of the world coordinate system, specify the spatial coordinate system according to the right - hand rule, and obtain the position of each small ball in the world coordinate system according to the model size, and then obtain the mapping relationship between the center of the small ball and the world coordinate system in one - to - one correspondence in the camera coordinate system.

[0063] 4) For the target points inside the model, linear interpolation, quadratic interpolation or cubic interpolation is performed on the position of the target points in the camera coordinate system according to formulas (2), (3), and (4). R(x) is the coefficient of the interpolation point, and the accurate position of the target points in the specified world coordinate system is obtained.

[0064]

[0065]

[0066]

[0067] 5) Divide the three - dimensional small - ball model into eight regions. Each local region contains eight small balls. The target point is M, and the distances between the target point and the centers of each ball are MO kn . According to formula (5), solve the sum of the distances between the target point and the positions of the centers of the small balls in each region, and determine the region to which the target point belongs.

[0068] |MO k1 + MO k2 +... MO k8 | (k = 1, 2......8) (5)

[0069] 6) According to the coordinates of the centers of the eight small balls in the local region in the camera coordinate system and the world coordinate system, solve the external parameter transformation matrix of the local region, including the rotation matrix R and the translation vector T. (X c , Y c , Z c ) are the coordinates of the target point in the camera coordinate system, and (X w , Y w , Z w ) are the coordinates of the target point in the world coordinate system. The positioning of the target point is realized through formula (6).

[0070]

[0071] For multi-camera pose solution:

[0072] 1) Use two RGBD cameras to collect the same stereo matrix ball model to obtain RGB information and depth information

[0073] 2) Obtain the positions of the balls in the camera coordinate system and the world coordinate system under the two cameras

[0074] 3) Respectively obtain the transformation matrices between the camera coordinate systems and the world coordinate systems of the two cameras, including rotation matrices R1, R2 and translation vectors T1, T2

[0075] 4) For the position P of the center of the same ball, which are P1 and P2 in the camera coordinate systems of the two cameras respectively, the pose transformation matrix of the two cameras, including the rotation matrix R and the translation vector T, can be obtained according to equations (7) and (8)

[0076]

[0077]

Claims

1. An RGBD camera coordinate conversion and visual positioning method based on three-dimensional matrix small balls, characterized in that It includes the following steps: Step 1, fabricate a three-dimensional matrix model containing multiple small balls; Fabricate a three-dimensional matrix model containing multiple small balls, and small balls of different colors are provided at the corners and centers of each three-dimensional matrix; Step 2, generate the conversion relationship between the camera coordinate system and the world coordinate system; Step 2.1, through an RGB camera, encode different small balls in the three-dimensional matrix model using color information; Step 2.2, through a depth camera, obtain the point cloud of the small balls using depth information, and further calculate the positions of the centers of the small balls; Step 3, locate the position of the target point in the world coordinate system in space; Locating the target point using the interpolation method includes the following steps: Step 3.1.1, use an RGBD camera to image the three-dimensional matrix model and the target point in space; Step 3.1.2, obtain the position of the target point in the camera coordinate system, and perform linear interpolation, quadratic interpolation, or cubic interpolation in combination with the positions of the centers of the small balls of the three-dimensional matrix model in the camera coordinate system obtained in Step 2 to obtain the position of the target point in the world coordinate system; Locating the target point using the local transformation matrix method includes the following steps: Step 3.2.1, use an RGBD camera to image the three-dimensional matrix model and the target point in space; Step 3.2.2, divide the three-dimensional matrix model into several regions, and each local region contains several small balls; Step 3.2.3, select the corresponding local region according to the position of the target point; Step 3.2.4, solve the external parameter transformation matrix of the local region according to the corresponding relationship between the camera coordinate system and the world coordinate system of the small balls in the local region obtained in Step 2, and realize the solution of the position of the target point in the world coordinate system; Step 4, locate the target point with a constrained plane in space; When the target point is constrained to a certain plane in space, the position of the target point in the world coordinate system is solved by arranging plane matrix small balls on the constrained plane; Step 4.1, use an RGBD camera to image the plane matrix small ball model and the target point on the constrained plane; Step 4.2, encode the small balls using color information, solve the positions of the centers of the small balls in the camera coordinate system using depth information, and obtain the one-to-one mapping relationship between the camera coordinate system and the world coordinate system of the centers of the small balls according to the parameters of the plane small ball model; Step 4.3, solve the position of the target point through interpolation or local external parameter matrix; Step 5, solve the pose of the target object with a constrained plane in space; When the target object is constrained to a certain plane in space, the pose of the target object is solved by arranging plane model matrix small balls on the constrained plane to obtain the positions of several target points on the target object; Step 6, optimize the parameters of the external parameter matrix and the poses of the RGB camera and the depth camera; The coordinate conversion relationship obtained through the three-dimensional matrix small ball model combined with the optimization method can optimize the external parameter matrix obtained by the traditional checkerboard calibration method; Correct the pose parameters of the RGB camera and the depth camera provided by the RGBD camera according to the corresponding relationship between the positions of the centers of the same small ball under the RGB camera and the depth camera; Step 7, solve the multi-camera pose transformation matrix; For multiple RGBD cameras, by imaging the same small ball model, the pose transformation relationship between multiple cameras is obtained.

2. The RGBD camera coordinate conversion and visual positioning method based on three-dimensional matrix balls according to claim 1, wherein: Locate the center position of the small ball through the depth information of the RGBD camera, and use the positions of points A, B, and C on the spherical point cloud of the small ball to solve for the position of the center O of the ball; according to the position correspondence between the RGB camera and the depth camera, as well as the color coding of the small ball, obtain the position of each small ball in the camera coordinate system, and finally establish a one-to-one correspondence between the world coordinate system and the camera coordinate system.

3. A method for RGBD camera coordinate conversion and visual positioning based on three-dimensional matrix balls according to claim 1, characterized in that: Using the positions of the small ball in the camera coordinate system and the world coordinate system, when positioning the target point, through linear interpolation, quadratic interpolation, or cubic interpolation of the position of the target point in the camera coordinate system, obtain the accurate position of the target point in the world coordinate system.

4. A method for RGBD camera coordinate conversion and visual positioning based on three-dimensional matrix balls according to claim 1, characterized in that: The space is divided into n regions by three-dimensional matrix small balls. Each region consists of several small balls, and each local region consists of m small balls. Denote the center positions of the balls in the camera coordinate system as O 11 , O 12 , O 13 ...O 1m ...O k1 , O k2 , O k3 …O km . When positioning the target point A, by comparing the sum of the distances between the target point in the camera coordinate system and the centers of all the balls in each local region |AO k1 + AO k2 + …AO km |, select the region with the minimum sum of distance values as the local region; when positioning the target point, obtain the coordinate transformation matrix or external parameter matrix in the local region through the corresponding relationship between the camera coordinate systems of the n small balls in the local region and the world coordinate system, and then obtain the position of the target point in the world coordinate system according to the position of the target point in the camera coordinate system.

5. A method for RGBD camera coordinate conversion and visual positioning based on three-dimensional matrix balls according to claim 4, characterized in that: For the case of multiple cameras, obtain the pose transformation relationship between multiple cameras, including the rotation matrix R and the translation vector T, through the correspondence between the camera coordinate system and the world coordinate system of the same stereo matrix small ball obtained by multiple cameras.

6. A method for RGBD camera coordinate conversion and visual positioning based on three-dimensional matrix small balls according to claim 4, characterized in that: According to the correspondence between the camera coordinate system and the world coordinate system of the obtained stereo matrix small ball, optimize the external parameter matrix obtained by checkerboard calibration through the gradient descent method, Newton's method, or heuristic optimization method.

7. A method for coordinate conversion and visual positioning of an RGBD camera based on three-dimensional matrix small balls according to claim 1, characterized in that: For the target point with a constraint plane, perform dimensionality reduction operations by arranging a plane matrix small ball model on the constraint plane, and simplify it by solving the transformation matrix from the camera coordinate system to the world coordinate system or obtaining the position of the target point through interpolation.

8. A method for RGBD camera coordinate conversion and visual positioning based on three-dimensional matrix balls according to claim 1, characterized in that: For the target object with a constraint plane, arrange a plane small ball model on the constraint plane, and solve the pose of the target object according to the positions of multiple target points on the target object.

9. A method for RGBD camera coordinate transformation and visual positioning based on three-dimensional matrix small balls according to claim 1, characterized in that: When using the RGBD camera to obtain depth information, solve the problem of unstable measurement of the RGBD camera by taking the average value of the depth through multiple measurements.

10. A method for RGBD camera coordinate conversion and visual positioning based on three-dimensional matrix small balls according to claim 1, characterized in that: Based on the image processing method, obtain the center position of the small ball from the image collected by the RGB camera, and obtain the center position of the small ball through the depth camera; use the positions of the center of the same small ball in the RGB camera and the depth camera to correct the relative pose parameters of the RGB camera and the depth camera provided by the RGBD camera.

Images