Scanning mapping method and device

Abstract

The invention relates to a scanning mapping method and device, and belongs to the technical field of mobile mapping and three-dimensional reconstruction, the method is applied to a scanning mapping device, the device comprises a laser radar, an IMU and a fisheye camera, and the method comprises the following steps: obtaining point cloud data obtained by scanning of the laser radar at each moment, measurement data of the IMU and image data shot by the fisheye camera; determining pose data of the scanning and mapping device at each moment according to the point cloud data, the measurement data and the image data at each moment; performing coordinate conversion processing on the point cloud data at each moment according to the pose data at each moment to obtain coordinates of the point cloud data in a global coordinate system; and constructing a target map according to the coordinates of the point cloud data in the global coordinate system. The pose of the scanning mapping device is determined according to the observation data of the laser radar, the IMU and the fisheye camera, and the pose robustness can be improved. And mapping is carried out according to the pose, so that the mapping accuracy can be improved.

Description

Technical Field

[0001] The present invention relates to the technical field of mobile mapping and three-dimensional reconstruction, and in particular to a scanning mapping method and device. Background Art

[0002] Simultaneous Localization and Mapping (SLAM) is a technology that uses information obtained by sensors to build an environmental map during movement and estimate its own position without prior environmental information. This technology is currently widely used in unmanned vehicles, drones, robots and other fields.

[0003] Currently, the two most mainstream SLAM technology solutions are lidar SLAM and visual SLAM. However, with the increasing complexity of SLAM devices performing tasks in unknown environments, existing methods based on a single sensor (lidar or camera) cannot achieve accurate and robust SLAM device pose estimation in a large range of complex environments, and thus cannot meet the perception and positioning requirements of SLAM devices. Summary of the invention

[0004] In view of this, it is necessary to provide a scanning mapping method and device to solve the problem that the existing technology cannot meet the perception and positioning requirements of SLAM devices.

[0005] In order to solve the above problems, in a first aspect, the present invention provides a scanning mapping method, which is applied to a scanning mapping device, wherein the scanning mapping device includes a laser radar, an inertial measurement unit, and a fisheye camera, and the method includes: Acquire point cloud data scanned by the laser radar, measurement data of the inertial measurement unit, and image data taken by the fisheye camera at each moment; Determining the position and posture data of the scanning mapping device at each moment according to the point cloud data, the measurement data and the image data at each moment; Performing coordinate conversion processing on the point cloud data at each moment according to the posture data at each moment to obtain the coordinates of the point cloud data in the global coordinate system; A target map is constructed based on the coordinates of the point cloud data in the global coordinate system.

[0006] Optionally, the image data includes image feature points; and determining the position and posture data of the scanning mapping device at each moment according to the point cloud data, the measurement data and the image data at each moment includes: Performing pre-integration calculation on the measurement data of the inertial measurement unit at each moment to obtain the pre-integrated observation value at each moment; The pose data of the scanning mapping device is obtained by solving the following formula:

[0007]

[0008] in, is the pose data of the scanning mapping device; X is the pose data set to be determined; for A priori estimate of ; represents the subtraction operation defined on the three-dimensional rotation group; For measurement covariance matrix of reliability; is the covariance matrix that measures the reliability of point cloud data; is the covariance matrix that measures the reliability of the pre-integrated observations; is the covariance matrix that measures the reliability of image feature points; , , and for multiplying the operation results of the subtraction operation, the error equation of the laser radar observation, the error equation of the pre-integrated observation value of the inertial measurement unit, and the error equation of the fisheye camera observation respectively when solving the target equation; , , are respectively the number of points in the point cloud data, the number of pre-integrated observation data and the number of image feature points; is the coordinate of the jth point in the point cloud data in the inertial measurement unit coordinate system; For the pre-integrated observations; For the i The coordinates of the image feature points in the inertial measurement unit coordinate system; is the error equation of the laser radar observation; is the error equation of the pre-integrated observation value of the inertial measurement unit; is the error equation observed by the fisheye camera.

[0009] Optionally, the pre-integrated observation values ​​include: relative rotation change observation values, relative speed change observation values ​​and relative position change observation values, and the image data includes the image principal point; ,in, n is the normal vector of the surface element corresponding to the point, q are the coordinates of the center point of the surface element; ,in, , , They are the residual of the relative rotation change observation value from time i to time j, the residual of the relative speed change observation value, and the residual of the relative position change observation value; , where (X, Y, Z) is the coordinate of the i-th point in the global coordinate system obtained after the i-th point in the point cloud data is transformed according to the initial pose data; , is the focal length of the fisheye camera, , ( , ) is the coordinate of the feature point in the image coordinate system after dedistortion processing, ( , ) are the coordinates of the principal point in the image coordinate system after dedistortion.

[0010] Optionally, performing coordinate conversion processing on the point cloud data at each moment according to the posture data at each moment to obtain coordinates of the point cloud data in a global coordinate system includes: The point cloud data is subjected to coordinate transformation processing by the following formula:

[0011] in, is the coordinate of the point cloud data in the global coordinate system, is the posture data.

[0012] Optionally, performing coordinate conversion processing on the point cloud data at each moment according to the posture data at each moment to obtain coordinates of the point cloud data in a global coordinate system includes: Performing closed-loop identification on the point cloud data at each moment to obtain at least one closed-loop pair; generating posture correction data according to the closed-loop pair; Correcting the posture data at each moment according to the posture correction data; The point cloud data at each moment is subjected to coordinate transformation processing according to the corrected posture data to obtain the coordinates of the point cloud data in the global coordinate system.

[0013] Optionally, performing closed-loop identification on the point cloud data at each moment to obtain at least one closed-loop pair includes: Merge the temporally adjacent point cloud data and generate a submap based on the merged point cloud data; Closed-loop identification is performed on each of the sub-maps to obtain at least one closed-loop pair; each of the closed-loop pairs includes two target sub-maps that meet the closed-loop requirements.

[0014] Optionally, generating posture correction data according to the closed-loop pair includes: Determining the overlap rate of the target submaps in each closed-loop pair on the object space; The closed-loop pairs whose overlap rate is greater than a preset overlap rate threshold are taken as target closed-loop pairs; According to the target closed-loop pair, pose correction data is generated.

[0015] Optionally, the laser radar includes a first laser radar and a second laser radar, and the scanning areas of the first laser radar and the second laser radar overlap; the closed-loop recognition of the point cloud data at each moment to obtain at least one closed-loop pair includes: Determine redundant point cloud data in an overlapping area of ​​first point cloud data obtained by the first laser radar scanning and second point cloud data obtained by the second laser radar scanning at each moment; Removing the redundant point cloud data in the collection of the first point cloud data and the second point cloud data at each moment to obtain target point cloud data at each moment; Closed-loop identification is performed on the target point cloud data at each moment to obtain at least one closed-loop pair.

[0016] Optionally, the image data includes an image; and the method further includes: Projecting the points in the point cloud data respectively into preset images adjacent to each other in space to obtain preset image points corresponding to each point; Determine whether the image point exceeds the visual range of the image corresponding to the image point, and use the image point that does not exceed the visual range as the target image point; The target map is colored according to the target image points corresponding to each point.

[0017] In a second aspect, the present invention further provides a scanning mapping method device, comprising: a laser radar, an inertial measurement unit, a fisheye camera and a processor; the processor is used for the steps in any one of the above-mentioned scanning mapping methods.

[0018] The beneficial effects of the present invention are: After the scanning mapping device obtains the point cloud data obtained by the laser radar scanning at each moment, the measurement data of the IMU and the image data taken by the fisheye camera; the position and posture data of the scanning mapping device at each moment are determined according to the point cloud data of the laser radar at each moment, the measurement data of the IMU and the image data of the fisheye camera at each moment, which can improve the robustness of the position and posture of the scanning mapping device. Then, according to the position and posture data at each moment, the point cloud data at each moment is subjected to coordinate conversion processing to obtain the coordinates of the point cloud data in the global coordinate system; according to the coordinates of the point cloud data in the global coordinate system, the target map is constructed, which can improve the accuracy of the coordinates of the point cloud data, and thus improve the accuracy of the target map. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 A schematic diagram of the structure of a scanning mapping device provided by the present invention; Figure 2 A schematic diagram of a flow chart of an embodiment of the scanning mapping method provided by the present invention; Figure 3 A schematic diagram of a point-to-surface error measurement provided by the present invention; Figure 4 (a) is a schematic diagram of a single laser radar scanning field of view provided by the present invention; Figure 4 (b) is a schematic diagram of a dual laser radar scanning field of view provided by the present invention; Figure 5 A schematic diagram of a real-time data processing flow provided by the present invention; Figure 6 A schematic diagram of another data processing flow of a real-time terminal provided by the present invention; L-1 and L-2 are the left and right non-repetitive scanning laser radars respectively; C-1 is a fisheye camera; B-1 is a microprocessor; B-2 is a handle with a built-in lithium battery. DETAILED DESCRIPTION

[0020] The technical solutions in the embodiments of the present invention will be described clearly and completely below in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present invention.

[0021] In the description of the embodiments of the present invention, unless otherwise specified, "plurality" means two or more. "First", "second", etc. involved in the embodiments of the present invention are used to distinguish similar objects, and are not used to describe a specific order or sequence, nor are they used to indicate or imply their relative importance or implicitly indicate the number of technical features indicated. It should be understood that the data used in this way can be interchangeable where appropriate, so that the embodiments of the present application can be implemented in an order other than those illustrated or described here, and the objects distinguished by "first", "second", etc. are generally of one type, and the number of objects is not limited. For example, the first object can be one or more.

[0022] Reference to "embodiments" herein means that a particular feature, structure, or characteristic described in conjunction with the embodiments may be included in at least one embodiment of the present invention. The appearance of the phrase in various places in the specification does not necessarily refer to the same embodiment, nor is it an independent or alternative embodiment that is mutually exclusive with other embodiments. It is explicitly and implicitly understood by those skilled in the art that the embodiments described herein may be combined with other embodiments.

[0023] Reference Figure 1 , showing a schematic diagram of the structure of a scanning mapping device provided by the present invention. The scanning mapping device of this embodiment includes at least two non-repetitive scanning laser radars L-1 and L-2, a fisheye camera C-1, an inertial measurement unit (IMU), a microprocessor B-1, a handle B-2 with a built-in lithium battery, and a display screen.

[0024] The scanning mapping device of this embodiment can be handheld, easy to carry and use, and the built-in short-life battery can realize seamless replacement of the handle battery without pausing the task or shutting down. Compared with the fixed single laser radar (insufficient scanning field of view) and the rotating single laser radar (which expands the scanning field of view, but the algorithm is complex, and it needs to be installed on a rotating platform, and the device structure is also complex) used in the prior art, the two non-repetitive scanning laser radars L-1 and L-2 of this embodiment expand the scanning field of view and ensure a more complete scanning. At the same time, the data processing is lighter, which can effectively reduce the time cost of data processing. In addition, more sufficient constraint information is provided for scanning registration pose estimation, and more stable mapping can be achieved in various scenes such as streets, woodlands, and tunnels. The fisheye camera C-1 of this embodiment has a field of view angle greater than 180° and a pixel resolution higher than 12 million, which can be used to solve the problem that traditional laser scanners cannot output color point clouds and three-dimensional color models. The display screen of this embodiment can be set on the side facing the holder, which can display the working status of each sensor (laser radar, fisheye camera and IMU) and the results of the current scan.

[0025] The control method of the scanning mapping device may include connecting a mobile phone via a display screen or via wifi, projecting the control end to the mobile phone browser, and controlling the operation status of the scanning mapping device on the browser side. The transmission method of the scanning result data can support three transmission methods: downloading via wifi, transmitting using an SD card as a storage medium, and transmitting via a TypeC cable.

[0026] Reference Figure 2 , shows a schematic flow chart of an embodiment of the scanning mapping method provided by the present invention, the method is applied to a scanning mapping device, the scanning mapping device includes a laser radar, an IMU and a fisheye camera, and the method includes: S201, obtaining point cloud data obtained by laser radar scanning at each moment, measurement data of IMU, and image data taken by fisheye camera.

[0027] The laser radar may be a non-repetitive scanning laser radar, and the number of laser radars may be multiple. When the number of laser radars is multiple, the point cloud data scanned by each laser radar may be aligned, and then the coordinates of the aligned point cloud data may be converted to the coordinates in the IMU coordinate system before proceeding to the next step.

[0028] For example, the laser radar includes a right LiDAR and a left LiDAR, and the right LiDAR coordinate system can be aligned to the external parameters of the left LiDAR coordinate system. , align the point cloud data of the right LiDAR to the left LiDAR coordinate system. After alignment, the left LiDAR coordinate system can be aligned to the external parameters of the IMU coordinate system , convert the point cloud data of the left LiDAR into the IMU coordinate system. Specifically, coordinate alignment and coordinate conversion can be performed using the following formula:

[0029] in, is the coordinate of the right LiDAR point cloud data in the IMU coordinate system; is the coordinate of the point cloud data of the right LiDAR in the coordinate system of the right LiDAR, j = 1, 2, …, N (N is the number of laser points in the LiDAR frame).

[0030] The measurement data of the IMU may include the position data, speed data, and rotation data of the scanning mapping device at each moment.

[0031] The image data captured by the fisheye camera may include the image, the ORB feature points of the image, and the principal point of the image. After obtaining the image captured by the fisheye camera, the image can be first dedistorted, and then the ORB (Oriented FAST and Rotated BRIEF) feature points of the image can be extracted, and the coordinates of the ORB feature points in the dedistorted image coordinate system can be determined, as well as the external parameters aligned to the IMU coordinate system based on the image coordinate system. Convert the coordinates of the ORB feature points in the image coordinate system to the IMU coordinate system.

[0032] The principal point of an image refers to the intersection of the image plane and the optical axis during the camera imaging process. Ideally, the principal point is located at the center of the image, that is, the center point of the image. After obtaining the principal point of the image, the coordinates of the principal point in the dedistorted image coordinate system can be determined.

[0033] S202, determining the position and posture data of the scanning mapping device at each moment according to the point cloud data, measurement data and image data at each moment.

[0034] The observation error equations of LiDAR, IMU and fisheye camera can be used as constraints, and the Error State Iterated Kalman Filter Update (ESIKF) algorithm can be used to solve the pose data of the scanning mapping device at each moment in real time based on the point cloud data, measurement data and image data at each moment. The pose data can include: rotation , Pan , carrier speed , IMU's gyro and accelerometer bias , and the gravity direction vector .

[0035] S203, performing coordinate conversion processing on the point cloud data at each moment according to the posture data at each moment, to obtain the coordinates of the point cloud data in the global coordinate system.

[0036] According to the coordinates of the point cloud data in the IMU coordinate system at each time and the posture data solved at each time, the point cloud data is subjected to coordinate transformation processing to obtain the coordinates of the point cloud data in the global coordinate system. The global coordinate system can be the world coordinate system.

[0037] Specifically, the coordinate transformation of point cloud data can be performed using the following formula:

[0038] in, is the coordinate of the point cloud data in the global coordinate system, is the posture data.

[0039] S204: construct a target map according to the coordinates of the point cloud data in the global coordinate system.

[0040] The scanning mapping device of this embodiment obtains the point cloud data obtained by the laser radar scanning at each moment, the measurement data of the IMU and the image data taken by the fisheye camera; the position and posture data of the scanning mapping device at each moment are determined according to the point cloud data of the laser radar at each moment, the measurement data of the IMU and the image data of the fisheye camera, which can improve the robustness of the position and posture of the scanning mapping device. Then, according to the position and posture data at each moment, the point cloud data at each moment is subjected to coordinate conversion processing to obtain the coordinates of the point cloud data in the global coordinate system; according to the coordinates of the point cloud data in the global coordinate system, the target map is constructed, which can improve the accuracy of the coordinates of the point cloud data, and thus improve the accuracy of the target map.

[0041] In one embodiment, before performing S201, the timestamp alignment of each laser radar, IMU and fisheye camera can be achieved through the hard trigger mode of the pulse per second (PPS) signal. The basic principle of alignment is that the PPS signal sending chip sends an alignment signal to each sensor at an interval of 1 second. After receiving the signal, the sensor will reset its own timestamp once, and the second position time of the scanning mapping device will continue to use the global time recorded by the timing chip.

[0042] In one embodiment, before performing S201, the intrinsic parameters of the fisheye camera lens may be calibrated. Specifically, the Kalibr calibration tool may be used to process the calibration image collected in the indoor calibration field, and then output the intrinsic parameters of the fisheye camera. In this embodiment, the fisheye camera model adopts the KannalaBrandt model, so the expression of the intrinsic parameter matrix is:

[0043] in, are the radial and tangential focal lengths of the lens, respectively, ( ) are the coordinates of the principal point, is the lens distortion coefficient.

[0044] In one embodiment, the pre-integrated observation values ​​include: relative rotation change observation values, relative speed change observation values ​​and relative position change observation values; the image data includes image feature points and image principal points; then S202 may specifically include: First, the IMU measurement data at each moment is pre-integrated and calculated to obtain the pre-integrated observation value at each moment.

[0045] Pre-integration calculation is to integrate the discrete IMU measurement data within a specified time period (such as between camera frames or LiDAR frames) to calculate the changes in the relative position, relative speed and relative rotation of the mapping scanning device between two frames. The formula for pre-integration calculation is:

[0046] in, They are the relative rotation change observation value, relative speed change observation value and relative position change observation value of the mapping scanning device from time i to time j respectively; is the zero bias of the gyroscope and accelerometer in the IMU; is Gaussian noise.

[0047] Then, the pose data of the scanning mapping device is obtained by solving the following state optimization objective equation:

[0048]

[0049] in, is the pose data of the scanning mapping device; X is the pose data set to be determined; for The prior estimate of , that is, the pose and of the scanning mapping device at the current moment deduced by the IMU pre-integrated observation value combined with the historical IMU measurement data; For measurement covariance matrix of reliability; is the covariance matrix that measures the reliability of point cloud data; is the covariance matrix that measures the reliability of the pre-integrated observations; is the covariance matrix that measures the reliability of image feature points; , , They are the number of points in the point cloud data, the number of pre-integrated observation data, and the number of image feature points; is the coordinate of the jth point in the point cloud data in the IMU coordinate system; For the pre-integrated observations; For the i The coordinates of the image feature points in the IMU coordinate system; is the error equation of lidar observation; is the error equation of the pre-integrated observation value of IMU; is the error equation observed by the fisheye camera. is a subtraction operation defined on SO(3), the three-dimensional rotation group. The specific definition of is: and ,That right The subtraction operation is ,in is a logarithmic transformation based on the Rodriguez formula.

[0050] Before solving this equation using the ESIKF algorithm, the four covariance matrices in the formula are used to measure and the reliability of the three sensor observations. When solving the equations, each covariance matrix will be multiplied by the error equations of various observations (the covariance matrix is ​​actually used to weight the observations here). During the process of solving the equations, the covariance matrix will also be iteratively updated. The updated covariance matrix is ​​used to measure the reliability of the pose after solving the optimization.

[0051] In the above state optimization objective equation, ,in, nis the normal vector of the surface element corresponding to the point, q are the coordinates of the center point of the surface element. Figure 3 , which shows a schematic diagram of a point-to-surface error measurement provided by an embodiment of the present invention. The point-to-surface error is measured by calculating the Euclidean distance between the LiDAR point and the corresponding surfel. For a LiDAR point in a LiDAR frame, its distance to the corresponding surfel is defined as , when the scanning mapping device pose reaches the optimal, the distance from the point to the surface element is the shortest.

[0052] ,in, , , They are i Time has come j The residual of the relative rotation change observation at the moment, the residual of the relative velocity change observation and the residual of the relative position change observation; , where (X, Y, Z) is the coordinate of the i-th point in the global coordinate system obtained after the i-th point in the point cloud data is transformed according to the initial pose data; , is the focal length of the fisheye camera, , ( , ) is the coordinate of the feature point in the dedistorted image coordinate system, ( , ) are the coordinates of the principal point in the dedistorted image coordinate system.

[0053] After clarifying the error equations of LiDAR and fisheye camera, the standard solution of ESIKF algorithm can be used to solve the pre-integration calculation formula and realize the real-time solution and update of the position and posture of the scanning mapping device.

[0054] In one embodiment, the step of S203 may specifically include: performing closed-loop identification on the point cloud data at each moment to obtain at least one closed-loop pair; generating posture correction data based on the closed-loop pair; correcting the posture data at each moment based on the posture correction data; performing coordinate conversion processing on the point cloud data at each moment based on the corrected posture data to obtain the coordinates of the point cloud data in the global coordinate system.

[0055] Considering that LiDAR observation has higher reliability and immediacy than visual observation, this embodiment adopts a closed-loop recognition method based on LiDAR observation. Specifically, in order to reduce the adverse effect of non-repetitive scanning LiDAR observation on the recall rate, the present invention adopts a recursive least squares-based loop closure detection (RLS-LCD) algorithm for initial closed-loop recognition.

[0056] Closed-loop recognition refers to identifying point cloud pairs representing the same location in point cloud data collected at different time points. These point cloud pairs are called closed-loop pairs. The relative pose transformation is calculated between the closed-loop pairs. Based on this transformation, the accumulated error of the mapping scanning device between the closed-loop pairs can be corrected.

[0057] In this embodiment, closed-loop recognition is performed in real time during the scanning process, while closed-loop optimization is performed immediately after the scanning is completed. Considering that in the case of pure SLAM (i.e., not relying on external independent observations such as RTK for drift correction), the scanning time of common mobile mapping tasks is generally short, in order to save computing power, the present invention executes both closed-loop constraints and closed-loop optimization after the scanning is completed. After the scan is completed, the closed-loop constraints of each closed-loop pair are first calculated, and then the odometer (scanning mapping device posture) and closed-loop constraints output by the real-time end are input into the factor graph for posture graph optimization, thereby eliminating the accumulated drift to achieve the purpose of posture optimization.

[0058] In one embodiment, the step of performing closed-loop identification on point cloud data at each moment to obtain at least one closed-loop pair may specifically include: merging time-sequentially adjacent point cloud data and generating a submap based on the merged point cloud data; performing closed-loop identification on each submap to obtain at least one closed-loop pair; each closed-loop pair includes two target submaps that meet the closed-loop requirements.

[0059] For example, i and i+1 Point cloud data merging at each moment, j and j+1 The point cloud data at each moment is merged, and then sub-maps are generated separately i Hezi Map j After closed-loop identification, the sub-map is determined i Hezi Map j If the closed loop requirement is met, it is determined that submap i and submap j form a closed loop pair 。 The more common frame-to-frame recognition in this embodiment has a higher recognition rate and recall rate.

[0060] In one embodiment, the step of generating posture correction data based on the closed-loop pairs may specifically include: determining the overlap rate of the target submaps in each closed-loop pair on the object side; taking the closed-loop pairs whose overlap rate is greater than a preset overlap rate threshold as the target closed-loop pairs; and generating posture correction data based on the target closed-loop pairs.

[0061] Using the depth information provided by LiDAR observation as a priori, the pose output by the state optimization objective equation can be used to calculate the coverage of the two target submaps in the closed-loop pair in the object space, and then the overlap ratio of the two object space ranges is calculated. When the ratio is < 0.55, the closed-loop pair is eliminated. Otherwise, the closed-loop constraints can be retained and Pose Graph Optimization (PGO) can be used.

[0062] In one embodiment, the laser radar includes a first laser radar and a second laser radar, and the scanning areas of the first laser radar and the second laser radar overlap; the step of performing closed-loop identification on the point cloud data at each moment to obtain at least one closed-loop pair may specifically include: determining redundant point cloud data in the overlapping area of ​​the first point cloud data scanned by the first laser radar at each moment and the second point cloud data scanned by the second laser radar; removing the redundant point cloud data in the collection of the first point cloud data and the second point cloud data at each moment to obtain the target point cloud data at each moment; performing closed-loop identification on the target point cloud data at each moment to obtain at least one closed-loop pair.

[0063] The step of performing closed-loop identification on the target point cloud data at each moment to obtain at least one closed-loop pair can refer to the above two embodiments. For example, the temporally adjacent target point cloud data can be merged, and a submap can be generated based on the merged target point cloud data; closed-loop identification is performed on each submap to obtain at least one closed-loop pair; each closed-loop pair includes two target submaps that meet the closed-loop requirements.

[0064] Reference Figure 4 (a) shows a schematic diagram of a single laser radar scanning field of view provided by the present invention, and a reference Figure 4 (b) shows a schematic diagram of a dual laser radar scanning field of view provided by an embodiment of the present invention. The scanning areas of the two laser radars overlap in the middle.

[0065] In one embodiment, the image data includes an image; the scanning mapping method also includes: projecting points in the point cloud data into preset images that are adjacent in space, respectively, to obtain preset image points corresponding to each point; judging whether the image point exceeds the field of view of the image corresponding to the image point, and taking the image point that does not exceed the field of view as the target image point; and coloring the target map according to the target image point corresponding to each point.

[0066] In one embodiment, further processing may be performed on multiple target image points corresponding to each point. Specifically, the six image points in the neighborhood of the target image point may be mapped to the object space according to the projection transformation formula to obtain the 3D coordinates of the six image points in the object space coordinate system. If the depth change of the 3D coordinates of the six image points is less than ( The value can be 0.1m), then the target image points corresponding to the 6 image points are retained, otherwise the target image points corresponding to the 6 image points are discarded. Finally, the RGB information of the target image points corresponding to the LiDAR points is extracted, and the LiDAR points are weighted and colored according to the consistency of the normal direction of the laser point and the camera angle of view. The normal direction of the laser point is obtained by using the Random Sample Consensus (RANSAC) algorithm based on the 6 neighboring points of the laser point, and the positive and negative directions of the normal direction are determined according to the direction vector from the LiDAR to the laser point. The specific weighted loss kernel function uses a Gaussian kernel, that is, the larger the normal direction of the laser point and the camera angle of view, the lower the weight.

[0067] Reference Figure 5 , showing a schematic diagram of a real-time data processing flow provided by the present invention. The scanning mapping device starts scanning, and then calibrates the dual LiDAR based on the dual LiDAR external parameter calibration of the common view feature registration between frames. After the calibration is completed, the state of each sensor is initialized. During the scanning process, the scanning mapping device performs real-time pose estimation, and mobile mapping is performed based on the real-time pose estimation. After the scanning is completed, closed-loop recognition is performed, and based on the recognition results, the various real-time pose estimates calculated previously are corrected, and the mapping is re-performed based on the corrected real-time pose estimates.

[0068] Reference Figure 6, showing another data processing flow diagram of the real-time end provided by the present invention. For dual LiDAR observation, the key processing includes: dual LiDAR observation coordinate alignment, LiDAR-IMU observation coordinate alignment, establishment of point-to-surface error equation, closed-loop identification and optimization based on the RLS-LCD algorithm. For fisheye camera observation, the key processing includes: image dedistortion, extraction of ORB feature points and feature matching, camera-IMU observation coordinate alignment, establishment of reprojection error equation, closed-loop verification of joint LiDAR depth and visual common view, and coloring of LiDAR point cloud map in post-processing. Among them, feature matching includes: extracting FAST (Features from Accelerated Segment Test) corner point features on the image and calculating its BRIEF (Binary Robust Independent Elementary Features) descriptor, and achieving feature matching by calculating the Hamming distance between BRIEF descriptors to obtain matching feature points. In order to accelerate the nearest neighbor search, the kernel-constrained cascade hashing algorithm can be used to query matching feature points from the feature point set of the candidate image. For IMU observations, the key processing includes: IMU pre-integration calculation and tightly coupled state estimation based on ESIKF.

[0069] An embodiment of the present invention further provides a scanning mapping device, comprising: a laser radar, an inertial measurement unit IMU, a fisheye camera and a processor; the processor is used to implement the steps in any one of the above-mentioned scanning mapping methods.

[0070] It should be noted that the implementation principle or implementation process of the above processor can refer to the embodiment of the above scanning and mapping method, and will not be described one by one here.

[0071] Those skilled in the art will appreciate that all or part of the processes of the above-mentioned embodiments can be implemented by instructing related hardware through a computer program, and the program can be stored in a computer-readable storage medium, wherein the computer-readable storage medium is a disk, an optical disk, a read-only storage memory, or a random access memory, etc.

[0072] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.

Claims

1. A scanning mapping method, characterized in that: Applied to a scanning mapping device, the scanning mapping device includes a laser radar, an inertial measurement unit and a fisheye camera, and the method includes: Acquire point cloud data scanned by the laser radar, measurement data of the inertial measurement unit, and image data taken by the fisheye camera at each moment; Determining the position and posture data of the scanning mapping device at each moment according to the point cloud data, the measurement data and the image data at each moment; Performing coordinate conversion processing on the point cloud data at each moment according to the posture data at each moment to obtain the coordinates of the point cloud data in the global coordinate system; A target map is constructed based on the coordinates of the point cloud data in the global coordinate system.

2. The scanning mapping method according to claim 1, characterized in that: The image data includes image feature points; and determining the position and posture data of the scanning mapping device at each moment according to the point cloud data, the measurement data and the image data at each moment, including: Performing pre-integration calculation on the measurement data of the inertial measurement unit at each moment to obtain the pre-integrated observation value at each moment; The pose data of the scanning mapping device is obtained by solving the following formula: in, is the pose data of the scanning mapping device; X is the pose data set to be determined; for A priori estimate of ; represents the subtraction operation defined on the three-dimensional rotation group; For measurement The covariance matrix of reliability; is the covariance matrix that measures the reliability of point cloud data; is the covariance matrix that measures the reliability of the pre-integrated observations; is the covariance matrix that measures the reliability of image feature points; , , and for multiplying the operation results of the subtraction operation, the error equation of the laser radar observation, the error equation of the pre-integrated observation value of the inertial measurement unit, and the error equation of the fisheye camera observation respectively when solving the target equation; , , are respectively the number of points in the point cloud data, the number of pre-integrated observation data and the number of image feature points; is the first j The coordinates of the points in the IMU coordinate system; For the pre-integrated observations; For the i The coordinates of the image feature points in the inertial measurement unit coordinate system; is the error equation of the laser radar observation; is the error equation of the pre-integrated observation value of the inertial measurement unit; is the error equation observed by the fisheye camera.

3. The scanning mapping method according to claim 2, characterized in that: The pre-integrated observation values ​​include: relative rotation change observation values, relative speed change observation values ​​and relative position change observation values, and the image data includes the image principal point; ,in, n is the normal vector of the surface element corresponding to the point, q are the coordinates of the center point of the surface element; ,in, , , They are the residual of the relative rotation change observation value from time i to time j, the residual of the relative speed change observation value, and the residual of the relative position change observation value; , where (X, Y, Z) is the coordinate of the i-th point in the global coordinate system obtained after the i-th point in the point cloud data is transformed according to the initial pose data; , is the focal length of the fisheye camera, , ( , ) is the coordinate of the feature point in the image coordinate system after dedistortion processing, ( , ) is the coordinate of the principal point in the image coordinate system after dedistortion.

4. The scanning mapping method according to claim 1, characterized in that: The step of performing coordinate conversion processing on the point cloud data at each moment according to the posture data at each moment to obtain the coordinates of the point cloud data in the global coordinate system includes: The point cloud data is subjected to coordinate transformation processing by the following formula: in, is the coordinate of the point cloud data in the global coordinate system, is the posture data.

5. The scanning mapping method according to claim 1, characterized in that: The step of performing coordinate conversion processing on the point cloud data at each moment according to the posture data at each moment to obtain the coordinates of the point cloud data in the global coordinate system includes: Performing closed-loop identification on the point cloud data at each moment to obtain at least one closed-loop pair; generating posture correction data according to the closed-loop pair; Correcting the posture data at each moment according to the posture correction data; The point cloud data at each moment is subjected to coordinate transformation processing according to the corrected posture data to obtain the coordinates of the point cloud data in the global coordinate system.

6. The scanning mapping method according to claim 5, characterized in that: The step of performing closed-loop identification on the point cloud data at each moment to obtain at least one closed-loop pair includes: Merge the temporally adjacent point cloud data and generate a submap based on the merged point cloud data; Closed-loop identification is performed on each of the sub-maps to obtain at least one closed-loop pair; each of the closed-loop pairs includes two target sub-maps that meet the closed-loop requirements.

7. The scanning mapping method according to claim 6, characterized in that: Generating posture correction data according to the closed-loop pair includes: Determining the overlap rate of the target submaps in each closed-loop pair on the object space; The closed-loop pairs whose overlap rate is greater than a preset overlap rate threshold are taken as target closed-loop pairs; According to the target closed-loop pair, pose correction data is generated.

8. The scanning mapping method according to claim 5, characterized in that: The laser radar includes a first laser radar and a second laser radar, and the scanning areas of the first laser radar and the second laser radar overlap; the closed-loop recognition is performed on the point cloud data at each moment to obtain at least one closed-loop pair, including: Determine redundant point cloud data in an overlapping area of ​​first point cloud data obtained by the first laser radar scanning and second point cloud data obtained by the second laser radar scanning at each moment; Removing the redundant point cloud data in the collection of the first point cloud data and the second point cloud data at each moment to obtain target point cloud data at each moment; Closed-loop identification is performed on the target point cloud data at each moment to obtain at least one closed-loop pair.

9. The scanning mapping method according to claim 1, characterized in that: The image data includes an image; the method further includes: Projecting the points in the point cloud data respectively into preset images adjacent to each other in space to obtain preset image points corresponding to each point; Determine whether the image point exceeds the visual range of the image corresponding to the image point, and use the image point that does not exceed the visual range as the target image point; The target map is colored according to the target image points corresponding to each point.

10. A scanning mapping device, characterized in that: include: LiDAR, inertial measurement unit, fisheye camera and processor; The processor is used to implement the steps in the scanning mapping method described in any one of claims 1 to 9.

Images