Robust Pose Graph Optimization for Calibration and Autonomous Robots Navigation

A Pose Graph (PG) is a powerful mathematical framework used to formulate a wide range of robotic problems. These problems can be expressed as Maximum a Posteriori (MAP) estimation tasks, which are efficiently solved using Non-Linear Least Squares (NLLS) optimization. The Pose Graph’s versatility extends across various state estimation problems, including Simultaneous Localization and Mapping (SLAM), Structure-from-Motion (SfM), optimal control, tracking, extrinsic sensor calibration, and multimodal sensor fusion. This broad applicability positions Pose Graph Optimization (PGO) as a fundamental tool for spatial perception—a critical skill that enables robots to ground sensor data into structured 3D representations, allowing them to perceive, model, and understand their surroundings effectively. Among the diverse applications of PGO, extrinsic calibration and SLAM are particularly crucial and interrelated. Extrinsic calibration involves estimating relative poses between sensors or various robot components, such as determining the pose between a robot base and an arm’s end effector mounted on it. Accurate extrinsic calibration is critical for sensor fusion and reliable autonomous task execution. Instead, SLAM is the problem of incrementally building a map of an unknown environment while simultaneously estimating the robot’s own pose using the perceived sensor information. This capability is essential for real-world deployment, allowing autonomous navigation and interaction within unfamiliar surroundings. This thesis emphasizes the versatility of PGs through two significant contributions. Firstly, it demonstrates their application in previously unexplored domains, specifically Hand-Eye (HE) calibration, introducing a general framework for estimating spatial relationships between a common reference frame and multiple cameras within robotic workspaces. Secondly, this research addresses a critical limitation of PGO: its inherent assumption that all the data used in the optimization is correct, which often does not hold true. Perception errors, known as outliers, frequently occur in perceptually aliased environments such as un- derwater, cave systems, and urban areas, where visually similar locations lead to incorrect correspondences. Because PGO is highly sensitive to incorrect constraints, even a small fraction of erroneous correspondences can severely distort the estimated trajectory and compromise system reliability. Consequently, this thesis tackles this limitation specifically within the SLAM context. It introduces novel Robust Pose Graph Optimization (RPGO) frameworks that mitigate the adverse effects of outliers through adaptive weighting schemes, consistency checks, and multi-horizon reasoning. By enhancing the resilience of PGO against erroneous constraints, the presented approaches contribute significantly to achieving reliable long-term autonomy in challenging exploration scenarios.