Factor Graphs in Optimization-Based Robotic Control—A Tutorial and Review

Factor graphs, initially developed as probabilistic graphical models, have been widely employed for solving large-scale inference problems in robotics, particularly in tasks such as pose estimation, Structure from Motion (SfM), or Simultaneous Localization and Mapping (SLAM). Their capability to efficiently model uncertainty and the locality of sensor data has made them crucial for robotic perception and situational awareness. Recently, factor graphs have evolved beyond their probabilistic origins and are also being applied to deterministic optimization problems, such as robotic planning and control. This paper first aims to provide a comprehensive tutorial on factor graphs and the formulation and solution of the related optimization problems within the context of robotics perception. In addition, we undertake a thorough review of approaches that extend factor graphs—traditionally solved by unconstrained optimization—to optimal control tasks, emphasizing how they handle the constraints intrinsic to control problems. Finally, we analyze the potential of factor graphs for the seamless integration of robotic situational awareness, planning, and control, which remains one of the most critical challenges in achieving fully autonomous robot operations in complex environments.