SE(3) Pose Estimation for Simultaneous Localization of Objects and Sensors in Dynamic Environments

Visual Simultaneous Localization and Mapping (SLAM) technology offers considerable advantages for enhancing cognitive abilities for mobile robots, especially under complex, dynamic environments. The creation of robust computer vision methods is essential to optimize the understanding of rigid object motion while ensuring precise localization of sensors or cameras. In this study, we introduce a tightly-integrated SLAM approach (MOT), designed to accurately capture SE (3) pose data for sensors/cameras and motion details of objects in the environment. The method initiates by building a streamlined EKF-tracker to determine and associate the real-time motion states of objects. Then, features are separated into static background and object-specific categories based on both instance mask and state information. Static features facilitate the initialization of camera poses, while dynamic features provide SE(3) pose data for objects under observation. Finally, we introduce a novel graph optimization technique to co-optimize the static 3D landmarks, object landmarks, camera poses, and moving object poses within the scene. The proposed approach has shown effectiveness on the KITTI and OMD datasets.