Skip to content
Snippets Groups Projects
Commit 59ac2662 authored by Sreekanth Arikatla's avatar Sreekanth Arikatla Committed by Ricardo Ortiz
Browse files

Fixes the crash issue that occurs due to race condition.

Sets the appropriate view angle for the camera.

Conflicts:
	Examples/LaparoscopicCamera/main.cpp
	VTKRendering/VTKViewer.cpp
	VirtualTools/LaparoscopicCameraCoupler.cpp
parent 587a1925
No related branches found
No related tags found
No related merge requests found
......@@ -23,7 +23,7 @@
#include <memory>
#include <string>
#include <math.h>
#include <cmath>
// Core SimMedTK includes
#include "Core/SDK.h"
......@@ -51,7 +51,7 @@
#include "VTKRendering/initVTKRendering.h"
#include "VTKRendering/VTKViewer.h"
bool createCameraNavigationScene(std::shared_ptr<SDK> sdk, std::string fileName)
bool createCameraNavigationScene(std::shared_ptr<SDK> sdk, const std::string &fileName)
{
auto meshRenderDetail = std::make_shared<RenderDetail>(SIMMEDTK_RENDER_NORMALS);
......@@ -128,7 +128,7 @@ int main(int ac, char** av)
createCameraNavigationScene(sdk, "./Target.vtk");
//-------------------------------------------------------
// Customize the viewer
// Set up the viewer
//-------------------------------------------------------
auto viewer = sdk->getViewerInstance();
......@@ -157,20 +157,21 @@ int main(int ac, char** av)
scene->addLight(light3);
//-------------------------------------------------------
// Create a Laparoscopic camera controller
// Create a Laparoscopic camera controller and connect
// it to the vtk viewer
//-------------------------------------------------------
auto camClient = std::make_shared<VRPNForceDevice>();
auto server = std::make_shared<VRPNDeviceServer>();
//get some user input and setup device url
std::string input = "navigator@localhost";
std::string input = "Phantom0@localhost";//"Phantom@10.171.2.217"
std::cout << "Enter the VRPN device URL(" << camClient->getDeviceURL() << "): ";
// std::getline(std::cin, input);
std::getline(std::cin, input);
// if (!input.empty())
// {
if (!input.empty())
{
camClient->setDeviceURL(input);
// }
}
auto camController = std::make_shared<LaparoscopicCameraCoupler>(camClient);
camController->setScalingFactor(50.0);
......@@ -178,7 +179,19 @@ int main(int ac, char** av)
camController->setCamera(
(std::static_pointer_cast<VTKViewer>(viewer))->getVtkCamera());
sdk->registerModule(server);
std::shared_ptr<VTKViewer> vtkViewer = std::static_pointer_cast<VTKViewer>(viewer);
vtkCamera* cam = vtkViewer->getVtkCamera();
// set the view angle of the camera. 80 deg for laparoscopic camera
cam->SetViewAngle(80.0);
// set the camera to be controlled by the camera controller
camController->setCamera(cam);
// Connect the camera controller to the vtk viewer to enable camera manipulation
vtkViewer->setCameraControllerData(camController->getCameraData());
sdk->registerModule(camClient);
sdk->registerModule(camController);
......
......@@ -54,6 +54,7 @@ public:
{
}
VTKRenderer(){}
///
/// \brief Callback method executed by the render window interactor.
......@@ -66,6 +67,11 @@ public:
{
if(timerId == *static_cast<int*>(callData) && !this->viewer->isTerminated())
{
if (cameraControllerData != nullptr)
{
updateCamera();
}
this->renderWindow->Render();
}
break;
......@@ -84,6 +90,30 @@ public:
}
}
///
/// \brief Update the camera from external source (optional)
///
void updateCamera()
{
vtkCamera* camera = getRenderWindow()->GetRenderers()->
GetFirstRenderer()->GetActiveCamera();
camera->SetPosition(
cameraControllerData->position[0],
cameraControllerData->position[1],
cameraControllerData->position[2]);
camera->SetViewUp(
cameraControllerData->upVector[0],
cameraControllerData->upVector[1],
cameraControllerData->upVector[2]);
camera->SetFocalPoint(
cameraControllerData->focus[0],
cameraControllerData->focus[1],
cameraControllerData->focus[2]);
}
///
/// \brief Return the render window
///
......@@ -195,6 +225,7 @@ public:
if (delegate)
{
renderer->AddActor(delegate->getActor());
}
}
}
......@@ -288,6 +319,7 @@ public:
VTKViewer *viewer;
vtkNew<vtkRenderWindow> renderWindow;
vtkNew<vtkRenderWindowInteractor> renderWindowInteractor;
std::shared_ptr<cameraConfigurationData> cameraControllerData;
};
VTKViewer::VTKViewer() : renderer(Core::make_unique<VTKRenderer> (this))
......@@ -324,6 +356,11 @@ vtkCamera* VTKViewer::getVtkCamera()
GetFirstRenderer()->GetActiveCamera();
}
void VTKViewer::setCameraControllerData(std::shared_ptr<cameraConfigurationData> camData)
{
renderer->cameraControllerData = camData;
};
void VTKViewer::initRenderingContext()
{
this->renderer->addRenderer();
......
......@@ -25,6 +25,7 @@
#define VTKVIEWER_H
#include "Core/ViewerBase.h"
#include "VirtualTools/LaparoscopicCameraCoupler.h"
class vtkCamera;
......@@ -84,6 +85,11 @@ public:
///
vtkCamera* getVtkCamera();
///
/// \brief Sets the position and orientation data from the camera controller
///
void setCameraControllerData(std::shared_ptr<cameraConfigurationData> camData);
protected:
///
/// \brief Renders the render operation to screen
......@@ -144,4 +150,4 @@ private:
std::unique_ptr<VTKRenderer> renderer;
};
#endif // VTKVIEWER_H
\ No newline at end of file
#endif // VTKVIEWER_H
......@@ -38,6 +38,8 @@ LaparoscopicCameraCoupler::LaparoscopicCameraCoupler(
this->poolDelay = std::chrono::milliseconds(100);
this->initialTransform.setIdentity();
this->name = "LaparoscopicCameraCoupler";
cameraPosOrientData = std::make_shared<cameraConfigurationData>();
}
LaparoscopicCameraCoupler::LaparoscopicCameraCoupler(
......@@ -47,6 +49,8 @@ LaparoscopicCameraCoupler::LaparoscopicCameraCoupler(
this->poolDelay = std::chrono::milliseconds(100);
this->initialTransform.Identity();
this->name = "LaparoscopicCameraCoupler";
cameraPosOrientData = std::make_shared<cameraConfigurationData>();
}
LaparoscopicCameraCoupler::~LaparoscopicCameraCoupler()
......@@ -131,7 +135,7 @@ void LaparoscopicCameraCoupler::exec()
while(!this->terminateExecution)
{
if(!this->updateTracker())
if(!this->updateCamera())
{
this->terminate();
}
......@@ -141,7 +145,7 @@ void LaparoscopicCameraCoupler::exec()
this->inputDevice->closeDevice();
}
bool LaparoscopicCameraCoupler::updateTracker()
bool LaparoscopicCameraCoupler::updateCamera()
{
if (!this->inputDevice)
{
......@@ -152,23 +156,10 @@ bool LaparoscopicCameraCoupler::updateTracker()
core::Quaterniond newRot = inputDevice->getOrientation();
core::Vec3d newPos = inputDevice->getPosition() * this->scalingFactor;
core::Vec3d transformedFocus = newRot._transformVector(core::Vec3d(0, 0, -200));
core::Vec3d transformedUpVector = newRot._transformVector(core::Vec3d(0, 1, 0));
// std::cout << "newPos" << newPos << std::endl;
// std::cout << "transformedUpVector" << transformedUpVector << std::endl;
// std::cout << "transformedFocus" << transformedFocus << std::endl;
this->camera->SetPosition(newPos[0], newPos[1], newPos[2]);
this->camera->SetViewUp(
transformedUpVector[0],
transformedUpVector[1],
transformedUpVector[2]);
this->camera->SetFocalPoint(
transformedFocus[0],
transformedFocus[1],
transformedFocus[2]);
// update the camera position, focus and up vector data
cameraPosOrientData->position = inputDevice->getPosition() * this->scalingFactor;
cameraPosOrientData->focus = newRot._transformVector(core::Vec3d(0, 0, -200));
cameraPosOrientData->upVector = newRot._transformVector(core::Vec3d(0, 1, 0));
return true;
}
......@@ -193,3 +184,8 @@ const core::Vec3d & LaparoscopicCameraCoupler::getOffsetPosition() const
{
return this->offsetPosition;
}
std::shared_ptr<cameraConfigurationData> LaparoscopicCameraCoupler::getCameraData()
{
return cameraPosOrientData;
};
\ No newline at end of file
......@@ -38,6 +38,16 @@
class DeviceInterface;
struct cameraConfigurationData
{
core::Vec3d position;
core::Vec3d focus;
core::Vec3d upVector;
cameraConfigurationData() : position(0, 0, 0), focus(0, 0, -1), upVector(0, 1, 0){};
~cameraConfigurationData(){}
};
class LaparoscopicCameraCoupler : public Module
{
public:
......@@ -153,6 +163,16 @@ public:
///
void setOffsetPosition(const core::Vec3d &offsetPosition);
///
/// \brief Update position and orientation of the camera based on device data
///
bool updateCamera();
///
/// \brief Returns the pointer to the camera data that is updated by this controller
///
std::shared_ptr<cameraConfigurationData> getCameraData();
///
/// \brief Module overrides
///
......@@ -165,11 +185,6 @@ public:
///
void exec() override;
///
/// \brief Update position an orientation of the model from device data.
///
bool updateTracker();
private:
TransformType initialTransform; //!< Transform applied to the position obtained from device
core::Quaterniond orientation; //!< Previous rotation quaternion from device
......@@ -186,6 +201,8 @@ private:
std::chrono::milliseconds poolDelay; //!< Pooling delay
vtkCamera* camera; //!< Pointer to rendering camera
std::shared_ptr<DeviceInterface> inputDevice; //!< Pointer to input device
std::shared_ptr<cameraConfigurationData> cameraPosOrientData;//!< camera config data
};
#endif // iSMTK_LAPAROSCOPIC_CAMERA_COUPLER_H
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment