# Obtain Camera Pose and camera real world position using SolvePnP C++

I am trying to measure the pose of a camera and I have done the following.

- Mark world 3-D(Assuming z=0, since it is flat) points on corners of a square on a flat surface and assume a world coordinate system.(in cms)

Have taken the top left corner of the square as my origin and given the world points in the following order(x,y)or(col,row): (0,0),(-12.8,0),(-12.8,12.8),(0,12.8) - in cms

Detect those points in my image.(in pixels) The image points and world points are in the same order.

I have calibrated my camera for

**intrinsic matrix**and**distortion coefficients**.I use

**SolvePnP**function to get rvec and tvec.I use

**Rodrigues**function to get rotation matrix.To check if rvec and tvec is correct, I project back the 3-D points(z=0) using

**ProjectPoints**into the image plane and I get the points correctly on my image with an error of 3 pixels on X- axis.Now I go ahead and calculate my camera position in the world frame using the formula:

**cam_worl_pos = - inverse(R) * tvec**. (This formula I have verified in many blogs and also this makes sense)

- But my
**cam_worl_pos**x,y, and z in cms do not seem to be correct.

**My doubt is, if I am able to project back the 3-D world point to image plane using rvec and tvec with (3 pixel error on X-axis and almost no error on Y axis, hope it is not too bad), then why am I not getting the camera position in world frame right.**

Also, I have a doubt on SolvPnP rvec and tvec solution, they might be one of the multiple solutions, but not the one which I want.

How do I get the right rvec and tvec from SolvPnp or any other suggestions to get rvec and tvec would also be helpful.

**EDITS** :

**Image Size** - 720(row) * 1280(col)

**Calibration pattern seen by camera**

The link has the picture of calibration pattern used.

**New Edits**

**World coordinate system following Right Hand Rule and the corresponding points detected in the image**

The left square is my world coordinate system which is a square of sides 12.8cm, the top left corner is the world origin (0,0). The red points are the 3-D world points detected in the image.

The image seen is after radial distortion correction of a fish eye lens camera.

**camera parameters**

```
cameraMatrix_Front=[908.65 0 642.88
0 909.28 364.95
0 0 1]
distCoeffs_Front=[-0.4589, 0.09462, -1.46*10^-3, 1.23*10^-3]
```

**OpenCV C++ code**:

```
vector<Point3f> front_object_pts;
Mat rvec_front;
Mat tvec_front;
Mat rotation_front;
Mat world_position_front_cam;
//Fill front object points(x-y-z order in cms)
//It is square of side 12.8cms on Z=0 plane
front_object_pts.push_back(Point3f(0, 0 ...
```

It's a fisheye camera, did you calibrate using the cv::fisheye::calibrate?

Hi! i am also having exactly the same problem. Did you find a solution for this?