Can set projection matrix or camera intrinsics in the ADF file (#243)

adnanmunawar · web-flow · commit 6beeac740ffd · 2025-03-26T13:10:46.000-04:00
diff --git a/adf_loader/version_1_0/adf_loader_1_0.cpp b/adf_loader/version_1_0/adf_loader_1_0.cpp
@@ -1250,6 +1250,7 @@ bool ADFLoader_1_0::loadCameraAttribs(YAML::Node *a_node, afCameraAttributes *at
     YAML::Node upNode = node["up"];
     YAML::Node clippingPlaneNode = node["clipping plane"];
     YAML::Node fieldViewAngleNode = node["field view angle"];
+    YAML::Node windowResolutionNode = node["window resolution"];
     YAML::Node orthoWidthNode = node["orthographic view width"];
     YAML::Node stereoNode = node["stereo"];
     YAML::Node controllingDevicesDataNode = node["controlling devices"];
@@ -1266,6 +1267,8 @@ bool ADFLoader_1_0::loadCameraAttribs(YAML::Node *a_node, afCameraAttributes *at
     YAML::Node depthShaderNode = node["depth compute shaders"];
     YAML::Node multiPassNode = node["multipass"];
     YAML::Node mouseControlMultiplierNode = node["mouse control multipliers"];
+    YAML::Node cameraIntrinsicsNode = node["camera intrinsics"];
+    YAML::Node projectionMatrixNode = node["projection matrix"];
 
     bool valid = true;
 
@@ -1329,9 +1332,20 @@ bool ADFLoader_1_0::loadCameraAttribs(YAML::Node *a_node, afCameraAttributes *at
         attribs->m_publishImageInterval = publishImageIntervalNode.as<uint>();
     }
 
+    if (windowResolutionNode.IsDefined()){
+        attribs->m_windowResolution.m_width = windowResolutionNode["width"].as<double>();
+        attribs->m_windowResolution.m_height = windowResolutionNode["height"].as<double>();
+        attribs->m_windowResolution.m_defined = true;
+    }
+
     if (publishImageResolutionNode.IsDefined()){
-        attribs->m_publishImageResolution.m_width = publishImageResolutionNode["width"].as<double>();
-        attribs->m_publishImageResolution.m_height = publishImageResolutionNode["height"].as<double>();
+        attribs->m_publishImageResolution.m_width = publishImageResolutionNode["width"].as<unsigned int>();
+        attribs->m_publishImageResolution.m_height = publishImageResolutionNode["height"].as<unsigned int>();
+        attribs->m_publishImageResolution.m_defined = true;
+    }
+    else if (attribs->m_windowResolution.m_defined){
+        cerr << "INFO! FOR CAMERA " << attribs->m_identificationAttribs.m_name << " PUBLISH IMAGE RESOLUTION NOT SPECIFIED THEREFORE USING DEFINED WINDOW RESOLUTION.";
+        attribs->m_publishImageResolution = attribs->m_windowResolution;
     }
 
     if (publishDepthNode.IsDefined()){
@@ -1345,6 +1359,7 @@ bool ADFLoader_1_0::loadCameraAttribs(YAML::Node *a_node, afCameraAttributes *at
     if (publishDepthResolutionNode.IsDefined()){
         attribs->m_publishDephtResolution.m_width = publishDepthResolutionNode["width"].as<double>();
         attribs->m_publishDephtResolution.m_height = publishDepthResolutionNode["height"].as<double>();
+        attribs->m_publishDephtResolution.m_defined = true;
     }
 
     if (publishDepthNoiseNode.IsDefined()){
@@ -1392,6 +1407,49 @@ bool ADFLoader_1_0::loadCameraAttribs(YAML::Node *a_node, afCameraAttributes *at
         }
     }
 
+    if (cameraIntrinsicsNode.IsDefined()){
+
+        bool defined = false;
+        if (cameraIntrinsicsNode["fx"].IsDefined()){ // Focal Length X
+            attribs->m_intrinsics.m_fx = cameraIntrinsicsNode["fx"].as<double>();
+            defined |= true;
+        }
+        if (cameraIntrinsicsNode["fy"].IsDefined()){ // Focal Length Y
+            attribs->m_intrinsics.m_fy = cameraIntrinsicsNode["fy"].as<double>();
+            defined |= true;
+        }
+        if (cameraIntrinsicsNode["f"].IsDefined()){ // Focal Length
+            attribs->m_intrinsics.m_fx = cameraIntrinsicsNode["f"].as<double>();
+            attribs->m_intrinsics.m_fy = cameraIntrinsicsNode["f"].as<double>();
+            defined |= true;
+        }
+        if (cameraIntrinsicsNode["cx"].IsDefined()){ // Principal Offset X
+            attribs->m_intrinsics.m_cx = cameraIntrinsicsNode["cx"].as<double>();
+            defined |= true;
+        }
+        if (cameraIntrinsicsNode["cy"].IsDefined()){ // Principal Offset Y
+            attribs->m_intrinsics.m_cy = cameraIntrinsicsNode["cy"].as<double>();
+            defined |= true;
+        }
+        if (cameraIntrinsicsNode["s"].IsDefined()){ // Shear
+            attribs->m_intrinsics.m_s = cameraIntrinsicsNode["s"].as<double>();
+            defined |= true;
+        }
+        attribs->m_intrinsics.m_defined = defined;
+    }
+
+    if (projectionMatrixNode.IsDefined()){
+        try{
+            attribs->m_projectionMatrix = projectionMatrixNode.as<vector<vector<double>>>();
+            attribs->m_useCustomProjectionMatrix = true;
+            cerr << "INFO! CUSTOM PROJECTION MATRIX DEFINED FOR CAMERA " << attribs->m_identificationAttribs.m_name << endl;
+        }
+        catch(YAML::Exception& e){
+            cerr << "ERROR! FAILED TO READ CUSTOM PROJECTION MATRIX FOR CAMERA " << attribs->m_identificationAttribs.m_name << endl;
+            cerr << "EXCEPTION! " << e.what() << endl;
+        }
+    }
+
     return valid;
 }
 
diff --git a/ambf_framework/afAttributes.h b/ambf_framework/afAttributes.h
@@ -514,9 +514,11 @@ struct afImageResolutionAttribs{
     afImageResolutionAttribs(){
         m_width = 640;
         m_height = 480;
+        m_defined = false;
     }
-    double m_width;
-    double m_height;
+    unsigned int m_width;
+    unsigned int m_height;
+    bool m_defined;
 };
 
 
@@ -548,6 +550,23 @@ struct afMouseControlScales{
     double m_arcball;
 };
 
+struct afCameraIntrinsics{
+    afCameraIntrinsics(){
+        m_defined = false;
+        m_fx = 100.;
+        m_fy = 100.;
+        m_cx = 0.;
+        m_cy = 0.;
+        m_s = 0.;
+    }
+    bool m_defined;
+    double m_fx; // Focal length X
+    double m_fy; // Focal Length Y
+    double m_cx; // Principal Offset X
+    double m_cy; // Principal Offset Y
+    double m_s; // Shear
+};
+
 ///
 /// \brief The afCameraAttributes struct
 ///
@@ -570,6 +589,7 @@ struct afCameraAttributes: public afBaseObjectAttributes
         m_publishImageInterval = 1;
         m_publishDepthInterval = 10;
         m_multiPass = false;
+        m_useCustomProjectionMatrix  = false;
     }
 
     afVector3d m_lookAt;
@@ -594,13 +614,18 @@ struct afCameraAttributes: public afBaseObjectAttributes
     afMouseControlScales m_mouseControlScales;
     bool m_multiPass;
 
+    afImageResolutionAttribs m_windowResolution;
     afImageResolutionAttribs m_publishImageResolution;
     afImageResolutionAttribs m_publishDephtResolution;
     afNoiseModelAttribs m_depthNoiseAttribs;
 
     afHierarchyAttributes m_hierarchyAttribs;
     afKinematicAttributes m_kinematicAttribs;
 
+    vector<vector <double>> m_projectionMatrix; // column major
+    afCameraIntrinsics m_intrinsics;
+    bool m_useCustomProjectionMatrix;
+
     virtual void resolveRelativePathAttribs(afPath a_parentPath){
         if (m_pathsResolved == false){
             afBaseObjectAttributes::resolveRelativePathAttribs(a_parentPath);
diff --git a/ambf_framework/afFramework.cpp b/ambf_framework/afFramework.cpp
@@ -6449,10 +6449,6 @@ bool afCamera::createFromAttribs(afCameraAttributes *a_attribs)
     setName(a_attribs->m_identificationAttribs.m_name);
     setNamespace(a_attribs->m_identificationAttribs.m_namespace);
 
-    m_camPos << a_attribs->m_kinematicAttribs.m_location.getPosition();
-    m_camLookAt << a_attribs->m_lookAt;
-    m_camUp << a_attribs->m_up;
-
     setOrthographic(a_attribs->m_orthographic);
 
     if (a_attribs->m_stereo){
@@ -6481,11 +6477,53 @@ bool afCamera::createFromAttribs(afCameraAttributes *a_attribs)
     }
 
     //////////////////////////////////////////////////////////////////////////////////////
+    m_camPos << a_attribs->m_kinematicAttribs.m_location.getPosition();
+    m_camLookAt << a_attribs->m_lookAt;
+    m_camUp << a_attribs->m_up;
     // position and orient the camera
     setView(m_camPos, m_camLookAt, m_camUp);
+    m_camera->setClippingPlanes(a_attribs->m_nearPlane, a_attribs->m_farPlane);
+
+    if (a_attribs->m_intrinsics.m_defined){
+        const int rows = 4, cols = 4;
+        for (int r = 0 ; r < rows ; r++){
+            for (int c = 0 ; c < cols ; c++){
+                getInternalCamera()->m_projectionMatrix(r, c) = 0.0;
+            }
+        }
+        computeProjectionFromIntrinsics(&a_attribs->m_intrinsics,
+                                        a_attribs->m_windowResolution.m_width,
+                                        a_attribs->m_windowResolution.m_height,
+                                        a_attribs->m_nearPlane,
+                                        a_attribs->m_farPlane);
+
+        cerr << "INFO! USING CAMERA INTRINSICS FOR " << getQualifiedName() << endl;
+        cerr << "\t Focal Length(x, y): (" << a_attribs->m_intrinsics.m_fx << ", " << a_attribs->m_intrinsics.m_fy << ")" << endl;
+        cerr << "\t Principal Offset (x, y): (" << a_attribs->m_intrinsics.m_cx << ", " << a_attribs->m_intrinsics.m_cy << ")" << endl;
+        cerr << "\t Shear: " << a_attribs->m_intrinsics.m_s << endl;
+    }
+    else if (a_attribs->m_useCustomProjectionMatrix){
+        getInternalCamera()->m_useCustomProjectionMatrix = true;
+        const int rows = 4, cols = 4;
+        for (int r = 0 ; r < rows ; r++){
+            for (int c = 0 ; c < cols ; c++){
+                getInternalCamera()->m_projectionMatrix(r, c) = a_attribs->m_projectionMatrix[r][c];
+            }
+        }
+
+        cerr << "INFO! USING CUSTOM PROJECT MATRIX FOR CAMERA: " << getQualifiedName() << endl;
+        for (int r = 0 ; r < rows ; r++){
+            cerr << "\t[";
+            for (int c = 0 ; c < cols ; c++){
+                cerr << a_attribs->m_projectionMatrix[r][c] << " ";
+            }
+            cerr << "\t]" << endl;
+        }
+
+    }
+
     m_initialTransform = getLocalTransform();
     // set the near and far clipping planes of the camera
-    m_camera->setClippingPlanes(a_attribs->m_nearPlane, a_attribs->m_farPlane);
 
     // set stereo mode
     m_camera->setStereoMode(m_stereoMode);
@@ -6593,15 +6631,18 @@ bool afCamera::createWindow()
 
     m_monitor = m_monitors[m_monitorNumber];
 
-    // compute desired size of window
     const GLFWvidmode* mode = glfwGetVideoMode(m_monitor);
-    int w = 0.8 * mode->width;
-    int h = 0.5 * mode->height;
-    int x = 0.5 * (mode->width - w);
-    int y = 0.5 * (mode->height - h);
+    // compute desired size of window
+    afCameraAttributes* camAttribs = (afCameraAttributes*)getAttributes();
+    if (camAttribs->m_windowResolution.m_defined){
+        m_width = camAttribs->m_windowResolution.m_width;
+        m_height = camAttribs->m_windowResolution.m_height;
 
-    m_width = w;
-    m_height = h;
+    }
+    else{
+        m_width = 0.8 * mode->width;
+        m_height = 0.5 * mode->height;
+    }
 
     if (getVisibleFlag() == false){
         cerr << "INFO! CAMERA \"" << m_name << "\" SET TO INVISIBLE. THEREFORE IT IS ONLY VIEWABLE"
@@ -6612,7 +6653,16 @@ bool afCamera::createWindow()
         glfwWindowHint(GLFW_VISIBLE, GLFW_TRUE);
     }
 
-    m_window = glfwCreateWindow(w, h, window_name.c_str(), nullptr, s_mainWindow);
+    int screenOriginX, screenOriginY;
+    glfwGetMonitorPos(m_monitor, &screenOriginX, &screenOriginY);
+
+    int windowOriginX = 0.5 * (mode->width - m_width);
+    int windowOriginY = 0.5 * (mode->height - m_height);
+
+    m_window = glfwCreateWindow(m_width, m_height, window_name.c_str(), nullptr, s_mainWindow);
+    // set position of window
+    glfwSetWindowPos(m_window, screenOriginX + windowOriginX, screenOriginY + windowOriginY);
+
     if (s_windowIdx == 0){
         s_mainWindow = m_window;
     }
@@ -6635,14 +6685,6 @@ bool afCamera::createWindow()
     // get width and height of window
     glfwGetWindowSize(m_window, &m_width, &m_height);
 
-
-    int xpos, ypos;
-    glfwGetMonitorPos(m_monitor, &xpos, &ypos);
-    x += xpos; y += ypos;
-
-    // set position of window
-    glfwSetWindowPos(m_window, x, y);
-
 //    glfwSetWindowMonitor(m_window, m_monitor, m_win_x, m_win_y, m_width, m_height, mode->refreshRate);
 
     // initialize GLEW library
@@ -6705,6 +6747,32 @@ bool afCamera::assignWindowCallbacks(afCameraWindowCallBacks *a_callbacks)
     return true;
 }
 
+bool afCamera::computeProjectionFromIntrinsics(const afCameraIntrinsics *a_attribs, double a_width, double a_height, double a_nearPlane, double a_farPlane){
+    double fx = a_attribs->m_fx;
+    double fy = a_attribs->m_fy;
+    double cx = a_attribs->m_cx;
+    double cy = a_attribs->m_cy;
+    double s = a_attribs->m_s;
+    double W = a_width;
+    double H = a_height;
+    double image_center_x = W/2.;
+    double image_center_y = H/2.;
+    double n = a_nearPlane;
+    double f = a_farPlane;
+
+    getInternalCamera()->m_useCustomProjectionMatrix = true;
+    getInternalCamera()->m_projectionMatrix(0,0) = (2 * fx) / W;
+    getInternalCamera()->m_projectionMatrix(0,1) = (2 * s) / W;
+    getInternalCamera()->m_projectionMatrix(0,2) = (W - 2 * cx) / W;
+    getInternalCamera()->m_projectionMatrix(1,1) = (2 * fy) / H;
+    getInternalCamera()->m_projectionMatrix(1,2) = (-H + 2 * cy) / H;
+    getInternalCamera()->m_projectionMatrix(2,2) = (-f - n) / (f - n);
+    getInternalCamera()->m_projectionMatrix(2,3) = (-2 * f * n) / (f - n);
+    getInternalCamera()->m_projectionMatrix(3,2) = -1.0;
+
+    return true;
+}
+
 ///
 /// \brief afCamera::publishDepthCPUBased
 ///
@@ -6893,6 +6961,11 @@ void afCamera::render(afRenderOptions &options)
         // get width and height of window
         glfwGetFramebufferSize(m_window, &m_width, &m_height);
 
+        afCameraIntrinsics intrinsics = (((afCameraAttributes*)getAttributes())->m_intrinsics);
+        if (intrinsics.m_defined){
+            computeProjectionFromIntrinsics(&intrinsics, m_width, m_height, getInternalCamera()->getNearClippingPlane(), getInternalCamera()->getFarClippingPlane());
+        }
+
         // Update the Labels in a separate sub-routine
         if (options.m_updateLabels && !m_publishDepth && !m_publishImage){
             updateLabels(options);
@@ -6960,6 +7033,11 @@ void afCamera::renderSkyBox(){
 void afCamera::renderFrameBuffer(){
     if (m_publishImage || m_publishDepth){
 
+        afCameraIntrinsics intrinsics = (((afCameraAttributes*)getAttributes())->m_intrinsics);
+        if (intrinsics.m_defined){
+            computeProjectionFromIntrinsics(&intrinsics, m_frameBuffer->getWidth(), m_frameBuffer->getHeight(), getInternalCamera()->getNearClippingPlane(), getInternalCamera()->getFarClippingPlane());
+        }
+
         activatePreProcessingShaders();
 
         m_frameBuffer->renderView();
diff --git a/ambf_framework/afFramework.h b/ambf_framework/afFramework.h
@@ -1927,6 +1927,8 @@ class afCamera: public afBaseObject{
 
     bool assignWindowCallbacks(afCameraWindowCallBacks* a_callbacks);
 
+    bool computeProjectionFromIntrinsics(const afCameraIntrinsics* a_attribs, double a_width, double a_height, double a_nearPlane, double a_farPlane);
+
     // Since we changed the order of ADF loading such that cameras are loaded before
     // bodies etc. we wouldn't be able to find a body defined as a parent in the
     // camera data-block in the ADF file. Thus after loading the bodies, this method
