Strengthening robot perception in information-degraded environments for aerial inspection and maintenance

DEPARTAMENTO DE INGENIER´ IA DE SISTEMAS Y AUTOM´ ATICA ESCUELA SUPERIOR DE INGENIER´ IA UNIVERSIDAD DE SEVILLA Strengthening Robot Perception in Information-Degraded Environments for Aerial Inspection and Maintenance por Julio Jos´e L´opez Paneque Graduado en Ingenier´ıa Electr´onica, Rob´otica y Mecatr´onica M´aster en Ingenier´ıa Electr´onica, Rob´otica y Autom´atica PROPUESTA DE TESIS DOCTORAL PARA LA OBTENCI ´ ON DEL T´ ITULO DE DOCTOR POR LA UNIVERSIDAD DE SEVILLA SEVILLA, 2024 Directores Dr.Ing. Jose Ramiro Mart´ınez de Dios, Catedr´atico Dr.Ing. An´ıbal Ollero Baturone, Catedr´atico ii UNIVERSIDAD DE SEVILLA Memoria para optar al grado de Doctor por la Universidad de Sevilla Autor: Julio Jos´e L´opez Paneque T´ıtulo: Strengthening Robot Perception in Information-Degraded Environments for Aerial Inspection and Maintenance Departamento: Departamento de Ingenier´ıa de Sistemas y Autom´atica V◦B◦Director: Jose Ramiro Mart´ınez de Dios V◦B◦Director: An´ıbal Ollero Baturone El autor: Julio Jos´e L´opez Paneque iii iv A B´arbara, por haberme acompa˜nado en este viaje y todos los que nos quedan por vivir. A mis padres, por vivir con ilusi´on cada uno de mis pasos en el mundo acad´emico. v vi Agradecimientos En primer lugar, quiero dar las gracias a Ramiro Mart´ınez. Hace ya casi diez a˜nos que me anim´o a empezar mi viaje por el mundo acad´emico, que finalmente ha dado lugar a esta Tesis Doctoral. Me gustar´ıa tambi´en agradecerle esta Tesis a An´ıbal Ollero, que me incluy´o en el GRVC y me permiti´o desde el inicio implicarme en incontables proyectos de investigaci´on, cada cual m´as apasionante. A mis compa˜neros del GRVC, que con los a˜nos han dejado de caber en una lista tan corta como esta. Este tiempo no habr´ıa sido lo mismo sin vosotros. A V´ıctor, Mar´ıa, Fernando, Ra´ul, Ga˜n´an, Juan, Pedro, Rafa, Rebeca, Carlos, ´ Alvaro, Javier, V´ıctor Vega, Arturo, Jose Manuel, Juanma, Juan Pablo, Augusto, Iv´an, ´ Ambar, Fran, Alejandro, Manu, y a todos los dem´as que hab´eis estado ah´ı. Al profesor Davide Scaramuzza y los investigadores del grupo RPG, por acogerme como uno m´as durante mi tiempo all´ı. Adem´as de haber aprendido mucho de vosotros, me llevo los buenos momentos que compartimos. A mis amigos, y sobre todo a los que tuvieron m´as paciencia en los momentos dif´ıciles. En especial, a Gonzalo, que ha sido un apoyo constante, y que ha sido un referente para contrastarme y buscar ser mejor ingeniero y cient´ıfico. A mi familia, en toda su extensi´on. En especial, a mis padres, que siempre me mostraron el mundo de la investigaci´on y me animaron a perseguir mis metas independientemente de cu´ales fueran. Por ´ultimo, a B´arbara, mi prometida, por estar siempre ah´ı y ser un faro en los momentos m´as oscuros. Un camino tan largo se hace llevadero cuando es contigo. Muchas gracias, Julio Jos´e L´opez Paneque vii Esta Tesis Doctoral ha sido financiada por el programa estatal de Formaci´on de Profesorado Universitario (FPU17/0632). El trabajo realizado en la Tesis se encuentra tambi´en financiado por el programa de Investigaci´on e Innovaci´on H2020 de la Uni´on Europea bajo las referencias No. 871479 (AERIAL-CORE), 644271 (AEROARMS), y 810321 (AEROTWIN), por el programa ”Advanced Grants” del European Research Council con la referencia No. 788247 (GRIFFIN), y por los proyectos nacionales de I+D+i ROBMIND (Ref. PDC2021-121524-I00), HAERA (Ref. PID2020-119027RB- I00) y ARM-Extend (Ref. PI2017-89790-R). Parte del trabajo tambi´en se encuentra en el contexto del projecto EuRoC-ARCOW financiado por la Comisi´on Europea con referencia No. 608849. This work was supported by the Spanish Ministerio de Universidades FPU Program (FPU17/0632). The work has also been supported by the European Union’s Horizon 2020 Research and Innovation Programme under grant agreements No. 871479 (AERIAL-CORE), 644271 (AEROARMS), and 810321 (AEROTWIN), by the European Research Council Advanced Grant No. 788247 (GRIFFIN), and by the Spanish Projects ROBMIND (Ref. PDC2021-121524-I00), HAERA (Ref. PID2020-119027RB- I00) and ARM-Extend (Ref. PI2017-89790-R) from the Programa Estatal de I+D+i. Part of the work has also been performed within the context of EuRoC-ARCOW project funded by the European Commission under contract 608849. viii Resumen Los robots a´ereos han emergido como una tecnolog´ıa clave en la Inspecci´on y Mantenimiento de infraestructuras industriales y civiles. Estos escenarios son entornos muy complejos que presentan situaciones donde no hay caracter´ısticas geom´etricas o visuales destacables, donde el escenario es muy similar o incluso sim´etrico en diferentes direcciones, donde la recepci´on GNSS es pobre o nula, y donde los sensores pueden inducir informaci´on err´onea que induzca a error a los algoritmos de percepci´on. Esta Tesis Doctoral aborda el dise˜no, desarrollo y validaci´on experimental de diferentes m´etodos para reforzar la percepci´on de los robots a´ereos en estos escenarios. Los m´etodos desarrollados permiten la operaci´on totalmente aut´onoma de los robots a´ereos sin depender de ninguna infraestructura externa, asumiendo que todos los sensores, m´etodos de percepci´on y dispositivos de procesamiento se encuentran a bordo del robot, y no requieren ninguna alteraci´on del entorno. A lo largo de la Tesis se han seguido tres enfoques principales para el dise˜no de los diferentes m´etodos. Primero, el uso de esquemas multisensor que aprovechan las sinergias entre sensores para mitigar las debilidades de cada dispositivo individual. Segundo, la selecci´on del modelo m´as adecuado para representar y estimar el estado del robot en cada escenario. Tercero, el uso de t´ecnicas de percepci´on activa que mejoren la calidad de las entradas de percepci´on. Los enfoques propuestos aportan soluciones complementarias, que han dado lugar al desarrollo de cinco m´etodos contenidos en esta tesis, dise˜nados en funci´on de las necesidades de cada aplicaci´on estudiada. En primer lugar, esta Tesis Doctoral presenta un m´etodo de localizaci´on robusta basado en un enfoque multisensor y multihip´otesis, que permite la navegaci´on segura y aut´onoma de robots a´ereos sin GNSS en escenarios complejos de inspecci´on industrial ix xvi Contents 2.3.2 The Prediction Stage . . . . . . . . . . . . . . . . . . . . . . . 33 2.3.3 The Update Stage . . . . . . . . . . . . . . . . . . . . . . . . . 36 2.4 Experimental Validation . . . . . . . . . . . . . . . . . . . . . . . . . 39 2.4.1 Performance Evaluation . . . . . . . . . . . . . . . . . . . . . 40 2.4.2 AblationStudy .......................... 45 2.4.3 Live Demonstrations in Real Scenarios . . . . . . . . . . . . . 46 2.5 Conclusions ................................ 47 3 Probabilistic Meshes for Long-range LiDAR Mapping 49 3.1 Introduction................................ 49 3.2 RelatedWork ............................... 51 3.3 SystemDescription............................ 53 3.3.1 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . 53 3.3.2 ProposedSystem ......................... 57 3.4 ProposedMethods ............................ 59 3.4.1 Mesh Generation . . . . . . . . . . . . . . . . . . . . . . . . . 60 3.4.2 Motion Estimation . . . . . . . . . . . . . . . . . . . . . . . . 63 3.4.3 MapUpdate............................ 66 3.5 MethodsValidation............................ 74 3.5.1 Plane Initialization & Point-Plane Contributions . . . . . . . . 75 3.5.2 Similarity Computation . . . . . . . . . . . . . . . . . . . . . 77 3.5.3 Plane Cluster Aggregation . . . . . . . . . . . . . . . . . . . . 79 3.5.4 Plane Quadrics for Intersections . . . . . . . . . . . . . . . . . 82 3.6 SystemValidation............................. 83 3.6.1 Geometrical Validation and Ablation Study . . . . . . . . . . 84 3.6.2 Experimental Validation . . . . . . . . . . . . . . . . . . . . . 88 3.7 Conclusions ................................ 92 4 Reactive LiDAR-based Mapping for Powerline Inspection 95 4.1 Introduction................................ 95 4.2 RelatedWork ............................... 97 4.3 GeneralDescription............................ 99 Contents xvii 4.3.1 Pointcloud Segmentation . . . . . . . . . . . . . . . . . . . . . 101 4.3.2 Multisensor Mapping . . . . . . . . . . . . . . . . . . . . . . . 108 4.3.3 Mapping-Aware Commands . . . . . . . . . . . . . . . . . . . 113 4.4 Experimental Validation . . . . . . . . . . . . . . . . . . . . . . . . . 117 4.4.1 Pointcloud Segmentation . . . . . . . . . . . . . . . . . . . . . 118 4.4.2 Multisensor Mapping . . . . . . . . . . . . . . . . . . . . . . . 123 4.4.3 Mapping-Aware Commands . . . . . . . . . . . . . . . . . . . 124 4.4.4 Live Demonstrations in Real Scenarios . . . . . . . . . . . . . 127 4.5 Conclusions ................................ 128 5 Perception-aware Navigation and Perching Approach on Powerlines 131 5.1 Introduction................................ 131 5.2 RelatedWork ............................... 133 5.3 SystemDescription............................ 134 5.3.1 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . 134 5.3.2 Mathematical Modeling . . . . . . . . . . . . . . . . . . . . . 135 5.3.3 Perching Trajectory Generation . . . . . . . . . . . . . . . . . 141 5.4 Experimental Validation . . . . . . . . . . . . . . . . . . . . . . . . . 145 5.4.1 Inspection Experiments . . . . . . . . . . . . . . . . . . . . . . 146 5.4.2 Perching Experiments . . . . . . . . . . . . . . . . . . . . . . 148 5.5 Conclusions ................................ 152 6 Localization of Missing Tools with Aerial Robots in Industrial Manufacturing 155 6.1 Introduction................................ 155 6.2 RelatedWork ............................... 157 6.3 Problem and System Description . . . . . . . . . . . . . . . . . . . . 158 6.3.1 Problem and Requirements . . . . . . . . . . . . . . . . . . . . 158 6.4 MissingToolSearch............................ 163 6.4.1 Outlier Filtering . . . . . . . . . . . . . . . . . . . . . . . . . 164 6.4.2 PF-based Initialization . . . . . . . . . . . . . . . . . . . . . . 167 6.4.3 EKF-Based Refinement . . . . . . . . . . . . . . . . . . . . . . 169 xviii Contents 6.5 Validation in the Factory . . . . . . . . . . . . . . . . . . . . . . . . . 169 6.5.1 Factory Adaptation and Risk Mitigation . . . . . . . . . . . . 169 6.5.2 Experimental Validation in Fully Working Conditions . . . . . 171 6.6 Conclusions ................................ 173 7 Conclusions and Future Works 175 7.1 Conclusions ................................ 175 7.2 FutureWork................................ 179 A Mathematical Derivations 183 A.1 Derivation of n1⊞ηn1Tn2⊞ηn2.................. 183 A.2 Point-Plane EKF cluster merge . . . . . . . . . . . . . . . . . . . . . 187 A.3 Alternative Quadrics for Intersections . . . . . . . . . . . . . . . . . . 188 B Datasets and Standalone Software 191 B.1 Datasets.................................. 191 B.1.1 Multisensor Localization in Industrial GNSS Denied Environments 191 B.1.2 Long-range Powerline Inspection . . . . . . . . . . . . . . . . . 192 B.1.3 The GRIFFIN Perception Dataset for Ornithopter Perception 193 B.2 Standalone Software . . . . . . . . . . . . . . . . . . . . . . . . . . . 195 B.2.1 Perception-Aware Powerline Perching Trajectory Generation for Multirotors ............................ 195 References 197 List of Figures 1.1 Live demonstration of the proposed multi-sensor localization method in the Wilhelmshavener Raffineriegesellschaft oil refinery (Germany). 8 1.2 Probabilistic mesh-based map computed during a long-range powerline inspection flight. Non-planar structures are kept managed by a pointbasedmap. ................................ 9 1.3 left) One electrical tower mapped with our reactive mapping scheme. right) Same tower mapped by flying with the quadrotor at a constant speed. ................................... 10 1.4 A quadrotor performing a perception-aware perching maneuver from (S) to (E), maximizing the visibility of the line during the whole trajectory. 10 1.5 Aerial robot performing autonomous missing tool search in the Centro Bah´ıa de C´adiz (CBC) Airbus D&S factory (Spain). . . . . . . . . . 11 2.1 Diagram of the proposed multi-sensor multi-hypothesis filter for robust GNSS-denied localization. . . . . . . . . . . . . . . . . . . . . . . . . 30 2.2 Depth error uncertainty σ resulting from triangularization and rasterization of a Velodyne 3D LiDAR pointcloud. The depth uncertainty is bounded, being highest at the location of the LiDAR points. . . . . . 32 2.3 Example of an estimated altimeter measurement hi,k and the true altimeter measurement hk......................... 38 2.4 DARIUS aerial robot used in the experiments. ©IEEE 2024 . . . . . 39 2.5 Pictures of the experiment scenarios: Top) ETSI, Bottom-left) Karting and Bottom-right) Bridge.©IEEE2024 ................ 41 xix xx List of Figures 2.6 Multisensor maps of the ETSI and Karting scenarios. LiDAR features are in orange, the UWB (Ultra-Wide Band) radio-range beacons in the scenario are in green. Camera texture features can be seen in the map. ©IEEE2024 ............................... 42 2.7 Results in one bridge inspection experiment. The figure shows the geometrical map component (in green), the localization obtained by our method (in blue) and the ground-truth localization (in magenta). Only the geometrical component of the map is shown for better visualization. ©IEEE2024 ............................... 43 2.8 3D localization errors in the experiment in Figure 2.7 obtained by the proposed method, LOAM, and ORB-SLAM2. ©IEEE 2024 . . . . . 44 2.9 Real industrial scenarios where the proposed method was used for autonomous aerial navigation: Left) 3 scenarios in the Wilhelmshavener Raffineriegesellschaft oil refinery at Wilhelmshaven (Germany); Right) Lafarge-Holcim cement factory in Jerez de la Frontera (C´adiz, Spain). 47 3.1 Example of the performance of different map representations in a geometrically poor scenario. The prior of the robot estimated pose differs from its true position (marked with ”+”). Given this scenario, it is only possible to correct the estimated robot pose in the vertical axis. Top) Using a point-based map, the state estimation scheme is not able to correct the error in the mean of the robot estimated pose. Also, the normals of the local map representations can be different to the surfaces they intend to represent, erroneously adding information in the horizontal axis, underestimating the final state covariance. Middle) Using a voxel-based map, local representations are estimated within the boundaries of the voxel, potentially introducing similar effects than with a point-based map. Bottom) Using a mesh-based map, the local correspondences tend to better resemble the intended surfaces, allowing to correct the vertical error without adding erroneous information in thehorizontalaxis. ........................... 55 List of Figures xxi 3.2 Proposed components for integrating a probabilistic-mesh-based map in a generic framework. The contributed modules are shown in green. Colored arrows denote the state of the system (pose and map). Black arrows denote the normal execution cycle for a new scan. . . . . . . . 58 3.3 Top) Changes in the reference frame ( W→V→V′ ) to align a direction vector nof any plane mwith the z axis, and later diagonalize its variance (recall that Σ n = Σm1:2,1:2 ). Notice that while the S2 basis remains the same for n V and n ′ V , the rotations along the z axis allow to diagonalize its covariance Σ n . Bottom) MonteCarlo estimations of Q ′ 1,1 and Q ′ 3,3 given nin a virtual reference frame V′and with diagonal covariance Σ′. 72 3.4 Top) Accuracy of the proposed plane similarity metric (based on 2nd- order propagation) against a MonteCarlo-based ground truth. Bottom) Accuracy of an alternative metric using 1st-order propagation. . . . . 78 3.5 Computation times for merging plane clusters as a function of the number of points represented in the merged cluster. . . . . . . . . . 80 3.6 Geodesic distance of different approaches for estimating the quadric matrix versus the ground-truth (MonteCarlo) solution of (3.23) . Notice that the scale is logarithmic. The figure was obtained with arbitrary values d = 10 . 5 m and Σ d = 1 . 5 m2 . Similar results are obtained with othervalues................................. 83 3.7 Test scenarios with planar maps that introduce LiDAR information in robot state estimation in different axes (see main text). The Li- DAR position and FoV are shown in red. The LiDAR initial and best-obtainable covariance are shown in blue and green (rotation covariance in the last case is shown per-axis). Best-obtainable covariance is computed by performing state estimation with the ideal (error-free) mapandmeasurements. ......................... 85 3.8 Ablation study of the different map uncertainty representations in the studied scenarios using the proposed mesh-based mapping scheme. . 88 3.9 LR-M aerial robot used in the experiments (left) and scenarios Alcala (right-top), ATLAS (right-center), and Burguillos (right-bottom). . . 89 xxii List of Figures 3.10 Trajectories of all the compared methods in Burguillos, augmented with tightly-coupled GPS fusion. . . . . . . . . . . . . . . . . . . . . . . . 92 3.11 Obtained maps with our method and evolution of the map size overtime with the compared methods in Alcala-F1 (top) and ATLAS-F2 (bottom). Map size is computed as follows: for Voxel-map, size of all the probabilistic planes and the probabilistic points in all octrees; for FAST-LIO2, size of all the points in the kd-tree; for PUMA, size of all the vertices and faces in the mesh; for our method, size of all the vertices and faces, all the probabilistic planes associated to the faces, all the cluster masks, and all the non-planar points (also seen separated in red). For all methods, only the size of the upper triangular part of the covariance matrices is considered. . . . . . . . . . . . . . . . . . 93 4.1 Scheme of the proposed autonomous mapping-aware navigation system.©IEEE2024............................. 101 4.2 Scheme of the proposed Pointcloud segmentation module. ©IEEE 2024103 4.3 Reflectivity histogram of the points cloud shown in Figure 4.4. ©IEEE 2024 .................................... 104 4.4 Top) Visual image collected in one powerline inspection flight. Bottom) Resulting points after Reflectivity Analysis. Points in MS are shown in green. Points in OS are shown in blue. Miss-classified points are in red. ©IEEE2024............................. 106 4.5 Results of the Metallic points Processing in the MS in Figure 4.4. A clusters of Tower points is shown in red. Three clusters of Powerline points are shown in green. ©IEEE2024 ................ 107 4.6 Results of the of Organic points Processing in the OS in Figure 4.4. Vegetation clusters are shown in yellow, whereas the Soil cluster is shown in blue. ©IEEE2024....................... 108 List of Figures xxiii 4.7 Left) Original result of the chosen mapping software in a power-line inspection experiment. The robot is coming back after inspecting a powerline section, and the drift in the map can be clearly noticeable in the power-lines and the tower. Right) Resulting map when including map uncertainty and GNSS using the proposed methodology. The drift is completely canceled out. . . . . . . . . . . . . . . . . . . . . . . . . 113 4.8 Proposed pipeline for mapping-aware trajectory adaptation. ©IEEE 2024 .................................... 114 4.9 VOI of a powerline tower during a mapping experiment. The bounding box of the VOI is colored in magenta. The grey areas are the unknown spaces. The occupied spaces appear in white. Empty spaces are not shown for clarity. ©IEEE2024 ..................... 115 4.10 LR-M powerline inspection aerial robot developed by the GRVC Robotics Lab. ©IEEE2024 ............................ 118 4.11 Validation scenarios of the proposed method: a) ETSI, b) ATLAS, c) Burguillos, and d) Alcala.©IEEE2024................. 119 4.12 Result of the proposed method in experiments conducted in ATLAS (left), Burguillos (center), and Alcala (right). Top) visual images. Center) corresponding LiDAR scan pointclouds. Bottom) segmented pointclouds representing Powerline (green color), Tower (red), Soil (blue), and Vegetation (yellow). ©IEEE 2024 . . . . . . . . . . . . . 120 4.13 Left) Resulting segmentation of contiguous scans obtained by the proposed method. Right) Same scans classified by PointNet++.©IEEE 2024 .................................... 121 4.14 Segmented map using the proposed method obtain in one ATLAS experiment. ©IEEE2024 ........................ 122 4.15 Validation of the Multisensor Mapping method in one experiment in the ETSI scenario: left) overlay of the low-level map (in red color) and the Leica map (in yellow and blue colors), assumed ground truth; right) map error histogram. The average error is 3mm and the RMSE error is3.8cm. ................................. 124 xxiv List of Figures 4.16 Performance of the proposed scheme along 23 s in one inspection experiment conducted in the Alcala scenario: a) longitudinal speed of the quadrotor along its trajectory and stop commands triggered along the experiment; and b)-d) lowest mapped and textured percentages of the VOI s corresponding to Powerline,Tower, and Vegetation categories. If there are no VOI s of a specific object, a 100% value is used. ©IEEE 2024 .................................... 126 4.17 left) One electrical tower mapped with our reactive navigation system. right) Same tower mapped by flying with the quadrotor at a constant speed. ................................... 127 4.18 3D map obtained in the powerline inspection experiment of Figure 4.16.128 4.19 Semantic map obtained with the proposed system in a 2 . 4 km powerline inspection flight, at the ATLAS Flight Test Center (Villacarrillo, Jaen, Spain). Powerlines are shown in yellow, electric towers in red, vegetation in green, and soil in blue. . . . . . . . . . . . . . . . . . . . . . . . . 128 5.1 Visualization of proposed mathematical modeling for (5.9) and (5.10) . ©IEEE2024 ............................... 140 5.2 Developed procedure for perching+recovery trajectory integration. ©IEEE2024 ............................... 144 5.3 Top) Position of the robot at different instants during a powerline inspection mission. Bottom-left) Waypoint-based vs actual robot trajectory performed during the mission. Waypoints are denoted with different numbers. The front of the robot is colored in green. The powerlines appear in red. Bottom-right) Collision avoidance constraint (5.14j) , line reprojection error (5.9) , and line cheirality (5.14h) and segment visibility (5.14i) constraints during the mission. The constraint violation zone is marked in red. ©IEEE 2024 . . . . . . . . . . . . . 147 List of Figures xxv 5.4 Top) Quadrotor following a perception-aware powerline perching trajectory from point S to point E . Bottom-left) Comparison of two perching maneuvers between S and E , with and without perception awareness. The black quadrotor follows the perception-aware maneuver (orange). The gray quadrotor follows the normal maneuver (blue). The orientation of the robot’s camera at each instant is shown in green. Bottom-right) Line reprojection error and segment visibility constraint during both maneuvers. The constraint violation zone is marked in red. Obstacle avoidance and line cheirality are always satisfied for both maneuvers, and thus are not shown. ©IEEE 2024 . . . . . . . . . . . 150 5.5 Tracked perching maneuver where the robot finishes upside-down. The numbers denote the evolution of the trajectory. ©IEEE 2024 . . . . 151 6.1 Aerial robot performing autonomous missing tool search in the Centro Bah´ıa de C´adiz (CBC) Airbus D&S factory (Spain). The upper part of the factory is less crowded, providing the necessary space for introducing autonomous aerial robots. . . . . . . . . . . . . . . . . . . . . . . . . 159 6.2 Functional architecture of the proposed system. . . . . . . . . . . . . 162 6.3 Layout of the factory with the selected navigation waypoints (in orange) and the followed trajectory for performing and missing tool search (in redandblue)................................ 163 6.4 Robot moving straight away from the tool (from x r,k1 to x r,k2 ). The red circles represent the 3 σr,k uncertainty of the robot’s positions (orange dots) and the 3 σt,k uncertainty of the radio range measurements (depicted as an uncertainty around the real tool position x true tool at the blue asterisk): Left) case of minimum difference between inlier measurements; and Right) case of maximum difference between inlier measurements. 165 6.5 Histogram of UWB measurement errors in one experiment in the factory before (in blue) and after (in red) outlier filtering. . . . . . . . . . . . 166 6Introduction real applications of aerial robots generally provide a highly constrained flight plan to be followed. However, recent works in aerial robotics are pursuing solutions for maximizing the visibility of some elements in the scenario while following the provided flight plan [60 – 63], or for minimizing the probability of observing information-degraded areas [64]. Others directly enforce the controller to maintain specific features in the robot’s Field of View (FoV) [65, 66]. This problem, called ”perception-awareness” among other names is still being explored by the community, and poses a promising solution that perfectly complements the other two approaches. The majority of existing works make use of Recursive Bayesian Filters (RBFs) for state estimation. Recursive Bayesian estimation has deep mathematical foundations based on the Bayes Theorem and assumes all measurements and models are subject to noise. In the Robotics community, the term ”Bayesian Filtering” is generally used for the estimation of the current state based on current and past measurements, and the term ”Bayesian Smoothing” is used when previous states are also estimated [67]. Bayesian Smoothing is mainly used in visual SLAM, where drifts in the state estimation are common and corrected with loop closure techniques [23, 25, 68, 69], which are now also being explored in LiDAR SLAM [36,70]. This PhD Thesis pursues perception techniques that are robust and deliver a consistent accuracy overtime, making Bayesian Filtering techniques more adequate for the envisioned use cases. The research performed in this PhD Thesis includes developments using Extended Kalman Filters (EKFs), Multi-Hypothesis Bayesian Filters, and on-Manifold iterated error-state Kalman Filters (IKFoM ). Unscented Kalman Filters (UKFs) and Extended Information Filters (EIFs) have also been explored but where disregarded in favor of the other solutions. Last, but not least, every method developed in this PhD Thesis has been implemented (typically in C++ under Robot Operation System) and has been validated in field experiments that have been conducted first in the GRVC Robotics Lab indoor and outdoor testbeds and finally in real I&M environments such as: a power grid system in ATLAS (Air Trafic Laboratory for Advanced unmanned Systems) (Villacarrillo Ja´en, Spain); the Centro Bah´ıa de C´adiz (CBC) Airbus D&S factory (C´adiz, Spain); 1.3 Contributions 7 the Lafarge-Holcim cement factory in Jerez de la Frontera (C´adiz, Spain); or the Wilhelmshavener Raffineriegesellschaft oil refinery at Wilhelmshaven (Germany). 1.3 Contributions All of the proposed contributions in this PhD Thesis are focused towards specific applications of aerial robots in I&M tasks, and provide successful results by combining one or multiple of the described approaches in Section 1.1 (see Table 1.1 later for a relation between the contributions and approaches). These contributions are enumerated next: Contribution 1: Development of a robust multisensor localization scheme for complex GNSS-denied environments. When navigating in complex industrial environments, aerial robots must fly close to the structures to perform I&M tasks. In these situations, the GNSS signal is generally degraded and can be completely lost during the flights. Keeping a high-quality and robust localization without relying on GNSS is thus vital to perform autonomous flights in these environments. This contribution describes a multi-sensor multi-hypothesis method for robust and accurate GNSS-denied localization. First, camera and LIDAR features are seamlessly integrated in the same statistical framework, benefiting from their synergies and providing robustness in scenarios with low or varying densities of LIDAR and visual features. Second, a multi-hypothesis approach is adopted to cope with visual and structural symmetries present in the scenario. The method operates in real time using feature and hypothesis filtering and efficient hypothesis refinement in a multi-core implementation. This contribution has been experimented for closed-loop aerial robot navigation in different urban and industrial scenarios, showing advantages over well-known singlesensor techniques. The contribution was published in work [71] and has been validated in real industrial scenarios such as a petrol refinery, and in close-range bridge inspection. Contribution 2: Introduction of probabilistic meshes in a LiDAR-based mapping scheme for enhancing robustness in long-range applications. This contribution addresses the problem of generating high-quality maps and trajectory estimates in long-range aerial inspection of civil infrastructures such as powerline 8Introduction Figure 1.1: Live demonstration of the proposed multi-sensor localization method in the Wilhelmshavener Raffineriegesellschaft oil refinery (Germany). systems. During these flights, the robots can traverse many areas with low geometrical information, such as crop fields with no vegetation. In these situations, the chosen map representation and the consideration of map uncertainty can highly impact the robustness of the robot motion estimation, thus affecting the quality of the resulting map. This contribution introduces the use of probabilistic meshes in LiDAR-based mapping, combining a probabilistic on-manifold formulation of planar objects that naturally reflects the measurement uncertainty in the map, and the use of meshes, which consistently represent planar surfaces and enable the use of decimation techniques to reduce the influence of the measurement noise in the map, while strongly reducing the map size. This contribution has been submitted for publication as work [72], and has been experimentally evaluated in powerline inspection flights in different scenarios. Contribution 3: Development of a reactive mapping scheme for powerline inspection based on semantic segmentation of LiDAR data. When performing inspection tasks over a powerline system, aerial robots generally follow a predefined path that covers the extent of the infrastructure, gathering data from LiDARs, cameras, and other sensors. Afterwards, the data is analyzed offline, generally days after performing the flight. This procedure is very inefficient and in many cases requires repeating the flights if during the offline analysis the collected data is found to be of insufficient quality for the inspection, which can happen e.g. if using cost-effective sensors while performing fast inspection flights. This contribution presents an onboard 1.3 Contributions 9 Figure 1.2: Probabilistic mesh-based map computed during a long-range powerline inspection flight. Non-planar structures are kept managed by a point-based map. reactive online powerline inspection system, that performs semantic segmentation to identify different elements in the scenario and adapts the velocity of the aerial platform to increase the resolution and precision of the mapping until the required quality is fulfilled. The robustness of the onboard perception is also improved by integrating map uncertainty in the state estimation scheme. The proposed system classifies each LiDAR scan into sets of Powerlines,Towers,Vegetation, and Soil point clusters, and uses them to ensure that objects from each category are mapped up to a certain resolution. Moreover, distances between powerlines and between vegetation and powerlines and electrical towers can be instantly computed using the output of the semantic classification, which is a necessary requirement for performing continuous maintenance of powerlines. This contribution was published in works [73,74], where the proposed system was implemented onboard and aerial robot and validated in powerline inspection experiments conducted in environments with different types conditions and vegetation. Contribution 4: Development of a method for perception-aware navigation and perching approach in powerlines. While inspecting powerline systems, maximizing the visibility of the inspected structure is vital to obtain high-accuracy perception results. Moreover, to enable continuous, robust inspection of powerlines without human intervention, the robots must be able to perch on the lines to recharge 10 Introduction Figure 1.3: left) One electrical tower mapped with our reactive mapping scheme. right) Same tower mapped by flying with the quadrotor at a constant speed. their batteries. This contribution presents a set of efficient mathematical representations of powerlines that allow to include their visibility as part of the trajectory tracking controller of the aerial robot, effectively keeping the lines focused in the Field of View (FoV) of the robot’s camera during the flights, as well as avoiding collisions with them. The proposed formulation is then integrated on a novel perching trajectory generation framework that computes perception-aware, collision-free, and dynamically-feasible maneuvers to guide the robot to the desired final state. This contribution was published in work [18], where the methods were validated onboard an agile quadrotor conducting powerline inspection and various perching maneuvers with final pitch values of up to 180°. Figure 1.4: A quadrotor performing a perception-aware perching maneuver from (S) to (E), maximizing the visibility of the line during the whole trajectory. Contribution 5: Design of a UAV-based tool localization scheme and validation in an AIRBUS D&S factory at full working conditions. This 1.3 Contributions 11 contribution covers the design, development, and validation of a fully autonomous aerial robotic system for manufacturing industries. The system was developed to perform light object logistics as well as missing tool search, with the latter being the focus of this contribution. Missing tools occur occasionally, originating delays and reducing productivity. Moreover, they are particularly harmful in aeronautic industries since they may cause Foreign Object Damage (FOD) events. To help find missing tools, both the aerial robot and the tools are equipped with an Ultra-Wide Band (UWB) emitter that can send requests and measure the range between tags in its radio coverage. UWB measurements are highly prone to outliers due to the metallic structures present in manufacturing plants. This contribution presents a combination of an outlier filtering scheme with a Bayesian localization method to robustly localize tools even under metallic structures. The presented robotic system, functionalities, and use case were published in work [75] and were extensively validated with Technology Readiness Level 7 (TRL-7) in the Centro Bah´ıa de C´adiz (CBC) Airbus D&S factory in fully working conditions. Figure 1.5: Aerial robot performing autonomous missing tool search in the Centro Bah´ıa de C´adiz (CBC) Airbus D&S factory (Spain). Additionally to these contributions, different datasets and algorithms have been publicly released during the development of this PhD Thesis. All the published resources are linked to peer-reviewed journal articles, or to the reviewed results of EU-funded projects. 12 Introduction Table 1.1 summarizes the main contributions of this PhD Thesis and provides context on how they cover the different followed approaches, as well as the Technology Readiness Level (TRL) that was achieved in the validation of each developed solution. Table 1.1: Classification of the proposed contributions Contrib. Approach Application Main sensor Fusion with State estimation scheme TRL other sensors C1 AP1-AP2 Industrial and LiDAR & Yes (IMU, Multi-hypothesis filter TRL-6 civil I&M camera altimeter, UWB) w/ selective refinement C2 AP1-AP2 Powerline LiDAR Yes (IMU, GNSS) IKFoM w/ point & TRL-5 inspection probabilistic mesh map C3 AP1-AP2-AP3 Powerline LiDAR Yes (IMU, IKFoM considering TRL-6 inspection camera, GNSS) map uncertainty C4 AP3 Powerline Camera Not relevant Not relevant TRL-4navigation & perching C5 AP2 Missing tools UWB Not relevant UWB outlier filter. TRL-7search in Sequential PF-EKF for industries managing multimodality 1.3.1 Publications The research presented along this PhD Thesis has been published in 4 journals, with JCR Impact Factors 5.2 (2 contributions), 12.1 (1 contribution), and 7.8 (1 contribution, submitted). The research has also been published in 7 international conferences and workshops, and among them, 2 contributed works have been presented at IEEE International Conference on Robotics and Automation (ICRA) and 1 presented at IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). The PhD candidate developed research in the FP7-ICT EUROC-ARCOW project.The team behind ARCOW (Aerial Robot Co-Worker in Plant Servicing) was one of the two finalist (out of > 35 teams) in the ”Aerial Servicing” track in the European Robotics Challenges (EUROC) project, and received the ”Best Drone-Based Solution” award in the 1st EU Drone Awards (2017). This PhD Thesis also produced publicly available resources in the form of 3 datasets and 4 software repositories, made available to the community. 1.3 Contributions 13 Journal Articles • Paneque, J. L., Mart´ınez-de Dios, J. R., & Ollero, A, Leveraging Probabilistic Meshes for Robust LiDAR Mapping, IEEE Transactions on Robotics, 2024 (Submitted). Links: Dataset,Code [72]. • Paneque, J. L., Mart´ınez-de Dios, J. R., Ollero, A., Hanover, D., Sun, S., Romero, A., & Scaramuzza, D., Perception-Aware Perching on Powerlines With Multirotors, IEEE Robotics and Automation Letters, vol. 7, no. 2, pp. 3077-3084, April 2022. ©IEEE 2024. Links: Code,Video [18]. • Perez-Grau, F. J., Martinez-de Dios, J. R., Paneque, J. L., Acevedo, J. J., Torres-Gonz´alez, A., Viguria, A., & Ollero, A, Introducing autonomous aerial robots in industrial manufacturing, Journal of Manufacturing Systems, Volume 60, pp. 312-324, July 2021. Links: Video [75]. • Rodr´ıguez-G´omez, J. P., Tapia, R., Paneque, J. L., Grau, P., G´omez Eguiluz, A., Mart´ınez-de Dios, J. R., & Ollero, A., The GRIFFIN Perception Dataset: Bridging the Gap Between Flapping-Wing Flight and Robotic Perception, IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 1066-1073, April 2021. ©IEEE 2024. Links: Dataset,Code,Video [76]. Awards • ”Best Drone-Based Solution” of the 1st EU Drone Awards (2017) to ”ARCOW. Aerial Robot Co-Worker in Plant Servicing”. International Conferences & Workshops • Paneque, J. L., Mart´ınez-de Dios, J. R., & Ollero, A, Multi-Sensor 6-DoF Localization For Aerial Robots In Complex GNSS-Denied Environments, 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Macau, China, 2019, pp. 1978-1984. ©IEEE 2024. Links: Dataset [71] 14 Introduction • Valseca, V., Paneque, J. L., Mart´ınez-de Dios, J. R., & Ollero, A., Real-time LiDAR-based Semantic Classification for Powerline Inspection, 2022 International Conference on Unmanned Aircraft Systems (ICUAS), Dubrovnik, Croatia, 2022, pp. 478-486. ©IEEE 2024. Links: Video [73]. • Paneque, J. L., Valseca, V., Mart´ınez-de Dios, J. R., & Ollero, A., Autonomous Reactive LiDAR-based Mapping for Powerline Inspection, 2022 International Conference on Unmanned Aircraft Systems (ICUAS), Dubrovnik, Croatia, 2022, pp. 962-971. ©IEEE 2024. Links: Video [74]. • Paneque, J. L. & Tapia, R., Hands-on tutorial on Heterogeneous Robot Cooperation, 2021 DroneDays International Workshop, Zagreb, Croatia, 2021. Links: Code [77]. • Paneque, J. L., Tutorial: SLAM in Complex Large-Scale GNSS-denied Environments, 2020 Breaking the Surface International Workshop, Zagreb, Croatia, 2020 [78]. • Egu´ıluz, A. G., Rodr´ıguez-G´omez, J. P., Paneque, J. L., Grau, P., M. de Dios, J. R. & Ollero, A., Towards flapping wing robot visual perception: Opportunities and challenges, 2019 Workshop on Research, Education and Development of Unmanned Aerial Systems (RED UAS), Cranfield, UK, 2019, pp. 335-343. [79]. • Egu´ıluz, A. G., Paneque, J. L., M. de Dios, J. R. & Ollero, A., Online Detection and Tracking of Pipes During UAV Flight in Industrial Environments, Robot 2019: Fourth Iberian Robotics Conference, Seville, Spain, 2019, pp. 28-39. [80]. Book chapters • Caballero, A., Paneque, J. L., Martinez-de-Dios, J. R., Maza, I., & Ollero, A. (2024). Multi-UAV Systems for Inspection of Industrial and Public Infrastructures. Infrastructure Robotics: Methodologies, Robotic Systems and Applications, Wiley-IEEE press, pp. 285-304, 2023. [4] 1.3 Contributions 15 • Polvillo, M., Paneque, J. L., & Martinez-de Dios, J. R. Absolute Localization and Mapping of Aerial Manipulators. Aerial Robotic Manipulation: Research, Development and Applications. Springer Tracts in Advanced Robotics, vol 129, pp. 229-238, 2019. [81] • Mart´ınez-de Dios, J. R., P´erez-Grau, F. J., Torres-Gonz´alez, A., Acevedo, J., Paneque, J. L., Viguria, A., & Ollero, A. (2020). GRVC-CATEC: Aerial robot coworker in plant servicing (ARCOW). Bringing Innovative Robotic Technologies from Research Labs to Industrial End-users: The Experience of the European Robotics Challenges, pp. 211-242, 2020. [82] Datasets • Multi-sensor mapping and localization. A dataset for testing multi-sensor mapping and localization schemes in different autonomous flights in an mock-up environment with a pipe, resembling an industrial plant. The recorded dataset contains measurements from a 3D Velodyne LIDAR, a visual sensor, an altimeter, an IMU, and Ultra-Wide-Band Time-of-Flight sensors, as well as RTK GNSS for ground truth, all mounted in an aerial vehicle. The dataset was used in the validation of work [71]. Dataset • Long-range powerline inspection. A dataset for performing long-range LiDAR- based mapping in powerline inspection flights. The dataset contains measurements from a 3D LiVOX Horizon LiDAR scanner, an IMU, and a GNSS receiver. The flights are recorded in three different scenarios and the dataset is used in the validation of work [72], and partially in work [73]. Dataset • The GRIFFIN Perception Dataset. A dataset for flapping-wing robot perception in different scenarios, with several onboard sensors such as an event camera, a regular camera, and two IMUs. The dataset also includes scenes with artificial landmarks, and scenes with people and objects, with bounding box annotations for training methosd. The dataset was published in work [76]. Dataset 22 Introduction 1.7 Notation Summary In PhD Thesis, we follow standard conventions for mathematical notation in the robotics community. For convenience, we now provide a small summary of the commonly used expressions during the document (see also Table 1.2). Specific notation that only appears briefly in some chapters (such as the use of different subscripts and superscripts) is not covered here, and is always detailed when used. We denote scalar magnitudes with lowercase s , vectors with bold lowercase v, and matrices with bold uppercase M. Orthonormal reference frames are defined with uppercase F . Translations between two coordinate frames are represented as vectors t F2 F1∈R3 , such that a vector v F2∈R3 is expressed in F1 as: v F1 =t F2 F1 +v F2 . For rotations, we use rotation matrices R ∈SO(3) , which can be expressed in different frames as R F1 =R F2 F1 R F2 . The rotation of a vector v F2∈R3 to frame F1 is thus: v F1 =R F2 F1 v F2 . Additionally to rotation matrices, rotations are also expressed with unit quaternions q ∈ SO(3), which can be expressed in different frames as q F1 =q F2 F1⊙ q F2 , where ⊙ denotes the Hamilton product between two quaternions. We define the rotation of a vector v F2∈R3 by a quaternion with the following abuse of notation: v F1 =q F2 F1⊙ v F2: =q F2 F1⊙0 vF2 . Finally, homogeneous transformation matrices between two coordinate frames T F2 F1 = hRF2 F1tF2 F1 01×31i∈SE(3) are used to express combined rotations and translations. We define the transformation of a vector v F2∈R3 by a homogeneous matrix with the following abuse of notation: vF1=TF2 F1vF2:=TF2 F1vF2 1 Manifold Formulation Following general conventions [83], given a manifold M we denote ⊞ : M×Rn7→ M as the operator that transforms x ∈ M to y ∈ M given a perturbation u ∈Rn defined in the homeomorphic space of M at x:x ⊞ u=y. Conversely, the operator ⊟ : M×M 7→ Rn can be used to obtain: y ⊟ x=u. Note that operators ⊞ and ⊟ depend on the evaluation at the manifold M , but the symbols ⊞ and ⊟ are used instead of the formally correct ⊞Mand ⊟Mfor cleanliness. Different manifolds can be easily compounded to create higher-order manifolds [83]. Particularly, the S2×R compound manifold is used in this PhD Thesis to represent 1.7 Notation Summary 23 Table 1.2: Summary of the used notation during this Thesis Rnreal coordinate n-space S2unit sphere manifold SO(3) 3D special orthogonal group SE(3) 3D special Euclidean group P(4) space of 4-dimensional symmetric positive-definite matrices tF2 F1∈R3translation of a magnitude from F2to F1 RF2 F1∈SO(3) ⊂R3×3rotation of a magnitude from F2to F1 qF2 F1∈SO(3) ⊂R4rotation of a magnitude from F2to F1 TF2 F1∈SE(3) ⊂R4×4transformation matrix from F2to F1 PF1point set in F1axes pF1∈Rnpoint in F1axes mF1∈S2×R⊂R4plane in F1axes ηp,ηm∈Rn,R3noise vector tangent to p,m Σp,Σm∈Rn×n,R3×3covariance of ηp,ηm Q∈P(4) ⊂R4×4quadric matrix planes, such that for a plane with mean m= { n , d} , S2 is the unit sphere manifold, used for its normal vector n, and R is used for its distance to the origin d . The noise affecting the plane is modeled as a perturbation ηm = {ηn, ηd}∈N (0 , Σ m ), in the R3 locally homeomorphic space to m∈S2×R. Using [83, Lemma 1]: m⊞ηm="n⊞ηn d⊞ηd#="n⊞ηn d+ηd#,Σm="Σn,Σnd ΣT nd,Σd#(1.1) 24 Introduction Chapter 2 Robust Multisensor Localization for Complex GNSS-denied Environments 2.1 Introduction When navigating in industrial environments, aerial robots must fly close to the structures to perform Inspection and Maintenance (I&M) tasks. In these situations, the GNSS signal typically used for localizing and controlling the robots is generally degraded, and can be completely lost during an important fraction of the flights. Keeping a high-quality and robust localization without relying on GNSS is thus a key requirement to perform autonomous flights. Moreover, these scenarios are generally complex and unstructured environments with a high density of obstacles and potentially strong lighting changes, hampering the use of different sensors at different parts of the flight. The work in this chapter is motivated within the context of the EU-H2020 AEROARMS project [1], in which an aerial robot autonomously navigates in industrial environments and performs physical-contact tasks such as gathering contact measurements or installing sensors. To provide this kind of capabilities without incurring in high risks, the designed perception system for the aerial robot must be highly 25 26 Robust Multisensor Localization for Complex GNSS-denied Environments robust to failures due to lack of sufficient geometrical or visual features (caused for instance by lighting changes or by the robot’s sensors being too close to the inspected structures, etc.) and also to failures caused by symmetries in the scenario (such as visually-alike sections or geometrically similar structures when moving towards different directions). Many existing real-time robot pose estimation methods employ only one main exteroceptive sensor such as a monocular/stereo camera or a 2D/3D LiDAR combined with an IMU [23,31 – 33,84 – 86]. Moreover, the methods that combine multiple sensors other tan IMU [21,28,39,42,87] are generally based on a mono-hypothesis approach, which can fail in complex scenarios with symmetries, not infrequent in urban or industrial settings. This chapter corresponds with the Contribution 1 of this PhD Thesis, and was published in [71]. It presents a robust multi-sensor multi-hypothesis 6-DoF localization filter for complex environments. The proposed scheme seamlessly integrates camera and LiDAR features in the same statistical framework and adopts a multi-hypothesis Recursive Bayesian Filter approach, that deals with potential symmetries in the scenario. The resulting scheme has been validated not only in the scenarios shown in the publication [71], but also in several autonomous industrial and civil I&M flights, during the reviews of the H2020 AEROARMS and AEROBI projects, leading to the best of our knowledge to one of the very first systems capable of GNSS-denied autonomous flight demonstrated in real industrial scenarios. The proposed filter is divided in two stages: In the Prediction stage the robot odometry is computed jointly considering LiDAR-camera features. In the Update stage the hypotheses are iteratively refined to maximize the consistency between LiDAR-camera features and a previously-computed multi-sensor map. The method has been designed to operate in real time using an efficient hypothesis filtering and refinement scheme, and has been coded in a multi-core software implementation. This chapter is structured as follows. Section 2.2 summarizes the related work for multisensor localization. Section 2.3 provides the designed approach and the developed methods. The experimental validation and comparison is in Section 2.4, first in controlled scenarios for comparison with other methods and later in real 2.2 Related Work 27 demonstrations in project reviews. Section 2.5 summarizes the conclusions for this chapter. 2.2 Related Work Robot localization is a traditional problem in robotics where many successful techniques have been developed. Generally, the problem of localization is divided in two sequential steps following the structure of Bayesian Filtering [88]: first, the obtention of the relative displacement with respect to the previous localization position (called odometry [21,24,49, 89], or state prediction or transition [23, 35]); then, the obtention of the absolute position of the robot with respect to a global or a local map (called lowfrequency odometry, filtering, or state update among other names [23, 24, 35, 89]). When the current localization is obtained, the new measurements can be integrated into the map. For LiDAR or camera-based schemes, this setup is commonly referred to as LiDAR odometry and mapping [21], or visual odometry [90], also adding the word ”inertial” to the name if an IMU is used [24,36,68]. Taking this one step further, if the robot detects loop closures (re-visiting of previous places) and uses them to update the map and past localization states, the term SLAM (Simultaneous Localization and Mapping) is used [23,85,88]. Generally, this term is preferred in camera-based systems, while recent LiDAR-based schemes still use the word ”odometry” even when integrating loop closures [24,38]. Many existing real-time robot pose estimation methods employ only one main exteroceptive sensor such as a monocular/stereo camera or a 2D/3D LiDAR [23,84 – 86], or in some cases combine it one or more proprioceptive sensors such as an IMU [31 – 33]. In few cases, some methods combined cameras and LiDARs in the same scheme [21,39]. Using one sole exteroceptive sensor does often not provide the required robustness and/or accuracy in complex environments, where sudden changes in lighting conditions, symmetrical structures in the scenario, and other adverse conditions are common. For instance, cameras capture texture and can be highly affected by lighting conditions. LiDARs capture geometry and can lead to wrong solutions in the presence of symmetrical structures. 28 Robust Multisensor Localization for Complex GNSS-denied Environments Some existing methods deal with combining different extereoceptive sensors [28,40 – 42,87], evidencing the interest in this line of research. However, the above methods have a mono-hypothesis approach, which can fail when a robot with unconstrained movements, such as an aerial robot, moves in scenarios with strong symmetries, e.g. in urban and industrial settings. Multi-hypothesis schemes are generally based on Monte-Carlo Localization (MCL), that uses a Particle Filter (PF) to manage a large number of state hypotheses (particles) to represent a generic random distribution of possible states, contrary to Kalman Filtering schemes that assume a unimodal distribution [88]. Each particle’s next state is predicted using the results from the odometry of the system, and then the likelihood of each hypothesis is updated by evaluating the consistency of the particle with the map. The PF can also adapt the number of particles when the distribution converges, leading to the Adaptive MCL solution (AMCL) [91]. These schemes are not widely adopted in camera or LiDAR localization, which instead rely on integrating more and more sensors to robustify the system [48,92]. They are more popular in radio-range SLAM, where they multi-modal initialization of radio beacons poses a challenging problem for mono-hypothesis schemes [93 – 96]. This is mainly due to the way PFs work: a traditional particle filter would require evaluating for each state hypothesis (tens, hundreds) the consistency of 3D LiDAR and camera features (thousands) with the multi-sensor map features (hundreds of thousands). Thus, their use is typically limited to 2D maps where the computation of the likelihood function is simplified [84,97,98]. Work [99] provided an pre-computed likelihood function for RGB-D camera measurements in 3D maps, although this does not fully exploit the information that could be obtained from LiDAR and camera features. This chapter presents a robust multi-sensor multi-hypothesis localization filter for complex scenarios. Camera and LiDAR features are tightly integrated in the same statistical framework, exploiting their synergies in a natural way. The system adopts a multi-hypothesis approach to cope with scenario symmetries, but largely reduces the computational burden of this approach by performing an efficient hypothesis refinement without affecting accuracy. 2.3 Proposed Multi-Sensor Filter 29 2.3 Proposed Multi-Sensor Filter Robustness and accuracy are the main drivers in the design of the proposed method. They are addressed by adopting an multi-sensor multi-hypothesis approach. Two main sensors are integrated: a stereo camera system and a 3D LiDAR. Stereo cameras provide images and depth images that are accurate for nearby objects but affected by lighting conditions and their depth error quickly increases with distance. LiDARs provide accurate point clouds at medium distances –up to 60-80 m–, but cannot provide texture and cannot receive accurate measurements from very near objects. Thus, stereo cameras are particularly informative when flying at low altitudes (takeoff/landing) or near obstacles, whereas LiDARs are blind when flying very near to objects and are very informative at medium distances. The proposed method treats jointly LiDAR and visual features in the same statistical framework, using a globally higher number of features, which enables selecting the most informative ones, and provides robustness in scenarios with low feature-density or with varying densities of LiDAR and visual features. Besides, our method can also integrate other optional onboard sensors such as an altimeter or an IMU. Moreover, the method can also integrate measurements coming from radio-range senors. In many environments such as industrial settings, there are sensors distributed in the scenario. These sensors can act as radio beacons: a robot equipped with a reader can measure the range to these beacons. If available, our method can also integrate these measurements, which are not as accurate as LiDAR or camera measurements, but provide a global position in the environment, that can be of great use for evaluating the consistency of the current hypotheses. As shown in Section 2.4, these optional sensors reduce the computational burden but have no significant effect on robustness or accuracy. For simplicity, in this chapter we assume that a multi-sensor map of the scenario is already available, and thus the method does not address the integration of new information into the map. Given a robust localization, this is a straightforward procedure that has been already documented in other works such as [21,23]. Moreover, for many applications, such as inspection or monitoring, most flights are to be 30 Robust Multisensor Localization for Complex GNSS-denied Environments performed in the same moderate-changing scenario, and thus this is not a strong requirement to evaluate the proposed method. Figure 2.1: Diagram of the proposed multi-sensor multi-hypothesis filter for robust GNSS-denied localization. Figure 2.1 presents a diagram of the proposed method. The overall scheme is divided in three stages: a preprocessing of the available measurements, a state prediction (odometry) stage, and a state update stage. First, geometrical and visual features are obtained from the LiDAR and camera inputs in Measurements Preprocessing, using the LiDAR scan to improve the depth estimation of the stereo camera. Next, these features are compared with the ones in the previous measurements to generate a local odometry in the Prediction Stage. The prediction is run at the frequency of the LiDAR scanner (10 Hz), but a Bundle Adjustment (BA) procedure is used to also consider the higher frequency of the stereo camera (60 Hz). Finally, in the Update Stage the proposed multi-hypothesis filter updates the current robot pose estimates using the available odometry, and then performs an efficient hypotheses refinement by using an iterative procedure: first, it analyzes the consistency of each measurement with the multi-sensor map; second, it refines the most consistent particles by performing an ICP iteration with the multi-sensor map. This procedure is repeated until convergence or until a timeout is reached. The computational burden of the scheme can be highly 2.3 Proposed Multi-Sensor Filter 31 reduced by using optionally available sensors such as altimeter, IMU and radio-range beacons. Next, each of the described stages is presented, giving details about their implementation. 2.3.1 Measurements Preprocessing This stage extracts 3D features from a stereo camera and a 3D LiDAR scanner, as well as metrics of their quality for pose estimation. The unified set of LiDAR-camera features at instant k is denoted as Fk during this chapter. The set of LiDAR features is denoted as Fl k, and the set of camera features, as Fc k. Two types of LiDAR features are extracted: edge points and planar points [22]. The quality of these features is related to the smoothness of the local surface between the feature point and its adjacent neighbours, which was defined in [22]: sj=1 N fl k,j      X i∈Neighs(j) fl k,j −pl i      2 ,(2.1) where f l k,j is the LiDAR feature j at instant k , and p l i is each of its total N neighboring points. For an edge feature, a higher value sj means that it is more informative than one with lower sj . For planar features, it is the opposite case. Hence, the quality of each feature fl k,j is q(fl k,j)=sjif the feature is an edge, or q(fl k,j)=s−1 jif it is planar. For the stereo images, SURF (Speeded Up Robust Features) features were selected as the most adequate for the target scenarios, after comparison with SIFT [100] and ORB [23] features. To lower the computational burden, the features are computed only at the right image of the camera. The used camera (ZED stereo) provides depth images, that are used in later to perform the 2D-3D match of the features of 2 consecutive measurements. These depth measurements had an accuracy that follows the formula σ = Z2α [101], with Z being the depth value, and the experimentally estimated value of α was of 0.00694 in the studied scenarios. This means that objects closer than 1.7 m had a depth estimation error lower than 2 cm. This error increases 38 Robust Multisensor Localization for Complex GNSS-denied Environments reflect the consistency between the obtained radio-range measurements {rk} and the expected measurements considering the pose of x[i] k. Figure 2.3: Example of an estimated altimeter measurement hi,k and the true altimeter measurement hk. In Step2 each hypothesis x[i] k that has been randomly drawn from χk is refined to optimize the consistency between Fk and the multi-sensor map features FMap . First, using the same matching criteria as in Section 2.3.2, the features in Fk|x[i] k of each of the hypotheses are matched to FMap . Then, using the Bundle Adjustment (BA) procedure presented in Section 2.3.2, the optimal transformation that minimizes the distance between Fk|x[i] k and FMap is computed. This BA problem only optimizes the current pose of each hypothesis, and thus it is less computationally expensive than the one used for odometry estimation. However, the matching of the LiDAR features involves high computational cost, since it has to be performed for each of the multiple hypotheses. We address this by implementing an efficient multi-core scheme for computing the multiple feature matching solutions, using the library nanoflann to further improve the query velocity. Finally, each computed transformation is used to update the corresponding hypothesis. The developed method can be implemented in real time with standard hardware onboard the robot. Since the Prediction stage is in general accurate, the Update stage often converges after one or two iterations. Moreover, different hypotheses frequently converge in the same one, decreasing the computational cost. For the experimental evaluation, our scheme used 60 hypotheses, refining 30% of them in each iteration. The statistical mean of the state hypotheses (rejecting outliers) is used as the final robot pose estimation. 2.4 Experimental Validation 39 2.4 Experimental Validation The method has been extensively validated in sets of experiments at different I&M scenarios: two bridges and a mockup of an industrial setting. This section provides implementation details of the method, analyses and compares its performance with existing well-known techniques, and showcases the live demonstrations where it was used to enable autonomous navigation in industrial environments, without available GNSS signal. The final implementation platform is the DARIUS aerial robot, which is a customdesign hexarotor with a MTOW (Maximum Take Off Weight) of 22 kg and a tip-to-tip blade size of 1700 mm. It achieves a flight endurance of up to 20 min, sufficient for the designed experiments. The platform is equipped with different sensors: a Velodyne HDL-32E, a ZED stereo camera, an XSENS MTI-100 IMU, a laser altimeter and an ToF radio-range sensor based on Ultra-Wide Band (UWB) technology, that receives range measurements from different beacons deployed along the scenario. These beacons simulate smart sensors that can be installed in industrial setups to provide different information when needed. For ground-truth purposes, DARIUS is equipped Figure 2.4: DARIUS aerial robot used in the experiments. ©IEEE 2024 40 Robust Multisensor Localization for Complex GNSS-denied Environments with a centimeter-level accuracy RTK D-GPS. For computing the method and logging information, the system has an Intel NUC and a Nvidia Jetson TX2. All sensors are synchronized at software level. The field of view of the LiDAR and camera systems was coincident during most of the performed flights, since the Velodyne LiDAR has a slightly inclined FoV when mounted horizontally. The proposed localization method was developed under Ubuntu 16.04 and ROS Kinetic. All the GPU-intensive calculations (SURF features detection, stereo depth estimations) are performed in the Jetson TX2 board, that is connected to the NUC board via Ethernet. The rest of the calculations (LiDAR feature matchings, bundle adjustment, multi-hypothesis filtering) happen in the NUC board. One CPU core in both the NUC and TX2 boards was left for data logging purposes. The pose estimation obtained from of the proposed localization scheme is sent to the Pixhawk autopilot via MAVROS, where it is fused with a high-frequency IMU and used for navigating the aerial robot in the performed experiments. 2.4.1 Performance Evaluation A total of more than 150 validation flights were performed at three different scenarios. The ETSI scenario is a bridge at the School of Engineering of Sevilla, see Figure 2.5-up. The Karting scenario is a industrial mockup environment, see 2.5-bottom-left. A dataset of different flights in this scenario is detailed in Appendix B.1. The Bridge scenario is located at a road bridge and was the demonstration site of the H2020 AEROBI project, see Figure 2.5-bottom-right. Demonstration flights in real industrial scenarios are shown later in this section but are not part of the validation published at [71] since these were performed after the submission of the work. The robot position ground-truth in the Karting and ETSI scenarios was provided by the RKT-GPS, whereas in the Bridge scenario it is given by a Leica TotalStation. The experiments were performed as follows. First, a multi-sensor map of the scenario was built in prior manually-assisted flights, see Figure 2.6. Next, a number of flights in fully-autonomous navigation mode were performed, using the proposed 2.4 Experimental Validation 41 Figure 2.5: Pictures of the experiment scenarios: Top) ETSI, Bottom-left) Karting and Bottom-right) Bridge.©IEEE 2024 method to compute pose estimations in real time using that map. Figure 2.7 shows the results obtained in one bridge inspection experiment. The proposed method was compared to two well-known and publicly available robot localization methods: LOAM (LiDAR Odometry and Mapping) [22], based on 3D LiDAR scans, and ORB-SLAM2 (Oriented FAST and rotated BRIEF -SLAM2) [23], based on visual images. The latest available codes of LOAM and ORB-SLAM2 at January 2019 were used. To provide a fair comparison, the LOAM code was modified to use all 32 channels available in the mounted LiDAR, and ORB-SLAM2 was used in stereo mode, and in localization mode with a pre-computed map. Also, during 42 Robust Multisensor Localization for Complex GNSS-denied Environments Figure 2.6: Multisensor maps of the ETSI and Karting scenarios. LiDAR features are in orange, the UWB (Ultra-Wide Band) radio-range beacons in the scenario are in green. Camera texture features can be seen in the map. ©IEEE 2024 this comparison, the optional sensors (IMU, altimeter, and UWB sensors) were not integrated in our method. All methods are initialized with the take-off pose of each flight. The measurements were logged and processed off-line with the three methods. Camera images were logged at 60Hz, and LiDAR measurements, at 10Hz. For a qualitative comparison, Figure 2.8 shows the 3D localization errors of all methods versus the ground-truth in one Karting experiment. In general, all methods provided accurate results in the X axis, but LOAM was prone to higher errors during the beginning of the flight due to the lack of rich geometrical features when flying at low altitudes after take-off. During mid-flight, LOAM and ORB-SLAM2 did not provide accurate results in the Y axis, since the robot was flying parallel to a pipe for performing inspection, leading to multi-modal solutions due to symmetries in 2.4 Experimental Validation 43 Figure 2.7: Results in one bridge inspection experiment. The figure shows the geometrical map component (in green), the localization obtained by our method (in blue) and the ground-truth localization (in magenta). Only the geometrical component of the map is shown for better visualization. ©IEEE 2024 the scenario, that made these methods lose accuracy. Finally, the Z estimation in ORB-SLAM2 was best when the robot was near to the ground at the end of the flight (since there are rich visual features on the floor), while LOAM gave better results for higher altitudes. Our proposed scheme provided relatively low errors during the full flight, by naturally exploiting the synergies between both sensors in the same statistical framework. Table 2.1 shows the absolute translation RMS errors (t RMSE ) and maximum errors (t max ) in a set of 3 different experiments performed for each of the three scenarios. The results in this table offered similar conclusions than those in Figure 2.8. Noticeably, LOAM and ORB-SLAM2 tend to give accurate results in many of the experiments, but at different times they are affected by the lack of features (either geometrical of visual), and by the different symmetries in the scenario. At landing and taking off, LOAM was the most affected method, as well as when flying near the scenario structures for 44 Robust Multisensor Localization for Complex GNSS-denied Environments 0 20 40 60 80 100 0.2 0.4 0.6 0.8 1 X error(m) Proposed method LOAM ORB-SLAM2 0 20 40 60 80 100 0.2 0.4 0.6 0.8 1 Y error(m) 0 20 40 60 80 100 Time (s) 0.2 0.4 0.6 0.8 Z error(m) Figure 2.8: 3D localization errors in the experiment in Figure 2.7 obtained by the proposed method, LOAM, and ORB-SLAM2. ©IEEE 2024 performing inspection. Contrarily, ORB-SLAM2 had most problems when the robot was very far from the visual features in the scenario, which is a common situation in large scenarios such as in the Bridge experiments. In these situations, the overall accuracy of these methods is affected, but more importantly, the variability in their errors, leading to sudden changes in the state estimation of the robot that may cause accidents when performing autonomous inspection. Our proposed method, combining the synergies of the sensors with a multi-hypothesis framework, delivers a significantly more stable solution. 2.4 Experimental Validation 45 Table 2.1: Comparison with other methods in different scenarios ©IEEE 2024 ORB-SLAM2 [23] LOAM [22] Ours [71] Experiment tRMSE (m) tmax (m) tRMSE (m) tmax (m) tRMSE (m) tmax (m) Karting1 0.56 1.89 0.62 1.24 0.13 0.28 Karting2 0.18 0.72 0.31 0.85 0.11 0.19 Karting3 0.51 0.97 0.40 1.12 0.21 0.26 ETSI1 0.69 1.34 1.31 2.08 0.23 0.34 ETSI2 0.50 0.81 0.74 1.35 0.13 0.21 ETSI3 0.23 0.42 0.41 0.70 0.11 0.19 Bridge1 1.68 3.12 0.49 0.78 0.14 0.23 Bridge2 1.19 2.41 0.53 0.91 0.18 0.25 Bridge3 0.84 1.73 0.34 0.69 0.21 0.32 2.4.2 Ablation Study The proposed method is envisioned to mainly use visual and LiDAR features, and has been validated against other schemes in Section 2.4.1 using only those two sensors. However, as explained in Section 2.3, it can also integrate measurements from other commonly-used sensors when available. Thus, we now briefly describe how integrating different sensors can have an influence on the resulting accuracy and the computational burden of the scheme. A total of four sets of sensors are analyzed: S1) camera+IMU+altimeter, S2) LiDAR+IMU+altimeter, S3) LiDAR+camera and S4) LiDAR+camera+IMU+altimeter+UWB. The performance of each setup is shown in Table 2.2, focusing on the mean translational RMS error (t RMSE ) and the computational time used in the Update stage (ut), where the majority of the computational burden of our method happens. The sensor setups S3 and S4 achieve the best accuracy by exploiting the combination of LiDAR and camera features. Noticeably, the computational burden in configurations S2-S4 is significantly higher than in S1. The presence of 3D LiDAR features in these configurations requires performing an iterative matching with the map, which is of great cost when using a multi-hypothesis approach. This burden is alleviated in configuration S2 by using an altimeter, and in configuration S4 by using all the optional sensors: altimeter, IMU, and UWB. The errors in configuration S3 and S4 46 Robust Multisensor Localization for Complex GNSS-denied Environments Table 2.2: Performance of the method with different sensor setups ©IEEE 2024 S1 S2 S3 S4 Experiment tRMSE ut tRMSE ut tRMSE ut tRMSE ut (m) (ms) (m) (ms) (m) (ms) (m) (ms) Karting1 0.39 6 0.34 48 0.13 55 0.09 27 Karting2 0.22 7 0.15 51 0.11 69 0.06 31 Karting3 0.34 6 0.35 44 0.21 61 0.12 23 ETSI1 0.46 8 0.62 65 0.23 73 0.11 28 ETSI2 0.37 6 0.51 61 0.13 91 0.09 42 ETSI3 0.18 7 0.24 78 0.11 85 0.06 33 Bridge1 1.36 11 0.18 81 0.14 157 0.11 59 Bridge2 1.27 9 0.43 89 0.18 175 0.12 65 Bridge3 0.72 12 0.29 78 0.21 147 0.08 56 are similar, but integrating the optional sensors significantly improves the execution time in S4. This is most noticeable at the Bridge experiments, that have the largest and most varied map. Even in these cases, the Update stage of S4 takes ≈ 60ms at most, which is enough for given the frame rate of the chosen sensor system. Reducing the number of hypotheses or the number of selected LiDAR features can reduce this time if necessary, at the cost of accuracy. 2.4.3 Live Demonstrations in Real Scenarios The proposed method was designed to allow the introduction of autonomous robots in industrial and civil scenarios to perform Inspection and Maintenance tasks. Given the accuracy and stability of the developed solution, the method has been used for this function onboard autonomous robots in the envisioned use cases, delivering very good results in multiple reviews of H2020 projects. Particularly, the developed system has been validated in the three following scenarios: First, in the inspection of a road bridge during the demonstration of the H2020 AEROBI project, already presented in Figure 2.5-bottom-right and included in publication [71]. Then, in the inspection of a petrol refinery during the demonstration of the H2020 AEROARMS project, see Figure 2.9. In this refinery, the method was adapted to perform real-time updates of 2.5 Conclusions 47 the LiDAR map, and camera features were not used since the robot flied far from the structures in the scenario. Finally, in the inspection of a cement factory in the H2020 AEROARMS project, see Figure 2.9. For videos including these reviews, see AEROBI Technical Demo,AEROARMS Final Summary. Figure 2.9: Real industrial scenarios where the proposed method was used for autonomous aerial navigation: Left) 3 scenarios in the Wilhelmshavener Raffineriegesellschaft oil refinery at Wilhelmshaven (Germany); Right) Lafarge-Holcim cement factory in Jerez de la Frontera (C´adiz, Spain). 2.5 Conclusions This chapter presented a robust multi-sensor multi-hypothesis robot localization method, motivated by the objective of enabling the safe and autonomous navigation navigation of aerial robots in complex, GNSS-denied industrial and civil I&M scenarios. The chapter corresponds with the Contribution 1 of this PhD Thesis. The proposed method follows three main ideas to achieve accuracy and robustness, and to perform with low computational burden. First, camera and LiDAR features are integrated in the same statistical framework, taking advantage of their synergies and compensating the situations where each sensor presents a low density of features. 54 Probabilistic Meshes for Long-range LiDAR Mapping present in these scenarios are locally planar, although non-planar structures (such as trees, vegetation, or powerlines) also exist. As seen on Section 3.2, point-based methods have been widely used for representing both locally planar and non-planar structures, and recently significant research effort has been made to provide more suitable map representations for planar structures [26, 29, 45, 114], since correctly representing them is critical to improve the state estimation of the system. In addition, given the probabilistic nature of LiDAR measurements, the resulting map obtained from them is also bound to uncertainty. Neglecting this uncertainty can lead to an overconfidence on the current state estimation, underestimating its covariance, which can hamper the introduction of other sensors’ information that may complement LiDAR measurements. Next, we illustrate the influence of map structure and map uncertainty in a LiDAR-based state estimation scheme, particularly when representing planar structures, which are the most prominent in long-range scenarios and thus, the focus of this chapter. First, motion estimation methods for LiDAR-based mapping require matching each new input LiDAR scan point with a certain map representation, and minimizing the distance to it, for all available points in the scan. For point-based methods in the case of planar points, this representation is computed from the N map points that are nearest to the input point, typically by fitting a plane to them. For voxel-based schemes, this representation is the plane contained in the voxel in which the input point falls (or the N nearest points if no plane was fitted). For mesh-based schemes, it is the plane of the nearest triangle to the input point. Depending on the selected map neighbors in the point-based scheme, the resulting local map representation can be highly deviated from the surface that the points intend to represent (see Figure 3.1). In the case of voxels, the way in which the space is divided can also generate unintended local representations. In both cases, these deviated map representations can directly affect the mean and covariance of the robot state estimation. The mean is affected since the LiDAR position will reflect the difference between the obtained and the ideal local planes. The covariance is affected since each point-plane distance measurement induces information in the perpendicular direction to that plane, and thus introducing erroneous planes results in undesired information gains in the robot state. 3.3 System Description 55 Map points Scan pointsGround LiDAR position Initial state estimate Voxel-based state estimate Point-based state estimate Voxel-based map planes Point-based local representations Mesh-based state estimate Mesh-based map Voxel-based spatial partitions Figure 3.1: Example of the performance of different map representations in a geometrically poor scenario. The prior of the robot estimated pose differs from its true position (marked with ”+”). Given this scenario, it is only possible to correct the estimated robot pose in the vertical axis. Top) Using a point-based map, the state estimation scheme is not able to correct the error in the mean of the robot estimated pose. Also, the normals of the local map representations can be different to the surfaces they intend to represent, erroneously adding information in the horizontal axis, underestimating the final state covariance. Middle) Using a voxel-based map, local representations are estimated within the boundaries of the voxel, potentially introducing similar effects than with a point-based map. Bottom) Using a mesh-based map, the local correspondences tend to better resemble the intended surfaces, allowing to correct the vertical error without adding erroneous information in the horizontal axis. For point-based schemes, there is no higher number of neighbors N that guarantees reducing this problem, since not all points may lie in the same planar surface. For voxel-based schemes, reducing the size of each voxel can help, at the cost of a higher 56 Probabilistic Meshes for Long-range LiDAR Mapping map size, but using too small voxels can even eventually result in overfitting undesired planes. In geometrically rich scenarios, the mean error tends to get compensated by the different measurements and their local representations, but the covariance will still be erroneously reduced further than desired. In geometrically poor scenarios, these effects can eventually have drastic consequences in the quality of the motion estimation. Finally, mesh-based representations are naturally intented to represent planar structures, and do not rely on locally estimated or spatially constrained planes for this purpose, showing clear advantages that justify the need for research in this direction. Second, most of the works shown in Section 3.2 consider that the points in each scan are affected by measurement noise, but do not consider how this noise propagates to the obtained representations in the map. Not considering uncertainty in the map results in overconfidence in the LiDAR point-map distances, whose covariances depend not only on the LiDAR scanning noise, but also on the map noise. This effect is alleviated in environments with rich geometrical information content. However, in geometrically poor scenarios, this overconfidence can have a direct impact on the robot pose estimation, significantly affecting both the mean and covariance of the state estimation. Other strategies such as [110] directly estimate the point-map distance measurement uncertainty, although this is only a partial solution to the problem. Recently, authors in [29] did consider map uncertainty in the developed scheme, although assumed planes ∈R6 that do not capture the on-manifold nature of this representation, which is vital to provide accurate uncertainty estimates. In summary, the above two effects have higher impact in geometrically poor environments, in which they can deteriorate the estimated mean and covariance of the robot pose, causing important errors in the map, and hampering the introduction of other sensors such as IMUs or GNSS, that could complement the lack of geometrical diversity. In this chapter, we propose to naturally integrate a probabilistic mesh representation of the locally planar map surfaces into a generic mapping framework, substituting other representations for these surfaces. Each triangle (face) in the mesh is associated to an imperfect plane with a covariance matrix. This representation addresses the different shortcomings of other solutions in two ways. First, different 3.3 System Description 57 triangles can be iteratively joined together via incremental decimation, considering the uncertainty of each face to reduce the mesh size while preserving its structure as much as possible. This reduction drastically alleviates the problem of point-to-map matching, since decimated faces tend to better resemble the intended map. Second, the uncertainty in each plane is obtained and considered for state estimation, solving the problem of underestimating its covariance. As an added benefit, decimation also reduces the map size (number of faces), which is of high interest in long-range mapping missions, e.g. in urban, agricultural, or industrial environments, where a high number of planar surfaces are present. 3.3.2 Proposed System This chapter presents the necessary modules for integrating probabilistic meshes in generic traditional LiDAR-based odometry and mapping schemes. Our aim is not to provide a new mapping method, but to enrich existing ones with the possibility of using probabilistic meshes inside them. We do not provide a complete off-the-shelf mapping system, but rather a set of contributions that do not depend on the rest of the design choices, giving high flexibility in their usage. A generic mapping scheme can be divided in two main components: Motion Estimation, which obtains the current robot state by comparing the input scan with the currently available map; and Map Update, which transforms and merges the input scan into the map. Figure 3.2 shows how a generic mapping scheme is adapted to include probabilistic meshes. To do so, we integrate new modules within Motion Estimation and Map Update, and also a new component (Mesh Generation) that is in charge of obtaining a mesh representation of the input scan before it is added to the map. The proposed modules, shown in green, are independent of the chosen state estimation framework (e.g., EKF, on-Manifold EKF, UKF, factor-based optimization), as well as compatible with the inclusion of other sensors, or with point-based representations for non-planar objects. The scheme is independent on the chosen tools for mesh manipulation (e.g., decimation, 2D mesh merging, nearest faces search). This chapter presents methods for dealing with the following components: 58 Probabilistic Meshes for Long-range LiDAR Mapping LIDAR planar pts. Motion Estimation Nearest Faces State Estimation Mesh Generation Point-Plane Contributions Triangulation & Quadrics Init. Plane Initialization Iterative Decimation Map-Clusters Matching Similarity Computation Cluster Aggregation Map-Clusters Merging Quadrics Gen. for Intersections Iterative Decimation Plane Aggregation Map Update Mesh Map Clusterization Pose Map Other Sensor Contributions Figure 3.2: Proposed components for integrating a probabilistic-mesh-based map in a generic framework. The contributed modules are shown in green. Colored arrows denote the state of the system (pose and map). Black arrows denote the normal execution cycle for a new scan. Mesh Generation This component addresses the mesh initialization for each new input scan. First, it generates the different faces through triangulation, and associates a probabilistic quadric to each face. Then, it decimates the mesh via iterative edge contraction and finally initializes all the plane representations using the current robot pose to relocate the mesh. We propose a novel on-manifold covariance formulation for these plane representations (see Section 3.4.1), contrary to traditional approaches that neglect these covariances or do not consider the on-manifold nature of the planes. Motion Estimation For each point in the input scan, this component finds its corresponding map face (plane), and computes its point-plane distance measurement, which is added into a state estimation framework that obtains the robot current state using these measurements and potentially others from additional sensors. In Section 3.4.2 we present the necessary terms to include the point and map uncertainty in an iterative on-manifold state estimation scheme. Map Update When a new mesh at map coordinates is ready, this component divides the mesh into a series of clusters of highly similar planes, by iteratively comparing sets of planes with a novel similarity metric based on 2nd-order uncertainty propagation (see Section 3.4.3) and aggregating those that are most alike. Then, the clusters are matched to those in the map and merged with them using a 2D representation of the 3.4 Proposed Methods 59 faces. New intersecting vertices are assigned with a novel probabilistic plane quadric obtained from spline approximations and fourth order uncertainty propagation (see Section 3.4.3). Finally, the plane representation of each face is updated (using the same aggregation scheme from Section 3.4.3) to reflect the different unions happening during decimation. The proposed components and methods enable the seamless integration of a probabilistic mesh-based map in a generic LiDAR-based mapping framework. The three components and their related proposed methods are presented respectively in Section 3.4.1, 3.4.2, and 3.4.3. The proposed approach and methods formulate planes on the S2×R manifold, thus avoiding inconsistencies in their estimation and additionally reducing the required memory space for covariance matrices by 40% (6 unique elements instead of 10). In Section 3.5 each of the proposed modules is experimentally validated individually or together with its related modules. In Section 3.6, the whole scheme is validated in a series of synthetic scenarios and real long-range mapping flights using [35] as a base mapping solution, which we use to process points marked as non-planar. Specific Notation In this chapter we will measure the similarity between two covariance matrices using the second term of the Bhattacharya distance for multivariate normal distributions. This is equivalent to using the whole formula but assuming zero-mean distributions with covariances given by the compared matrices. Thus, we make the following abuse of notation during the chapter: DB(Σa,Σb):= DB(N(0,Σa),N(0,Σb)) (3.1) 3.4 Proposed Methods This section presents the proposed components and modules for integrating probabilistic mesh-based representations in a generic mapping system. The execution starts with each new input LiDAR scan, which is assumed already undistorted using the 60 Probabilistic Meshes for Long-range LiDAR Mapping IMU measurements or the robot estimated motion [21,35]. Also, the LiDAR points have already been classified into planar, edge, or other features [21,112]. The proposed methods are only intended for points resembling locally planar surfaces (from now on, planar points or simply points). Mesh Generation and Motion Estimation are mainly executed in parallel, although the robot pose obtained in Motion Estimation is used in Mesh Generation. After them, Map Update integrates the mesh obtained from the LiDAR scan in the map. 3.4.1 Mesh Generation Denote each new input as PL , which is the set of 3D planar points pjL∀j in a new scan expressed in the LiDAR reference frame L .Mesh Generation converts PL into a set of 3D triangles (faces), obtains a reduced representation via iterative decimation, and associates each resulting triangle with a 3D plane with uncertainty. It includes the following modules: Triangulation & Quadrics Initialization First, an initial set of non-intersecting triangles from the input scan is generated. For LiDARs with a repetitive scanning pattern, this can be done by joining adjacent planar points in the scan. For LiDARs with non-repetitive scanning patterns (or potentially high scan distortions that impede simple triangulation methods), it performs a fast 2D Delaunay triangulation [121] in the polar angular coordinates of the input points PL , such that the resulting 3D triangles do not intersect with each other. Edges of > 1 m are removed, and then unconnected points (typically vegetation) are added to the non-planar point set. Next, to perform mesh decimation, we need an error metric that can be evaluated for each face. Quadric error metrics have been widely studied for this purpose for meshes assumed without noise [122], assigning each triangle’s plane with a matrix (plane quadric) that allows to find the distance of any point to that plane. Work [123] further proposed triangle quadrics that are weighted by the area of the faces, leading to more regular final triangulations. Recently, [124] proposed probabilistic versions 3.4 Proposed Methods 61 of these plane and triangle quadrics, that account for anisotropic Gaussian noise in each triangle plane (plane quadric) or in each of its 3 corresponding vertices (triangle quadric). The derivation of the triangle quadric in [124] can be obtained without any simplifying assumption, contrarily to the derivation of the plane quadric. Thus, we adopted the probabilistic triangle quadrics in [124] for initializing the quadrics of a new mesh, in which each plane is represented by only 3 points and their covariances. Later in Section 3.4.3, for other cases we propose a novel formulation of the probabilistic plane quadric that is less approximate than the plane quadric in [124], as is shown in Section 3.5. Iterative Decimation This module performs surface simplification via iterative edge contraction [122] using the previously initialized quadrics. In short, it assigns a quadric matrix to each vertex, that comes from summing all the triangle quadrics of the faces adjacent to it. Then, each vertex pair is contracted if the cost of substituting both vertices by a new one is lower than a threshold. The new vertex is computed from the union (sum) of the quadrics of both removed vertices. This is done in iterative passes (between 1 and 3 in the presented results) over all the mesh while increasing the cost threshold between passes. We used the fast quadric decimation software at [125], although any method that allows to input custom quadrics, e.g., [126], can be used. Plane Initialization Each of the faces of the decimated mesh represents 3 or more points from the scan. In this module we use these points to obtain a unique plane representation mwith perturbation covariance Σ m for each face. We transform each point pjL∈ PL (and its covariance) to the world frame W as pjW∈ PW using the robot pose obtained in Motion Estimation. Next, for each face f and the set of points Pf⊂ PW it represents, we compute the plane mwith perturbation covariance Σ m that minimizes the distance to each pjW∈ Pf. 62 Probabilistic Meshes for Long-range LiDAR Mapping Many existing methods [29, 35, 37, 110, 127] obtain plane mby assuming that the distances to each point in Pf have equal covariance, leading to a linear least squares problem that can be solved using Singular Value Decomposition (SVD) or similar algorithms. We follow this solution, which provides high accuracy for the given disposition and error characterization of LiDARs in typical mapping problems. Next, in our approach we also obtain the perturbation covariance Σmfor each plane. We propose to compute it as the parameter covariance for a cost function K that represents the weighted distances from plane mto a set of at least 3 points pjW∈ Pf with independent Gaussian noise: K=X ∀pjW∈PfpjW Tn+d2Sj,(3.2) where the distance from plane m= { n , d} to each point pjW is given by pjW Tn+d and S j is the information matrix of that distance. In this case S j is a scalar value obtained from linear uncertainty propagation as S j = (n T Σ j n) −1 , since the Jacobian of the plane-point distance w.r.t. pjWis equal to nT. In (3.2) , plane mcan be perturbed with noise ηm as m ⊞ηm . After introducing ηm into the cost K , the covariance Σ m can be approximated using the Jacobian of K with respect to ηm . This is a common practice in Gauss-Newtown optimization problems [128], and allows to compute Σ m while considering each S j and while retaining the on-manifold nature of m. The individual cost term kj for each point is now given as: kj=S1/2 j(pjW T(n⊞ηn) + d+ηd), S1/2 j= ((n⊞ηn)TΣj(n⊞ηn))−1/2,(3.3) and the Jacobian of that cost term evaluated at ηm=0is: Jj=h(pjW Tn+d)nTΣjb nB(n)Sj−pjW Tb nB(n),1iS 1 2 j,(3.4) where b n is the skew-symmetric matrix of nand the matrix B (n) is the orthonormal basis of the homeomorphic space of the S2 manifold at n(see [83, Figure 18] for a visual explanation). The derivation of J j can be obtained using (A.5) and the 3.4 Proposed Methods 63 definition of the Jacobian of a dot product, as is done in Appendix A.1 for other derivations. By concatenating all J j as J= . . . , JT j, . . . T∀pjW∈ Pf , the covariance is Σm= (JTJ)−1. Notice that, if the cost Kis near its minimum (i.e., if the value of mis close to its optimal and thus the problem is unbiased), the value (J T J) −1 is the Cram´er-Rao lower bound for Σm[128]. As shown later in Section 3.5.1, the proposed method significantly improves the results of alternative covariance formulations while requiring negligible additional computational cost. 3.4.2 Motion Estimation For simplicity, assume the state of the robot is fully represented by x= htL W,RL Wi∈ R3×SO(3) , which is the current pose of the LiDAR in the world frame W . The objective is to find the robot pose that minimizes the distance from each point p L∈ PL to its corresponding closest plane m W in the map (the indexes for the point and plane are dropped for brevity). Our approach is independent of the chosen state estimation framework, such using Kalman Filtering (KF) [34, 36], or factor graph optimization [24,89]. We find the on-manifold iterated KF (IKFoM from now on) proposed in [35] as a suitable base framework for our work. The IKFoM formulates the state as a compound manifold and its uncertainty as the covariance of potential perturbations defined in the homeomorphic Rn space to that manifold at the current state, providing a minimal representation without overparametrization. Besides, it is able to perform with low computational burden while still having significant accuracy. Motion estimation is composed of three modules: Nearest Faces,Point-Plane Contributions, and State Estimation. The first one is independent of the estimation framework. The two last have been particularized to the IKFoM, but can be modified to adopt other frameworks. Nearest Faces Our approach uses a mesh representation of the map and requires nearest planes query methods intended for sets of triangles, such as Bounding Volume Hierarchies. 70 Probabilistic Meshes for Long-range LiDAR Mapping merged as follows. First, we obtain the polygon representations of both 2D mesh clusters (map and input) using the approach in [135]. Then, we compute the intersections between both polygons and remove the parts of the input polygon that are inside of the map polygon. Next, we follow the approach in [136] to intersect, subdivide, and re-triangulate the obtained mesh using a constrained Delaunay triangulation. The result is a new mesh that keeps all the necessary faces and vertices from the merged meshes, and generates new vertices and faces for the intersections between them. Plane Quadrics for Intersections The new vertices appearing due to 2D intersections in Map-Clusters Merging require a quadric error function to be added to the mesh-based map. These vertices are not computed from actual LiDAR measurements, but rather are a construct to represent the merge of two clusters in the same plane. The merged mesh is decimated next, see Section 3.4.3, and this decimation should be guided by the initial quadrics computed in Mesh Generation and the quadrics already in the map. Thus, the quadrics for these new vertices should let the intersections move along the plane, and also move slightly perpendicularly to it, so that the decimation is still guided by the quadrics of the real 3D vertices. Since the joint planes in Map-Clusters Merging are represented by more than 3 points, plane quadrics are preferred for this case in contrast to the triangle quadrics used in Section 3.4.1. Extending the reasoning in [124] for planes in the manifold S2×R , we formulate the following quadric of a plane mwith Gaussian noise in its perturbation ηm: Q=Eh(m⊞ηm)(m⊞ηm)Ti∈P(4),(3.23) where P(4) is the space of positive definite matrices. Quadric Qcould be obtained directly through 2nd-order error propagation. Other alternatives are to propagate over (m⊞ηm) to obtain an R4 manifold-free variant of the plane, and then perform linear error propagation in two different ways. These alternatives for obtaining Qare detailed in Appendix A.3. However, all these methods ignore higher order terms for computing Qthat become relevant for this specific case. 3.4 Proposed Methods 71 The proposed solution overcomes this problem by computing quadric Qfor plane min a virtual reference frame where its formulation is simplified, and then obtaining a generalized approximation using error propagation and a polynomial representation. The approach is valid since the unit sphere is rotation-invariant, and thus the quadric can be computed in any rotated reference frame as long as its original orientation is recovered afterwards. We found this solution to be multiple orders of magnitude more accurate than other alternatives, see Section 3.5.4. We start by assuming a virtual reference frame V where mis aligned with the z axis, but its covariance remains the same, i.e., ΣmV = Σ m . The rotation to transform to and recover from this axes is always obtained in closed form, see (3.27) . In this frame V we apply a second rotation of angle α around axis z such that the components of Σ m related to nbecome diagonal (see both rotations in Figure 3.3), resulting in: Σ′=Rz(α)TΣmRz(α), α =1 2atan( 2Σm1,2 Σm1,1−Σm2,2 ),(3.24) where R z ( α ) is the rotation matrix and Σmi,j is the {i, j} -th entry in Σ m . After this rotation, the plane uncertainty, expressed as a random perturbation vector η′ m = {ηn′, η′ d} = {η′ 1, η′ 2, η′ d} ∈ N (0 , Σ ′ ) has zero correlation between the elements η′ 1 and η′ 2 , which are related to the uncertainty in n ′ , i.e., Σ ′ 1,2 = 0. In these new frame V′ , the plane m ′ equals h0,0,1, diT . Using this value, the expression (3.23) for the quadric formulation is simplified as: Q′ 1:3,1:3 =E    η2 1SC2 ζ0 0 0η2 2SC2 ζ0 0 0 C2 ζ    , Q′ 4,1:3 T=Q′ 1:3,4=E    (d+ηd)η1SCζ (d+ηd)η2SCζ (d+ηd)Cζ    , Q′ 4,4=Ed2, (3.25) 72 Probabilistic Meshes for Long-range LiDAR Mapping where ζ = pη2 1+η2 2 , SCζ = sinc ( ζ ) = sin ( ζ ) /ζ , and Cζ = cos ( ζ ). The expression Q′ i1:i2,j1:j2refers to the block of Q′from rows i1 to i2 and from columns j1 to j2. Figure 3.3: Top) Changes in the reference frame ( W→V→V′ ) to align a direction vector nof any plane mwith the z axis, and later diagonalize its variance (recall that Σ n = Σm1:2,1:2 ). Notice that while the S2 basis remains the same for n V and n ′ V , the rotations along the z axis allow to diagonalize its covariance Σ n . Bottom) MonteCarlo estimations of Q ′ 1,1 and Q ′ 3,3 given nin a virtual reference frame V′ and with diagonal covariance Σ′. Even with this new formulation, to the best of our knowledge, there is still not a closed solution to the expected value of the quadric, except for the exact solution Q ′ 4,4 = d2 + Σ ′ 3,3 . However, the proposed reformulation makes it easier to explore other alternatives: It is not difficult to notice that the 3 elements in the diagonal of Q ′ 1:3,1:3 only depend on Σ ′ 1,1 and Σ ′ 2,2 since there is no correlation between η′ 1 and η′ 2 . Each of these elements can be locally approximated by one or more low (3rd) order bivariate polynomial functions of Σ ′ 1,1 and Σ ′ 2,2 , see Figure 3.3. We found that using one polynomial function for each of the 3 intervals max (Σ ′ 1,1, Σ ′ 2,2 ) ∈ { (0 − 0 . 3] , (0 . 3 − 1] , (1 − 4] } is sufficient to cover all the planes in the experimentally analyzed datasets (the first two ranges cover the majority of cases). In addition, Q ′ 1,1 and Q ′ 2,2 use the same polynomial functions by swapping the input variables Σ ′ 1,1 and Σ ′ 2,2 . Hence, only 6 functions suffice to determine the elements in the diagonal of 3.4 Proposed Methods 73 Q ′ 1:3,1:3 . Finally, since the coefficients of the polynomials are computed in the virtual reference frame of the plane, they are general for any possible plane. Thus, they can be precomputed only once using a MonteCarlo analysis and will always be valid in the studied ranges of Σ ′ 1,1 and Σ ′ 2,2 . We provide the coefficients for the 6 proposed polynomials along with the code contributed in this PhD Thesis (available here), as well as the raw data from the MonteCarlo analysis for the interest of the reader. For Q ′ 1:3,4 , the polynomial analysis is not as efficient since it depends on multiple new variables, namely: d , Σ ′ 3,3 , Σ ′ 1,3 , and Σ ′ 2,3 . In this case we perform 4th-order uncertainty propagation for each term following [137, eq. (19)]. After reducing zero-valued terms related to correlation between η′ 1and η′ 2, we obtain: Q′ 1:3,4≈    Σ′ 1,31−1 2Σ′ 1,1+1 3Σ′ 2,2 Σ′ 2,31−1 2Σ′ 2,2+1 3Σ′ 1,1 d1−1 2Σ′ 1,1+ Σ′ 2,2+1 8Σ′2 1,1+ Σ′2 1,1   (3.26) This approximation is again only dependent on values expressed in the virtual reference frame V′ , and thus general to any plane. Hence, all elements in the whole quadric Q ′ are only dependent on Σ ′ and d and their obtention is general for any plane. The last step is to recover Q ′ as Qin the original reference frame. This requires to first undo the diagonalizing rotation from (3.24) and then undo the rotation from the original reference frame to the virtual reference frame V: Q=RVR4 z(α)Q′R4 z(α)TRT V,(3.27) R4 z(α) = hRz(α)03×1 01×31i,RV=hR1(m)R1(m′)T03×1 01×31i,(3.28) where R 1 ( · ) is a rotation matrix used to generate the orthonormal basis of the tangent space at a given orientation, see [83, eq. (67)]. The resulting quadric Qis now valid for the original reference frame. The proposed method enables approximating Qin a closed form, without expensive computations, and with an accuracy multiple orders of magnitude higher than other alternatives, see Section 3.5.4. The quadric should provide flexibility to facilitate the 74 Probabilistic Meshes for Long-range LiDAR Mapping next decimation step. This is done by simply increasing the value of Σ ′ 3,3 , so that the quadric can accomodate vertex contractions within small perpendicular distance to the represented plane, bringing flexibility during the posterior decimation. Iterative Decimation The resulting updated map mesh is decimated using iterative edge contraction as in Section 3.4.1. The quadrics obtained in Section 3.4.3 provide enough plasticity to allow decimation with no additional considerations. Plane Aggregation The planes of the different faces that have been merged during decimation are aggregated. To this purpose, we use the clustering technique presented in Section 3.4.3, but using it to merge multiple planes at once. Finally, Map Update can also include other operations and refinements. They include: vertex pair contraction to join vertices close to each other [122]; recomputation of the clusterization to search for potentially overlooked matches; or removal of possible erroneous faces (e.g., faces which are inconsistent with the current state and LiDAR input) through rasterization [129,130] with the new scans and the map mesh. 3.5 Methods Validation This section experimentally validates the proposed modules and compares them against a MonteCarlo-based ground-truth and against alternative methods in the state of the art. The validations in this section are focused on the usability and correctness of the proposed methods, while those in Section 3.6 analyze the effect of integrating probabilistic meshes in a mapping scheme. This section is divided in four parts with independent validations of the modules. All the given computation times were obtained in a consumer laptop with an Intel i7-8565U CPU and with 16 GB of RAM, using C++ implementations of the methods, that we make available here. 3.5 Methods Validation 75 3.5.1 Plane Initialization & Point-Plane Contributions In module Plane Initialization we proposed an on-manifold method to compute the perturbation covariance Σ m of a plane mthat bests represents (minimizes the distance to) a set of at least 3 points with noise. In many existing methods [29,35,37,110,127] the mean value of mis estimated with high accuracy using SVD, but leaving the open question on how to estimate its corresponding covariance Σ m . The value of Σ m is of high importance, since later in module Point-Plane Contributions, the different point-plane pairs are used to obtain distance measurements d and their covariances Σd, that are used in the motion estimation. In this validation we analyze different ways of computing Σ m , using them to obtain the distance measurements covariances Σ d , and comparing the resulting Σ d with a ground-truth covariance ΣdGT . Hence, this experiment validates both the Onmanifold method used in Plane Initialization and the models proposed for Point-Plane Contributions. Four methods for considering Σ m are analyzed: 1) our On-manifold method presented in Section 3.4.1; 2) No uncertainty, that assumes no uncertainty in m(i.e., Σ m =0) and is used by approaches such as [35,127]; 3) SVD-based, that computes Σ m from the result of the SVD algorithm but discarding the on-manifold nature of mand is used by approaches such as [29, 37]; and 4) Isotropic (used by approaches such as [110]) that does not consider Σ m , but directly estimate Σ d from the lowest eigenvalue of the SVD solution and the sensor distance variance Σ ∥p∥ considering them as Isotropic Gaussian distributions. The approaches are evaluated with data collected in a series of mapping flights (see Section 3.6) using a Livox Horizon LiDAR with a scanning rate of 240k pts/s at 10Hz. From each flight we processed between ∼ 200-700 randomly-selected scans in different types of scenarios that included flat soil, irregular soil with different inclinations from agricultural grooves, buildings, concrete roads, and buildings, among others. During the flights, the planar points ( ≈ 20k in each scan) are extracted and used in Mesh Generation as described in Section 3.4.1. The LiDAR error characterization considers both range ( σ =2 cm) and angular error ( σ =0.05 ◦ for each angle) [29]. The generated meshes for each scan (after execution of Iterative Decimation) were generally composed 76 Probabilistic Meshes for Long-range LiDAR Mapping of 4k-5k faces that mostly represented from N =3 to N =30 points each. Only around 1% of the faces represented more than 25 points. For each planar point in a scan, we obtain its N closest neighbors in the planar points subset of that scan (obtained with [73]), and compute (using SVD) the plane mthat best represents those N points, and the 4 covariance formulations from the above compared methods. From that point and plane, we compute the estimated point-plane distance d , and the different covariances Σ d resulting from the compared formulations. To obtain a ground-truth for d and Σ d , we perform a MonteCarlo simulation that generates between 10 4 and 10 7 random sets of the planar point and its N neighbors (until the ground-truth covariance Σ dGT converges with less than 1% variation), using their respective probability distributions. Finally, we compare each Σ d with the ground-truth Σ dGT . Although not shown in this analysis, the errors in d obtained by each formulation with the ground-truth were of millimeter level and were considered negligible, showing that SVD provides an accurate estimation of m, validating its widespread use in the state of the art. Table 3.1: Comparison of point-plane distance covariance formulations with N points. No uncertainty [35,127] SVD-based [29] Isotropic [110] On-manifold (ours) Ne Σde Σde Σdte Σdt 3 0.060 1.339 0.042 0.8 0.037 1.0 4 0.032 0.741 0.026 0.8 0.011 1.0 5 0.018 0.460 0.021 0.8 0.005 1.0 10 0.003 0.208 0.026 0.9 5e-4 1.2 30 0.001 0.068 0.068 1.1 3e-5 2.1 e Σd: = DB (Σ dGT , Σ d ) given the covariance Σd computed by each method using N points. Computation times (t) are in µs. The first 3 methods have similar times, and their average values are given. Table 3.1 shows the average errors (measured with the 2nd term of the Bhattacharyya distance, as in (3.1) ) between the Σ d estimated by each method and the ground-truth Σ dGT . Our method allows to significantly better resemble the groundtruth covariance of d , giving a Bhattacharyya distance to Σ dGT between 12% and 97% lower than the second best solution in each case. This high accuracy avoids errors in the estimation of the robot motion and its covariance. In addition, our method is 3.5 Methods Validation 77 very efficient (most faces represent from 3 to 10 points only), involving a negligible processing time when all the faces in a mesh are considered. In these experiments, the mean time to compute all the planes in each scan’s decimated mesh of ≈ 4k-5k faces was 3.8ms using a single thread of the processor. 3.5.2 Similarity Computation In module Clusterization at Section 3.4.3 we proposed a novel similarity metric to determine if two adjacent clusters C1 , C2 represented by planes m 1 ,m 2 with covariances Σ m1 , Σ m2 can be considered as one and hence, merged. The metric performs 2nd-order propagation over a non-linear function γ that represents the cosine of the angle between m 1 ,m 2 . Then, it uses the 3 σ confidence interval to determine if γ has high probability of being lower than a given threshold γjoin (i.e., γjoin ≤γ− 3 σγ ). Given this condition, the angle between m1,m2can be considered small enough for merging C1and C2. The value γjoin is a design parameter that may be changed by the user to enforce a more restrictive or permissive clustering. Thus, we are interested in assessing the validity of the proposed metric for any given γjoin , and validate it against a MonteCarlo-based ground truth for all possible values of γjoin ∈ [0 , 1], and for all possible angles α∈ [0 , 90] deg between n 1 and n 2 (the direction vectors of m 1 ,m 2 ). As explained in Section 3.4.3, the planes found in our experimental datasets all have covariances such that the sum of their eigenvalues lies in the interval (0 , 4]. Thus, we also validate the proposed metric for any possible Σm1, Σm2in that interval. To compute the ground truth for any two planes m 1 ,m 2 with covariances Σ m1 , Σ m2 , we perform a MonteCarlo simulation to obtain a sampling distribution for the angle γ between those planes. We generate random plane-pair samples, and compute the angle γ between the planes in each sample. Generally, between 10 4 and 10 5 samples are sufficient to properly represent the sampling distribution, such that its standard deviation converges with less than 1% variation. Next, given a value for γjoin , we measure the probability that γjoin ≤γ using the sampling distribution. If that probability is greater than 95%, the planes are considered similar. The metric is validated against the ground truth considering it as a binary classifier. For either the 78 Probabilistic Meshes for Long-range LiDAR Mapping ground truth or the proposed metric, we mark the similarity as True if the planes are considered similar, and as False, otherwise. Next, the Accuracy obtained by the binary classifier is calculated. Figure 3.4-top shows the obtained Accuracy values for any combination of γjoin and α . Each of the Accuracy values was obtained from a total of 10 4 setups of random combinations of Σ m1 and Σ m2 , modifying their maximum eigenvalues, the ratio between eigenvalues, and the correlation of each covariance (i.e., by rotating the covariance matrices). As shown, the proposed metric provides an Accuracy of ∼ 99% for any γjoin > 0 . 9, and of ∼ 98% for any γjoin > 0 . 8, which corresponds to an angle between m 1 ,m 2 of up to arccos (0 . 8) ≈ 37 ◦ . We consider this as a wide enough range for γjoin . Moreover, Accuracy values of > 95% are attained for almost all values of γjoin . Figure 3.4-bottom shows the lower Accuracy values obtained when using a 1st-order approximation of γ , which justifies the use of a 2nd-order propagation. For any γjoin > 0 . 8 the proposed metric also showed a Recall of ∼ 100% and a Precision of ∼ 99%, showing a considerable robustness against false negatives and false positives. The computation of the metric is very lightweight, only requiring 0.16 µ s per plane pair. A typical clustering of a (large) mesh involves a total of ∼ 20k comparisons, requiring only 3.18ms for computing the metric in all pairs in a scan. Figure 3.4: Top) Accuracy of the proposed plane similarity metric (based on 2nd-order propagation) against a MonteCarlo-based ground truth. Bottom) Accuracy of an alternative metric using 1st-order propagation. 3.5 Methods Validation 79 3.5.3 Plane Cluster Aggregation In module Clusterization at Section 3.4.3 we proposed a method to find the plane m ∗ that best resembles C1∪C2 , the union of two adjacent clusters represented by their planes m 1 ,m 2 with covariances Σm1 , Σm2 . Next, we validate the ability of this method to correctly resemble successive cluster unions. The accuracy of each cluster union could affect the similarity metrics that are then computed using them. Hence, we also validate the similarity metric when the planes and covariances come from the aggregated clusters. The validation is performed with experimental data from different flights, see Section 3.5.1. For each scan, we generate and decimate the mesh, and compute all the planes corresponding to its faces using Plane Initialization. Then, clustering, see Section 3.4.3, is performed. First, the similarity metric for each pair of adjacent clusters C1 and C2 is obtained. If its value is below γjoin (chosen as γjoin =0 . 94 for all our experiments), the clusters are aggregated into C1∪C2 , and the resulting plane m ∗ and its covariance are computed using the proposed plane-plane optimization method, see Section 3.4.3. The sets of points P1 and P2 are joined into P1∪P2 , corresponding to the joined cluster. These points will be removed once the mesh is integrated into the map. Finally, the list of clusters is updated, and the clustering procedure continues. We compare our method (denoted as Plane-Plane EKF in this section) against two point-based alternative methods. Method SVD re-estimate uses the joint set of points P1∪P2 to re-compute the aggregated plane and its covariance from scratch, using the method proposed in Section 3.4.1. Given the accuracy evaluation presented in Section 3.5.1, this method can be considered as a ground truth to obtain the merged plane. This method (with a different plane uncertainty formulation) is used in works such as [29] to update spatial partitions. The second alternative method, Point-Plane EKF, which is derived in Appendix A.2, uses the set of points P2\P1 to update the plane and covariance in C1 , using an update stage of an EKF. If the set P1\P2 is lower in size, the method uses it instead to update C2 , thus reducing the computational cost. These two alternative methods are based on points, and require significantly higher memory consumption than our approach. 86 Probabilistic Meshes for Long-range LiDAR Mapping Table 3.3 shows the mean and uncertainty errors ( e x and e Σx ) averaged across all tests in each of the scenarios. FAST-LIO2 delivers good accuracy in the state mean error since the N nearest closest points it uses to represent a map plane naturally tend to keep the mean displacement close to 0, even when not desired (recall Figure 3.1). However, the covariance estimation in FAST-LIO2 is the least accurate in scenarios with low geometrical information. In contrast, VoxelMap and Puma-LIO achieve better covariance estimation but with a higher error in the mean. All these three methods underestimate the covariance in these scenarios. In scenarios with rich geometrical information (”All axes” in Table 3.3), all methods provide a good estimation in mean, but voxel-based methods slightly overestimate the covariance due to the overparametrization of the out-of-manifold plane (VoxelMap), or due to the assumption of isotropic and lower-bounded point-to-plane distance covariance (Puma- LIO). On the other hand, for PUMA the mean error lies in between the first three methods (although worse than FAST-LIO2), but covariance estimation is not available since the framework is non-probabilistic. Finally, the proposed method delivers the best performance in terms of mean and covariance accuracy in all scenarios, with the exception of the rotation scenario where the difference in mean error with FAST-LIO2 is marginal. These results confirm the better estimation accuracy of the proposed method for scenarios with low geometrical information. Table 3.3: Results of compared methods in scenarios with different map geometries. FAST-LIO2 [35] Voxel-map [29] PUMA-Lio [110] PUMA [26] Prob. Mesh (ours) Type of scenario / Error metrics e xe Σxe xe Σxe xe Σxe xe Σxe xe Σx One axis (translation) 0.003 5.571 0.089 1.090 0.069 1.067 0.013 - 1e−4 0.006 Two axes (translation) 0.001 2.821 0.004 2.639 0.172 1.377 0.007 - 4e−4 0.041 All axes (translation) 0.001 0.009 0.001 0.508 0.010 1.166 0.009 - 0.001 0.002 Two axes (rotation) 4e−43.215 0.051 0.808 0.029 0.842 0.001 - 0.001 0.033 For translations, e x:=∥tGT −test∥(m). For rotations, e x:= Log(RT GT Rest) (rad). Always, e Σx:= DB(ΣGT ,Σest). We have compared the proposed method with point, voxel, and mesh-based methods with different map uncertainty representations in each case. While PUMA is also mesh-based, its map is computed differently than ours and does not consider uncertainty in its formulation. Thus, we still must determine whether the increased performance of our method is due to the proposed mesh-based formulation of the map 3.6 System Validation 87 or if the inclusion of on-manifold map uncertainty also plays a relevant role. To do so, we implement the 4 different uncertainty formulations described in Section 3.5.1 into the proposed probabilistic-mesh-based mapping scheme. Since the clustering scheme in Section 3.4.3 uses the proposed on-manifold map uncertainty for the similarity metric computation and for the cluster merge, we need to adapt it to the uncertainty models of the compared methods. Methods FAST-LIO2,Puma-LIO, and PUMA do not consider map uncertainty, and thus we compute the similarity metric using the mean direction vectors of each pair of compared clusters and considering a small fixed covariance Σ n = diag (0 . 03 , 0 . 03) 2 .VoxelMap considers uncertainty in the map, but this uncertainty does not consider the on-manifold nature of the planes and when used in the propagation of (3.13) it results in much higher estimated angles between planes, impeding the successful clustering of the mesh. Thus, we compute the similarity in the same way proposed for the rest of the methods. For all methods, we perform the cluster merge using the SVD re-estimation approach described in Section 3.5.3. Since this approach proved as the most accurate for performing cluster merge (at the cost of high memory and computational consumption), we consider it as a favorable comparison for the analyzed methods against ours. Figure 3.8 shows the ablation study for the different types of scenarios presented. Errors in the state mean are shown in the horizontal axis and errors in the state covariance (measured using the 2nd term of the Bhattacharyya distance, as in (3.1) ) are shown in the vertical axis. It can be seen that all mesh-based methods achieve significant mean and covariance accuracy (generally better than point, voxel or nonprobabilistic mesh-based methods in Table 3.3). However, using the on-manifold map uncertainty formulation consistently delivers the best results in all scenarios. Thus, we can conclude that both the chosen map representation and map uncertainty model play a significant role in the accuracy in state estimation. Next, we will analyze the behavior of the proposed method in real scenarios to further validate these conclusions. 88 Probabilistic Meshes for Long-range LiDAR Mapping Figure 3.8: Ablation study of the different map uncertainty representations in the studied scenarios using the proposed mesh-based mapping scheme. 3.6.2 Experimental Validation The proposed components and methods were experimentally validated in sets of long-range powerline mapping flights performed with two versions of LR-M aerial robot developed at the GRVC Robotics Lab, one based on DJI Matrice 600 (see Figure 3.9-left) and one based on a custom platform using a PX4-based autopilot. LR-M is equipped with a Livox Horizon 3D solid state LiDAR pointing at a pitch angle of -40º as main mapping sensor. That LiDAR has a field of view of 81.7º × 25.1º, a detection range of 260 m, a range standard deviation of ∼ 2 cm, and an angular precision of 0.05°. In addition, LR-M was endowed with a GPS receiver, an IMU, and a Jetson NVIDIA NX Xavier for logging. More than 60 Beyond Visual Line of Sight (BVLOS) flights were performed to validate the proposed method in different scenarios in which the robot traversed areas with different geometrical richness level, see Figure 3.9-right. ALCALA (a rural area) and ATLAS (an olive crop) were the geometrically richest scenarios, with eventually small sections where only few elements are at view. In contrast, Burguillos (a flat crop field without any vegetation) was the geometrically poorest. Two flights with different trajectories were analyzed for each scenario (the dataset of these flights is described in Appendix B.1.2). In addition, the analysis was 3.6 System Validation 89 Figure 3.9: LR-M aerial robot used in the experiments (left) and scenarios Alcala (right-top), ATLAS (right-center), and Burguillos (right-bottom). also performed with the KITTI LiDAR odometry dataset, [139], which was recorded with a Velodyne HDL-64E LiDAR. In this section, the proposed mapping method is compared with all the methods analyzed in Section 3.6, except for PUMA-Lio, which code is currently not available. For PUMA, the FoV was adapted to accommodate the Livox Horizon and the reprojected image size (see [26]) was proportionally rescaled to 256x64 px. Our method was implemented on top of FAST-LIO2 and relies on it for the processing non-planar points. Table 3.4 summarizes the absolute trajectory and rotational RMSE (ATE/ARE) in m and degrees for all the analyzed methods in the KITTI dataset (average results in scenarios 0-10) and in the provided long-range mapping missions. The KITTI dataset is consistently rich in geometrical features and all methods perform with high accuracy, except PUMA, which despite having a low relative error as shown in [26], it is overall affected by drift and results in higher ATE/ARE. Differently to PUMA, methods Voxel-map,FAST-LIO2, and Ours estimate the robot state using an EKF scheme, which i) is more robust to drift than the pure scan-to-mesh registration 90 Probabilistic Meshes for Long-range LiDAR Mapping used by PUMA and ii) allows integrating other sensors such as an IMU, which adds additional information. For the long-range mapping scenarios ALCALA and ATLAS, methods Voxel-map and Ours consistently deliver small ATE/ARE values. In contrast, FAST-LIO2 and PUMA are more prone to drift in flight sections with poor geometrical information, which happens e.g., in ALCALA-F1 near second 455 and ATLAS-F2 near second 265. FAST-LIO2 only considers a local map representation (nearest neighbors to each point), and also considers covariance only in the point-to-plane error and not in the map. Moreover, to tackle the high amount of map points, it uses a voxel filter of 0.5 m to reduce the map size, which prevents the method from capturing small geometrical details, inducing drift in sections with poor geometrical information (most notably in ATLAS-F2). PUMA generates non-probabilistic meshes from planar and non-planar LiDAR points. These meshes are not intended for representing irregular terrain and of non-planar objects such as trees, electric towers, and powerlines in these scenarios. Instead, our method, using probabilistic decimation and clustering, naturally adapts to irregular terrains conserving the present small geometrical details, and also rejects non-planar points even if they were initially introduced in Mesh Generation (since they end up outside of any cluster) as explained in Section 3.4.1 and Section 3.4.3. In Burguillos flights, the robot traverses multiple areas where the LiDAR FoV only captures a crop field with almost no vegetation and a section of a powerline. The captured information is not enough to correctly estimate the robot motion, and all the compared methods (including Ours) fail if using solely LiDAR inputs. IMU measurements (used in Voxel-map,FAST-LIO2 and Ours) are generally noisy and were not sufficient to maintain a correct estimation in these situations. To further tackle this problem, for the Burguillos flights we fused GPS measurements into all the compared methods using an open-source tightly-coupled GPS and odometry fusion method [140]. For PUMA, which does not provide an output covariance, a fixed standard deviation of 2 cm was used for all axes. The obtained ATE/ARE in all the methods are shown in Table 3.4. Figure 3.10 shows the evolution of the resulting trajectories for all methods in Burguillos-F1 considering the GPS-odometry fusion. It can be seen that, even after fusing the GPS measurements, all methods except 3.6 System Validation 91 Table 3.4: Trajectory/Rotation RMSE of compared methods in scenarios with different map geometries. FAST- Voxel- PUMA Ours LIO2 [35] map [29] [11] KITTI (avg) 1.81/4.92 1.24/2.93 22.14/3.01 1.36/2.71 ALCALA-F1 12.93/137.02 1.05/2.74 1.56/121.61 1.11/2.65 ALCALA-F2 0.47/5.64 0.41/5.71 5.15/115.22 0.42/5.43 ATLAS-F1 20.92/12.13 0.79/3.57 22.21/119 0.84/3.47 ATLAS-F2 90.83/165.81 9.66/9.87 11.39/120.51 3.04/5.14 Burguillos-F1* - - - 0.37/4.16 Burguillos-F2* - - - 0.98/6.35 Trajectory RMSE expressed in m, and rotation RMSE, in deg. * Only these flights integrated GPS measurements. Ours diverge when the robot enters geometrically poor areas of the scenario. In these areas the methods highly underestimate the covariance of the robot localization, and thus do not allow the GPS measurements to compensate for the (increasingly higher) error in the mean robot localization. Our method, as shown in Table 3.3, naturally avoids the underestimation in the robot covariance and enables correct fusion of GPS measurements. The maps generated by our method in Alcala-F1 and ATLAS-F2 are shown in Figure 3.11-left. These maps include the meshes obtained from planar points and also the mapped non-planar points (in red color) using FASTLIO2 as explained in Section 3.3. By naturally meshing and decimating probabilistic planes, and separating them from non-planar points, the proposed scheme reduces the size of the map in the areas where its quality is less affected, achieving high reductions while preserving details. Figure 3.11-right shows the evolution of the map size along flights Alcala-F1 and ATLAS-F2 in each of the compared methods. The resulting map size of our method is comparable to that of FAST-LIO2 (which leverages a big 0.5 m voxel filter in its map representation), while still retaining fine details. For fights with more planar sections (ALCALA-F1), our method results in an even lower map size than FAST-LIO2. 92 Probabilistic Meshes for Long-range LiDAR Mapping −50 0 50 100 150 x [m] −200 −150 −100 −50 0 y [m] Fast-LIO2 Voxelmap PUMA Ours Groundtruth Figure 3.10: Trajectories of all the compared methods in Burguillos, augmented with tightly-coupled GPS fusion. In this Section we did not provide computational comparisons between the methods. As shown in Section 3.5, all the proposed modules in our method are lightweight and able to run in real time. However, our full mapping method includes multiple parts (mesh decimation and merging, triangulation, etc.) from third-party implementations (with usually multiple available options) and the study of the best combination of these methods and libraries is out of the scope of this PhD Thesis. 3.7 Conclusions This chapter presented the necessary modules for integrating probabilistic meshes to represent locally planar surfaces in traditional LiDAR-based mapping schemes, enriching existing methods while still giving high flexibility for utilizing other sensors and map representations. The chapter corresponds with the Contribution 2 of this PhD Thesis. The proposed modules are developed using the on-manifold formulation of planar objects, providing consistent probabilistic representations of them while still being efficiently computable. Moreover, the intrinsic iterative decimation in our scheme 3.7 Conclusions 93 Figure 3.11: Obtained maps with our method and evolution of the map size overtime with the compared methods in Alcala-F1 (top) and ATLAS-F2 (bottom). Map size is computed as follows: for Voxel-map, size of all the probabilistic planes and the probabilistic points in all octrees; for FAST-LIO2, size of all the points in the kd-tree; for PUMA, size of all the vertices and faces in the mesh; for our method, size of all the vertices and faces, all the probabilistic planes associated to the faces, all the cluster masks, and all the non-planar points (also seen separated in red). For all methods, only the size of the upper triangular part of the covariance matrices is considered. additionally produces maps that are very low in size but still capture fine details in the geometry of the scenario. The developed modules allow to: i) efficiently initialize new meshes and associate uncertainty to each of their faces; ii) consider map uncertainty in the motion estimation of the robot; iii) associate and clusterize sets of planes that are similar to each other; and iv) merge imperfect 3D meshes via cluster matching and 2D aggregation, later recovering and refining the 3D mesh via iterative decimation. All the proposed modules have been experimentally validated, both individually and in a full mapping scheme in sets of long-range mapping scenarios, targeted at powerline inspection, evidencing the usability of the proposed approach, and the importance of the chosen map structure and the consideration of map uncertainty for scenarios with a low amount of geometrical features. 94 Probabilistic Meshes for Long-range LiDAR Mapping Chapter 4 Reactive LiDAR-based Mapping for Powerline Inspection 4.1 Introduction Electric power distribution systems are intricate structures that can extend for over thousands of miles across a whole country, or even millions of miles in the case of lowvoltage powerlines [141]. These systems also involve other structures such as electric towers or transformers, and require mandatory periodic inspections. Among other tasks, vegetation growth must be monitored by extracting accurate measurements of the distances between powerlines, and between vegetation and powerlines and electrical towers. Traditionally, these inspections have been conducted manually by teams of workers, involving extensive manpower and work hours in highly dangerous environments. As a faster and less dangerous solution, aerial robots equipped with highly accurate 3D Light Detection And Ranging (LiDAR) scanners and cameras have become widely used for this task [142–144]. Aerial robots generally perform inspection by following a predefined path that covers the extent of the infrastructure, gathering data from LiDARs, cameras, and other sensors. Afterwards, the data is analyzed offline in the lab/office, generally days after performing the flight. This double-stage procedure is very inefficient and in many cases requires repeating the flights if during the offline analysis the collected data is 95 102 Reactive LiDAR-based Mapping for Powerline Inspection a timestamp of when it was measured, but this information can be disregarded if the pointcloud is undistortioned, which is a general procedure in LiDAR-based systems [21,35]. The reflectivity value rj is a measure of the ability of a surface to reflect radiation. This is obtained as ratio between the reflected and emitted energy of the LiDAR, after performing distance correction among others. The reflectivity is thus a measure of the efficiency of an object to reflect incident light. It depends on the material of the object that caused the reflection, its physical and chemical state, its surface roughness and disposition, and also the incidence angle between the object and the LiDAR beam. The chosen LiDAR sensor (Livox Horizon) emits in a wavelength of 905 nm . At this wavelength, metals have in general high reflectivity [168]. In contrast, trees and soil typically have medium to low reflectivity [169, 170]. However, after performing experiments with the chosen sensor it was observed that the geometry of the different objects had more impact in the measured reflectivity than the material they ware made from. Powerlines and towers present severely lowered reflectivity properties (i.e.: powerlines are thin, and reflecting only a fraction of the diffused LiDAR ray, and they are also made of braided cables, which are worse reflectors than plain aluminium). In general with this sensor, powerlines offer the lowest reflectivity, followed by towers, trees and soil. However, these elements could not be separated solely by their reflectivity: powerlines and towers present similar values, but different sections of the towers have similar reflectivity as trees, and trees have very low reflectivity in their borders, but have higher reflectivity at their center, similar to the values for the soil. A more complex analysis is needed to properly discriminate between these classes. Figure 4.2 shows the scheme of the proposed Pointcloud Segmentation module, divided in three main blocks. First, in Reflectivity Analysis the input pointcloud is divided in two sets, MS and OS.MS contains the points that likely correspond to classes Powerline and Tower, whereas OS contain the points likely to correspond to Soil and Vegetation. Afterwards, in Metallic Points Processing, points in the MS set are classified into powerlines and towers, using Principal Component Analysis (PCA) 4.3 General Description 103 and region growing techniques. Likewise, in Organic Points Processing, points in the set OS are classified into vegetation and ground. Reflectivity Analysis Optimal Thresholding Spatial Filtering Metallic Points Processing Organic Points Processing High Ref. Points Low Ref. Points Region Growing Cluster PCA Unlabelled Clusters Local PCA Modified Reg. Growing Labelled Points LIDAR scan Powerlines Towers Soil Vegetation MS OS Figure 4.2: Scheme of the proposed Pointcloud segmentation module. ©IEEE 2024 Reflectivity Analysis Figure 4.3 shows a histogram of the reflectivity values in a pointcloud obtained at the time of Figure 4.4. It can be seen that the histogram is multi-modal, having two clear main modes. The lower mode corresponds to metallic points (powerlines, electric towers), whereas the higher mode corresponds to organic points (vegetation, soil). To separate points between these categories, a threshold T can be used. Points with reflectivity lower than Tare classified as metallic, and as organic otherwise: rj organic ≷ metallic T(4.1) The value of T can be automatically obtained by leveraging an Optimal Thresholding scheme. Optimal Thresholding selects the value of T from all possible values of r (in our case, r∈ [0 , 255]) that minimizes the probability of erroneously classifying points. In the studied case, the reflectivity histogram can be modeled as the sum of two distributions, p0 ( r ) and p1 ( r ), corresponding to each class ω0 and ω1 , i.e., h ( r ) = P0p0 ( r ) + P1p1 ( r ), where P0 and P1 are the a priori probabilities of ω0 and ω1 . 104 Reactive LiDAR-based Mapping for Powerline Inspection Figure 4.3: Reflectivity histogram of the points cloud shown in Figure 4.4. ©IEEE 2024 Given these conditions, the value Tis: T= min TE(r), E(r) = P1Z1 r p0(z)dz +P0Zr 1 p1(z)dz (4.2) Assuming Gaussian distributions for p0 ( r ) and p1 ( r ), T can be simply computed using the method in [171] as follows: AT2+BT +C= 0 A=σ2 0−σ2 1 B= 2µ0σ2 1−2µ1σ2 0 C=σ2 0µ2 1−σ2 1µ2 0+ 2σ2 0σ2 1log(P1σ0/P0σ1), (4.3) where µ0 and σ0 represent the mean and variance of p0 ( r ), and µ1 and σ1 , those of p1(r). As long the histogram has two modes (i.e. in LiDAR scans that contain a significant number of metallic points), the value of T is valid. Although powerlines are present in the great majority of LiDAR scans, in very few cases the number metallic points may not be sufficient to originate a mode in the reflectivity histogram. In such situations, the Optimal Thresholding method could fail, and the value of T falls back to a default number. Given that the reflectivity values of the materials present in the envisioned scenarios are rather steady in normal conditions, assigning an static value of T for 4.3 General Description 105 cases with low number of metallic points is a satisfactory approach. Moreover, the next performed steps in the semantic classification compensate small errors in T. Both histogram modes can have overlapping, such as in the case of the borders of vegetation, where points can have a low reflectivity value. These miss-classified points are in general distributed sparsely in the 3D environment. Hence, a spatial filter is used to remove isolated miss-classified points. For every metallic point p j , we extract its local neighborhood. If the number of organic points in that nighborhood is lower than a threshold, p j is considered miss-classified and is thus removed. The same is applied for isolated organic points. Using this simple local spatial filter, miss-classified points are removed successfully. The outputs after this filter are the points sets MS and OS, containing the points related to metallic objects and organic objects. Figure 4.4-top shows one visual image collected in a powerline inspection flight. Figure 4.4-bottom shows the results of the execution of both steps of Reflectivity Analysis on the LiDAR scan. Points in MS are shown in green, and points in OS, in blue. Red points are miss-classified points, removed using the described spatial filtering. Metallic Points Processing Points in MS as caused by powerlines or electrical towers. The first step to classify them is to assign each point in MS to classes Powerline and Tower using geometry. The points corresponding to powerlines are locally distributed in only one direction, while those corresponding to towers are typically arranged in two or in three different directions. To leverage this different geometrical arrangements, a local Principal Component Analysis (PCA) analysis is performed at the neighborhood of each point in MS . For every point p j , its N nearest neighbor points are identified. Then, PCA is applied to the resulting point set. PCA returns a set of eigenvalues, which have different ratios for points belonging to powerlines and to towers. Moreover, the minimum eigenvalue for points corresponding to powerlines is typically bounded below a very small threshold. Taking all of this into account, every point p j is classified following Algorithm 4.1. 106 Reactive LiDAR-based Mapping for Powerline Inspection Figure 4.4: Top) Visual image collected in one powerline inspection flight. Bottom) Resulting points after Reflectivity Analysis. Points in MS are shown in green. Points in OS are shown in blue. Miss-classified points are in red. ©IEEE 2024 Once the algorithm finishes, the points assigned as Powerline and Tower are clustered using a region growing method. We used the region growing algorithm adapted to use the Mahalanobis distance as described in [172], in order to make it robust to irregular cluster shapes. The output of this method are clusters of points classified as Powerline and Tower. The proposed scheme uses simple properties, but executes with low computational burden and is robust given a high enough number of neighbors N (between 5-10). The proposed spatial filtering can lead to small errors in the border area between powerlines and towers (see an example in Section 4.4), but this is of low relevance in our application. Figure 4.5 shows an example of the output of Metallic points Processing on the MS coming from Figure 4.4-bottom. A total of one Tower cluster and 3 Powerline clusters were segmented. 4.3 General Description 107 Algorithm 4.1 Metallic Points Processing 1: function Discriminate Points(MS) 2: for each pjin MS do 3: Get N neighbors of pjas n1,n2...nN 4: λ= PCA(n1,n2...nN) 5: if (λ1>R1λ2)&(λ2>R2λ3)&(λ3>TH1)then 6: Label pjas Powerline 7: else 8: Label pjas Tower 9: return MS, labels Figure 4.5: Results of the Metallic points Processing in the MS in Figure 4.4. A clusters of Tower points is shown in red. Three clusters of Powerline points are shown in green. ©IEEE 2024 Organic Points Processing Points in the OS set represent vegetation and soil present in the scenario. The objective is to classify them into clusters of points classified as Vegetation or Soil. Points in the OS are generally more irregularly distributed than those in the MS . Thus, a different approach is used for this categories. First, the region growing algorithm described in [173] is applied. The output of the algorithm is a set of clusters of points representing different vegetation (e.g., trees, bushes) or soil. Next, in each of these clusters, a PCA analysis is performed. Clusters originated by soil follow a planar distribution, resulting in a low relative value for their smallest eigenvalue. In contrast, clusters originated by vegetation generally resemble a 3D distribution, with high eigenvalues across all directions. Using this information, clusters with high 108 Reactive LiDAR-based Mapping for Powerline Inspection planarity (i.e.: ( λ1−λ2 ) /λ3> R3 , with R3 being a design parameter) are classified as Soil, and otherwise as Vegetation. Figure 4.6 shows an example of the execution of both steps of Organic points Processing on the point set OS shown in Figure 4.4-bottom. In this example a total of 14 Vegetation clusters and one Soil cluster were segmented. Figure 4.6: Results of the of Organic points Processing in the OS in Figure 4.4. Vegetation clusters are shown in yellow, whereas the Soil cluster is shown in blue. ©IEEE 2024 4.3.2 Multisensor Mapping This module builds in real time a map of the current area inspected by the robot. The module mainly uses the onboard LiDAR and IMU systems to estimate the current position of the robot and generate a 3D map of it, but also allows to integrate into the map the visual (texture) information coming from a camera, and the semantic information coming from the Pointcloud Segmentation module. To decease the computational burden of the module, texture and semantic information are not considered in state estimation, but are integrated into the map to be used for powerline inspection tasks. Additionally, the module can consider optional positional information coming from a GNSS receiver. The module is built with 4 main requirements in mind: • Low error. The generated map should have sufficient accuracy for performing a preliminary geometric and visual analysis of the powerline corridor with it, and to allow the detection and avoidance of possible obstacles in the scenario, being fed to the Trajectory Planning module for this purpose. 4.3 General Description 109 • Robustness to different types of scenarios and vegetation, with potentially low geometrical features. The method can take advantage of GNSS signal reception but must be functional in the absence of it. • Real-time execution. The module should involve moderate computational consumption, to be executed together with the whole proposed system onboard an aerial robot. • Low memory footprint to function in large-scale scenarios that are typical in power line inspection. The module also must be compatible with LiDARs with a non-repetitive scanning pattern (such as the Livox Horizon used for this work). Different state-of-the-art mapping softwares were analyzed to use the most suitable as a basis for the proposed Multisensor Mapping module. A total of 4 state-of-the-art LiDAR-based mapping softwares, all available at February 2022, were analyzed: LOAM-Livox [46] is an adaptation from LOAM [21] to consider Livox LiDARs. A further development to perform loop closure detection and correction is given at [174]. Livox-Mapping [175] is a similar solution, but using a tailored feature extraction module. LiLi-OM [70] also provides a novel feature extraction method, and leverages a series of three tightlycoupled optimization problems to perform state estimation. FAST-LIO2 [35] builds on top of an error-state on-manifold iterative Kalman Filter (IKFoM ) state estimation scheme and uses efficient data structures such as incremental kd-trees to highly reduce the computational burden of mapping. We analyzed the performance of all these mapping softwares in different flights conducted in the target powerline inspection scenarios (a dataset of multiple of these flights is described in Appendix B.1.2), which are described in Section 4.4. In all cases, FAST-LIO2 offered the highest robustness and accuracy, and was able to operate in real time during the whole duration of the flight. This is mostly due to two reasons: i) it is able to use all of the points of each scan without computing intermediate features, leveraging finer geometrical details that when using a feature extractor; and ii) it has a low memory and CPU footprint, running in real time without discarding sensor data, resulting in a finer position estimate than the other softwares. 110 Reactive LiDAR-based Mapping for Powerline Inspection To integrate texture information in the obtained map, each LiDAR scan is reprojected in the camera coordinate frames and colored with the RGB values of the corresponding pixels at that time. Points with no corresponding pixels, are assigned with a null color. The extrinsic camera-LiDAR calibration was obtained using a checkerboard 1 , and the intrinsic calibration of the camera was obtained with [176]. Semantic information from the corresponding LiDAR scan is directly appended to the mapped points with no additional requirements. The target scenarios present situations with almost no geometrical features, preventing this method to function robustly. This topic was extensively addressed during Chapter 3. To cope with the lack of geometrical information, this work includes methods for strengthening the functioning of the mapping solution by: i) integrating map uncertainty in the local representation of the map computed by FAST-LIO2, and ii) integrating GNSS measurements (when available) directly in the state estimation of the scheme, deriving the effect of these measurements in the employed IKFoM filter. They both are presented in the following. Integration of Map Uncertainty Assume the state of the robot to be fully represented by x= [t L W, R L W ] ∈R3×SO(3) , which is the current pose of the LiDAR in the world frame W . When a new LiDAR scan PL is received in the inertial frame L , the current robot pose can be estimated by minimizing the distance from each point in PL to the map. The chosen mapping framework, FAST-LIO2 [35] considers that all points in the map represent locally planar surfaces. Thus, for each point p L∈ PL (index j omitted for brevity), there will be a small local plane m W = { n W, dW} ∈ S2×R in the map reference frame W that is nearest to pW (the projection of pL in W ). Plane m W is computed using the 5 nearest map points to pW . To match the current scan with the map, the distance from p W to m W should be minimized for every point in the scan. This distance is the measurement model used in the IKFoM for state estimation: h(x,η) = RL W(pL+ηp) + tL WTnW+dW,(4.4) 1https://github.com/Livox-SDK/livox_camera_lidar_calibration 4.3 General Description 111 where R L W∈SO3 and t L W∈R3 are the rotation and translation from frame W to frame L , and ηp∈R3 is a random vector representing the noise in p L . This measurement model considers the statistical error of the LiDAR point p L , but does not consider the error of the local map representation m W . Instead of adding it, method [35] assumes that the measurement zp can have some error in it (not to be confused with the error present in p L ). This approach helps to compensate the problem, but does not provide a reliable solution in information-degraded environments, as extensively explained in Chapter 3. In contrast, we propose to include the current map uncertainties in the measurement model to improve this behavior. The necessary derivations to provide this integration are given in Section 3.4.2, and not detailed again here for brevity. Moreover, Section 3.4.1 provides a method to compute the on-manifold covariance representation of mW. The resulting method requires a marginal computational time (0.2 µ s for each computed LiDAR plane), and provides an improvement of 4 times in the estimation of the covariance of the measurement model, see Table 3.1. By integrating map uncertainty in the state estimation of [35], the total RMSE in the robot state estimation was reduced by 40%. Using this result in combination with optional GNSS measurements, the resulting method provides adequate results for the envisioned application. The integration of GNSS measurements is presented as follows. Integration of GNSS Measurements The GNSS sensor provides measurements of its current position in a global reference frame G . To integrate these measurements, the state of the robot is now formulated as x= [t L W, R L W t W G, R W G∈R3×SO(3) ×R3×SO(3) ], where t W G and R W G are respectively the translation and rotation between frames G and W , which are initially unknown but constant during the same flight (they depend on the exact rotation between the robot’s initial position and the direction to the magnetic North). Given this, GNSS measurements can be integrated as a generic vector measurement in R3 . The measurement model corresponds to its estimated position of the GNSS receiver at the 118 Reactive LiDAR-based Mapping for Powerline Inspection Figure 4.10: LR-M powerline inspection aerial robot developed by the GRVC Robotics Lab. ©IEEE 2024 of the MS ; and a value of R3 = 0 . 001 for the PCA in the OS . The region growing algorithms had a leaf size of 0 . 35 m and a smoothness threshold of 0 . 08 rad. For the Mapping-Aware Commands module, the chosen scanned and textured percentage thresholds were: SPp = 85%, SPt = 75%, SPv = 75%, TPp = 80%, and TPt = 70%, and the reference velocity of the aerial robot was of 4.5 m. 4.4.1 Pointcloud Segmentation Figure 4.12 shows examples of the results obtained by the proposed semantic segmentation method int the ATLAS (left), Burguillos (center), and Alcala (right) scenarios, at times where multiple categories can be clearly seen in the scan. The visual images at those times are shown in Figure 4.12-top. At Figure 4.12-center the original LiDAR pointcloud can be seen. The resulting clusters representing the different categories can be seen in Figure 4.12-bottom, with different colors for each category. The method properly segmented all different types of objects. This satisfactory performance (systematic in the conducted experiments) is now analyzed and compared with other existing techniques. 4.4 Experimental Validation 119 Figure 4.11: Validation scenarios of the proposed method: a) ETSI, b) ATLAS, c) Burguillos, and d) Alcala.©IEEE 2024 The proposed segmentation scheme was evaluated against a commonly used state- of-the art Deep Learning (DL) segmentation method described at [179], denoted as PointNet++. The DL method was trained for the 4 present categories in the experiments with: i) scans of our performed flights, and ii) data from the DALES dataset for aerial LiDAR imaging [180], so less-represented classes (Towers,Powerlines) could be better learned by the method. The trained PointNet++ does not consider reflectivity: this value depends on the chosen LiDAR and other factors such as the distance to the scenario. Reflectivity values in our experiments where different to those in [180], and the method could not be trained only with our data. The chosen grid size for PointNet++ was of 25x25 m, with 2048 points per grid. To compare both methods, we manually labeled LiDAR scans of 1 flight in each scenario every 2 m. The values of Precision,Recall, and the corresponding F1 score were obtained for each of the semantic categories. Table 4.1 summarizes the performance of both methods in the conducted experiments. A visual comparison is shown in Figure 4.13, with all categories present in the 120 Reactive LiDAR-based Mapping for Powerline Inspection Figure 4.12: Result of the proposed method in experiments conducted in ATLAS (left), Burguillos (center), and Alcala (right). Top) visual images. Center) corresponding LiDAR scan pointclouds. Bottom) segmented pointclouds representing Powerline (green color), Tower (red), Soil (blue), and Vegetation (yellow). ©IEEE 2024 scenario. It can be seen that our method and PointNet++ deliver similar results in the Vegetation and Soil categories, but differ mainly in the identification of Powerline and Tower categories. In the case of category Powerline, our method correctly segments almost all powerline sections, eventually classifying some small straight sections of towers as Powerline. This is due to the chosen cluster size, which was not constrained to a minimum value to allow the identification of small sections of powerlines, which otherwise would have been considered noise and discarded. Also, some small sections of the powerline that are in the immediate vicinity of towers are misclassified as Tower, as aforementioned in Section 4.3.1. In contrast, PointNet++ never missclassified any object as Powerline, but many sections of the powerline were classified either as Vegetation or Tower. In general, that method had more difficulties in transitions between powerlines and towers, with missclasified areas of several meters. Second, in the case of category Tower, our method behaved appropriately but had some missclassificiations at the immediate vicinity of towers and powerlines, as explained 4.4 Experimental Validation 121 above. Some borders that were very close to the bottom of the towers could also be detected as part of it. The amount of Tower points was in general very low, and thus, these corner cases become more significant in the final scores. For this category, PointNet++ gave a much higher amount of false positives and negatives, (see Figure 4.13), delivering much worse performance than our method. The learning pipeline was not able to fully learn how to discriminate this category from the data in [180] and our flights. Table 4.1: Overall performance of the proposed scheme and PointNet++ [179] in all the performed experiments. ©IEEE 2024 Metric Precision(%) Recall(%) F1score(%) Ours [179] Ours [179] Ours [179] Powerline 96.23 100.0 96.51 77.78 96.36 87.50 Tower 84.95 41.48 86.72 50.33 85.82 45.48 Vegetation 99.63 99.27 98.89 99.86 99.26 99.57 Soil 98.01 99.95 99.84 99.72 98.92 99.83 Figure 4.13: Left) Resulting segmentation of contiguous scans obtained by the proposed method. Right) Same scans classified by PointNet++.©IEEE 2024 For the Vegetation and Soil categories, both methods delivered almost perfect results. Most noticeably, PointNet++ was able learn how to detect the Soil with almost no error, but an important amount of false positives were present in the category Vegetation near the powerlines and towers of the scenarios. In contrast, our method provided a better discrimination between Vegetation,Powerlines, and 122 Reactive LiDAR-based Mapping for Powerline Inspection Towers, but was not as good for discriminating between Soil and Vegetation in their immediate vicinity, at times classifying the bottom-most part (few centimeters bellow the ground) of the different trees as part of the soil. This is arguably a lesser problem: the correct classification of powerlines and towers, as well as the the top part of the trees (nearest to these elements) is what really matters in the use-case application. In addition to segmentation quality, we measured the computational behavior of both methods in the NVIDIA Jetson Xavier NX integrated in our aerial robot. PointNet++ ran on the system’s GPU (with also an initial preprocessing running on the CPU), and our method ran entirely on the available CPU, using a single thread. For the conducted experiments, our method performs segmentation consistently in a mean time of 54 ms .PointNet++ required in mean 122 ms to process each scan. This value depends on the amount of grids with enough point density to perform segmentation (2048), which usually were between 1 and 2. The used LiDAR provides scans at 10 Hz, and thus PointNet++ was not able to perform in real time. Our method can segment the scans, and integrate them into the Multisensor Mapping module (which runs in a different process) after the state of the system is estimated. Figure 4.14 shows an example map built in real time integrating this segmentation. If needed, multiple sections of our method can still be make more efficient, for instance using custom libraries such as nanoflann [106] for performing faster 3D neighbor searches as done in other chapters of this PhD Thesis. Figure 4.14: Segmented map using the proposed method obtain in one ATLAS experiment. ©IEEE 2024 4.4 Experimental Validation 123 Finally, although it is not relevant in the conducted experiments, there is a clear advantage in PointNet++: its ability to learn and adapt to new categories different (given enough labeled data for training). In the case of powerline inspection, only some categories such as Car and Building may have minor appearances during the flights. These are generally mapped as Soil in our scheme, since they have high reflectivity and are planar. On the other hand, because PointNet++ is learning-based method, it requires a high amount of labeled data before it starts performing successfully. Each new type of LiDAR sensor requires re-training the method to have the best segmentation results, which may be a complex task, while our method only has a small set of parameters that can be visually tuned by any worker. Finally, our method is able to work using the full laser scan, while PointNet++ needs to first divide it into subgrids of a specific length and amount of points, and finally interpolate the categories of those points to the obtained scan, potentially reducing the accuracy of the final result. 4.4.2 Multisensor Mapping The accuracy and robustness of the proposed mapping method have already been discussed in the Multisensor Mapping module. By considering map uncertainty and leveraging GNSS measurements, the method is able to perform with practically no drift in the performed experiments, see Figure 4.7. The only exception is the Burguillos scenario, where the method performs with some drift but is still able to work. This scenario is of special difficulty, since during multiple sections it presents almost no geometrical features. Chapter 3 presents a much more in-depth analysis on how to strengthen the mapping method in this case, proposing the use of probabilistic meshes that integrate map uncertainty in their formulation. For comparison, the proposed method in this chapter works in the Burguillos-F1 flight with a trajectory RMSE of 4 . 31 m, in contrast to the proposed method in Chapter 3, that worked with 0 . 37 m RMSE (both integrating GNSS measurements). Notice that the original FAST-LIO2 is not able to work in this scenario, even when integrating GNSS measurements. This improvement motivated the research in Chapter 3. 124 Reactive LiDAR-based Mapping for Powerline Inspection Having validated the state estimation, we now validate the mapping accuracy of the scheme. To this purpose, a Leica Total Station is used to obtain a millimeter accuracy map of a studied scenario. We perform this validation in the ETSI scenario, since it is of moderate size and can be mapped with the Leica within 1 hour. Using the Leica to map long-range powerline inspection scenarios is not possible, since the presence of trees, buildings, and other objects (see Figure 4.14) impedes the mapping of the whole scenario, which would take several weeks to be done. For the ETSI scenario, the proposed system was executed onboard the robot, obtaining a map of the environment. The map was aligned with Leica map using an ICP algorithm. The result of one experiment is shown in Figure 4.15-left, which overlays both maps, ours in red color and the Leica map in yellow and blue colors. The map error histogram is given in Figure 4.15-right. It shows that the statistical average of the error between the generated map and the Leica is of 3mm, and that the RMSE error is of 3.8 cm. This mapping quality is deemed sufficient for performing a preliminary analysis in powerline inspection. Figure 4.15: Validation of the Multisensor Mapping method in one experiment in the ETSI scenario: left) overlay of the low-level map (in red color) and the Leica map (in yellow and blue colors), assumed ground truth; right) map error histogram. The average error is 3mm and the RMSE error is 3.8 cm. 4.4.3 Mapping-Aware Commands Next, the performance of the complete scheme, integrating all modules and executed onboard the LR-M robot, is demonstrated. Figure 4.16 shows an example of its 4.4 Experimental Validation 125 performance along 23 s in one of the flights performed in the Alcala scenario. During navigation, module Mapping-Aware Commands receives the segmented and texturized clouds, as well as the current state estimation from modules Multisensor Mapping and Pointcloud Segmentation. With these, it computes the different VOI s of the elements in the scenario, as well as the corresponding volumetric maps for performing quality analysis. Figure 4.16-b-d shows the lowest scanned percentages of the VOI s that correspond to categories Powerline,Tower, and Vegetation. For the first two categories, it also show the lowest texturized percentages of the VOI s along the flight. For visualization, the vales SPp , SPt , and SPv , (scanned percentage thresholds of each category), as well as TPp and TPt (texturized percentage thresholds). If any VOI has a percentage below the corresponding threshold, the module commands to smoothly stop the motion of the robot. Figure 4.16-d shows these stop commands, which are triggered along the experiment, as well as and the longitudinal speed of the quadrotor. A constant reference speed of 4 . 5 m/s is given to the robot, unless a stop command is triggered. Figure 4.16 starts at time t = 198 s during the flight. At t≃ 199 s , a small bush appears in the LiDAR FOV, generating a new VOI with almost 0% scanned percentage. This triggers a stop command. With few following LiDAR scans, the percentage raised over SPv = 75%, thus finalizing the stop command. Few seconds after at time t≃ 201 s , the electric tower in Figure 4.17 appears in the LiDAR FoV. The scanned percentage of this tower’s VOI started with an intermediate value, since the VOI had not reached yet its full size. The percentage started to drop, and only increased when the VOI reached its full size. After integrating subsequent scans, the percentage raised, finally reaching SPt = 75% when the robot had acquired enough scans of the tower. This value never reaches 100%: there are many occluded areas due to the structure of the tower, which are kept as ”unknown”. In contrast, the textured percentage of the tower does not start growing until the tower is in the FOV of the camera, but then it grew faster and satisfied its threshold TPt = 70% sooner, since it only takes into account the ”occupied” points, and ”free” points or possible unknown occlusions are not considered. After the tower was mapped, at time t≃ 207 s another set of bushes appeared. 126 Reactive LiDAR-based Mapping for Powerline Inspection 0 2 4 v (m/s) Robot velocity and Stop commands a) b) c) d) Robot velocity Stop commands 0 1 Percentages (p.u.) Powerline VOIs Scanned percentage Textured percentage SPp TPp 0 1 Percentages (p.u.) Tower VOIs Scanned percentage Textured percentage SPt TPt 198 200 202 204 206 208 210 212 214 216 Trajectory time (s) 0 1 Percentages (p.u.) Vegetation VOIs Scanned percentage SPv Figure 4.16: Performance of the proposed scheme along 23 s in one inspection experiment conducted in the Alcala scenario: a) longitudinal speed of the quadrotor along its trajectory and stop commands triggered along the experiment; and b)-d) lowest mapped and textured percentages of the VOI s corresponding to Powerline, Tower, and Vegetation categories. If there are no VOI s of a specific object, a 100% value is used. ©IEEE 2024 For the Powerline category, VOI s were always present during the experiment. In general, they maintained constant and high scanned and textured percentages. However, at time t≃ 203 s a new set of powerlines appeared, with only few samples of them. Given this, both percentages to drop, but are quickly recovered when subsequent scans are received, and quickly raise above SPt = 85% and TPt = 80%. Powerlines are very thin objects that only present small occlusions, and thus they can be generally scanned with ease. Next, at time t≃ 210 s the powerline starts going downhill, causing the robot to detect segments of it, that are far from the current ones, making the VOI s grow bigger, and causing the scanned and textured percentages to drop slightly. Moments after, these percentages are again over their corresponding thresholds, allowing the robot to reach again its full velocity. Using the proposed reactive system, the robot took a total of 424 s to perform the complete inspection of the powerline, stopping when it was necessary to ensure a sufficient quality in the data gathering. 4.4 Experimental Validation 127 The same inspection mission was repeated without executing the proposed method, by keeping the robot with at constant longitudinal speed of 4 . 5 m/s along the mission, without adding reactivity. The quadrotor took 302 s to perform the experiments, did not inspect the electrical towers with the required quality, requiring the flight to be repeated in a future mission. Figure 4.17 compares the map that can be obtained around one electrical tower by using our method (left), and without using it (right). The quality difference between them can be clearly noticed, such as at the bottom of the tower, where the LiDAR scans are not as dense since the robot flies mostly above the powerlines. The scanned percentage of the corresponding VOI of the tower with a constant speed was in the range of [50 − 60]%, not adequate for powerline inspection. Using a lower velocity to compensate this can result in significantly higher inspection times. The proposed scheme only reduces the velocity of the robot when it is necessary to increase the data gathering quality. Therefore, the proposed system reduces inspection times while ensuring mapping quality. A 3D map built in the full experiment can be seen in Figure 4.18. Figure 4.17: left) One electrical tower mapped with our reactive navigation system. right) Same tower mapped by flying with the quadrotor at a constant speed. 4.4.4 Live Demonstrations in Real Scenarios The proposed system was designed to perform reactive autonomous navigation in real powerline inspection scenarios. The geometric map generated by the system, and enriched with semantic information, is of high value to efficiently perform automated inspection tasks such as measuring the distance between the powerline system and the