ENH: Add offsets to the camera controller

Base/SceneElements/Controllers/imstkCameraController.cpp

@@ -30,20 +30,6 @@ namespace imstk
 void
 CameraController::initModule()
 {
-    auto pos = m_camera.getPosition();
-    auto viewUp = m_camera.getViewUp();
-    auto focus = m_camera.getFocalPoint();
-
-    m_translationOffset = pos;
-
-    auto viewNormal = (pos - focus).normalized();
-    auto viewSide = viewUp.cross(viewNormal).normalized();
-    viewUp = viewNormal.cross(viewSide);
-    Mat3d rot;
-    rot.col(0) = viewSide;
-    rot.col(1) = viewUp;
-    rot.col(2) = viewNormal;
-    m_rotationOffset = Quatd(rot);
 }
 
 void
@@ -58,8 +44,12 @@ CameraController::runModule()
         }
     }
 
-    const Vec3d p = getPosition();
-    const Quatd r = getRotation();
+    Vec3d p = getPosition();
+    Quatd r = getRotation();
+
+    // Apply Offsets over the device pose
+    p = p + m_cameraTranslationOffset;      // Offset the device position
+    r *= m_cameraRotationalOffset;          // Apply camera head rotation offset
 
     // Set camera info
     m_camera.setPosition(p);
@@ -70,7 +60,45 @@ CameraController::runModule()
 }
 
 void
-CameraController::cleanUpModule()
+CameraController::setOffsetUsingCurrentCameraPose()
+{
+    auto pos = m_camera.getPosition();
+    auto viewUp = m_camera.getViewUp();
+    auto focus = m_camera.getFocalPoint();
+
+    m_translationOffset = pos;
+
+    auto viewNormal = (pos - focus).normalized();
+    auto viewSide = viewUp.cross(viewNormal).normalized();
+    viewUp = viewNormal.cross(viewSide);
+    Mat3d rot;
+    rot.col(0) = viewSide;
+    rot.col(1) = viewUp;
+    rot.col(2) = viewNormal;
+    m_rotationOffset = Quatd(rot);
+}
+
+void
+CameraController::setCameraRotationOffset(const Quatd& r)
+{
+    m_cameraRotationalOffset = r;
+}
+
+void
+CameraController::setCameraTranslationOffset(const Vec3d& t)
+{
+    m_cameraTranslationOffset = t;
+}
+
+const Vec3d&
+CameraController::getCameraTranslationOffset() const
+{
+    return m_cameraTranslationOffset;
+}
+
+const Quatd&
+CameraController::getCameraRotationOffset() const
 {
+    return m_cameraRotationalOffset;
 }
 } // imstk

Base/SceneElements/Controllers/imstkCameraController.h

@@ -53,23 +53,44 @@ public:
     ///
     ~CameraController() = default;
 
+    ///
+    /// \brief Set the offsets based on the current camera pose
+    ///
+    void setOffsetUsingCurrentCameraPose();
+
+    ///
+    /// \brief Get/Set translation offset of the camera
+    ///
+    const Vec3d& getCameraTranslationOffset() const;
+    void setCameraTranslationOffset(const Vec3d& t);
+
+    ///
+    /// \brief Get/Set rotation offset of the camera
+    ///
+    const Quatd& getCameraRotationOffset() const;
+    void setCameraRotationOffset(const Quatd& r);
+
+
 protected:
     ///
     /// \brief
     ///
-    void initModule() override;
+    virtual void initModule() override;
 
     ///
     /// \brief
     ///
-    void runModule() override;
+    virtual void runModule() override;
 
     ///
     /// \brief
     ///
-    void cleanUpModule() override;
+    void cleanUpModule() override {};
 
     Camera& m_camera; ///< Camera controlled by the external device
+
+    Vec3d m_cameraTranslationOffset = WORLD_ORIGIN;     ///< Translation offset for the camera over tracking data
+    Quatd m_cameraRotationalOffset = Quatd::Identity(); ///< camera head angle offset (in deg)
 };
 } // imstk
 

Base/SceneElements/Controllers/imstkSceneObjectController.h

@@ -37,7 +37,7 @@ namespace imstk
 ///
 class SceneObjectController : public SceneObjectControllerBase
 {
-    using ControllerCallbackFunction = std::function<void(SceneObjectController* hdapiClient)>;
+using ControllerCallbackFunction = std::function<void(SceneObjectController* hdapiClient)>;
 public:
     ///
     /// \brief Constructor