Почему моя функция solvePNP () в opencv дает неправильные результаты, а MATLAB — более разумные?

Деталь:
Я использую solvePnP () в OpenCV3 в ROS Kinetic, чтобы оценить позу моего робота по светодиодным маркерам.
Разрешение камеры 128 * 128.
Когда я запускаю свой код в opencv, он всегда дает нелепые результаты.
Кажется, я не понял PNP, но MATLAB дает более разумные результаты.
Я все еще запутался после того, как прочитал документы OpenCV и Matlab.
Я не знаю, где я ошибаюсь.

Вот код C ++:

vector<Point2f> p2d;
p2d.push_back(Point2f(65,55));
p2d.push_back(Point2f(91,57));
p2d.push_back(Point2f(63,83));
p2d.push_back(Point2f(90,85));

vector<Point3f> p3d;
p3d.push_back(Point3f(0.687076330185,0.966806054115,1.80424296856));//topleft
p3d.push_back(Point3f(-0.18462985754,0.96837836504,1.79057383537));//topright
p3d.push_back(Point3f(0.691743910313,0.056151561439,1.73733282089));//bottonleft
p3d.push_back(Point3f(-0.179948031902,0.0538983009756,1.74941062927));//bottonright

Mat cameraMatrix = Mat::zeros(3,3,CV_64FC1);//fx,0,u0;0,fy,v0;0,0,1
cameraMatrix.at<double>(0,0) = 1.138594e+02;
//cameraMatrix.at<double>(0,1) = -0.0797;
cameraMatrix.at<double>(1,1) = 1.139064e+02;
cameraMatrix.at<double>(0,2) = 7.63093e+01;
cameraMatrix.at<double>(1,2) = 6.68583e+01;
cameraMatrix.at<double>(2,2) = 1;
Mat distortion = Mat::zeros(5,1,CV_64FC1);//k1,k2,p1,p2,k3
distortion.at<double>(0,0) = -2.837e-01;
distortion.at<double>(1,0) = 0.0882e+00;
distortion.at<double>(2,0) = -0.001774514973451;
distortion.at<double>(3,0) = 6.2359e-05;
distortion.at<double>(4,0) = 7.973409664780424e-04;

Mat rvec = Mat::zeros(3,1,CV_64FC1);
Mat tvec = Mat::zeros(3,1,CV_64FC1);
solvePnP(Mat(p3d),Mat(p2d),cameraMatrix,distortion,rvec,tvec,false,CV_ITERATIVE);

Результаты функции CV_ITERATIVE показаны ниже:

pose:
position:
x: 0.498402438763
y: 0.409237140455
z: 1.87770535106
orientation:
x: -0.0660728111319
y: 0.0615960086814
z: 0.995333090477
w: -0.0339463450942

Результат функции CV_EPNP показан ниже:

pose:
position:
x: 0.0447529023645
y: 0.640655332601
z: 1.92705476321
orientation:
x: 0.0605125209219
y: -0.00392024438163
z: 0.997734267547
w: -0.0291409996095

Результаты функции CV_P3P показаны ниже:

pose:
position:
x: 0.609777855777
y: 0.962804613723
z: 2.20123033932
orientation:
x: -0.114062563556
y: -0.0956252432628
z: 0.988152306053
w: -0.0374241163864

Вот код Matlab:

tangentialDistortion = [-0.001774514973451,6.230176254781805e-05];
radialDistortion = [-0.283675926885368,0.088238919987596,7.973409664780424e-04];

IntrinsicMatrix = [113.859398285567 0   0
-0.0797237890933342 113.906402839770    0
76.3090526736352    66.8576711008518    1];
cameraParams = cameraParameters('IntrinsicMatrix',IntrinsicMatrix,'RadialDistortion',radialDistortion,'TangentialDistortion',tangentialDistortion);
imagePoints = [65,55;91,57;63,83;90,85];
[undistortedPoints, reprojectionErrors] = undistortPoints(imagePoints,cameraParams);
worldPoints = [0.687080681324,0.966781198978,1.80424845219;
-0.18462985754,0.96837836504,1.79057383537;
0.691743910313,0.056151561439,1.73733282089;
-0.179948031902,0.0538983009756,1.74941062927
];
[worldOrientation,worldLocation] = estimateWorldCameraPose(undistortedPoints,worldPoints,cameraParams);

Результат функции Matlab (P3P) показан ниже:

orientation:
-0.990479259493302  0.0577122306046552  -0.124980538293912
-0.0690583764694848 -0.993684823835936  0.0884387444367233
-0.119087266964189  0.0962276951650630  0.988209721430740
location:
0.734846969941507   0.244533065842483   -1.87846167697158

Основное положение держателя камеры:

//ABC are 3 leds on camera holder
0.513234317303 ,0.13388569653   ,-1.93677461147  //A
0.199585437775 ,0.0487480945885 ,-1.94162213802  //B
0.0308324769139,0.00147355603985,-1.9436986446   //C
0.2478840773   ,0.0613691157,    -1.9406984647   //average

Кто-нибудь может мне помочь?

Эта проблема беспокоила меня долгое время, и я не мог найти никакого связанного ответа.
Спасибо за ваше время!

0

Решение

Задача ещё не решена.

Другие решения

Других решений пока нет …

По вопросам рекламы [email protected]