A method, system, device and medium for indoor three-dimensional reconstruction based on multi-sensor fusion

Abstract

The invention discloses a method, system, device and medium for indoor three-dimensional reconstruction of multi-sensor fusion, which belongs to the technical field of three-dimensional reconstruction, collects laser radar point cloud data, odometer data and RGB-D data of each frame when the robot moves; obtains the current frame posture of the robot; judges whether the current frame is a key frame according to the change of the current frame posture, and if so, stores the current frame in the key frame set; if not, judges whether the next frame is a key frame; obtains RGB-D data that matches the current key frame time, and generates a color point cloud according to the matching RGB-D data and the current frame posture; performs loop detection on the current key frame, and optimizes the posture of all key frames by graph optimization; uses the posture of all optimized key frames to correct and fuse the spatial position of the generated color point cloud to generate the final color point cloud for indoor three-dimensional reconstruction. The posture calculation of the invention is based on single-line laser data, with small calculation amount and high precision.

Description

Technical Field

[0001] The present invention belongs to the technical field of three-dimensional reconstruction, and in particular relates to a multi-sensor fusion indoor three-dimensional reconstruction method, system, equipment and medium. Background Art

[0002] 3D reconstruction is a technology that uses various sensors and algorithms to measure the geometric information of objects from images, videos, point clouds, depth maps and other data, and restore the 3D model. Currently, the commonly used 3D reconstruction methods are:

[0003] 1. Reconstructing point cloud maps by matching multi-line laser radar point cloud data. This method uses multi-line laser radars to match frames to calculate relative poses and build maps at the same time. Multi-line laser radars are relatively expensive, and the generated point cloud maps do not have color and semantic information.

[0004] 2. Use the gimbal or environment tags to calculate the depth camera external parameters, and stitch multiple frames of data into a three-dimensional point cloud map. This method of calculating external parameters through the gimbal or environment tags is suitable for the reconstruction of a single object. When the indoor room environment is large, multiple tags need to be deployed, which is not very practical.

[0005] 3. Use visual SLAM to calculate the pose through optical flow method or feature point method matching and reconstruct the map; this type of visual SLAM involves image processing algorithms (such as feature extraction, feature matching, and pose estimation), which has high computational complexity and poor accuracy during real-time processing.

[0006] Therefore, there is an urgent need for a three-dimensional reconstruction method based on single-line laser radar with low computational complexity and high accuracy. Summary of the invention

[0007] In view of the deficiencies in the prior art, the present invention provides a multi-sensor fusion indoor three-dimensional reconstruction method, system, device and medium, which are based on single-line laser data, have small calculation amount and high precision.

[0008] The present invention provides the following technical solutions:

[0009] In a first aspect, a multi-sensor fusion indoor 3D reconstruction method is provided, comprising:

[0010] Collect lidar point cloud data, odometer data, and RGB-D data for each frame when the robot moves;

[0011] Based on the lidar point cloud data and odometer data of the current frame and the previous frame, obtain the current frame pose of the robot;

[0012] Determine whether the current frame is a key frame according to the change of the current frame posture. If so, store the current frame in the key frame set; if not, determine whether the next frame is a key frame according to the change of the next frame posture.

[0013] Get the RGB-D data that matches the current key frame time, and generate a color point cloud based on the matching RGB-D data and the current frame pose;

[0014] Perform loop closure detection on the current keyframe, and optimize the poses of all keyframes in the keyframe set through graph optimization;

[0015] The generated color point cloud is corrected and fused in spatial position using the optimized poses of all key frames to generate the final color point cloud for indoor 3D reconstruction.

[0016] Optionally, the obtaining of the current frame pose of the robot based on the laser radar point cloud data and odometer data of the current frame and the previous frame includes:

[0017] The ICP algorithm is used to align the laser radar point cloud data of the current frame and the previous frame to obtain the rotation matrix with the minimum alignment error. and translation matrix ;

[0018] Obtain the rotation matrix based on the mileage increment through the mileage increment data of the current frame and the previous frame and translation matrix ;

[0019] The rotation matrix , translation matrix , rotation matrix and translation matrix Perform fusion to obtain the current frame pose of the robot , specifically:

[0020]

[0021]

[0022]

[0023] in, is the rotation matrix The weight of is the translation matrix The weight of and is the optimal rotation matrix and displacement matrix after fusion, is the identity matrix, is the pose of the previous frame.

[0024] Optionally, judging whether the current frame is a key frame according to the change in the current frame posture is specifically: when the displacement and angle of the current frame and the previous key frame of the robot are both greater than a set threshold, the current frame is a key frame.

[0025] Optionally, the process of acquiring RGB-D data matching the current key frame time and generating a color point cloud according to the matching RGB-D data is as follows:

[0026] According to the timestamp of the current key frame, find the RGB-D data closest to its time; the RGB-D data includes a depth image and a color image; the RGB-D data is collected by an RGB-D camera; the RGB-D camera includes a depth camera and an RGB camera;

[0027] According to the known intrinsic parameter matrix of the depth camera and the depth value of each pixel in the depth image, each pixel is converted into a three-dimensional point in the depth camera coordinate system. ;

[0028]

[0029]

[0030] in, , and are the horizontal coordinate, vertical coordinate and depth value of the pixel in the depth image respectively; , , , and They are the five internal parameters of the depth camera;

[0031] According to the intrinsic parameter matrix of the depth camera, each three-dimensional point in the depth camera coordinate system is Map to color image and get each 3D point The pixel coordinates of the image are obtained by obtaining the corresponding color value in the color image according to the pixel coordinates. ;

[0032] Each 3D point The three-dimensional coordinates and the corresponding color values Merge to generate a colored point cloud.

[0033] Optionally, the method further includes filtering the generated color point cloud by bounding box filtering.

[0034] Optionally, loop closure detection is performed on the current key frame, and posture optimization is performed on all key frames in the key frame set by means of graph optimization. The specific process is as follows:

[0035] Based on the pose of the current key frame and the pose of the historical key frames, select the candidate frames to be detected that meet the following conditions;

[0036]

[0037] in, The ID of the current key frame; It is the ID of the historical keyframe; and They are the x and y axis coordinates of the current keyframe respectively; and They are the x and y axis coordinates of the historical keyframes respectively;

[0038] The current key frame is matched with each candidate frame to be detected by the ICP algorithm. If the match is successful, the current key frame and the candidate frame to be detected form a loop constraint.

[0039] Insert all loop constraints of the current keyframe into the pose graph to obtain the loop constraints of all keyframes in the keyframe set;

[0040] Construct a cost function that aggregates all loop constraints , and solve the optimal pose of all key frames that minimize the cost function to achieve pose optimization for all key frames;

[0041]

[0042]

[0043] in, Keyframe and keyframes The error term of is the information matrix representing the weight of the error term; Keyframe and keyframes The calculated relative pose of is the key frame obtained by loop closure detection and keyframes The relative position of and They are the key frames obtained based on the lidar point cloud data and the odometer data respectively. and keyframes 's posture.

[0044] Optionally, the optimized postures of all key frames are used to correct and fuse the generated color point cloud in terms of spatial position, so as to generate a final color point cloud for indoor 3D reconstruction. The specific process is as follows:

[0045] Convert the color point cloud from the depth camera coordinate system to the robot coordinate system;

[0046]

[0047] in, is the point cloud of the current key frame in the robot coordinate system, is the color point cloud coordinate of the current key frame in the depth camera coordinate system, is the external parameter matrix from the depth camera coordinates to the robot coordinate system;

[0048] Using the updated poses of all key frames, transform the point cloud of the current key frame in the robot coordinate system to the world coordinate system;

[0049]

[0050] in, is the point cloud in the world coordinate system of the current keyframe, is the pose of the current key frame solved by graph optimization;

[0051] Superimpose the point clouds of all key frames in the world coordinate system to generate the final color point cloud map.

[0052] In a second aspect, a multi-sensor fusion indoor 3D reconstruction system is provided, comprising:

[0053] The acquisition module is used to collect the lidar point cloud data, odometer data and RGB-D data of each frame when the robot moves;

[0054] A posture acquisition module is used to obtain the current frame posture of the robot based on the lidar point cloud data and odometer data of the current frame and the previous frame;

[0055] The key frame acquisition module is used to determine whether the current frame is a key frame according to the change of the current frame posture. If so, the current frame is stored in the key frame set; if not, the next frame is determined according to the change of the next frame posture.

[0056] The color point cloud generation module is used to obtain the RGB-D data matching the current key frame time, and generate a color point cloud based on the matching RGB-D data and the current frame pose;

[0057] The pose optimization module is used to perform loop detection on the current keyframe and optimize the poses of all keyframes in the keyframe set by graph optimization.

[0058] The color point cloud update module is used to use the optimized poses of all key frames to correct and fuse the generated color point cloud in space to generate the final color point cloud for indoor 3D reconstruction.

[0059] According to a third aspect, a computer device is provided, comprising a processor and a memory; wherein, when the processor executes a computer program stored in the memory, the steps of the indoor three-dimensional reconstruction method based on multi-sensor fusion as described in any one of the first aspects are implemented.

[0060] In a fourth aspect, a computer-readable storage medium is provided for storing a computer program; when the computer program is executed by a processor, the steps of the indoor three-dimensional reconstruction method based on multi-sensor fusion as described in any one of the first aspects are implemented.

[0061] Compared with the prior art, the present invention has the following beneficial effects:

[0062] The present invention calculates the real-time posture through a single-line laser radar slam algorithm with a loop, stores the key frame posture through motion filtering, synchronously obtains the depth data and RGB data in the most recent time, and generates a point cloud map through the real-time posture and the external parameters from the depth camera coordinate system to the robot coordinate system; when calculating the real-time posture, only the single-line laser data needs to be processed, and the calculation amount is small. With loop detection, the accumulated error can be eliminated and the overall point cloud accuracy is improved; the present invention calculates the single-line laser radar data, and the calculation complexity is low, the accuracy is high and the cost is low compared to the visual feature matching; in addition, the present invention does not need to use a special gimbal or add labels to the environment to calculate the external parameters, and is easy to deploy. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] Figure 1 It is a schematic flow chart of the indoor three-dimensional reconstruction method of multi-sensor fusion of the present invention.

[0064] Figure 2 It is a raster map after the first frame of laser point cloud data is inserted in the example given in Embodiment 2 of the present invention.

[0065] Figure 3 This is the grid map after 132 key frames are inserted into the example given in Embodiment 2 of the present invention.

[0066] Figure 4 It is a grid map after graph optimization and updating of the example given in Embodiment 2 of the present invention.

[0067] Figure 5 This is the final color point cloud map generated by the example given in Example 2 of the present invention. DETAILED DESCRIPTION

[0068] The present invention is further described below in conjunction with the accompanying drawings. The following examples are only used to more clearly illustrate the technical solution of the present invention, and cannot be used to limit the scope of protection of the present invention. It should be noted that the term "comprising" and any variation thereof in the specification and claims of the present invention and the above-mentioned drawings are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device comprising a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0069] Example 1

[0070] like Figure 1 As shown, a multi-sensor fusion indoor 3D reconstruction method is provided, comprising the following steps:

[0071] Step S1: Collect lidar point cloud data, odometer data, and RGB-D data for each frame when the robot moves.

[0072] The lidar point cloud data is obtained through a single-line lidar, the odometer data is obtained through the odometer, and the RGB-D data is obtained through the RGB-D camera. The lidar, odometer and RGB-D camera are all installed on the robot; the RGB-D data includes depth images and color images; the RGB-D camera includes a depth camera and an RGB camera; it is necessary to calibrate in advance or obtain the external parameters of the depth camera and RGB camera based on the structural parameters, the internal parameters of the depth camera, and the external parameters of the RGB-D camera relative to the lidar.

[0073] Step S2: Based on the lidar point cloud data and odometer data of the current frame and the previous frame, the current frame pose of the robot is obtained.

[0074] Step S2 specifically includes the following sub-steps:

[0075] Step S21: Use the ICP algorithm to perform point cloud registration on the lidar point cloud data of the current frame and the previous frame to obtain the rotation matrix with the minimum registration error. and translation matrix .

[0076] Specifically, the laser data is grid filtered. If it is the first frame point cloud, the current machine is set to the initial position. ; If it is not the first frame point cloud, inter-frame matching is performed and the residual is expressed by the following formula :

[0077]

[0078] in, is the sequential ID of the point cloud, is the number of point clouds, is the rotation matrix, is the translation matrix, is the point cloud of the previous frame, Current frame point cloud.

[0079] Theoretically, the error term When it is closest to 0, and is the optimal solution, so we construct a least squares problem and find the solution that minimizes the sum of squared errors. and .

[0080]

[0081] Step S22: Obtain the rotation matrix based on the mileage increment through the mileage increment data of the current frame and the previous frame and translation matrix .

[0082] Get the rotation matrix through the odometer increment data and translation matrix The method can refer to the prior art.

[0083] Step S23: Rotate the matrix , translation matrix , rotation matrix and translation matrix Perform fusion to obtain the current frame pose of the robot , specifically:

[0084]

[0085]

[0086]

[0087] in, is the rotation matrix The weight of is the translation matrix The weight of and is the optimal rotation matrix and displacement matrix after fusion, is the identity matrix, is the pose of the previous frame. and are both greater than or equal to 0 and less than or equal to 1, and Determined based on actual laser matching and odometer accuracy.

[0088] Step S3: Determine whether the current frame is a key frame based on the change in the current frame posture. If so, store the current frame in the key frame set; if not, determine whether the next frame is a key frame based on the change in the next frame posture.

[0089] When the displacement and angle of the robot's current frame and the previous key frame are both greater than the set threshold, the current frame is the key frame.

[0090] The calculated pose enters the motion filter to determine whether to save it as a key frame. When the displacement and angle of the latest laser frame matching pose and the previous key frame are greater than the set threshold (the threshold can be adjusted according to the environment size and accuracy requirements), the frame data is stored as the latest key frame.

[0091] The calculation formula of displacement is:

[0092]

[0093] The angle is calculated as:

[0094]

[0095] in, , and The x, y and angle of the pose calculated in step S2 respectively; , , They are respectively the x, y and angle of the last saved keyframe pose.

[0096] When the displacement or angle between the current frame and the previous key frame of the robot is less than the set threshold, the current frame is not a key frame, and the process returns to step S2 to obtain the posture of the next frame.

[0097] Step S4: Obtain RGB-D data matching the current key frame time, and generate a color point cloud based on the matching RGB-D data and the current frame pose.

[0098] Step S4 specifically includes the following sub-steps:

[0099] Step S41: According to the timestamp of the current key frame, find the RGB-D data closest to its time.

[0100] Specifically, calculate the absolute value of the difference between the timestamps of the last 30 frames and the timestamp of the latest key frame , select the frame of RGB data and Depth data with the smallest difference.

[0101]

[0102] in, Indicates the timestamp difference, Represents the key frame timestamp, Indicates the current image timestamp.

[0103] Step S42: According to the known intrinsic parameter matrix of the depth camera and the depth value of each pixel in the depth image, each pixel is converted into a three-dimensional point in the depth camera coordinate system. .

[0104] The intrinsic parameter matrix of the depth camera is ;

[0105] 3D point in the depth camera coordinate system for:

[0106]

[0107]

[0108] in, , and are the horizontal coordinate, vertical coordinate and depth value of the pixel in the depth image respectively; , , , and They are the five internal parameters of the depth camera; is the coordinate of the principal point of the depth image, is the focal length of the depth camera, and are the physical sizes of pixels in the x-axis and y-axis directions of the depth image, respectively.

[0109] Step S43: According to the intrinsic parameter matrix of the depth camera, each three-dimensional point in the depth camera coordinate system is Map to color image and get each 3D point The pixel coordinates of the image are obtained by obtaining the corresponding color value in the color image according to the pixel coordinates. .

[0110] According to the external parameter matrix from the color camera to the depth camera, each three-dimensional point in the depth camera coordinate system is Map to color image and get each 3D point The pixel coordinates of .

[0111]

[0112] in, It is the external parameter matrix from the color camera to the depth camera; The coordinates are 3D points of for Convert 3D points to color images.

[0113] Extract the RGB color information of the corresponding u and v pixels from the RGB image and add it to the generated point cloud, so that the point cloud becomes data with (X, Y, Z, R, G, B) information.

[0114] Step S44: Each three-dimensional point The three-dimensional coordinates and the corresponding color values Merge to generate a colored point cloud.

[0115] In some other embodiments, step S4 further includes a sub-step of step S45; step S45: filtering the generated color point cloud by bounding box filtering.

[0116] Specifically, since the distance measurement accuracy of the depth camera for long-distance points decreases, the bounding box filtering method is used to remove the points that are too far and too close to the color point cloud generated in step S44, and the minimum point of the bounding box in the depth camera coordinate system is set based on the edge point of the image. The maximum point is Filter all points in the colored point cloud by the following conditions.

[0117]

[0118] If the point meets all the conditions, it is considered to be inside the bounding box and the point is retained; otherwise, the point is removed.

[0119] Step S5: Perform loop detection on the current key frame, and optimize the poses of all key frames in the key frame set by means of graph optimization.

[0120] Only laser matching in step S2 will produce cumulative errors after the robot moves for a long time, so it is necessary to eliminate them through loop detection and graph optimization.

[0121] Step S5 specifically includes the following sub-steps:

[0122] Step S51: based on the pose of the current key frame and the pose of the historical key frames, select candidate frames to be detected that meet the following conditions;

[0123]

[0124] in, The ID of the current key frame; It is the ID of the historical keyframe; and They are the x and y axis coordinates of the current keyframe respectively; and They are the x and y axis coordinates of the historical keyframes respectively.

[0125] Step S52: performing point cloud matching between the current key frame and each candidate frame to be detected by using the ICP algorithm. If the match is successful, the current key frame and the candidate frame to be detected form a loop constraint.

[0126] If the point cloud matching is unsuccessful, the current key frame and the candidate frame to be detected do not constitute a loop constraint.

[0127] Step S53: insert all loop constraints of the current key frame into the pose graph to obtain loop constraints of all key frames in the key frame set.

[0128] Step S54: Construct a cost function that aggregates all loop constraints , and solve the optimal pose of all key frames that minimize the cost function to achieve pose optimization for all key frames;

[0129]

[0130]

[0131] in, Keyframe and keyframes The error term of is the information matrix representing the weight of the error term; Keyframe and keyframes The calculated relative pose of is the key frame obtained by loop closure detection and keyframes The relative position of and They are the key frames obtained based on the lidar point cloud data and the odometer data respectively. and keyframes 's posture.

[0132] The method for solving the above cost function is to linearize the above cost function and then use Gauss-Newton to solve the optimal solution.

[0133] For a specific error term The linearization process is carried out by adjusting the node pose and Calculating the Jacobian matrix gives:

[0134]

[0135] in, and is the increment to be solved, and is the error function for and The Jacobian matrix of .

[0136] Construct the increment equation in the form Ax = b.

[0137]

[0138] in, is the information matrix, is the error term;

[0139] Update the pose as follows:

[0140]

[0141] Update the solved increment to the node pose , at this time, all key frame poses are considered to be the optimal solution.

[0142] Step S6: Using the optimized poses of all key frames, the generated color point cloud is corrected and fused in terms of spatial position to generate the final color point cloud for indoor 3D reconstruction.

[0143] Step S6 specifically includes the following sub-steps:

[0144] Step S61: converting the color point cloud from the depth camera coordinate system to the robot coordinate system;

[0145]

[0146] in, is the point cloud of the current key frame in the robot coordinate system, is the color point cloud coordinate of the current key frame in the depth camera coordinate system, is the external parameter matrix from the depth camera coordinates to the robot coordinate system;

[0147] Step S62: using the updated positions and poses of all key frames, transforming the point cloud of the current key frame in the robot coordinate system into the world coordinate system;

[0148]

[0149] in, is the point cloud in the world coordinate system of the current keyframe, is the pose of the current key frame solved by graph optimization.

[0150] Step S63: superimpose the point clouds of all key frames in the world coordinate system to generate a final color point cloud map.

[0151] The final color point cloud map can be saved in the required format, such as pcd or ply format, for subsequent loading and viewing.

[0152] Example 2

[0153] An example of indoor 3D reconstruction based on multi-sensor fusion is given.

[0154] In the actual test, turtlrbot chassis, Wanji 716 radar, rk3399 development board, ubuntu22.04, and ros2 humble version environment were used to build a three-dimensional point cloud in the test room.

[0155] 1. Sensor Data Preparation

[0156] According to the structure of the robot, the external parameter rotation matrix of the robot's depth camera coordinate system (camera) relative to the chassis center coordinate system (base_link) is obtained as , the translation matrix is Running the chassis-related ros driver can read the lidar, odometer, and depth camera data normally.

[0157] 2. Laser matching

[0158] Control the robot movement, start building the map and save the keyframes, use the previous frame pose as the starting point, calculate R and t through the inter-frame matching ICP algorithm, and synchronously obtain the odometer increment information and ,Since the odometer rotation error on the actual chassis is large and the translation error is small, we set , .

[0159] Calculate the optimal and , further calculate the current pose , and so on. The raster map after the first frame of laser point cloud data is inserted is as follows Figure 2 shown.

[0160] 3. Keyframe Saving

[0161] Calculate the pose difference of the previous key frame of the current frame, and use the motion filter to determine whether to insert it as a key frame. The displacement threshold used by the motion filter in this example is 0.2m, and the rotation angle threshold is 3°. If the conditions are met, insert a new key frame Id=1, 2, 3..., if not, return to the step to continue matching the next frame.

[0162] The chassis moves to the rotation matrix: Translation Matrix , 132 key frames have been inserted. After inserting 132 key frames, the grid map is as follows Figure 3 shown.

[0163] 4. Generate point cloud from nearest neighbor RGB-D data

[0164] Find the most recent RGB data and depth number corresponding to the 132nd key frame timestamp; the corresponding color point cloud can be generated according to the algorithm in step S4.

[0165] 5. Loop Detection and Graph Optimization

[0166] The latest keyframe is matched with the keyframes in history. The keyframe with ID = 43 matches the original map and the error term is constructed. , forming a pose graph optimization, solving the problem of minimizing the error term Id 0-43 key frame pose as the optimal solution. After graph optimization, the complete map is as follows Figure 4 shown.

[0167] 6. Generate point cloud map

[0168] According to the formula in step S6, the point cloud is converted from the camera coordinate system to the map coordinate system to generate a colored three-dimensional point cloud map.

[0169] 7. Save the map

[0170] Save the corresponding raster map and point cloud map into corresponding files. The 3D reconstruction results are as follows: Figure 5 shown.

[0171] Example 3

[0172] A multi-sensor fusion indoor 3D reconstruction system, comprising:

[0173] The acquisition module is used to collect the lidar point cloud data, odometer data and RGB-D data of each frame when the robot moves;

[0174] A posture acquisition module is used to obtain the current frame posture of the robot based on the lidar point cloud data and odometer data of the current frame and the previous frame;

[0175] The key frame acquisition module is used to determine whether the current frame is a key frame according to the change of the current frame posture. If so, the current frame is stored in the key frame set; if not, the next frame is determined according to the change of the next frame posture.

[0176] The color point cloud generation module is used to obtain the RGB-D data matching the current key frame time, and generate a color point cloud based on the matching RGB-D data and the current frame pose;

[0177] The pose optimization module is used to perform loop detection on the current keyframe and optimize the poses of all keyframes in the keyframe set by graph optimization.

[0178] The color point cloud update module is used to use the optimized poses of all key frames to correct and fuse the generated color point cloud in space to generate the final color point cloud for indoor 3D reconstruction.

[0179] For more specific processes of the above method, please refer to the corresponding contents disclosed in the aforementioned embodiments, which will not be repeated here.

[0180] Example 4

[0181] A computer device is provided, comprising a processor and a memory; wherein the processor implements the steps of the above-mentioned multi-sensor fusion indoor three-dimensional reconstruction method when executing a computer program stored in the memory.

[0182] For more specific processes of the above method, please refer to the corresponding contents disclosed in the aforementioned embodiments, which will not be repeated here.

[0183] Example 5

[0184] A computer-readable storage medium is provided for storing a computer program; when the computer program is executed by a processor, the steps of the above-mentioned multi-sensor fusion indoor three-dimensional reconstruction method are implemented.

[0185] For more specific processes of the above method, please refer to the corresponding contents disclosed in the aforementioned embodiments, which will not be repeated here.

[0186] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the system, device, and storage medium disclosed in the embodiment, since they correspond to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part.

[0187] Those skilled in the art can clearly understand that the technology in the embodiments of the present invention can be implemented by means of software plus a necessary general hardware platform. Based on this understanding, the technical solution in the embodiments of the present invention can be essentially or partly contributed to the prior art in the form of a software product, which can be stored in a storage medium such as ROM / RAM, a magnetic disk, an optical disk, etc., and includes several instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in the various embodiments of the present invention or certain parts of the embodiments.

[0188] The above are only preferred embodiments of the present invention, and the protection scope of the present invention is not limited to the above embodiments. All technical solutions under the concept of the present invention belong to the protection scope of the present invention. It should be pointed out that for ordinary technicians in this technical field, some improvements and modifications without departing from the principle of the present invention should be regarded as the protection scope of the present invention.

Claims

1. A multi-sensor fusion indoor 3D reconstruction method, characterized in that: include: Collect lidar point cloud data, odometer data, and RGB-D data for each frame when the robot moves; Based on the lidar point cloud data and odometer data of the current frame and the previous frame, obtain the current frame pose of the robot; Determine whether the current frame is a key frame according to the change of the current frame posture. If so, store the current frame in the key frame set; if not, determine whether the next frame is a key frame according to the change of the next frame posture. Get the RGB-D data that matches the current key frame time, and generate a color point cloud based on the matching RGB-D data and the current frame pose; Perform loop closure detection on the current keyframe, and optimize the poses of all keyframes in the keyframe set through graph optimization; Using the optimized poses of all key frames, the generated color point cloud is corrected and fused in spatial position to generate the final color point cloud for indoor 3D reconstruction. The loop detection is performed on the current key frame, and the posture optimization is performed on all key frames in the key frame set by means of graph optimization. The specific process is as follows: Based on the pose of the current key frame and the pose of the historical key frames, select the candidate frames to be detected that meet the following conditions; ； in, is the ID of the current keyframe; The ID of the historical keyframe; and They are the x and y axis coordinates of the current keyframe respectively; and They are the x and y axis coordinates of the historical keyframes respectively; The current key frame is matched with each candidate frame to be detected by the ICP algorithm. If the match is successful, the current key frame and the candidate frame to be detected form a loop constraint. Insert all loop constraints of the current keyframe into the pose graph to obtain the loop constraints of all keyframes in the keyframe set; Construct a cost function that aggregates all loop constraints , and solve the optimal pose of all key frames that minimize the cost function to achieve pose optimization for all key frames; ； ； in, Keyframe and keyframes The error term of is the information matrix representing the weight of the error term; Keyframe and keyframes The calculated relative pose of is the key frame obtained by loop closure detection and keyframes The relative position of and They are the key frames obtained based on the lidar point cloud data and the odometer data respectively. and keyframes 's posture; The optimized poses of all key frames are used to correct and fuse the generated color point cloud in spatial position to generate the final color point cloud for indoor 3D reconstruction. The specific process is as follows: Convert the color point cloud from the depth camera coordinate system to the robot coordinate system; ； in, is the point cloud of the current key frame in the robot coordinate system, is the color point cloud coordinate of the current key frame in the depth camera coordinate system, is the external parameter matrix from the depth camera coordinates to the robot coordinate system; Using the updated poses of all key frames, transform the point cloud of the current key frame in the robot coordinate system to the world coordinate system; ； in, is the point cloud in the world coordinate system of the current keyframe, is the pose of the current key frame solved by graph optimization; Superimpose the point clouds of all key frames in the world coordinate system to generate the final color point cloud map.

2. The indoor 3D reconstruction method based on multi-sensor fusion according to claim 1, characterized in that: The current frame pose of the robot is obtained based on the laser radar point cloud data and odometer data of the current frame and the previous frame. The specific process is as follows: The ICP algorithm is used to align the laser radar point cloud data of the current frame and the previous frame to obtain the rotation matrix with the minimum alignment error. and translation matrix ; Obtain the rotation matrix based on the mileage increment through the mileage increment data of the current frame and the previous frame and translation matrix ; The rotation matrix , translation matrix , rotation matrix and translation matrix Perform fusion to obtain the current frame pose of the robot , specifically: ； ； ； in, is the rotation matrix The weight of is the translation matrix The weight of and is the optimal rotation matrix and displacement matrix after fusion, is the identity matrix, is the pose of the previous frame.

3. The indoor 3D reconstruction method based on multi-sensor fusion according to claim 1, characterized in that: The determining whether the current frame is a key frame based on the change in the current frame posture is as follows: when the displacement and angle of the current frame and the previous key frame of the robot are both greater than a set threshold, the current frame is a key frame.

4. The method for indoor 3D reconstruction based on multi-sensor fusion according to claim 1, characterized in that: The specific process of obtaining RGB-D data matching the current key frame time and generating a color point cloud according to the matching RGB-D data is as follows: According to the timestamp of the current key frame, find the RGB-D data closest to its time; the RGB-D data includes a depth image and a color image; the RGB-D data is collected by an RGB-D camera; the RGB-D camera includes a depth camera and an RGB camera; According to the known intrinsic parameter matrix of the depth camera and the depth value of each pixel in the depth image, each pixel is converted into a three-dimensional point in the depth camera coordinate system. ; ； ； in, , and are the horizontal coordinate, vertical coordinate and depth value of the pixel in the depth image respectively; , , , and They are the five internal parameters of the depth camera; According to the intrinsic parameter matrix of the depth camera, each three-dimensional point in the depth camera coordinate system is Map to color image and get each 3D point The pixel coordinates of the image are obtained by obtaining the corresponding color value in the color image according to the pixel coordinates. ; Each 3D point The three-dimensional coordinates and the corresponding color values Merge to generate a colored point cloud.

5. The method for indoor 3D reconstruction based on multi-sensor fusion according to claim 4, characterized in that: It also includes filtering the generated color point cloud through bounding box filtering.

6. A multi-sensor fusion indoor 3D reconstruction system, characterized in that: include: The acquisition module is used to collect the lidar point cloud data, odometer data and RGB-D data of each frame when the robot moves; A posture acquisition module is used to obtain the current frame posture of the robot based on the lidar point cloud data and odometer data of the current frame and the previous frame; The key frame acquisition module is used to determine whether the current frame is a key frame according to the change of the current frame posture. If so, the current frame is stored in the key frame set; if not, the next frame is determined according to the change of the next frame posture. The color point cloud generation module is used to obtain the RGB-D data matching the current key frame time, and generate a color point cloud based on the matching RGB-D data and the current frame pose; The pose optimization module is used to perform loop detection on the current keyframe and optimize the poses of all keyframes in the keyframe set by graph optimization. The color point cloud update module is used to correct and fuse the generated color point cloud in terms of spatial position using the optimized poses of all key frames to generate the final color point cloud for indoor 3D reconstruction; The loop detection is performed on the current key frame, and the posture optimization is performed on all key frames in the key frame set by means of graph optimization. The specific process is as follows: Based on the pose of the current key frame and the pose of the historical key frames, select the candidate frames to be detected that meet the following conditions; ； in, is the ID of the current keyframe; The ID of the historical keyframe; and They are the x and y axis coordinates of the current keyframe respectively; and They are the x and y axis coordinates of the historical keyframes respectively; The current key frame is matched with each candidate frame to be detected by the ICP algorithm. If the match is successful, the current key frame and the candidate frame to be detected form a loop constraint. Insert all loop constraints of the current keyframe into the pose graph to obtain the loop constraints of all keyframes in the keyframe set; Construct a cost function that aggregates all loop constraints , and solve the optimal pose of all key frames that minimize the cost function to achieve pose optimization for all key frames; ； ； in, Keyframe and keyframes The error term of is the information matrix representing the weight of the error term; Keyframe and keyframes The calculated relative pose of is the key frame obtained by loop closure detection and keyframes The relative position of and They are the key frames obtained based on the lidar point cloud data and the odometer data respectively. and keyframes 's posture; The optimized poses of all key frames are used to correct and fuse the generated color point cloud in spatial position to generate the final color point cloud for indoor 3D reconstruction. The specific process is as follows: Convert the color point cloud from the depth camera coordinate system to the robot coordinate system; ； in, is the point cloud of the current key frame in the robot coordinate system, is the color point cloud coordinate of the current key frame in the depth camera coordinate system, is the external parameter matrix from the depth camera coordinates to the robot coordinate system; Using the updated poses of all key frames, transform the point cloud of the current key frame in the robot coordinate system to the world coordinate system; ； in, is the point cloud in the world coordinate system of the current keyframe, is the pose of the current key frame solved by graph optimization; Superimpose the point clouds of all key frames in the world coordinate system to generate the final color point cloud map.

7. A computer device, characterized in that: It comprises a processor and a memory; wherein, when the processor executes the computer program stored in the memory, it implements the steps of the indoor three-dimensional reconstruction method of multi-sensor fusion as described in any one of claims 1-5.

8. A computer-readable storage medium, characterized in that: Used to store computer programs; when the computer programs are executed by a processor, the steps of the indoor three-dimensional reconstruction method based on multi-sensor fusion described in any one of claims 1 to 5 are implemented.

Images