Specify camera parameters using standard robotics conventions.

PiperOrigin-RevId: 569147555
Change-Id: I3443f95e5de56a17024f5e5c03f33888dd583edb

src/user/user_model.cc

@@ -1623,6 +1623,8 @@ void mjCModel::CopyTree(mjModel* m) {
       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_sensorsize+2*cid, pc->sensor_size, 2);
+      copyvec(m->cam_intrinsic+4*cid, pc->intrinsic, 4);
       copyvec(m->cam_user+nuser_cam*cid, pc->userdata.data(), nuser_cam);
     }
 
@@ -3084,6 +3086,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
     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);
+    copyvec(cameras[i]->intrinsic, m->cam_intrinsic+4*i, 4);
 
     if (nuser_cam) {
       copyvec(cameras[i]->userdata.data(), m->cam_user + nuser_cam*i, nuser_cam);

src/user/user_objects.cc

@@ -17,6 +17,7 @@
 #include <algorithm>
 #include <cmath>
 #include <cstddef>
+#include <cstdio>
 #include <cstdlib>
 #include <cstring>
 #include <sstream>
@@ -1950,6 +1951,12 @@ mjCCamera::mjCCamera(mjCModel* _model, mjCDef* _def) {
   ipd = 0.068;
   userdata.clear();
   resolution[0] = resolution[1] = 1;
+  principal_length[0] = principal_length[1] = 0;
+  principal_pixel[0] = principal_pixel[1] = 0;
+  focal_length[0] = focal_length[1] = 0;
+  focal_pixel[0] = focal_pixel[1] = 0;
+  sensor_size[0] = sensor_size[1] = 0;
+  mjuu_setvec(intrinsic, 0, 0, 0, 0);
 
   // clear private variables
   body = 0;
@@ -2010,6 +2017,39 @@ void mjCCamera::Compile(void) {
     throw mjCError(this, "fovy too large in camera '%s' (id = %d, value = %d)",
                    name.c_str(), id, fovy);
   }
+
+  // check that specs are not duplicated
+  if ((principal_length[0] && principal_pixel[0]) ||
+      (principal_length[1] && principal_pixel[1])) {
+    throw mjCError(this, "principal length duplicated in camera '%s' (id = %d)",
+                   name.c_str(), id);
+  }
+
+  if ((focal_length[0] && focal_pixel[0]) ||
+      (focal_length[1] && focal_pixel[1])) {
+    throw mjCError(this, "focal length duplicated in camera '%s' (id = %d)",
+                   name.c_str(), id);
+  }
+
+  // compute number of pixels per unit length
+  if (sensor_size[0]>0 && sensor_size[1]>0) {
+    float pixel_density[2] = {
+      (float)resolution[0] / sensor_size[0],
+      (float)resolution[1] / sensor_size[1],
+    };
+
+    // defaults are zero, so only one term in each sum is nonzero
+    intrinsic[0] = focal_pixel[0] / pixel_density[0] + focal_length[0];
+    intrinsic[1] = focal_pixel[1] / pixel_density[1] + focal_length[1];
+    intrinsic[2] = principal_pixel[0] / pixel_density[0] + principal_length[0];
+    intrinsic[3] = principal_pixel[1] / pixel_density[1] + principal_length[1];
+
+    // fovy with principal point at (0, 0)
+    fovy = mju_atan2((float)sensor_size[1]/2, intrinsic[1]) * 360.0 / mjPI;
+  } else {
+    intrinsic[0] = model->visual.map.znear;
+    intrinsic[1] = model->visual.map.znear;
+  }
 }
 
 

src/user/user_objects.h

@@ -465,7 +465,13 @@ class mjCCamera : public mjCBase {
   double ipd;                     // inter-pupilary distance
   double pos[3];                  // position
   double quat[4];                 // orientation
+  float intrinsic[4];             // camera intrinsics [length]
+  float sensor_size[2];           // sensor size [length]
   float resolution[2];            // resolution [pixel]
+  float focal_length[2];          // focal length [length]
+  float focal_pixel[2];           // focal length [pixel]
+  float principal_length[2];      // principal point [length]
+  float principal_pixel[2];       // principal point [pixel]
   std::vector<double> userdata;   // user data
   mjCAlternative alt;             // alternative orientation specification