From 027cfd1201d9d3c405cef36e48eb408ffb93c3ff Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Tue, 23 Jun 2026 04:14:36 -0700 Subject: [PATCH] simplify camera projection sensor PiperOrigin-RevId: 936571266 Change-Id: I5647e61fcc81ba6eb482dd95d3c40438f478bb18 --- src/engine/engine_sensor.c | 74 ++------------ test/engine/engine_sensor_test.cc | 165 +++++++++++++++++++++++++++++- 2 files changed, 172 insertions(+), 67 deletions(-) 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"(