3D Reconstruction System Based on Multi Sensor

In tracking thread part mainly through the depth sensor RGB camera ORB feature extracting and feature matching, and triangulation to establish a link between characteristics of map point depth and 3 d coordinate, follow-up images by tracking motion model, reference key frames and relocation camera pose, with a minimum weight projection error to optimize the current position, Then determine whether to generate a new key frame according to the preset conditions.

Local map thread part is mainly on camera RGB camera get key frames is dealing with the key frames tracking thread creation, update the map point and the corresponding relationship between the key frames, delete maps the newly added observations are less map point, then to the high degree of common view of key frames by triangulation restores map, Repeat map key points of key frames and adjacent key frames are checked. When all key frames in the key frame queue are processed, local trapped set adjustment is performed for the current key frame, adjacent key frames and observed map points. Finally, the pose of key frames and map point accuracy are optimized by minimizing reprojection error. In the process of map building, the whole map building process is divided into three parts: the first part is the driving part, responsible for driving the sensor, here is replaced by the data set; The second part is pose estimation. ORB_SLAM2 obtains the camera data and outputs the camera pose. Finally, 3d scene recovery is realized by drawing part to provide environment map for robot navigation in complex scene.

Local map processing thread insert key frames, mainly includes three processes, respectively is closed loop testing, calculation of similarity transformation matrix, and the closed loop correction, the closed-loop detection is by calculating the word bag similar score candidate key frames, then for each key frame to calculate similarity transformation matrix, by random sampling to select the optimal consistency of key frames, Finally, the key frame pose was optimized by the Essential Graph, and the global trapped set adjustment was performed to obtain the global consistent environment map and camera running track.

Dense built figure thread is to perform map point depth range search key frames, then match the price within the depth range, perform keyframe the initial depth, according to the principle of similar depth of adjacent pixels to obtain the initial depth map for inverse depth fusion adjacent pixels and filling vacant pixels, through the depth of the adjacent key frames optimal depth information fusion, Furthermore, the final depth map was obtained by intra-frame filling and external point elimination. Finally, the 3d reconstruction of indoor environment was obtained by point cloud splicing. The specific steps for dense reconstruction are as follows:

Step 1: Estimation of scene depth range: The image obtained by the RGB camera in the depth camera is input as the key frame, and every map point observed by the key frame at any time is projected into the key frame image. The depth value of the map point under the coordinates of the key frame is calculated, and the maximum or minimum depth is selected to set the inverse depth search range of the scene (ρmin, ρ Max).

Innovative marketing module data including 3 axis gyroscope, 3 axis accelerometer and magnetometer sensor are independent of each other and not easy to influence each other between inertial sensor, the integral solution of displacement and rotation, with the passage of time accumulated error is bigger, data are available in a short period of time only, and magnetometers easily affected by the local environment, need correction in advance, Therefore generally do not adopt the method of integral calculation pose directly, instead of multi-sensor fusion method can get good posture calculation results, the current in order to achieve good posture combination of inertial sensor fusion method which USES the visual sensor, vision sensors in feature points clear, feature rich texture of effect is better, Met feature points such as glass, white walls, but fewer cases, recovery scenarios are defective or not working, and IMU sensor use for a long time has very big accumulated error, but the advantage in a short period of time when the displacement data of high accuracy, so adopt the way of visual and inertial data fusion compensate each other, can get good posture calculation results.

The acceleration of IMU coordinate system can be obtained by the accelerometer and gyroscope of IMU, as shown in the following formula (3-1). On the left is the direct output of IMU, RWB_(t) is the rotation matrix from IMU coordinate system to the world coordinate system, while RWB(t)T {\rm{R}}_{{\rm{WB}}({\rm{t}})}^{\rm{T}} is the rotation matrix representing the world coordinate system to IMU coordinate system. w^a(t) represents the acceleration in the world coordinate system, w^g(t) represents the gravity vector in the world coordinate system, b^a_(t) and b^a_(t) represent the offset of the accelerometer and gyroscope, η^a_(t) and η^g_(t) represent the noise of accelerometer and gyroscope respectively, and the effect of earth rotation is ignored, thus assuming that the world coordinate system is an inertial system. The formula is as follows: (1) Bα(t)=RWBT(t)(Wα(t)−wg)+ba(t)+ηa(t) {B^{\alpha (t)}} = R_{WB}^T(t)({W^{\alpha (t)}} - wg) + {b^a}(t) + {\eta ^a}(t) (2) BϖWB(t)=BϖWB(t)+bg(t)+ηg(t) {B^\varpi}WB(t) = {B^\varpi}WB(t) + {b^g}(t) + {\eta ^g}(t)

The tracking thread can be divided into two situations: map update and no map update. At the beginning, there was no map update, and map update was completed in local map building and closed-loop thread. When the map is updated, the overall optimization state vector should be constructed first, including rotation, translation, accelerometer paranoia and gyroscope paranoia at the current moment J. The overall error equation includes visual reprojection error and IMU measurement error, and the last moment is expressed. (3) θ={RWBj, ppBj, w=vBj, bgj, baj} \theta = \{R_{WB}^j,\,{pp}_B^j,\,w = v_B^j,\,b_g^j,\,b_a^j\} (4) θ*=argθ min ((∑kEproig(k,j)+EIUM(i,j)) {\theta ^*} = {\arg_\theta}\,\min \,((\sum\limits_k {{E_{proig}}({\rm{k}},j) + {E_{IUM}}(i,j}))

When there is no map update, the overall optimization state vector is constructed, including the rotation, translation velocity displacement, accelerometer bias and gyroscope bias of the current moment J +1 and the last moment J. The overall error equation includes visual reprojection error and IMU measurement error, as shown in the formula: (5) θ={RWBj, PWj, vWj, baj,RWBj+1, vWj+1, bgj+1, baj+1} \theta = \{R_{WB}^j,\,P_W^j,\,v_W^j,\,b_a^j,R_{WB}^{j + 1},\,v_W^{j + 1},\,b_g^{j + 1},\,b_a^{j + 1}\}

This article USES the experimental equipment is independently developed by the indoor mobile robot, and above the robot of the original part of the reform so as to build the experiment platform of the topic, because this topic considering the hardware sensors and integration between software development is practical, need for robot original laser radar sensor will be demolished, Control system is a model for the small new Air15 lenovo workstation PC platform is used to control the robot on the surrounding environment scanning, the PC platform for Intel processor i5, run for 16 gb memory, graphics card memory is 4 gb, hardware platform is made of the robot include the robots, robot control, mobile robot perception of these three parts, Among them, Microsoft InteRealsense Depth Camera D435 is used as the main sensor, while the IMU sensor of MODEL MPU9250 is installed. The bottom of the robot is composed of chassis and quadrupedal wheels, which can realize 360° free movement of the robot.

The Inter Realsense Depth Camera D435 is the preferred solution for applications such as phase robot navigation and object recognition. The Realsense D435 offers global shutter sensors and a larger lens for better low-light performance than cheaper D415 cameras. The D435 also features the more powerful RealSense module D430. The D435 camera has four circular holes on its front, running from left to right. The first and third are the IR Stereo Cameral. The second is an IR laser Projector, and the fourth is a color camera (color sensor). As shown below:

Figure 7.

This paper uses lenovo Xiaoxin Air15 computer to build Windows 10 home edition and Ubuntu18.04 mirror double operating system as the system software experimental platform. Compared with building virtual machines (Vmware Workstation Pro), Vmware workstation Pro has a faster loading speed, which is convenient, fast, safe and free, and has a convenient configuration environment. Ubuntu18.04 features simple and beautiful interface, convenient configuration environment, and a large number of third-party libraries and dependencies, including pangloin0.5, eigen3.4.5, OpenCV, etc. Eigen provides fast linear algebra operations on the matrix, but also includes the steps to solve the equation, many of the upper software library is also using Eigen matrix operations OpenCV in this paper is mainly on Realsense D435 depth image processing and optical flow tracking.

In this experiment, the calibration of sensors is a link that cannot be ignored. For the calibration of cameras, a checkerboard calibration grid should be prepared in advance, as shown in the figure 8.

Figure 8.

In this project, the calibration plate is fixed and images at different positions and angles of the calibration plate are collected by moving cameras. In this paper, 15 pictures from different angles are selected, as shown in Figure 9.

Figure 9.

Then, the calibration algorithm was used to extract corner information of each checkerboard calibration board, calculate the homography matrix of each image, and calibrate the external parameters and distortion parameters of the camera according to the size of the checkerboard, as shown in Figure 10. It can be seen that the corner information of the calibration board was extracted accurately.

Figure 10.

According to the reprojection error test, the average error is 0.28213 pixels, which meets the requirements of this experiment. The experimental calibration results are shown in Table 3.

Of multiple sensors is mainly on the camera and IMU sensor calibration, through access to the inside of the camera, in order to get more accurate camera calibration need to get the camera and joint of IMU sensor calibration for the transformation matrix, transformation matrix can give 3 d reconstruction system provides a good initial value, higher accuracy, In this paper, Kalibr tool is used for joint calibration of the two. Just now, internal parameters of the camera have been obtained. Again, TOPIC and IMU data in the image obtained by the camera are extracted through ROS communication. Sufficient rotation and translation camera can collect enough data from different angles to make the calibration results between sensors more accurate. The calibration results are shown in Table 4.

Figure 11 shows the reprojection error diagram:

Figure 11.

In this paper, the RGB-D sensor loaded by the robot is used instead of the IMU sensor module to carry out dense THREE-DIMENSIONAL reconstruction. The displacement curves of the robot in the X and Y directions and the trajectory of the robot are shown in Figure 12. After several experiments, it was found that dense reconstruction using a separate sensor would appear as follows

Figure 12.

The effect shown in Figure 12, and the effect shown in Figure 13 is produced by using the multi-sensor fusion mode:

Figure 13.

As shown in Figure 13, the total length of the robot's moving track is 5m, and the end point positioning error is 0.12m in the X direction and 0.16m in the Y axis direction, indicating that the algorithm in this paper can work stably and effectively in the process of robot movement.

By Fig 12, 13, 14 and table 5. shows that only rely on RGBD sensor vision camera, the colour image and depth map as the input for 3 d reconstruction, in the harsh conditions in complex scene and scene, camera is easy to track lost can't find between scanned, so will break, reconstruction of the map will not be able to use, However, when RGBD camera is constructed with IMU sensor for 3D reconstruction, the problem of image fracture and loss caused by the above single sensor can be alleviated in most implementations.

Figure 14.

Figure 15.