chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 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
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);