Adds position to the camera based on the device end effector's position.

The orientation is yet to be set.

Examples/CMakeLists.txt

@@ -38,6 +38,7 @@ endif(SimMedTK_USE_OCULUS)
 # add_subdirectory(renderCube)
 # add_subdirectory(renderCubeToTexture)
 add_subdirectory(vegaFem)
+add_subdirectory(LaparoscopicCamera)
 # add_subdirectory(MultipleObjects)
 # add_subdirectory(CollisionDetectionSpatialHashing)
 # add_subdirectory(CollisionDetectionBVH)

Examples/LaparoscopicCamera/CMakeLists.txt

+set(APP LaparoscopicCamera)
+
+add_executable(${APP} main.cpp)
+target_link_libraries(${APP}
+  Core
+  Geometry
+  VTKRendering
+  # Rendering
+  # RenderDelegates
+  Simulators
+  Mesh
+  Collision
+  ContactHandling
+  Event
+  # ExamplesCommon
+  IO
+  Devices
+  VirtualTools
+)
+
+set_target_properties(${APP}
+    PROPERTIES
+    RUNTIME_OUTPUT_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+)
+
+set(${APP}_DATA "${CMAKE_CURRENT_BINARY_DIR}")
+if(NOT EXISTS ${${APP}_DATA})
+    file(MAKE_DIRECTORY ${${APP}_DATA})
+endif()
+
+set(FILE_URL http://midas3.kitware.com/midas/download/item/318708/box.tar)
+set(FILE_SHA1 5c6e34c0de6ed2b16263aff2f33098f98a2bb875)
+set(LOCAL_FILE ${EXAMPLES_DATA}/box.tar)
+
+midas_download(${FILE_URL} ${FILE_SHA1} ${LOCAL_FILE})
+
+# Extract data
+set(DATA
+    ${${APP}_DATA}/box.bou
+    ${${APP}_DATA}/box.config
+    ${${APP}_DATA}/box.interp
+    ${${APP}_DATA}/box.mass
+    ${${APP}_DATA}/box.vtk
+    ${${APP}_DATA}/box.veg
+)
+
+add_custom_command(
+    TARGET ${APP}
+    POST_BUILD
+#     BYPRODUCTS ${DATA}
+    COMMAND ${CMAKE_COMMAND} -E tar xf ${LOCAL_FILE}
+    WORKING_DIRECTORY ${${APP}_DATA}
+    COMMENT "Unpacking ${LOCAL_FILE}"
+)
\ No newline at end of file

Examples/LaparoscopicCamera/main.cpp

+// This file is part of the SimMedTK project.
+// Copyright (c) Center for Modeling, Simulation, and Imaging in Medicine,
+//                        Rensselaer Polytechnic Institute
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+//---------------------------------------------------------------------------
+//
+// Authors:
+//
+// Contact:
+//---------------------------------------------------------------------------
+
+#include <memory>
+
+// Core SimMedTK includes
+#include "Core/SDK.h"
+
+// Include required types scene objects
+#include "Simulators/VegaFemSceneObject.h"
+#include "Core/StaticSceneObject.h"
+#include "Mesh/VegaVolumetricMesh.h"
+#include "Devices/VRPNForceDevice.h"
+
+#include "VirtualTools/ToolCoupler.h"
+#include "VirtualTools/LaparoscopicCameraCoupler.h"
+
+// Include required simulators
+#include "Simulators/VegaFemSimulator.h"
+#include "Simulators/DefaultSimulator.h"
+
+#include "Core/CollisionPair.h"
+#include "Collision/PlaneCollisionModel.h"
+#include "Collision/MeshCollisionModel.h"
+#include "Collision/PlaneToMeshCollision.h"
+
+#include "ContactHandling/PenaltyContactFemToStatic.h"
+
+#include "IO/initIO.h"
+
+#include "VTKRendering/initVTKRendering.h"
+#include "VTKRendering/VTKViewer.h"
+
+int main(int ac, char** av)
+{
+    initVTKRendering();
+    initIODelegates();
+    const bool useVTKRenderer = true;
+
+    auto sdk = SDK::createStandardSDK();
+
+    //-------------------------------------------------------
+    // Create scene actor 1:  plane
+    //-------------------------------------------------------
+    auto staticSimulator = std::make_shared<DefaultSimulator>(sdk->getErrorLog());
+
+    // create a static plane scene object of given normal and position
+    auto staticObject = std::make_shared<StaticSceneObject>();
+
+    auto plane = std::make_shared<PlaneCollisionModel>(
+      core::Vec3d(0.0, -3.0, 0.0),
+      core::Vec3d(0.0, 1.0, 0.0));
+    plane->getPlaneModel()->setWidth(5);
+    if (ac > 2)
+      plane->getPlaneModel()->setWidth(atof(av[2]));
+    staticObject->setModel(plane);
+
+    sdk->addSceneActor(staticObject, staticSimulator);
+
+    //-------------------------------------------------------
+    // Create scene actor 2:  dragon
+    //-------------------------------------------------------
+    auto staticSimulator2 = std::make_shared<DefaultSimulator>(sdk->getErrorLog());
+
+    // create a static plane scene object of given normal and position
+    auto staticDragon = std::make_shared<StaticSceneObject>();
+
+    auto dragenModel = std::make_shared<MeshCollisionModel>();
+    dragenModel->loadTriangleMesh("./asianDragon.vtk");
+    dragenModel->getMesh()->scale(Eigen::UniformScaling<double>(0.2));
+    staticDragon->setModel(dragenModel);
+
+    auto dragonRenderDetail = std::make_shared<RenderDetail>(SIMMEDTK_RENDER_FACES);
+
+    dragonRenderDetail->setAmbientColor(Color(0.2, 0.2, 0.2, 1.0));
+    dragonRenderDetail->setDiffuseColor(Color(0.0, 0.8, 0.0, 1.0));
+    dragonRenderDetail->setSpecularColor(Color(0.4, 0.4, 0.4, 1.0));
+    dragonRenderDetail->setShininess(20.0);
+
+    dragenModel->setRenderDetail(dragonRenderDetail);
+
+    sdk->addSceneActor(staticDragon, staticSimulator2);
+
+    //-------------------------------------------------------
+    // Register both object simulators
+    //-------------------------------------------------------
+    auto sdkSimulator = sdk->getSimulator();
+
+    //-------------------------------------------------------
+    // Customize the viewer
+    //-------------------------------------------------------
+
+    auto viewer = sdk->getViewerInstance();
+
+    viewer->setViewerRenderDetail(SIMMEDTK_VIEWERRENDER_GLOBAL_AXIS);
+
+    // Get Scene
+    auto scene = sdk->getScene(0);
+    viewer->registerScene(scene, SMRENDERTARGET_SCREEN, "Collision pipeline demo");
+
+    // Setup Scene lighting
+    auto light1 = Light::getDefaultLighting();
+    light1->lightPos.setPosition(core::Vec3d(-25.0, 10.0, 10.0));
+    scene->addLight(light1);
+
+    auto light2 = Light::getDefaultLighting();
+    light2->lightPos.setPosition(core::Vec3d(25.0, 10.0, 10.0));
+    scene->addLight(light2);
+
+    //-------------------------------------------------------
+    // Create a Laparoscopic camera controller
+    //-------------------------------------------------------
+    auto camClient = std::make_shared<VRPNForceDevice>();
+
+    //get some user input and setup device url
+    std::string input = "Phantom@10.171.2.217";//"Phantom0@localhost";
+    std::cout << "Enter the VRPN device URL(" << camClient->getDeviceURL() << "): ";
+    std::getline(std::cin, input);
+
+    if (!input.empty())
+    {
+        camClient->setDeviceURL(input);
+    }
+    auto camController = std::make_shared<LaparoscopicCameraCoupler>(camClient);
+    camController->setScalingFactor(20.0);
+
+    viewer->init(); // viewer should be initialized to be able to retrieve the camera
+    camController->setCamera((std::static_pointer_cast<VTKViewer>(viewer))->getVtkCamera());
+
+    sdk->registerModule(camClient);
+    sdk->registerModule(camController);
+
+    //-------------------------------------------------------
+    // Run the SDK
+    //-------------------------------------------------------
+    sdk->run();
+
+    //cleanup
+    sdk->releaseScene(scene);
+
+    return 0;
+}

VTKRendering/VTKViewer.cpp

@@ -30,6 +30,7 @@
 #include <vtkNew.h>
 #include <vtkSmartPointer.h>
 #include <vtkRenderer.h>
+#include <vtkRendererCollection.h>
 #include <vtkRenderWindow.h>
 #include <vtkRenderWindowInteractor.h>
 #include <vtkInteractorStyleSwitch.h>
@@ -291,6 +292,13 @@ void VTKViewer::render()
 
     this->endModule();
 }
+
+vtkCamera* VTKViewer::getVtkCamera()
+{
+    return this->renderer->getRenderWindow()->GetRenderers()->
+        GetFirstRenderer()->GetActiveCamera();
+}
+
 void VTKViewer::initRenderingContext()
 {
     this->renderer->addRenderer();

VTKRendering/VTKViewer.h

@@ -26,6 +26,8 @@
 
 #include "Core/ViewerBase.h"
 
+class vtkCamera;
+
 class VTKViewer : public ViewerBase
 {
 
@@ -77,6 +79,11 @@ public:
     ///
     void endFrame() override {}
 
+    ///
+    /// \brief Returns the pointer to the current vtk camera
+    ///
+    vtkCamera* getVtkCamera();
+
 protected:
     ///
     /// \brief Renders the render operation to screen
@@ -137,5 +144,4 @@ private:
     std::unique_ptr<VTKRenderer> renderer;
 };
 
-
-#endif // VTKVIEWER_H
+#endif // VTKVIEWER_H
\ No newline at end of file

VirtualTools/CMakeLists.txt

@@ -3,10 +3,10 @@ simmedtk_add_library(VirtualTools
   SOURCES
 #     CurvedGrasper.cpp
     ToolCoupler.cpp
-	#LaparoscopicCameraCoupler.cpp
+	LaparoscopicCameraCoupler.cpp
   PUBLIC_HEADERS
     ToolCoupler.h
-	#LaparoscopicCameraCoupler.h
+	LaparoscopicCameraCoupler.h
 #     CurvedGrasper.h
 )
 

VirtualTools/LaparoscopicCameraCoupler.cpp

@@ -63,12 +63,12 @@ std::shared_ptr< DeviceInterface > LaparoscopicCameraCoupler::getInputDevice()
     return this->inputDevice;
 }
 
-void LaparoscopicCameraCoupler::setCamera(std::shared_ptr<vtkCamera> newCamera)
+void LaparoscopicCameraCoupler::setCamera(vtkCamera* newCamera)
 {
     this->camera = newCamera;
 }
 
-std::shared_ptr<vtkCamera> LaparoscopicCameraCoupler::getcamera() const
+vtkCamera* LaparoscopicCameraCoupler::getcamera() const
 {
     return this->camera;
 }
@@ -142,23 +142,30 @@ void LaparoscopicCameraCoupler::exec()
 
 bool LaparoscopicCameraCoupler::updateTracker()
 {
-    if(!this->inputDevice)
+
+    if (!this->inputDevice)
     {
         std::cout << "Invalid input device" << std::endl;
         return false;
     }
-    core::Quaterniond rot = inputDevice->getOrientation();
-    core::Vec3d pos = inputDevice->getPosition() * this->scalingFactor;
 
-    Eigen::Quaterniond rotation(rot * this->orientation);
-    Eigen::Translation3d translation(pos - rotation*this->position);
 
-    this->mesh->transform(/*this->initialTransform**/translation*rotation);
+    /*this->prevPosition = this->position;
+    this->prevOrientation = this->orientation;*/
+
+    core::Quaterniond newRot = inputDevice->getOrientation();
+    core::Vec3d newPos = inputDevice->getPosition() * this->scalingFactor;
+
+    this->camera->SetPosition(newPos[0], newPos[1], newPos[2]);
+    return true;
 
-    this->mesh->getRenderDelegate()->modified(); //tell the renderer to update
+    Eigen::Quaterniond deltaRotation(newRot * this->orientation.conjugate());
+    Eigen::Translation3d translationPrev(-this->position);
+    Eigen::Translation3d translationPresent(newPos);
+    Eigen::Translation3d translationOffset(this->offsetPosition);
 
-    this->position = pos;
-    this->orientation = rot.conjugate();
+    this->position = newPos + this->offsetPosition;
+    this->orientation = this->offsetOrientation * newRot;
 
     return true;
 }

VirtualTools/LaparoscopicCameraCoupler.h

@@ -74,12 +74,12 @@ public:
     /// \brief Set the pointer to the mesh to control
     /// \param newMesh A pointer to an allocated mesh
     ///
-    void setCamera(std::shared_ptr<vtkCamera> newCamera);
+    void setCamera(vtkCamera* newCamera);
 
     ///
     /// \brief Get the output device for this tool coupler
     ///
-    std::shared_ptr<vtkCamera> getcamera() const;
+    vtkCamera* getcamera() const;
 
     ///
     /// \brief Get the current pooling delay
@@ -173,6 +173,10 @@ public:
 private:
     TransformType initialTransform; //!< Transform applied to the position obtained from device
     core::Quaterniond orientation; //!< Previous rotation quaternion from device
+
+    core::Quaterniond prevOrientation; //!< Previous rotation quaternion from phantom
+    core::Vec3d prevPosition;          //!< Previous position from phantom
+
     core::Vec3d position;          //!< Previous position from device
     double scalingFactor;          //!< Scaling factor for physical to virtual translation
 
@@ -180,7 +184,7 @@ private:
     core::Vec3d offsetPosition;          //!< Previous position from device
 
     std::chrono::milliseconds poolDelay;  //!< Pooling delay
-    std::shared_ptr<vtkCamera> camera; //!< Pointer to rendering camera
+    vtkCamera* camera; //!< Pointer to rendering camera
     std::shared_ptr<DeviceInterface> inputDevice;  //!< Pointer to input device
 };
 

VirtualTools/ToolCoupler.cpp

@@ -38,7 +38,6 @@ ToolCoupler::ToolCoupler(std::shared_ptr< DeviceInterface > inputDevice,
 
     this->offsetPosition.setZero();
     this->offsetOrientation.setIdentity();
-    //this->offsetOrientation = core::Quaterniond(-30.0,1,0,0);
 
     this->pollDelay = std::chrono::milliseconds(100);
     this->mesh = toolMesh;
@@ -53,7 +52,6 @@ ToolCoupler::ToolCoupler(std::shared_ptr<DeviceInterface> inputDevice)
 
     this->offsetPosition.setZero();
     this->offsetOrientation.setIdentity();
-    //this->offsetOrientation = core::Quaterniond(-30.0, 1, 0, 0);
 
     this->pollDelay = std::chrono::milliseconds(100);
     this->initialTransform.Identity();
@@ -69,9 +67,6 @@ ToolCoupler::ToolCoupler(std::shared_ptr< DeviceInterface > inputDevice,
 
     this->offsetPosition.setZero();
     this->offsetOrientation.setIdentity();
-    //this->offsetOrientation = core::Quaterniond(-30, 1, 0, 0);
-
-
 
     this->pollDelay = std::chrono::milliseconds(100);
     this->mesh = toolMesh;
@@ -138,7 +133,8 @@ const Eigen::Quaternion<double> &ToolCoupler::getOrientation() const
     return this->orientation;
 }
 
-void ToolCoupler::setOrientation(const Eigen::Map< Eigen::Quaternion< double > >& newOrientation)
+void ToolCoupler::setOrientation(
+    const Eigen::Map< Eigen::Quaternion< double > >& newOrientation)
 {
     this->orientation = newOrientation;
 }