A method for constructing semi-dense point cloud maps based on monocular cameras

By applying the semi-dense point cloud map construction method with ORB-SLAM2 and RAFT optical flow method on a monocular camera, the problem of lighting impact and high cost when building a dense point cloud map of a monocular camera is solved, and high-quality semi-dense is achieved Point cloud map construction and global map stitching, suitable for robot and drone navigation.

CN114677484BActive Publication Date: 2024-11-08NANJING UNIV OF POSTS & TELECOMM
View PDF 0 Cites 1 Cited by

Patent Information

Application Number
CN202210253256.8
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-03-15
Publication Date
2024-11-08
Estimated Expiration
2042-03-15

AI Technical Summary

Technical Problem

Existing monocular cameras are susceptible to light when building dense point cloud maps, resulting in map construction deviations, and are costly. The amount of information about dense maps is insufficient, affecting robot and drone navigation.

Method used

Using the semi-dense point cloud map construction method based on the ORB-SLAM2 algorithm and RAFT optical flow method, the semi-dense point cloud map model is constructed through steps S1-S9, and applied it to the global map construction under the world coordinate system to reduce costs and Reduce the impact of light.

Benefits of technology

It realizes the construction of high-quality semi-dense point cloud maps under monocular camera conditions, which reduces costs, reduces lighting impact, and improves the accuracy and information of the map. It is suitable for robot and drone navigation.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN114677484B_ABST
    Figure CN114677484B_ABST
Patent Text Reader

Abstract

The present invention discloses a method for constructing a semi-dense point cloud map based on a monocular camera. The method comprises a key frame image discrimination module, a matching image block extraction module, a homography matrix calculation module, a feature point correspondence calculation module, a triangulation calculation module, a three-dimensional space point angle calculation module, a camera coordinate system semi-dense point cloud map module, and a global map module. The method takes an image sequence acquired by a monocular camera and arranged in time sequence as input and a global map as output to construct a semi-dense point cloud map model. The semi-dense point cloud map model is applied to complete the construction of a global map in a world coordinate system. The semi-dense point cloud map model constructed by the method is based on a monocular camera, has a low cost compared to a binocular camera, is not easily affected by illumination, and the constructed map is not prone to deviation.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of image processing, and in particular to a method for constructing a semi-dense point cloud map based on a monocular camera. Background Art

[0002] Dense point cloud maps have important applications in areas such as robot movement and unmanned driving. In the construction of traditional dense and semi-dense point cloud maps, RGB-D cameras or binocular cameras are generally used, which are more expensive than monocular cameras. In the construction of monocular camera maps, there are often more sparse maps and less map information, which is not conducive to the navigation of robots or drones. Existing monocular cameras usually use direct methods or traditional optical flow methods to construct dense maps, which are particularly susceptible to lighting, resulting in deviations in map construction. Summary of the invention

[0003] The purpose of the present invention is to provide a method for constructing a semi-dense point cloud map based on a monocular camera, by constructing a semi-dense point cloud map model and applying the semi-dense point cloud map model to complete the construction of a global map in a world coordinate system.

[0004] To achieve the above functions, the present invention designs a semi-dense point cloud map construction method based on a monocular camera, executes steps S1-S9 according to a preset period, obtains a semi-dense point cloud map model, and then applies the semi-dense point cloud map model to complete the construction of a global map of a preset target scene in a world coordinate system;

[0005] S1. For a preset target scene, a monocular camera moving along a preset trajectory acquires an image of the preset target scene, and takes the image sequence acquired by the monocular camera and arranged in time sequence as input, and discriminates each key frame image in the image sequence based on the ORB-SLAM2 algorithm, and constructs a key frame image set arranged in time sequence, and takes the transformation matrix corresponding to the key frame image set and each key frame image as output, and constructs a key frame image discrimination module;

[0006] S2. Take a key frame image from the key frame image set The second key frame image after Key frame image The reference frame image of the key frame image set, where i is the number of each key frame image in the key frame image set, t iAt the moment of the i-th key frame image in the key frame image set, each key frame image and the reference frame image corresponding to each key frame image are used as input, and based on the ORB-SLAM2 algorithm, ORB feature point extraction and matching are performed for each key frame image and each key frame image and its corresponding reference frame image, and matching image blocks are constructed according to the pixel positions of each successfully matched feature point, and each matching image block is used as output to construct a matching image block extraction module;

[0007] S3. Taking the matching image block as input, based on the three-dimensional space points corresponding to the successfully matched feature points in the matching image block, according to the homography matrix calculation, taking the rotation matrix and translation vector between each key frame image and its corresponding reference frame image as output, construct a homography matrix calculation module;

[0008] S4. Taking the matching image blocks output by the matching image block extraction module as input, based on the pixel positions of the successfully matched feature points in the matching image blocks, using the RAFT streamer method, calculate the horizontal coordinate offset and the vertical coordinate offset of the pixel positions of the successfully matched feature points in each key frame image relative to the pixel positions of the successfully matched feature points in each corresponding reference frame image, and taking the matrix composed of the horizontal coordinate offset and the vertical coordinate offset as output, construct a feature point correspondence calculation module;

[0009] S5. Taking the rotation matrix and translation vector between each key frame image and its corresponding reference frame image output by the homography matrix calculation module as input, based on the triangulation algorithm, taking the position and depth of each three-dimensional space point mapped in the normalized plane of each key frame image, and the position and depth of the three-dimensional space point mapped in the normalized plane of each reference frame image as output, a triangulation calculation module is constructed;

[0010] S6. Based on the height value of the preset scene corresponding to each three-dimensional space point, the position of the three-dimensional space point whose height value is less than the preset upper limit value and greater than the preset lower limit value mapped in the normalized plane of each key frame image and the position of each three-dimensional space point mapped in the normalized plane of each reference frame image are used as input, based on the angle calculation of the vector from the position of each three-dimensional space point mapped in the normalized plane of each key frame image to the optical center of each key frame image and the vector from the position of each three-dimensional space point mapped in the normalized plane of each reference frame image to the optical center of each reference frame image, the angle higher than the preset lower limit threshold is used as output, and a three-dimensional space point angle calculation module is constructed;

[0011] S7. Taking the position and depth of the three-dimensional space point mapping corresponding to each angle output by the three-dimensional space point angle calculation module in the normalized plane of each key frame image as input, and taking the semi-dense point cloud map corresponding to each matching image block in the camera coordinate system as output, a camera coordinate system semi-dense point cloud map module is constructed;

[0012] S8. Taking the semi-dense point cloud map corresponding to each matching image block in the camera coordinate system as input, based on the transformation matrix corresponding to each key frame image output by the key frame image discrimination module, respectively calculate the semi-dense point cloud map in the world coordinate system, splice the semi-dense point cloud maps in each world coordinate system to form a global map, take the global map as output, and construct a global map module;

[0013] S9. Based on the key frame image discrimination module, matching image block extraction module, homography matrix calculation module, feature point correspondence calculation module, triangulation calculation module, three-dimensional space point angle calculation module, camera coordinate system semi-dense point cloud map module, and global map module, a semi-dense point cloud map model is constructed with the time-series image sequence acquired by the monocular camera as input and the global map as output.

[0014] As a preferred technical solution of the present invention: the key frame image set in step S2 is: in is each key frame image in the key frame image set, i is the number of each key frame image in the key frame image set, t i is the moment of the i-th key frame image in the key frame image set, N K is the total number of key frame images in the key frame image set; and its corresponding reference frame image Perform ORB feature point extraction and matching, and record the pixel position of each successfully matched feature point as follows:

[0015]

[0016] In the formula, is the set of pixel positions of each feature point that is successfully matched, Key frame images The pixel horizontal and vertical coordinates of each feature point that is successfully matched; The reference frame images The pixel horizontal and vertical coordinates of each feature point that is successfully matched, t i The set of logarithms of each feature point successfully matched in the key frame image at the moment and the reference frame image;

[0017] For each key frame image The pixel positions of each feature point that is successfully matched are listed in the range By row range Construct a matching image block consisting of each successfully matched feature point.

[0018] As a preferred technical solution of the present invention: in step S3, the matching image block is used as input, based on the three-dimensional space points corresponding to the successfully matched feature points in the matching image block, according to the homography matrix calculation, the rotation matrix and translation vector between each key frame image and its corresponding reference frame image are used as output, and the specific steps of constructing the homography matrix calculation module are as follows:

[0019] S31: Normalize each successfully matched feature point in the matching image block, randomly select 8 pairs of successfully matched feature points, and solve the homography matrix based on the eight-point method. The solution method is as follows:

[0020] p 1 =Hp 2

[0021] In the formula, p 1 is the feature point in the key frame image, p 2 are the feature points in the reference frame image that match the feature points in the key frame image, and H is the homography matrix;

[0022] S32: Based on the Faugeras SVD decomposition method, the homography matrix is ​​decomposed into a rotation matrix and a translation vector. The specific method is as follows:

[0023]

[0024] Where D is the distance from the camera's far point to the plane where the three-dimensional space point is located, R is the rotation matrix, is the translation vector, n is the normal vector of the plane where the three-dimensional space point is located, U is an m×m matrix, where m is the number of rows of the homography matrix H, ∧ is an m×n matrix, all elements except the elements on the diagonal are 0, and each element on the main diagonal is called a singular value, where n is the number of columns of the homography matrix H, and V is an n×n matrix.

[0025] As a preferred technical solution of the present invention: in step S4, each matching image block output by the matching image block extraction module is used as input, and based on the pixel position of each successfully matched feature point in the matching image block, the RAFT streamer method is used to calculate the horizontal coordinate offset and the vertical coordinate offset of the pixel position of each successfully matched feature point in each key frame image relative to the pixel position of each successfully matched feature point in each reference frame image, and the matrix composed of each horizontal coordinate offset and the vertical coordinate offset is used as output to construct the feature point correspondence calculation module. The specific steps are as follows:

[0026] S41: For the pixel positions of each successfully matched feature point in each key frame image, the RAFT streamer method is used to calculate the horizontal axis displacement u of the pixel positions of each successfully matched feature point in each key frame image relative to the pixel positions of each successfully matched feature point in each reference frame image. (I,J) , longitudinal axis displacement v (I,J) , where I is the pixel horizontal coordinate of each successfully matched feature point in the key frame image, and J is the pixel vertical coordinate of each successfully matched feature point in the key frame image;

[0027] S42: Based on the pixel position (I, J) of each successfully matched feature point in the key frame image, the horizontal axis displacement u of the pixel position of each successfully matched feature point in each key frame image relative to the pixel position of each successfully matched feature point in each reference frame image (I,J) , longitudinal axis displacement v (I,J) , the pixel position (ref_i, ref_j) of each successfully matched feature point in the reference frame image is obtained according to the following formula:

[0028] ref_i=I+u (I,J)

[0029] ref_j=J+v (I,J)

[0030] Where ref_i is the pixel horizontal coordinate of the successfully matched feature point in the reference frame image, and ref_j is the pixel vertical coordinate of the successfully matched feature point in the reference frame image.

[0031] As a preferred technical solution of the present invention: the specific calculation method of the triangulation algorithm in step S5 is as follows:

[0032] z r p r =T rw P w

[0033] z k p k =T kw P w

[0034] T rw =[R rw |t rw ]

[0035] T kw =[R kw |t kw ]

[0036] Where P w =[X,Y,Z,1] Tis a three-dimensional space point, where X, Y, and Z are the coordinates of the three-dimensional space point in the preset scene, and T rw =[R rw |t rw ] is the camera pose of the reference frame image, R rw represents the rotation matrix from the world coordinate system to the reference frame image coordinate system, t rw Represents the translation vector from the world coordinate system to the reference frame image coordinate system, T kw =[R kw |t kw ] is the camera pose of the key frame image, R kw Represents the rotation matrix from the world coordinate system to the key frame image coordinate system, t kw Represents the translation vector from the world coordinate system to the key frame image coordinate system, the three-dimensional space point P w The position of the mapping in the normalized plane of the reference frame image is where x r ,y r 、z r They are three-dimensional space points P w The horizontal coordinate, vertical coordinate, depth, and three-dimensional space point P mapped in the normalized plane of the reference frame image w The position of the mapping in the normalized plane of the key frame image is where x k ,y k 、z k They are three-dimensional space points P w The horizontal coordinate, vertical coordinate, and depth mapped in the normalized plane of the keyframe image.

[0037] As a preferred technical solution of the present invention: in step S6, the angle between the vector of the position of each three-dimensional space point mapped in the normalized plane of each key frame image to the optical center of each key frame image and the vector of the position of each three-dimensional space point mapped in the normalized plane of each reference frame image to the optical center of each reference frame image is calculated as follows:

[0038] a·b=|a||b|cosθ

[0039] Where a is the vector from the position of the three-dimensional space point mapped in the normalized plane of the key frame image to the optical center of the key frame image, b is the vector from the position of the three-dimensional space point mapped in the normalized plane of the reference frame image to the optical center of the reference frame image, and θ is the angle between the two vectors.

[0040] As a preferred technical solution of the present invention: in step S6, the preset upper limit value of the height value of the preset scene corresponding to each three-dimensional space point is 5, and the preset lower limit value is 0.

[0041] As a preferred technical solution of the present invention: in step S7, based on the position and depth of the three-dimensional space point mapping corresponding to each angle output by the three-dimensional space point angle calculation module in the normalized plane of each key frame image, the method for calculating the semi-dense point cloud map corresponding to each matching image block in the camera coordinate system is as follows:

[0042] zu=x·f x +c x

[0043] zv=y·f y +c y

[0044] d=z·s

[0045] Where x, y, and z are the coordinates of the three-dimensional space point in the camera coordinate system, u, v, and d are the pixel horizontal coordinate, vertical coordinate, and depth of the three-dimensional space point in the pixel coordinate system, respectively. x 、f y is the focal length of the camera on the x-axis and y-axis respectively, c x 、c y is the aperture center, and s is the scaling factor of the depth map.

[0046] As a preferred technical solution of the present invention: in step S8, based on the transformation matrix corresponding to each key frame image output by the key frame image discrimination module, the method for calculating the semi-dense point cloud map in the world coordinate system is as follows:

[0047]

[0048] Where T is the transformation matrix, X W , Y W , Z W is the coordinate of the three-dimensional space point in the world coordinate system.

[0049] Beneficial effects: Compared with the prior art, the advantages of the present invention include:

[0050] The present invention designs a semi-dense point cloud map construction method based on a monocular camera. The semi-dense point cloud map model constructed by the method is based on a monocular camera, which has low cost compared to RGB-D cameras and binocular cameras commonly used in the prior art. The RAFT optical flow method is used in the image processing process, which is not easily affected by lighting compared to the direct method or traditional optical flow method commonly used in the prior art, and the constructed map is not prone to deviation. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 is a flow chart of a method for constructing a semi-dense point cloud map based on a monocular camera according to an embodiment of the present invention;

[0052] Figure 2 It is a semi-dense point cloud map result of an office scene according to the semi-dense point cloud map construction method based on a monocular camera provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0053] The present invention will be further described below in conjunction with the accompanying drawings. The following embodiments are only used to more clearly illustrate the technical solution of the present invention, and are not intended to limit the scope of protection of the present invention. Figure 1 , a method for constructing a semi-dense point cloud map based on a monocular camera provided in an embodiment of the present invention, executes steps S1-S9 according to a preset period to obtain a semi-dense point cloud map model, and then applies the semi-dense point cloud map model to complete the construction of a global map of a preset target scene in a world coordinate system;

[0054] S1. For a preset target scene, a monocular camera moving along a preset trajectory acquires an image of the preset target scene, and takes the image sequence acquired by the monocular camera and arranged in time sequence as input, and discriminates each key frame image in the image sequence based on the ORB-SLAM2 algorithm, and constructs a key frame image set arranged in time sequence, and takes the transformation matrix corresponding to the key frame image set and each key frame image as output, and constructs a key frame image discrimination module;

[0055] S2. Take a key frame image from the key frame image set The second key frame image after Key frame image The reference frame image of the key frame image set, where i is the number of each key frame image in the key frame image set, t i The method is to extract and match ORB feature points of each key frame image and its corresponding reference frame image at the moment of the i-th key frame image in the key frame image set. Based on the ORB-SLAM2 algorithm, the ORB feature points are extracted and matched for each key frame image and its corresponding reference frame image. According to the pixel positions of each successfully matched feature point, a matching image block is constructed. With each matching image block as output, a matching image block extraction module is constructed.

[0056] In one embodiment, an ORB-SLAM2 thread is used to extract a key frame image set. When a preset target scene changes significantly or the number of ORB feature points is less than a preset lower limit, a new key frame image is inserted, and the key frame image set is screened to remove key frame images with reduced co-visual depth. When a key frame image is the last two frames in the key frame image set, the key frame image is discarded and no corresponding reference frame image is selected for it.

[0057] The key frame image set in step S2 is in is each key frame image in the key frame image set, i is the number of each key frame image in the key frame image set, t i is the moment of the i-th key frame image in the key frame image set, N K is the total number of key frame images in the key frame image set; and its corresponding reference frame image Perform ORB feature point extraction and matching, and record the pixel position of each successfully matched feature point as follows:

[0058]

[0059] In the formula, is the set of pixel positions of each feature point that is successfully matched, Key frame images The pixel horizontal and vertical coordinates of each feature point that is successfully matched; The reference frame images The pixel horizontal and vertical coordinates of each feature point that is successfully matched, t i The set of logarithms of each feature point successfully matched in the key frame image at the moment and the reference frame image;

[0060] For each key frame image The pixel positions of each feature point that is successfully matched are listed in the range By row range Construct a matching image block consisting of each successfully matched feature point.

[0061] S3. Taking the matching image block as input, based on the three-dimensional space points corresponding to the successfully matched feature points in the matching image block, according to the homography matrix calculation, with the rotation matrix and translation vector between each key frame image and its corresponding reference frame image as output, a homography matrix calculation module is constructed.

[0062] The specific steps of step S3 to construct the homography matrix calculation module are as follows:

[0063] S31: Normalize each successfully matched feature point in the matching image block, randomly select 8 pairs of successfully matched feature points, and solve the homography matrix based on the eight-point method. The solution method is as follows:

[0064] p 1 =Hp 2

[0065] In the formula, p 1 is the feature point in the key frame image, p 2 are the feature points in the reference frame image that match the feature points in the key frame image, and H is the homography matrix;

[0066] S32: Based on the Faugeras SVD decomposition method, the homography matrix is ​​decomposed into a rotation matrix and a translation vector. The specific method is as follows:

[0067]

[0068] Where D is the distance from the camera's far point to the plane where the three-dimensional space point is located, R is the rotation matrix, is the translation vector, n is the normal vector of the plane where the three-dimensional space point is located, U is an m×m matrix, where m is the number of rows of the homography matrix H, ∧ is an m×n matrix, all elements except the elements on the diagonal are 0, and each element on the main diagonal is called a singular value, where n is the number of columns of the homography matrix H, and V is an n×n matrix.

[0069] S4. Taking the matching image blocks output by the matching image block extraction module as input, based on the pixel positions of each successfully matched feature point in the matching image blocks, the RAFT streamer method is used to calculate the horizontal coordinate offset and the vertical coordinate offset of the pixel position of each successfully matched feature point in each key frame image relative to the pixel position of each successfully matched feature point in each reference frame image, and taking the matrix composed of the horizontal coordinate offset and the vertical coordinate offset as output, a feature point correspondence calculation module is constructed.

[0070] The specific steps of constructing the feature point correspondence calculation module in step S4 are as follows:

[0071] S41: For the pixel positions of each successfully matched feature point in each key frame image, the RAFT streamer method is used to calculate the horizontal axis displacement u of the pixel positions of each successfully matched feature point in each key frame image relative to the pixel positions of each successfully matched feature point in each reference frame image. (I,J) , longitudinal axis displacement v (I,J) , where I is the pixel horizontal coordinate of each successfully matched feature point in the key frame image, and J is the pixel vertical coordinate of each successfully matched feature point in the key frame image;

[0072] S42: Based on the pixel position (I, J) of each successfully matched feature point in the key frame image, the horizontal axis displacement u of the pixel position of each successfully matched feature point in each key frame image relative to the pixel position of each successfully matched feature point in each reference frame image (I,J) , longitudinal axis displacement v (I,J) , the pixel position (ref_i, ref_j) of each successfully matched feature point in the reference frame image is obtained according to the following formula:

[0073] ref_i=I+u (I,J)

[0074] ref_j=J+v (I,J)

[0075] Where ref_i is the pixel horizontal coordinate of the successfully matched feature point in the reference frame image, and ref_j is the pixel vertical coordinate of the successfully matched feature point in the reference frame image.

[0076] S5. Taking the rotation matrix and translation vector between each key frame image output by the homography matrix calculation module and their corresponding reference frame images as input, based on the triangulation algorithm, with the position and depth of each three-dimensional space point mapped in the normalized plane of each key frame image, and the position and depth of the three-dimensional space point mapped in the normalized plane of each reference frame image as output, a triangulation calculation module is constructed.

[0077] The specific calculation method of the triangulation algorithm in step S5 is as follows:

[0078] z r p r =T rw P w

[0079] z k p k =T kw P w

[0080] T rw =[R rw |t rw ]

[0081] T kw =[R kw |t kw ]

[0082] Where P w =[X,Y,Z,1] T is a three-dimensional space point, where X, Y, and Z are the coordinates of the three-dimensional space point in the preset scene, and T rw =[R rw |t rw ] is the camera pose of the reference frame image, which represents the transformation from the world coordinate system to the reference frame image coordinate system, R rw represents the rotation matrix from the world coordinate system to the reference frame image coordinate system, t rw Represents the translation vector from the world coordinate system to the reference frame image coordinate system, T kw =[R kw |t kw ] is the camera pose of the key frame image, which represents the transformation from the world coordinate system to the key frame image coordinate system, R kw Represents the rotation matrix from the world coordinate system to the key frame image coordinate system, t kwRepresents the translation vector from the world coordinate system to the key frame image coordinate system, the three-dimensional space point P w The position of the mapping in the normalized plane of the reference frame image is where x r ,y r 、z r They are three-dimensional space points P w The horizontal coordinate, vertical coordinate, depth, and three-dimensional space point P mapped in the normalized plane of the reference frame image w The position of the mapping in the normalized plane of the key frame image is where x k ,y k 、z k They are three-dimensional space points P w The horizontal coordinate, vertical coordinate, and depth mapped in the normalized plane of the keyframe image.

[0083] S6. Based on the height value of the preset scene corresponding to each three-dimensional space point, the position of the three-dimensional space point whose height value is less than the preset upper limit value and greater than the preset lower limit value mapped in the normalized plane of each key frame image, and the position of each three-dimensional space point mapped in the normalized plane of each reference frame image are used as input, based on the angle calculation of the vector from the position of each three-dimensional space point mapped in the normalized plane of each key frame image to the optical center of each key frame image, and the angle from the position of each three-dimensional space point mapped in the normalized plane of each reference frame image to the optical center of each reference frame image, the angle higher than the preset lower limit threshold is used as output, and a three-dimensional space point angle calculation module is constructed.

[0084] In step S6, the angle between the vector from the position of each three-dimensional space point mapped in the normalized plane of each key frame image to the optical center of each key frame image and the vector from the position of each three-dimensional space point mapped in the normalized plane of each reference frame image to the optical center of each reference frame image is calculated as follows:

[0085] a·b=|a||b|cosθ

[0086] Where a is the vector from the position of the three-dimensional space point mapped in the normalized plane of the key frame image to the optical center of the key frame image, b is the vector from the position of the three-dimensional space point mapped in the normalized plane of the reference frame image to the optical center of the reference frame image, and θ is the angle between the two vectors.

[0087] In one embodiment, in step S6, the preset upper limit value of the height value of the preset scene corresponding to each three-dimensional space point is 5, and the preset lower limit value is 0.

[0088] In one embodiment, in step S6, the preset lower limit threshold of the angle between the vector from the position of each three-dimensional space point mapped in the normalized plane of each key frame image to the optical center of each key frame image and the vector from the position of each three-dimensional space point mapped in the normalized plane of each reference frame image to the optical center of each reference frame image is 25°.

[0089] S7. Take the position and depth of the three-dimensional space point mapping corresponding to each angle output by the three-dimensional space point angle calculation module in the normalized plane of each key frame image as input, and take the semi-dense point cloud map corresponding to each matching image block in the camera coordinate system as output, and construct a camera coordinate system semi-dense point cloud map module.

[0090] The method for calculating the semi-dense point cloud map corresponding to each matching image block in the camera coordinate system in step S7 is as follows:

[0091] zu=x·f x +c x

[0092] zv=y·f y +c y

[0093] d=z·s

[0094] Where x, y, and z are the coordinates of the three-dimensional space point in the camera coordinate system, u, v, and d are the pixel horizontal coordinate, vertical coordinate, and depth of the three-dimensional space point in the pixel coordinate system, respectively. x 、f y is the focal length of the camera on the x-axis and y-axis respectively, c x 、c y is the aperture center, and s is the scaling factor of the depth map.

[0095] S8. Taking the semi-dense point cloud map corresponding to each matching image block in the camera coordinate system as input, based on the transformation matrix corresponding to each key frame image output by the key frame image discrimination module, respectively calculate the semi-dense point cloud map in the world coordinate system, splice the semi-dense point cloud maps in each world coordinate system to form a global map, take the global map as output, and construct a global map module.

[0096] The method for calculating the semi-dense point cloud map in the world coordinate system in step S8 is as follows:

[0097]

[0098] Where T is the transformation matrix, X W , Y W , Z W is the coordinate of the three-dimensional space point in the world coordinate system.

[0099] S9. Based on the key frame image discrimination module, matching image block extraction module, homography matrix calculation module, feature point correspondence calculation module, triangulation calculation module, 3D space point angle calculation module, camera coordinate system semi-dense point cloud map module, and global map module, a semi-dense point cloud map model is constructed with the image sequence obtained by the monocular camera and arranged in time sequence as input and the global map as output, referring to Figure 2 The preset scene in the figure is an office scene. Figure 2 This is a semi-dense point cloud map result of an office scene according to the semi-dense point cloud map construction method based on a monocular camera provided in an embodiment of the present invention.

[0100] The embodiments of the present invention are described in detail above with reference to the accompanying drawings, but the present invention is not limited to the above embodiments, and various changes can be made within the knowledge scope of ordinary technicians in this field without departing from the purpose of the present invention.

Claims

1. A method for constructing a semi-dense point cloud map based on a monocular camera, characterized in that: Execute steps S1-S9 according to a preset cycle to obtain a semi-dense point cloud map model, and then apply the semi-dense point cloud map model to complete the construction of a global map of a preset target scene in a world coordinate system; S1. For a preset target scene, a monocular camera moving along a preset trajectory acquires an image of the preset target scene, and takes the image sequence acquired by the monocular camera and arranged in time sequence as input, and discriminates each key frame image in the image sequence based on the ORB-SLAM2 algorithm, and constructs a key frame image set arranged in time sequence, and takes the transformation matrix corresponding to the key frame image set and each key frame image as output, and constructs a key frame image discrimination module; S2. Take a key frame image from the key frame image set The second key frame image after Key frame image The reference frame image of the key frame image set, where i is the number of each key frame image in the key frame image set, t i At the moment of the i-th key frame image in the key frame image set, each key frame image and the reference frame image corresponding to each key frame image are used as input, and based on the ORB-SLAM2 algorithm, ORB feature point extraction and matching are performed for each key frame image and each key frame image and its corresponding reference frame image, and matching image blocks are constructed according to the pixel positions of each successfully matched feature point, and each matching image block is used as output to construct a matching image block extraction module; S3. Taking the matching image block as input, based on the three-dimensional space points corresponding to the successfully matched feature points in the matching image block, according to the homography matrix calculation, taking the rotation matrix and translation vector between each key frame image and its corresponding reference frame image as output, construct a homography matrix calculation module; S4. Taking the matching image blocks output by the matching image block extraction module as input, based on the pixel positions of the successfully matched feature points in the matching image blocks, using the RAFT streamer method, calculate the horizontal coordinate offset and the vertical coordinate offset of the pixel positions of the successfully matched feature points in each key frame image relative to the pixel positions of the successfully matched feature points in each corresponding reference frame image, and taking the matrix composed of the horizontal coordinate offset and the vertical coordinate offset as output, construct a feature point correspondence calculation module; S5. Taking the rotation matrix and translation vector between each key frame image and its corresponding reference frame image output by the homography matrix calculation module as input, based on the triangulation algorithm, taking the position and depth of each three-dimensional space point mapped in the normalized plane of each key frame image, and the position and depth of the three-dimensional space point mapped in the normalized plane of each reference frame image as output, a triangulation calculation module is constructed; S6. Based on the height value of the preset scene corresponding to each three-dimensional space point, the position of the three-dimensional space point whose height value is less than the preset upper limit value and greater than the preset lower limit value mapped in the normalized plane of each key frame image and the position of each three-dimensional space point mapped in the normalized plane of each reference frame image are used as input, based on the angle calculation of the vector from the position of each three-dimensional space point mapped in the normalized plane of each key frame image to the optical center of each key frame image and the vector from the position of each three-dimensional space point mapped in the normalized plane of each reference frame image to the optical center of each reference frame image, the angle higher than the preset lower limit threshold is used as output, and a three-dimensional space point angle calculation module is constructed; S7. Taking the position and depth of the three-dimensional space point mapping corresponding to each angle output by the three-dimensional space point angle calculation module in the normalized plane of each key frame image as input, and taking the semi-dense point cloud map corresponding to each matching image block in the camera coordinate system as output, a camera coordinate system semi-dense point cloud map module is constructed; S8. Taking the semi-dense point cloud map corresponding to each matching image block in the camera coordinate system as input, based on the transformation matrix corresponding to each key frame image output by the key frame image discrimination module, respectively calculate the semi-dense point cloud map in the world coordinate system, splice the semi-dense point cloud maps in each world coordinate system to form a global map, take the global map as output, and construct a global map module; S9. Based on the key frame image discrimination module, matching image block extraction module, homography matrix calculation module, feature point correspondence calculation module, triangulation calculation module, three-dimensional space point angle calculation module, camera coordinate system semi-dense point cloud map module, and global map module, a semi-dense point cloud map model is constructed with the time-series image sequence acquired by the monocular camera as input and the global map as output.

2. The method for constructing a semi-dense point cloud map based on a monocular camera according to claim 1, characterized in that: The key frame image set in step S2 is in is each key frame image in the key frame image set, i is the number of each key frame image in the key frame image set, t i is the moment of the i-th key frame image in the key frame image set, N K is the total number of key frame images in the key frame image set; and its corresponding reference frame image Perform ORB feature point extraction and matching, and record the pixel position of each successfully matched feature point as follows: In the formula, is the set of pixel positions of each feature point that is successfully matched, Key frame images The pixel horizontal and vertical coordinates of each feature point that is successfully matched; The reference frame images The pixel horizontal and vertical coordinates of each feature point that is successfully matched, t i The set of logarithms of each feature point successfully matched in the key frame image at the moment and the reference frame image; For each key frame image The pixel positions of each feature point that is successfully matched are listed in the range By row range Construct a matching image block consisting of each successfully matched feature point.

3. The method for constructing a semi-dense point cloud map based on a monocular camera as claimed in claim 2, characterized in that: In step S3, the matching image block is used as input, and the three-dimensional space points corresponding to the successfully matched feature points in the matching image block are calculated according to the homography matrix, and the rotation matrix and translation vector between each key frame image and its corresponding reference frame image are used as output. The specific steps of constructing the homography matrix calculation module are as follows: S31: Normalize each successfully matched feature point in the matching image block, randomly select 8 pairs of successfully matched feature points, and solve the homography matrix based on the eight-point method. The solution method is as follows: p1=Hp2 Where p1 is the feature point in the key frame image, p2 is the feature point in the reference frame image that matches the feature point in the key frame image, and H is the homography matrix; S32: Based on the Faugeras SVD decomposition method, the homography matrix is ​​decomposed into a rotation matrix and a translation vector. The specific method is as follows: Where D is the distance from the camera's far point to the plane where the three-dimensional space point is located, R is the rotation matrix, is the translation vector, n is the normal vector of the plane where the three-dimensional space point is located, U is an m×m matrix, where m is the number of rows of the homography matrix H, ∧ is an m×n matrix, all elements except the elements on the diagonal are 0, and each element on the main diagonal is called a singular value, where n is the number of columns of the homography matrix H, and V is an n×n matrix.

4. The method for constructing a semi-dense point cloud map based on a monocular camera as claimed in claim 3, characterized in that: In step S4, each matching image block output by the matching image block extraction module is used as input, and based on the pixel position of each successfully matched feature point in the matching image block, the RAFT streamer method is used to calculate the horizontal coordinate offset and the vertical coordinate offset of the pixel position of each successfully matched feature point in each key frame image relative to the pixel position of each successfully matched feature point in each reference frame image, and the matrix composed of each horizontal coordinate offset and the vertical coordinate offset is used as output to construct the feature point correspondence calculation module. The specific steps are as follows: S41: For the pixel positions of each successfully matched feature point in each key frame image, the RAFT streamer method is used to calculate the horizontal axis displacement u of the pixel positions of each successfully matched feature point in each key frame image relative to the pixel positions of each successfully matched feature point in each reference frame image. (I,J) , longitudinal axis displacement v (I,J) , where I is the pixel horizontal coordinate of each successfully matched feature point in the key frame image, and J is the pixel vertical coordinate of each successfully matched feature point in the key frame image; S42: Based on the pixel positions (I, J) of each successfully matched feature point in the key frame image, the horizontal axis displacement u of the pixel positions of each successfully matched feature point in each key frame image relative to the pixel positions of each successfully matched feature point in each reference frame image (I,J) , longitudinal axis displacement v (I,J) , the pixel position (ref_i, ref_j) of each successfully matched feature point in the reference frame image is obtained according to the following formula: ref_i=I+u (I,J) ref_j=J+v (I,J) Where ref_i is the pixel horizontal coordinate of the successfully matched feature point in the reference frame image, and ref_j is the pixel vertical coordinate of the successfully matched feature point in the reference frame image.

5. The method for constructing a semi-dense point cloud map based on a monocular camera according to claim 4, characterized in that: The specific calculation method of the triangulation algorithm in step S5 is as follows: z r P r =T rw P w z k P k =T kw P w T rw =[R rw |t rw ] T kw =[R kw |t kw ] Where P w =[X,Y,Z,1] T is a three-dimensional space point, where X, Y, and Z are the coordinates of the three-dimensional space point in the preset scene, and T rw =[R rw |t rw ] is the camera pose of the reference frame image, R rw represents the rotation matrix from the world coordinate system to the reference frame image coordinate system, t rw Represents the translation vector from the world coordinate system to the reference frame image coordinate system, T kw =[R kw |t kw ] is the camera pose of the key frame image, R kw Represents the rotation matrix from the world coordinate system to the key frame image coordinate system, t kw Represents the translation vector from the world coordinate system to the key frame image coordinate system, the three-dimensional space point P w The position of the mapping in the normalized plane of the reference frame image is where x r ,y r 、z r They are three-dimensional space points P w The horizontal coordinate, vertical coordinate, depth, and three-dimensional space point P mapped in the normalized plane of the reference frame image w The position of the mapping in the normalized plane of the key frame image is where x k ,y k 、z k They are three-dimensional space points P w The horizontal coordinate, vertical coordinate, and depth mapped in the normalized plane of the keyframe image.

6. The method for constructing a semi-dense point cloud map based on a monocular camera according to claim 5, characterized in that: In step S6, the angle between the vector from the position of each 3D space point mapped in the normalized plane of each key frame image to the optical center of each key frame image and the vector from the position of each 3D space point mapped in the normalized plane of each reference frame image to the optical center of each reference frame image is calculated as follows: a·b=|a||b|cosθ Where a is the vector from the position of the three-dimensional space point mapped in the normalized plane of the key frame image to the optical center of the key frame image, b is the vector from the position of the three-dimensional space point mapped in the normalized plane of the reference frame image to the optical center of the reference frame image, and θ is the angle between the two vectors.

7. The method for constructing a semi-dense point cloud map based on a monocular camera according to claim 6, characterized in that: The preset upper limit value in step S6 is 5, and the preset lower limit value is 0.

8. The method for constructing a semi-dense point cloud map based on a monocular camera according to claim 7, characterized in that: In step S7, based on the position and depth of the three-dimensional space point mapping corresponding to each angle output by the three-dimensional space point angle calculation module in the normalized plane of each key frame image, the method for calculating the semi-dense point cloud map corresponding to each matching image block in the camera coordinate system is as follows: to=x·f x +c x zv=y·f y +c y d=z·s Where x, y, and z are the coordinates of the three-dimensional space point in the camera coordinate system, u, v, and d are the pixel horizontal coordinate, vertical coordinate, and depth of the three-dimensional space point in the pixel coordinate system, respectively. x 、f y is the focal length of the camera on the x-axis and y-axis respectively, c x 、c y is the aperture center, and s is the scaling factor of the depth map.

9. The method for constructing a semi-dense point cloud map based on a monocular camera according to claim 8, characterized in that: In step S8, based on the transformation matrix corresponding to each key frame image output by the key frame image discrimination module, the method for calculating the semi-dense point cloud map in the world coordinate system is as follows: Where T is the transformation matrix, X W , Y W , Z W is the coordinate of the three-dimensional space point in the world coordinate system.

Citation Information

Cited By

  • Attribute transfer function for re-meshed content with small or isolated mesh triangles and associated bit stream structure and syntax

    US12776104B1