Visual Simultaneous Localization and Mapping ( VSLAM ) methods applied to indoor 3 D topographical and radiological mapping in real-time

New developments in the field of robotics and computer vision enable to merge sensors to allow fast real-time localization of radiological measurements in the space/volume with near real-time radioactive sources identification and characterization. These capabilities lead nuclear investigations to a more efficient way for operators’ dosimetry evaluation, intervention scenarios and risks mitigation and simulations, such as accidents in unknown potentially contaminated areas or during dismantling operations. In this communication, we will present our current developments of an instrument that combines these methods and parameters for specific applications in the field of nuclear investigations.


Introduction
Nuclear back-end activities such as decontamination and dismantling lead stakeholders to develop new methods in order to decrease operators dose rate integration and increase the efficiency of waste management.One of the current fields of investigations concerns exploration of potentially contaminated premises.These explorations are preliminary to any kind of operation; they must be precise, exhaustive and reliable, especially concerning radioactivity localization in volume.
Furthermore, after Fukushima nuclear accident, and due to lack of efficient indoor investigations solutions, operators were conducted to find new methods of investigations in order to evaluate the dispersion of radionuclides in destroyed zones, especially for outdoor areas, using Global Positioning Systems (GPS) and Geographical Information Systems (GIS) as described in [1].In both cases, nuclear dismantling and accidents situations, the first aim is to explore unknown potentially contaminated areas and premises so as to locate radioactive sources.Previous methods needed Geographical Information systems and Global positioning systems or placement of markers inside the building before localization of measurements, but plans and maps are often out-dated or unavailable.
Since the end of 2000's years, new emergent technologies in the field of video games and robotics enable to consider fast computations due to new embedded GPU and CPU architectures.Since the Microsoft Kinect™ has been released in 2010, a lot of developers "hacked" the 3D camera system in order to use 3D video streams in many fields of use such as robotics, motion capture or 3D imaging processing algorithms development.During the few following years, Light and low power consuming 3D cameras enabled to consider new 3D reconstruction of environment methods such as Simultaneous Localization And Mapping (SLAM) based on visual odometry [2].This paper will present new progresses in merging SLAM and nuclear measurement in motion methods in order to detect, locate, and evaluate the activity of radioactive sources in 3D.
These new methods enable to reconstruct indoor areas and eventually out-door areas in real-time and 3D and also reconstruct 3D radioactive sources in volume.
In 2013, AREVA D&S started an R&D program for developing new investigations technics based on autonomous sensing robotics and localization apparatus in order to provide new efficient exploration and characterization methods of contaminated premises and areas.This work corresponds to MANUELA TM system developments by Areva D&S.Part of this work is protected by a patent (number WO2015024694) [3].

General method
The presented method is based on two completely different technics.The first one, which is called SLAM, is well known in the field of robotics, it constitutes a specific branch of computer perception R&D.The second one, as described in [4], concerns radioactive sources localization and activity quantification from in-situ measurements and data acquisitions.The usual method for these acquisition are time consuming for operators and, in consequence, integrated dose of workers during these investigation could be decreased.
Chen et al. [5] described such a mapping system based on merging RGBD-camera with radioactive sensors.The presented system automatically detects radioactive sources by estimating their intensities during acquisition with a deported computer.Our aim was to build a complete autonomous system for being totally independent of any external structures, including GPS.
A great problem for an autonomous apparatus (such as robot) is to locate itself in unknown environments, in order to compute appropriate motions and trajectories in volume.A simple formulation of this problem is that the apparatus must know its position in a map in order to estimate its trajectory.Using sensors, the system will build a map and compute its next position (translation and orientation in volume) at each acquisition step.
In order to compute SLAM inherent calculation in autonomous and light device development context, hardware specifications investigations are particularly important, due to required software performances.

Hardware
The presented radiological mapping system is embedded and designed for real-time investigations inside contaminated areas or premises.The whole system is enclosed and autonomous, and needs no external marker or network for being active.However, the operator's real-time intervention requires real-time reconstruction and visualization, which is very performance-consuming.

Sensors
The system software input uses different sensors: -3D camera based on active stereoscopy.As shown in figure 1, this camera's output consists in two different kinds of frame, a normal colour pixels image, and a depth map.The depth map is based on active stereoscopy technique and provides each colour pixel distance to sensor.-Nuclear measurements sensors including a dose rate meter and a micro CZT spectrometer (Figure 2) Figure 2: Outcoming data from nuclear measurements sensors

Computing unit
The 3D reconstruction and nuclear measurements are performed fully embedded, in real-time, due to operator interactions and acquisition time optimization in contaminated environment.Furthermore, the computing hardware must be fan-less in order to avoid nuclear contamination.So as to satisfy these constraints, the embedded CPU must be enough powerful for supporting parallel processing.

Simultaneous localization And Mapping
SLAM concept (simultaneous localization and mapping) can be performed by merging different kind of sensors, such as Inertial Measurement Units (IMU), accelerometers, sonars, lidars, cameras for example.In our method, only 3D cameras are used.We propose to merge two different kinds of algorithms so as to reconstruct the environment in 3D, compute the trajectory with 6 degrees of freedom (translation, pitch, roll, and yaw) in volume, and merge measurements with the device's poses.
Two problems appear during that kind of acquisition.First of all, slight error during the odometry computation causes a non-regular drift of the trajectory.The second problem concerns the memory management of acquisitions in real time.Indeed, 3D video gross data can quickly cost a considerable amount of active memory during the acquisition.Then, implementing a circular buffer is necessary for increasing the scanning volume up to hundreds of cube meters.
In order to develop our measurement method, we modified the RtabMap software [6-7-8] provided by IntroLab (Sherbrooke).By this way, we are able to use visual odometry with 3D cameras in order to reconstruct the environment and compute the device trajectory at 25Hz.
Pose-graph visual Slam is based on the principle that each acquisition step is a combination of constraints links between observations.These constraints are established using features detection and extractions of each processed image.This kind of SLAM problem is represented with graphs of constraints.Each observation of the robot creates node, new links and constraints.This method allows fast node recognition and including loop & closure based-on optimization methods.

Visual Odometry
The goal of visual odometry is to detect the relative motion between two poses of the camera, and then to back-project the 3D and RGB streams in a computing reconstruction volume.This problem can be exposed as equation 1.This equation describes the transformation of each pixel of the camera to a 3D point, depending on intrinsic and extrinsic camera parameters (Equation 1).Visual odometry is processed on a real-time RGB data stream to detect the motions of the device in volume and get the colour for reconstruction.Simultaneously, the corresponding depth stream is used for calculating the rotation/translation matrix between two successive frames.The corresponding pixel in the depth map is also extracted.
Depending on the pine-hole model, features are then back-projected in volume.
As described in figure 3, unique correspondences are researched between two sets of consecutive images.If enough unique correspondences are detected, then, odometry is processed.A RANdom SAmple Consensus (RANSAC) algorithm calculates the best spatial transformation between input images (rotation and translation).

Loop and closure
Errors during the pose matrix computation cause a non-regular drift of the trajectory.Graph-Slam and constrained optimization methods based on loop-andclosure correct this drift when a previously scanned zone is reached by adding constraints to previous acquired constraint graph as described in figure 4 [6].

Nuclear measurement Management and location
Measurements in motion related work [9] by F. Panza, describes a system used within motions in two dimensions with collimated measurements probes.In this case, using leads collimator could be possible, but our case concerns a handheld system measuring in a near 4pi sphere, and moving with six degrees or freedom.
All the data (nuclear measurement and positioning, 3D geographical and trajectory reconstruction) are performed in real-time while the device can have different kinds of status: moving or motionless.All the nuclear measurements are considered isotropic.
Each set of measurement (integrated or not) is attached to the graph-slam geographical constraints structure.This allows performing trajectory optimizations and measurement positioning optimization simultaneously.
In order to satisfy the "real-time" constraint, a user interface displays every current measurement and process step in real-time in order to provide all pertinent and essential information to the operator (figure 5).

Continuous measurement
Dose rate measurements are processed during the 3D reconstruction with a lower frequency (around 2Hz) than video processing (around 20-25Hz).In order to manage the nuclear measurement positioning, we had to find a compromise between positioning uncertainty, which depends on the counting time and counting uncertainty that depends on the inverse counting time.So, first and foremost, dose rate measurement is positioned at half path distance during integration (Figure 6).Gamma spectrometry measurements are processed with an even lower frequency (around 0.3-0.5Hz)than the dose rate measurements.Consequently, the uncertainty on spectrum positioning is more important, compared to dose rate positioning.So as to compensate this error, dose rate values will help to distribute weighted spectrums for the acquired one (Figure 7).

Integrating measurement
In some case, very precise measurements are required to build a representative map of the environment.The fast pose calculation method we use allows considering the device as a 3D accelerometer with a higher frequency than nuclear measurements.While the 3D video stream is being acquired, the acceleration of the device is estimated and if the device is motionless, measurements can be integrated at the current pose (Figure 8) Figure 8: radioactive measurements positioning

Near real-time post-processing, sources localization
At the end of acquisition, radioactive source localization computation methods are available with a set of algorithms that provide interpolations and backprojections of measured radioactive data in volume.The algorithms are optimized for providing results in a few seconds, even if uncertainties could be reduced by more accurate methods.

Measurements 3D interpolation
For interpolating measurements in 3D, we use a simple deterministic Inverse Distance Weighting (IDW) method, which is accurate enough considering the usual radioprotection operating accuracy.Furthermore, this fast computed method allows operators to consider the operating room state of contamination very quickly with this embedded method.The used IDW method is described within equation 2 and 3. with: ‫ܞ‬ሺ‫ܠ‬ሻ: interpolated value at x ‫ܟ‬ ܑ : weight of the measurement point i ۲ ‫ܠ‪ǡ‬ܠ‬ ܑ ‫ܘ‬ : distance between current interpolated point and measurement point i. ‫:ܖ‬ number of measured points 2.5.2Dose rate back projection Back projection method is also deterministic and uses the 3D reconstruction to compute radiation emission zones in volume.This method is described within equations 4 and 5.
ሺሻ ൌ ሺሺ ሺሻሻ Each point of the 3D point cloud (x in eq.4) is considered as a possible radioactive source, then, emerging mean fluency or dose rate at the 3D reconstruction point (B(x) in eq.4) is computed for every measured point (X in eq.4).Then, variance distribution of the back projected value enables to evaluate the possibility of radioactive source presence in volume at the back projected

Topographical study
Topographical measurements can be performed as soon as the acquisition is terminated.This function gives instant information on the situation of premises.

Offline post-processing
The device output data can be processed in back office with a set of tools for estimating accurate gammaemitting sources localization and quantification in volume.It also provides tools for estimating effects of a dismantling or decommissioning operation on dose rate distribution and allows the user to estimate the exposure of operators during interventions.
Next subparagraphs will present these different tools such as radioactive sources quantification, operators' avatars, and topographic studies.

2.6.1Topographical measurements
The dedicated post-processing software provides two kinds of topographical study tools, according to figure 9: a global grid containing the scanned volume for global intervention prevision and a drag and drop tool for specific structure measurements (volumes, length, and thickness).

3D Nuclear Measurements interpolation
Nuclear measurement interpolation characterization tool (Figure 10) is based on IDW, and uses the same principle than the near real time post-processing interpolation method, however, slights modifications of scales allows user to refine the computation and then locate low emitting sources.Moreover, spectrometry can be exploited by interpolating user's defined region of interest of the spectrum.This enables specific studies concerning radionuclides diffusion in the investigated area, such as 137 Cs or 60 Co containers localization for example.

3D Back-projection and avatar dose integration simulation
Figure 11: Radioactive measurements 3D back-projection Back-projection algorithm also benefits of an improved interface in order to locate accurately radionuclides in volume using spectrometry (Figure 11).
Avatars of operators can be used for estimating previsions of their exposure before operations.This dose rate integration estimation is performed by extrapolating dose rates from measurements points.

Sources activities estimations
Radioactive sources activities are estimated with a set of algorithms combining 3D transfer functions calculations and minimization methods (Figure 12).The first step of radioactive sources estimation consists in modelling them according to available data provided by the results of acquisition on a hand, and by operating documents on the other hand (volume, enclosure type, shielding, materials, radionuclides) (Figure 13).Equation [6] describes the transfer function calculation method.
݊ : Number of attenuating volumes on the ray path ሶ ௨ : Dose Rate at Measurement point (considering the Build-up factor) ‫ܣ‬ ௩ : Volume activity of the radioactive source.‫ܥ‬ : Dose Rate -gamma fluency conversion coefficient ‫ݑܤ‬ : Build-up factor for the "i" attenuating volume ߤ : Linear attenuation coefficient of the « i » attenuating volume ݀ : Path length in the « i » attenuating volume.
The numerical integration method for source kernels distribution in the source is a Gauss-Legendre integration based-on method.
• Radioactive sources activities minimization method.The minimization method is based on iterative technique for which each step consists in considering a different combination of radioactive sources.
The equation system resolution method is based on the most important transfer function selection at each step of calculation.
The following figure (figure 14) presents the whole algorithm process for computing sources activities.

Benchmarks
Since June 2016, a set of benchmarks is performed in order to compare the performances of this acquisition system with different systems that can be considered as references.Different parts of the system are compared such as: -3D volumetric reconstruction -3D trajectory reconstruction -Dose Rate measurements -Spectrometry measurements -3D nuclear measurements interpolation -3D radioactive source localization without collimator.

3D volumetric reconstructions
This part of the benchmark has been realized with a 3D scanner and is based on point cloud registration techniques such as Iterative Closest Points (ICP) method.We used "Cloud compare", developed by Telecom ParisTech and EDF R&D department.
The reference point cloud was acquired with a 3D laser scanner from Faro Corporation (Table 1

Benchmark example
As an experimental test, we used a storage room "as it is" (Figure 15) Figure 15: Benchmark situation Room dimensions: 25 square meters, 2.5 meters high.

Results:
Following clouds superposition prepositioning, the ICP registration algorithm is performed in order to compute the best cloud-to-cloud matching.Then, pointto-point distance is computed for the whole reference point cloud.
Figures 16 to 18 show a part of the benchmark procedure.
The first step in figure 16 concerns Faro and MANUELA point clouds superposition in order to prepare the matching process.The second step consists in using ICP matching, and then computing point-to-point distances in the best matching case.In figure 17, the colour scale displays the distribution of point-to-point distances between point clouds, and the chart shows the distribution of these distances in the whole scene.

Uncertainties estimation 4.1 General considerations
Current work is going on estimating uncertainties on each step of the acquisition.
Uncertainties estimation and propagation in such kind of acquisition and processing system is necessary for interpreting the results, estimating the relative probability of presence of radioactive sources, and building sensibility analysis in order to increase the system performance, by detecting the most uncertainty generator step in the process.
The whole process chain depends on four simple entries, the RGB image, the depth map, the dose rate measurements and the spectra measurements.Each one of these entry elements is tainted by systematic and stochastic uncertainties.Moreover, each step of process generates uncertainties and amplifies the input uncertainties.
In this paragraph, we will describe all parts of the acquisition process and present the current results on a new method we use for propagating uncertainties in realtime.We compared this new method to a Monte-Carlo calculation that will be considered as the reference.
In the case of input detectors, we will only consider the model generated or counting (nuclear) measurement uncertainties.
The acquisition and processing devices is decomposable in a few building blocks (sensor or processing technic).They will be considered separately in order to quantify each block uncertainty (Figure 19).Each block will be considered as a linear system such as described in equation [7][8].
ሺǡ ǡ ሻ: Output vector ሺܽ ௫ǡ௫ ǡ ǥ ǡ ܽ ௫ǡ௫ ሻ: Transformation matrix ሺǡ ǡ ሻ: Input vector Each of these blocks generates an intrinsic uncertainty and amplifies inputs errors.Then, the final หȁܱ݀ȁห will represent the global uncertainty on the acquisition and processing chain.

Monte Carlo methods
Monte Carlo methods are efficient for propagating and estimating models uncertainties accurately.The main problem in such method is computing time.This method will be considered as reference for comparison with the new method we developed.Furthermore, Monte-Carlo method will provide accurate results on each acquisition and process steps and will help to estimate how performances will be increased.

Interval arithmetic method
Interval arithmetic methods have been initially developed due to rounding errors of floating values in computing calculation.The principle is to describe a scalar value with an interval.This help to consider not finite floating values as a duo of floating scalars with specific rounding policies.For example, ߨ ‫א‬ ሾ͵ǤͳͶǢ ͵Ǥͳͷሿ.
This description of values will change arithmetic rules.For example, mathematical product becomes: ሾെͷǢ െͶǤͷሿ ‫כ‬ ሾെͲǤͷǢ ʹሿ ൌ ሾെͳͲǢ ʹǤͷሿ With this arithmetic, we developed a method for propagating uncertainties as boundaries of a noisy system.We will compare this method with Monte-Carlo uncertainties estimations method.We choose, as an example case, the 3D camera projection Matrix (pinehole model).

3D Camera calibrations
The 3D camera output data is a duet of frames, a RGB one and a depth map.Each of them can be described as pinholes.Then, pixels 3D back-projections in volume are considered in figure (13) ‫‬ Depth at pixel (u,v) ǡ ‫‬ Coordinates of the considered pixel ୶ ‫‬ Focal length along x ௬ ‫‬ Focal length along y ୶ ǡ ୷ ǣ Lens centering on the optical axis ‫ݔ‬ǡ ‫ݕ‬ǡ ‫ݖ‬ ‫‬ Projected point coordinates in volume.
Values of interest in this models are (x,y,z) coordinates of the projected pixel.These coordinates, depending on features detection, will be processed in the VSLAM algorithm.
So as to provide interpretable results, the pinhole must be calibrated, and the calibration process will give interpretable back-projection uncertainties on (x,y) coordinates.Uncertainty on z coordinate will be estimated in function of the depth, material and radioactivity level.Indeed, depth map is computed with 4.1.1Uncertainty estimation, general description active stereoscopy, which involves infrared coded grids projection and interpretations, which can be sensitive to external interferences.

Uncertainties estimations on camera calibration coefficients
The calibration of the camera consists in minimizing the pine hole projection matrix elements, using a wellknown projection pattern.
The calculation of back-projections errors is performed with a Monte-Carlo random selection of calibration values ܿ ௫ ǡ ܿ ௬ ǡ ݂ ௫ ǡ ݂ ௬ on a hand, and then, these back-projection errors are compared with intervals estimations.Deviations estimations methods are described with equation 14 and 15.

Monte-Carlo estimation:
This method gives exhaustive results on the camera response, but it takes around 2 minutes to be computed for each frame, which is not suitable with real-time computations.
The Figure 20 shows results of Monte-Carlo simulation varying the pinhole projection matrix elements; each matrix element could vary from -10 to +10%.1e+6 random selections have been performed.The resulting error due to the lenses system will not cause a larger deviation than 80 pixels on the photographic sensor.Provided results are less exhaustive than with Monte-Carlo methods, nevertheless, these results are bounding phase space provided by Monte-Carlo method.Furthermore, executing time is around 5ms per frame, which is compliant with real-time analysis and processing.
The figures 23-24-25 show comparisons between uncertainties computed by Monte-Carlo and by intervals.This graphical representation represents the bounding of Monte-Carlo computed values by interval arithmetic method.Interval arithmetic results have been added to Monte-Carlo charts in order to figure both methods results (interval arithmetic results have been encircled to remain visible).
The simulation has been performed considering exactly the same matrix element variations.Indeed, matrix element have been defined as intervals (equation 16)

Spectrum measurements
uncertainties also depend on energy/efficiency calibration quality and acquisition parameters.

SLAM method uncertainties
During SLAM acquisitions, each pose computed by odometry and re-estimated following to loop and closure algorithm is associated to a variance, due to statistical way of calculating spatial transforms which gives a confidence index on each pose computation.Concerning the 3D reconstruction, uncertainty will combine poses computations and 3D projection uncertainties.

Ongoing work
Current work concerns the estimation of each building-block uncertainties generation and amplification, such as depth map variations as a function of the environment conditions, spatial transforms variance due to descriptors variations, applications of back-projection or geostatistical interpolation.
Radioactive sources localization uncertainties generation and propagation will also be estimated in order to provide a complete method for estimating uncertainties and errors in such kind of system that combines acquisition of physical measurements and automatic processing in near real-time.
Also, a full benchmark of the system is performed in order to correlate its results with real-time uncertainties estimations.Each building-block will be compared to well-known references systems.For example, in order to compare 3D reconstructions, we will use a Faro scanner as reference, and concerning trajectory computation, Vicon™ camera system will provide reference of spatial motions of the device.

Conclusions
The new presented method for 3D Indoor / Real-time Topographical and Radiological Mapping (ITRM), with Visual Simultaneous Localization And Mapping (VSLAM) has been developed to perform near real time acquisitions and post processing for radiological characterization of premises.Such method allows optimizations of operational time, personnel dose and waste minimization.One of the major challenges consists in optimizing the mobile experimental apparatus in order to satisfy the desired performances, by generating optimized programming code, selecting high performance computer units and measuring instrument.Such an optimization needs also a careful study of uncertainties introduced by the used instrumentation and developed algorithms.Indeed, physical measurement must be provided with associated uncertainty, in order to estimate its pertinence.Also, this uncertainties study method will finally enable performing a sensibility analysis of the whole system.Consequently, it will help to define the building blocks to improve and increase the system's performances.
Finally, this new approach that we developed and applied for radiological measurements within the nuclear field could be of interest in other domains by adapting the sensors for the required measurements (Chemical, environmental...).This ongoing work is funded by ANRT (Agence Nationale de la Recherche et des Technologies) under a CIFRE contract (number 2013/1542) between AREVA/STMI and CNRS.

Figure 1 :
Figure 1: Outcoming data from 3D sensor (left: depth-map, right: RGB image) depth at pixel (u,v) ǡ ‫‬ coordinates of the considered pixel ୶ ‫‬ focal length along x ௬ ‫‬ focal length along y ୶ ǡ ୷ ǣ lens centering on the optical axis ୟǡୠ ‫‬ element of the rotation matrix ୟ ǣ element of the translation vector ‫ݔ‬ǡ ‫ݕ‬ǡ ‫ݖ‬ ‫‬ projected point coordinates in volume.

Figure 4 :
Figure 4: Loop and closure optimization

Figure 13 :
Figure 13: Radioactive sources scene modeling In order to satisfy the nearest real-time calculation constraint, the transfer function calculation method is deterministic and based on ray-tracing and radioactive kernels point distribution in volume.Potential radioactive sources are designed by the user with the help of localization algorithm.Equation[6] describes the transfer function calculation method.

Figure 17 :
Figure 17: Cloud-to-cloud distance computationThe last graph displays the point-to-point distances distributions as a Gaussian (Figure18shows the chart of figure17with a different binning).

Figure 18 :
Figure 18: Cloud distance diagram General conclusion of 3D point cloud reconstruction Benchmark shows a Gaussian distribution of cloud-tocloud distance.Global cloud-to-cloud mean distance is around 7.3e -2 m (ߪ ൌ10.2e -2 m).

Figure 20 :
Figure 20: Monte-Carlo estimation of pinhole model uncertainties generationThe second diagram (Figure21) shows the error amplification by the projection matrix.In this specific case, it doesn't have physical sense because it would represent the variation of a vertex coordinates as a function of pixel coordinate variation, which has no physical sense.The pinhole model is linear, consequently, variation of output variation norms are distributed along a straight line.

Figure 21 :
Figure 21: Linear system error amplification by Monte-Carlo methods The last diagram (Figure 22) combines errors generated and amplified by the pinhole projection matrix.And this error will be considered as input error by the next processing block of the system.(for each value of ԡ݀‫ܫ‬ԡ, ԡௗ்ԡ ԡ்ԡ ൏ ͲǤͳሻ