SolvePnP returns invalid result

I use the solvePnP function to evaluate my robot's pose with visual markers. Several times I get the wrong results in two frames of contractors. In the problem.cpp file, you can see one of these results.

Sets of points correspond to the same marker in two integer frames. The difference between them is very small, and the result of the PnP solution is very different, but only in the rotation vector. The translation of the text is in order.

This happens approximately once every 30 frames. I tested the CV_ITERATIVE and CV_P3P method with the same data, and they return the same result.

This is an example of a problem:

#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/core.hpp>
#include <iostream>
using namespace std;
using namespace cv;
int main(){
vector<Point2f> points1, points2;

//First point set
points1.push_back(Point2f(384.3331f,  162.23618f));
points1.push_back(Point2f(385.27521f, 135.21503f));
points1.push_back(Point2f(409.36746f, 139.30315f));
points1.push_back(Point2f(407.43854f, 165.64435f));

//Second point set
points2.push_back(Point2f(427.64938f, 158.77661f));
points2.push_back(Point2f(428.79471f, 131.60953f));
points2.push_back(Point2f(454.04532f, 134.97353f));
points2.push_back(Point2f(452.23096f, 162.13156f));

//Real object point set
vector<Point3f> object;
object.push_back(Point3f(-88.0f,88.0f,0));
object.push_back(Point3f(-88.0f,-88.0f,0));
object.push_back(Point3f(88.0f,-88.0f,0));
object.push_back(Point3f(88.0f,88.0f,0));

//Camera matrix
Mat cam_matrix = Mat(3,3,CV_32FC1,Scalar::all(0));
cam_matrix.at<float>(0,0) = 519.0f;
cam_matrix.at<float>(0,2) = 320.0f;
cam_matrix.at<float>(1,1) = 522.0f;
cam_matrix.at<float>(1,2) = 240.0f;
cam_matrix.at<float>(2,2) = 1.0f;

//PnP
Mat rvec1i,rvec2i,tvec1i,tvec2i;
Mat rvec1p,rvec2p,tvec1p,tvec2p;
solvePnP(Mat(object),Mat(points1),cam_matrix,Mat(),rvec1i,tvec1i,false,CV_ITERATIVE);
solvePnP(Mat(object),Mat(points2),cam_matrix,Mat(),rvec2i,tvec2i,false,CV_ITERATIVE);
solvePnP(Mat(object),Mat(points1),cam_matrix,Mat(),rvec1p,tvec1p,false,CV_P3P);
solvePnP(Mat(object),Mat(points2),cam_matrix,Mat(),rvec2p,tvec2p,false,CV_P3P);

//Print result
cout<<"Iterative: "<<endl;
cout<<" rvec1 "<<endl<<" "<<rvec1i<<endl<<endl;
cout<<" rvec2 "<<endl<<" "<<rvec2i<<endl<<endl;
cout<<" tvec1 "<<endl<<" "<<tvec1i<<endl<<endl;
cout<<" tvec1 "<<endl<<" "<<tvec2i<<endl<<endl;

cout<<"P3P: "<<endl;
cout<<" rvec1 "<<endl<<" "<<rvec1p<<endl<<endl;
cout<<" rvec2 "<<endl<<" "<<rvec2p<<endl<<endl;
cout<<" tvec1 "<<endl<<" "<<tvec1p<<endl<<endl;
cout<<" tvec1 "<<endl<<" "<<tvec2p<<endl<<endl;

return 0;

}

And this is the result:

Iterative: 
rvec1 
[-0.04097605099283788; -0.3679435501353919; 0.07086072250132323]
rvec2 
[0.4135950235376482; 0.6834759799439329; 0.1049879790744613]
tvec1 
[502.4723979671957; -582.21069174737; 3399.430492848247]
tvec2 
[774.9623278021523; -594.8332356366083; 3338.42153723169]
P3P: 
rvec1 
[-0.08738607323881876; -0.363959462471951; 0.06617591006606272]
 rvec2 
[0.4239629869157338; 0.7210136877984544; 0.1133539043199323]
tvec1 
[497.3941378807597; -574.3015171812298; 3354.522829883918]
tvec1 
[760.2641421675842; -582.2718972605966; 3275.390313948845]

Thank.

+5
source share
1 answer

, 640x480 . , - .

Observed markers

. , . x y. , . .

, projectPoints.

// Compute covariance matrix of rotation and translation
Mat J;
vector<Point2f> p;
projectPoints(object, rvec1i, tvec1i, cam_matrix, Mat(), p, J);
Mat Sigma = Mat(J.t() * J, Rect(0,0,6,6)).inv();

// Compute standard deviation
Mat std_dev;
sqrt(Sigma.diag(), std_dev);
cout << "rvec1, tvec1 standard deviation:" << endl << std_dev << endl;

rvec1, tvec1:
[0,0952506404906549; +0,09211686006979068; +0,02923763901152477; +18,61834775099151; +21,61443561870643; +124,9111908058696]

, , ( ). , x y , z, .

matlab, :

imagesc(sqrt(abs(Sigma)))

Covariance visualization

, z x y ( ). , .

, Extended Kalman Filter - . , , . OpenCV , .

, , , - !

+9

All Articles