Optimal Pose Estimation and Covariance Analysis with Simultaneous Localization and Mapping Applications