This paper focuses on the motion estimation problem of ground vehicles using odometry and monocular visual sensors. While the keyframe-based batch optimization methods become the mainstream approach in mobile vehicle localization and mapping, the keyframe poses are usually represented by SE(3) in vision-based methods or SE(2) in methods based on range scanners. For a ground vehicle, this paper proposes a new SE(2)-constrained SE(3) parameterization of its poses, which can be easily achieved in the batch optimization framework using specially formulated edges. Utilizing such a parameterization of poses, a complete odometry-vision-based motion estimation system is developed. The system is designed in a commonly used structure of graph optimization, providing high modularity and flexibility for further implementation or adaptation. Its superior performance in terms of accuracy on a ground vehicle platform is validated by real-world experiments in industrial indoor environments.

Extrinsic calibration is one of the most important problems in robotics, whose objective is to estimate the relative poses among the sensors and the robot. Currently, most on-line solutions of extrinsic calibration are based on the Gaussian filters, which estimate all the system states iteratively. However, a large number of the system states are not related to calibration. Estimating all these redundant states may highly increase the dimensionality of the problem, and therefore, undermine the calibration performance in both efficiency and robustness. In this paper, we propose an innovative on-line calibration algorithm, called constraint Gaussian filter with virtual measurements (VMCGF). The nature of VMCGF is a filter with a compact state vector containing only the states of interest, also called essential states. Violating the modeling principles of the traditional Gaussian filters, the measurements cannot be expressed by the observation function with the essential states solely. Exploiting the constraints between the measurements and the essential states, virtual measurements are generated according to the properties of the generalized chi-square distribution. Although originally developed to solve the calibration problem, VMCGF is a general filtering algorithm, that can be applied to solve other problems that might suffer from redundant states. The implementation of VMCGF on a camera odometry calibration problem is introduced, and its observability properties are analyzed. Both simulations and experiments are conducted to validate our algorithm.

In this paper, we propose a hierarchical pose graph optimization algorithm, which hierarchically divides a large pose graph into subgraphs and solves the optimization problem for each subgraph independently. Applying a modified graph partitioning algorithm, normalized cut, the original graph could be partitioned into subgraphs, which are conditionally independent on a set of key nodes. A modified normalized cut algorithm is applied to automatically partition a pose graph into several subgraphs, which are independent on each other when conditioned on a small number of keynodes. Preserving keynodes only, a simplified upper level graph is generated by a pose graph sparsification algorithm. Given the optimization results of all keynodes, which are obtained by solving the upper level graph, each subgraph in the lower level can be solved efficiently without concerning other subgraphs. The scale of each optimization is limited during graph partitioning. Therefore, the efficiency of the optimization is improved. Experiments with both public standard datasets and a dataset collected by an autonomous guided vehicle (AGV) system are conducted to test our algorithm.