A vision-enhanced laser odometry method based on online correction

Abstract

The present invention discloses a method for visually enhanced laser odometry based on online correction. The color image and three-dimensional point cloud of the scene are collected, and a vertex map, a normal map and a vertex shading map are generated by preprocessing to construct a visual laser odometry data set; the vertex map and the vertex shading map are input into the visually enhanced laser odometry network to promote the fusion of visual laser modalities during the training process; after the training is completed, the vertex map and the vertex shading map sequence to be measured are input into the network to predict the inter-frame pose, and the online correction module is used for iterative optimization, and the global pose is obtained by multiplying multiple optimized inter-frame poses to realize the visual laser odometry. The method of the present invention utilizes the laser-dominated fusion strategy to give full play to the complementary characteristics of the visual laser modality, and can perform unsupervised training, has good universality and versatility, and is less difficult to optimize and has significantly improved performance.

Description

Technical Field

[0001] The present invention relates to an unsupervised vision-laser odometry method in the technical field of computer vision, and in particular to a vision-enhanced laser odometry method based on online correction. Background Art

[0002] Odometry, also known as pose estimation, is a key issue in tasks such as autonomous driving and augmented reality. Traditional odometry methods often follow a three-step process: feature extraction, feature matching, and pose calculation. However, these methods rely heavily on handcrafted features and are susceptible to problems in complex scenes with drastic lighting changes and weak textures. Recently, deep learning methods have demonstrated their superior performance in feature extraction for many computer vision problems, such as image classification and object detection. As a result, deep learning-based odometry methods have begun to attract increasing research attention. For example, Zhou et al.'s "Unsupervised Learning of Depth and Ego-Motion from Video," published in the Proceedings of the IEEE / CVF Conference on Computer Vision and Pattern Recognition, and Cho et al.'s "Unsupervised Geometry-Aware Deep LiDAR Odometry," published in the 2020 IEEE International Conference on Robotics and Automation, are unsupervised bullet screen visual odometry and laser odometry methods, respectively. They can learn directly from monocular image sequences or 3D point cloud sequences without any ground truth.

[0003] However, to date, a large portion of research on deep learning odometry methods has focused on visual odometry (VO) or laser odometry (LO), while research on visual-lidar odometry (VLO) is relatively limited. The complementary advantages of visual-lidar odometry have been applied in many computer vision tasks (such as 3D object detection, depth completion, and scene flow estimation), making VLO research a promising research direction.

[0004] Existing deep learning visual-lidar odometry methods all use a vision-led fusion strategy, such as "Self-supervised Visual-LiDAR Odometry with Flip Consistency" published by Li et al. in the 2021 IEEE Winter Conference on Applications of Computer Vision. This method projects the 3D point cloud collected by the lidar onto the camera imaging plane to obtain a sparse depth map with the same resolution as the monocular color image. This is then used as input for predicting the inter-frame pose. However, this approach faces two problems. First, it is difficult to fuse features between the sparse depth map and the dense color image. Second, this method requires the simultaneous prediction of a dense depth map and the inter-frame pose to achieve unsupervised training, which makes the pose easily affected by the noise in the predicted depth map.

[0005] Unsupervised learning can use online optimization methods to further optimize the prediction results of the network model during the testing phase. According to the type of optimization parameters in the optimization process, it can be divided into online learning and online correction. The former optimizes the network weights, while the latter directly uses the network's prediction results as the variables to be optimized. Compared with online learning, the online correction method can significantly reduce the number of parameters to be optimized and has higher computational efficiency. However, the existing odometry methods using online optimization all adopt a vision-dominated strategy, such as "Deep Online Correction for Monocular Visual Odometry" published by Zhang et al. in "2021IEEE International Conference on Robotics and Automation". This method requires the simultaneous prediction of dense depth maps and inter-frame poses to construct the optimization objective function, resulting in the performance of pose optimization being extremely susceptible to noise interference from the predicted depth. Summary of the Invention

[0006] In order to solve the problems existing in the background technology, the present invention provides a visually enhanced laser odometry method based on online correction, which adopts a laser-dominated fusion strategy, projects a three-dimensional point cloud into a dense vertex graph through spherical projection, finds the corresponding pixel position of each 3D point on the vertex graph on the color image, and colors the point with the color of the corresponding pixel to obtain a vertex coloring graph. The present invention inputs the vertex graph and the vertex coloring graph sequence into the visually enhanced laser odometry network to predict the pose. Since the vertex graph and the vertex coloring graph are both 2D dense images, the present invention can better perform visual-laser modality fusion. At the same time, the method of the present invention does not require additional prediction of the depth map, and only needs to predict the inter-frame pose to construct a loss function for unsupervised training. Subsequently, in the testing phase, the present invention introduces a pose online correction module to further optimize the predicted pose of the trained visually enhanced laser odometry network PoseNet, and introduces difficult example mining technology to screen difficult examples that contribute more to the optimization to promote the optimization effect.

[0007] This paper utilizes a laser-led fusion strategy to design a vision-laser odometry. Compared to vision-led fusion methods that project 3D point clouds into sparse depth maps, this not only allows for better vision-laser modality fusion, but also requires only inter-frame pose prediction for training, simplifying the optimization problem and significantly improving online optimization results, resulting in significant performance gains. Compared to vision-led odometry methods using online optimization, this paper achieves superior performance across key error metrics.

[0008] The technical solution adopted in the present invention is:

[0009] 1) A color camera and lidar are used to jointly acquire a color image sequence and a corresponding 3D point cloud sequence of the scene. The color image sequence and the corresponding 3D point cloud sequence of the scene are then preprocessed to generate a corresponding vertex map sequence, normal map sequence, and vertex shading map sequence. The color image sequence and the corresponding vertex map sequence, normal map sequence, and vertex shading map sequence form a visual-laser odometry dataset.

[0010] 2) Use the visual-laser odometry dataset to train the visually enhanced laser odometry network PoseNet to obtain the trained visually enhanced laser odometry network PoseNet;

[0011] 3) In the color image sequence and the corresponding 3D point cloud sequence to be tested, a pair of image inputs is formed by two adjacent frames of the color image to be tested and the corresponding 3D point cloud. Each image input pair is preprocessed to generate a corresponding pair of vertex graph and vertex shading graph input to be tested. All the pairs of vertex graph and vertex shading graph inputs to be tested are then sequentially input into the trained visual enhancement laser odometry network PoseNet, and the predicted inter-frame pose corresponding to each image input pair to be tested is output respectively.

[0012] 4) Based on the color image sequence to be tested, the posture online correction module is used to optimize and correct the predicted inter-frame postures corresponding to each pair of image inputs to be tested, and obtain each corrected inter-frame posture;

[0013] 5) Multiply the corrected inter-frame poses in sequence to obtain the global pose.

[0014] The specific embodiment of 1) is:

[0015] A color camera collects color images of the scene at continuous moments to obtain a color image sequence, and then a lidar collects three-dimensional point clouds of the same scene at the same continuous moments to obtain a three-dimensional point cloud sequence; for each frame of three-dimensional point cloud in the three-dimensional point cloud sequence, each frame of three-dimensional point cloud is first spherically projected to obtain the corresponding vertex map, and then the normal vector of each point is solved based on the vertex map to obtain the corresponding normal vector map, and finally the corresponding color of each point on the vertex map in the corresponding color image is found through camera imaging projection to obtain the vertex shading map. After traversing the three-dimensional point cloud sequence, the corresponding vertex map sequence, normal vector map sequence and vertex shading map sequence are obtained based on the three-dimensional point cloud sequence processing. The visual-laser odometry dataset consists of the color image sequence and the corresponding vertex map sequence, normal vector map sequence and vertex shading map sequence.

[0016] In the above 2), the total loss function of the visually enhanced laser odometry network PoseNet is calculated based on the color image, vertex map, vertex shading map, normal vector map and predicted inter-frame pose. During each training, the input of the visually enhanced laser odometry network PoseNet is the vertex map of two consecutive frames in the vertex map sequence and the corresponding two-frame vertex shading map, and the output of the visually enhanced laser odometry network PoseNet is the predicted inter-frame pose of the current two frames.

[0017] The visually enhanced laser odometry network PoseNet consists of a feature encoder and a pose predictor connected together;

[0018] The feature encoder is composed of 11 convolution modules connected in sequence. The input of the visual enhanced laser odometry network PoseNet is cascaded through the channels and then used as the input of the first convolution module. The output of the eleventh convolution module is used as the output of the feature encoder. The pose predictor includes two convolution modules and two convolution pooling units. The output of the feature encoder is used as the input of the twelfth convolution module. The twelfth convolution module is connected to the thirteenth convolution module, and the thirteenth convolution module is connected to the two convolution pooling units. The two convolution pooling units output the translation vector and Euler angle vector in the inter-frame pose respectively. The output of the two convolution pooling units is used as the output of the visual enhanced laser odometry network PoseNet.

[0019] The structures of the two convolutional pooling units are the same, which are composed of a convolution module, a global average pooling module and a convolution layer connected in sequence. The structures of all convolutional modules are the same, which are composed of a convolution layer, a batch normalization layer and an activation function layer connected in sequence.

[0020] The total loss function of the visually enhanced laser odometry network PoseNet is calculated based on the color image, vertex map, vertex shading map, normal vector map and predicted inter-frame pose, specifically:

[0021] S1: Generate the corresponding valid vertex binary mask map Mv according to each vertex map. The calculation formula is as follows:

[0022]

[0023] Among them, Mv(u) represents the vertex validity at pixel position u in the valid vertex binary mask map Mv, V(u) represents the point coordinates of pixel position u in the vertex map, is the indicator function, |||| is the L2 norm;

[0024] S2: Calculate the cosine similarity between the normal vectors of each point in the normal vector map and the four surrounding neighboring points to obtain the confidence of the current point, thereby obtaining the confidence map C corresponding to the normal vector map. Based on the normal vector map and the confidence map C, the corresponding effective normal vector binary mask Mn is generated. The calculation formula is as follows:

[0025]

[0026]

[0027] Among them, δc represents the confidence threshold, represents the set of 4 pixel positions adjacent to the pixel position u, Mn(u) represents the validity of the normal vector of the pixel position u in the valid normal vector binary mask Mn, C(u) represents the confidence of the normal vector of the pixel position u in the confidence map, N(u) represents the normal vector of the pixel position u in the normal map, N(u i) represents the neighboring pixels u of pixel position u in the normal vector map i The normal vector of

[0028] S3: Generate the corresponding effective color binary mask Mc based on the vertex coloring graph Vc. The calculation formula is as follows:

[0029]

[0030] Among them, Mc(u) represents the color validity of the vertex corresponding to the pixel position u in the effective color binary mask Mc, and Vc(u) represents the color of the pixel position u in the vertex coloring graph Vc;

[0031] S4: Construct a geometric loss function based on the valid vertex binary mask map Mv and the valid normal vector binary mask Mn. The formula is as follows:

[0032]

[0033]

[0034] M geo =Mv t+1 ⊙Mn t+1

[0035] p t =V t (π(p′ t ))

[0036] in, Represents the geometric loss function value, p′ t Denotes the inter-frame pose P predicted by the network t←t+1 The transformation matrix T obtained by conversion t←t+1 For each point p in the vertex graph of the t+1 frame t+1 Transform to the tth frame to get the projection point, Represents the projection point p′ t Vertex graph V at frame t t The matching points in d geo () indicates the calculation of two matching points p′ t and The function of the point-surface matching error between geo Represents a binary mask of a local plane area, ⊙ represents element-by-element multiplication; ||||1 represents the operation of taking the L1 norm, M geo (u t+1 ) represents the local planar region binary mask M geo Medium pixel u t+1 The geometric error validity, Mv t+1 Represents the effective vertex binary mask map of the t+1th frame, Mn t+1Represents the effective normal vector binary mask of the t+1th frame, V t represents the vertex graph of the t-th frame; C t () represents the confidence of each pixel position in the confidence map of the tth frame, and π() represents the spherical projection;

[0037] S5: Construct a visual loss function based on the effective color binary mask Mc and the effective normal vector binary mask Mn. The formula is as follows:

[0038]

[0039] M vis =Mc t+1 ⊙(1-Mn t+1 )

[0040]

[0041] in, Represents the visual loss function value, M vis The binary mask representing the valid pixels with low confidence in the normal vector; u′ t Denotes the inter-frame pose P predicted by the network t←t+1 The transformation matrix T obtained by conversion t←t+1 For each point p in the vertex graph of the t+1 frame t+1 After transforming to the tth frame, the projection point p′ is obtained t , and then use the camera projection function to find the projection point p′ t In the color image I t The projected pixel coordinates on Represents the projected pixel coordinate u′ t In the color image I t Color on, d vis (u t+1 ) represents the calculation of pixel u t+1 and u′ t A function of the color difference between

[0042] S6: Construct the total loss function based on the geometric loss function and the visual loss function:

[0043]

[0044] in, Represents the total loss function value, and λ represents the weight of the visual loss function.

[0045] The specific aspects of 4) are:

[0046] 4.1) Input each image to be tested into the corresponding predicted inter-frame pose P t←t+1 As the initial inter-frame pose of the current two frames

[0047] 4.2) According to the current inter-frame pose of the current two frames Calculate the corresponding matching points of each point in the vertex map of the next frame in the vertex map of the previous frame, and calculate the relative standard deviation of each point in the vertex map of the next frame based on the corresponding matching points in the vertex map of the previous frame, so as to obtain the relative standard deviation map RSD of the current two frames;

[0048] 4.3) According to the relative standard deviation (RSD) of the current two frames, the following formula is used to filter and obtain the difficult binary mask:

[0049] RSD(u t+1 )=Std(d p ) / mean(d p )

[0050]

[0051] Among them, RSD(u t+1 ) represents the vertex graph V of the t+1 frame t+1 midpoint p t+1 The corresponding relative standard deviation, std() represents the function for calculating the standard deviation, and mean() represents the function for calculating the mean. Represents pixel u t+1 Whether there are difficult cases;

[0052] 4.4) Based on the binary mask M of the local plane area geo and hard example binary masks Calculate the local planar area mask that only contains hard examples

[0053]

[0054] 4.5) Based on the visual loss function and the local planar region mask containing only hard examples Construct the loss function for difficult example mining, the formula is as follows:

[0055]

[0056]

[0057] in, represents the loss function value of hard example mining, represents the visual loss function value, represents the geometric loss function value calculated in the hard example area, and λ represents the weight of the visual loss function;

[0058] 4.5) Taking the loss function of hard example mining as the optimization objective function, the pose between the current two frames is Perform gradient descent update to obtain the optimized inter-frame pose;

[0059] 4.6) Repeat 4.2)-4.5) N times to obtain the corrected inter-frame pose of the current two frames;

[0060] 4.7) Repeat 4.1)-4.6) to predict the inter-frame pose P corresponding to the remaining image input pairs to be tested t←t+1 Perform optimization correction to obtain all corrected inter-frame poses.

[0061] In 4.2), for each matching point p in the vertex map of the previous frame t , first, the current matching point p t Points in a local window of 5×7 centered on As a candidate matching point, then calculate the current matching point p t The matching error between all candidate matching points is obtained to obtain the candidate matching error set d p ,Finally, calculate the candidate matching error set d p The relative standard deviation is used as the current matching point p t The relative standard deviation.

[0062] The method of the present invention uses a laser-led fusion strategy to design a visually enhanced laser odometry network. The original three-dimensional lidar point cloud is projected into a vertex graph via spherical projection, and then colored with the corresponding color image to obtain a vertex-colored graph, achieving a dense representation of the visual-laser modality data. The visually enhanced laser odometry network uses a continuous vertex graph and a vertex-colored graph as input and is trained in a self-supervised manner. During the testing phase, the network parameters of the visually enhanced laser odometry network are fixed, and the inter-frame pose predicted by the visually enhanced laser odometry network is iteratively updated using an online pose correction module.

[0063] The present invention first constructs a visual-laser odometry dataset, which includes a sequence of continuous color images, vertex maps, normal vector maps and vertex shading maps. The training data is used to train a visual-enhanced laser odometry network in a small batch training manner. The parameters of the trained visual-enhanced laser odometry network are fixed, and the predicted inter-frame pose of the visual-enhanced laser odometry network is further optimized through an online pose correction module.

[0064] The present invention can achieve performance close to or even better than that of methods with global optimization or closed-loop modules without introducing any global optimization or closed-loop modules, and is at the forefront of the field in terms of translation performance and rotation performance.

[0065] The present invention has the following beneficial effects:

[0066] 1. Compared with the odometry method of vision-dominated fusion strategy, the present invention processes the input vision-laser modality data into dense 2D images, which can promote the effect of vision-laser modality fusion and make more full use of the complementary characteristics of vision-laser modalities.

[0067] 2. Compared with the vision-driven odometry method using online optimization, the present invention does not require additional prediction of dense depth maps, which greatly reduces the complexity of optimization, avoids noise interference in predicted depth, and has better optimization effect.

[0068] 3. The present invention does not require additional true value labels during the entire training process, so it has good versatility and universality. BRIEF DESCRIPTION OF THE DRAWINGS

[0069] Figure 1 It is a flow chart of an example of the present invention.

[0070] Figure 2 It is a schematic diagram of the visually enhanced laser odometry network model designed in an example of the present invention.

[0071] Figure 3 Schematic diagram of the online posture correction process designed in the example of the present invention.

[0072] Figure 4 This is an example of the pose estimation result of the odometer method designed in the example of the present invention. DETAILED DESCRIPTION

[0073] The present invention will be further described below with reference to the accompanying drawings and specific embodiments.

[0074] like Figure 1 As shown, the embodiment and specific implementation process of the present invention are as follows:

[0075] like Figure 1 As shown, the following steps are included:

[0076] 1) A color camera and lidar are used to jointly acquire a color image sequence and a corresponding 3D point cloud sequence of the scene. The color image sequence and the corresponding 3D point cloud sequence of the scene are then preprocessed to generate a corresponding vertex map sequence, normal map sequence, and vertex shading map sequence. The visual-lidar odometry dataset consists of the color image sequence and the corresponding vertex map sequence, normal map sequence, and vertex shading map sequence. In the vertex map sequence, normal map sequence, and vertex shading map sequence, the corresponding vertex map, normal map, and vertex shading map are all dense 2D images.

[0077] 1) Specifically:

[0078] A color camera collects color images of the scene at continuous moments to obtain a color image sequence, and then a lidar collects three-dimensional point clouds of the same scene at the same continuous moments to obtain a three-dimensional point cloud sequence; for each frame of three-dimensional point cloud in the three-dimensional point cloud sequence, each frame of three-dimensional point cloud is first spherically projected to obtain the corresponding vertex map, and then the normal vector of each point is solved based on the vertex map to obtain the corresponding normal vector map, and finally the corresponding color of each point on the vertex map in the corresponding color image is found through camera imaging projection to obtain the vertex shading map. After traversing the three-dimensional point cloud sequence, the corresponding vertex map sequence, normal vector map sequence and vertex shading map sequence are obtained based on the three-dimensional point cloud sequence processing. The visual-laser odometry dataset consists of the color image sequence and the corresponding vertex map sequence, normal vector map sequence and vertex shading map sequence.

[0079] More specifically, the vertex map is generated by projecting the 3D point cloud onto a 2D sphere. The spherical projection used is:

[0080]

[0081] The spherical projection process is recorded as u=π(p), p=[p x , p y , p z ] T represents a 3D point in a 3D point cloud, u is the corresponding 2D pixel coordinate of the 3D point p on the vertex graph, and π() represents the spherical projection. Thus, the present invention can generate a 2D vertex graph V, f h Represents the horizontal field of view of the 3D point cloud, f v represents the vertical field of view of the 3D point cloud, and f vu Then f v The upper part of δ h and δ v Represents the horizontal and vertical resolutions of the vertex graph, respectively. After determining the pixel position u corresponding to the 3D point p, the present invention sets V(u) = p, otherwise V(u) = 0, where V(u) represents the 3D coordinates of the corresponding point in the vertex graph V at the pixel position u.

[0082] For each point p on the vertex graph V, calculate the mean Hong and covariance matrix ∑ in its neighboring point set. After performing singular value decomposition on the covariance matrix ∑, the singular vector corresponding to the minimum singular value obtained is the estimated normal vector n, and then the corresponding normal vector graph N is obtained. The calculation formulas for the mean Hong and covariance matrix ∑ are as follows:

[0083]

[0084]

[0085] in, Represents the neighboring point set of each point p on the vertex graph V, satisfying is a 5×7 local window centered at pixel position u on the vertex graph, || represents the number of elements in the set, and p i represents the neighboring points of point p, and |||| represents the function for calculating the L2 norm.

[0086] For valid pixel positions in the vertex graph V, that is, positions in the vertex graph V that satisfy V(u)=p, N(u)=n, otherwise N(u)=0, where N(u) represents the normal vector of the pixel position u in the normal vector graph N.

[0087] Through camera imaging projection, find the corresponding color of each point on the vertex graph V in the corresponding color image I and obtain the vertex coloring graph Vc. Specifically, for each point p on the vertex graph, the present invention finds its corresponding pixel u′ on the color image through the camera projection function:

[0088]

[0089] The camera projection process is recorded as u′=π in this invention. c (p), K represents the intrinsic parameter matrix of the camera, T C←L Represents the transformation matrix from the lidar coordinate system to the camera coordinate system.

[0090] Since the obtained projection coordinate u′ is a continuous value, the present invention uses a bilinear interpolation strategy to obtain its corresponding color Vc(u) represents the color of pixel position u in the generated vertex shading graph, Represents the color of the color image I at the pixel position u′ obtained by the bilinear interpolation strategy, and then obtains the corresponding vertex coloring graph Vc.

[0091] 2) Use the visual-laser odometry dataset to train the visually enhanced laser odometry network PoseNet to obtain the trained visually enhanced laser odometry network PoseNet;

[0092] 2) In the visual enhanced laser odometry network PoseNet, the total loss function is based on the color image I, vertex map V, vertex coloring map Vc, normal vector map N and predicted inter-frame pose P t←t+1During each training session, the input to the visually enhanced laser odometry network PoseNet is the vertex graph V of two consecutive frames in the vertex graph sequence and the corresponding two-frame vertex shading graph Vc. During multiple training sessions of the visually enhanced laser odometry network PoseNet, the frame order of the previous and next input vertex graphs is randomly set. The output of the visually enhanced laser odometry network PoseNet is the predicted inter-frame pose between the current two frames.

[0093] like Figure 2 As shown in the figure, the visually enhanced laser odometry network PoseNet consists of a feature encoder and a pose predictor connected together; the feature encoder concatenates the vertex graphs of the two input frames and the corresponding vertex shading graphs to obtain a multimodal tensor, then extracts multimodal features from the multimodal tensor and inputs them into the pose predictor, which outputs the predicted inter-frame pose.

[0094] The feature encoder is composed of 11 convolutional modules connected in sequence. The input of the visual enhanced laser odometry network PoseNet (i.e., the input of the feature encoder) is channel-concatenated and then used as the input of the first convolutional module. The output of the eleventh convolutional module is used as the output of the feature encoder. The horizontal and vertical convolution steps of the convolutional layers of the 2nd, 6th, and 10th convolutional modules are (1, 2), while the horizontal and vertical convolution steps of the convolutional layers of the 4th and 8th convolutional modules are (2, 2). The horizontal and vertical convolution steps of the convolutional layers of the remaining convolutional modules are all (1, 1). The pose predictor includes two convolutional modules and two convolutional pooling units. The output of the feature encoder is used as the input of the twelfth convolutional module. The twelfth convolutional module is connected to the thirteenth convolutional module, and the thirteenth convolutional module is connected to two convolutional pooling units. The two convolutional pooling units output the translation vector and Euler angle vector in the inter-frame pose respectively. The output of the two convolutional pooling units is used as the output of the visual enhanced laser odometry network PoseNet.

[0095] The two convolutional pooling units have the same structure, consisting of a convolutional module, a global average pooling module, and a convolutional layer connected in sequence. The global average pooling module performs the global average pooling operation. All convolutional modules have the same structure, consisting of a convolutional layer, a batch normalization layer, and an activation function layer connected in sequence.

[0096] The total loss function of the visual enhanced laser odometry network PoseNet is based on the color image I, vertex map V, vertex coloring map Vc, normal vector map N and predicted inter-frame pose P t←-t+1 Calculate, specifically:

[0097] S1: Generate the corresponding valid vertex binary mask map Mv according to each vertex map, which is used to indicate the valid pixels corresponding to the 3D points in the vertex map. The calculation formula is as follows:

[0098]

[0099] Among them, Mv(u) represents the validity of the vertex at pixel position u in the valid vertex binary mask map Mv, 0 represents the invalid vertex, 1 represents the valid vertex, V(u) represents the point coordinates of pixel position u in the vertex map, is the indicator function, |||| is the L2 norm, which is used to calculate the distance from the 3D point p to the center of the lidar;

[0100] S2: Calculate the cosine similarity between the normal vectors of each point in the normal vector map and the four surrounding neighboring points to obtain the confidence of the current point, thereby obtaining the confidence map C corresponding to the normal vector map. Based on the normal vector map and the confidence map C, the corresponding valid normal vector binary mask Mn is generated. The valid normal vector binary mask Mn represents the area in the vertex map where reliable normal vectors exist. The calculation formula is as follows:

[0101]

[0102]

[0103] Among them, δ c represents the confidence threshold, Represents the set of 4 pixel positions adjacent to the pixel position u, Mn(u) represents the validity of the normal vector of the pixel position u in the valid normal vector binary mask Mn, 0 represents the invalid normal vector, 1 represents the valid normal vector, C(u) represents the confidence of the normal vector of the pixel position u in the confidence map, N(u) represents the normal vector of the pixel position u in the normal vector map, N(u i ) represents the neighboring pixels u of pixel position u in the normal vector map i The normal vector of

[0104] S3: Generate the corresponding effective color binary mask Mc based on the vertex shading graph Vc, which is used to represent the effective pixel area in the vertex shading graph Vc. The calculation formula is as follows:

[0105]

[0106] Among them, Mc(u) represents the color validity of the vertex corresponding to the pixel position u in the valid color binary mask Mc, 0 represents the invalid color, 1 represents the valid color, and Vc(u) represents the color of the pixel position u in the vertex coloring graph Vc.

[0107] S4: Construct a geometric loss function based on the valid vertex binary mask map Mv and the valid normal vector binary mask Mn. The formula is as follows:

[0108]

[0109]

[0110] M geo =Mv t+1 ⊙Mn t+1

[0111] p t =V t (π(p′ t ))

[0112] in, Represents the geometric loss function value, p′ t Denotes the inter-frame pose P predicted by the network t←t+1 The transformation matrix T obtained by conversion t←t+1 For each point p in the vertex graph of the t+1 frame t+1 Transform to the tth frame to get the projection point, that is Represents the projection point p′ t Vertex graph V at frame t t The matching points in d geo () indicates the calculation of two matching points p′ t and The function of the point-surface matching error between geo Represents a binary mask of a local plane area, ⊙ represents element-by-element multiplication; ||||1 represents the operation of taking the L1 norm, M geo (u t+1 ) represents the local planar region binary mask M geo Medium pixel u t+1 The geometric error validity, 0 means the geometric error is invalid, 1 means it is valid, Mv t+1 Represents the effective vertex binary mask map of the t+1th frame, Mn t+1 Represents the effective normal vector binary mask of the t+1th frame, V t represents the vertex graph of the t-th frame; C t () represents the confidence of each pixel position in the confidence map of the tth frame, and π() represents the spherical projection;

[0113] S5: Construct a visual loss function based on the effective color binary mask Mc and the effective normal vector binary mask Mn. The formula is as follows:

[0114]

[0115] M vis =Mc t+1 ⊙(1-Mn t+1 )

[0116]

[0117] in, Represents the visual loss function value, Mvis The binary mask representing the valid pixels with low confidence in the normal vector; u′ t Denotes the inter-frame pose P predicted by the network t←t+1 The transformation matrix T obtained by conversion t←t+1 For each point p in the vertex graph of the t+1 frame t+1 After transforming to the tth frame, the projection point p′ is obtained t , and then use the camera projection function to find the projection point p′ t In the color image I t The projected pixel coordinates on Represents the projected pixel coordinate u′ t In the color image I t Color on, d vis (u t+1 ) represents the calculation of pixel u t+1 and u′ t function of the color difference between

[0118] S6: Construct the total loss function based on the geometric loss function and the visual loss function:

[0119]

[0120] in, Represents the total loss function value, and λ represents the weight of the visual loss function.

[0121] 3) In the color image sequence and the corresponding 3D point cloud sequence of the continuous moments to be tested, a pair of image inputs to be tested is composed of two adjacent frames of color images to be tested and the corresponding 3D point clouds. There are color images and 3D point clouds of the same frame between adjacent image input pairs to be tested. After preprocessing each pair of image inputs to be tested, the corresponding vertex graph to be tested and vertex shading graph input pairs are generated. Then all the vertex graph to be tested and vertex shading graph input pairs are sequentially input into the trained visual enhancement laser odometry network PoseNet, and the predicted inter-frame pose P corresponding to each pair of image inputs to be tested is output respectively. t←t+1 , P t←t+1 =[t t←t+1 , r t←t+1 ], where t t←t+1 is the translation vector, r t←t+1 represents the Euler angle vector;

[0122] 4) If Figure 3 As shown, according to the color image sequence to be tested, the posture online correction module is used to optimize and correct the predicted inter-frame postures corresponding to each pair of image inputs to be tested, and each corrected inter-frame posture is obtained;

[0123] 4) Specifically:

[0124] 4.1) Input each image to be tested into the corresponding predicted inter-frame pose P t←t+1 As the initial inter-frame pose of the current two frames

[0125] 4.2) According to the current inter-frame pose of the current two frames Calculate the corresponding matching points of each point in the vertex map of the next frame in the vertex map of the previous frame. Specifically, the inter-frame pose The transformation matrix T obtained by conversion t←t+1 For each point p in the vertex graph of the t+1 frame t+1 Transform to the vertex map of the tth frame to obtain the projection point pt′, and then determine the projection point p′ t Vertex graph V at frame t t Matching points in Calculate the relative standard deviation of each point in the vertex map of the next frame based on the corresponding matching points in the vertex map of the previous frame, so as to obtain the relative standard deviation map RSD of the current two frames;

[0126] 4.2), for each matching point p in the vertex graph of the previous frame t , first, the current matching point p t Points in a local window of 5×7 centered on As a candidate matching point, then calculate the current matching point p t The matching error between all candidate matching points is obtained to obtain the candidate matching error set d p , Finally, calculate the candidate matching error set d p The relative standard deviation is used as the current matching point p t The relative standard deviation.

[0127] 4.3) According to the relative standard deviation (RSD) of the current two frames, the following formula is used to filter and obtain the difficult binary mask: Used to mark 3D points that belong to hard examples:

[0128] RSD(u t+1 )=std(d p ) / mean(d p )

[0129]

[0130] Among them, RSD(u t+1 ) represents the vertex graph V of the t+1 frame t+1 midpoint p t+1 The corresponding relative standard deviation, std() represents the function for calculating the standard deviation, and mean() represents the function for calculating the mean. Represents pixel u t+1 Whether there is a difficult case, 1 means there is a difficult case, 0 means there is no difficult case;

[0131] 4.4) Based on the binary mask M of the local plane area geo and hard example binary masks Calculate the local planar area mask that only contains hard examples

[0132]

[0133] 4.5) Based on the visual loss function and the local planar region mask containing only hard examples Construct the loss function for difficult example mining, the formula is as follows:

[0134]

[0135]

[0136] in, represents the loss function value of hard example mining, represents the visual loss function value, represents the geometric loss function value calculated in the hard area, and λ represents the weight of the visual loss function.

[0137] 4.5) Taking the loss function of hard example mining as the optimization objective function, the pose between the current two frames is Perform gradient descent update to obtain the optimized inter-frame pose;

[0138] 4.6) Repeat 4.2)-4.5) N times to obtain the corrected inter-frame pose of the current two frames;

[0139] 4.7) Repeat 4.1)-4.6) to predict the inter-frame pose P corresponding to the remaining image input pairs to be tested t←t+1 Perform optimization correction to obtain all corrected inter-frame poses.

[0140] 5) The global pose is obtained by multiplying the corrected inter-frame poses in sequence. The global pose is used as the complete motion trajectory of the sequence to be measured, realizing the visually enhanced laser odometry method.

[0141] To verify the effectiveness of the present invention, we conducted experiments on the public visual-laser odometry dataset KITTIOdometry Benchmark and compared it with existing monocular visual odometry methods, binocular privileged visual odometry methods, and deep privileged visual odometry methods.

[0142] The KITTI Odometry Benchmark has 22 sequences, 00 to 21, of which sequences 00 to 10 provide ground truth poses for validation. Each sequence uses four monocular cameras (two grayscale cameras and two color cameras) and a Velodyne HDL-64 lidar to capture images and 3D point clouds, respectively, with the images and 3D point clouds aligned in time. Taking sequence 00 as an example, each camera captured 4541 images with a resolution of 1241x376, while the lidar captured 4541 frames of 3D point clouds. Data preprocessing yields a vertex map, normal map, and vertex shading map. For computational efficiency, the input image resolution is scaled to 192x624, and training samples consist of two consecutive color images, the vertex map, normal map, and vertex shading map. Like most current self-supervised visual odometry methods, this paper uses sequences 00 to 08 for training and sequences 09 to 10 for testing.

[0143] This paper mainly uses the official performance indicators in KITTI Odometry Benchmark to evaluate network performance: average translation relative drift t rel (unit: % / 100m), and the average rotational relative drift r rel (Unit: degrees / 100m). These two indicators are calculated on subsequences of length (100m, 200m, ..., 800m) of the entire sequence.

[0144] The details of network training are as follows:

[0145] In S02, the model parameters of the visually enhanced laser odometry network were randomly initialized using Xavier and optimized using the Adam optimizer. The batch size for each input to the network was set to 4. The learning rate was initially set to 0.0001 and was reduced to 0.8 times after every 30,000 iterations, for a total of 300,000 training iterations.

[0146] In S03, the model parameters of the visually enhanced laser odometry network trained in S02 are fixed. The predicted inter-frame pose of the visually enhanced laser odometry network is then optimized online using the pose online correction module. This optimization is performed using the Adam optimizer, with a learning rate of 0.025 for the translation vector and 0.0025 for the Euler angle vector.

[0147] The experiment mainly consists of two parts. The first part is the control variable experiment of each module in the present invention to illustrate the effectiveness of each module in the present invention. The second part is the comparative experiment between the present invention method and the existing mainstream visual odometry method.

[0148] Part I: Control variable experiments of each module in the present invention, used to illustrate the effectiveness of each module in the present invention;

[0149] Input modality and loss function validity: There are three input modalities: laser modality (L), visual modality (V) and visual-laser modality (V+L). L means only laser modality data is input, V means only visual modality data is input, and V+L means both visual and laser modality data are input. The training loss function of this invention is also divided into two categories: only geometric training loss function and use the total training loss function This yields three variants of the vision-enhanced laser odometry network, designated UnVELO1, UnVELO2, and UnVELO3. For intuitive comparison, we also tested the visual-laser odometry method (VLO) using a vision-led fusion strategy. The experimental results are shown in Table 1.

[0150] Experimental results show that simultaneous input of visual and laser modalities can achieve better performance, and the laser-dominated fusion method is superior to the vision-dominated method. In addition, the introduction of the visual loss function can further utilize the complementary advantages of the visual and optical modalities.

[0151] Table 1. Performance comparison of various variants of monocular visual odometry network on KITTI Odometry Benchmark

[0152]

[0153] Effectiveness of pose online correction: Here, the present invention tests the pose correction results of the UnVELO3 model under different iteration numbers and hard example mining strategies, which are expressed as UnVELO3+OC-N (where N represents the number of iterations). UnVELO3+OC-40w / oHSM means that no hard example mining is performed during the pose online correction. At the same time, the present invention also applies the pose online correction technology to the vision-dominated visual-laser odometry method VLO. In particular, since VLO also requires additional prediction of dense depth maps, the present invention also tests the simultaneous online optimization of predicted depth maps and inter-frame poses, which is recorded as VLO+OC-40Opt-Dep. The results are shown in Table 2. Figure 4 The present invention also shows the trajectory comparison of UnVELO3, UnVELO3+OC-40, VLO, VLO+OC-40 and VLO+OC-40Opt-Dep on sequences 09 and 10, as well as the distribution of translation and rotation errors. Figure 4 (a) shows the trajectory comparison of the above four models on sequence 09, where GT represents the true value of the trajectory; Figure 4(b) shows the average translation relative drift distributions of the above four models calculated on subsequences of length (100m, 200m, ..., 800m) on sequence 09; Figure 4 (c) shows the average rotational relative drift distributions calculated by the above four models on subsequences of length (100m, 200m, ..., 800m) on sequence 09. Figure 4 (d) Figure 4 (e) and Figure 4 (f) corresponds to the result on sequence 10.

[0154] Experimental results show that as the number of iterations increases, online pose correction technology can significantly improve the accuracy of predicted pose, while the laser-driven strategy method significantly outperforms the vision-driven method. Simultaneously optimizing the predicted depth during the optimization process can lead to performance degradation, demonstrating that the method of this invention can effectively avoid the influence of noise on the predicted depth. Furthermore, hard example mining technology can further enhance the effectiveness of online optimization.

[0155] Table 2. Performance comparison of the visual-lidar odometry network and the monocular visual odometry network under different training strategies on the KITTIOdometry Benchmark

[0156]

[0157] Part II: Table 3 shows a comparative experiment between our proposed method and existing deep learning odometry methods. The compared methods include three unsupervised end-to-end odometry methods, two hybrid odometry methods (deep learning odometry combined with a traditional global optimization module), and four odometry methods based on online optimization. All four online learning-based methods use vision-driven strategies and require simultaneous prediction of dense depth and inter-frame pose.

[0158] This method employs a laser-driven fusion strategy, requiring only inter-frame pose prediction for training. This avoids noise interference in depth prediction and achieves higher optimization efficiency, offering significant advantages over other vision-driven online optimization methods. Furthermore, the method utilizes vision-laser multimodal input, leveraging the complementary advantages of the two modalities. This method also offers superior performance compared to single-modal hybrid odometry methods combined with a global optimization module.

[0159] Table 3. Performance comparison with existing visual odometry methods on the KITTI Odometry Benchmark

[0160]

[0161]

[0162] The above description is merely a specific embodiment of the present invention and does not limit the present invention in any form. Any technician familiar with the present invention may use the technical content disclosed above to change or modify it into an equivalent example with equivalent changes. However, any simple modification, equivalent change and modification made to the above examples based on the technical essence of the present invention without departing from the technical solution of the present invention shall still fall within the scope of protection of the present invention.

Claims

1. A vision-enhanced laser odometry method based on online correction, characterized in that: The following steps are involved: 1) A color camera and lidar are used to jointly acquire a color image sequence and a corresponding 3D point cloud sequence of the scene. The color image sequence and the corresponding 3D point cloud sequence of the scene are then preprocessed to generate a corresponding vertex map sequence, normal map sequence, and vertex shading map sequence. The color image sequence and the corresponding vertex map sequence, normal map sequence, and vertex shading map sequence form a visual-laser odometry dataset. 2) Use the visual-laser odometry dataset to train the visually enhanced laser odometry network PoseNet to obtain the trained visually enhanced laser odometry network PoseNet; 3) In the color image sequence and the corresponding 3D point cloud sequence to be tested, a pair of image inputs is formed by two adjacent frames of the color image to be tested and the corresponding 3D point cloud. Each image input pair is preprocessed to generate a corresponding pair of vertex graph and vertex shading graph input to be tested. All the pairs of vertex graph and vertex shading graph inputs to be tested are then sequentially input into the trained visual enhancement laser odometry network PoseNet, and the predicted inter-frame pose corresponding to each image input pair to be tested is output respectively. 4) Based on the color image sequence to be tested, the posture online correction module is used to optimize and correct the predicted inter-frame postures corresponding to each pair of image inputs to be tested, and obtain each corrected inter-frame posture; 5) Multiply the corrected inter-frame poses in sequence to obtain the global pose.

2. A visually enhanced laser odometry method based on online correction according to claim 1, characterized in that: The specific embodiment of 1) is: A color camera collects color images of the scene at continuous moments to obtain a color image sequence, and then a lidar collects three-dimensional point clouds of the same scene at the same continuous moments to obtain a three-dimensional point cloud sequence; for each frame of three-dimensional point cloud in the three-dimensional point cloud sequence, each frame of three-dimensional point cloud is first spherically projected to obtain the corresponding vertex map, and then the normal vector of each point is solved based on the vertex map to obtain the corresponding normal vector map, and finally the corresponding color of each point on the vertex map in the corresponding color image is found through camera imaging projection to obtain the vertex shading map. After traversing the three-dimensional point cloud sequence, the corresponding vertex map sequence, normal vector map sequence and vertex shading map sequence are obtained based on the three-dimensional point cloud sequence processing. The visual-laser odometry dataset consists of the color image sequence and the corresponding vertex map sequence, normal vector map sequence and vertex shading map sequence.

3. The method of visually enhanced laser odometry based on online correction according to claim 1, characterized in that: In the above 2), the total loss function of the visually enhanced laser odometry network PoseNet is calculated based on the color image, vertex map, vertex shading map, normal vector map and predicted inter-frame pose. During each training, the input of the visually enhanced laser odometry network PoseNet is the vertex map of two consecutive frames in the vertex map sequence and the corresponding two-frame vertex shading map, and the output of the visually enhanced laser odometry network PoseNet is the predicted inter-frame pose of the current two frames.

4. The method of visually enhanced laser odometry based on online correction according to claim 1, characterized in that: The visually enhanced laser odometry network PoseNet consists of a feature encoder and a pose predictor connected together; The feature encoder is composed of 11 convolution modules connected in sequence. The input of the visual enhanced laser odometry network PoseNet is cascaded through the channels and then used as the input of the first convolution module. The output of the eleventh convolution module is used as the output of the feature encoder. The pose predictor includes two convolution modules and two convolution pooling units. The output of the feature encoder is used as the input of the twelfth convolution module. The twelfth convolution module is connected to the thirteenth convolution module, and the thirteenth convolution module is connected to the two convolution pooling units. The two convolution pooling units output the translation vector and Euler angle vector in the inter-frame pose respectively. The output of the two convolution pooling units is used as the output of the visual enhanced laser odometry network PoseNet.

5. The method of visually enhanced laser odometry based on online correction according to claim 4, characterized in that: The structures of the two convolutional pooling units are the same, which are composed of a convolution module, a global average pooling module and a convolution layer connected in sequence. The structures of all convolutional modules are the same, which are composed of a convolution layer, a batch normalization layer and an activation function layer connected in sequence.

6. The method of visually enhanced laser odometry based on online correction according to claim 3, characterized in that: The total loss function of the visual enhanced laser odometry network PoseNet is calculated based on the color image, vertex map, vertex shading map, normal vector map and predicted inter-frame pose, specifically: S1: Generate the corresponding valid vertex binary mask map Mv according to each vertex map. The calculation formula is as follows: Among them, Mv(u) represents the vertex validity at pixel position u in the valid vertex binary mask map Mv, and V(u) represents the point coordinates of pixel position u in the vertex map. is the indicator function, || || is the L2 norm; S2: Calculate the cosine similarity between the normal vectors of each point in the normal vector map and the four surrounding neighboring points to obtain the confidence of the current point, thereby obtaining the confidence map C corresponding to the normal vector map. Based on the normal vector map and the confidence map C, the corresponding effective normal vector binary mask Mn is generated. The calculation formula is as follows: Among them, δ c represents the confidence threshold, represents the set of 4 pixel positions adjacent to the pixel position u, Mn(u) represents the validity of the normal vector of the pixel position u in the valid normal vector binary mask Mn, C(u) represents the confidence of the normal vector of the pixel position u in the confidence map, N(u) represents the normal vector of the pixel position u in the normal map, N(u i ) represents the neighboring pixels u of pixel position u in the normal vector map i The normal vector of S3: Generate the corresponding effective color binary mask Mc based on the vertex coloring graph Vc. The calculation formula is as follows: Among them, Mc(u) represents the color validity of the vertex corresponding to the pixel position u in the effective color binary mask Mc, and Vc(u) represents the color of the pixel position u in the vertex coloring graph Vc; S4: Construct a geometric loss function based on the valid vertex binary mask map Mv and the valid normal vector binary mask Mn. The formula is as follows: M geo =Mv t+1 ⊙Mn t+1 p t =V t (π(p′ t )) in, Represents the geometric loss function value, p′ t Denotes the inter-frame pose P predicted by the network t←t+1 The transformation matrix T obtained by conversion t←t+1 For each point p in the vertex graph of the t+1 frame t+1 Transform to the tth frame to get the projection point, Represents the projection point p′ t Vertex graph V at frame t t The matching points in d geo () indicates calculating two matching points p′ t and The function of the point-surface matching error between geo Represents a binary mask of a local plane area, ⊙ represents element-by-element multiplication; || ||1 represents the operation of taking the L1 norm, M geo (u t+1 ) represents the local planar region binary mask M geo Medium pixel u t+1 The geometric error validity, Mv t+1 Represents the effective vertex binary mask map of the t+1th frame, Mn t+1 Represents the effective normal vector binary mask of the t+1th frame, V t represents the vertex graph of the t-th frame; C t () represents the confidence of each pixel position in the confidence map of the tth frame, and π() represents the spherical projection; S5: Construct a visual loss function based on the effective color binary mask Mc and the effective normal vector binary mask Mn. The formula is as follows: M vis =Mc t+1 ⊙(1-Mn t+1 ) in, Represents the visual loss function value, M vis The binary mask representing the valid pixels with low confidence in the normal vector; u′ t Denotes the inter-frame pose P predicted by the network t←t+1 The transformation matrix T obtained by conversion t←t+1 For each point p in the vertex graph of the t+1 frame t+1 After transforming to the tth frame, the projection point p′ is obtained t , and then use the camera projection function to find the projection point p′ t In the color image I t The projected pixel coordinates on Represents the projected pixel coordinate u′ t In the color image I t Color on, d vis (u t+1 ) represents the calculation of pixel u t+1 and u′ t A function of the color difference between S6: Construct the total loss function based on the geometric loss function and the visual loss function: in, Represents the total loss function value, and λ represents the weight of the visual loss function.

7. The method of visually enhanced laser odometry based on online correction according to claim 1, characterized in that: The specific aspects of 4) are: 4.1) Input each image to be tested into the corresponding predicted inter-frame pose P t←t+1 As the initial inter-frame pose of the current two frames 4.2) According to the current inter-frame pose of the current two frames Calculate the corresponding matching points of each point in the vertex map of the next frame in the vertex map of the previous frame, and calculate the relative standard deviation of each point in the vertex map of the next frame based on the corresponding matching points in the vertex map of the previous frame, so as to obtain the relative standard deviation map RSD of the current two frames; 4.3) According to the relative standard deviation (RSD) of the current two frames, the following formula is used to filter and obtain the difficult binary mask: RSD(u t+1 )=std(d p ) / mean(d p ) Among them, RSD(u t+1 ) represents the vertex graph V of the t+1th frame t+1 midpoint p t+1 The corresponding relative standard deviation, std() represents the function for calculating the standard deviation, and mean() represents the function for calculating the mean. Represents pixel u t+1 Whether there are difficult cases; 4.4) Based on the binary mask M of the local plane area geo and hard example binary masks Calculate the local planar area mask that only contains hard examples 4.5) Based on the visual loss function and the local planar region mask containing only hard examples Construct the loss function for difficult example mining, the formula is as follows: in, represents the loss function value of hard example mining, represents the visual loss function value, represents the geometric loss function value calculated in the hard example area, and λ represents the weight of the visual loss function; 4.5) Taking the loss function of hard example mining as the optimization objective function, the pose between the current two frames is Perform gradient descent update to obtain the optimized inter-frame pose; 4.6) Repeat 4.2)-4.5) N times to obtain the corrected inter-frame pose of the current two frames; 4.7) Repeat 4.1)-4.6) to predict the inter-frame pose P corresponding to the remaining image input pairs to be tested t←t+1 Perform optimization correction to obtain all corrected inter-frame poses.

8. The method of visually enhanced laser odometry based on online correction according to claim 7, characterized in that: In 4.2), for each matching point p in the vertex map of the previous frame t , first, the current matching point p t Points in a local window of 5×7 centered on As a candidate matching point, then calculate the current matching point p t The matching error between all candidate matching points is obtained to obtain the candidate matching error set d p ,Finally, calculate the candidate matching error set d p The relative standard deviation is used as the current matching point p t The relative standard deviation.

Images