fix: Snapshot camera timestamps on the physics thread for ground truth accuracy

Library/include/graphics/OpenGLView.h

@@ -130,6 +130,12 @@ namespace sf
         //! A method saying if the view works in continuous update mode.
         bool isContinuous();
 
+        //! A method to set the simulation time [s] when the pose is committed.
+        void SetPendingCaptureTime(double t);
+
+        //! A method returning the simulation time [s] of the last captured frame.
+        double getCaptureTime() const;
+
         //! A method extracting frustium planes from the view-projection matrix.
         /*!
          \param frustum a pointer to the 6 frustum planes
@@ -145,6 +151,9 @@ namespace sf
         GLuint renderFBO;
         bool enabled;
         bool continuous;
+        double tempCaptureTime_;    //physics thread writes, GL thread reads
+        double pendingCaptureTime_; //GL thread only
+        double captureTime_;        //GL thread only
         ViewUBO viewUBOData;
     };
 }

Library/include/sensors/vision/Camera.h

@@ -102,6 +102,9 @@ namespace sf
          \return pointer to the image data buffer
          */
         virtual void* getImageDataPointer(unsigned int index = 0) = 0;
+
+        //! A method returning the simulation time [s] of the last captured frame.
+        Scalar getLastCaptureTime() const;
 
     protected:
         Scalar fovH;

Library/src/graphics/OpenGLDepthCamera.cpp

@@ -138,6 +138,7 @@ void OpenGLDepthCamera::SetupCamera(glm::vec3 _eye, glm::vec3 _dir, glm::vec3 _u
 
 void OpenGLDepthCamera::UpdateTransform()
 {
+    pendingCaptureTime_ = tempCaptureTime_;
     eye = tempEye;
     dir = tempDir;
     up = tempUp;
@@ -349,6 +350,7 @@ void OpenGLDepthCamera::DrawLDR(GLuint destinationFBO, bool updated)
         glGetTexImage(GL_TEXTURE_2D, 0, GL_RED, GL_FLOAT, NULL);
         glBindBuffer(GL_PIXEL_PACK_BUFFER, 0);
         OpenGLState::UnbindTexture(TEX_POSTPROCESS1);
+        captureTime_ = pendingCaptureTime_;
         newData = true;
     }
 }

Library/src/graphics/OpenGLEventBasedCamera.cpp

@@ -178,6 +178,7 @@ void OpenGLEventBasedCamera::SetupCamera(glm::vec3 _eye, glm::vec3 _dir, glm::ve
 
 void OpenGLEventBasedCamera::UpdateTransform()
 {
+    pendingCaptureTime_ = tempCaptureTime_;
     eye = tempEye;
     dir = tempDir;
     up = tempUp;
@@ -329,6 +330,7 @@ void OpenGLEventBasedCamera::DrawLDR(GLuint destinationFBO, bool updated)
         glGetTexImage(GL_TEXTURE_2D, 0, GL_RG_INTEGER, GL_INT, NULL);
         glBindBuffer(GL_PIXEL_PACK_BUFFER, 0);
         OpenGLState::UnbindTexture(TEX_POSTPROCESS1);
+        captureTime_ = pendingCaptureTime_;
         newData = true;
     }
 }

Library/src/graphics/OpenGLFLS.cpp

@@ -504,6 +504,7 @@ void OpenGLFLS::DrawLDR(GLuint destinationFBO, bool updated)
         glGetTexImage(GL_TEXTURE_2D, 0, GL_RGB, GL_UNSIGNED_BYTE, NULL);
         glBindBuffer(GL_PIXEL_PACK_BUFFER, 0);
         OpenGLState::UnbindTexture(TEX_POSTPROCESS1);
+        captureTime_ = pendingCaptureTime_;
         newData_ = true;
     }
 }

Library/src/graphics/OpenGLMSIS.cpp

@@ -491,6 +491,7 @@ void OpenGLMSIS::DrawLDR(GLuint destinationFBO, bool updated)
         glGetTexImage(GL_TEXTURE_2D, 0, GL_RGB, GL_UNSIGNED_BYTE, NULL);
         glBindBuffer(GL_PIXEL_PACK_BUFFER, 0);
         OpenGLState::UnbindTexture(TEX_POSTPROCESS1);
+        captureTime_ = pendingCaptureTime_;
         newData_ = true;
     }
 }

Library/src/graphics/OpenGLOpticalFlowCamera.cpp

@@ -137,6 +137,7 @@ void OpenGLOpticalFlowCamera::SetupCamera(glm::vec3 _eye, glm::vec3 _dir, glm::v
 
 void OpenGLOpticalFlowCamera::UpdateTransform()
 {
+    pendingCaptureTime_ = tempCaptureTime_;
     eye = tempEye;
     dir = tempDir;
     up = tempUp;
@@ -367,6 +368,7 @@ void OpenGLOpticalFlowCamera::DrawLDR(GLuint destinationFBO, bool updated)
         glGetTexImage(GL_TEXTURE_2D, 0, GL_RGB, GL_UNSIGNED_BYTE, NULL);
         glBindBuffer(GL_PIXEL_PACK_BUFFER, 0);
         OpenGLState::UnbindTexture(TEX_POSTPROCESS1);
+        captureTime_ = pendingCaptureTime_;
         newData = true;
     }
 }

Library/src/graphics/OpenGLRealCamera.cpp

@@ -132,6 +132,7 @@ void OpenGLRealCamera::SetupCamera(glm::vec3 _eye, glm::vec3 _dir, glm::vec3 _up
 
 void OpenGLRealCamera::UpdateTransform()
 {
+    pendingCaptureTime_ = tempCaptureTime_;
     eye = tempEye;
     dir = tempDir;
     up = tempUp;
@@ -187,6 +188,7 @@ void OpenGLRealCamera::DrawLDR(GLuint destinationFBO, bool updated)
         glGetTexImage(GL_TEXTURE_2D, 0, GL_RGB, GL_UNSIGNED_BYTE, NULL);
         glBindBuffer(GL_PIXEL_PACK_BUFFER, 0);
         OpenGLState::UnbindTexture(TEX_POSTPROCESS1);
+        captureTime_ = pendingCaptureTime_;
         newData = true;
     }
 

Library/src/graphics/OpenGLSSS.cpp

@@ -493,8 +493,9 @@ void OpenGLSSS::DrawLDR(GLuint destinationFBO, bool updated)
         glGetTexImage(GL_TEXTURE_2D, 0, GL_RGB, GL_UNSIGNED_BYTE, NULL);
         glBindBuffer(GL_PIXEL_PACK_BUFFER, 0);
         OpenGLState::UnbindTexture(TEX_POSTPROCESS1);
+        captureTime_ = pendingCaptureTime_;
         newData_ = true;
     }
 }
-   
+
 }

Library/src/graphics/OpenGLSegmentationCamera.cpp

@@ -136,6 +136,7 @@ void OpenGLSegmentationCamera::SetupCamera(glm::vec3 _eye, glm::vec3 _dir, glm::
 
 void OpenGLSegmentationCamera::UpdateTransform()
 {
+    pendingCaptureTime_ = tempCaptureTime_;
     eye = tempEye;
     dir = tempDir;
     up = tempUp;
@@ -341,6 +342,7 @@ void OpenGLSegmentationCamera::DrawLDR(GLuint destinationFBO, bool updated)
         glGetTexImage(GL_TEXTURE_2D, 0, GL_RGB, GL_UNSIGNED_BYTE, NULL);
         glBindBuffer(GL_PIXEL_PACK_BUFFER, 0);
         OpenGLState::UnbindTexture(TEX_POSTPROCESS1);
+        captureTime_ = pendingCaptureTime_;
         newData = true;
     }
 }

Library/src/graphics/OpenGLSonar.cpp

@@ -79,6 +79,7 @@ void OpenGLSonar::SetupSonar(glm::vec3 _eye, glm::vec3 _dir, glm::vec3 _up)
 
 void OpenGLSonar::UpdateTransform()
 {
+    pendingCaptureTime_ = tempCaptureTime_;
     eye_ = tempEye_;
     dir_ = tempDir_;
     up_ = tempUp_;

Library/src/graphics/OpenGLThermalCamera.cpp

@@ -132,6 +132,7 @@ void OpenGLThermalCamera::SetupCamera(glm::vec3 _eye, glm::vec3 _dir, glm::vec3
 
 void OpenGLThermalCamera::UpdateTransform()
 {
+    pendingCaptureTime_ = tempCaptureTime_;
     eye = tempEye;
     dir = tempDir;
     up = tempUp;
@@ -328,6 +329,7 @@ void OpenGLThermalCamera::DrawLDR(GLuint destinationFBO, bool updated)
         glGetTexImage(GL_TEXTURE_2D, 0, GL_RGB, GL_UNSIGNED_BYTE, NULL);
         glBindBuffer(GL_PIXEL_PACK_BUFFER, 0);
         OpenGLState::UnbindTexture(TEX_POSTPROCESS1);
+        captureTime_ = pendingCaptureTime_;
         newData = true;
     }
 }

Library/src/graphics/OpenGLView.cpp

@@ -38,6 +38,7 @@ OpenGLView::OpenGLView(GLint x, GLint y, GLint width, GLint height)
     viewportHeight = height + height % 2;
     enabled = true;
 	continuous = false;
+    tempCaptureTime_ = 0.0; pendingCaptureTime_ = 0.0; captureTime_ = 0.0;
     viewUBOData.VP = glm::mat4(1.f);
     viewUBOData.eye = glm::vec3(0.f);
     ExtractFrustumFromVP(viewUBOData.frustum, viewUBOData.VP);
@@ -72,6 +73,16 @@ bool OpenGLView::isContinuous()
 	return continuous;
 }
 
+void OpenGLView::SetPendingCaptureTime(double t)
+{
+    tempCaptureTime_ = t;
+}
+
+double OpenGLView::getCaptureTime() const
+{
+    return captureTime_;
+}
+
 void OpenGLView::SetViewport()
 {
     OpenGLState::Viewport(0, 0, viewportWidth, viewportHeight);

Library/src/sensors/vision/Camera.cpp

@@ -26,6 +26,9 @@
 #include "sensors/vision/Camera.h"
 
 #include "entities/SolidEntity.h"
+#include "graphics/OpenGLView.h"
+#include "core/GraphicalSimulationApp.h"
+#include "core/SimulationManager.h"
 
 namespace sf
 {
@@ -69,13 +72,22 @@ bool Camera::getDisplayOnScreen(unsigned int& x, unsigned int& y, float& scale)
     return screen;
 }
 
+Scalar Camera::getLastCaptureTime() const
+{
+    OpenGLView* v = getOpenGLView();
+    return v ? Scalar(v->getCaptureTime()) : Scalar(0);
+}
+
 void Camera::UpdateTransform()
 {
     Transform cameraTransform = getSensorFrame();
     Vector3 eyePosition = cameraTransform.getOrigin(); //O
     Vector3 direction = cameraTransform.getBasis().getColumn(2); //Z
     Vector3 cameraUp = -cameraTransform.getBasis().getColumn(1); //-Y
     SetupCamera(eyePosition, direction, cameraUp);
+    OpenGLView* v = getOpenGLView();
+    if(v) v->SetPendingCaptureTime(
+        (double)SimulationApp::getApp()->getSimulationManager()->getSimulationTime(true));
 }
 
 std::vector<Renderable> Camera::Render()

Library/src/sensors/vision/Multibeam2.cpp

@@ -26,6 +26,7 @@
 #include "sensors/vision/Multibeam2.h"
 
 #include "core/GraphicalSimulationApp.h"
+#include "core/SimulationManager.h"
 #include "graphics/OpenGLPipeline.h"
 #include "graphics/OpenGLContent.h"
 #include "graphics/OpenGLDepthCamera.h"
@@ -204,6 +205,7 @@ void Multibeam2::SetupCamera(size_t index, const Vector3& eye, const Vector3& di
     glm::vec3 dir_ = glm::vec3((GLfloat)dir.x(), (GLfloat)dir.y(), (GLfloat)dir.z());
     glm::vec3 up_ = glm::vec3((GLfloat)up.x(), (GLfloat)up.y(), (GLfloat)up.z());
     cameras[index].cam->SetupCamera(eye_, dir_, up_);
+    cameras[index].cam->SetPendingCaptureTime((double)SimulationApp::getApp()->getSimulationManager()->getSimulationTime(true));
 }
 
 void Multibeam2::InstallNewDataHandler(std::function<void(Multibeam2*)> callback)