SolvePnPRansac always returns an identity rotation matrix
I am using OpenCV 3.1 and I am trying to estimate the rotation between two frames of a camera. First, I detect some keypoints, define their descriptors and match them. Finally, I have to see the rotation with solvePnPRansac (supposing all points lay on the z=0 plane), but it does always return the identity matrix as the rotation even though the image is rotated. Here is the part of the code that does not seem to work: (Get_Euler_angles is a function I made and works:
std::vector< DMatch > good_matches;
for( int i = 0; i < descriptors_object.rows; i++ )
{
if( matches[i].distance <= factor*min_dist )
{ good_matches.push_back( matches[i]); }
}
std::vector<Point3f> obj3;
std::vector<Point2f> scene2;
Point3f aux3;
Point2f aux2;
for( size_t i = 0; i < good_matches.size(); i++ )
{
//-- Get the keypoints from the good matches
aux3.x=keypoints_object[ good_matches[i].queryIdx ].pt.x;
aux3.y=keypoints_object[ good_matches[i].queryIdx ].pt.y;
aux3.z=0;
obj3.push_back(aux3);
aux2.x=keypoints_object[ good_matches[i].queryIdx ].pt.x;
aux2.y=keypoints_object[ good_matches[i].queryIdx ].pt.y;
scene2.push_back(aux2);
}
Mat rvec, tvec, rmat;
Mat cameraMatrix = (Mat_<float>(3,3) << 374.67,0,320.5,0,374.67,180.5,0,0,1);
solvePnPRansac( obj3, scene2, cameraMatrix, noArray(), rvec, tvec);
Rodrigues(rvec, rmat);
std::vector<double> euler_angles (3);
std::cout << "rvec from solvePnPRansac: " << std::endl << rvec << std::endl;
std::cout << "rmat from solvePnPRansac: " << std::endl << rmat << std::endl;
euler_angles=get_Euler_angles(rmat);
std::cout << "euler angles from solvePnPRansac: " << std::endl << "[" << euler_angles[0] << " "<< euler_angles[1] << " " << euler_angles[2] << "]" << std::endl;
std::cout << "tvec from solvePnPRansac: " << std::endl << tvec << std::endl;
If I use two images as input I get the matches and a translation vector, but rotation vector is empty:
rvec from solvePnPRansac:
[0;
0;
0]
rmat from solvePnPRansac:
[1, 0, 0;
0, 1, 0;
0, 0, 1]
euler angles from solvePnPRansac:
[-0 0 0]
tvec from solvePnPRansac:
[-320.5000258050974;
-180.4999954623273;
374.6697010743459]