diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index b8e1b949..0ce354ec 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -3053,6 +3053,35 @@ and the +Y axis points up. Thus the frame position and orientation are the key a dimensions are determined by the size of the rendering context. This attribute serves as a convenient location to save the required resolution when creating a context. +.. _body-camera-focal: + +:at:`focal`: :at-val:`real(2), "0 0"` + Focal length of the camera in length units. It is mutually exclusive with :ref:`fovy `. + See :ref:`CCamera` for details. + +.. _body-camera-focalpixel: + +:at:`focalpixel`: :at-val:`int(2), "0 0"` + Focal length of the camera in pixel units. If both :at:`focal`: and :at:`focalpixel`: are specified, the former is + ignored. + +.. _body-camera-principal: + +:at:`principal`: :at-val:`real(2), "0 0"` + Principal point of the camera in length units. It is mutually exclusive with :ref:`fovy `. + +.. _body-camera-principalpixel: + +:at:`principalpixel`: :at-val:`real(2), "0 0"` + Principal point of the camera in pixel units. If both :at:`principal`: and :at:`principalpixel`: are specified, the + former is ignored. + +.. _body-camera-sensorsize: + +:at:`sensorsize`: :at-val:`real(2), "0 0"` + Size of the camera sensor in length units. It is mutually exclusive with :ref:`fovy `. If + specified, :ref:`resolution ` and :ref:`focal ` are required. + .. _body-camera-ipd: :at:`ipd`: :at-val:`real, "0.068"` @@ -6700,8 +6729,6 @@ if omitted. | All site attributes are available here except: name, class. -.. _default-camera: - .. _default-camera-fovy: .. _default-camera-resolution: @@ -6724,11 +6751,26 @@ if omitted. .. _default-camera-user: +.. _default-camera-resolution: + +.. _default-camera-focal: + +.. _default-camera-focalpixel: + +.. _default-camera-principal: + +.. _default-camera-principalpixel: + +.. _default-camera-sensorsize: + +.. _default-camera: + + :el-prefix:`default/` |-| **camera** (?) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ | This element sets the attributes of the dummy :ref:`camera ` element of the defaults class. -| All camera attributes are available here except: name, class. +| All camera attributes are available here except: name, class, mode, target. .. _default-light: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 0bdc4640..b301da97 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -354,7 +354,9 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`xyaxes` | :ref:`zaxis` | :ref:`euler` | :ref:`mode` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`target` | :ref:`user` | | | | +| | | | :ref:`target` | :ref:`focal` | :ref:`focalpixel` | :ref:`principal` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`principalpixel` | :ref:`sensorsize` | :ref:`user` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| body |br| |_| |L| | | .. table:: | @@ -1230,11 +1232,13 @@ | :ref:`camera | ? | :class: mjcf-attributes | | ` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`fovy` | :ref:`ipd` | :ref:`pos` | :ref:`quat` | | +| | | | :ref:`fovy` | :ref:`ipd` | :ref:`resolution` | :ref:`pos` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`resolution` | :ref:`axisangle` | :ref:`xyaxes` | :ref:`zaxis` | | +| | | | :ref:`quat` | :ref:`axisangle` | :ref:`xyaxes` | :ref:`zaxis` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`euler` | :ref:`mode` | :ref:`user` | | | +| | | | :ref:`euler` | :ref:`mode` | :ref:`focal` | :ref:`focalpixel` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`principal` | :ref:`principalpixel` | :ref:`sensorsize` | :ref:`user` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| default |br| |_| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index 4ef82bfa..02aecec6 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -106,17 +106,26 @@ General 19. Added camera :ref:`resolution` attribute and :ref:`camprojection` sensor. If camera resolution is set to positive values, the camera projection sensor will report the location of a target site, projected onto the camera image, in pixel coordinates. +20. Added :ref:`camera` calibration attributes: + + - The new attributes are :ref:`resolution`, :ref:`focal`, + :ref:`focalpixel`, :ref:`principal`, + :ref:`principalpixel` and :ref:`sensorsize`. + - Visualize the calibrated frustum using the :ref:`mjVIS_CAMERA` visualization flag when these + attributes are specified. See the following + `example model `__. + - Note that these attributes only take effect for offline rendering and do not affect interactive visualisation. Python bindings ^^^^^^^^^^^^^^^ -20. Fixed `#870 `__ where calling ``update_scene`` with an invalid +21. Fixed `#870 `__ where calling ``update_scene`` with an invalid camera name used the default camera. Bug fixes ^^^^^^^^^ -21. Fixed a bug that was causing :ref:`geom margin` to be ignored during the construction of +22. Fixed a bug that was causing :ref:`geom margin` to be ignored during the construction of midphase collision trees. diff --git a/doc/includes/references.h b/doc/includes/references.h index b1b5be9c..3fe050c7 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -440,6 +440,7 @@ typedef enum mjtGeom_ { // type of geometric shape mjGEOM_LINE, // line mjGEOM_SKIN, // skin mjGEOM_LABEL, // text label + mjGEOM_TRIANGLE, // triangle connecting a frame mjGEOM_NONE = 1001 // missing geom type } mjtGeom; @@ -1010,6 +1011,8 @@ struct mjModel_ { mjtNum* cam_mat0; // global orientation in qpos0 (ncam x 9) int* cam_resolution; // [width, height] in pixels (ncam x 2) mjtNum* cam_fovy; // y-field of view (deg) (ncam x 1) + float* cam_intrinsic; // [focal length; principal point] (ncam x 4) + float* cam_sensorsize; // sensor size (ncam x 2) mjtNum* cam_ipd; // inter-pupilary distance (ncam x 1) mjtNum* cam_user; // user data (ncam x nuser_cam) @@ -1822,6 +1825,7 @@ struct mjvGLCamera_ { // OpenGL camera // camera projection float frustum_center; // hor. center (left,right set to match aspect) + float frustum_width; // width (not used for rendering) float frustum_bottom; // bottom float frustum_top; // top float frustum_near; // near @@ -2053,6 +2057,7 @@ struct mjvSceneState_ { mjtNum* cam_fovy; mjtNum* cam_ipd; + float* cam_sensorsize; mjtByte* light_directional; mjtByte* light_castshadow; diff --git a/doc/modeling.rst b/doc/modeling.rst index c01ae259..20718400 100644 --- a/doc/modeling.rst +++ b/doc/modeling.rst @@ -973,6 +973,31 @@ Here we describe the XML attributes common to all sensor types, so as to avoid r :at:`user`: :at-val:`real(nuser_sensor), "0 0 ..."` See :ref:`User parameters `. +.. _CCamera: + +Cameras +~~~~~~~ + +Besides the default, user-controllable, free camera, "fixed" cameras can be attached to the kinematic tree. + +Extrinsics + By default, camera frames are attached to the parent body. The optional :ref:`mode` and + :ref:`target` attributes can be used to specify camera that track (move with) or target (look at) + a body or subtree. Cameras look towards the negative Z axis of the camera frame, while positive X and Y correspond to + *right* and *up* in the image plane, respectively. + +Intrinsics + Camera intrinsics are specified using :ref:`ipd` (inter-pupilary distance, required for + stereoscopic rendering and VR) and :ref:`fovy` (vertical field of view, in degrees). + + The above specification implies a perfect point camera with no aberrations. However when calibrating real cameras, + two types of linear aberration can be expressed using standard rendering pipelines. The first is different focal + lengths in the vertical and horizontal directions (axis-aligned astigmatism). The second is a non-centered principal + point. These can be specified using the :ref:`focal` and :ref:`principal` + attributes. When these calibration-related attributes are used, the physical + :ref:`sensor size` and camera :ref:`resolution` must also be + specified. In this case, the rendering frustum can be visualized. + .. _CComposite: Composite objects diff --git a/doc/overview.rst b/doc/overview.rst index a2f980a7..68bb46a9 100644 --- a/doc/overview.rst +++ b/doc/overview.rst @@ -515,8 +515,10 @@ Camera Multiple cameras can be defined in a model. There is always a default camera which the user can freely move with the mouse in the interactive visualizer. However it is often convenient to define additional cameras that are either fixed to the world, or are attached to one of the bodies and move with it. In addition to the camera position and -orientation, the user can adjust the field of view and the inter-pupilary distance for stereoscopic rendering, as -well as create oblique projections needed for stereoscopic virtual environments. +orientation, the user can adjust the vertical field of view and the inter-pupilary distance for stereoscopic rendering, +as well as create oblique projections needed for stereoscopic virtual environments. When modeling real cameras with +imperfect optics, it is possible to specify separate focal lengths for the horizontal and vertical directions and a +non-centered principal point. Light ^^^^^ diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 3535f95f..0a6d71ba 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -111,6 +111,7 @@ typedef enum mjtGeom_ { // type of geometric shape mjGEOM_LINE, // line mjGEOM_SKIN, // skin mjGEOM_LABEL, // text label + mjGEOM_TRIANGLE, // triangle connecting a frame mjGEOM_NONE = 1001 // missing geom type } mjtGeom; @@ -742,6 +743,8 @@ struct mjModel_ { mjtNum* cam_mat0; // global orientation in qpos0 (ncam x 9) int* cam_resolution; // [width, height] in pixels (ncam x 2) mjtNum* cam_fovy; // y-field of view (deg) (ncam x 1) + float* cam_intrinsic; // [focal length; principal point] (ncam x 4) + float* cam_sensorsize; // sensor size (ncam x 2) mjtNum* cam_ipd; // inter-pupilary distance (ncam x 1) mjtNum* cam_user; // user data (ncam x nuser_cam) diff --git a/include/mujoco/mjvisualize.h b/include/mujoco/mjvisualize.h index a8fada4c..41d918b3 100644 --- a/include/mujoco/mjvisualize.h +++ b/include/mujoco/mjvisualize.h @@ -199,6 +199,7 @@ struct mjvGLCamera_ { // OpenGL camera // camera projection float frustum_center; // hor. center (left,right set to match aspect) + float frustum_width; // width (not used for rendering) float frustum_bottom; // bottom float frustum_top; // top float frustum_near; // near @@ -454,6 +455,7 @@ struct mjvSceneState_ { mjtNum* cam_fovy; mjtNum* cam_ipd; + float* cam_sensorsize; mjtByte* light_directional; mjtByte* light_castshadow; diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 77276dd8..7dea2823 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -261,6 +261,8 @@ X ( int, cam_bodyid, ncam, 1 ) \ X ( int, cam_targetbodyid, ncam, 1 ) \ X ( int, cam_resolution, ncam, 2 ) \ + XMJV( float, cam_sensorsize, ncam, 2 ) \ + X ( float, cam_intrinsic, ncam, 4 ) \ X ( mjtNum, cam_pos, ncam, 3 ) \ X ( mjtNum, cam_quat, ncam, 4 ) \ X ( mjtNum, cam_poscom0, ncam, 3 ) \ diff --git a/introspect/enums.py b/introspect/enums.py index 52a9e391..fbee5402 100644 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -92,6 +92,7 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjGEOM_LINE', 103), ('mjGEOM_SKIN', 104), ('mjGEOM_LABEL', 105), + ('mjGEOM_TRIANGLE', 106), ('mjGEOM_NONE', 1001), ]), )), diff --git a/introspect/enums_test.py b/introspect/enums_test.py index 5caa7a8c..2d53f655 100644 --- a/introspect/enums_test.py +++ b/introspect/enums_test.py @@ -61,6 +61,7 @@ class EnumsTest(absltest.TestCase): self.assertEqual(enum_decl.values['mjGEOM_ARROW'], 100) self.assertEqual(enum_decl.values['mjGEOM_ARROW1'], 101) self.assertEqual(enum_decl.values['mjGEOM_ARROW2'], 102) + self.assertEqual(enum_decl.values['mjGEOM_TRIANGLE'], 106) # Skip a few... self.assertEqual(enum_decl.values['mjGEOM_NONE'], 1001) diff --git a/introspect/structs.py b/introspect/structs.py index 677704ca..a06e9a67 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -1899,6 +1899,20 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='y-field of view (deg) (ncam x 1)', ), + StructFieldDecl( + name='cam_intrinsic', + type=PointerType( + inner_type=ValueType(name='float'), + ), + doc='[focal length; principal point] (ncam x 4)', + ), + StructFieldDecl( + name='cam_sensorsize', + type=PointerType( + inner_type=ValueType(name='float'), + ), + doc='sensor size (ncam x 2)', + ), StructFieldDecl( name='cam_ipd', type=PointerType( @@ -4730,6 +4744,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='float'), doc='hor. center (left,right set to match aspect)', ), + StructFieldDecl( + name='frustum_width', + type=ValueType(name='float'), + doc='width (not used for rendering)', + ), StructFieldDecl( name='frustum_bottom', type=ValueType(name='float'), @@ -5885,6 +5904,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='', ), + StructFieldDecl( + name='cam_sensorsize', + type=PointerType( + inner_type=ValueType(name='float'), + ), + doc='', + ), StructFieldDecl( name='light_directional', type=PointerType( diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index 4c07c3ff..277b3d0b 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -829,6 +829,7 @@ Euler integrator, semi-implicit in velocity. self.assertEqual(mujoco.mjtGeom.mjGEOM_ARROW, 100) self.assertEqual(mujoco.mjtGeom.mjGEOM_ARROW1, 101) self.assertEqual(mujoco.mjtGeom.mjGEOM_ARROW2, 102) + self.assertEqual(mujoco.mjtGeom.mjGEOM_TRIANGLE, 106) self.assertEqual(mujoco.mjtGeom.mjGEOM_NONE, 1001) def test_enum_from_int(self): diff --git a/python/mujoco/structs.cc b/python/mujoco/structs.cc index 90862175..df00498e 100644 --- a/python/mujoco/structs.cc +++ b/python/mujoco/structs.cc @@ -2044,6 +2044,7 @@ This is useful for example when the MJB is not available as a file on disk.)")); c.get()->var = rhs; \ }) X(frustum_center); + X(frustum_width); X(frustum_bottom); X(frustum_top); X(frustum_near); diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 1df8bd29..473a6475 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -251,7 +251,7 @@ void mj_defaultVisual(mjVisual* vis) { setf4(vis->rgba.actuatornegative, .2, .6, .9, 1.); setf4(vis->rgba.actuatorpositive, .9, .4, .2, 1.); setf4(vis->rgba.com, .9, .9, .9, 1.); - setf4(vis->rgba.camera, .6, .9, .6, 1.); + setf4(vis->rgba.camera, .6, .9, .6, .3); setf4(vis->rgba.light, .6, .6, .9, 1.); setf4(vis->rgba.selectpoint, .9, .9, .1, 1.); setf4(vis->rgba.connect, .2, .2, .8, 1.); diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index ca10adaa..91756051 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -188,7 +188,10 @@ static void get_xquat(const mjModel* m, const mjData* d, mjtObj type, int id, in static void cam_project(mjtNum sensordata[2], const mjtNum target_xpos[3], const mjtNum cam_xpos[3], const mjtNum cam_xmat[9], - const int cam_res[2], mjtNum cam_fovy) { + const int cam_res[2], mjtNum cam_fovy, + const float cam_intrinsic[4], const float cam_sensorsize[2]) { + mjtNum fx, fy; + // translation matrix (4x4) mjtNum translation[4][4] = {0}; translation[0][0] = 1; @@ -212,10 +215,15 @@ static void cam_project(mjtNum sensordata[2], const mjtNum target_xpos[3], } // focal transformation matrix (3x4) - mjtNum height = (mjtNum) cam_res[1]; - mjtNum fy = .5 / mju_tan(cam_fovy * mjPI / 360.) * height; + if (cam_sensorsize[0] && cam_sensorsize[1]) { + fx = cam_intrinsic[0] / cam_sensorsize[0] * cam_res[0]; + fy = cam_intrinsic[1] / cam_sensorsize[1] * cam_res[1]; + } else { + fx = fy = .5 / mju_tan(cam_fovy * mjPI / 360.) * cam_res[1]; + } + mjtNum focal[3][4] = {0}; - focal[0][0] = -fy; + focal[0][0] = -fx; focal[1][1] = fy; focal[2][2] = 1.0; @@ -307,7 +315,8 @@ void mj_sensorPos(const mjModel* m, mjData* d) { case mjSENS_CAMPROJECTION: // camera projection cam_project(d->sensordata+adr, d->site_xpos+3*objid, d->cam_xpos+3*refid, - d->cam_xmat+9*refid, m->cam_resolution+2*refid, m->cam_fovy[refid]); + d->cam_xmat+9*refid, m->cam_resolution+2*refid, m->cam_fovy[refid], + m->cam_intrinsic+4*refid, m->cam_sensorsize+2*refid); break; case mjSENS_RANGEFINDER: // rangefinder diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index 98885330..c4542204 100644 --- a/src/engine/engine_vis_interact.c +++ b/src/engine/engine_vis_interact.c @@ -722,6 +722,7 @@ mjvGLCamera mjv_averageCamera(const mjvGLCamera* cam1, const mjvGLCamera* cam2) cam.frustum_bottom = 0.5f * (cam1->frustum_bottom + cam2->frustum_bottom); cam.frustum_top = 0.5f * (cam1->frustum_top + cam2->frustum_top); cam.frustum_center = 0.5f * (cam1->frustum_center + cam2->frustum_center); + cam.frustum_width = 0.5f * (cam1->frustum_width + cam2->frustum_width); cam.frustum_near = 0.5f * (cam1->frustum_near + cam2->frustum_near); cam.frustum_far = 0.5f * (cam1->frustum_far + cam2->frustum_far); diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 19749a95..76169823 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -523,6 +523,17 @@ static void drawBoundingBox(mjvGeom* thisgeom, mjData* d, mjvScene* scn, +// computes the camera frustum +static void getFrustum(float zver[2], float zhor[2], float znear, + const float K[4], const float sensorsize[2]) { + zhor[0] = znear / K[0] * (sensorsize[0]/2.f - K[2]); + zhor[1] = znear / K[0] * (sensorsize[0]/2.f + K[2]); + zver[0] = znear / K[1] * (sensorsize[1]/2.f - K[3]); + zver[1] = znear / K[1] * (sensorsize[1]/2.f + K[3]); +} + + + // add abstract geoms void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, const mjvPerturb* pert, int catmask, mjvScene* scn) { @@ -1461,6 +1472,87 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, } } + // camera frustum + if (vopt->flags[mjVIS_CAMERA]) { + float rgba[] = {1, 1, 0, .2}; + mjtNum vnear[4][3], vfar[4][3]; + mjtNum center[3]; + mjtNum znear = m->vis.map.znear * m->stat.extent; + mjtNum zfar = m->vis.map.zfar * m->stat.extent; + float zver[2], zhor[2]; + for (int i=0; i < m->ncam; i++) { + if (m->cam_sensorsize[2*i+1] == 0) { + continue; + } + getFrustum(zver, zhor, znear, m->cam_intrinsic + 4*i, m->cam_sensorsize + 2*i); + + // frustum frame to convert from planes to vertex representation + mjtNum *cam_xpos = d->cam_xpos+3*i; + mjtNum *cam_xmat = d->cam_xmat+9*i; + mjtNum x[] = {cam_xmat[0], cam_xmat[3], cam_xmat[6]}; + mjtNum y[] = {cam_xmat[1], cam_xmat[4], cam_xmat[7]}; + mjtNum z[] = {cam_xmat[2], cam_xmat[5], cam_xmat[8]}; + + // vertices of the near plane + mju_addScl3(center, cam_xpos, z, -znear); + mju_addScl3(vnear[0], center, x, -zhor[0]); + mju_addScl3(vnear[1], center, x, zhor[1]); + mju_addScl3(vnear[2], center, x, zhor[1]); + mju_addScl3(vnear[3], center, x, -zhor[0]); + mju_addToScl3(vnear[0], y, -zver[0]); + mju_addToScl3(vnear[1], y, -zver[0]); + mju_addToScl3(vnear[2], y, zver[1]); + mju_addToScl3(vnear[3], y, zver[1]); + + // vertices of the far plane + zhor[0] *= zfar / znear; + zhor[1] *= zfar / znear; + zver[0] *= zfar / znear; + zver[1] *= zfar / znear; + mju_addScl3(center, cam_xpos, z, -zfar); + mju_addScl3(vfar[0], center, x, -zhor[0]); + mju_addScl3(vfar[1], center, x, zhor[1]); + mju_addScl3(vfar[2], center, x, zhor[1]); + mju_addScl3(vfar[3], center, x, -zhor[0]); + mju_addToScl3(vfar[0], y, -zver[0]); + mju_addToScl3(vfar[1], y, -zver[0]); + mju_addToScl3(vfar[2], y, zver[1]); + mju_addToScl3(vfar[3], y, zver[1]); + + // triangulation and wireframe of the frustum + for (int e=0; e<4; e++) { + START + mju_sub3(x, vfar[e], vnear[e]); + mju_sub3(y, vnear[(e+1)%4], vnear[e]); + mju_cross(z, x, y); + mjtNum tri1[3] = {mju_normalize3(x), mju_normalize3(y), mju_normalize3(z)}; + mjtNum xmat1[9] = {x[0], y[0], z[0], x[1], y[1], z[1], x[2], y[2], z[2]}; + mjv_initGeom(thisgeom, mjGEOM_TRIANGLE, tri1, vnear[e], xmat1, rgba); + FINISH + START + mju_sub3(y, vnear[(e+1)%4], vfar[e]); + mju_sub3(x, vfar[(e+1)%4], vfar[e]); + mju_cross(z, x, y); + mjtNum tri2[3] = {mju_normalize3(x), mju_normalize3(y), mju_normalize3(z)}; + mjtNum xmat2[9] = {x[0], y[0], z[0], x[1], y[1], z[1], x[2], y[2], z[2]}; + mjv_initGeom(thisgeom, mjGEOM_TRIANGLE, tri2, vfar[e], xmat2, rgba); + FINISH + START + mjv_connector(thisgeom, mjGEOM_LINE, 3, vnear[e], vnear[(e+1)%4]); + f2f(thisgeom->rgba, rgba, 4); + FINISH + START + mjv_connector(thisgeom, mjGEOM_LINE, 3, vfar[e], vfar[(e+1)%4]); + f2f(thisgeom->rgba, rgba, 4); + FINISH + START + mjv_connector(thisgeom, mjGEOM_LINE, 3, vnear[e], vfar[e]); + f2f(thisgeom->rgba, rgba, 4); + FINISH + } + } + } + // lights objtype = mjOBJ_LIGHT; category = mjCAT_DECOR; @@ -1907,24 +1999,27 @@ void mjv_makeLights(const mjModel* m, mjData* d, mjvScene* scn) { // update camera only void mjv_updateCamera(const mjModel* m, mjData* d, mjvCamera* cam, mjvScene* scn) { mjtNum ca, sa, ce, se, move[3], *mat; - mjtNum headpos[3], forward[3], up[3], right[3], ipd, fovy, znear, zfar; + mjtNum headpos[3], forward[3], up[3], right[3], ipd; // return if nothing to do if (!m || !cam || cam->type == mjCAMERA_USER) { return; } - // get znear, zfar - znear = m->vis.map.znear * m->stat.extent; - zfar = m->vis.map.zfar * m->stat.extent; + // initialize frustum + float zver[2], zhor[2] = {0, 0}; + float znear = m->vis.map.znear * m->stat.extent; + float zfar = m->vis.map.zfar * m->stat.extent; // get headpos, forward[3], up, right, ipd, fovy switch (cam->type) { case mjCAMERA_FREE: case mjCAMERA_TRACKING: - // get global ipd and fovy + // get global ipd ipd = m->vis.global.ipd; - fovy = m->vis.global.fovy; + + // compute image size from global fovy + zver[0] = zver[1] = (float)znear * mju_tan(m->vis.global.fovy * (float)(mjPI/360.0)); // move lookat for tracking if (cam->type == mjCAMERA_TRACKING) { @@ -1965,7 +2060,13 @@ void mjv_updateCamera(const mjModel* m, mjData* d, mjvCamera* cam, mjvScene* scn // get camera-specific ipd and fovy ipd = m->cam_ipd[cid]; - fovy = m->cam_fovy[cid]; + + // get frustum from intrinsics or from fovy + if (m->cam_sensorsize[2*cid+1]) { + getFrustum(zver, zhor, znear, m->cam_intrinsic + 4*cid, m->cam_sensorsize + 2*cid); + } else { + zver[0] = zver[1] = (float)znear * mju_tan(m->cam_fovy[cid] * (float)(mjPI/360.0)); + } // get pointer to camera orientation matrix mat = d->cam_xmat + 9*cid; @@ -1997,12 +2098,13 @@ void mjv_updateCamera(const mjModel* m, mjData* d, mjvCamera* cam, mjvScene* scn scn->camera[view].up[i] = (float)up[i]; } - // set symmetric frustum - scn->camera[view].frustum_center = 0; - scn->camera[view].frustum_top = (float)znear * tanf(fovy * (float)(mjPI/360.0)); - scn->camera[view].frustum_bottom = -scn->camera[view].frustum_top; - scn->camera[view].frustum_near = (float)znear; - scn->camera[view].frustum_far = (float)zfar; + // set symmetric frustum using intrinsic camera matrix + scn->camera[view].frustum_top = zver[1]; + scn->camera[view].frustum_bottom = -zver[0]; + scn->camera[view].frustum_center = (zhor[1] - zhor[0]) / 2; + scn->camera[view].frustum_width = (zhor[1] + zhor[0]) / 2; + scn->camera[view].frustum_near = znear; + scn->camera[view].frustum_far = zfar; } // disable model transformation (do not clear float data; user may need it later) diff --git a/src/render/render_gl3.c b/src/render/render_gl3.c index b2608c35..526e130c 100644 --- a/src/render/render_gl3.c +++ b/src/render/render_gl3.c @@ -404,6 +404,14 @@ static void renderGeom(const mjvGeom* geom, int mode, const float* headpos, } break; + case mjGEOM_TRIANGLE: // triangle + glBegin(GL_TRIANGLES); + glVertex3f(0, 0, 0); + glVertex3f(size[0], 0, 0); + glVertex3f(0, size[1], 0); + glEnd(); + break; + case mjGEOM_SKIN: // skin // vertex positions glEnableClientState(GL_VERTEX_ARRAY); @@ -585,8 +593,9 @@ static void setView(int view, mjrRect viewport, const mjvScene* scn, const mjrCo } // compute frustum halfwidth so as to match viewport aspect ratio - float halfwidth = 0.5f * (float)viewport.width/(float)viewport.height * - (cam.frustum_top - cam.frustum_bottom); + float halfwidth = cam.frustum_width ? cam.frustum_width + : 0.5f * (float)viewport.width / (float)viewport.height * + (cam.frustum_top - cam.frustum_bottom); // set projection glMatrixMode(GL_PROJECTION); diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 5312e9cb..8cecdde7 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -1623,6 +1623,8 @@ void mjCModel::CopyTree(mjModel* m) { m->cam_fovy[cid] = (mjtNum)pc->fovy; m->cam_ipd[cid] = (mjtNum)pc->ipd; copyvec(m->cam_resolution+2*cid, pc->resolution, 2); + copyvec(m->cam_sensorsize+2*cid, pc->sensor_size, 2); + copyvec(m->cam_intrinsic+4*cid, pc->intrinsic, 4); copyvec(m->cam_user+nuser_cam*cid, pc->userdata.data(), nuser_cam); } @@ -3084,6 +3086,7 @@ bool mjCModel::CopyBack(const mjModel* m) { cameras[i]->fovy = (double)m->cam_fovy[i]; cameras[i]->ipd = (double)m->cam_ipd[i]; copyvec(cameras[i]->resolution, m->cam_resolution+2*i, 2); + copyvec(cameras[i]->intrinsic, m->cam_intrinsic+4*i, 4); if (nuser_cam) { copyvec(cameras[i]->userdata.data(), m->cam_user + nuser_cam*i, nuser_cam); diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 44174121..e7de19bc 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -17,6 +17,7 @@ #include #include #include +#include #include #include #include @@ -1950,6 +1951,12 @@ mjCCamera::mjCCamera(mjCModel* _model, mjCDef* _def) { ipd = 0.068; userdata.clear(); resolution[0] = resolution[1] = 1; + principal_length[0] = principal_length[1] = 0; + principal_pixel[0] = principal_pixel[1] = 0; + focal_length[0] = focal_length[1] = 0; + focal_pixel[0] = focal_pixel[1] = 0; + sensor_size[0] = sensor_size[1] = 0; + mjuu_setvec(intrinsic, 0, 0, 0, 0); // clear private variables body = 0; @@ -2010,6 +2017,39 @@ void mjCCamera::Compile(void) { throw mjCError(this, "fovy too large in camera '%s' (id = %d, value = %d)", name.c_str(), id, fovy); } + + // check that specs are not duplicated + if ((principal_length[0] && principal_pixel[0]) || + (principal_length[1] && principal_pixel[1])) { + throw mjCError(this, "principal length duplicated in camera '%s' (id = %d)", + name.c_str(), id); + } + + if ((focal_length[0] && focal_pixel[0]) || + (focal_length[1] && focal_pixel[1])) { + throw mjCError(this, "focal length duplicated in camera '%s' (id = %d)", + name.c_str(), id); + } + + // compute number of pixels per unit length + if (sensor_size[0]>0 && sensor_size[1]>0) { + float pixel_density[2] = { + (float)resolution[0] / sensor_size[0], + (float)resolution[1] / sensor_size[1], + }; + + // defaults are zero, so only one term in each sum is nonzero + intrinsic[0] = focal_pixel[0] / pixel_density[0] + focal_length[0]; + intrinsic[1] = focal_pixel[1] / pixel_density[1] + focal_length[1]; + intrinsic[2] = principal_pixel[0] / pixel_density[0] + principal_length[0]; + intrinsic[3] = principal_pixel[1] / pixel_density[1] + principal_length[1]; + + // fovy with principal point at (0, 0) + fovy = mju_atan2((float)sensor_size[1]/2, intrinsic[1]) * 360.0 / mjPI; + } else { + intrinsic[0] = model->visual.map.znear; + intrinsic[1] = model->visual.map.znear; + } } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 21dc7cbb..efd07bb3 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -465,7 +465,13 @@ class mjCCamera : public mjCBase { double ipd; // inter-pupilary distance double pos[3]; // position double quat[4]; // orientation + float intrinsic[4]; // camera intrinsics [length] + float sensor_size[2]; // sensor size [length] float resolution[2]; // resolution [pixel] + float focal_length[2]; // focal length [length] + float focal_pixel[2]; // focal length [pixel] + float principal_length[2]; // principal point [length] + float principal_pixel[2]; // principal point [pixel] std::vector userdata; // user data mjCAlternative alt; // alternative orientation specification diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 8c843e1c..52c511d2 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -148,8 +148,9 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { "hfield", "mesh", "fitscale", "rgba", "fluidshape", "fluidcoef", "user"}, {"site", "?", "13", "type", "group", "pos", "quat", "material", "size", "fromto", "axisangle", "xyaxes", "zaxis", "euler", "rgba", "user"}, - {"camera", "?", "11", "fovy", "ipd", "pos", "quat", "resolution", - "axisangle", "xyaxes", "zaxis", "euler", "mode", "user"}, + {"camera", "?", "16", "fovy", "ipd", "resolution", "pos", "quat", "axisangle", "xyaxes", + "zaxis", "euler", "mode", "focal", "focalpixel", "principal", "principalpixel", + "sensorsize", "user"}, {"light", "?", "12", "pos", "dir", "directional", "castshadow", "active", "attenuation", "cutoff", "exponent", "ambient", "diffuse", "specular", "mode"}, {"pair", "?", "7", "condim", "friction", "solref", "solreffriction", "solimp", @@ -257,9 +258,9 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { {">"}, {"site", "*", "15", "name", "class", "type", "group", "pos", "quat", "material", "size", "fromto", "axisangle", "xyaxes", "zaxis", "euler", "rgba", "user"}, - {"camera", "*", "14", "name", "class", "fovy", "ipd", "resolution", - "pos", "quat", "axisangle", "xyaxes", "zaxis", "euler", - "mode", "target", "user"}, + {"camera", "*", "19", "name", "class", "fovy", "ipd", "resolution", "pos", "quat", + "axisangle", "xyaxes", "zaxis", "euler", "mode", "target", "focal", "focalpixel", + "principal", "principalpixel", "sensorsize", "user"}, {"light", "*", "15", "name", "class", "directional", "castshadow", "active", "pos", "dir", "attenuation", "cutoff", "exponent", "ambient", "diffuse", "specular", "mode", "target"}, @@ -1461,13 +1462,28 @@ void mjXReader::OneCamera(XMLElement* elem, mjCCamera* pcam) { ReadAttr(elem, "pos", 3, pcam->pos, text); ReadQuat(elem, "quat", pcam->quat, text); ReadAlternative(elem, pcam->alt); - ReadAttr(elem, "fovy", 1, &pcam->fovy, text); ReadAttr(elem, "ipd", 1, &pcam->ipd, text); - ReadAttr(elem, "resolution", 2, pcam->resolution, text); + + bool has_principal = ReadAttr(elem, "principalpixel", 2, pcam->principal_pixel, text) || + ReadAttr(elem, "principal", 2, pcam->principal_length, text); + bool has_focal = ReadAttr(elem, "focalpixel", 2, pcam->focal_pixel, text) || + ReadAttr(elem, "focal", 2, pcam->focal_length, text); + bool needs_sensorsize = has_principal || has_focal; + bool has_sensorsize = ReadAttr(elem, "sensorsize", 2, pcam->sensor_size, text, needs_sensorsize); + bool has_fovy = ReadAttr(elem, "fovy", 1, &pcam->fovy, text); + bool needs_resolution = has_focal || has_sensorsize; + ReadAttr(elem, "resolution", 2, pcam->resolution, text, needs_resolution); + if (pcam->resolution[0] < 0 || pcam->resolution[1] < 0) { throw mjXError(elem, "camera resolution cannot be negative"); } + if (has_fovy && has_sensorsize) { + throw mjXError( + elem, + "either 'fovy' or 'sensorsize' attribute can be specified, not both"); + } + // read userdata ReadVector(elem, "user", pcam->userdata, text); diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index c9306dd4..93e35a2a 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -406,10 +406,23 @@ void mjXWriter::OneCamera(XMLElement* elem, mjCCamera* pcam, mjCDef* def) { // defaults and regular WriteAttr(elem, "ipd", 1, &pcam->ipd, &def->camera.ipd); - WriteAttr(elem, "fovy", 1, &pcam->fovy, &def->camera.fovy); WriteAttrKey(elem, "mode", camlight_map, camlight_sz, pcam->mode, def->camera.mode); WriteAttr(elem, "resolution", 2, pcam->resolution, def->camera.resolution); + // resolution if positive + WriteAttr(elem, "resolution", 2, pcam->resolution, def->camera.resolution); + + // camera intrinsics if specified + if (pcam->sensor_size[0]>0 && pcam->sensor_size[1]>0) { + WriteAttr(elem, "sensorsize", 2, pcam->sensor_size); + WriteAttr(elem, "focal", 2, pcam->focal_length, def->camera.focal_length); + WriteAttr(elem, "focalpixel", 2, pcam->focal_pixel, def->camera.focal_pixel); + WriteAttr(elem, "principal", 2, pcam->principal_length, def->camera.principal_length); + WriteAttr(elem, "principalpixel", 2, pcam->principal_pixel, def->camera.principal_pixel); + } else { + WriteAttr(elem, "fovy", 1, &pcam->fovy, &def->camera.fovy); + } + // userdata if (writingdefaults) { WriteVector(elem, "user", pcam->userdata); diff --git a/test/engine/testdata/vis_visualize/frustum.xml b/test/engine/testdata/vis_visualize/frustum.xml new file mode 100644 index 00000000..17e50555 --- /dev/null +++ b/test/engine/testdata/vis_visualize/frustum.xml @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index e7dd3b42..d11e4f4b 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -754,6 +754,81 @@ TEST_F(CameraSpecTest, FovyLimits) { mj_deleteModel(m); } +TEST_F(CameraSpecTest, DuplicatedFocalIgnorePixel) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m, NotNull()) << error.data(); + EXPECT_NEAR(m->cam_intrinsic[0], 5e-4, 1e-6); // focal length in meters (x) + EXPECT_NEAR(m->cam_intrinsic[1], 5e-4, 1e-6); // focal length in meters (y) + mj_deleteModel(m); +} + +TEST_F(CameraSpecTest, FovyFromResolution) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m, NotNull()) << error.data(); + EXPECT_NEAR(m->cam_fovy[0], 41.112, 1e-3); + EXPECT_NEAR(m->cam_intrinsic[0], 8e-3, 1e-6); // focal length in meters (x) + EXPECT_NEAR(m->cam_intrinsic[1], 8e-3, 1e-6); // focal length in meters (y) + EXPECT_EQ(m->cam_intrinsic[2], 0); // principal point in meters (x) + EXPECT_EQ(m->cam_intrinsic[3], 0); // principal point in meters (y) + mj_deleteModel(m); +} + +TEST_F(CameraSpecTest, FovyFromResolutionPixel) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m, NotNull()) << error.data(); + EXPECT_NEAR(m->cam_fovy[0], 41.112, 1e-3); + EXPECT_NEAR(m->cam_intrinsic[0], 8e-3, 1e-6); // focal length in meters (x) + EXPECT_NEAR(m->cam_intrinsic[1], 8e-3, 1e-6); // focal length in meters (y) + EXPECT_EQ(m->cam_intrinsic[2], 0); // principal point in meters (x) + EXPECT_EQ(m->cam_intrinsic[3], 0); // principal point in meters (y) + mj_deleteModel(m); +} + // ------------- test actuator order ------------------------------------------- using ActuatorTest = MujocoTest; diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index dad4144e..ca485d8e 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -407,6 +407,61 @@ TEST_F(XMLReaderTest, InvalidDoubleOrientation) { } } +// ---------------------- test camera parsing --------------------------------- + +TEST_F(XMLReaderTest, CameraInvalidFovyAndSensorsize) { + static constexpr char xml[] = R"( + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m, testing::IsNull()); + EXPECT_THAT(error.data(), HasSubstr("either 'fovy' or 'sensorsize'")); +} + +TEST_F(XMLReaderTest, CameraPricipalRequiresSensorsize) { + static constexpr char xml[] = R"( + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m, testing::IsNull()); + EXPECT_THAT(error.data(), HasSubstr("attribute missing: 'sensorsize'")); +} + +TEST_F(XMLReaderTest, CameraSensorsizeRequiresResolution) { + static constexpr char xml[] = R"( + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m, testing::IsNull()); + EXPECT_THAT(error.data(), HasSubstr("attribute missing: 'resolution'")); +} + +// ---------------------- test inertia parsing -------------------------------- + TEST_F(XMLReaderTest, InvalidInertialOrientation) { static constexpr char xml[] = R"( diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 0266a05f..ac5c8161 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -190,6 +190,7 @@ public enum mjtGeom : int{ mjGEOM_LINE = 103, mjGEOM_SKIN = 104, mjGEOM_LABEL = 105, + mjGEOM_TRIANGLE = 106, mjGEOM_NONE = 1001, } public enum mjtCamLight : int{ @@ -5218,6 +5219,8 @@ public unsafe struct mjModel_ { public double* cam_mat0; public int* cam_resolution; public double* cam_fovy; + public float* cam_intrinsic; + public float* cam_sensorsize; public double* cam_ipd; public double* cam_user; public int* light_mode; @@ -5699,6 +5702,7 @@ public unsafe struct mjvGLCamera_ { public fixed float forward[3]; public fixed float up[3]; public float frustum_center; + public float frustum_width; public float frustum_bottom; public float frustum_top; public float frustum_near; @@ -5993,6 +5997,7 @@ public unsafe struct model { public float* site_rgba; public double* cam_fovy; public double* cam_ipd; + public float* cam_sensorsize; public byte* light_directional; public byte* light_castshadow; public byte* light_active;