I am trying to write an EKF that can estimate the covariance of a pose estimate, where the estimation is being done by a PNP algorithm and 3D-2D correspondences in images. Although EKF based camera SLAM is pretty common, I've noticed that usually those techniques tend to integrate IMU data, as well as try to refine the map points along with the pose of the robot itself, thus considering both the map and localization somewhat unknown. But if I want to consider the map points as predefined and stable, and just want to compute the pose of a camera in 6 DOF, a relationship that is expressed simply between 3D and 2D points as:

$ \begin{bmatrix} x \\ y \\ 1 \end{bmatrix} = K*\begin{bmatrix} R && t \end{bmatrix} * \begin{bmatrix} X \\ Y \\ Z \\ 1 \end{bmatrix} $

where $R$ and $t$ are the unknowns. This result can further be optimized by trying to minimize the reprojection error, but I am wondering how this can be reconstructed as an EKF measurement equation, and thereby estimate the state covariance, with my state containing 3D coordinates and the Euler angles of the camera pose as $[x, y, z, r, p, y]$.



Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Browse other questions tagged or ask your own question.