"This work presents a novel method to efficiently factorize the combination of multiple factor graphs having common variables of estimation. The fast-paced innovation in the algebraic graph theory has enabled new tools of state estimation like factor graphs. Recent factor graph formulation for Simultaneous Localization and Mapping (SLAM) like Incremental Smoothing and Mapping using the Bayes tree (ISAM2) has been very successful and garnered much attention. Variable ordering, a well-known technique in linear algebra is employed for solving the factor graph. Our primary contribution in this work is to reuse the variable ordering of the graphs being combined to find the ordering of the fused graph. In the case of mapping, multiple robots provide a great advantage over single robot by providing a faster map coverage and better estimation quality. This coupled with an inevitable increase in the number of robots around us produce a demand for faster algorithms. For example, a city full of self-driving cars could pool their observation measurements rapidly to plan a traffic free navigation. By reusing the variable ordering of the parent graphs we were able to produce an order-of-magnitude difference in the time required for solving the fused graph. We also provide a formal verification to show that the proposed strategy does not violate any of the relevant standards. A common problem in multi-robot SLAM is relative pose graph initialization to produce a globally consistent map. The other contribution addresses this by minimizing a specially formulated error function as a part of solving the factor graph. The performance is illustrated on a publicly available SuiteSparse dataset and the multi-robot AP Hill dataset."

As the capabilities of autonomous vehicles increase, their use in situations that are dangerous or dull for humans is becoming more popular. Autonomous systems are currently being used in several military and civilian domains, including search and rescue operations, disaster relief coordination, infrastructure inspection and surveillance missions. In order to perform high level mission autonomy tasks, a method is required for the vehicle to localize itself, as well as generate a map of the environment. Algorithms which allow the vehicle to concurrently localize and create a map of its surroundings are known as solutions to the Simultaneous Localization and Mapping (SLAM) problem. Certain high level tasks, such as drivability analysis and obstacle avoidance, benefit from the use of a dense map of the environment, and are typically generated with the use of point cloud data. The point cloud data is incorporated into SLAM algorithms with scan registration techniques, which determine the relative transformation between two sufficiently overlapping point clouds. The Normal Distributions Transform (NDT) algorithm is a promising method for scan registration, however many issues with the NDT approach exist, including a poor convergence basin, discontinuities in the NDT cost function, and unreliable pose estimation in sparse, outdoor environments. This thesis presents methods to overcome the shortcomings of the NDT algorithm, in both 2D and 3D scenarios. To improve the convergence basin of NDT for 2D scan registration, the Multi-Scale k-Means NDT (MSKM-NDT) algorithm is presented, which divides a 2D point cloud using k-means clustering and performs the scan registration optimization over multiple scales of clustering. The k-means clustering approach generates fewer Gaussian distributions when compared to the standard NDT algorithm, allowing for evaluation of the cost function across all Gaussian clusters. Cost evaluation across all the clusters guarantees that the optimization will converge, as it resolves the issue of discontinuities in the cost function found in the standard NDT algorithm. Experiments demonstrate that the MSKM-NDT approach can be used to register partially overlapping scans with large initial transformation error, and that the convergence basin of MSKM-NDT is superior to NDT for the same test data. As k-means clustering does not scale well to 3D, the Segmented Greedy Cluster NDT (SGC-NDT) method is proposed as an alternative approach to improve and guarantee convergence using 3D point clouds that contain points corresponding to the ground of the environment. The SGC-NDT algorithm segments the ground points using a Gaussian Process (GP) regression model and performs clustering of the non ground points using a greedy method. The greedy clustering extracts natural features in the environment and generates Gaussian clusters to be used within the NDT framework for scan registration. Segmentation of the ground plane and generation of the Gaussian distributions using natural features results in fewer Gaussian distributions when compared to the standard NDT algorithm. Similar to MSKM-NDT, the cost function can be evaluated across all the clusters in the scan, resulting in a smooth and continuous cost function that guarantees convergence of the optimization. Experiments demonstrate that the SGC-NDT algorithm results in scan registrations with higher accuracy and better convergence properties than other state-of-the-art methods for both urban and forested environments.

As the capabilities of autonomous vehicles increase, their use in situations that are dangerous or dull for humans is becoming more popular. Autonomous systems are currently being used in several military and civilian domains, including search and rescue operations, disaster relief coordination, infrastructure inspection and surveillance missions. In order to perform high level mission autonomy tasks, a method is required for the vehicle to localize itself, as well as generate a map of the environment. Algorithms which allow the vehicle to concurrently localize and create a map of its surroundings are known as solutions to the Simultaneous Localization and Mapping (SLAM) problem. Certain high level tasks, such as drivability analysis and obstacle avoidance, benefit from the use of a dense map of the environment, and are typically generated with the use of point cloud data. The point cloud data is incorporated into SLAM algorithms with scan registration techniques, which determine the relative transformation between two sufficiently overlapping point clouds. The Normal Distributions Transform (NDT) algorithm is a promising method for scan registration, however many issues with the NDT approach exist, including a poor convergence basin, discontinuities in the NDT cost function, and unreliable pose estimation in sparse, outdoor environments. This thesis presents methods to overcome the shortcomings of the NDT algorithm, in both 2D and 3D scenarios. To improve the convergence basin of NDT for 2D scan registration, the Multi-Scale k-Means NDT (MSKM-NDT) algorithm is presented, which divides a 2D point cloud using k-means clustering and performs the scan registration optimization over multiple scales of clustering. The k-means clustering approach generates fewer Gaussian distributions when compared to the standard NDT algorithm, allowing for evaluation of the cost function across all Gaussian clusters. Cost evaluation across all the clusters guarantees that the optimization will converge, as it resolves the issue of discontinuities in the cost function found in the standard NDT algorithm. Experiments demonstrate that the MSKM-NDT approach can be used to register partially overlapping scans with large initial transformation error, and that the convergence basin of MSKM-NDT is superior to NDT for the same test data. As k-means clustering does not scale well to 3D, the Segmented Greedy Cluster NDT (SGC-NDT) method is proposed as an alternative approach to improve and guarantee convergence using 3D point clouds that contain points corresponding to the ground of the environment. The SGC-NDT algorithm segments the ground points using a Gaussian Process (GP) regression model and performs clustering of the non ground points using a greedy method. The greedy clustering extracts natural features in the environment and generates Gaussian clusters to be used within the NDT framework for scan registration. Segmentation of the ground plane and generation of the Gaussian distributions using natural features results in fewer Gaussian distributions when compared to the standard NDT algorithm. Similar to MSKM-NDT, the cost function can be evaluated across all the clusters in the scan, resulting in a smooth and continuous cost function that guarantees convergence of the optimization. Experiments demonstrate that the SGC-NDT algorithm results in scan registrations with higher accuracy and better convergence properties than other state-of-the-art methods for both urban and forested environments.

Localization is a fundamental part of achieving fully autonomous vehicles. A localization system needs to constantly provide accurate information about the position of the vehicle and failure could lead to catastrophic consequences. Global Navigation Satellite Systems (GNSS) can supply accurate positional measurements but are susceptible to disturbances and outages in environments such as indoors, in tunnels, or nearby tall buildings. A common method called simultaneous localization and mapping (SLAM) creates a spatial map and simultaneously determines the position of a robot or vehicle. Utilizing different sensors for localization can increase the accuracy and robustness of such a system if used correctly. This thesis uses a graph-based version of SLAM called graph SLAM which stores previous measurements in a factor graph, making it possible to adjust the trajectory and map as new information is gained. The best position state estimation is gained by optimizing the graph representing the log-likelihood of the data. To treat GNSS outliers in a graph SLAM system, robust optimization techniques can be used, and this thesis investigates two techniques called realizing, reversing, recovering (RRR), and dynamic covariance scaling (DCS). High-end GNSS and Lidar sensors are used to gather a data set on a suburban public road. Information about the position and orientation of the vehicle are inferred from the data set using graph SLAM together with robust techniques in three different scenarios. The scenarios contain disturbances called multipathing, Gaussian disturbances, and outages. A parameter study examines the free parameters Φ in DCS and the p-value in the RRR method. The localization performance varies less when changing the free parameter in RRR than in DCS. The localization performance from RRR is consistent for most values of p. DCS shows greater variation in the localization performance for different values of Φ. In the tested cases, results conclude that Φ should be set to 2.5 for the most consistent localization across all states. RRR performed best with a p-value set to 0.85. A lower value led to too many discarded measurements which decreased performance. DCS outperforms RRR across the tested scenarios but further testing is needed to determine whether RRR is better suited for handling larger errors. / Lokalisering är en fundamental del i att uppnå självkörande fordon. Lokaliseringssytemets uppgift är att kontinuerligt förse exakt information om fordonets position, och vid fel kan detta leda till katastrofala följder. Global Navigation Satellite Systems (GNSS) används ofta i ett lokaliseringssystem för att uppnå exakta positionsmätningar, men i vissa miljöer så som parkeringshus, tunnlar eller storstäder kan störningar uppstå. Genom att förlita sig på fler typer av sensorer kan lokaliseringen bli mer noggrann och robust mot störningar. En vanlig metod som kan skatta ett fordons position och samtidigt skapa en karta över omgivningen är simultaneous localization and mapping SLAM. I detta examensarbete används graph SLAM, en version av SLAM som utnyttjar en faktorgraf för att representera mätvärden och sedan estimera position av fordonet. Robusta metoder kan användas inom SLAM för hantering av felaktiga mätningar i ett grafbaserat SLAM-nätverk, och här undersöks två metoder, realizing, reversing, recovering (RRR) och dynamic covariance scaling DCS. Data från GNSS och Lidarsensorer av hög kvalitet samlades in på en offentlig väg i stadsmiljö. I tre olika scenarion beräknas testfordonets position och orientering med graph SLAM tillsammans med de två robusta metoderna som undersöks. Scenarion utgör fall med olika typer av störningar som agerar på gnss-mätningarna. Störningarna är av typerna multipath, Gaussiskt brus, samt avbrott. DCS presterar bättre jämfört med RRR under de tester som utförts. En parameterstudie har utförts som undersöker parametern Φ i DCS och p i RRR. När Φ varieras i DCS ger det en större skillnad på resultatet än när p varieras i RRR. Detta indikerar att det är lättare att hantera och använda RRR optimalt. Trots att DCS presterar bättre än RRR i de testade fallen, krävs vidare undersökning för att besluta om RRR hanterar stora fel bättre än DCS. De bästa inställningarna visades vara 2,5 för Φ i DCS och större än 0,85 för p i RRR.

SLAM (Simultaneous Localization And Mapping) has been a very active and almost ubiquitous problem in the field of mobile and autonomous robotics for over two decades. For many years, filter-based methods have dominated the SLAM literature, but a change of paradigms could be observed recently. Current state of the art solutions of the SLAM problem are based on efficient sparse least squares optimization techniques. However, it is commonly known that least squares methods are by default not robust against outliers. In SLAM, such outliers arise mostly from data association errors like false positive loop closures. Since the optimizers in current SLAM systems are not robust against outliers, they have to rely heavily on certain preprocessing steps to prevent or reject all data association errors. Especially false positive loop closures will lead to catastrophically wrong solutions with current solvers. The problem is commonly accepted in the literature, but no concise solution has been proposed so far. The main focus of this work is to develop a novel formulation of the optimization-based SLAM problem that is robust against such outliers. The developed approach allows the back-end part of the SLAM system to change parts of the topological structure of the problem\'s factor graph representation during the optimization process. The back-end can thereby discard individual constraints and converge towards correct solutions even in the presence of many false positive loop closures. This largely increases the overall robustness of the SLAM system and closes a gap between the sensor-driven front-end and the back-end optimizers. The approach is evaluated on both large scale synthetic and real-world datasets. This work furthermore shows that the developed approach is versatile and can be applied beyond SLAM, in other domains where least squares optimization problems are solved and outliers have to be expected. This is successfully demonstrated in the domain of GPS-based vehicle localization in urban areas where multipath satellite observations often impede high-precision position estimates.

SLAM (Simultaneous Localization And Mapping) has been a very active and almost ubiquitous problem in the field of mobile and autonomous robotics for over two decades. For many years, filter-based methods have dominated the SLAM literature, but a change of paradigms could be observed recently. Current state of the art solutions of the SLAM problem are based on efficient sparse least squares optimization techniques. However, it is commonly known that least squares methods are by default not robust against outliers. In SLAM, such outliers arise mostly from data association errors like false positive loop closures. Since the optimizers in current SLAM systems are not robust against outliers, they have to rely heavily on certain preprocessing steps to prevent or reject all data association errors. Especially false positive loop closures will lead to catastrophically wrong solutions with current solvers. The problem is commonly accepted in the literature, but no concise solution has been proposed so far. The main focus of this work is to develop a novel formulation of the optimization-based SLAM problem that is robust against such outliers. The developed approach allows the back-end part of the SLAM system to change parts of the topological structure of the problem\'s factor graph representation during the optimization process. The back-end can thereby discard individual constraints and converge towards correct solutions even in the presence of many false positive loop closures. This largely increases the overall robustness of the SLAM system and closes a gap between the sensor-driven front-end and the back-end optimizers. The approach is evaluated on both large scale synthetic and real-world datasets. This work furthermore shows that the developed approach is versatile and can be applied beyond SLAM, in other domains where least squares optimization problems are solved and outliers have to be expected. This is successfully demonstrated in the domain of GPS-based vehicle localization in urban areas where multipath satellite observations often impede high-precision position estimates.

Die Sensordatenfusion ist eine allgegenwärtige Aufgabe im Bereich der mobilen Robotik und darüber hinaus. In der vorliegenden Arbeit wird das typischerweise verwendete Verfahren zur Sensordatenfusion in der Robotik in Frage gestellt und anhand von neuartigen Algorithmen, basierend auf einem Faktorgraphen, gelöst sowie mit einer korrespondierenden Extended-Kalman-Filter-Implementierung verglichen. Im Mittelpunkt steht dabei das technische sowie algorithmische Sensorkonzept für die Navigation eines Flugroboters im Innenbereich. Ausführliche Experimente zeigen die Qualitätssteigerung unter Verwendung der neuen Variante der Sensordatenfusion, aber auch Einschränkungen und Beispiele mit nahezu identischen Ergebnissen beider Varianten der Sensordatenfusion. Neben Experimenten anhand einer hardwarenahen Simulation wird die Funktionsweise auch anhand von realen Hardwaredaten evaluiert.