2

I have a car with a camera, an imu and a gps on top of it. The camera takes pictures while driving around. The pictures and the gps coordinates are synchronized. Now, I am trying to get the camera pose from the pictures (relative to the imu), but solvePnP does not end up with an extrinsic camera calibration that is even similar to my expected results.

For more background: I am using Python (3.6.8) with OpenCV 4.0.0.

The following data I know:

  1. I have 8 markers and their exact positions in world coordinates.
  2. I have the trajectory of the imu in world coordinates.
  3. I have pixel coordinates of the markers for several pictures (6576 x 4384 px).
  4. I have the intrinsic camera calibration.
In [4]: image_points
Out[4]:
array([[1911., 2115.],
       [2443., 2631.],
       [1427., 2570.],
       [1409., 2271.],
       [1396., 1912.],
       [1549., 1770.],
       [2247., 1787.],
       [2606., 1794.]], dtype=float32)

In [5]: world_points
Out[5]: 
array([[-1.5156984e+00, -1.3494657e+00,  0.0000000e+00],
       [-2.9987667e+00,  0.0000000e+00,  0.0000000e+00],
       [-9.3132257e-10,  0.0000000e+00,  0.0000000e+00],
       [ 0.0000000e+00, -8.5239327e-01,  0.0000000e+00],
       [-1.5532847e-02, -1.8538033e+00,  0.0000000e+00],
       [-5.0486135e-01, -2.2495930e+00,  0.0000000e+00],
       [-2.5055323e+00, -2.2484162e+00,  0.0000000e+00],
       [-3.4857810e+00, -2.2520051e+00,  0.0000000e+00]], dtype=float32)

In [6]: cameraMatrix
Out[6]: 
matrix([[ 2.81923164e+03, -1.36877792e+00,  3.26989822e+03],
        [ 0.00000000e+00,  2.81857995e+03,  2.24198230e+03],
        [ 0.00000000e+00,  0.00000000e+00,  1.00000000e+00]])

In [7]: distCoeffs
Out[7]: array([ 0.0278163 , -0.01819595, -0.01031101, -0.0023199 , -0.02813449])

Currently, my process consists of:

  1. I convert all world coordinates into a local marker coordinate system so that the z-coordinate of all markers is 0 (see Out[4] in the code example). At first, I tried it without this step but I read a lot of tutorials and it seems that the calibration somehow assumes this, but I am not sure. Please, correct me if I'm wrong.
  2. I use solvePnP to get rvec and tvec.
  3. I apply cv2.Rogrigues(rvec) to get the rotationMatrix
  4. Then, the camera position in marker coordinates is calculated by camPos = -np.matrix(rotationMatrix).T * np.matrix(tvec)

After that, I compare the distance between imu and estimated camera position and it ends up to be something around 1m, but not even consistently. These are the results for 5 sample images.

array([[0.65556006],
       [1.19668318],
       [1.37138227],
       [0.64020471],
       [0.55105675]])

However, the distance between imu and camera should be pretty exactly 2.4m (manually measured) and not change at all (since both are fixed at the top of the car).

Is it possible that solvePnP outputs wrong results or am I doing mistakes somewhere else in my process?

Similar question but with no answers would be this.

trippeljojo
  • 65
  • 1
  • 11

0 Answers0