solvepnpransac gives bad results

asked 2013-10-16 15:09:33 -0500

Artur gravatar image

hi. i have a problem with solvepnpransac. it just gives bad results. i have no idea whats wrong... here is a little program which shows the problem. there are 5 perfect point-correspondences. These should perfectly fit to a camera at point (1,0,0), showing into the x-direction, with a focallength of 1. solvePnP on the other hand gives very good results.

#include "opencv2/core/core.hpp"
#include "opencv2/calib3d/calib3d.hpp"

#include <iostream>

using namespace cv;
using namespace std;

int main( int argc, char** argv ){
    Mat imagePoint(0,2,CV_32F);
    Mat iPoint = (Mat_<float>(1,2) << 0,0);
    imagePoint.push_back(iPoint);
    iPoint = (Mat_<float>(1,2) << 0,1);
    imagePoint.push_back(iPoint);
    iPoint = (Mat_<float>(1,2) << 1,0);
    imagePoint.push_back(iPoint);
    iPoint = (Mat_<float>(1,2) << 0,2);
    imagePoint.push_back(iPoint);
    iPoint = (Mat_<float>(1,2) << 2,0);
    imagePoint.push_back(iPoint);

    Mat worldPoint(0,3,CV_32F);
    Mat wPoint = (Mat_<float>(1,3) << 10,0,0);
    worldPoint.push_back(wPoint);
    wPoint = (Mat_<float>(1,3) << 4,3,0);
    worldPoint.push_back(wPoint);
    wPoint = (Mat_<float>(1,3) << 4,0,3);
    worldPoint.push_back(wPoint);
    wPoint = (Mat_<float>(1,3) << 3,4,0);
    worldPoint.push_back(wPoint);
    wPoint = (Mat_<float>(1,3) << 3,0,4);
    worldPoint.push_back(wPoint);
    Mat cameraMatrix = (Mat_<float>(3,3) << 1,0,0,0,1,0,0,0,1);
    Mat distCoeffs = (Mat_<float>(4,1) << 0,0,0,0);
    Mat rvec(3,1,CV_32F);
    Mat tvec(3,1,CV_32F);
    solvePnPRansac(worldPoint, imagePoint, cameraMatrix, distCoeffs, rvec, tvec);
    //solvePnP(worldPoint, imagePoint, cameraMatrix, distCoeffs, rvec, tvec);
    Mat rotate(3,3,CV_32F);
    Rodrigues(rvec, rotate);
    Mat estimatePosition = -((Mat)rotate.t()) * tvec;
    cout << estimatePosition << endl;
}
edit retag flag offensive close merge delete