A laser slam color attachment method combining image and point cloud features

Abstract

The application provides a laser SLAM color attaching method combining image and point cloud features, which comprises the following steps: acquiring point cloud data collected by a laser radar and image data collected by a camera; performing pose rectification and feature extraction on the point cloud data to obtain point cloud features with a rectified pose; performing feature extraction and semantic division on the image data to obtain image feature points with semantic labels; determining an optimized pose of the image according to the point cloud features and the image features through multi-source feature matching and joint optimization; and attaching color to the point cloud data according to the optimized pose, color information of the image and spatial structure information of the point cloud to generate colored point cloud. The method can alleviate the influence of timestamp errors and calibration errors to a certain extent and improve the accuracy and stability of color attachment by matching and optimizing the features extracted from the image and the point cloud.

Description

Technical Field

[0001] This invention relates to the field of point cloud processing, and more particularly to a laser SLAM colorization method that combines image and point cloud features. Background Technology

[0002] With the rapid development of robotics and 3D modeling technologies, Simultaneous Localization and Mapping (SLAM) technology has been widely applied in autonomous navigation, augmented reality, and 3D reconstruction. Laser SLAM, due to its high precision and stability, has become a current research hotspot. However, relying solely on laser point clouds for mapping lacks rich color information, limiting its application in visual perception and environmental understanding. Therefore, how to effectively combine image data with laser point clouds to achieve high-precision colored point clouds has become a key challenge in current technological development. Limitations of existing colorization algorithms...

[0003] Existing color-matching algorithms typically rely on several key factors when matching image color information with laser point clouds: Firstly, camera timestamp accuracy, meaning the color-matching process needs to accurately synchronize the camera's image capture time with the laser scanning time. The accuracy of the camera timestamp directly affects the accuracy of the color information matching with the point cloud. Secondly, camera and laser calibration, i.e., accurate intrinsic and extrinsic parameter calibration, is fundamental to ensuring consistency between image and point cloud data. Calibration errors can cause color information to be appended to incorrect locations, reducing the color-matching effect. Laser SLAM mapping depends on the laser's pose information; high-precision pose estimation is crucial for generating accurate point cloud maps.

[0004] However, existing coloring algorithms face the following problems in practical applications: For example, insufficient calibration accuracy: if the calibration accuracy of the camera and laser is insufficient, or if there are errors in the laser's pose estimation, the coloring effect will significantly decrease, especially in environments with drastic pose changes, where the matching of color information and point cloud positions is prone to errors. For example, inaccurate timestamps: especially consumer-grade cameras, which often cannot provide high-precision timestamp information. This leads to discrepancies in the time synchronization between image and laser data, further affecting the coloring effect and resulting in uneven color distribution or misalignment in the generated colored point cloud. For example, the influence of dynamic environments: when there are dynamic objects or rapid movement in the environment, the time synchronization and pose estimation upon which the coloring algorithm relies are difficult to handle, leading to unstable coloring results. Summary of the Invention

[0005] This invention provides a laser SLAM color assignment method that combines image and point cloud features to address the aforementioned technical deficiencies in the background art.

[0006] This invention provides a laser SLAM color assignment method that combines image and point cloud features, comprising the following steps:

[0007] Acquire point cloud data collected by lidar and image data collected by camera;

[0008] The point cloud data is subjected to attitude correction and feature extraction to obtain point cloud features with corrected attitude.

[0009] Feature extraction and semantic segmentation are performed on the image data to obtain image feature points with semantic labels;

[0010] Based on the point cloud features and the image features, the optimal pose of the image is determined through multi-source feature matching and joint optimization.

[0011] Based on the optimized pose, the color information of the image, and the spatial structure information of the point cloud, colors are added to the point cloud data to generate a colored point cloud.

[0012] Preferably, as one possible implementation, the step of performing pose correction and feature extraction on the point cloud data to obtain point cloud features with corrected pose includes:

[0013] The point cloud of the lidar is synchronized in time and its attitude is corrected using inertial measurement unit data to obtain a motion-compensated point cloud in the global coordinate system.

[0014] Line and surface features are extracted from the point cloud after attitude correction to construct a geometric structure feature map of the point cloud.

[0015] Preferably, as one possible implementation, the method of using inertial measurement unit data to perform time synchronization and attitude correction on the lidar point cloud includes:

[0016] Acceleration and angular velocity data of the inertial measurement unit are collected synchronously for the corresponding time period based on the timestamp of the lidar;

[0017] After filtering the data from the inertial measurement unit, the attitude change of the device during the scanning period is obtained by integration.

[0018] The attitude of the inertial measurement unit is converted into the attitude of the lidar according to the calibration relationship, and attitude interpolation is performed on the timestamp of each laser point.

[0019] The coordinates of each interpolated laser point are transformed to the global coordinate system, and then uniformly transformed to the reference pose of the current frame to complete the point cloud motion distortion correction.

[0020] Preferably, as one possible implementation, the extraction of line and surface features from the pose-corrected point cloud includes:

[0021] After denoising and filtering the point cloud, the gradient information of local neighborhood points is calculated, and edge line features are extracted through clustering and line fitting.

[0022] Principal component analysis is used to estimate the local normal vectors of the point cloud, and planar features are extracted by combining region growing and random sampling consensus algorithms to construct a point cloud structure map containing line and surface features.

[0023] Preferably, as one possible implementation, the step of performing feature extraction and semantic segmentation on the image data to obtain image feature points with semantic labels includes:

[0024] Scale-invariant feature transformation key points are extracted from images and descriptors are generated; a semantic segmentation network is used to perform pixel-level classification of images, identifying categories such as rod-shaped objects, buildings, ground, and dynamic targets;

[0025] Based on the semantic segmentation results, the extracted image feature points are divided into line-type feature points, planar feature points, and dynamic target feature points, and feature points on dynamic targets are filtered out.

[0026] Preferably, as one possible implementation, the step of determining the optimized pose of the image based on the point cloud features and the image features through multi-source feature matching and joint optimization includes:

[0027] Based on the optimized pose of the lidar and the calibration relationship between sensors, the initial pose of the image is calculated; based on the initial pose, spatial neighborhood search and feature descriptor matching are performed in the existing image set to obtain corresponding points between images;

[0028] Based on the corresponding points, construct collinearity constraint equations and jointly solve for the coordinates of the three-dimensional spatial points corresponding to the corresponding points.

[0029] Based on semantic labels, points with the same name are divided into planar and linear categories, and distance residual constraints are constructed with the surface features and line features in the point cloud map, respectively.

[0030] By combining all planar residuals, linear residuals, and collinear constraints, the image pose is nonlinearly optimized to obtain the optimized image pose.

[0031] Preferably, as one possible implementation; the step of classifying points of the same name into planar and linear categories based on semantic labels, and constructing distance residual constraints with the area features and line features in the point cloud map respectively, includes:

[0032] For points of the same name that are classified as planes, the distance residual from the 3D point to the plane is constructed by fitting a local plane in the corresponding neighborhood of the point cloud map through principal component analysis and random sampling consistency.

[0033] For points of the same name that are classified as straight lines, a spatial straight line is fitted in the corresponding neighborhood of the point cloud map using principal component analysis, and the angle or distance residual from the 3D point to the straight line is constructed.

[0034] Preferably, as one possible implementation; the step of appending color to the point cloud data based on the optimized pose, the color information of the image, and the spatial structure information of the point cloud includes:

[0035] For each spatial point in the point cloud, based on its position and normal vector, images with an angle less than a preset threshold with respect to the normal vector of that point are selected from all candidate images.

[0036] The spatial points are projected onto the selected candidate images to obtain the corresponding pixel positions;

[0037] Determine whether the pixel location belongs to a dynamic target area. If it does, exclude the image and re-filter.

[0038] When a suitable non-dynamic region image is found, the color value of the pixel is assigned to the corresponding spatial point to complete the point cloud coloring.

[0039] Preferably, as one possible implementation; after filtering out images whose angle with the normal vector of the point is less than a preset threshold, the method further includes: sorting the candidate images according to the distance between the spatial point and the optical center of the image, and preferentially selecting the images that are closer to the point for projection and color assignment.

[0040] The technical solutions provided by the embodiments of the present invention may include the following beneficial effects:

[0041] This invention discloses a laser SLAM colorization method combining image and point cloud features. The proposed matching method for generating colored point clouds, based on image features and point cloud spatial features, offers an effective solution. By matching and optimizing features extracted from images and point clouds, the impact of timestamp and calibration errors can be mitigated to some extent, improving the accuracy and stability of colorization. This method no longer relies on high-precision time synchronization and strict calibration accuracy; instead, it achieves more robust colorization results by jointly optimizing the geometric features of images and the spatial features of point clouds. Especially when using low-cost consumer cameras, the feature-matching method effectively improves colorization results, broadening the applicability of SLAM technology in practical applications. Attached Figure Description

[0042] Figure 1 This is a schematic diagram of a case study of a laser SLAM color assignment method that combines image and point cloud features according to the present invention.

[0043] Figure 2 This is a flowchart illustrating a specific operation of a laser SLAM colorization method combining image and point cloud features, according to Embodiment 2 of the present invention.

[0044] Figure 3The effect of coloring a traditional distance-based coloring algorithm; Figure 4 This is a color-adding effect diagram of a laser SLAM color-adding method combining image and point cloud features according to Embodiment 2 of the present invention; Detailed Implementation

[0045] To enable those skilled in the art to better understand the technical solutions in this specification, the technical solutions in the embodiments of this specification will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this specification, and not all embodiments. Based on the embodiments in this specification, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of this specification.

[0046] Example 1

[0047] Embodiment 1 of the present invention provides a laser SLAM color assignment method that combines image and point cloud features, including the following steps:

[0048] S10. Acquire point cloud data collected by lidar and image data collected by camera;

[0049] S20. Perform attitude correction and feature extraction on the point cloud data to obtain point cloud features with corrected attitude.

[0050] S30. Perform feature extraction and semantic segmentation on the image data to obtain image feature points with semantic labels;

[0051] S40. Based on the point cloud features and the image features, determine the optimal pose of the image through multi-source feature matching and joint optimization;

[0052] S50. Based on the optimized posture, the color information of the image, and the spatial structure information of the point cloud, the color is added to the point cloud data to generate a colored point cloud.

[0053] Analysis of the above technical solution reveals that this method achieves high-precision colored point cloud generation by fusing point cloud and image data. First, geometric structural features are extracted from the point cloud, and semantically meaningful visual features are extracted from the image, laying the foundation for multi-source feature alignment. Second, by jointly optimizing and matching the two types of features, the image pose is accurately calibrated, resolving the pose error problem between sensors. Finally, based on the optimized pose, the image color is accurately mapped to the point cloud, generating a colored point cloud with both geometric accuracy and rich color information. This process enhances the visual realism and semantic integrity of the point cloud, providing multimodal data support for 3D scene understanding.

[0054] Preferably, as one possible implementation, the step of performing pose correction and feature extraction on the point cloud data to obtain point cloud features with corrected pose includes:

[0055] The point cloud of the lidar is synchronized in time and its attitude is corrected using inertial measurement unit data to obtain a motion-compensated point cloud in the global coordinate system.

[0056] Line and surface features are extracted from the point cloud after attitude correction to construct a geometric structure feature map of the point cloud.

[0057] This technical solution uses inertial measurement unit (IMU) data to perform motion compensation on point clouds, eliminating distortions caused by carrier motion during lidar scanning and obtaining stable point clouds in a global coordinate system. Based on this, line and surface geometric features are extracted to construct a structural map. This step significantly improves the spatial consistency and feature representation reliability of the point cloud, providing a high-precision geometric constraint basis for subsequent accurate matching with image features.

[0058] Preferably, as one possible implementation, the method of using inertial measurement unit data to perform time synchronization and attitude correction on the lidar point cloud includes:

[0059] Acceleration and angular velocity data of the inertial measurement unit are collected synchronously for the corresponding time period based on the timestamp of the lidar;

[0060] After filtering the data from the inertial measurement unit, the attitude change of the device during the scanning period is obtained by integration.

[0061] The attitude of the inertial measurement unit is converted into the attitude of the lidar according to the calibration relationship, and attitude interpolation is performed on the timestamp of each laser point.

[0062] The coordinates of each interpolated laser point are transformed to the global coordinate system, and then uniformly transformed to the reference pose of the current frame to complete the point cloud motion distortion correction.

[0063] This technical solution, based on timestamp synchronization and filtering, utilizes inertial measurement unit data integration to obtain continuous attitude changes of the device, and converts these changes into lidar attitude through calibration relationships, performing interpolation correction on each lidar point. This step unifies the discrete point cloud to the same reference attitude, effectively eliminating point cloud stretching or distortion caused by motion, improving the geometric accuracy of the point cloud in the global coordinate system, and creating a prerequisite for feature extraction and cross-sensor alignment.

[0064] Preferably, as one possible implementation, the extraction of line and surface features from the pose-corrected point cloud includes:

[0065] After denoising and filtering the point cloud, the gradient information of local neighborhood points is calculated, and edge line features are extracted through clustering and line fitting.

[0066] Principal component analysis is used to estimate the local normal vectors of the point cloud, and planar features are extracted by combining region growing and random sampling consensus algorithms to construct a point cloud structure map containing line and surface features.

[0067] It should be noted that the above steps involve extracting line features by computing the local gradient of the denoised point cloud and clustering it, and then extracting surface features by combining principal component analysis and random sampling consensus algorithm. This method can robustly identify structural elements (such as building edges and ground planes) from the point cloud, construct a hierarchical geometric feature map, enhance the point cloud's ability to represent scene structure, and provide rich geometric references for subsequent association with image semantic features.

[0068] Preferably, as one possible implementation, the step of performing feature extraction and semantic segmentation on the image data to obtain image feature points with semantic labels includes:

[0069] Scale-invariant feature transformation key points are extracted from images and descriptors are generated; a semantic segmentation network is used to perform pixel-level classification of images, identifying categories such as rod-shaped objects, buildings, ground, and dynamic targets;

[0070] Based on the semantic segmentation results, the extracted image feature points are divided into line-type feature points, planar feature points, and dynamic target feature points, and feature points on dynamic targets are filtered out.

[0071] This technical solution extracts scale-invariant feature transformation key points from images and assigns them descriptors. Simultaneously, it classifies pixels using a semantic segmentation network to identify different object categories in the scene. Furthermore, it combines semantic labels to classify feature points into linear, planar, etc., and removes feature points on dynamic targets. This step introduces semantic information while preserving visual feature discriminability, reducing interference from dynamic objects, and improving the stability and semantic consistency of feature matching.

[0072] Preferably, as one possible implementation, the step of determining the optimized pose of the image based on the point cloud features and the image features through multi-source feature matching and joint optimization includes:

[0073] Based on the optimized pose of the lidar and the calibration relationship between sensors, the initial pose of the image is calculated; based on the initial pose, spatial neighborhood search and feature descriptor matching are performed in the existing image set to obtain corresponding points between images;

[0074] Based on the corresponding points, construct collinearity constraint equations and jointly solve for the coordinates of the three-dimensional spatial points corresponding to the corresponding points.

[0075] Based on semantic labels, points with the same name are divided into planar and linear categories, and distance residual constraints are constructed with the surface features and line features in the point cloud map, respectively.

[0076] By combining all planar residuals, linear residuals, and collinear constraints, the image pose is nonlinearly optimized to obtain the optimized image pose.

[0077] It should be noted that the above steps involve obtaining corresponding points through feature matching between images based on the initial pose, reconstructing the 3D point coordinates through collinearity constraints, and then constructing residual constraints based on semantic classification using the corresponding points and the midline and surface features of the point cloud to jointly optimize the image pose. This process, through multi-source geometry and semantic constraints, jointly corrects the image pose, significantly improving the accuracy of pose estimation and ensuring the accuracy of the projection relationship during subsequent color mapping.

[0078] Preferably, as one possible implementation; the step of classifying points of the same name into planar and linear categories based on semantic labels, and constructing distance residual constraints with the area features and line features in the point cloud map respectively, includes:

[0079] For points of the same name that are classified as planes, the distance residual from the 3D point to the plane is constructed by fitting a local plane in the corresponding neighborhood of the point cloud map through principal component analysis and random sampling consistency.

[0080] For points of the same name that are classified as straight lines, a spatial straight line is fitted in the corresponding neighborhood of the point cloud map using principal component analysis, and the angle or distance residual from the 3D point to the straight line is constructed.

[0081] It should be noted that the above steps are for planar points of the same name, constructing the point-to-plane distance residual by fitting a local plane; for linear points of the same name, constructing the point-to-line residual by fitting a spatial line. This differential residual modeling based on semantic classification fully utilizes the structural priors of the point cloud and the semantic information of the image, enhancing the robustness of the optimization system and improving the adaptability of pose optimization to scene structural features.

[0082] Preferably, as one possible implementation; the step of appending color to the point cloud data based on the optimized pose, the color information of the image, and the spatial structure information of the point cloud includes:

[0083] For each spatial point in the point cloud, based on its position and normal vector, images with an angle less than a preset threshold with respect to the normal vector of that point are selected from all candidate images.

[0084] The spatial points are projected onto the selected candidate images to obtain the corresponding pixel positions;

[0085] Determine whether the pixel location belongs to a dynamic target area. If it does, exclude the image and re-filter.

[0086] When a suitable non-dynamic region image is found, the color value of the pixel is assigned to the corresponding spatial point to complete the point cloud coloring.

[0087] Analysis of the above technical solutions reveals that: this solution selects images with suitable viewing angles based on the normal vectors of each point in the point cloud, avoiding texture stretching or occlusion caused by excessively large viewing angles; it also determines whether pixels are located in dynamic areas through projection, eliminating color interference from dynamic objects. This step achieves accurate and robust assignment of color information, ensuring that the colored point cloud has realistic colors and spatial consistency, improving the reliability of visualization effects and subsequent applications.

[0088] Preferably, as one possible implementation; after filtering out images whose angle with the normal vector of the point is less than a preset threshold, the method further includes: sorting the candidate images according to the distance between the spatial point and the optical center of the image, and preferentially selecting the images that are closer to the point for projection and color assignment.

[0089] Analysis of the above technical solutions reveals that, based on selecting images from suitable viewing angles, this solution further prioritizes nearby images for projection color mapping, sorting them by distance. This strategy reduces color noise caused by image resolution limitations or blurred textures in distant images, improves the clarity and detail retention of color mapping, and enables the colored point cloud to have higher quality texture representation in near-scene areas.

[0090] Embodiment 1 of this invention provides a laser SLAM color assignment method that combines image and point cloud features. The proposed matching method, based on image features and point cloud spatial features, offers an effective solution. By matching and optimizing features extracted from images and point clouds, the impact of timestamp and calibration errors can be mitigated to some extent, improving the accuracy and stability of color assignment. This method no longer relies on high-precision time synchronization and strict calibration accuracy; instead, it achieves a more robust color assignment effect by jointly optimizing the geometric features of images and the spatial features of point clouds. Especially when using low-cost consumer-grade cameras, the feature-matching method effectively improves the color assignment effect, broadening the applicability of SLAM technology in practical applications.

[0091] Example 2

[0092] Based on the above-described Embodiment 1, this invention relates to Embodiment 2, which relates to a laser SLAM color assignment method combining image and point cloud features, specifically a laser simultaneous localization and mapping (SLAM) color assignment method combining image and point cloud features. This method is a detailed technical solution. First, the method uses a laser to perform laser SLAM mapping, and obtains the camera's pose through camera and laser calibration. Then, environmental features, such as spatial lines and surfaces, are extracted from the point cloud, and corresponding image points are extracted from the image. These image points are projected onto the point cloud, and based on the projection results, they are determined to be planar points or spatial edge points, thereby constructing a residual equation. Simultaneously, combined with the collinearity of corresponding points in the image, joint optimization is performed to adjust the image pose. Finally, color information is appended to the point cloud using the optimized image pose, thereby achieving high-precision color point cloud generation.

[0093] The specific steps of implementing the method of the present invention are described in detail below:

[0094] S101. Coarse calibration of sensor data: First, coarse calibration is performed on the inertial measurement unit (IMU), lidar and camera to determine the initial spatial relationship between each sensor.

[0095] S102, Sensor Data Synchronization: Synchronize sensor data, including IMU, camera and LiDAR. IMU and LiDAR need to be precisely synchronized in time, while image time can be coarsely aligned to ensure initial alignment of different sensor data in time.

[0096] S103, IMU Data Correction: This step utilizes IMU data to correct the attitude of the lidar data, rectifying errors caused by sensor movement and improving the attitude accuracy of the radar data. The main steps include: Synchronous Acquisition: Based on the timestamps of the laser scanner data, synchronously acquire inertial measurement unit (IMU) data for the corresponding time period, including IMU linear acceleration and angular velocity measurements; Data Filtering: Use a Kalman filter algorithm to filter the IMU data, removing measurement noise and optimizing data quality to improve the accuracy of subsequent processing. The attitude (rotation and translation) of the IMU during the scanning period is estimated by integrating angular velocity data (to determine direction) and double-integrated acceleration data (to determine position). The synchronous attitude of the laser with the same frequency as the IMU is obtained through the calibration relationship between the IMU and the laser; Attitude Interpolation: Interpolate the timestamps of each laser point in the laser scanner data to obtain the device attitude corresponding to each time point. Interpolation methods can be linear interpolation or higher-precision curve fitting interpolation to ensure that the estimated attitude is as close as possible to the actual attitude. Coordinate transformation: Each laser point is transformed to the global coordinate system according to its corresponding attitude using a transformation matrix. This operation can effectively correct positional deviations caused by device motion. Subsequently, the laser points in the global coordinate system are uniformly transformed to the device attitude at the end of the current frame to ensure data consistency and accuracy.

[0097] S104. Point Cloud Feature Extraction: Feature points are extracted from each frame of the point cloud. Edge detection and plane fitting algorithms are used to extract line and surface features from the point cloud. Edge Detection Extraction: First, noise reduction and filtering are performed on the point cloud to improve data quality and reduce errors in subsequent processing. The gradient of each point in the point cloud is calculated, and areas with high gradients may be edges of objects. This can be achieved by comparing the position difference between each point and its neighbors. From the extracted edge points, clear line features are obtained by fitting straight lines through cluster analysis or least squares. Plane Fitting Extraction: In this embodiment, Principal Component Analysis (PCA) is used to estimate the normal vector of the local point cloud data and find the principal direction. Based on the similarity of the point normal vectors, a region growing algorithm is used to cluster points that constitute the same plane. For each candidate region of the plane, a plane fitting algorithm (such as the Random Sample Consensus Algorithm RANSAC) is applied to accurately define the position and orientation of each plane.

[0098] S105. Image Feature Extraction and Semantic Segmentation: Simultaneously, image feature points are extracted from the image, such as through keypoint detection using SIFT or Harris operators, and semantic segmentation of the image is performed using deep learning algorithms (e.g., identifying information such as buildings, ground, and poles). Based on the semantic information, feature points are divided into planar points and linear points. Feature points located on pole-like objects are classified as linear points, while feature points located on buildings and the ground are classified as planar points. The steps are as follows:

[0099] S1051, Feature Point Extraction: This embodiment uses SIFT (Scale-Invariant Feature Transform) for feature extraction. This algorithm can detect rotation-invariant and scale-invariant keypoints and generate a descriptor for each feature point. SIFT searches for keypoints on images at various scales and matches them using local image features. Utilizing the sensitivity of local window views to brightness changes, it detects edge intersections with high-discrepancy directions; these are typically good feature points suitable for 3D image reconstruction and point tracking.

[0100] S1052. Semantic Segmentation Using Deep Learning: Utilizing the advanced segmentation network U-Net, images are classified at the pixel level. These models can learn the complex relationships between pixels in an image and their surrounding context, achieving accurate scene analysis. Currently, they are mainly categorized into rod-shaped objects, buildings, ground, and dynamically moving targets.

[0101] S1053. Semantic Segmentation of Image Feature Points: Based on the semantic segmentation results of the deep learning model, image feature points are classified. Feature points located on rod-shaped objects are classified as line points; while feature points located on buildings and the ground are classified as planar points. These areas typically appear as large, flat, or continuous surfaces in the image. Dynamic targets are often moving vehicles, pedestrians, etc., on pixels with motion blur. In this embodiment, feature points located on dynamic targets need to be filtered out to prevent them from interfering with the optimization results. Based on the image feature points and the semantic segmentation results, each extracted image feature point is assigned a semantic label.

[0102] S106. Initial map construction: Use the radar data after the first frame of correction as the initial map to build a basic point cloud map.

[0103] S107. Data Matching and Attitude Optimization: For each subsequent frame of radar data, matching is performed based on the extracted features and the initial map to construct point-to-line constraints and point-to-area constraints, and the radar attitude is optimized. The steps are as follows:

[0104] Point cloud matching: For the extracted line features and surface features, perform neighborhood search matching with the features in the base map to obtain the corresponding map feature line equations and surface equations;

[0105] Construct residual equations that minimize the distance from the extracted planar points to the map plane, and simultaneously minimize the distance from the line feature points to the map line features. The line feature residual equation can be constructed as follows:

[0106] ;

[0107] in, For the extracted line parameters, For the current frame pose that needs optimization, The extracted line features.

[0108] The surface feature residual equation can be constructed as follows:

[0109] ;

[0110] in, For the extracted plane parameters, For the current frame pose that needs optimization, The extracted surface features.

[0111] Based on the sets of linear and surface feature residuals, this embodiment uses a nonlinear least squares method for iteration, and the residual set can be expressed as:

[0112] ;

[0113] in Using a predefined paradigm, the residuals of lines and surfaces are optimized by solving this function to obtain the final radar pose that needs optimization. This embodiment can use the Ceres solver to set the number of iterations for multiple iterations.

[0114] S108. Image Attitude Estimation: Using the optimized LiDAR attitude and the known transformation matrix between the LiDAR and the camera, this embodiment can calculate the estimated attitude of the current image. This transforms the LiDAR attitude into the camera coordinate system using the transformation matrix. Specifically, this involves mapping the LiDAR's position and rotation information, as well as the calibration parameters between the camera and the LiDAR, into the camera's coordinate system, thereby obtaining the camera's precise position and orientation in the global coordinate system.

[0115] ;in, For the camera pose, The pose of the lidar. The calibration parameters for the lidar to the camera.

[0116] S109. Image Feature Matching: After obtaining the initial image pose, search for photos in the spatial neighborhood and perform image feature matching to obtain corresponding matching points for the images. The steps include:

[0117] S1091. Establishing a neighborhood search space: Based on the acquired initial image pose, determine the approximate position and orientation of the image in space. On this basis, for all image location points, this embodiment constructs a KDTREE and performs a neighborhood search on the current image's position to obtain other images spatially adjacent to the target image.

[0118] S1092. Perform feature matching: Based on the image feature points obtained above, this embodiment uses feature descriptors to match feature points between different images. This embodiment uses FLANN (Fast Library for Approximate Nearest Neighbors) for fast nearest neighbor search. After finding a matching point, this embodiment introduces some matching filtering mechanisms to exclude incorrect matches, such as ratio test. The ratio of the nearest neighbor distance to the second nearest neighbor distance of one feature point must be lower than a certain threshold to ensure the uniqueness and accuracy of the match.

[0119] S1093. Confirmation of Corresponding Points: Evaluate the matching results and confirm corresponding points. Corresponding points refer to corresponding feature points in two or more images. They represent projections of the same three-dimensional point. Geometric consistency checks, such as constraints from the fundamental matrix or homography matrix, are used to verify whether the matching points conform to homography or stereo geometric constraints. The fundamental matrix represents the geometric relationship between the two images, and the constraints can be expressed as:

[0120] ;in, For the current corresponding feature point, This is the transpose of the coordinates of corresponding points with the same name. The corresponding fundamental matrix constraints;

[0121] The homography matrix represents the transformation from one plane viewpoint to another, and its expression can be given as: ; in, For the current corresponding feature point, For the coordinates of the corresponding points, This is the corresponding homography matrix constraint.

[0122] S110. Point Cloud Structure Information Extraction: Based on the initial position of the image, search for its neighborhood in the point cloud map and calculate the normal to extract the structural information in the point cloud, such as significant planes and columns. The steps are as follows:

[0123] S1101, Neighborhood Search: First, based on the initial location of the provided imagery, determine the corresponding spatial location in the point cloud map. Set a suitable search radius to ensure sufficient coverage for structural analysis. Using a spatial index such as KDTREE, quickly retrieve all point cloud data within this neighborhood for subsequent processing and analysis.

[0124] S1102, Normal Vector Calculation: For each point in the neighborhood, calculate the normal vector based on the distribution of its surrounding points. This is typically achieved by fitting the point and its nearest neighbors to an optimal plane in the least-squares sense. The normal vector of each point provides directional information about the surface at that point, which is crucial for subsequent structure identification. To reduce the impact of calculation errors and external noise, the calculated normal vectors are smoothed. In this embodiment, a weighted average is used to consider the normal vector directions of neighboring points.

[0125] S1103, Plane Detection: By clustering and analyzing normal vectors, point clouds with similar orientations are grouped together as a single plane candidate. The RANSAC (Random Sample Consensus) algorithm is used to further validate and optimize the plane model, extracting significant planar structures such as walls and ground surfaces. After extracting the planar point clouds, a point cloud planar map is constructed, which is then registered with planar feature points in the image.

[0126] S1104. Line Detection: Identifying point cloud clusters with good straight-line model fitting performance. PCA (Positive Linear Aspect) analysis is performed on neighboring points to determine the eigenvalues ​​corresponding to their covariance matrix. If two eigenvalues ​​are significantly smaller than the third eigenvalue, this embodiment considers the point a line space point. These points typically belong to rod-like structures. This embodiment calculates the principal direction of the line corresponding to each point. After point cluster filtering, it is used to construct a point cloud straight-line map, facilitating subsequent registration with straight-line feature points in the image.

[0127] S111. Construction of Multidimensional Constraint Equations and Attitude Optimization: Based on the initial pose of the image and the matched corresponding points, its spatial 3D points are calculated, and a neighborhood search is performed on the map. According to semantic information, image planar corresponding points are matched with planar points in the point cloud, and image line corresponding points are matched with line points in the point cloud. Combining the collinearity constraints of image location points, image corresponding points, and corresponding spatial 3D points, multidimensional constraint equations are constructed to optimize the image pose.

[0128] First, image-corresponding points can be jointly observed from multiple images, meaning the same ground feature can appear in multiple images. Therefore, on each image, the image location point, the image-corresponding point, and the corresponding 3D spatial point all lie on a straight line, allowing us to construct the following constraint formula:

[0129] That is, formula (1); where, , The pose and position of one of the images corresponding to the same point in each image. for The transpose of , where K is the intrinsic parameter matrix for each corresponding image. This is the transpose of the intrinsic parameter matrix for each corresponding image. These are the homogeneous coordinates of the pixel coordinates for each corresponding point in the image. Let be the 3D spatial coordinates of the corresponding point of the image to be determined.

[0130] By optimizing the above equations, this embodiment can obtain the spatial 3D points corresponding to all image-corresponding points. .

[0131] Image plane corresponding point residual construction: After obtaining the spatial 3D point, if the corresponding image corresponding point is a planar point, this embodiment needs to search for neighboring points in the laser planar point set map. PCA principal component analysis is then performed using these neighboring points. This embodiment can obtain the parameters of the spatial plane and simultaneously determine whether the point is a planar point in the point cloud. The calculation formula is as follows:

[0132] ;

[0133] Where A, B, C, and D are the fitted plane parameters. , y, z represents the corresponding neighborhood point of the search. If the neighborhood points can fit a good plane, then... The value is relatively small; in this embodiment, a plane error of less than 3cm is generally considered a plane point.

[0134] Finally, in this embodiment, all planar points and their corresponding image-matching points are combined to construct constraint equations:

[0135] ;

[0136] in , , z is a 3D point calculated from the image matching corresponding point based on its pose and the corresponding camera intrinsic parameters, obtained by least squares optimization using formula (1). , , , Search for planar points that fit the neighborhood of a 3D point in a point cloud map.

[0137] Image line corresponding point residual construction: If the image corresponding point of the spatial point is a line feature point, this embodiment searches for the neighborhood point set of the current point in the point cloud line map based on the point location. PCA principal component analysis is performed through the neighborhood points. This embodiment can obtain the position and direction of the line. By judging the distance from all points in the neighborhood point set to the line, the distance from all points in the neighborhood point set to the fitted line is made less than a certain value, such as 5cm. The fitting formula is as follows:

[0138] );

[0139] in, Based on the searched neighboring points, The centroid is obtained by solving over all neighborhood points. The principal direction of the line fitted to the searched neighborhood points;

[0140] Finally, in this embodiment, all straight line points and their corresponding image matching points are combined to construct the constraint equations: ;

[0141] in Formula (1) for matching corresponding points in an image is obtained by optimizing 3D points using least squares. , for Search for spatial line parameters that fit a neighborhood in a straight point cloud map.

[0142] Pose optimization: For pose-corresponding points involved in constructing the optimized image, extract the residuals of the corresponding points in the image plane according to the steps described above. Residual at corresponding points of the image line The following constraint equations are constructed: ;in Based on the predefined paradigm, by solving this function, the residuals of corresponding points in the plane and the corresponding points in the line are optimized to obtain the final pose that needs to be optimized. In this embodiment, a nonlinear least squares optimization algorithm can be used to perform multiple iterations to obtain the optimal pose for each frame of the image.

[0143] S112. Point Cloud Color Assignment: Utilizing the optimized image pose, color information is accurately applied to the point cloud based on the image pose. Conventionally, the most recent or most recent image is used for color assignment. This paper innovatively selects images based on the results of deep learning segmentation and the normal information of the point cloud.

[0144] This embodiment first obtains candidate images of the current spatial point based on the location of the spatial point and the optimized position of the image, and sorts them according to their distance from the spatial point. Then, for the candidate images, it calculates the angle between the normal of the current image and the spatial point, and the calculation formula is as follows:

[0145] ;

[0146] in, For the current spatial point, The location of the corresponding image. The normal to the current spatial point has been calculated using formula S110. The value is used for judgment. If the current value is less than 90°, the image is considered a correct candidate image, and the next calculation is performed. The current point is projected onto the image, and its pixel position is calculated using the following formula: ;

[0147] in, For spatial points on the map, This is the transpose matrix of the corresponding image's pose. For the location of the corresponding image, It is the inverse of the camera intrinsic parameter matrix. These are the pixels on the corresponding image.

[0148] After obtaining the pixel location, this embodiment determines whether the pixel location is on a dynamic target area of ​​the image. Semantic information for each pixel is obtained in step S105. If the pixel is on a dynamic target, this embodiment needs to re-filter the images of the candidate area, repeating step S112 until a suitable photo is selected or the candidate photo range is narrowed down. After finding a suitable photo, this embodiment assigns the obtained pixel value to the current spatial point. Thus, this embodiment can obtain its accurate color information, thereby realizing the point cloud color assignment function.

[0149] In summary, this invention discloses a laser SLAM colorization method combining image and point cloud features. The proposed matching method for generating colored point clouds, based on image features and point cloud spatial features, offers an effective solution. By utilizing features extracted from images and point clouds for matching and optimization, the impact of timestamp and calibration errors can be mitigated to some extent, improving the accuracy and stability of colorization. This method no longer relies on high-precision time synchronization and strict calibration accuracy; instead, it achieves more robust colorization results by jointly optimizing the geometric features of images and the spatial features of point clouds. Especially when using low-cost consumer-grade cameras, the feature-matching method effectively improves colorization results and broadens the applicability of SLAM technology in practical applications.

[0150] The above description is merely an embodiment of the present invention and does not limit the patent scope of the present invention. Any equivalent structural or procedural transformations made based on the content of the present invention's specification and drawings, or direct or indirect applications in other related technical fields, are similarly included within the patent protection scope of the present invention.

Claims

1. A laser SLAM color assignment method combining image and point cloud features, characterized in that, include: Acquire point cloud data collected by lidar and image data collected by camera; The point cloud data is subjected to attitude correction and feature extraction to obtain point cloud features with corrected attitude. Feature extraction and semantic segmentation are performed on the image data to obtain image feature points with semantic labels; Based on the point cloud features and the image features, the optimal pose of the image is determined through multi-source feature matching and joint optimization. Based on the optimized pose, the color information of the image, and the spatial structure information of the point cloud, colors are added to the point cloud data to generate a colored point cloud.

2. The method according to claim 1, characterized in that, The process of performing pose correction and feature extraction on the point cloud data to obtain point cloud features with corrected pose includes: The point cloud of the lidar is synchronized in time and its attitude is corrected using inertial measurement unit data to obtain a motion-compensated point cloud in the global coordinate system. Line and surface features are extracted from the point cloud after attitude correction to construct a geometric structure feature map of the point cloud.

3. The method according to claim 2, characterized in that, The method of using inertial measurement unit data to perform time synchronization and attitude correction on lidar point clouds includes: Acceleration and angular velocity data of the inertial measurement unit are collected synchronously for the corresponding time period based on the timestamp of the lidar; After filtering the data from the inertial measurement unit, the attitude change of the device during the scanning period is obtained by integration. The attitude of the inertial measurement unit is converted into the attitude of the lidar according to the calibration relationship, and attitude interpolation is performed on the timestamp of each laser point. The coordinates of each interpolated laser point are transformed to the global coordinate system, and then uniformly transformed to the reference pose of the current frame to complete the point cloud motion distortion correction.

4. The method according to claim 2, characterized in that, The extraction of line and surface features from the pose-corrected point cloud includes: After denoising and filtering the point cloud, the gradient information of local neighborhood points is calculated, and edge line features are extracted through clustering and line fitting. Principal component analysis is used to estimate the local normal vectors of the point cloud, and planar features are extracted by combining region growing and random sampling consensus algorithms to construct a point cloud structure map containing line and surface features.

5. The method according to claim 1, characterized in that, The step of extracting features and semantically segmenting the image data to obtain image feature points with semantic labels includes: Scale-invariant feature transformation key points are extracted from images and descriptors are generated; a semantic segmentation network is used to perform pixel-level classification of images, identifying categories such as rod-shaped objects, buildings, ground, and dynamic targets; Based on the semantic segmentation results, the extracted image feature points are divided into line-type feature points, planar feature points, and dynamic target feature points, and feature points on dynamic targets are filtered out.

6. The method according to claim 1, characterized in that, The step of determining the optimized pose of the image based on the point cloud features and the image features through multi-source feature matching and joint optimization includes: Based on the optimized pose of the lidar and the calibration relationship between sensors, the initial pose of the image is calculated; based on the initial pose, spatial neighborhood search and feature descriptor matching are performed in the existing image set to obtain corresponding points between images; Based on the corresponding points, construct collinearity constraint equations and jointly solve for the coordinates of the three-dimensional spatial points corresponding to the corresponding points. Based on semantic labels, points with the same name are divided into planar and linear categories, and distance residual constraints are constructed with the surface features and line features in the point cloud map, respectively. By combining all planar residuals, linear residuals, and collinear constraints, the image pose is nonlinearly optimized to obtain the optimized image pose.

7. The method according to claim 6, characterized in that, The step of classifying points of the same name into planar and linear categories based on semantic labels, and constructing distance residual constraints with the planar and linear features in the point cloud map respectively, includes: For points of the same name that are classified as planes, the distance residual from the 3D point to the plane is constructed by fitting a local plane in the corresponding neighborhood of the point cloud map through principal component analysis and random sampling consistency. For points of the same name that are classified as straight lines, a spatial straight line is fitted in the corresponding neighborhood of the point cloud map using principal component analysis, and the angle or distance residual from the 3D point to the straight line is constructed.

8. The method according to claim 1, characterized in that, The step of appending color to the point cloud data based on the optimized pose, the color information of the image, and the spatial structure information of the point cloud includes: For each spatial point in the point cloud, based on its position and normal vector, images with an angle less than a preset threshold with respect to the normal vector of that point are selected from all candidate images. The spatial points are projected onto the selected candidate images to obtain the corresponding pixel positions; Determine whether the pixel location belongs to a dynamic target area. If it does, exclude the image and re-filter. When a suitable non-dynamic region image is found, the color value of the pixel is assigned to the corresponding spatial point to complete the point cloud coloring.

9. The method according to claim 8, characterized in that, After filtering out images whose angle with the normal vector of the point is less than a preset threshold, the method further includes: sorting the candidate images according to the distance between the spatial point and the optical center of the image, and prioritizing the selection of images that are closer to the point for projection and color assignment.

Images