diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c
index 8e4d2c9c..fe28a292 100644
--- a/src/engine/engine_sensor.c
+++ b/src/engine/engine_sensor.c
@@ -284,29 +284,7 @@ static void cam_project(mjtNum sensordata[2], const mjtNum target_xpos[3],
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;
- 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)
+ // focal transformation
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];
@@ -314,48 +292,16 @@ static void cam_project(mjtNum sensordata[2], const mjtNum target_xpos[3],
fx = fy = .5 / mju_tan(cam_fovy * mjPI / 360.) * cam_res[1];
}
- mjtNum focal[3][4] = {0};
- focal[0][0] = -fx;
- focal[1][1] = fy;
- focal[2][2] = 1.0;
+ // relative position in world frame
+ mjtNum relative_pos[3];
+ mju_sub3(relative_pos, target_xpos, cam_xpos);
- // 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] / 2.0;
- image[1][2] = (mjtNum)cam_res[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];
- }
- }
+ // project to camera frame: cam_pos = cam_xmat^T * relative_pos
+ mjtNum cam_pos[3];
+ mju_mulMatTVec(cam_pos, cam_xmat, relative_pos, 3, 3);
// avoid dividing by tiny numbers
- mjtNum denom = pixel_coord_hom[2];
+ mjtNum denom = cam_pos[2];
if (mju_abs(denom) < mjMINVAL) {
if (denom < 0) {
denom = mju_min(denom, -mjMINVAL);
@@ -365,8 +311,8 @@ static void cam_project(mjtNum sensordata[2], const mjtNum target_xpos[3],
}
// compute projection
- sensordata[0] = pixel_coord_hom[0] / denom;
- sensordata[1] = pixel_coord_hom[1] / denom;
+ sensordata[0] = -fx * (cam_pos[0] / denom) + 0.5 * (mjtNum)cam_res[0];
+ sensordata[1] = fy * (cam_pos[1] / denom) + 0.5 * (mjtNum)cam_res[1];
}
diff --git a/test/engine/engine_sensor_test.cc b/test/engine/engine_sensor_test.cc
index c2c10fbf..45ffd6ac 100644
--- a/test/engine/engine_sensor_test.cc
+++ b/test/engine/engine_sensor_test.cc
@@ -982,9 +982,9 @@ TEST_F(SensorTest, CameraProjection) {
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
- // call step to update sensors
- mj_step(model.get(), data.get());
- mj_step1(model.get(), data.get()); // update values of position-based sensors
+ // update positions and sensors
+ mj_fwdPosition(model.get(), data.get());
+ mj_sensorPos(model.get(), data.get());
EXPECT_THAT(model->cam_resolution[0], 1920);
EXPECT_THAT(model->cam_resolution[1], 1200);
mjtNum eps = 1e-4;
@@ -996,6 +996,165 @@ TEST_F(SensorTest, CameraProjection) {
EXPECT_NEAR(data->sensordata[5], 600, eps);
}
+// previous implementation of cam_project to verify the new one
+static void cam_project_old(
+ 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 float cam_intrinsic[4], const float cam_sensorsize[2]) {
+ mjtNum fx, fy;
+
+ // translation matrix (4x4)
+ mjtNum translation[4][4] = {};
+ 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] = {};
+ 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)
+ 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] = {};
+ focal[0][0] = -fx;
+ focal[1][1] = fy;
+ focal[2][2] = 1.0;
+
+ // image matrix (3x3)
+ mjtNum image[3][3] = {};
+ image[0][0] = 1;
+ image[1][1] = 1;
+ image[2][2] = 1;
+ image[0][2] = (mjtNum)cam_res[0] / 2.0;
+ image[1][2] = (mjtNum)cam_res[1] / 2.0;
+
+ // projection matrix (3x4): product of all 4 matrices
+ mjtNum proj[3][4] = {};
+ 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;
+}
+
+TEST_F(SensorTest, CameraProjectionComparison) {
+ constexpr char xml[] = R"(
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ )";
+ char error[1024];
+ MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
+ ASSERT_THAT(model.get(), NotNull()) << error;
+ MjDataPtr data = MakeData(model);
+
+ mj_fwdPosition(model.get(), data.get());
+ mj_sensorPos(model.get(), data.get());
+
+ for (int i = 0; i < model->nsensor; ++i) {
+ if (model->sensor_type[i] == mjSENS_CAMPROJECTION) {
+ int objid = model->sensor_objid[i];
+ int refid = model->sensor_refid[i];
+
+ mjtNum expected[2];
+ cam_project_old(expected,
+ data->site_xpos + 3*objid,
+ data->cam_xpos + 3*refid,
+ data->cam_xmat + 9*refid,
+ model->cam_resolution + 2*refid,
+ model->cam_fovy[refid],
+ model->cam_intrinsic + 4*refid,
+ model->cam_sensorsize + 2*refid);
+
+ int adr = model->sensor_adr[i];
+ EXPECT_NEAR(data->sensordata[adr], expected[0], MjTol(1e-10, 1e-3));
+ EXPECT_NEAR(data->sensordata[adr + 1], expected[1], MjTol(1e-10, 1e-3));
+ }
+ }
+}
+
TEST_F(SensorTest, InsideSite) {
constexpr char xml[] = R"(