Rebuilt a real environment in MuJOCo: Setting the intrinsic and extrinsic camera matrix #2253

KieDani · 2024-11-26T09:08:21Z

KieDani
Nov 26, 2024

Intro

Hi!

I am a researcher at University of Augsburg, I use MuJoCo for my research on computer vision for automatic sport's video analysis. I am currently working with table tennis images. Using some known 3D points on the image, I did successfully calibrate a real camera and obtained the intrinsic and extrinsic camera matrix. Now, I want to rebuild the setup environment in MuJoCo, but I am struggling to set the camera matrix correctly. So my question is: How to set up the MuJoCo camera to rebuild an environment with given intrinsic and extrinsic camera matrices?

My setup

I am using the Python API of MuJoCo. Version 3.2.3.

My question

Currently, MuJoCo only renders a black screen, which is probably because the camera points into the wrong direction. How do I have to set up the MuJoCo environment, such that the scene is correctly rendered? The image dimensions, the intrinsic matrix and the extrinsic matrix are given.
I am especially not sure how to set up the camera intrinsics (focal, resolution, sensorsize). Moreover, I am unsure about the conventions for the sign of the forward vector.

I already checked that the camera matrices correctly map the table points to the corresponding locations in the image plain. However, it seems that MuJoCo uses some different conventions that are not obvious to me.

Thank you for your help and clarifications! I hope this thread can lead to a summary of the camera conventions used in MuJoCo.

Minimal model and/or code that explain my question

This is the main function. I want to use the given camera matrices to set up the MuJoCo environment. I added multiple prints to ensure that the parameters are set correctly. I am not sure about the sign of the forward vector (but I cannot set it directly).

def test_render(Mext, Mint):
    '''Using Mujoco to render the table tennis scene with the given camera matrices as sanity check.
    Args:
        Mexts (np.array): Extrinsic matrices.
        Mints (np.array): Intrinsic matrices.
    '''
    fx, fy, cx, cy = Mint[0, 0], Mint[1, 1], Mint[0, 2], Mint[1, 2]
    # camera location
    R = Mext[:3, :3]
    t = Mext[:3, 3]
    c = -np.dot(R.T, t)
    # right direction
    r = R[:, 0]
    # up direction
    u = R[:, 1]


    XML = f"""
        <mujoco>
          <option cone="elliptic" gravity="0 0 -9.81" integrator="implicit" timestep="0.0001" density="1.2" viscosity="0.00002" />
          <asset>
            <texture name="table_texture" type="cube" filefront="table.png" fileup="black.png" filedown="black.png" fileleft="black.png" fileright="black.png" />
            <material name="table_material" reflectance=".2" texture="table_texture" texuniform="false"/>
            <texture name="net_texture" type="cube" fileleft="net.png" fileright="net.png" />
            <material name="net_material" reflectance="0" texture="net_texture" texuniform="false" texrepeat="1 1" />
          </asset>
          <visual>
            <global offheight="{HEIGHT}" offwidth="{WIDTH}"/>
          </visual>
          <worldbody>
            <light diffuse=".5 .5 .5" pos="-2 -2 10" dir="0.1 0.1 -1"/>
            <geom name="table_geom" type="box" pos="0 0 0.38" size="1.53 0.765 0.38" material="table_material"/>
            <geom name="net_geom" type="box" pos="0 0 0.76" size="0.02 0.9175 0.1525" material="net_material" rgba="1 1 1 0.6" />
            <body name="ball" pos="-0.5 0 1">
                <joint type="free"/>
                <geom type="sphere" size="0.02" rgba="1 0 0 1"/>
            </body>
            <!-- Define the camera -->
            <camera name="main" 
            focal="{fx} {fy}" 
            resolution="{WIDTH} {HEIGHT}" 
            principal="{cx} {cy}" 
            sensorsize="1 1" 
            pos="{c[0]} {c[1]} {c[2]}" 
            mode="fixed" 
            xyaxes="{r[0]} {r[1]} {r[2]} {u[0]} {u[1]} {u[2]}"/>
          </worldbody>
        </mujoco>
        """
    # initialize the simulation
    model = mujoco.MjModel.from_xml_string(XML)
    data = mujoco.MjData(model)

    renderer = mujoco.Renderer(model, HEIGHT, WIDTH)
    mujoco.mj_step(model, data)
    renderer.update_scene(data, camera='main')

    print('R', R.flatten())
    print('data.cam_xmat[0]', data.cam_xmat[0])
    print('c', c)
    print('data.cam_xpos[0]', data.cam_xpos[0])

    print('-----')

    print('c', c)
    print('renderer.scene.camera[0].pos', renderer.scene.camera[0].pos)
    print('f', -R[:, 2])
    print('renderer.scene.camera[0].forward', renderer.scene.camera[0].forward)
    print('u', u)
    print('renderer.scene.camera[0].up', renderer.scene.camera[0].up)
    print('fx, fy, cx, cy', fx, fy, cx, cy)
    print('renderer.model.cam_intrinsic[camera_id]', renderer.model.cam_intrinsic[0])


    img = renderer.render()
    plt.imshow(img)
    plt.show()

This code is used as sanity check to ensure that the camera matrices transform the table points correctly to image coordinates:

HEIGHT, WIDTH = 1440, 2560
table_points = np.array([
    [-1.53, 0.765, 0.76], # 0 close left
    [-1.53, -0.765, 0.76], # 1 close right
    [0.0, 0.765, 0.76], # 2 center left
    [0.0, -0.765, 0.76], # 3 center right
    [1.53, 0.765, 0.76], # 4 far left
    [1.53, -0.765, 0.76], # 5 far right
    [0.0, 0.917, 0.76], # 6 net left bottom
    [0.0, -0.917, 0.76], # 7 net right bottom
    [0.0, 0.0, 0.76], # 8 net center bottom
    [0.0, 0.917, 0.912], # 9 net left top
    [0.0, -0.917, 0.912], # 10 net right top
    [-1.53, 0, 0.76], # 11 close center
    [1.53, 0, 0.76], # 12 far center
])
table_connections = [
    (0, 2), (2, 4), # left side
    (1, 3), (3, 5), # right side
    (0, 1), (4, 5) , # front side + back side
    (6, 2), (2, 3), (3, 7), # center line
    (6, 9), (7, 10), (9, 10), # net
    (11, 8), (12, 8), # middle line
]

def plot_table(Mint, Mext, img_path=None):
    '''Plot the table points in the image plane.
    Args:
        Mints (np.array): Intrinsic matrix.
        Mexts (np.array): Extrinsic matrix.
        img_path (str): Path to the image of the scene (optional)
    '''
    table_cam = world2cam(table_points, Mext)
    table_img = cam2img(table_cam, Mint)
    if img_path is None:
        # draw lines between the table points
        for connection in table_connections:
            plt.plot(table_img[connection, 0], table_img[connection, 1], 'k')
        # draw close left point of table in purple
        plt.scatter(table_img[0, 0], table_img[0, 1], c='purple', label='Table close left')
        # draw top right net point of table in orange
        plt.scatter(table_img[10, 0], table_img[10, 1], c='orange', label='Table net right top')
        # draw bottom right net point of table in pink
        plt.scatter(table_img[7, 0], table_img[7, 1], c='pink', label='Table net right bottom')
        plt.xlabel('X')
        plt.ylabel('Y')
        plt.xlim(0, WIDTH)
        plt.ylim(HEIGHT, 0)
        plt.legend()
        plt.show()
    else:
        from matplotlib import image
        data = image.imread(img_path)
        for connection in table_connections:
            plt.plot(table_img[connection, 0], table_img[connection, 1], 'k')
        # draw close left point of table in purple
        plt.scatter(table_img[0, 0], table_img[0, 1], c='purple', label='Table close left')
        # draw top right net point of table in orange
        plt.scatter(table_img[10, 0], table_img[10, 1], c='orange', label='Table net right top')
        # draw bottom right net point of table in pink
        plt.scatter(table_img[7, 0], table_img[7, 1], c='pink', label='Table net right bottom')
        plt.imshow(data)
        plt.show()

def cam2img(r_cam, Mints):
    '''Project a batch of 3D points to image coordinates.'''
    if len(r_cam.shape) == 1:
        if len(Mints.shape) == 2:
            r_img = np.einsum('i j, j -> i', Mints, r_cam)
            r_img = r_img[:2] / r_img[2]
        else:
            raise ValueError('Shape not supported.')
    elif len(r_cam.shape) == 2:
        if len(Mints.shape) == 3:
            r_img = np.einsum('b i j, b j -> b i', Mints, r_cam)
            r_img = r_img[:, :2] / r_img[:, 2:3]
        elif len(Mints.shape) == 2:
            r_img = np.einsum('i j, b j -> b i', Mints, r_cam)
            r_img = r_img[:, :2] / r_img[:, 2:3]
        else:
            raise ValueError('Shape not supported.')
    else:
        raise ValueError('Shape not supported.')
    return r_img


def world2cam(r_world, Mexts):
    '''Transform a batch of 3D points from world to camera coordinates.'''
    if len(r_world.shape) == 1:
        if len(Mexts.shape) == 2:
            r_world = np.concatenate([r_world, np.ones_like(r_world[0:1])])
            r_cam = np.einsum('i j, j -> i', Mexts, r_world)
            r_cam = r_cam[:3] / r_cam[3]
        else:
            raise ValueError('Shape not supported.')
    elif len(r_world.shape) == 2:
        if len(Mexts.shape) == 3:
            r_world = np.concatenate([r_world, np.ones((r_world.shape[0], 1))], axis=1)
            r_cam = np.einsum('b i j, b j -> b i', Mexts, r_world)
            r_cam = r_cam[:, :3] / r_cam[:, 3:4]
        elif len(Mexts.shape) == 2:
            r_world = np.concatenate([r_world, np.ones((r_world.shape[0], 1))], axis=1)
            r_cam = np.einsum('i j, b j -> b i', Mexts, r_world)
            r_cam = r_cam[:, :3] / r_cam[:, 3:4]
        else:
            raise ValueError('Shape not supported.')
    else:
        raise ValueError('Shape not supported.')
    return r_cam

Here I define the intrinsic and extrinsic matrix. Importantly, fx and fy differ since I am working with real cameras. In my case, both fx and fy are positive. Is this a problem in MuJoCo?

if __name__ == '__main__':
    Mint = np.array([
        [2710, 0.0, (WIDTH - 1) / 2],
        [0.0, 2907, (HEIGHT - 1) / 2],
        [0.0, 0.0, 1.0]
    ])
    Mext = np.array([[-9.99994361e-01, 1.26002403e-03, 3.11287461e-03, 1.18661176e-03],
                     [-2.27324391e-03, 4.28299688e-01, -9.03633891e-01, 9.95210820e-01],
                     [-2.47184365e-03, -9.03635872e-01, -4.28294409e-01, 1.07987652e+01],
                     [0.0, 0.0, 0.0, 1.0]])
    test_render(Mext, Mint)
    plot_table(Mint, Mext, img_path='mypath/singlescene.png') # uncomment this line if you want to run the code
    plot_table(Mint, Mext)

The output of the print statements is as follows. Please note that renderer.scene.camera[0].pos gives unexpected values. Moreover I am not sure if the sign of renderer.scene.camera[0].forward is correct.

R [-0.99999436  0.00126002  0.00311287 -0.00227324  0.42829969 -0.90363389
 -0.00247184 -0.90363587 -0.42829441]
data.cam_xmat[0] [-0.99999436  0.00126002  0.00311287 -0.00227324  0.42829969 -0.90363389
 -0.00247184 -0.90363587 -0.42829441]
c [0.03014182 9.33190163 5.52435329]
data.cam_xpos[0] [0.03014182 9.33190163 5.52435329]
-----
c [0.03014182 9.33190163 5.52435329]
renderer.scene.camera[0].pos [0.06414163 9.331979   5.5244374 ]
f [-0.00311287  0.90363389  0.42829441]
renderer.scene.camera[0].forward [-0.00311287  0.9036339   0.42829442]
u [ 0.00126002  0.42829969 -0.90363587]
renderer.scene.camera[0].up [ 0.00126002  0.4282997  -0.90363586]
fx, fy, cx, cy 2710.0 2907.0 1279.5 719.5
renderer.model.cam_intrinsic[camera_id] [2710.  2907.  1279.5  719.5]

I attached the created images:
The rendered image is simply black. Probably due to facing the wrong direction or because of a severe mistake in setting the camera intrinsics.

My sanity checks show that the 3D table points are correctly transformed into image coordinates. So the camera matrices have to be correct. I am thus probably either using a wrong camera convention or I make a mistake in the xml.

Confirmations

I searched the latest documentation thoroughly before posting.
I searched previous Issues and Discussions, I am certain this has not been raised before.

yuvaltassa · 2024-11-26T15:35:32Z

yuvaltassa
Nov 26, 2024
Maintainer

Re camera convention, this is well documented, so I don't think you searched very hard. MuJoCo uses right-handed frames and the X and Y axis point right and up in the image space, so the camera looks in the negative Z direction.

Regarding instrisics, there is some documentation here, but I think you might need to look at the code for more information. For example the camprojection sensor's implementation is here

0 replies

yj-Tang · 2025-01-07T09:35:20Z

yj-Tang
Jan 7, 2025

Hi @KieDani, have you solved your problem? I experienced the same case as you. After the camera intrinsic parameters are set, the rendered image can not be achieved any more. There pops an error: XML Error: Schema violation: unrecognized attribute: 'focalpixel'. I am using the same version of mujoco as you. And I followed the official document,
<camera name="front" mode="fixed" pos="0.073155193856 0.137483875 -0.10038647848849" quat = "-0.1440867 -0.67697673 -0.1873971 0.69701066" focalpixel="912.36 910.26" resolution="1280 720" principal="648.57 363.66" sensorsize="1 1" />

While when the camera intrinsic parameters is not set, it works well. There is nothing I can find related to setting the camera intrinsic on the internet. @yuvaltassa Do you maybe have any suggestion on this point?

0 replies

KieDani · 2025-01-07T10:05:15Z

KieDani
Jan 7, 2025
Author

Given fx, fy, c, u, r (calculated as in my example above), I now set the camera stuff in the xml as

camera name="main" 
            focal="{fx/WIDTH} {fy/HEIGHT}"
            resolution="{WIDTH} {HEIGHT}"
            sensorsize="1 1"
            pos="{c[0]} {c[1]} {c[2]}" 
            mode="fixed" 
            xyaxes="{r[0]} {r[1]} {r[2]} {u[0]} {u[1]} {u[2]}"/>

I obtain the intrinsic and extrinsic camera matrices from the mujoco model as

def _calc_cammatrices(data, camera_name):
    camera_id = mujoco.mj_name2id(data.model, mujoco.mjtObj.mjOBJ_CAMERA, camera_name)
    weird_R = eo.rearrange(data.cam_xmat[camera_id], '(i j) -> i j', i=3, j=3).T
    R = np.eye(3)
    R[0, :] = weird_R[0, :]
    R[1, :] = - weird_R[1, :]
    R[2, :] = - weird_R[2, :]
    cam_pos = data.cam_xpos[camera_id]
    t = -np.dot(R, cam_pos)

    ex_mat = np.eye(4)
    ex_mat[:3, :3] = R
    ex_mat[:3, 3] = t

    fx = data.model.cam_intrinsic[camera_id][0] / data.model.cam_sensorsize[camera_id][0] * data.model.cam_resolution[camera_id][0]
    fy = data.model.cam_intrinsic[camera_id][1] / data.model.cam_sensorsize[camera_id][1] * data.model.cam_resolution[camera_id][1]
    cx = (WIDTH - 1) / 2
    cy = (HEIGHT - 1) / 2
    in_mat = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1], [0, 0, 0]])

    return ex_mat, in_mat

I hope this is enough to help you.

0 replies

yj-Tang · 2025-01-08T03:29:48Z

yj-Tang
Jan 8, 2025

Thanks for the quick reply! I fixed it. And my problem was still the version mismatch

0 replies

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Rebuilt a real environment in MuJOCo: Setting the intrinsic and extrinsic camera matrix #2253

{{title}}

{{editor}}'s edit

{{editor}}'s edit

Replies: 4 comments

{{title}}

{{title}}

{{editor}}'s edit

{{editor}}'s edit

{{title}}

{{title}}

Select a reply

Rebuilt a real environment in MuJOCo: Setting the intrinsic and extrinsic camera matrix #2253

KieDani Nov 26, 2024

Intro

My setup

My question

Minimal model and/or code that explain my question

Confirmations

Replies: 4 comments

yuvaltassa Nov 26, 2024 Maintainer

yj-Tang Jan 7, 2025

KieDani Jan 7, 2025 Author

yj-Tang Jan 8, 2025

KieDani
Nov 26, 2024

yuvaltassa
Nov 26, 2024
Maintainer

yj-Tang
Jan 7, 2025

KieDani
Jan 7, 2025
Author

yj-Tang
Jan 8, 2025