SIMULTANEOUS REGISTRATION AND INTEGRATION OF TWO SEQUENTIAL VELODYNE POINT CLOUDS USING VOXEL-BASED LEAST SQUARE ADJUSTMENT

Abstract. In this paper, a model for simultaneous registration and 3D modelling of Velodyne VLP 32e laser scanner point clouds based on least square adjustment methods was developed. Considering that the most of proposed methods for registration of point clouds which obtained by mobile mapping systems have applications in navigation and visualization. They usually do not pay enough attention to geometric accuracy, error propagation, and weights analysis. In addition, in these methods, some point correspondence solutions are used which increase the computation time and decrease the accuracy. Therefore, the purpose of this paper is to develop a model based on the least square adjustment and focus on the weight of the plane parameters which created by a robust least square fitting algorithm. It also simultaneously creates a 3D environmental model and registers point clouds. To do this, it utilizes both point cloud voxelization and differential planes techniques. The result illustrates the high capability of the proposed solution with the optimum weight of plane parameters to 100, and average distance between two scans can reach to below 10 mm.In addition, the best voxel size was 10 cm which is twice of point cloud resolutions.



INTRODUCTION
The mapping industry has made rapid progress over the past few years.The uses of mobile mapping devices and LiDAR (Light Detection and Ranging) technology to create 3D point clouds are examples of these applications that are widely used in remote sensing, machine vision, and robotics.There are several ways to get 3D data, which can be stereo images, LiDAR and sonar scan, flash LiDAR, and so on.The problem of mapping in outdoor environments can be done using auxiliary data such as GNSS system, IMU, and wheel encoder.But mapping in an indoor environment and underground space which GNSS (Global Navigation Satellite System) signal is denied, become a fascinating topic for researchers.Many solutions can be found for this problem but Simultaneous localization and mapping (SLAM) (H.Durrant-Whyte, T. Bailey, 2006) is the most popular technique which is used for this task.The core of SLAM problem is scan matching, the process of aligning two scans that belong to same scene, which estimate the relative position and orientation of two scans.All scan matching algorithms are divided into pairwise and incremental scan matching (LV, 2016).In pairwise scan matching, new scan matches with the previous scan to find relative transformation.In incremental scan matching, new scans is match with global map after that the global map is updated.One of the most accurate and fastest 3D data acquisition systems is the hand-held mobile laser scanner system which acquired data in sequential scan manner.So these data should be integrated together by scan matching algorithm.Most scan matching algorithms are based on identifying the corresponding points in both target and source cloud and minimizing the distance between these points, which sometimes due to incorrect identification of correspondence points, these algorithm do not work very well.Although a system which can be used for mapping and focus on geometric accuracy, adjustment and error propagation is less available.The purpose of this work is to develop a system which focuses on registration and integration of data simultaneously and consider the weight of the unknown parameters and detect blunders in which registration process id done without using of correspondence point.
Also, we know that with increasing data acquisition time, size of saved data will be larger and it's hard to process cumulative point cloud data, so we want to create a 3D model of the environment in real-time and updating it over the time.For this aim, we voxelize the environment and create a differential planes of the object which all these planes create a uniform object based on least square adjustment.On this basis, we want to develop equations that simultaneously carry out registration and integration, to have an integrated 3D model of the environment, and also the amount of captured data and calculation does not increase so much.

RELATED WORK
Many works on laser scan matching have been presented, so we do not give here a comprehensive study.We will review only a few most relevant and recent works, including feature based algorithm, ICP family (Paul J. Besl;Neil D. McKay, 1992), and other new methods.
ICP is used to estimate the translation and rotation parameters that brings two scan into the best alignment by finding the correspondence points and minimizing the distance between two scans.
One of the most well-known ICP algorithm is Generalized-ICP (Aleksandr S, Haehnel D, and Thrun S, 2009)  , cross-correlation and differential evolution is used for aligning two scans.The cross-correlation is used between two scans as an efficient measure of scan alignment and the differential evolution algorithm is used to look for transformation parameters which align scans.

METHODOLOGY
The most of the registration methods for the sequential point clouds are focus on navigation and visualization, so the goal is to provide an algorithm for registration of sequential point clouds using the least squares adjustment methods.So proposed algorithm is as follows, which will be further explain below.
Figure 1.Flowchart of the proposed method It's obvious that distance between the first and second scan in sequential point clouds is low, so the point cloud voxelization approach for find correspondence area was used.If voxel size be more than the maximum distance between of two sequential scan, the correspondence point fall in same voxel.By gridding the point cloud, the key step of most registration algorithm, finding correspondence points, was eliminated.

Figure 2-Voxelize point cloud
The next step is to create an initial model which is created by fitting the plane to the points inside each voxel.The aim of all registration algorithm is to decrease distance between to point clouds, so by fitting the plane to the source point cloud, we are trying to move the target points to these planes.The prerequisite for creating a plane in a voxel is that the number of points within it, be greater than a threshold, which makes the planes created more accurate.Using a robust least square fitting algorithm, the inlier points inside the voxel are detected and the plane parameters are computed.Now that the initial model has been form, the second scan points will be added to the voxels.Xiao et al (J.Xiao, B. Adler and H. Zhang, 2012)stated that the transformation parameter can be computed at least by three unparalleled plane, so in our algorithm lots of differential plane are created which are unparalleled that can be used in registration process.If in each voxel the number of new points is sufficient (more than a threshold), these voxels are selected and the initial plane parameters are used as initial observation and the new points are utilized to obtain the registration parameters and improve plane parameters simultaneously.
Figure 4-Plane fitted to inliers points (red) (blue points are outliers) Where: : ( *  + ) = 0 (1) A is a matrix of plane parameter R is the rotation matrix T is the translation matrix This equation indicates that new points should be placed on the plane which was created from the previous scan, and also improved the plane parameters, which requires more new equations to be considered as initial observations, and W is a weight matrix to increase the accuracy of plane parameter .
=  0 ,   =  0 ,   =  0 ,   =  0 (2) To do this the least squares adjustment method was used so that the new points should be placed in the equation of the initial planes, but this does not happen because of noise and blunder.So our solution is to use the least squares adjustment. Where: 0 is a matrix of the initial value  is the unknown parameter (registration and plane parameter) Finally, we update the plane and registration parameters after each iteration.
=   −  ⋮   =   −  (5) Note that the equations are solved repeatedly, so after each step, we must update the calculated values for the unknown and plane parameters and re-use them as new observations.Also, the condition for stopping the solution of the equation is to reach proper precision.

EXPERIMENT
At first, the characteristics of data used in experiments will be introduced and then describe the tests.

Data
In this research indoor point cloud data was used which was collected by Velodyne VLP-32e laser scanner .The scanner uses 32 IR lasers to measure distances to objects.The laser used in this sensor has a wavelength of 903nm which is eye safe (Class 1) and can measure up to 200 meters with a precision of 3cm.The horizontal field of view of the sensor is 360 degrees, and the vertical field of view of the sensor is 40 degrees (from -25 to +15).The vertical angular resolution of the sensor is 0.33 degrees, and the horizontal resolution is from 0.1 to 0.4 degrees based on rotation speed (RPM).Also, the sensor ranges from 5 to 20 Hz.The sensor is capable of capturing 600,000 points per second in single-return mode and 1,200,000 points in the dual-return.

Parameters Setting and Algorithm Results
Once the data is captured, it is time to voxelize the points cloud.The algorithm was tested with several voxel dimension and weight parameter.The voxel dimensions used in the implementation of the algorithm are 10, 15, and 20 cm, and the threshold of the number of points for plane construction is 10 points.The value of the weight matrix for the plane parameters were considered are including 1, 5, 10, 20, 50, and 100.
Also to illustrate the effect of outlier points on the algorithm, two methods were employed when adding the second scan points: In the first method outlier points were removed, but in the second method the algorithm was implemented without eliminating those points.The results show that if the outlier points are not identified and removed, the algorithm does not perform well and respond correctly.Figure 6 shows the result of proposed registration for two sequential scan.
In evaluation section, two approaches have been used for evaluating proposed method.In the first approach, the average distance between the reference and registered clouds with different weight and voxel dimensions was computed.The results show that the algorithm outputs with smaller voxel dimensions as well as the lower weight parameter are better.The results of this evaluation can be seen in Table 1.Also, compared to the ICP algorithm with an average distance of 9.725 mm between points, better results were obtained.Figure 6.Output of the proposed algorithm (red is 1 st , green is 2 nd scan and white is registered point clouds) In the second approach, the registered point cloud with the proposed algorithm is compared with result of ICP algorithm.The results of this evaluation show that distance between more than 91% of the points are less than 12 mm (Fig. 8) and the average distance between results of the two methods is less than 5 mm.Finally, comparison between 3 different voxel size show that this parameter should not be very large and optimum size is twice of point cloud resolutions in which best registration result was obtained.

CONCLUSION
Today hand held and wearable mobile laser scanner systems have widely utilized for navigation and visualization of indoor and outdoor environments but we need to develop these systems in mapping application with focus on geometric accuracy, adjustment and error propagation.
In proposed algorithm, the focus is on simultaneous registration and integration of cumulative point clouds as well as weight analysis of plane parameters.The results indicate that the best weight is 100.
Also, if the dimension of the voxel is much larger than point cloud resolutions, the algorithm does not produce good results.
which combine the ICP and point-to-plane ICP algorithms into a single probabilistic framework.In(Jiayuan L, Ruofei Z, Qingwu H, and Mingyao A, 2016) feature-based scan matching framework is proposed in which distinct feature point is extracted based on an extended 1D SIFT and line feature is extracted based on Split and Merge.Then, these feature is described by distance histogram and the pairs which achieve the best matching score is chosen as potential correspondence.Finally the relative position and orientation of two scans is estimated.(Ryu K, Dantanarayana L, Furukawa T, & Dissanayake G, 2016) Proposed a grid based scan-to-map matching technique which new scan matches with the map.The map is represented as a grid map with multiple Normal Distribution (ND) in each cell.(Soonyong P, and Sung-Kee P, 2016) Proposed a scan matching method for global localization based on metric-topological hybrid map.It use coarse-to-fine registration approach which in coarse registration, by 2D geometric histogram based template matching, candidate site is selected.Then, the SSM method is used for determining the correct site and after that the relative pose is estimated.(Qian C, Hongjuan Z, Jian T, Bijun L, and Hui L, 2019) Proposed a scanto-map algorithm for 2d SLAM in indoor environment.It used a grid based orthogonal feature weighted occupancy likelihood map (OWOLM) as the map representation method which is built from all previous scans.Then, scan-to-map matching is utilized to find the transformation parameter.(Jiang G, Lei Y, Guodong L, Weina X, and Yongsheng O, 2019) Proposed a scan matching method based on Fast Fourier Transform.The scan data within a range of distance changed to various image.FFT is applied to the image to determine the transformation parameters.Finally ICP is applied to determine the match.(Mohamed H, Adel M, Mohamed E, Naser E, and Abu S, 2017) Proposed a method based on a reference key frame which comprised of feature-tofeature and point-to-point approaches.The algorithm depends on the ICP during the lack of linear feature and switches back to the RKF once linear feature are detected.(Peter B, and Wolfgang S., 2003) Normal Distribution Transform algorithm subdivided 2d workspace into cell with constant size.Then, normal distribution for each cell is computed.After that by building NDT two scans will align together and source points will map to the reference coordinate frame.Finally Newton's optimization algorithm is applied to the normal value of each mapped point and best parameter will be find.In (Lu J, Wanjia W, Zhejun F, Shuyue B, and Congling G, 2018), point cloud was down sampled by voxelization then Gaussian mixture model was established to compute the value of negative likelihood function.Finally, EM algorithm was used to solve the rotation and translation parameters.In(Jaromir K, Pavel K, Michal P, and Petr M, 2019)

Figure 3 -
Figure 3-Correspondence points which are in same voxel (1st scan is red and 2nd scan is blue)

Figure 7 .Figure 8 -
Figure 7.Comparison of output of the proposed (white) and ICP algorithms (red)

Table 1 -
Average distance between source and registered cloud for different parameters