chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

Specify camera parameters using standard robotics conventions.

Browse Source 
PiperOrigin-RevId: 569147555
Change-Id: I3443f95e5de56a17024f5e5c03f33888dd583edb
This commit is contained in:
Alessio Quaglino
2023-09-28 05:18:36 -07:00
committed by Copybara-Service
parent ca60de79c8
commit 36d2ffe4f2
28 changed files with 528 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+45 -3
View File
@@ -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 <body-camera-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-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 <body-camera-fovy>`. If
specified, :ref:`resolution <body-camera-resolution>` and :ref:`focal <body-camera-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 <body-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:
doc/XMLschema.rst
+8 -4
View File
@@ -354,7 +354,9 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`xyaxes<body-camera-xyaxes>` | :ref:`zaxis<body-camera-zaxis>` | :ref:`euler<body-camera-euler>` | :ref:`mode<body-camera-mode>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`target<body-camera-target>` | :ref:`user<body-camera-user>` | | | |
| | | | :ref:`target<body-camera-target>` | :ref:`focal<body-camera-focal>` | :ref:`focalpixel<body-camera-focalpixel>` | :ref:`principal<body-camera-principal>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`principalpixel<body-camera-principalpixel>` | :ref:`sensorsize<body-camera-sensorsize>` | :ref:`user<body-camera-user>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| body |br| |_| |L| | | .. table:: |
@@ -1230,11 +1232,13 @@
| :ref:`camera | ? | :class: mjcf-attributes |
| <default-camera>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`fovy<default-camera-fovy>` | :ref:`ipd<default-camera-ipd>` | :ref:`pos<default-camera-pos>` | :ref:`quat<default-camera-quat>` | |
| | | | :ref:`fovy<default-camera-fovy>` | :ref:`ipd<default-camera-ipd>` | :ref:`resolution<default-camera-resolution>` | :ref:`pos<default-camera-pos>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`resolution<default-camera-resolution>` | :ref:`axisangle<default-camera-axisangle>` | :ref:`xyaxes<default-camera-xyaxes>` | :ref:`zaxis<default-camera-zaxis>` | |
| | | | :ref:`quat<default-camera-quat>` | :ref:`axisangle<default-camera-axisangle>` | :ref:`xyaxes<default-camera-xyaxes>` | :ref:`zaxis<default-camera-zaxis>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`euler<default-camera-euler>` | :ref:`mode<default-camera-mode>` | :ref:`user<default-camera-user>` | | |
| | | | :ref:`euler<default-camera-euler>` | :ref:`mode<default-camera-mode>` | :ref:`focal<default-camera-focal>` | :ref:`focalpixel<default-camera-focalpixel>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`principal<default-camera-principal>` | :ref:`principalpixel<default-camera-principalpixel>` | :ref:`sensorsize<default-camera-sensorsize>` | :ref:`user<default-camera-user>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| default |br| |_| |L| | | .. table:: |
doc/changelog.rst
+11 -2
View File
@@ -106,17 +106,26 @@ General
19. Added camera :ref:`resolution<body-camera-resolution>` attribute and :ref:`camprojection<sensor-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<body-camera>` calibration attributes:
- The new attributes are :ref:`resolution<body-camera-resolution>`, :ref:`focal<body-camera-focal>`,
:ref:`focalpixel<body-camera-focalpixel>`, :ref:`principal<body-camera-principal>`,
:ref:`principalpixel<body-camera-principalpixel>` and :ref:`sensorsize<body-camera-sensorsize>`.
- Visualize the calibrated frustum using the :ref:`mjVIS_CAMERA<mjtVisFlag>` visualization flag when these
attributes are specified. See the following
`example model <https://github.com/deepmind/mujoco/blob/main/test/engine/testdata/vis_visualize/frustum.xml>`__.
- Note that these attributes only take effect for offline rendering and do not affect interactive visualisation.
Python bindings
^^^^^^^^^^^^^^^
20. Fixed `#870 <https://github.com/google-deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
21. Fixed `#870 <https://github.com/google-deepmind/mujoco/issues/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<body-geom-margin>` to be ignored during the construction of
22. Fixed a bug that was causing :ref:`geom margin<body-geom-margin>` to be ignored during the construction of
midphase collision trees.
doc/includes/references.h
+5
View File
@@ -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;
doc/modeling.rst
+25
View File
@@ -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 <CUser>`.
.. _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<body-camera-mode>` and
:ref:`target<body-camera-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<body-camera-ipd>` (inter-pupilary distance, required for
stereoscopic rendering and VR) and :ref:`fovy<body-camera-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<body-camera-focal>` and :ref:`principal<body-camera-principal>`
attributes. When these calibration-related attributes are used, the physical
:ref:`sensor size<body-camera-sensorsize>` and camera :ref:`resolution<body-camera-resolution>` must also be
specified. In this case, the rendering frustum can be visualized.
.. _CComposite:
Composite objects
doc/overview.rst
+4 -2
View File
@@ -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
^^^^^
include/mujoco/mjmodel.h
+3
View File
@@ -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)
include/mujoco/mjvisualize.h
+2
View File
@@ -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;
include/mujoco/mjxmacro.h
+2
View File
@@ -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 ) \
introspect/enums.py
+1
View File
@@ -92,6 +92,7 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjGEOM_LINE', 103),
('mjGEOM_SKIN', 104),
('mjGEOM_LABEL', 105),
('mjGEOM_TRIANGLE', 106),
('mjGEOM_NONE', 1001),
]),
)),
introspect/enums_test.py
+1
View File
@@ -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)
introspect/structs.py
+26
View File
@@ -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(
python/mujoco/bindings_test.py
+1
View File
@@ -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):
python/mujoco/structs.cc
+1
View File
@@ -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);
src/engine/engine_io.c
+1 -1
View File
@@ -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.);
src/engine/engine_sensor.c
+14 -5
View File
@@ -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
src/engine/engine_vis_interact.c
+1
View File
@@ -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);
src/engine/engine_vis_visualize.c
+115 -13
View File
@@ -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)
src/render/render_gl3.c
+11 -2
View File
@@ -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);
src/user/user_model.cc
+3
View File
@@ -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);
src/user/user_objects.cc
+40
View File
@@ -17,6 +17,7 @@
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <sstream>
@@ -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;
}
}
src/user/user_objects.h
+6
View File
@@ -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<double> userdata; // user data
mjCAlternative alt; // alternative orientation specification
src/xml/xml_native_reader.cc
+23 -7
View File
@@ -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);
src/xml/xml_native_writer.cc
+14 -1
View File
@@ -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);
test/engine/testdata/vis_visualize/frustum.xml
Vendored
+30
View File
@@ -0,0 +1,30 @@
<!-- Please activate Camera rendering when loading this model.
The frustum should match exactly the fron face of the box. -->
<mujoco>
<visual>
<map znear="0.01"/>
</visual>
<asset>
<texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".8 .6 .4" rgb2=".2 .3 .4"/>
<material name="grid" texture="grid" texrepeat="9 16" texuniform="false" reflectance=".2"/>
</asset>
<worldbody>
<light pos="0 0 3"/>
<geom type="plane" size="10 10 .01"/>
<geom type="box" size=".375 .6 .1" pos="1.1 0 1" material="grid" euler="0 90 0"/>
<!-- 8mm focal length lenses -->
<camera pos="0 0 1" xyaxes="0 -1 0 0 0 1" focalpixel="1600 1600" resolution="1920 1200"
sensorsize="9.6e-3 6e-3"/>
<body euler="0 0 90">
<geom type="box" size=".375 .6 .1" pos="1.1 2 1" material="grid" euler="0 90 0"/>
<!-- 8mm focal length lenses -->
<camera pos="0 2 1" xyaxes="0 -1 0 0 0 1" focal="8e-3 8e-3" resolution="1920 1200"
sensorsize="9.6e-3 6e-3" principalpixel="200 0"/>
</body>
</worldbody>
</mujoco>
test/user/user_objects_test.cc
+75
View File
@@ -754,6 +754,81 @@ TEST_F(CameraSpecTest, FovyLimits) {
mj_deleteModel(m);
}
TEST_F(CameraSpecTest, DuplicatedFocalIgnorePixel) {
static constexpr char xml[] = R"(
<mujoco>
<visual>
<map znear="0.01"/>
</visual>
<worldbody>
<body>
<geom size="1"/>
<camera focal="8e-3 8e-3" focalpixel="100 100"
resolution="1920 1200" sensorsize="9.6e-3 6e-3"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>
<visual>
<map znear="0.01"/>
</visual>
<worldbody>
<body>
<geom size="1"/>
<!-- 8mm focal length lenses -->
<camera focal="8e-3 8e-3" resolution="1920 1200" sensorsize="9.6e-3 6e-3"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>
<visual>
<map znear="0.01"/>
</visual>
<worldbody>
<body>
<geom size="1"/>
<!-- 8mm focal length lenses -->
<camera focalpixel="1600 1600" resolution="1920 1200" sensorsize="9.6e-3 6e-3"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> 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;
test/xml/xml_native_reader_test.cc
+55
View File
@@ -407,6 +407,61 @@ TEST_F(XMLReaderTest, InvalidDoubleOrientation) {
}
}
// ---------------------- test camera parsing ---------------------------------
TEST_F(XMLReaderTest, CameraInvalidFovyAndSensorsize) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<camera fovy="1" sensorsize="1 1" resolution="100 100"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<camera principal="1 1"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<camera sensorsize="1 1"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>
unity/Runtime/Bindings/MjBindings.cs
+5
View File
@@ -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;