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

Extend rangefinder sensor to support cameras.

Browse Source 
PiperOrigin-RevId: 848255889
Change-Id: I6e1a9ed13d29d2242558625a47e24e47ad6e87db
This commit is contained in:
Yuval Tassa
2025-12-23 12:03:37 -08:00
committed by Copybara-Service
parent 4a871990e9
commit 9d646e6548
17 changed files with 383 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+20 -7
View File
@@ -6373,12 +6373,20 @@ site frame. The output is a 3D vector.
:el-prefix:`sensor/` |-| **rangefinder** (*)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
This element creates a rangefinder. It measures the distance to the nearest geom surface, along the ray defined by the
positive Z-axis of the sensor site. If the ray does not intersect any geom surface, the sensor output is -1. If the
origin of the ray is inside a geom, the surface is still sensed (but not the inner volume). Geoms attached to the same
body as the sensor site are excluded. Invisible geoms, defined as geoms whose rgba (or whose material rgba) has alpha=0,
are also excluded. Note however that geoms made invisible in the visualizer by disabling their geom group are not
excluded; this is because sensor calculations are independent of the visualizer.
This element creates a rangefinder.
- If associated with a :ref:`site<sensor-rangefinder-site>`, it measures the distance to the nearest geom surface, along
the ray defined by the positive Z-axis of the site.
- If associated with a :ref:`camera<sensor-rangefinder-camera>`, it outputs one distance measurement for each pixel in
the camera image. Note that cameras face the :ref:`negative Z-axis<body-camera>` of their frame. The number of
measurements in this case is equal to product of the camera's width and height
:ref:`resolutions<body-camera-resolution>`.
If a ray does not intersect any geom surface, the sensor output is -1. If the origin of the ray is inside a geom, the
surface is still detected. Geoms attached to the same body as the sensor site/camera are excluded. Invisible geoms,
defined as geoms whose rgba (or whose material rgba) has alpha=0, are also excluded. Note however that geoms made
invisible in the visualizer by disabling their geom group are not excluded; this is because sensor calculations are
independent of the visualizer.
.. _sensor-rangefinder-name:
@@ -6393,9 +6401,14 @@ excluded; this is because sensor calculations are independent of the visualizer.
.. _sensor-rangefinder-site:
:at:`site`: :at-val:`string, required`
:at:`site`: :at-val:`string, optional`
The site where the sensor is attached.
.. _sensor-rangefinder-camera:
:at:`camera`: :at-val:`string, optional`
The camera where the sensor is attached.
.. _sensor-camprojection:
:el-prefix:`sensor/` |-| **camprojection** (*)
doc/XMLschema.rst
+2 -2
View File
@@ -940,9 +940,9 @@
| :ref:`rangefinder | \* | :class: mjcf-attributes |
| <sensor-rangefinder>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<sensor-rangefinder-name>` | :ref:`site<sensor-rangefinder-site>` | :ref:`cutoff<sensor-rangefinder-cutoff>` | :ref:`noise<sensor-rangefinder-noise>` | |
| | | | :ref:`name<sensor-rangefinder-name>` | :ref:`site<sensor-rangefinder-site>` | :ref:`camera<sensor-rangefinder-camera>` | :ref:`cutoff<sensor-rangefinder-cutoff>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`user<sensor-rangefinder-user>` | | | | |
| | | | :ref:`noise<sensor-rangefinder-noise>` | :ref:`user<sensor-rangefinder-user>` | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| sensor |br| |_| |L| | | .. table:: |
doc/changelog.rst
+2
View File
@@ -20,6 +20,8 @@ General
^^^^^^^
- Camera frustum visualization is now triggered by setting :ref:`resolution<body-camera-resolution>` to values larger
than 1. Relatedly, frustum visualization also works for :ref:`orthographic<body-camera-projection>` cameras.
- Rangefinder sensors can now be attached to a camera using the :ref:`ragefinder/camera<sensor-rangefinder-camera>`
attribute. In this case, the sensor will cast multiple ray, one for each camera pixel.
- Non-breaking ABI changes:
- The type of the ``sig`` (signature) argument of :ref:`mj_stateSize` and related functions has been changed from
doc/includes/references.h
+1 -2
View File
@@ -678,7 +678,6 @@ typedef enum mjtObj_ { // type of MujoCo object
mjOBJ_FRAME = 100, // frame
mjOBJ_DEFAULT, // default
mjOBJ_MODEL // entire model
} mjtObj;
typedef enum mjtSensor_ { // type of sensor
// common robotic sensors, attached to a site
@@ -689,7 +688,7 @@ typedef enum mjtSensor_ { // type of sensor
mjSENS_FORCE, // 3D force between site's body and its parent body
mjSENS_TORQUE, // 3D torque between site's body and its parent body
mjSENS_MAGNETOMETER, // 3D magnetometer
mjSENS_RANGEFINDER, // scalar distance to nearest geom or site along z-axis
mjSENS_RANGEFINDER, // scalar distance to nearest geom along z-axis
mjSENS_CAMPROJECTION, // pixel coordinates of a site in the camera image
// sensors related to scalar joints, tendons, actuators
include/mujoco/mjmodel.h
+1 -2
View File
@@ -296,7 +296,6 @@ typedef enum mjtObj_ { // type of MujoCo object
mjOBJ_FRAME = 100, // frame
mjOBJ_DEFAULT, // default
mjOBJ_MODEL // entire model
} mjtObj;
@@ -309,7 +308,7 @@ typedef enum mjtSensor_ { // type of sensor
mjSENS_FORCE, // 3D force between site's body and its parent body
mjSENS_TORQUE, // 3D torque between site's body and its parent body
mjSENS_MAGNETOMETER, // 3D magnetometer
mjSENS_RANGEFINDER, // scalar distance to nearest geom or site along z-axis
mjSENS_RANGEFINDER, // scalar distance to nearest geom along z-axis
mjSENS_CAMPROJECTION, // pixel coordinates of a site in the camera image
// sensors related to scalar joints, tendons, actuators
src/engine/engine_sensor.c
+56 -5
View File
@@ -435,11 +435,62 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
break;
case mjSENS_RANGEFINDER: // rangefinder
rvec[0] = d->site_xmat[9*objid+2];
rvec[1] = d->site_xmat[9*objid+5];
rvec[2] = d->site_xmat[9*objid+8];
d->sensordata[adr] = mj_ray(m, d, d->site_xpos+3*objid, rvec, NULL, 1,
m->site_bodyid[objid], NULL);
if (objtype == mjOBJ_SITE) {
rvec[0] = d->site_xmat[9*objid+2];
rvec[1] = d->site_xmat[9*objid+5];
rvec[2] = d->site_xmat[9*objid+8];
d->sensordata[adr] = mj_ray(m, d, d->site_xpos+3*objid, rvec, NULL, 1,
m->site_bodyid[objid], NULL);
} else {
// camera-attached rangefinder: depth image
const int width = m->cam_resolution[2*objid];
const int height = m->cam_resolution[2*objid+1];
const int bodyexclude = m->cam_bodyid[objid];
const mjtNum* cam_xpos = d->cam_xpos + 3*objid;
const mjtNum* cam_xmat = d->cam_xmat + 9*objid;
const int projection = m->cam_projection[objid];
// compute focal length in pixels using helper
mjtNum fx, fy, cx, cy, ortho_extent;
mju_camIntrinsics(m, objid, &fx, &fy, &cx, &cy, &ortho_extent);
if (projection == mjPROJ_PERSPECTIVE) {
// perspective: all rays share origin, different directions
const int npixel = width * height;
mj_markStack(d);
mjtNum* vec = mjSTACKALLOC(d, 3*npixel, mjtNum);
int* geomid = mjSTACKALLOC(d, npixel, int);
// compute ray directions using helper (normalized)
for (int row = 0; row < height; row++) {
for (int col = 0; col < width; col++) {
int idx = row*width + col;
mjtNum origin[3];
mju_camPixelRay(origin, vec + 3*idx, cam_xpos, cam_xmat,
col, row, fx, fy, cx, cy, projection, ortho_extent);
}
}
// cast all rays
mj_multiRay(m, d, cam_xpos, vec, NULL, 1, bodyexclude,
geomid, d->sensordata + adr, npixel, mjMAXVAL);
mj_freeStack(d);
} else {
// orthographic: parallel rays, different origins
for (int row = 0; row < height; row++) {
for (int col = 0; col < width; col++) {
int idx = row*width + col;
mjtNum origin[3], direction[3];
mju_camPixelRay(origin, direction, cam_xpos, cam_xmat,
col, row, fx, fy, cx, cy, projection, ortho_extent);
d->sensordata[adr + idx] = mj_ray(m, d, origin, direction, NULL, 1,
bodyexclude, NULL);
}
}
}
}
break;
src/engine/engine_support.c
+40
View File
@@ -743,3 +743,43 @@ int mju_condataSize(int dataspec) {
}
return size;
}
// compute camera pixel parameters from model, output are:
// pixel units: fx, fy (focal lengths), cx, cy (principal point)
// length units: extent
void mju_camIntrinsics(const mjModel* m, int camid,
mjtNum* fx, mjtNum* fy, mjtNum* cx, mjtNum* cy, mjtNum* extent) {
const int width = m->cam_resolution[2*camid];
const int height = m->cam_resolution[2*camid+1];
const float* sensorsize = m->cam_sensorsize + 2*camid;
const float* intrinsic = m->cam_intrinsic + 4*camid;
const mjtProjection projection = (mjtProjection)m->cam_projection[camid];
switch (projection) {
case mjPROJ_PERSPECTIVE:
if (sensorsize[0] && sensorsize[1]) {
// intrinsic-based perspective camera
*fx = intrinsic[0] / sensorsize[0] * width;
*fy = intrinsic[1] / sensorsize[1] * height;
*cx = intrinsic[2] / sensorsize[0] * width;
*cy = intrinsic[3] / sensorsize[1] * height;
} else {
// fovy-based perspective camera
*fx = *fy = 0.5 / mju_tan(m->cam_fovy[camid] * mjPI / 360.0) * height;
*cx = (mjtNum)width / 2.0;
*cy = (mjtNum)height / 2.0;
}
break;
case mjPROJ_ORTHOGRAPHIC:
// orthographic: normalize pixel offset to [-1, 1]
*fx = (mjtNum)width / 2.0;
*fy = (mjtNum)height / 2.0;
*cx = *fx;
*cy = *fy;
break;
}
// extent only used for orthographic cameras
*extent = m->cam_fovy[camid];
}
src/engine/engine_support.h
+6
View File
@@ -120,6 +120,12 @@ MJAPI const char* mj_versionString(void);
// return total size of data fields in a contact sensor bitfield specification
MJAPI int mju_condataSize(int dataSpec);
// compute camera pixel parameters from model
// outputs: fx, fy (focal length in pixels), cx, cy (principal point), ortho_extent
void mju_camIntrinsics(const mjModel* m, int camid,
mjtNum* fx, mjtNum* fy, mjtNum* cx, mjtNum* cy,
mjtNum* ortho_extent);
#ifdef __cplusplus
}
#endif
src/engine/engine_util_misc.c
+38 -1
View File
@@ -417,6 +417,8 @@ mjtNum mju_wrap(mjtNum wpnt[6], const mjtNum x0[3], const mjtNum x1[3],
}
//------------------------------ misc geometry -----------------------------------------------------
// all 3 semi-axes of a geom
void mju_geomSemiAxes(mjtNum semiaxes[3], const mjtNum size[3], mjtGeom type) {
switch (type) {
@@ -494,7 +496,42 @@ int mju_insideGeom(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3
}
// ----------------------------- Flex interpolation ------------------------------------------------
// compute ray origin and direction for pixel (col, row) in camera image
// for perspective: origin is unchanged, direction is computed
// for orthographic: direction is -Z in camera frame, origin is offset from camera center
void mju_camPixelRay(mjtNum origin[3], mjtNum direction[3],
const mjtNum cam_xpos[3], const mjtNum cam_xmat[9],
int col, int row, mjtNum fx, mjtNum fy, mjtNum cx, mjtNum cy,
int projection, mjtNum ortho_extent) {
// pixel center (row 0 = top of image)
mjtNum px = col + 0.5 - cx;
mjtNum py = row + 0.5 - cy;
if (projection == mjPROJ_PERSPECTIVE) {
// origin is camera position
mju_copy3(origin, cam_xpos);
// direction in camera frame: (x/fx, -y/fy, -1), then normalized
mjtNum dir_cam[3] = {px / fx, -py / fy, -1.0};
mju_mulMatVec3(direction, cam_xmat, dir_cam);
mju_normalize3(direction);
} else {
// orthographic: parallel rays, direction is -Z in camera frame
direction[0] = -cam_xmat[2];
direction[1] = -cam_xmat[5];
direction[2] = -cam_xmat[8];
// origin offset in camera frame (ortho_extent is full height, use half for each side)
mjtNum half_extent = ortho_extent / 2;
mjtNum offset_cam[3] = {px / fx * half_extent, -py / fy * half_extent, 0};
mjtNum offset_world[3];
mju_mulMatVec3(offset_world, cam_xmat, offset_cam);
mju_add3(origin, cam_xpos, offset_world);
}
}
// ----------------------------- flex interpolation ------------------------------------------------
mjtNum static inline phi(mjtNum s, int i, int order) {
if (order == 1) {
src/engine/engine_util_misc.h
+7
View File
@@ -57,6 +57,13 @@ MJAPI void mju_geomSemiAxes(mjtNum semiaxes[3], const mjtNum size[3], mjtGeom ty
int mju_insideGeom(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3], mjtGeom type,
const mjtNum point[3]);
// compute ray origin and direction for pixel (col, row) in camera image
// directions are normalized so ray functions return actual 3D distance
void mju_camPixelRay(mjtNum origin[3], mjtNum direction[3],
const mjtNum cam_xpos[3], const mjtNum cam_xmat[9],
int col, int row, mjtNum fx, mjtNum fy, mjtNum cx, mjtNum cy,
int projection, mjtNum ortho_extent);
// ----------------------------- Flex interpolation ------------------------------------------------
// evaluate the deformation gradient at p using the nodal dof values
src/engine/engine_vis_visualize.c
+55 -13
View File
@@ -2478,22 +2478,64 @@ static void addRangefinderGeoms(const mjModel* m, mjData* d, const mjvOption* vo
for (int i=0; i < m->nsensor; i++) {
if (m->sensor_type[i] == mjSENS_RANGEFINDER) {
// sensor data
mjtNum dst = d->sensordata[m->sensor_adr[i]];
int sid = m->sensor_objid[i];
int objid = m->sensor_objid[i];
int adr = m->sensor_adr[i];
// null output: nothing to render
if (dst < 0) {
continue;
// site-attached rangefinder
if (m->sensor_objtype[i] == mjOBJ_SITE) {
mjtNum dst = d->sensordata[adr];
// null output: nothing to render
if (dst < 0) {
continue;
}
// make ray
mjtNum* from = d->site_xpos+3*objid;
mjtNum to[3] = {from[0] + d->site_xmat[9*objid+2]*dst,
from[1] + d->site_xmat[9*objid+5]*dst,
from[2] + d->site_xmat[9*objid+8]*dst};
addConnector(scn, mjGEOM_LINE, 3, from, to, m->vis.rgba.rangefinder,
i, mjCAT_DECOR, mjOBJ_SENSOR);
}
// make ray
mjtNum* from = d->site_xpos+3*sid;
mjtNum to[3] = {from[0] + d->site_xmat[9*sid+2]*dst,
from[1] + d->site_xmat[9*sid+5]*dst,
from[2] + d->site_xmat[9*sid+8]*dst};
addConnector(scn, mjGEOM_LINE, 3, from, to, m->vis.rgba.rangefinder,
i, mjCAT_DECOR, mjOBJ_SENSOR);
// camera-attached rangefinder
else if (m->sensor_objtype[i] == mjOBJ_CAMERA) {
const int width = m->cam_resolution[2*objid];
const int height = m->cam_resolution[2*objid+1];
const mjtNum* cam_xpos = d->cam_xpos + 3*objid;
const mjtNum* cam_xmat = d->cam_xmat + 9*objid;
const int projection = m->cam_projection[objid];
// compute focal length in pixels using helper
mjtNum fx, fy, cx, cy, ortho_extent;
mju_camIntrinsics(m, objid, &fx, &fy, &cx, &cy, &ortho_extent);
// draw ray for each pixel
for (int row = 0; row < height; row++) {
for (int col = 0; col < width; col++) {
int idx = row*width + col;
mjtNum dst = d->sensordata[adr + idx];
// null output: nothing to render
if (dst < 0) {
continue;
}
// compute ray origin and direction
mjtNum origin[3], direction[3];
mju_camPixelRay(origin, direction, cam_xpos, cam_xmat,
col, row, fx, fy, cx, cy, projection, ortho_extent);
// compute endpoint
mjtNum to[3];
mju_addScl3(to, origin, direction, dst);
addConnector(scn, mjGEOM_LINE, 3, origin, to, m->vis.rgba.rangefinder,
i, mjCAT_DECOR, mjOBJ_SENSOR);
}
}
}
} else if (m->sensor_type[i] == mjSENS_GEOMFROMTO) {
// sensor data
mjtNum* fromto = d->sensordata + m->sensor_adr[i];
src/user/user_api.cc
+9 -1
View File
@@ -1252,7 +1252,6 @@ void mjs_deleteUserValue(mjsElement* element, const char* key) {
int mjs_sensorDim(const mjsSensor* sensor) {
switch (sensor->type) {
case mjSENS_TOUCH:
case mjSENS_RANGEFINDER:
case mjSENS_JOINTPOS:
case mjSENS_JOINTVEL:
case mjSENS_TENDONPOS:
@@ -1275,6 +1274,15 @@ int mjs_sensorDim(const mjsSensor* sensor) {
case mjSENS_CLOCK:
return 1;
case mjSENS_RANGEFINDER:
if (sensor->objtype == mjOBJ_CAMERA) {
const mjCCamera* camera = static_cast<const mjCCamera*>(
static_cast<mjCSensor*>(sensor->element)->get_obj());
return static_cast<int>(camera->spec.resolution[0]) *
static_cast<int>(camera->spec.resolution[1]);
}
return 1; // site-attached: single ray
case mjSENS_CAMPROJECTION:
return 2;
src/user/user_objects.cc
+7 -1
View File
@@ -7313,7 +7313,6 @@ void mjCSensor::Compile(void) {
case mjSENS_FORCE:
case mjSENS_TORQUE:
case mjSENS_MAGNETOMETER:
case mjSENS_RANGEFINDER:
case mjSENS_CAMPROJECTION:
// must be attached to site
if (objtype != mjOBJ_SITE) {
@@ -7329,6 +7328,13 @@ void mjCSensor::Compile(void) {
}
break;
case mjSENS_RANGEFINDER:
// must be attached to site or camera
if (objtype != mjOBJ_SITE && objtype != mjOBJ_CAMERA) {
throw mjCError(this, "sensor must be attached to site or camera");
}
break;
case mjSENS_JOINTPOS:
case mjSENS_JOINTVEL:
case mjSENS_JOINTACTFRC:
src/xml/xml_native_reader.cc
+7 -3
View File
@@ -454,7 +454,7 @@ std::vector<const char*> MJCF[nMJCF] = {
{"torque", "*", "name", "site", "cutoff", "noise", "user"},
{"magnetometer", "*", "name", "site", "cutoff", "noise", "user"},
{"camprojection", "*", "name", "site", "camera", "cutoff", "noise", "user"},
{"rangefinder", "*", "name", "site", "cutoff", "noise", "user"},
{"rangefinder", "*", "name", "site", "camera", "cutoff", "noise", "user"},
{"jointpos", "*", "name", "joint", "cutoff", "noise", "user"},
{"jointvel", "*", "name", "joint", "cutoff", "noise", "user"},
{"tendonpos", "*", "name", "tendon", "cutoff", "noise", "user"},
@@ -4052,8 +4052,12 @@ void mjXReader::Sensor(XMLElement* section) {
sensor->reftype = mjOBJ_CAMERA;
} else if (type == "rangefinder") {
sensor->type = mjSENS_RANGEFINDER;
sensor->objtype = mjOBJ_SITE;
ReadAttrTxt(elem, "site", objname, true);
bool use_site = ReadAttrTxt(elem, "site", objname, false);
bool use_camera = ReadAttrTxt(elem, "camera", objname, false);
if (use_site == use_camera) {
throw mjXError(elem, "rangefinder requires exactly one of 'site' or 'camera'");
}
sensor->objtype = use_site ? mjOBJ_SITE : mjOBJ_CAMERA;
}
// sensors related to scalar joints, tendons, actuators
src/xml/xml_native_writer.cc
+5 -1
View File
@@ -2029,7 +2029,11 @@ void mjXWriter::Sensor(XMLElement* root) {
break;
case mjSENS_RANGEFINDER:
elem = InsertEnd(section, "rangefinder");
WriteAttrTxt(elem, "site", sensor->get_objname());
if (sensor->objtype == mjOBJ_SITE) {
WriteAttrTxt(elem, "site", sensor->get_objname());
} else {
WriteAttrTxt(elem, "camera", sensor->get_objname());
}
break;
case mjSENS_CAMPROJECTION:
elem = InsertEnd(section, "camprojection");
test/engine/engine_sensor_test.cc
+91 -1
View File
@@ -67,7 +67,7 @@ static vector<mjtNum> GetSensor(const mjModel* model,
using SensorTest = MujocoTest;
// --------------------- test sensor disableflag ------------------------------
// --------------------- test sensor disable flag ------------------------------
TEST_F(SensorTest, DisableSensors) {
constexpr char xml[] = R"(
@@ -1052,5 +1052,95 @@ TEST_F(SensorTest, InsideSite) {
mj_deleteModel(model);
}
TEST_F(SensorTest, RangefinderCamera) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom type="plane" size="10 10 .1"/>
<body pos="0 0 2">
<camera name="persp" xyaxes="1 0 0 0 1 0" resolution="3 3" fovy="90"/>
<camera name="ortho" euler="0 45 0" resolution="3 3"
projection="orthographic" fovy="2"/>
</body>
</worldbody>
<sensor>
<rangefinder camera="persp"/>
<rangefinder camera="ortho"/>
</sensor>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
// sensordata dimension should be 3x3 + 3x3 = 18
EXPECT_EQ(model->nsensordata, 18);
mjData* data = mj_makeData(model);
mj_forward(model, data);
mjtNum tol = 1e-6;
mjtNum height = 2.0;
mjtNum fy = 1.5;
mjtNum offsets[3] = {-1.0, 0.0, 1.0}; // pixel center - principal point
// perspective camera: rays diverge, distance varies with angle
for (int row = 0; row < 3; row++) {
for (int col = 0; col < 3; col++) {
int idx = row * 3 + col;
mjtNum dx = offsets[col] / fy;
mjtNum dy = offsets[row] / fy;
mjtNum expected = height * mju_sqrt(1 + dx*dx + dy*dy);
EXPECT_NEAR(data->sensordata[idx], expected, tol)
<< "perspective pixel (" << row << ", " << col << ")";
}
}
// orthographic camera: tilted 45 degrees around Y axis
// rays are parallel at 45 degrees, distance depends on pixel x-offset
// for center pixel at (0,0,2): distance = 2 / cos(45) = 2*sqrt(2)
// for off-center pixels: x-offset shifts origin, affecting where ray hits z=0
mjtNum extent = 2.0; // fovy for orthographic
mjtNum half_extent = extent / 2;
mjtNum fx = 1.5; // width / 2 for 3x3 image
mjtNum cx = 1.5; // principal point
mjtNum cos45 = mju_sqrt(0.5);
mjtNum sin45 = mju_sqrt(0.5);
for (int row = 0; row < 3; row++) {
for (int col = 0; col < 3; col++) {
int idx = 9 + row * 3 + col; // offset by first sensor's 9 values
// pixel offset in camera frame: matches mju_camPixelRay formula
mjtNum px_cam = (col + 0.5 - cx) / fx * half_extent;
// camera tilted 45° around Y: local +X maps to world (+cos45, 0, -sin45)
mjtNum origin_z = height - px_cam * sin45;
// ray hits z=0 plane: distance = origin_z / cos45
mjtNum expected = origin_z / cos45;
EXPECT_NEAR(data->sensordata[idx], expected, tol)
<< "orthographic pixel (" << row << ", " << col << ")";
}
}
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(SensorTest, RFCamera) {
const string xml_path =
GetTestDataFilePath("engine/testdata/sensor/rfcamera.xml");
char error[1024];
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
mj_step(model, data);
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
test/engine/testdata/sensor/rfcamera.xml
Vendored
+36
View File
@@ -0,0 +1,36 @@
<mujoco model="rangefinder camera">
<visual>
<rgba frustum="1 1 0 0.1"/>
</visual>
<worldbody>
<light pos="0 0 3"/>
<!-- ground plane -->
<geom type="plane" size="5 5 .1" rgba=".3 .4 .5 1"/>
<!-- scattered objects for rays to hit -->
<geom type="sphere" pos="0 0 .4" size=".4" rgba="1 .3 .3 1"/>
<geom type="box" pos="1 0.5 .35" size=".35 .35 .35" rgba=".3 1 .3 1"/>
<geom type="cylinder" pos="-0.8 0.6 .4" size=".4 .4" rgba=".3 .3 1 1"/>
<geom type="capsule" pos="0.5 -0.7 0" size=".25 .35" zaxis="1 0.5 0" rgba="1 1 .3 1"/>
<geom type="ellipsoid" pos="-0.5 -0.5 .2" size=".4 .3 .25" rgba="1 .3 1 1"/>
<!-- mocap body with perspective camera -->
<body pos="1 0 2" euler="0 20 0" mocap="true">
<geom type="box" pos="0 0 .3" size=".2 .2 .2"/>
<camera name="perspective" resolution="4 4" fovy="60"/>
</body>
<!-- mocap body with orthographic camera -->
<body pos="-1 0 2" euler="0 -20 0" mocap="true">
<geom type="box" pos="0 0 .3" size=".2 .2 .2"/>
<camera name="orthographic" resolution="4 4" fovy="1.5" projection="orthographic"/>
</body>
</worldbody>
<sensor>
<rangefinder camera="perspective"/>
<rangefinder camera="orthographic"/>
</sensor>
</mujoco>