Need help for pose estimation of an object using open cv c++

Question

I have the intrinsic and distortion matrix from a calibrated camera. I am using open cv 3.2 c++. I am getting error output of pose estimation of the object. Help me to sort out this error. The values and the error image are attached below.

Mat K = (Mat_<double>(3, 3) << 5489.58773029126, 0, 1167.86282865392, 0, 5481.84660546223, 1047.21450461614, 0, 0, 1);
Mat dist = (Mat_<double>(1, 5) << -0.111931171641671, 0.087488429523756156, 0.000844290429230941, 0.00204127293599477,0);  

I already have image and object points, so proceeding with solvepnp for getting rotation and translation vector.So,

Mat rvecs, tvecs;
vector<Point3f> end_point3D;
vector<Point2f> end_point2D;
end_point3D.push_back(Point3f(50, 0, 0));
end_point3D.push_back(Point3f(0, 50, 0));
end_point3D.push_back(Point3f(0, 0, 50));

solvePnP(Object_points, Image_points, K, dist, rvecs, tvecs);
projectPoints(end_point3D, rvecs, tvecs, K, dist, end_point2D, noArray(), 0.0);

    cv::line(image, Image_points[0], end_point2D[0], cv::Scalar(255, 0, 0), 6);
    cv::line(image, Image_points[0], end_point2D[1], cv::Scalar(0, 255, 0), 6);
    cv::line(image, Image_points[0], end_point2D[2], cv::Scalar(0, 0, 255), 6);

The image is given below.error output of estimated pose

EDIT: The object points and image points are ordered properly in the same way. I am sure about the ordering, I have done the row-major ordering.

Size sq_size(6, 6);
int  Sq_length = 30;
vector<Point3f>Object_points;

for (int r = 0; r < sq_size.height; r++)
    for (int c = 0; c < sq_size.width; c++)
        Object_points.push_back(Point3f(r*Sq_length, c*Sq_length, 0));

vector<Point2f>Image_points;
for (int i = 0; i < 36; i++)
    Image_points.push_back((Point2f)op_cent[i]);
//row-major ordered image points

I have two doubts, 1.Though they are properly ordered, sometimes the pose results wrong as shown below, What could be the reason?

Error pose output

2.If I draw, estimated pose only at Image_points[0], it comes out to be right,but not at any other points. Can someone explain why the pose is wrong at Image_points[30]? At any point the objects pose should be same right?

Answer 1

solvePnP (documentation is for OpenCV 4.0.1, visit https://docs.opencv.org/ to browse the correct documentation according to your OpenCV version) needs at least 4 points to compute the camera pose.

You are using OpenCV 3.2 but an assert has been added in later OpenCV version.

Answer 2

I currently working to write software about pose estimation. but your problem description is not really clear to me. perhaps you can make more updated view on your case.

here is my opinions that might be helpful:

I use initial rotation and translation in rvecs and tvecs, I input those initial position as mat object, I need 3 initial rotation and 3 initial position. the solvePnP will take it as initial starting point for further calculation. it gives me pretty accurate results.

for your code, you give too little information for me to make a comment on what was going on before/after those code.