C++ (Cpp) Isometry3d::pretranslate Examples

Example #1

File: common.cpp Project: WhiteLiu/WhiteLiu

Eigen::Isometry3d transformEstimation(const Mat& rgb1,const Mat& rgb2,const Mat& depth1,const Mat& depth2,const CAMERA_INTRINSIC_PARAMETERS& C)
{
    vector<KeyPoint> keyPts1,keyPts2;
    Mat descriptors1,descriptors2;

    extractKeypointsAndDescripotrs(rgb1,rgb2,keyPts1,keyPts2,descriptors1,descriptors2);


    vector<DMatch> matches;
    descriptorsMatch(rgb1,rgb2,keyPts1,keyPts2,descriptors1,descriptors2,matches);


    vector<Point2f> points;
    vector<Point3f> objectPoints;
    vector<Eigen::Vector2d> imagePoints1,imagePoints2;

    getObjectPointsAndImagePoints(depth1,depth2,keyPts1,keyPts2,matches,points,objectPoints,imagePoints1,imagePoints2,C);

    Mat translation,rotation;
    double camera_matrix_data[3][3] = {
        {C.fx, 0, C.cx},
        {0, C.fy, C.cy},
        {0, 0, 1}    };

    Mat cameraMatrix(3,3,CV_64F,camera_matrix_data);


    solvePnPRansac(objectPoints,points,cameraMatrix,Mat(),rotation,translation,false, 100, 1.0, 20);


    Mat rot;
    Rodrigues(rotation,rot);

    Eigen::Matrix3d r;
    Eigen::Vector3d t;

    cout<<rot<<endl;
    cout<<translation<<endl;

    r<< ((double*)rot.data)[0],((double*)rot.data)[1],((double*)rot.data)[2],
            ((double*)rot.data)[3],((double*)rot.data)[4],((double*)rot.data)[5],
            ((double*)rot.data)[6],((double*)rot.data)[7],((double*)rot.data)[8];
    t<<((double*)translation.data)[0],((double*)translation.data)[1],((double*)translation.data)[2];

    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    T.rotate(r);
    T.pretranslate(t);
    cout<<T.matrix()<<endl;

    BundleAdjustmentOptimization(objectPoints,imagePoints1,imagePoints2);

    return T;
}