Commit 2b962d4d authored by Sam Horvath's avatar Sam Horvath

ENH: Scope angulation now functioning correctly

parent 56e2f58e
Pipeline #58921 failed with stage
...@@ -127,7 +127,7 @@ void setup() { ...@@ -127,7 +127,7 @@ void setup() {
// (115200 chosen because it is required for Teapot Demo output, but it's // (115200 chosen because it is required for Teapot Demo output, but it's
// really up to you depending on your project) // really up to you depending on your project)
Serial.begin(57600); Serial.begin(57600);
Serial.println("setup"); //Serial.println("setup");
while (!Serial); // wait for Leonardo enumeration, others continue immediately while (!Serial); // wait for Leonardo enumeration, others continue immediately
// NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
......
...@@ -39,18 +39,20 @@ FLSCameraController::getCameraHeadAngleOffset() const ...@@ -39,18 +39,20 @@ FLSCameraController::getCameraHeadAngleOffset() const
return m_cameraHeadAngleOffset; return m_cameraHeadAngleOffset;
} }
void void
FLSCameraController::setCameraAngulationOffset(const double angle) FLSCameraController::setCameraAngulation(const double angle)
{ {
m_cameraAngulationOffset = angle; m_cameraAngulation = angle;
m_cameraAngulationRotOffset = Quatd(Eigen::AngleAxisd(angle*PI / 180., Vec3d(0., 1., 0.))); m_cameraAngulationRotOffset = Quatd(Eigen::AngleAxisd(angle*PI / 180., Vec3d(0., 1., 0.)));
} }
double double
FLSCameraController::getCameraAngulationOffset() const FLSCameraController::getCameraAngulation() const
{ {
return m_cameraAngulationOffset; return m_cameraAngulation;
} }
void void
FLSCameraController::setArduinoDevice(std::shared_ptr<VRPNArduinoDeviceClient> aClient) FLSCameraController::setArduinoDevice(std::shared_ptr<VRPNArduinoDeviceClient> aClient)
...@@ -58,30 +60,26 @@ FLSCameraController::setArduinoDevice(std::shared_ptr<VRPNArduinoDeviceClient> a ...@@ -58,30 +60,26 @@ FLSCameraController::setArduinoDevice(std::shared_ptr<VRPNArduinoDeviceClient> a
arduinoActive = true; arduinoActive = true;
arduinoClient = aClient; arduinoClient = aClient;
m_rollOffset = arduinoClient->getRoll();
}
void
FLSCameraController::setCameraAngulationTranslationOffset(const double t)
{
m_angulationTranslationOffset = t;
}
double
FLSCameraController::getCameraAngulationTranslationOffset() const
{
return m_angulationTranslationOffset;
} }
void void
FLSCameraController::runModule() FLSCameraController::runModule()
{ {
if (arduinoActive)
//Get head angle from Arduino, performing calibration if this is the first valid report
if (arduinoActive & calibrated)
{ {
this->setCameraHeadAngleOffset(arduinoClient->getRoll() - m_rollOffset); this->setCameraHeadAngleOffset(arduinoClient->getRoll() - m_rollOffset);
} }
else if (arduinoActive)
{
if (arduinoClient->getRoll() != 0){
std::cout << "FLS Camera Controller: Calibration complete; Safe to move camera" << std::endl;
m_rollOffset = arduinoClient->getRoll();
calibrated = true;
}
}
auto roff = Quatd(Eigen::AngleAxisd(m_cameraHeadAngleOffset*PI / 180., Vec3d(0., 0., 1.))); auto roff = Quatd(Eigen::AngleAxisd(m_cameraHeadAngleOffset*PI / 180., Vec3d(0., 0., 1.)));
roff *= m_cameraAngulationRotOffset;
this->setCameraRotationOffset(roff); this->setCameraRotationOffset(roff);
if (!m_trackingDataUptoDate) if (!m_trackingDataUptoDate)
...@@ -96,18 +94,22 @@ FLSCameraController::runModule() ...@@ -96,18 +94,22 @@ FLSCameraController::runModule()
Vec3d p = getPosition(); Vec3d p = getPosition();
Quatd r = getRotation(); Quatd r = getRotation();
m_cameraTranslationOffset = r*Vec3d(m_angulationTranslationOffset*cos(m_cameraAngulationOffset*PI / 180.),
0., //Adjust the upward angulation position to the center of the ROM
m_angulationTranslationOffset*sin(m_cameraAngulationOffset*PI / 180.)); auto angulationDirectionOffset = Quatd(Eigen::AngleAxisd(-90*PI / 180., Vec3d(0., 0., 1.)));
r *= angulationDirectionOffset;
// Apply Offsets over the device pose // Apply Offsets over the device pose
p += m_cameraTranslationOffset; // Offset the device position p += m_cameraTranslationOffset; // Offset the device position
r *= m_cameraRotationalOffset; // Apply camera head rotation offset
//Apply offset from angulation
r *= m_cameraAngulationRotOffset;
// Set camera pose // Set camera pose
m_camera.setPosition(p); m_camera.setPosition(p); //position of camera
m_camera.setFocalPoint(r*FORWARD_VECTOR + p); m_camera.setFocalPoint(r*FORWARD_VECTOR + p); //direction camera is looking
m_camera.setViewUp(r*UP_VECTOR); m_camera.setViewUp(m_cameraRotationalOffset*UP_VECTOR); //Orientation of camera
m_trackingDataUptoDate = false; m_trackingDataUptoDate = false;
} }
......
...@@ -55,16 +55,10 @@ public: ...@@ -55,16 +55,10 @@ public:
const double getCameraHeadAngleOffset() const; const double getCameraHeadAngleOffset() const;
/// ///
/// \brief Get/Set the rotation offset due to angulation /// \brief Get/Set the angulation
/// ///
void setCameraAngulationOffset(const double angle); void setCameraAngulation(const double angle);
double getCameraAngulationOffset() const; double getCameraAngulation() const ;
///
/// \brief Get/Set the translation offset due to angulation
///
void setCameraAngulationTranslationOffset(const double t);
double getCameraAngulationTranslationOffset() const;
void setArduinoDevice(std::shared_ptr<VRPNArduinoDeviceClient> aClient); void setArduinoDevice(std::shared_ptr<VRPNArduinoDeviceClient> aClient);
...@@ -75,11 +69,12 @@ protected: ...@@ -75,11 +69,12 @@ protected:
/// ///
void runModule() override; void runModule() override;
double m_cameraHeadAngleOffset = 0; ///< camera head angle offset (in deg) double m_cameraHeadAngleOffset = 0; ///< camera head angle offset (in deg)
Quatd m_cameraAngulationRotOffset = Quatd::Identity();///< Rotation offset for the camera via telescope angulation double m_cameraAngulation = 0; ///the actual angulation!!! 0, 30, 45 deg
double m_angulationTranslationOffset = 0.1; Quatd m_cameraAngulationRotOffset = Quatd::Identity(); ///< Rotation offset for the camera via telescope angulation
std::shared_ptr<VRPNArduinoDeviceClient> arduinoClient; std::shared_ptr<VRPNArduinoDeviceClient> arduinoClient;
bool arduinoActive = false; bool arduinoActive = false;
bool calibrated = false;
double m_rollOffset = 0; double m_rollOffset = 0;
}; };
} // imstk } // imstk
......
...@@ -31,7 +31,7 @@ endif() ...@@ -31,7 +31,7 @@ endif()
include(imstkAddExternalProject) include(imstkAddExternalProject)
imstk_add_external_project( VRPN imstk_add_external_project( VRPN
GIT_REPOSITORY https://github.com/sjh26/vrpn GIT_REPOSITORY https://github.com/sjh26/vrpn
GIT_TAG e8cfc645c33f4072723e503d7aa85f3a37646de6 GIT_TAG 6721b5ea8972cf6bcbaccdd2d32479dd77b74b53
INSTALL_COMMAND ${SKIP_STEP_COMMAND} INSTALL_COMMAND ${SKIP_STEP_COMMAND}
CMAKE_ARGS CMAKE_ARGS
-DBUILD_TESTING:BOOL=OFF -DBUILD_TESTING:BOOL=OFF
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment