Intrinsic and Extrinsic Camera Properties

Post Reply
goktug
Posts: 4
Joined: Mon Oct 21, 2019 1:41 pm

Intrinsic and Extrinsic Camera Properties

Post by goktug »

I need to use calibration matrices of pybullet camera (intrinsic and extrinsic camera matrices) in my project. However, I could not find them. As far as I know, pybullet only provides view matrix and projection matrix of the camera. According to my research, pybullet benefits from OpenGL.

I have checked and examined OpenGL documentation to construct intrinsic and extrinsic matrices from projection and view matrix. However, I could not find a clear conversion. What I have found up to now is two of three components in intrinsic matrix in pybullet by making extensive amount of calculation and research:

Intrinsic Components:
------------------------------
* Focal length values (fx, fy)
* Two principal point values (cx, cy)
* Pixel Skew (s)

What I found:
------------------
* Focal Length: Since there is only one camera eye, there is only one focal length, which is equal to 1 / tan(q/2) in which q refers to the angle "field of view" in computeProjectionMatrixFOV function.

* Principal Values: The principal points are indices of middle pixel in the image, which is 250 for 500x500 image.

However, In spite of my 2 days research, I could not find pixel skew value. Is there anyone who can provide pixel skew and extrinsic matrix of pybullet camera image ? I think that we need these matrices for many computer vision purposes.
zachjiang
Posts: 1
Joined: Thu Jun 17, 2021 3:09 pm

Re: Intrinsic and Extrinsic Camera Properties

Post by zachjiang »

Check out this post in stackoverflow:
https://stackoverflow.com/questions/604 ... 0#60450420

The skew is 0
Emkey
Posts: 3
Joined: Thu Feb 03, 2022 12:45 pm

Re: Intrinsic and Extrinsic Camera Properties

Post by Emkey »

I stumbled across your post and hoped it would work. I will share my results with you now. Basically I got an RGBD image I managed to get finally, and from how I set the camera, the pinhole prime default settings makes the camera be in a different position (you will be able to see it in the blank void spaces of the images). The depth of the pointcloud and the following depth plot are correct in the sense that depth is shown. So it means that the RGBD image is also in a correct format and properly processed (??) The first image is the pointcloud of a table, a robotic arm and a rectangular object on top. The second image is the same pointcloud, but turning it with my mouse, as it is in 3D. The third image is the depth plot. As you can see, there are some blanks that are where the real camera should be pointing.

Code: Select all

    pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
    rgbd_image,
    o3d.camera.PinholeCameraIntrinsic(
        o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault))
    # Flip it, otherwise the pointcloud will be upside down.
    pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])

    o3d.visualization.draw_geometries([pcd])
depth_shown.png
depth_shown.png (55.86 KiB) Viewed 31914 times
But when changing it to your calculations made + the post I had already checked and rechecked to try and get my head around something, I got this. Taking into account I know fov.

Code: Select all

    q = fov
    Fx = 1/(math.tan(q/2))
    Fy = Fx

    Cx = IMG_SIZE/2
    Cy = Cx

    cam = o3d.camera.PinholeCameraIntrinsic(IMG_SIZE, IMG_SIZE, Fx, Fy, Cx, Cy)
    
    pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
        rgbd_image,cam)
As you can see in the following images, the camera now is in the perfect position, so the calculations seem right. But for some reason, the depth is not getting displayed. The Pointlcoud and the same pointcloud turning it a bit with the mouse, and the depth plot, are completely flat.
depth_not_shown.png
depth_not_shown.png (52.97 KiB) Viewed 31914 times
Rooholla
Posts: 1
Joined: Sun Feb 05, 2023 7:18 pm

Re: Intrinsic and Extrinsic Camera Properties

Post by Rooholla »

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 no easy already coded solutions.

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

Code: Select all

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:

Code: Select all

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

    _, _, rgb, depth, segmentation = b.getCameraImage(W, H, viewMatrix, projectionMatrix, shadow = True)
    plt.imshow(rgb)
Above returns images without distortion. Hopefully it will be useful.
fillipyang7
Posts: 1
Joined: Sat Jun 11, 2022 9:43 am
Contact:

Re: Intrinsic and Extrinsic Camera Properties

Post by fillipyang7 »

Rooholla wrote: Sun Feb 05, 2023 7:23 pm 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 no easy already coded solutions.

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

Code: Select all

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:

Code: Select all

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

    _, _, rgb, depth, segmentation = b.getCameraImage(W, H, viewMatrix, projectionMatrix, shadow = True)
    plt.imshow(rgb)
Above returns images without distortion. Hopefully it will be useful.
Thanks a lot for help! Thanks to your solution, I found a mistake, now everything is clear.
Post Reply