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