Simulate camera with distortion in pyBullet

Question

I am trying to configure a camera in pyBullet based on intrinsic and extrinsic parameters obtained from calibrating a real camera.

What I have

The camera is calibrated with OpenCV, giving me a camera matrix

|f_x  0  c_x|
| 0  f_y c_y|
| 0   0   1 |

and a vector of distortion coefficients

(k_1, k_2, p_1, p_2, k_3)

(I further have the pose of the camera but this is not relevant for the actual question, so I leave it out here.)

What I already did

Unfortunately the computeProjectionMatrix function of pyBullet is a bit limited. It assumes f_x = f_y and c_x, c_y being exactly at the center of the image, which is both not true for my camera. Therefore I compute the projection matrix myself as follows (based on this):

projection_matrix = [
    [2/w * f_x,  0,  (w - 2c_x)/w,  0],
    [0,  2/h * f_y,  (2c_y - h)/h,  0],
    [0, 0, A, B],
    [0, 0, -1, 0],
]

where w,h are width and height of the image, A = (near + far)/(near - far) and B = 2 * near * far / (near - far), near and far defining the range on the z-axis that is included in the image (see pybullet.computeProjectionMatrix).

What is still missing (my actual question)

The above already gives me better results but the rendered images it still don't match exactly with the real images. I suspect one reason for this might be that the distortion is not taken into account.

So finally coming to my question:

How can I implement distortion for the simulated camera using the parameters I got from calibrating the real camera?

Is there a way I can integrate this in the projection matrix? If not, is there an other way?

Answer 1

As pointed out in the comments, non-linear distortion cannot be integrated into the matrices. What I am doing now is to first render the image without distortion and then distort the resulting image in a second step, using the code from this answer.

The image shrinks a bit due to the distortion, so when keeping the image size fixed there will be some empty area at the edges of the image. To compensate for this, I render the image at a slightly larger size than needed and then crop after distorting. Note that the centre point (c_x, c_y) needs to be adjusted accordingly, when increasing the size.

To illustrate with some pseudo code:

desired_image_size = (width, height)

# add 10% padding on each size
padding = desired_image_size * 0.1
render_image_size = desired_image_size + 2 * padding

# shift the centre point accordingly (other camera parameters are not changed)
c_x += padding[0]
c_y += padding[1]

# render image using the projection_matrix as described in the question
image = render_without_distortion(projection_matrix, camera_pose)

image = distort_image(image)

# remove the padding
image = image[padding[0]:-padding[0], padding[1]:-padding[1]]

This results in images that match very well with the ones from the real camera.

The complete implementation can be found here.

Answer 2

I searched extensively to find a compact answer to constructing the view and projection matrices using calibrated K and ROS TF extrinsic poses but to my amusement, I found none.

I wrote and tested the following two functions which computes the matrices required for simulating real cameras in pybullet. Hopefully it will be useful:

from pyquaternion import Quaternion
import numpy as np


def cvK2BulletP(K, w, h, near, far):
    """
    cvKtoPulletP converst the K interinsic matrix as calibrated using Opencv
    and ROS to the projection matrix used in openGL and Pybullet.

    :param K:  OpenCV 3x3 camera intrinsic matrix
    :param w:  Image width
    :param h:  Image height
    :near:     The nearest objects to be included in the render
    :far:      The furthest objects to be included in the render
    :return:   4x4 projection matrix as used in openGL and pybullet
    """ 
    f_x = K[0,0]
    f_y = K[1,1]
    c_x = K[0,2]
    c_y = K[1,2]
    A = (near + far)/(near - far)
    B = 2 * near * far / (near - far)

    projection_matrix = [
                        [2/w * f_x,  0,          (w - 2*c_x)/w,  0],
                        [0,          2/h * f_y,  (2*c_y - h)/h,  0],
                        [0,          0,          A,              B],
                        [0,          0,          -1,             0]]
    #The transpose is needed for respecting the array structure of the OpenGL
    return np.array(projection_matrix).T.reshape(16).tolist()


def cvPose2BulletView(q, t):
    """
    cvPose2BulletView gets orientation and position as used 
    in ROS-TF and opencv and coverts it to the view matrix used 
    in openGL and pyBullet.
    
    :param q: ROS orientation expressed as quaternion [qx, qy, qz, qw] 
    :param t: ROS postion expressed as [tx, ty, tz]
    :return:  4x4 view matrix as used in pybullet and openGL
    
    """
    q = Quaternion([q[3], q[0], q[1], q[2]])
    R = q.rotation_matrix

    T = np.vstack([np.hstack([R, np.array(t).reshape(3,1)]),
                              np.array([0, 0, 0, 1])])
    # Convert opencv convention to python convention
    # By a 180 degrees rotation along X
    Tc = np.array([[1,   0,    0,  0],
                   [0,  -1,    0,  0],
                   [0,   0,   -1,  0],
                   [0,   0,    0,  1]]).reshape(4,4)
    
    # pybullet pse is the inverse of the pose from the ROS-TF
    T=Tc@np.linalg.inv(T)
    # The transpose is needed for respecting the array structure of the OpenGL
    viewMatrix = T.T.reshape(16)
    return viewMatrix

The above two functions give you the required matrices to get images from the pybullet environment like this:

projectionMatrix = cvK2BulletP(K, w, h, near, far)
viewMatrix = cvPose2BulletView(q, t)

_, _, rgb, depth, segmentation = b.getCameraImage(W, H, viewMatrix, projectionMatrix, shadow = True)

Above returns images without distortion. For this, you could use the answer provided earlier.