class mrpt::vision::pnp::CPnP

Overview

This class is used for Pose estimation from a known landmark using a monocular camera.

The toolkit comprises of state of the art PnP (Perspective n Point) algorithms

The Python Bindings pnp_perf_comp.py can be used to generate performance comparison using standard tests between the different algorithms.

<Untitled>

Chandra Mangipudi

17/08/16

#include <mrpt/vision/pnp_algos.h>

class CPnP
{
public:
    // methods

    bool dls(
        const Eigen::Ref<Eigen::MatrixXd> obj_pts,
        const Eigen::Ref<Eigen::MatrixXd> img_pts,
        int n,
        const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
        Eigen::Ref<Eigen::MatrixXd> pose_mat
        );

    bool epnp(
        const Eigen::Ref<Eigen::MatrixXd> obj_pts,
        const Eigen::Ref<Eigen::MatrixXd> img_pts,
        int n,
        const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
        Eigen::Ref<Eigen::MatrixXd> pose_mat
        );

    bool upnp(
        const Eigen::Ref<Eigen::MatrixXd> obj_pts,
        const Eigen::Ref<Eigen::MatrixXd> img_pts,
        int n,
        const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
        Eigen::Ref<Eigen::MatrixXd> pose_mat
        );

    bool p3p(
        const Eigen::Ref<Eigen::MatrixXd> obj_pts,
        const Eigen::Ref<Eigen::MatrixXd> img_pts,
        int n,
        const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
        Eigen::Ref<Eigen::MatrixXd> pose_mat
        );

    bool rpnp(
        const Eigen::Ref<Eigen::MatrixXd> obj_pts,
        const Eigen::Ref<Eigen::MatrixXd> img_pts,
        int n,
        const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
        Eigen::Ref<Eigen::MatrixXd> pose_mat
        );

    bool ppnp(
        const Eigen::Ref<Eigen::MatrixXd> obj_pts,
        const Eigen::Ref<Eigen::MatrixXd> img_pts,
        int n,
        const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
        Eigen::Ref<Eigen::MatrixXd> pose_mat
        );

    bool posit(
        const Eigen::Ref<Eigen::MatrixXd> obj_pts,
        const Eigen::Ref<Eigen::MatrixXd> img_pts,
        int n,
        const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
        Eigen::Ref<Eigen::MatrixXd> pose_mat
        );

    bool lhm(
        const Eigen::Ref<Eigen::MatrixXd> obj_pts,
        const Eigen::Ref<Eigen::MatrixXd> img_pts,
        int n,
        const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
        Eigen::Ref<Eigen::MatrixXd> pose_mat
        );
};

Methods

bool dls(
    const Eigen::Ref<Eigen::MatrixXd> obj_pts,
    const Eigen::Ref<Eigen::MatrixXd> img_pts,
    int n,
    const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
    Eigen::Ref<Eigen::MatrixXd> pose_mat
    )

hesch Direct Least Squares (DLS) - PnP : Algorithm formulates position as a function of rotation.

Use Cayley’s rotation theorem to represent the rotation using parameters ($s_1, s_2, s_3$). Solve the rotation using multi-variate polynomial expressions

Parameters:

obj_pts

Object points in Camera Co-ordinate system {C} nX3 (only 4 points used) array [p_x, p_y, p_z]

img_pts

Image points in pixels nX3 (only 4 points used) array containing pixel data from camera [u, v, 1]

n

number of 2D-3D correspondences

cam_intrinsic

Camera Intrinsic matrix

pose_mat

Output pose vector 6X1, pose_mat[0:2]-> Translation, pose_mat[3:5] -> Quaternion vector component

Returns:

success flag

bool epnp(
    const Eigen::Ref<Eigen::MatrixXd> obj_pts,
    const Eigen::Ref<Eigen::MatrixXd> img_pts,
    int n,
    const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
    Eigen::Ref<Eigen::MatrixXd> pose_mat
    )

lepetit Efficient-PnP (EPnP) - Algorithm takes 4 control points based on n points and uses the control points to compute the pose

Parameters:

obj_pts

Object points in Camera Co-ordinate system {C} nX3 (only 4 points used) array [p_x, p_y, p_z]

img_pts

Image points in pixels nX3 (only 4 points used) array containing pixel data from camera [u, v, 1]

n

number of 2D-3D correspondences

cam_intrinsic

Camera Intrinsic matrix

pose_mat

Output pose vector 6X1, pose_mat[0:2]-> Translation, pose_mat[3:5] -> Quaternion vector component

Returns:

success flag

bool upnp(
    const Eigen::Ref<Eigen::MatrixXd> obj_pts,
    const Eigen::Ref<Eigen::MatrixXd> img_pts,
    int n,
    const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
    Eigen::Ref<Eigen::MatrixXd> pose_mat
    )

Kneip2014 Unified-PnP (UPnP) : Algorithm to compute pose from unknown camera intrinsic matrix

Parameters:

obj_pts

Object points in Camera Co-ordinate system {C} nX3 (only 4 points used) array [p_x, p_y, p_z]

img_pts

Image points in pixels nX3 (only 4 points used) array containing pixel data from camera [u, v, 1]

n

number of 2D-3D correspondences

cam_intrinsic

Camera Intrinsic matrix

pose_mat

Output pose vector 6X1, pose_mat[0:2]-> Translation, pose_mat[3:5] -> Quaternion vector component

Returns:

success flag

bool p3p(
    const Eigen::Ref<Eigen::MatrixXd> obj_pts,
    const Eigen::Ref<Eigen::MatrixXd> img_pts,
    int n,
    const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
    Eigen::Ref<Eigen::MatrixXd> pose_mat
    )

kneip P3P - A closed form solution for n=3, 2D-3D correspondences

Parameters:

obj_pts

Object points in Camera Co-ordinate system {C} nX3 (only 4 points used) array [p_x, p_y, p_z]

img_pts

Image points in pixels nX3 (only 4 points used) array containing pixel data from camera [u, v, 1]

n

number of 2D-3D correspondences

cam_intrinsic

Camera Intrinsic matrix

pose_mat

Output pose vector 6X1, pose_mat[0:2]-> Translation, pose_mat[3:5] -> Quaternion vector component

Returns:

success flag

bool rpnp(
    const Eigen::Ref<Eigen::MatrixXd> obj_pts,
    const Eigen::Ref<Eigen::MatrixXd> img_pts,
    int n,
    const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
    Eigen::Ref<Eigen::MatrixXd> pose_mat
    )

xie Robust (R-PnP)- A closed form solution with intermediate P3P computations

Parameters:

obj_pts

Object points in Camera Co-ordinate system {C} nX3 (only 4 points used) array [p_x, p_y, p_z]

img_pts

Image points in pixels nX3 (only 4 points used) array containing pixel data from camera [u, v, 1]

n

number of 2D-3D correspondences

cam_intrinsic

Camera Intrinsic matrix

pose_mat

Output pose vector 6X1, pose_mat[0:2]-> Translation, pose_mat[3:5] -> Quaternion vector component

Returns:

success flag

bool ppnp(
    const Eigen::Ref<Eigen::MatrixXd> obj_pts,
    const Eigen::Ref<Eigen::MatrixXd> img_pts,
    int n,
    const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
    Eigen::Ref<Eigen::MatrixXd> pose_mat
    )

garro Procrustes-PnP(PPnP) Algorithm : Iterative SVD based algorithm

Parameters:

obj_pts

Object points in Camera Co-ordinate system {C} nX3 array [p_x, p_y, p_z]

img_pts

Image points in pixels nX3 array containing pixel data from camera [u, v, 1]

n

number of 2D-3D correspondences

cam_intrinsic

Camera Intrinsic matrix

pose_mat

Output pose vector 6X1, pose_mat[0:2]-> Translation, pose_mat[3:5] -> Quaternion vector component

Returns:

success flag

bool posit(
    const Eigen::Ref<Eigen::MatrixXd> obj_pts,
    const Eigen::Ref<Eigen::MatrixXd> img_pts,
    int n,
    const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
    Eigen::Ref<Eigen::MatrixXd> pose_mat
    )

dementhon Pose from Orthogoanlity and Scaling :Iterative (POSIT) - A Geometric algorithm to compute scale and orthogonality independently

Parameters:

obj_pts

Object points in Camera Co-ordinate system {C} nX3 array [p_x, p_y, p_z]

img_pts

Image points in pixels nX3 array containing pixel data from camera [u, v, 1]

n

number of 2D-3D correspondences

cam_intrinsic

Camera Intrinsic matrix

pose_mat

Output pose vector 6X1, pose_mat[0:2]-> Translation, pose_mat[3:5] -> Quaternion vector component

Returns:

success flag

bool lhm(
    const Eigen::Ref<Eigen::MatrixXd> obj_pts,
    const Eigen::Ref<Eigen::MatrixXd> img_pts,
    int n,
    const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
    Eigen::Ref<Eigen::MatrixXd> pose_mat
    )

lu Lu-Hager-Mjolsness(LHM)-PnP algorithm : Iterative algorithm to reduce object space error

Parameters:

obj_pts

Object points in Camera Co-ordinate system {C} nX3 array [p_x p_y p_z]

img_pts

Image points in pixels nX3 array containing pixel data from camera [u, v, 1]

n

number of 2D-3D correspondences

cam_intrinsic

Camera Intrinsic matrix

pose_mat

Output pose vector 6X1, pose_mat[0:2]-> Translation, pose_mat[3:5] -> Quaternion vector component

Returns:

success flag