STUDY AND DEVELOPMENT OF A LOW COST “OptInertial” 3D SCANNER

The 3d scanner domain normally relies on the presence of many tools and technologies. They are mainly divided between contact and non-contact ones, but at present a new trend is coming up, starting from the articulated arms family, where the traditional mechanical joints, allowing flexible measures on complex objects, have been replaced by handy scanner with laser trackers, or with markers. These new systems have the advantage to get the entire object shape without changing the object position itself, but moving the scanning device without the involvement of long software alignments and post treatment operations. Anyway, the use of laser trackers obliges the operator to maintain the scanner head always visible, while the marker usage is quite critical because the measure precision depends strongly on the marker position. Starting from these considerations, the research project presented in this paper has been focused on the design and development of a flexible 3D scanner, without trackers and markers, that merge low-cost optical technologies together with low-cost inertial sensors. Thanks to this synergy, the developed 3d scanner is able to measure the three-dimensional shape of the object recording its movement and transferring it to a control unit for obtaining a point cloud. The paper presents the main concepts of the 3d scanner design and is concluded by an experimental phase made with the use of a benchmark.


INTRODUCTION
In recent years there has been a progressive development in the domain related to the 3D acquisition. Many fields of application, albeit for different reasons, and purposes saw the introduction of three-dimensional relief: from quality control at the end of the production process to surveying large areas of territory, from the digitization of architectural monuments and even entire buildings to the reconstruction and digitization of small and complex objects. A further contribution to the development of three-dimensional acquisition systems is given by the remarkable possibilities offered by today's three-dimensional visualization and 3D modelling: the widespread that 3D is taking in areas of daily life with the progressive development and diffusion of products such as virtual reality or augmented reality means that the stimulus in the search for new methods of acquiring competitive, low-cost and thus increasingly accessible to many is truly remarkable.
Since the fields of application are numerous, there are many types of 3D scanners based on different technologies that have been developed over the years. There are a variety of technologies to capture and digitize the three-dimensional shape of an object. A well-known and well-established classification divides them into two categories: contact and non-contact 3D scanners. Non-contact 3D scanning systems that do not require a direct contact with the object to be acquired can be further divided into two main categories, active scanners and passive scanners [1].
While the traditional contact systems, which probe the subject through physical contact while this is supported on a floor with a precision surface and which can be constituted by a carriage system with rigid arms, that allows the tip of the scanning to slide along each of the 3 axes of reference, or an articulated arm equipped with high precision sensors for determining the angular position taken from the joints on the end of which is placed the scanning head have almost reached the peak of their development, the hand-held scanners are gradually spreading [2].
These systems offer the great advantage of eliminating the physical support that connects the scanning head to the reference base, ensuring in this way to achieve a further degree of freedom in the scanning process.
However, in order to properly reconstruct the surface acquired and be able to consistently replace the various samples acquired during the same scan session, it is necessary to determine with high precision the position and orientation that the scanning head assumes during the whole scanning process.
This fundamental issue has been addressed in two main different ways considering noncontact or contact hand-held scanners.
For what concerns non-contact hand-held scanners, the position of the scanning head is determined triangulating the positions of known points directly applied on the surface being scanned or placed near the object to be scanned. Even if the advantages of having no more physical constraints due to the presence of a rigid structure to determine the position of the scanning head are obvious, the need to apply the markers implies a further step in the preparation of the scanning session. Furthermore, the quality and the correctness of the position determination of the scanning head is strictly related to the number of markers that are visible in each scanning frame which often implies a great number of the overall markers to be placed or a particular attention for the operator.
On the other hand, contact hand-held scanners, which have not the possibility to scan markers, require some external tracking method to determine the position of the probe. This point is usually solved using laser or optical trackers or a photogrammetric solution using 3 or more cameras providing the complete 6 degrees of freedom of the scanner. The need of constantly maintain a visual link between the hand-held scanner and the tracking station reduces the degree of freedom in the operator's movement during the scanning session.

METHODOLOGY: THE THREE-DIMENSIONAL DATA ACQUISITION
Starting from these considerations the research project presented in this paper focuses on the design and development of a flexible 3D scanner [3], without trackers and markers, by merging optical technologies together with an inertial MARG sensors.
In particular, the procedure for tracing the device's movements consists in an initial step during which the orientation of the device itself is determined by the use of a low-cost MARG board. As for the displacement, the distance (considered as a 2D path) that the device covers sliding along the surface of the object to be scanned is directly measured using an optical sensor. This sensor should be maintained constantly in contact in order to guarantee the same reference system; during the whole acquisition session the object to be scanned must be maintained fixed in its initial position.
Thereafter, assuming that the XY plane of the optical sensor is constantly tangent to the surface to be scanned ( Fig. 1), it is possible to rotate the two-dimensional information acquired by the optical sensor in accordance with the estimated orientation provided by the MARG (in the form of the direct cosine matrix ) sensors and combine with each other the two information to reconstruct the three-dimensional path followed by the device during the scanning session. Many studies and research activities have dealt with the use of MARG (Magnetic Angular Rate Gravity) sensors for determining the orientation of an object. The interest in this particular set of sensors is related to the fact that the information provided by accelerometer, gyroscope and magnetometer are very suitable to be processed with sensor fusion algorithms [4] [5]. In fact, although these three sensors measure physical quantities very different from each other, the information provided by them can be used to derive the pose of an object: gyroscopes measure the angular velocities from which the angular displacement that the sensor performs starting from an initial position is obtained through a simple integration, while accelerometer and magnetometer measure in the sampling instants the direction, the intensity and the direction of actual physical quantities (respectively magnetic and the gravitational field) which can be used to derive the linear transformation (i.e. the rotation) that binds them to an initial reference position (Fig. 2).
There are two different sensors fusion tools to process MARG data: Complementary Filter and Kalman Filter. The basic idea behind complementary filtering is that orientation drift errors resulting from gyro output errors can be bounded by aiding the gyros with additional sensors, the information from which allows correcting the gyro orientation solution [6].
On the other hand, the tradition of using Kalman filters (KFs) and their extended (EKF) version for nonlinear models is well established when complementary filters are developed to blend the outputs from gyros and the sensors of an aiding system. Furthermore, the usage of KF combined with quaternion representation is widely discussed and used [7,8,9], thus the decision to design an attitude estimation algorithm based on quaternion representation and an EKF as a sensor fusion tool.

Figure 2: Three-Dimensional data acquisition strategy, main steps
The common formulation of the Discrete Extended Kalman Filter involves the definition of two elements. The first one is the nonlinear dynamic model of the system that employs the following formulation: , where is the noise on the system that is assumed to have a Normal Gaussian distribution described using the system noise covariance matrix . The second element is a non linear measurement model with the following formulation: .
Where is the noise on the system that is assumed to have a Normal Gaussian distribution described using the measurement noise covariance matrix .
In particular, if the noise on the system model and on the measurement model have they individual components uncorrelated, and are diagonal matrices with constant elements on their diagonal.
The system noise covariance matrix is used to compute the a priori error covariance matrix , which is commonly defined as .
While the system noise covariance matrix is used to compute the a Kalman Gain , which is usually defined as , where the matrix represents the linearized matrix of the measurement model. The Kalman Gain is used to determine the a posteriori error covariance matrix , which can be computed as follow: .
The advantages of using a quaternion representation are mainly related to the deletion of trigonometric functions in the attitude determination algorithm that results both on a reduction of the overall complexity of the orientation estimation process and in the possibility of avoid singularities due to phenomena as the so called "gimbal lock". In fact, quaternions are an excellent mean to represent rotation. Besides, a widely established strategy propose to use quaternion estimation algorithms or iterated algorithm for non-linear problem to solve the attitude determination and combine the information provided by accelerometer and magnetometer [10,11]: this solution implies as a great advantage that the data provided by the accelerometer and the magnetometer can directly used in the EKF without the need of a further linearization step which involves the design of a more complex EKF [12,13].

Quaternions
A quaternion is a four-dimensional vector defined in a four-dimensional space with base in which i, j, k, are iper-complex numbers that satisfy the anti-commutative law. Quaternions are formed by a scalar real part and a vectorial part . Quaternions are an excellent mean for representing rotation. An arbitrary rotation of a frame (i.e. the mobile Body Reference frame) relative to (i.e the fixed Navigation Reference frame) can be achieved through a rotation of an angle around a unit vector (i.e. a versor which defines the axis of rotation and the direction of rotation in accordance to the right hand rule) defined in the frame .Considering the fact that the quaternion defined this way contains as elements the Euler parameters we claim to the definition of the unitary quaternion as .
One of the most interesting properties of the quaternions is the well-known quaternion product (denoted here with ) determined by the Hamilton rule as follow , where q and p are both quaternions and that has the useful matrix representation , where represents the antisymmetric matrix in the matrix-vector notation for the vector cross product and .
The last fundamental equation that gives a further justification to the use of quaternions is derived from quaternion differentiation, according to which the derivative of a quaternion can be simply obtained as , and, by using the matrix notation for the Hamilton product already discussed, it is finally obtained .

Gauss-Newton Method for Quaternion Convergence
Following the approach proposed in [11] the Gauss-Newton algorithm is used to determine a quaternion defining the orientation. The Gauss-Newton method is a really efficient for solving Non-Linear Least Squared Estimation problems, i.e. a problem that, given a function , finds an argument of F that minimizes the value of the so-called objective (or cost) function. This method can be simply implemented through the use of the well-known recursive formula , where represents the vector containing the variables of the non-linear system to be found and f defines the non-linear systems itself, while is the Jacobian of the non-linear system. The quaternion estimation problem can be thus defined as the non-linear problem given by the difference between the measurements done in the body frame (i.e. the reference frame of the sensors) and the known real values of the gravity and the magnetic field rotated in the body frame, that is , where • represents the estimated quaternion that minimizes the solution of the problem; • the matrix contains the two rotation matrices that carries the real physical values from the navigation frame to the body frame ; • is a vector that contains the real quantities to be measured expressed in in the navigation frame .
In particular, the reference quantities are assumed to be equal to the first measurements acquired, so that the initial attitude is set as the starting reference for the device and for the whole process of attitude determination; • is the vector that contains the measurements in the body frame in which the component of acceleration and magnetic field are respectively normalized. Now, considering that a rotation matrix can be usefully represented in the quaternion components as , it is possible to compute the Jacobian according to its definition .
In the end the quaternion estimation at each step can be easily computed according to the iteration formula previously given, so that . Figure 3 underlines the fast convergence of the Gauss-Newton method: the algorithm, applied on simulated data that should represent a rotation of 35° around the versor defined as v = [1, 1, 1] (the quaternion to be found is ), converges in 3-4 iterations to the correct quaternion estimation starting from an initial estimate of . The fast convergence guarantees the absence of computational lag and the stability of the algorithm also using raw and noisy data. Furthermore, the efficiency and the accuracy of the overall method can me further improved by mean of the calibration-compensation routine proposed in [14,15], thanks to with the mean value of the cost function F(x) for a standard acquisition session remain under the threshold of .

Designing the EKF
Because of the use of the Gauss-Newton method for quaternion estimation and convergence the design of the EKF is significantly simplified. In fact, this approach allows to have the quaternion components directly available as measurements. Thus, the measurement model can does not contain any nonlinearity and can be easily describe stacking the angular velocities provided by the gyroscope and the quaternion components provided by the Gauss-Newton algorithm .
Consequently, as underlined before, the problem relate to nonlinearities is restricted only to the plant model, according to which the evolution of the attitude is described by the quaternions' update from the gyro measurements is , where represents the State Transition Matrix defined as .
In order to linearize the plant model it is sufficient to use a series approximation tailored to the first order, so that we have .
In order to reduce the influence of both the eventual distortions in the magnetometer's measurements and the accelerations superimposed on the gravity force sensed by the accelerometer, a mechanism of adaptation of the measurements noise covariance matrix is implemented. The proposed approach is similar to the one discussed by Sabatini in [6]. In particular we proposed to consider the reliability of the information provided by the accelerometer and the magnetometer using the objective function provided by the Gauss-Newton algorithm for quaternion convergence as discriminant for understanding the consistency of these samples. In fact, as already discussed, this "tool" directly describes the convergence towards a unique solution for the given problem of finding an orientation, or better a quaternion, starting from two measurements of physical quantities (i.e. the gravity and the magnetic vectors): in case of mismatching of the two samples because of disturbances, the objective function F(x) underlines the problem assuming high values. In particular, due to the fact that the same sources of error did not affect contemporarily both the accelerometer and the magnetometer, the cost function is typically able to highlight them separately. This means that • shocks or strong vibrations applied to the device that corrupt the accelerometer's measurements are not sensed by the magnetometer that continues to sense the same magnetic field ⇒ the information become inconsistent and the objective function assumes high values; • external disturbances or random spikes measured by the magnetometer do not affect the gravity force, thus the samples provided by the accelerometer ⇒ the information mismatch and the objective function assumes high values.
In the aforesaid cases the measurement noise covariance is set to extremely high values so as to force the filter to rely only on the gyroscope information for obtaining the state vector update: .
Rather than dealing with acceleration and magnetic disturbances as time-varying components of the bias vectors, the proposed validation tests aim at precluding the measurements from influencing the filter behaviour, when detected disturbances are characterized by high magnitude and long, but even short, durations. The remaining part of the covariance matrix that refers to angular velocity measurement in not included in the covariance matrix adaptation and maintains a fixed value. In particular, the values included in the diagonal of the measurements covariance matrix have been obtained performing a statistical analysis maintaining both the device at rest and considering random movements, thus the system has been modelled as a random walk phenomena.
The following figure shows the results of the designed EKF that merges the data provided by the quaternion estimation from rate measurements and the ones obtained from the Gauss-Newton algorithm to give an accurate and reliable orientation information. Figure 4 shows the angles solved using the different methods for an acquisition session during which the device has been moved randomly and then has been replaced to the initial position. The direct confrontation underlines the reliability of the designed filter.
The red lines represent the angles obtained using only the gyroscope measurements, the green ones the angles solved by the Gauss-Newton algorithm and the blue the ones resulted from the EKF.
It is evident the usual drift error introduced by the gyroscope, while the attitude information derived from accelerometer and magnetometer measurements demonstrated to be unbiased but noisy.
Hopefully, the attitude determination obtained using the designed EKF results at the same time unbiased (in fact after restoring the device to the initial position the error found is less than 0,2°) and only slightly noisy. Thus, the filter fusion "tool" is able to keep the favourable attributes of each source of information, rejecting at the same time the negative features.

TESTING AND RESULTS
The measurements unit used is a low-cost that embeds a 3 axes gyroscope ITG-3200 produced by Inven Sense which guarantee a sensitivity of 14,375 LSB/(°/s) and low total RMS noise (about 0.35 RMS), a 3 axes accelerometer ADXL345 produced by Analog Devices with a good resolution of 3,9 mg/LSB and an adequate bandwidth of 3.2 KHz, a digital compass (magnetometer) HMC 5883L with a good sensitivity of 0.73 mGauss/LSB which enables 1° to 2° compass heading accuracy. The codes for processing the acquired data are run on a general purposes computer. For what concerns the optical sensors, it is a ADNS-2620 Optical Mouse Sensor produced by Avago Technologies that guarantees a maximum frame speed of 3000 fps and a resolution of 400 cpi (counting per inches).
The codes used to process the data provided by the sensors are implemented in Matlab and are run in real-time on a general purpose computer. The information are read from the sensors by mean of a microcontroller; then the data are sent from the device to the computing station using a Bluetooth interface which guarantee a transmission rate of 56700 Baud. Figure 5 shows the designed prototype and highlights the different constituting parts, while figure 6 focuses on the sensors mounted on the MARG board and the Optical Sensor.  As previously introduced, in order to obtain a correct 3D tracking, the data acquired from the optical sensor are multiplied at each step by the direct cosine matrix where represents in all respect a rotation matrix which can be directly obtained from the quaternion components as where represent the quaternion components, as defined in section 2.1, while the subscripts and B of the variable x, y, and z stand respectively for the navigation and the body reference frame as defined in section 2.2.
The third coordinate in the optical sensor measurements is set to zero, in the sense that the plane of the optical sensor is assumed to be always tangent to the shape to be acquired: as underlined in the last equation, the third dimension is obtained by combining the orientation estimation (MARG) and displacement measurements (Optical Sensor). In order be able to perform a direct confrontation between the reconstructed shape and the original one, the body used for the test sessions has been realized in wood not as a free form, but as a body with specified geometry. In particular, its relevant face has been designed with a smooth profile ( fig. 9) allowing the evaluation of the scanning system resolution for variations along the Z coordinate. Once the measurements on a 3D space have been acquired, they result as a point cloud that defines the trajectory followed by the device.  In particular, figure 9 shows the "strategy" used to scan the test surface: instead of trying to cover the whole surface with samples, it is preferred to follow some main "guidelines" that can completely define the shape of the body on which the device is moved: in this sense, they look like splines that envelope the scanned object. Then, the data are fitted to derive a 3D shape using a simple surface matching routine ( figure 11).
As previously underlined, the aim of these tests is to evaluate the degree of accuracy that the integration between the orientation estimation (MARG) and the displacement measurements (Optical Sensors) can achieve in the detection of variations along its Z direction, while the accuracy along the X and Y directions are not considered. The reason of this choice is that the system has been design for low-cost reverse engineering applications where the absolute position measurements are not important, but the shape correspondence, that is given by Z coordinate relative values. In other words, X and Y coordinate values provide only a spatial information of the point clouds location related to a fixed reference system, that could be easily managed during the post-processing phase.  Thus, in order to estimate the quality of the measurements along the Z direction, the projection of the acquired samples on the XZ plane and the lateral profile of the reference geometry described in figure 8 have been directly compared: in particular, the error along the Z coordinate has been computed as the difference between the ideal profile and the XZ projection of the sample . The two profiles have been aligned along the Z axis in order to minimize the mean of the error . Figure 11 shows both the graphical result of the profile confrontation and the spatial distribution of error along the X axis (upper and lower boundaries and the mean error in between are depicted). In order to characterize the error distribution also from a statistical point of view, the occurrences of each value of has been firstly determined and then plotted as a function of (figure 12); the mean value is mm while the computed standard deviation equals mm.

DISCUSSION AND CONCLUSIONS
In this paper the design of an innovative low cost hand-held optical and inertial scanner is presented and discussed in all its constituting features. After an initial survey on the state of the art and the reasons for the choice of MARG board and the importance of obtaining an accurate orientation determination, quaternions are introduced as the mean used in the algorithms to deal with rotations of the device. Then, the use of Gauss-Newton method for quaternion estimation and convergence is discussed and its efficiency is demonstrated with experimental results on real data. Then an EKF is designed as a sensor fusion tool to provide a consistent and robust outcome in the attitude determination. Results underline that the orientation estimation is obtained with and high degree of accuracy and that the information related to the attitude of the device can be obtained to rotate the displacement data sampled by the optical sensor.
In the end, an experimentation phase has been developed for understanding the real behaviour of the device from a spatial and a statistical point of view. The presence of a maximum Z error of 4-5 mm could be addressed to the system measurement logic related with the estimated orientation provided by MARG. As explained in the methodology section, the acquisition strategy employed by the device requires a continuous contact with the surface. From the starting point till the last one the system could not rely on other reference points, so as longer is the scanning path as higher will be the error . So considering that our test has been developed on a length of more than 3m, taking into consideration the whole path on the benchmark, that value is reasonable for the lowcost sensors employed. Moreover an error mean value along Z axis minor than 2 mm could be an interesting result for many reverse engineering applications.
Furthermore, due to the fact that among the sensors there is still the presence of an accelerometer, future developments should include in the 3D tracking routine also the displacements data provided by a two times integration of the acceleration samples and provide a further source on which sensor fusion techniques similar to the one proposed for a attitude determination can be applied in order to increase the overall reliability of the device.
Future work should focus on the redesign of the hardware structure of the device in order to reduce the physical constraints due to the size of the probe and increase the easiness and the comfort for the operator.