Systems and methods for creating images of an environment includes controlling at least one camera to acquire imaging data from the environment and selecting, from the imaging data, a three-dimensional-two-dimensional correspondence as a control point for use in a perspective-n-point problem to determine a position and orientation of the at least one camera from n known correspondences between three-dimensional object points and their two-dimensional image projections in the environment. The method also includes reprojecting at least a selected number of the projections, determining a reprojection error for each of the projections, and performing a weight assignment of reprojection errors to distinguish the inliers form outliers. These steps of the method are repeated to apply the weight assignment to outliers in a decreased fashion during iterations to reduce an impact of outliers in the real-time display of the environment.