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

Add camera projection sensor.

Browse Source 
BEGIN_PUBLIC
Add camera projection sensor.
END_PUBLIC

PiperOrigin-RevId: 564726114
Change-Id: I33b8e5562eff29c21538bf8f38ce3116c7dfc5a9
This commit is contained in:
Alessio Quaglino
2023-09-12 08:15:14 -07:00
committed by Copybara-Service
parent ccda87aafa
commit 8064ad59c8
19 changed files with 369 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_io.c
+3
View File
@@ -1612,6 +1612,9 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) {
case mjSENS_CLOCK:
return 1;
case mjSENS_CAMPROJECTION:
return 2;
case mjSENS_ACCELEROMETER:
case mjSENS_VELOCIMETER:
case mjSENS_GYRO:
src/engine/engine_sensor.c
+90
View File
@@ -187,6 +187,91 @@ 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) {
// translation matrix (4x4)
mjtNum translation[4][4] = {0};
translation[0][0] = 1;
translation[1][1] = 1;
translation[2][2] = 1;
translation[3][3] = 1;
translation[0][3] = -cam_xpos[0];
translation[1][3] = -cam_xpos[1];
translation[2][3] = -cam_xpos[2];
// rotation matrix (4x4)
mjtNum rotation[4][4] = {0};
rotation[0][0] = 1;
rotation[1][1] = 1;
rotation[2][2] = 1;
rotation[3][3] = 1;
for (int i=0; i<3; i++) {
for (int j=0; j<3; j++) {
rotation[i][j] = cam_xmat[j*3+i];
}
}
// focal transformation matrix (3x4)
mjtNum height = (mjtNum) cam_res[1];
mjtNum fy = .5 / mju_tan(cam_fovy * mjPI / 360.) * height;
mjtNum focal[3][4] = {0};
focal[0][0] = -fy;
focal[1][1] = fy;
focal[2][2] = 1.0;
// image matrix (3x3)
mjtNum image[3][3] = {0};
image[0][0] = 1;
image[1][1] = 1;
image[2][2] = 1;
image[0][2] = ((mjtNum)cam_res[0] - 1) / 2.0;
image[1][2] = ((mjtNum)cam_res[1] - 1) / 2.0;
// projection matrix (3x4): product of all 4 matrices
mjtNum proj[3][4] = {0};
for (int i=0; i<3; i++) {
for (int j=0; j<3; j++) {
for (int k=0; k<4; k++) {
for (int l=0; l<4; l++) {
for (int n=0; n<4; n++) {
proj[i][n] += image[i][j] * focal[j][k] * rotation[k][l] * translation[l][n];
}
}
}
}
}
// projection matrix multiplies homogenous [x, y, z, 1] vectors
mjtNum pos_hom[4] = {0, 0, 0, 1};
mju_copy3(pos_hom, target_xpos);
// project world coordinates into pixel space, see:
// https://en.wikipedia.org/wiki/3D_projection#Mathematical_formula
mjtNum pixel_coord_hom[3] = {0};
for (int i=0; i<3; i++) {
for (int j=0; j<4; j++) {
pixel_coord_hom[i] += proj[i][j] * pos_hom[j];
}
}
// avoid dividing by tiny numbers
mjtNum denom = pixel_coord_hom[2];
if (mju_abs(denom) < mjMINVAL) {
if (denom < 0) {
denom = mju_min(denom, -mjMINVAL);
} else {
denom = mju_max(denom, mjMINVAL);
}
}
// compute projection
sensordata[0] = pixel_coord_hom[0] / denom;
sensordata[1] = pixel_coord_hom[1] / denom;
}
//-------------------------------- sensor ----------------------------------------------------------
// position-dependent sensors
@@ -221,6 +306,11 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
mju_mulMatTVec(d->sensordata+adr, d->site_xmat+9*objid, m->opt.magnetic, 3, 3);
break;
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]);
break;
case mjSENS_RANGEFINDER: // rangefinder
rvec[0] = d->site_xmat[9*objid+2];
rvec[1] = d->site_xmat[9*objid+5];
src/user/user_model.cc
+2
View File
@@ -1586,6 +1586,7 @@ void mjCModel::CopyTree(mjModel* m) {
copyvec(m->cam_quat+4*cid, pc->locquat, 4);
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_user+nuser_cam*cid, pc->userdata.data(), nuser_cam);
}
@@ -3044,6 +3045,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
copyvec(cameras[i]->quat, m->cam_quat+4*i, 4);
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);
if (nuser_cam) {
copyvec(cameras[i]->userdata.data(), m->cam_user + nuser_cam*i, nuser_cam);
src/user/user_objects.cc
+22 -1
View File
@@ -1949,6 +1949,7 @@ mjCCamera::mjCCamera(mjCModel* _model, mjCDef* _def) {
fovy = 45;
ipd = 0.068;
userdata.clear();
resolution[0] = resolution[1] = 1;
// clear private variables
body = 0;
@@ -2003,6 +2004,12 @@ void mjCCamera::Compile(void) {
if (targetbodyid==body->id) {
throw mjCError(this, "parent-targeting in camera '%s' (id = %d)", name.c_str(), id);
}
// make sure the image size is finite
if (fovy >= 180) {
throw mjCError(this, "fovy too large in camera '%s' (id = %d, value = %d)",
name.c_str(), id, fovy);
}
}
@@ -4044,6 +4051,7 @@ void mjCSensor::Compile(void) {
case mjSENS_TORQUE:
case mjSENS_MAGNETOMETER:
case mjSENS_RANGEFINDER:
case mjSENS_CAMPROJECTION:
// must be attached to site
if (objtype!=mjOBJ_SITE) {
throw mjCError(this,
@@ -4054,19 +4062,32 @@ void mjCSensor::Compile(void) {
if (type==mjSENS_TOUCH || type==mjSENS_RANGEFINDER) {
dim = 1;
datatype = mjDATATYPE_POSITIVE;
} else if (type==mjSENS_CAMPROJECTION) {
dim = 2;
datatype = mjDATATYPE_REAL;
} else {
dim = 3;
datatype = mjDATATYPE_REAL;
}
// set stage
if (type==mjSENS_MAGNETOMETER || type==mjSENS_RANGEFINDER) {
if (type==mjSENS_MAGNETOMETER || type==mjSENS_RANGEFINDER || type==mjSENS_CAMPROJECTION) {
needstage = mjSTAGE_POS;
} else if (type==mjSENS_GYRO || type==mjSENS_VELOCIMETER) {
needstage = mjSTAGE_VEL;
} else {
needstage = mjSTAGE_ACC;
}
// check for camera resolution for camera projection sensor
if (type==mjSENS_CAMPROJECTION) {
mjCCamera* camref = (mjCCamera*) model->FindObject(mjOBJ_CAMERA, refname);
if (!camref->resolution[0] || !camref->resolution[1]) {
throw mjCError(this,
"camera projection sensor requires camera resolution '%s' (id = %d)",
name.c_str(), id);
}
}
break;
case mjSENS_JOINTPOS:
src/user/user_objects.h
+1
View File
@@ -465,6 +465,7 @@ class mjCCamera : public mjCBase {
double ipd; // inter-pupilary distance
double pos[3]; // position
double quat[4]; // orientation
float resolution[2]; // resolution [pixel]
std::vector<double> userdata; // user data
mjCAlternative alt; // alternative orientation specification
src/xml/xml_native_reader.cc
+14 -3
View File
@@ -80,7 +80,7 @@ void ReadPluginConfigs(tinyxml2::XMLElement* elem, mjCPlugin* pp) {
//---------------------------------- MJCF schema ---------------------------------------------------
static const int nMJCF = 203;
static const int nMJCF = 204;
static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"mujoco", "!", "1", "model"},
{"<"},
@@ -151,7 +151,7 @@ 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", "?", "10", "fovy", "ipd", "pos", "quat",
{"camera", "?", "11", "fovy", "ipd", "pos", "quat", "resolution",
"axisangle", "xyaxes", "zaxis", "euler", "mode", "user"},
{"light", "?", "12", "pos", "dir", "directional", "castshadow", "active",
"attenuation", "cutoff", "exponent", "ambient", "diffuse", "specular", "mode"},
@@ -264,7 +264,7 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{">"},
{"site", "*", "15", "name", "class", "type", "group", "pos", "quat",
"material", "size", "fromto", "axisangle", "xyaxes", "zaxis", "euler", "rgba", "user"},
{"camera", "*", "13", "name", "class", "fovy", "ipd",
{"camera", "*", "14", "name", "class", "fovy", "ipd", "resolution",
"pos", "quat", "axisangle", "xyaxes", "zaxis", "euler",
"mode", "target", "user"},
{"light", "*", "15", "name", "class", "directional", "castshadow", "active",
@@ -398,6 +398,7 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"force", "*", "5", "name", "site", "cutoff", "noise", "user"},
{"torque", "*", "5", "name", "site", "cutoff", "noise", "user"},
{"magnetometer", "*", "5", "name", "site", "cutoff", "noise", "user"},
{"camprojection", "*", "6", "name", "site", "camera", "cutoff", "noise", "user"},
{"rangefinder", "*", "5", "name", "site", "cutoff", "noise", "user"},
{"jointpos", "*", "5", "name", "joint", "cutoff", "noise", "user"},
{"jointvel", "*", "5", "name", "joint", "cutoff", "noise", "user"},
@@ -1476,6 +1477,10 @@ void mjXReader::OneCamera(XMLElement* elem, mjCCamera* pcam) {
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);
if (pcam->resolution[0] < 0 || pcam->resolution[1] < 0) {
throw mjXError(elem, "camera resolution cannot be negative");
}
// read userdata
ReadVector(elem, "user", pcam->userdata, text);
@@ -3013,6 +3018,12 @@ void mjXReader::Sensor(XMLElement* section) {
psen->type = mjSENS_MAGNETOMETER;
psen->objtype = mjOBJ_SITE;
ReadAttrTxt(elem, "site", psen->objname, true);
} else if (type=="camprojection") {
psen->type = mjSENS_CAMPROJECTION;
psen->objtype = mjOBJ_SITE;
ReadAttrTxt(elem, "site", psen->objname, true);
ReadAttrTxt(elem, "camera", psen->refname, true);
psen->reftype = mjOBJ_CAMERA;
} else if (type=="rangefinder") {
psen->type = mjSENS_RANGEFINDER;
psen->objtype = mjOBJ_SITE;
src/xml/xml_native_writer.cc
+7 -1
View File
@@ -408,6 +408,7 @@ void mjXWriter::OneCamera(XMLElement* elem, mjCCamera* pcam, mjCDef* def) {
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);
// userdata
if (writingdefaults) {
@@ -1648,6 +1649,11 @@ void mjXWriter::Sensor(XMLElement* root) {
elem = InsertEnd(section, "rangefinder");
WriteAttrTxt(elem, "site", psen->objname);
break;
case mjSENS_CAMPROJECTION:
elem = InsertEnd(section, "camprojection");
WriteAttrTxt(elem, "site", psen->objname);
WriteAttrTxt(elem, "camera", psen->refname);
break;
// sensors related to scalar joints, tendons, actuators
case mjSENS_JOINTPOS:
@@ -1836,7 +1842,7 @@ void mjXWriter::Sensor(XMLElement* root) {
WriteVector(elem, "user", psen->userdata);
// add reference if present
if (psen->reftype != mjOBJ_UNKNOWN) {
if (psen->reftype != mjOBJ_UNKNOWN && psen->type != mjSENS_CAMPROJECTION) {
WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
WriteAttrTxt(elem, "refname", psen->refname);
}