CN110163968A - RGBD camera large-scale three dimensional scenario building method and system - Google Patents

Abstract

The invention discloses a method and system for constructing a large-scale three-dimensional scene with an RGBD camera. The method includes the following steps: obtaining a depth image and a color image of an RGBD camera, generating a three-dimensional point cloud, and registering the point cloud to a laser radar reference system; obtaining The robot attitude data and the scanning point data of the laser radar are processed by the local optimization algorithm of the Cartographerd algorithm to generate sub-graphs and robot odometer information; according to the robot odometer information, the generated sub-graphs are bound to the corresponding point cloud After setting, the submaps are spliced to generate a 3D map.

Description

Method and system for constructing large-scale three-dimensional scene with RGBD camera

technical field

The present disclosure relates to the technical field of three-dimensional modeling, in particular to a method and system for constructing a large-scale three-dimensional scene with an RGBD camera based on a Cartographer algorithm.

Background technique

In the intelligent application of mobile robots, the intelligence of the robot largely depends on the positioning accuracy of the robot in the scene, so it is necessary to provide a high-precision environment map for the robot. Environmental maps generally have two forms: two-dimensional maps and three-dimensional maps. Among them, the two-dimensional maps constructed by using two-dimensional lidar only have two-dimensional two-dimensional data, and more information will be lost during the application process, which is not conducive to the navigation and positioning of robots. Therefore, the 3D map has extremely important application and research value in many aspects.

Compare 3D lidar, depth cameras, and binocular cameras that generate 3D data. Among them, the RGBD camera is limited by the measurement principle, measurement accuracy and measurement distance, and the effect is poor in the application process of large-scale scene mapping. When encountering large interference or the robot runs fast, it is easy to lose its position, although it can The problem of position loss can be solved by changing the constraint relationship between images, but it will inevitably increase the consumption of computer resources. Compared with RGBD cameras, although the anti-interference ability of 3D lidar and binocular camera is stronger, the price of 3D lidar is very expensive, and the generated point cloud is sparse and lacks color information; binocular camera needs to spend additional Computing resources are compared with image computing depth data, both of which are insufficient in speed and accuracy for indoor mapping. Therefore, this paper proposes a method of fusing the two sensors of lidar and RGBD camera, using laser SLAM to output the pose of the robot and superimposing the point cloud of RGBD camera to perform dense mapping in large scenes.

Several popular laser SLAM algorithms are: (1) Gmapping algorithm is a mapping algorithm based on the improved particle filter method. In large scenes, a large number of particles will consume more resources; (2) Karto algorithm, using The method of graph optimization replaces the particle filter, and uses SPA to adjust the graph, but there will be a large number of landmarks inserted in the case of large-scale scene mapping, and it will consume a lot of memory; (3) the Hector algorithm uses the Gauss-Newton method to scan Point sets are scanned and matched, although the memory consumption is less, but the sensor requirements are high, and a high-precision laser radar is required to build a better map; (4) the Cartographer algorithm adopts the method of attitude optimization, and the laser radar The data frame is matched with the submap, and the global optimization is performed by finding the closed-loop constraint in the global optimization process. Compared with the first three methods, this scheme can eliminate most of the accumulated errors and does not require a particularly high-precision laser radar.

In the currently popular 3D reconstruction methods of RGBD cameras, the VSLAM (visual simultaneous localization and mapping) algorithm is mostly used to calculate the constraint relationship between the feature points of the RGBD image at the front end and superimpose the point cloud according to this relationship, and at the same time, the front end data is processed at the back end. The method of loop-closing optimization constructs 3D maps. The inventor found during the research and development process that this method is limited by the RGBD camera measurement principle, measurement accuracy and measurement distance. In the case of large scenes, complex environments, and a large amount of sunlight interference, the mapping accuracy is poor and the robot position is easy to lose. , and it will consume a lot of resources during the calculation, and a computer with higher performance is required to ensure the calculation speed.

Contents of the invention

In order to overcome the deficiencies of the above-mentioned prior art, this disclosure takes indoor robots equipped with lidar and RGBD cameras as the research object, and proposes a method and system for constructing large-scale 3D scenes with RGBD cameras based on the Cartographer algorithm. With strong interference ability, the pose of the robot is calculated by the Cartographer laser SLAM algorithm, and real-time point clouds are inserted into the submap generated by Cartographer to realize real-time mapping; at the same time, the loopback optimization function of Cartographer is used to adjust the previously inserted pose. Experiments in real-world scenarios show that, compared to simply using lidar or RGBD cameras for mapping, the accuracy is higher and the mapping speed is faster.

The technical solution of a large-scale three-dimensional scene construction method for RGBD cameras based on the Cartographer algorithm provided by the present disclosure is:

A kind of RGBD camera large-scale three-dimensional scene construction method based on Cartographer algorithm, this method comprises the following steps:

Obtain the depth image and color image of the RGBD camera, generate a 3D point cloud, and register the point cloud to the lidar reference system;

Obtain robot attitude data and laser radar scan point data, use the local optimization algorithm of the Cartographerd algorithm to process them, and generate subgraphs and robot odometer information;

According to the robot odometer information, the generated sub-map is bound to the corresponding point cloud, and the sub-map is stitched to generate a 3D map.

Further, the point cloud generation method is:

Register the RGBD camera, and register the depth camera reference system to the color camera reference system;

Calculate the three-dimensional position information of any point in the color image in space according to the corresponding pixel value in the registered depth image;

Traverse the entire depth image in turn to obtain the point cloud corresponding to the camera.

Further, the step of registering the point cloud to the lidar frame of reference includes:

Obtain the distance between the optical center of the color camera on the robot chassis and the center of the lidar and its deviation angle on the yaw angle;

The coordinates of each point in the camera point cloud are transformed using a transformation matrix, and the set of scanned points of the lidar is registered with the point cloud.

Further, the step of binding the submap to the corresponding point cloud includes:

Record the pose information and timestamp of the generated subimage, compare the timestamp generated when the subimage is generated with the timestamp of the depth image collected by the RGBD camera, and select the depth image information of the RGBD camera closest to the subimage in time;

Synchronize the RGBD camera depth image information and robot pose information closest to the sub-image in time;

Calculate the horizontal displacement difference and the rotation angle difference between the robot and the sub-image when generating the sub-image to form a rotation matrix, and use the rotation matrix to transform the point cloud into the reference system of its corresponding sub-image;

Bind the point cloud transformed into the sub-image reference system with the sub-image information, and displace the point cloud, and translate the transformed point cloud to the location of the sub-image space.

Further, when the pose of the sub-picture changes, the step of re-translating the point cloud bound to the sub-picture whose pose changes occurs according to the change of the sub-picture.

Another aspect of the present disclosure provides a technical solution for a large-scale three-dimensional scene construction method of an RGBD camera based on the Cartographer algorithm:

A large-scale three-dimensional scene construction system based on RGBD camera based on Cartographer algorithm, the system includes:

The point cloud registration module is used to obtain the depth image and color image of the RGBD camera, generate a three-dimensional point cloud, and register the point cloud to the lidar reference system;

The subgraph generation module is used to obtain the robot attitude data and the scanning point data of the laser radar, and use the local optimization algorithm of the Cartographerd algorithm to process it to generate subgraphs and robot odometer information;

The point cloud binding module is used to bind the generated submap with the corresponding point cloud according to the robot odometer information;

3D building blocks for splicing submaps to generate 3D maps;

The loopback point cloud module is used to re-translate the point cloud bound to the sub-image whose pose has changed according to the change of the sub-image when the pose of the sub-image changes.

The technical solution of the computer-readable storage medium provided by another aspect of the present disclosure is:

A computer-readable storage medium, on which a computer program is stored, and when the program is executed by a processor, the steps in the above-mentioned method for constructing a large-scale three-dimensional scene with an RGBD camera based on the Cartographer algorithm are realized.

A technical solution of a computer device provided by another aspect of the present disclosure is:

A computer device comprising a memory, a processor, and a computer program stored on the memory and operable on the processor, when the processor executes the program, the above-mentioned large-scale three-dimensional scene construction of the RGBD camera based on the Cartographer algorithm is realized steps in the method.

Through the above technical solution, the beneficial effects of the present disclosure are:

(1) This disclosure adopts the large field of view of the laser radar and the high robustness of the Cartographer algorithm, which greatly improves the anti-interference ability and algorithm running speed of the 3D reconstruction of the RGBD camera in a large scene;

(2) This disclosure uses the loop-back optimization function of the Cartographer algorithm to re-translate the point cloud corresponding to the sub-image whose pose has changed, eliminates the cumulative error generated during the mapping process, and improves the accuracy of the RGBD camera 3D reconstruction map.

Description of drawings

The accompanying drawings constituting a part of the present disclosure are used to provide a further understanding of the present disclosure, and the exemplary embodiments and descriptions of the present disclosure are used to explain the present application, and do not constitute undue limitations on the present disclosure.

Fig. 1 is the flowchart one of embodiment one based on the RGBD camera large-scale three-dimensional scene construction method of Cartographer algorithm;

Fig. 2 is the flow chart two of embodiment one based on the RGBD camera large-scale three-dimensional scene construction method of Cartographer algorithm;

Fig. 3 (a) is the registration figure of KinectV2 depth image and RGB image before embodiment one is corrected;

Fig. 3 (b) is the registration figure of KinectV2 depth image and RGB image after embodiment one correction;

Fig. 4 is embodiment one Kinect and radar point cloud registration figure;

Fig. 5 is the binding schematic diagram of embodiment one Submap and point cloud data;

Fig. 6 (a) is the effect diagram of Cartographer loopback before embodiment 1 loopback;

Fig. 6 (b) is the Cartographer loop-back effect diagram after the loop-back of embodiment one;

Fig. 7 (a) and Fig. 7 (b) are the result figure obtained by the large-scale three-dimensional scene construction method of RGBD camera of embodiment one;

Figure 7(c) and Figure 7(d) are the result graphs obtained by the RTAB algorithm;

Figure 7(e) and Figure 7(f) are color images and depth images under sunlight interference;

Fig. 8 (a) and Fig. 8 (b) are result figure obtained by embodiment one RGBD camera large-scale three-dimensional scene construction method;

Fig. 8 (c) and Fig. 8 (d) are the point cloud overlay accuracy schematic diagram of embodiment one RGBD camera large-scale three-dimensional scene construction method;

Fig. 8 (e) and Fig. 8 (f) are the result figure obtained by RTAB algorithm;

Figure 9 is a schematic diagram of Cartographer's mapping accuracy;

Figure 10(a) and Figure 10(b) are schematic diagrams of the comparison of experimental accuracy on the second floor;

Figure 10(c) and Figure 10(d) are schematic diagrams of the comparison of experimental accuracy on the first floor.

Detailed ways

The present disclosure will be further described below in conjunction with the accompanying drawings and embodiments.

It should be noted that the following detailed description is exemplary and intended to provide further explanation of the present disclosure. Unless defined otherwise, all technical and scientific terms used in this disclosure have the same meaning as commonly understood by one of ordinary skill in the art to which this disclosure belongs.

It should be noted that the terminology used here is only for describing specific implementations, and is not intended to limit the exemplary implementations according to the present application. As used herein, unless the context clearly dictates otherwise, the singular is intended to include the plural, and it should also be understood that when the terms "comprising" and/or "comprising" are used in this specification, they mean There are features, steps, operations, means, components and/or combinations thereof.

Embodiment one

This embodiment takes indoor robots equipped with lidar and RGBD cameras as the research object, and proposes a large-scale three-dimensional scene construction method based on Cartographer algorithm for RGBD cameras. The algorithm calculates the pose of the robot, and inserts the real-time point cloud into the submap generated by Cartographer to realize real-time mapping. At the same time, the loopback optimization function of Cartographer is used to adjust the previously inserted pose. Experiments in real-world scenarios show that the mapping accuracy is higher and the mapping speed is faster than that of simply using lidar or RGBD cameras.

Please refer to accompanying drawing 1, described RGBD camera large-scale three-dimensional scene construction method based on Cartographer algorithm comprises the following steps:

S101. Acquire a depth image and a color image of an RGBD camera, generate a three-dimensional point cloud, and register the point cloud to a lidar reference system.

Specifically, the implementation of step 101 is as follows:

S1011. Acquire a depth image and a color image of an RGBD camera.

Specifically, the depth image data and the color image data collected by the RGBD camera Kinectv2 are obtained.

In order to combine the data collected by the RGBD camera Kinectv2 with the data collected by the lidar, it is necessary to register the data of the two sensors in the same coordinate system.

S1012, generating a point cloud.

In order to obtain an accurate point cloud, the RGBD camera is first registered, and the depth camera reference frame is registered to the RGB camera reference frame, as shown in Fig. 3(a) and Fig. 3(b).

After the registration is completed, any point (x _img , y _img ) in the RGB image can calculate the depth value of the point according to the corresponding pixel value in the depth image after registration, and according to the formula (1), (2 ), (3) to calculate the position of the point in space p=[p _x , p _y , p _z ] ^T , and traverse the entire depth image to obtain the point cloud corresponding to the camera.

Among them, Z ₀ represents the distance from the camera to the reference plane used for calibration, f represents the focal length of the depth camera, b represents the distance from the depth camera to the RGB camera, and (c _x , _cy ) represents the optical center coordinates of the depth camera.

S1013, registration of the point cloud to the lidar coordinate system.

After obtaining the camera point cloud, in order to accurately insert the point cloud into the raster map generated by Cartographer, it is necessary to register the point cloud and the lidar scanning points.

Measure the distance between the optical center of the RGB camera on the robot chassis and the center of the lidar and the angle of deviation on the Yaw axis ξ=(ξ _x ,ξ _y ,ξ _z ,ξ _θ ), because the camera and the radar are placed relatively horizontally, so The offsets of the Pitch and Roll axes can be ignored. After measuring the above data, use the transformation matrix T _ξ to transform the coordinates of each point in the camera point cloud:

Among them, p is the position of the point in space.

The registration of the scanning point set and the point cloud of the lidar can be completed, and the effect diagram is shown in Figure 4.

S102. Obtain robot attitude and lidar data, and use the local optimization algorithm of the Cartographerd algorithm to process the obtained data to generate subgraphs and robot odometer information.

Cartographer is a cross-platform, multi-sensor fusion laser SLAM algorithm proposed by Google in 2016. It consists of two parts: local optimization (Local SLAM) and global optimization (Global SLAM).

Please refer to Figure 2. In the front-end local optimization algorithm, Cartographer mainly combines the newly inserted data with the corresponding subgraph by fitting the newly inserted lidar scanning point data and the acceleration and angle data measured by the inertial measurement unit (IMU). The position matching problem between is equivalent to a least squares problem, so that the probability of matching between the scanning data point set and the sub-graph is the largest, so as to find the best insertion point of the data in the sub-graph, and at the new scanning point The subgraph is updated in real time after the data is inserted.

The subgraph generated in the local optimization algorithm is a new concept introduced by the Cartographer algorithm. It is used to represent a part of the global map and presented in the map in the form of a probability grid. Each of its grids has a fixed probability value to represent the probability that the grid is blocked. After new scan point data is inserted into the subgraph, a hits set and misses set representing the grid points are recalculated. , by comparing with the hits set and misses set before the subgraph, use the probability formula (2) to update the probability value p _hits or p _miss of the grid points represented by the hits and misses sets. And output the current subgraph after inserting enough scan point sets, and start the update of the new subgraph.

M _new (x) = clamp (odd ^-1 (odds (M _old (x)) · odds (p _hits/misses ))) (6).

In the back-end global optimization algorithm, Cartographer will record each subgraph generated by the local optimization algorithm and its scanning point data and its position for loop detection, and use the branch and bound method to develop The window detects the loopback, compares the newly inserted data with the previously recorded data, and establishes the loopback constraint. Finally, the SPA (Sparse Pose Adjustment) algorithm is used to optimize the positions of all subgraphs. Figure 6(a) and Figure 6(b) are the effect diagrams before and after the loopback in the real scene. It can be clearly seen that most of the accumulated errors can be eliminated through the loopback correction.

In this embodiment, the robot odometer information is the path information of the robot, including coordinate changes and angle changes of the robot on the X-axis and Y-axis per unit time.

S103. Bind the sub-image obtained in step 102 with the corresponding point cloud, and stitch the sub-images to generate a three-dimensional map.

In this embodiment, the mapping work is completed by using the sub-image generated by the Cartographer algorithm as a key frame, and inserting the corresponding point cloud into the corresponding space of the sub-image. At the same time, at the back end, Cartographer will perform loopback optimization on the sub-image and the scan point set inserted into the sub-image before, and adjust the point cloud inserted into the sub-image by adjusting the pose information of the sub-image. Therefore, in order to make the point cloud information between sub-images can be accurately superimposed, it is necessary to bind the point cloud information with the sub-images, and find a method that consumes less resources to update the adjusted point cloud.

Specifically, the specific steps of binding the sub-image to the corresponding point cloud are as follows:

As the robot advances, as the lidar scanning points are continuously updated, Cartographer will continuously output robot pose information in the form of the ROS topic "/tf". Record the pose information as δ=[δ _x , δ _y , δ _z , δ _pitch , δ _roll , δ _yaw ] ^T , and compare the timestamp of the /tf topic and the image topic with the depth image of the real-time RGBD camera, RGB Images are time-synchronized. At the same time, whenever a new sub-image is generated, the pose information of the newly generated sub-image is recorded as φ=[φ _x , φ _y , φ _z , φ _pitch , φ _roll , φ _yaw ] ^T and timestamp, through Compared with the timestamp of the image, the depth image information and robot pose information closest to the sub-image are selected.

Since Cartographer will continuously generate submaps during the running process, as the submaps are superimposed, the registered point cloud bound on the submaps will also be superimposed, and a global map can be generated. Figure 5 shows the online running process The superimposition effect of the point cloud bound by Submap ₁₄ and the point cloud bound by Submap ₁₅ .

In the process of synchronizing image information and robot pose information, the release frequency of image information is 25HZ, and the release frequency of robot pose is 70HZ. Time synchronization between the two can control the time difference between the two within ±14ms. Different from the above synchronization process, since the publishing frequency of the subimage is set to 5HZ, when Cartographer outputs the newly generated subimage, the received image data has a theoretical time error of ±40ms, but there are many calculations in the actual operation process, especially During the loopback process, program blocking may occur, and the time error between the sub-image and the matched point cloud data is generally about 120ms. At this time, the attitude δ and φ of the point cloud received are not necessarily equal. In this embodiment, the robot moves at a speed of 2m/s, and there is generally about 20cm between the sub-image and the robot pose matched by the sub-image. error. And the attitude value of the newly generated sub-image of Cartographer points to the default direction, which represents the orientation of the sub-image. After the new scanning point data is added in the later stage, this value will be continuously adjusted, and the attitude value of the orientation of the robot in the robot pose is different two variables. At the same time, when Cartographer performs loopback optimization, the pose of the loopback point cloud will not be adjusted, but the pose of the subimage corresponding to the loopback point cloud will be adjusted.

Therefore, it is necessary to transform the point cloud, calculate the horizontal displacement and rotation angle between the robot and the sub-image through formula (7) and formula (8), and use the Eigen library of C++ to initialize the rotation matrix R _Δ to the Euler angle The direction r _Δ points to. Bring the rotation matrix R _Δ and the Euler angle t _Δ into the Euclidean transformation matrix Trans _Δ , and use the formula (9) to transform the point cloud P into the reference frame of its corresponding subimage.

t _Δ =[δ _x -φ _x , δ _y -φ _y , δ _z -φ _z ] ^T (7)

r _Δ ＝[δ _pitch -φ _pitch ，δ _roll -φ _roll ，δ _yaw -φ _yaw ] ^T (8)

Bind and record the point cloud transformed into the sub-image reference system with the sub-image information, and then displace the point cloud according to formula (11), and then translate the transformed point cloud to the location of the sub-image space.

t _Submap = [φ _x , φ _y , φ _z ] ^T (10)

Among them, t _Submap is the pose of the corresponding submap in space; p is the pose information bound to the points in the point cloud on the submap.

In this embodiment, the subgraph is bound to the corresponding point cloud. After the loopback, when the pose information of the subgraph needs to be adjusted, it is not necessary to perform pose adjustment on the point cloud bound to the subgraph whose pose needs to be adjusted. Transform to shorten the time spent in loopback.

As the Cartographer algorithm is running, the sub-graphs are gradually spliced to generate a global 3D map, and the point clouds bound to the corresponding point clouds will also be spliced to realize the construction of the RGBD camera 3D scene graph.

S104, looping back the point cloud, when the pose of the sub-image changes, the point cloud bound to the sub-image with the change in pose is translated according to the change of the sub-image.

With the continuous addition of subgraphs, Cartographer will use the DFS branch and bound method to perform loop closure detection, establish loop closure constraints, and use the SPA method to adjust the pose φ of the subgraph in real time. That is, the pose φ of the previously established sub-image will not only be adjusted during loop closure, but will also be adjusted during sparse pose correction, and the frequency of sub-image pose adjustment will be relatively high.

In the embodiment, the PointXYZRGB type in the PCL library is used to store and superimpose the point cloud. If all the point clouds stored before re-translation and superimposition take a long time after each sub-image changes, real-time cannot be realized. Point cloud overlay and display.

To solve this problem, the transformation and superposition are performed in partitions during the pose transformation of the sub-images. Use the class template Vector in the C++ library to define an area point cloud array of PointXYZRGB type, and insert the area point cloud array after superimposing the set threshold number of point clouds. When the pose of the sub-image changes, only the corresponding area point cloud is re-translated and superimposed, and all area point clouds are converted into ROS topics and published in the timer callback function.

In the mapping experiment, when the pose of 11 sub-images changes and the point cloud is re-stacked, it takes 2.1s, and the display frequency of RVIZ can basically be stabilized at more than 5 frames (27 sub-images p) without point cloud re-stacking. Basically has no effect on real-time display.

Specifically, the specific implementation steps of regional point cloud overlay are as follows:

Obtain the subgraph output by the Catographer algorithm and the generated real-time point cloud;

Judging whether there is a new sub-image, if it is new, look for the real-time point cloud with the smallest time difference with this time;

Transform the real-time point cloud to the position of the relative new submap according to formula (9);

If the new subimage is added to the latest residual point cloud, and the number of subimages in the residual point cloud is greater than the set threshold, add the point cloud to the residual point cloud and use it as an area point The cloud is recorded to memory while clearing the contents of the residual point cloud.

If the new subimage is added to the latest residual point cloud, and the number of subimages in the residual point cloud does not reach the set threshold, add the point cloud to the residual point cloud and wait for the next new subimage Appear to judge.

If it is found that the pose of the previously generated sub-image changes, determine which area point cloud the sub-image is in, and re-translate the point cloud in the area point cloud according to formula (11), and output it.

The large-scale three-dimensional scene map obtained in this embodiment can be used for intelligent spraying of vegetable greenhouses. The RGBD camera three-dimensional scene construction method proposed in this embodiment can be used to construct a three-dimensional environment map of vegetable greenhouses, and the robot can be controlled to move in vegetable greenhouses to spray plants. medicine or fertilization.

Experimental verification

Experimental verification is carried out on the RGBD camera 3D scene construction method proposed in this embodiment based on the Cartographer algorithm. In the course of this experiment, a field test is carried out using the Mecanumlen mobile platform equipped with KinectV2 camera, SICKTIM561 lidar and IMU. Carry out the computer configuration of step in the method for constructing the three-dimensional scene of RGBD camera: Macbookpro (Apple), i7 6700 2.6GHZCPU, 16GRAM, 512GRAM. In the process of building the map, the point cloud data in the range of 0.2m to 5.5m with high accuracy of Kinect sensor depth data is selected for calculation, and the original point cloud and the superimposed point cloud are processed by using a voxel filter (Voxel Filter) with a grid size of 0.03m. The point cloud is down-sampled, and the point cloud data is sent to RVIZ for display in the form of a ROS topic. The experimental site is selected in the hall on the first floor and the circular corridor on the second floor of the main building of the campus, as shown in Figure 7(a), Figure 7(b), Figure 7(c), Figure 7(d), Figure 7(e) and Figure 7( f) as shown. And choose the RTAB (Real Time Appearance-Based Mapping) indoor mapping algorithm with better accuracy and running speed than RGB-DSLAMV2 and DVOSLAM (Dense Visual Odometry and SLAM) for comparative experiments.

First, in order to verify the ability of the RGBD camera 3D scene construction method proposed in this example to suppress interference, a field experiment was carried out in the corridor hall with uneven illumination on the second floor of the main building of the campus. The robot ran around the corridor at a constant speed of 2m/s for one and a half circles With the help of the corridor walls, the lidar and RGBD cameras can measure more data, and better odometer accuracy can be obtained without loop correction. There is a glass wall at the entrance of the second floor, and there will be a lot of sunlight. From the depth images in Figure 7(e) and Figure 7(f), it can be seen that RGBD cameras lose most of the depth information under the interference of sunlight. It has a great impact on the RTAB algorithm that only uses visual information, and a large error will be introduced in the calculation process of RTAB's visual odometer. None of them could complete the construction of the three-dimensional map. During each experiment, the position of the robot would be lost when approaching the door, resulting in the failure of the map construction. However, the 3D map constructed in the only successful mapping experiment is also far from the real scene, and ghosting, dislocation and distortion appear in many places, as shown in Figure 7(c) and Figure 7(d) Show. Compared with RGBD cameras, lidar has stronger anti-noise performance, and the effective measurement distance is longer. At the same time, the loopback optimization function of Cartographer is used to eliminate most of the accumulated errors in the mapping process, so that the method proposed in this embodiment can be widely used. The advantages of the 3D reconstruction operation in the scene are more obvious, and a more accurate 3D map can be constructed in a large scene, as shown in Figure 7(a) and Figure 7(b).

In order to compare the accuracy of the point cloud superposition of the method in this embodiment in the process of 3D reconstruction of large scenes, a comparative experiment was carried out on the first floor of the main building of the campus where there is no sunlight at night and the indoor light source is relatively uniform. The hall is 27m long, 16m wide, and has an area of about is 300m ² , the robot runs around the column on the first floor for one and a half circles at a constant speed of 2m/s. From the mapping effects shown in Figure 8(c) and Figure 8(d), it can be seen that using the Submap pose calculated by Cartographer to superimpose the point cloud data can also obtain a better mapping effect, which is similar to that used in the RTAB algorithm. Compared with the results obtained by aligning the point cloud of the visual odometry (as shown in Figure 8(e) and Figure 8(f)), the point cloud calculated using the algorithm in this paper can be more accurate at a resolution of 5cm. Accurate alignment, the superimposed point cloud is basically free of ghosting.

Since the algorithm is not verified in a laboratory environment, it is not possible to set up optical tracking instruments in the main building. Therefore, in order to verify the method of this embodiment and the mapping accuracy of RTAB, a field measurement was carried out on the first floor of the main building, and the mapping accuracy of Cartographer was first verified. As shown in Figure 9, the distance error between the two horizontal supporting columns is 2cm, the error between the two vertical supporting columns is 1cm, the distance error of the longest horizontal part of the hall is about 4cm, and the distance between the longest vertical part The error is 7cm. It is consistent with the error range in the experimental results of Wolfgang Hess et al. in the paper Real-Time Loop Closure in 2D LIDAR SLAM.

Then draw the trajectory calculated by the method of this embodiment and the RTAB algorithm on the same map, and compare the mapping accuracy of the algorithm in this paper and the RTAB algorithm through the distance error between the two trajectories. By Figure 10 (a), Figure 10 (b), Figure 10 (c), Figure 10 (d) shown. It can be seen that the RTAB algorithm based on visual odometry will have a large calculation error greater than 1m in the case of open scenes or large-scale interference, which is much larger than the 5cm error of the Cartographer algorithm. Therefore, the three-dimensional odometry constructed by the method of this embodiment Maps are more accurate.

The RGBD camera large-scale three-dimensional scene construction method based on the Cartographer algorithm proposed in this embodiment aims at the problem that the RGBD visual SLAM algorithm is easy to lose its own position and the accuracy of mapping is not enough in the case of large scenes and large disturbances, and the laser SLAM algorithm is applied. In the 3D mapping of large scenes, and on-site experiments were carried out in the main building of the campus, it was verified online that the algorithm in this paper is more real-time, accurate and anti-interference than the traditional mapping algorithm that only uses visual odometry data.

Embodiment two

The present embodiment provides a system for constructing a large-scale three-dimensional scene of a RGBD camera based on the Cartographer algorithm, the system comprising:

The point cloud registration module is used to obtain the depth image and color image of the RGBD camera, generate a three-dimensional point cloud, and register the point cloud to the lidar reference system;

The subgraph generation module is used to obtain the robot attitude data and the scanning point data of the lidar, and use the local optimization algorithm of the Cartographerd algorithm to process it to generate subgraphs and robot odometer information;

The point cloud binding module is used to bind the generated submap with the corresponding point cloud according to the robot odometer information;

The 3D building module is used to splicing sub-images to generate a 3D map, and the point cloud bound to the corresponding point cloud will also be spliced to realize the construction of the RGBD camera 3D scene graph;

The loopback point cloud module is used to re-translate the point cloud bound to the sub-image whose pose has changed according to the change of the sub-image when the pose of the sub-image changes.

Embodiment three

This embodiment provides a computer-readable storage medium on which a computer program is stored, and when the program is executed by a processor, the steps in the method for constructing a large-scale three-dimensional scene of an RGBD camera based on the Cartographer algorithm as shown in Figure 1 or Figure 2 are realized. step.

Embodiment four

This embodiment provides a computer device, including a memory, a processor, and a computer program stored on the memory and operable on the processor. When the processor executes the program, the computer program shown in FIG. 1 or FIG. 2 is implemented. Steps in the method for constructing large-scale three-dimensional scenes with RGBD cameras based on the Cartographer algorithm.

Those skilled in the art should understand that the embodiments of the present disclosure may be provided as methods, systems, or computer program products. Accordingly, the present disclosure may take the form of a hardware embodiment, a software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present disclosure may take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, optical storage, etc.) having computer-usable program code embodied therein.

The present disclosure is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It should be understood that each procedure and/or block in the flowchart and/or block diagram, and a combination of procedures and/or blocks in the flowchart and/or block diagram can be realized by computer program instructions. These computer program instructions may be provided to a general purpose computer, special purpose computer, embedded processor, or processor of other programmable data processing equipment to produce a machine such that the instructions executed by the processor of the computer or other programmable data processing equipment produce a An apparatus for realizing the functions specified in one or more procedures of the flowchart and/or one or more blocks of the block diagram.

These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing apparatus to operate in a specific manner, such that the instructions stored in the computer-readable memory produce an article of manufacture comprising instruction means, the instructions The device realizes the function specified in one or more procedures of the flowchart and/or one or more blocks of the block diagram.

These computer program instructions can also be loaded onto a computer or other programmable data processing device, causing a series of operational steps to be performed on the computer or other programmable device to produce a computer-implemented process, thereby The instructions provide steps for implementing the functions specified in the flow chart or blocks of the flowchart and/or the block or blocks of the block diagrams.

Those of ordinary skill in the art can understand that all or part of the processes in the methods of the above embodiments can be implemented through computer programs to instruct related hardware, and the programs can be stored in a computer-readable storage medium. During execution, it may include the processes of the embodiments of the above-mentioned methods. Wherein, the storage medium may be a magnetic disk, an optical disk, a read-only memory (Read-Only Memory, ROM) or a random access memory (Random Access Memory, RAM) and the like.

Although the specific implementation of the present disclosure has been described above in conjunction with the accompanying drawings, it does not limit the protection scope of the present disclosure. Those skilled in the art should understand that on the basis of the technical solutions of the present disclosure, those skilled in the art do not need to pay creative work Various modifications or variations that can be made are still within the protection scope of the present disclosure.

Claims (10)

1. a kind of RGBD camera large-scale three dimensional scenario building method based on Cartographer algorithm, characterized in that this method The following steps are included:

The depth image and color image of RGBD camera are obtained, generates three-dimensional point cloud, and point cloud registering to laser radar is referred to System；

The number of scan points evidence for obtaining robot pose data and laser radar, utilizes the local optimum of Cartographerd algorithm Algorithm handles it, spanning subgraph and robot odometer information；

According to robot odometer information, the subgraph of generation is bound with corresponding cloud, splicing subgraph generates dimensionally Figure.

2. the RGBD camera large-scale three dimensional scenario building method according to claim 1 based on Cartographer algorithm, It is characterized in that described cloud generation method are as follows:

RGBD camera is registrated, depth camera referential is registrated under color camera referential；

According to the three-dimensional position of any point in the corresponding calculated for pixel values color image in depth image after registration in space Confidence breath；

Whole picture depth image is successively traversed, corresponding cloud of camera is obtained.

3. the RGBD camera large-scale three dimensional scenario building method according to claim 1 based on Cartographer algorithm, It is characterized in that described include: by point cloud registering to the step of laser radar referential

Robot chassis is obtained to enamel the angle of deviation of the camera photocentre at a distance from laser radar center and its in yaw angle Degree；

It is converted using coordinate of the transformation matrix to each point in camera point cloud, by the scanning point set of laser radar and puts cloud It is registrated.

4. the RGBD camera large-scale three dimensional scenario building method according to claim 1 based on Cartographer algorithm, It is characterized in that the subgraph by generation includes: the step of binding with a corresponding cloud

The posture information and timestamp of the subgraph generated are recorded, the timestamp generated when by spanning subgraph and the acquisition of RGBD camera are deep The timestamp of degree image compares, and selects the RGBD camera depth image information on the time near subgraph；

Time synchronization will be carried out near the RGBD camera depth image information of subgraph and robot posture information on time；

The differential seat angle of horizontal displacement difference and rotation when calculating spanning subgraph between robot and subgraph, forms spin matrix, makes In the referential that Cloud transform is corresponded to subgraph with spin matrix to it；

The point cloud transformed in subgraph referential and picture information are bound, and a cloud is displaced, by transformed cloud Move to Sub-graph Space position.

5. the RGBD camera large-scale three dimensional scenario building method according to claim 1 based on Cartographer algorithm, It is characterized in that further including that pose point cloud bound in the subgraph changed is occurred according to subgraph when subgraph pose changes The step of variation is translated again.

6. the RGBD camera large-scale three dimensional scenario building method according to claim 5 based on Cartographer algorithm, It is characterized in that described there is the step of point cloud bound in the subgraph changed is translated again according to subgraph variation packet for pose It includes:

Judge the region point cloud where the subgraph changed occurs in pose, the point cloud in the region point cloud is changed again according to subgraph Move to Sub-graph Space position.

7. a kind of RGBD camera large-scale three dimensional scenario building system based on Cartographer algorithm, characterized in that the system Include:

Point cloud registering module generates three-dimensional point cloud, and point cloud is matched for obtaining the depth image and color image of RGBD camera Standard arrives laser radar referential；

Subgraph generation module is utilized for obtaining the number of scan points evidence of robot pose data and laser radar The Local Optimization Algorithm of Cartographerd algorithm handles it, spanning subgraph and robot odometer information；

Point cloud binding module, for according to robot odometer information, the subgraph of generation to be bound with corresponding cloud；

Three-dimensional building module generates three-dimensional map for splicing subgraph.

8. the RGBD camera large-scale three dimensional scenario building system according to claim 7 based on Cartographer algorithm, It is characterized in that the system further include:

Winding point cloud module, for point cloud bound in the subgraph changed being occurred in pose and is pressed when subgraph pose changing It is translated again according to subgraph variation.

9. a kind of computer readable storage medium, is stored thereon with computer program, characterized in that the program is executed by processor The Shi Shixian RGBD camera large-scale three dimensional scene structure for example of any of claims 1-6 based on Cartographer algorithm Step in construction method.

10. a kind of computer equipment including memory, processor and stores the meter that can be run on a memory and on a processor Calculation machine program, characterized in that realize when the processor executes described program and be based on as of any of claims 1-6 Step in the RGBD camera large-scale three dimensional scenario building method of Cartographer algorithm.