Merge pull request #3150 from kevinzakka:fix/principal-point-frustum-sign

PiperOrigin-RevId: 878610460
Change-Id: I82e1c30c9c263a9981c619d6a401b417f95c39a2

src/engine/engine_vis_visualize.c

@@ -2910,8 +2910,8 @@ void mjv_updateCamera(const mjModel* m, const mjData* d, mjvCamera* cam, mjvScen
     scn->camera[view].orthographic = orthographic;
 
     // 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_top = zver[0];
+    scn->camera[view].frustum_bottom = -zver[1];
     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 = zclip[0];

test/engine/engine_vis_visualize_test.cc

@@ -108,5 +108,51 @@ TEST_F(MjvSceneTest, UpdateSceneGeomsExhausted) {
   mj_deleteModel(model);
 }
 
+TEST_F(MjvSceneTest, PrincipalPointFrustumSign) {
+  constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <camera name="cam" pos="0 0 1" zaxis="0 0 1"
+              sensorsize="0.01 0.01" focal="0.01 0.01"
+              principal="0 0.002"/>
+    </worldbody>
+  </mujoco>
+  )";
+
+  mjModel* model = LoadModelFromString(xml);
+  ASSERT_THAT(model, NotNull());
+  mjData* data = mj_makeData(model);
+  mj_forward(model, data);
+
+  InitSceneObjects(model);
+
+  // point camera at the fixed cam
+  cam_.type = mjCAMERA_FIXED;
+  cam_.fixedcamid = 0;
+  mjv_updateCamera(model, data, &cam_, &scn_);
+
+  float top = scn_.camera[0].frustum_top;
+  float bottom = scn_.camera[0].frustum_bottom;
+
+  // with cy > 0 the principal point is above center, so the frustum should
+  // extend further downward than upward: |bottom| > top
+  EXPECT_GT(-bottom, top);
+
+  // verify exact values against the pinhole model
+  float znear = model->vis.map.znear * model->stat.extent;
+  float cy = model->cam_intrinsic[3];
+  float fy = model->cam_intrinsic[1];
+  float sh = model->cam_sensorsize[1];
+  float half = znear / fy * (sh / 2);
+  float offset = znear / fy * cy;
+
+  EXPECT_FLOAT_EQ(top, half - offset);
+  EXPECT_FLOAT_EQ(bottom, -(half + offset));
+
+  mj_deleteData(data);
+  FreeSceneObjects();
+  mj_deleteModel(model);
+}
+
 }  // namespace
 }  // namespace mujoco