diff --git a/Modules/Bundles/org.mitk.gui.qt.toftutorial/src/internal/QmitkToFTutorialView.cpp b/Modules/Bundles/org.mitk.gui.qt.toftutorial/src/internal/QmitkToFTutorialView.cpp
index d0093cb9bb..b7db784681 100644
--- a/Modules/Bundles/org.mitk.gui.qt.toftutorial/src/internal/QmitkToFTutorialView.cpp
+++ b/Modules/Bundles/org.mitk.gui.qt.toftutorial/src/internal/QmitkToFTutorialView.cpp
@@ -1,193 +1,193 @@
 /*=========================================================================
 
 Program:   Medical Imaging & Interaction Toolkit
 Language:  C++
 Date:      $Date$
 Version:   $Revision$ 
  
 Copyright (c) German Cancer Research Center, Division of Medical and
 Biological Informatics. All rights reserved.
 See MITKCopyright.txt or http://www.mitk.org/copyright.html for details.
 
 This software is distributed WITHOUT ANY WARRANTY; without even
 the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
 PURPOSE.  See the above copyright notices for more information.
 
 =========================================================================*/
 
 // Blueberry
 #include <berryISelectionService.h>
 #include <berryIWorkbenchWindow.h>
 
 // Qmitk
 #include "QmitkToFTutorialView.h"
 #include "QmitkStdMultiWidget.h"
 
 // Qt
 #include <QMessageBox>
 
 // mitk includes
+#include <mitkCameraIntrinsics.h> // class holding the intrinsic parameters of the according camera
 #include <mitkSurface.h>
 
 // MITK-ToF related includes
 #include <mitkToFConfig.h> // configuration file holding e.g. plugin paths or path to test file directory
 #include <mitkToFDistanceImageToSurfaceFilter.h> // filter from module ToFProcessing that calculates a surface from the given range image
 #include <mitkToFImageGrabberCreator.h> // creator class that provides pre-configured ToFCameraDevices
 #include <mitkToFImageGrabber.h> // allows access to images provided by the ToF camera
-#include <mitkPinholeCameraModel.h> // class representing a pinhole camera and holding the intrinsic parameters of the according camera
 
 
 
 const std::string QmitkToFTutorialView::VIEW_ID = "org.mitk.views.toftutorial";
 
 QmitkToFTutorialView::QmitkToFTutorialView()
 : QmitkFunctionality()
 , m_Controls( 0 )
 , m_MultiWidget( NULL )
 {
 }
 
 QmitkToFTutorialView::~QmitkToFTutorialView()
 {
 }
 
 
 void QmitkToFTutorialView::CreateQtPartControl( QWidget *parent )
 {
   // build up qt view, unless already done
   if ( !m_Controls )
   {
     // create GUI widgets from the Qt Designer's .ui file
     m_Controls = new Ui::QmitkToFTutorialViewControls;
     m_Controls->setupUi( parent );
  
     connect( m_Controls->step1Button, SIGNAL(clicked()), this, SLOT(OnStep1()) );
     connect( m_Controls->step2Button, SIGNAL(clicked()), this, SLOT(OnStep2()) );
   }
 }
 
 
 void QmitkToFTutorialView::StdMultiWidgetAvailable (QmitkStdMultiWidget &stdMultiWidget)
 {
   m_MultiWidget = &stdMultiWidget;
 }
 
 
 void QmitkToFTutorialView::StdMultiWidgetNotAvailable()
 {
   m_MultiWidget = NULL;
 }
 
 
 void QmitkToFTutorialView::OnStep1()
 {
   // clean up data storage
   RemoveAllNodesFromDataStorage();
   // use ToFImageGrabber to create instance of ToFImageGrabber that holds a ToFCameraMITKPlayerDevice for playing ToF data
   mitk::ToFImageGrabber::Pointer tofImageGrabber = mitk::ToFImageGrabberCreator::GetInstance()->GetMITKPlayerImageGrabber();
   // set paths to test data
   std::string distanceFileName = MITK_TOF_DATA_DIR;
   distanceFileName.append("/PMDCamCube2_MF0_IT0_20Images_DistanceImage.pic"); 
   std::string amplitudeFileName = MITK_TOF_DATA_DIR;
   amplitudeFileName.append("/PMDCamCube2_MF0_IT0_20Images_AmplitudeImage.pic"); 
   std::string intensityFileName = MITK_TOF_DATA_DIR;
   intensityFileName.append("/PMDCamCube2_MF0_IT0_20Images_IntensityImage.pic"); 
   // set file name property in image grabber. This will be propagated to the corresponding device and controller class
   tofImageGrabber->SetProperty("DistanceImageFileName",mitk::StringProperty::New(distanceFileName));
   tofImageGrabber->SetProperty("AmplitudeImageFileName",mitk::StringProperty::New(amplitudeFileName));
   tofImageGrabber->SetProperty("IntensityImageFileName",mitk::StringProperty::New(intensityFileName));
   // connect to device
   if (tofImageGrabber->ConnectCamera())
   {
     //// start camera (internally starts thread that continuously grabs images from the camera)
     tofImageGrabber->StartCamera();
     // update image grabber which itself represents the source of a MITK filter pipeline
     tofImageGrabber->Update();
     // grab distance image
     mitk::Image::Pointer distanceImage = tofImageGrabber->GetOutput(0);
     // grab amplitude image
     mitk::Image::Pointer amplitudeImage = tofImageGrabber->GetOutput(1);
     // grab intensity image
     mitk::Image::Pointer intensityImage = tofImageGrabber->GetOutput(2);
     //add distance image to data storage
     mitk::DataNode::Pointer distanceNode = mitk::DataNode::New();
     distanceNode->SetName("Distance Image");
     distanceNode->SetData(distanceImage);
     this->GetDefaultDataStorage()->Add(distanceNode);
     //add amplitude image to data storage
     mitk::DataNode::Pointer amplitudeNode = mitk::DataNode::New();
     amplitudeNode->SetName("Amplitude Image");
     amplitudeNode->SetData(amplitudeImage);
     this->GetDefaultDataStorage()->Add(amplitudeNode);
     //add intensity image to data storage
     mitk::DataNode::Pointer intensityNode = mitk::DataNode::New();
     intensityNode->SetName("Intensity Image");
     intensityNode->SetData(intensityImage);
     this->GetDefaultDataStorage()->Add(intensityNode);
     // stop camera (terminate internally used thread)
     tofImageGrabber->StopCamera();
     // disconnect from camera
     tofImageGrabber->DisconnectCamera();
     // adjust views to new data in DataStorage
     mitk::RenderingManager::GetInstance()->InitializeViews(distanceImage->GetGeometry());
   }
   else
   {
     MITK_ERROR<<"Connection to ToF camera could not be established";
   }
 }
 
 void QmitkToFTutorialView::OnStep2()
 {
   // Check if distance image is available
   mitk::DataNode::Pointer distanceNode = this->GetDefaultDataStorage()->GetNamedNode("Distance Image");
   if (distanceNode.IsNotNull())
   {
     // get distance image from node and check if node contains image
     mitk::Image::Pointer distanceImage = dynamic_cast<mitk::Image*>(distanceNode->GetData());
     if (distanceImage.IsNotNull())
     {
-      // create pinhole camera model that holds intrinsic parameters of the ToF camera
-      mitk::PinholeCameraModel::Pointer cameraIntrinsics = mitk::PinholeCameraModel::New();
+      // create object of CameraIntrinsics that holds intrinsic parameters of the ToF camera
+      mitk::CameraIntrinsics::Pointer cameraIntrinsics = mitk::CameraIntrinsics::New();
       // change focal length and use defaults for other parameters such as inter pixel distance or principal point
-      cameraIntrinsics->SetFocalLength(13.3);
+      cameraIntrinsics->SetFocalLength(13.3,13.3);
       // set up filter for surface calculation
       mitk::ToFDistanceImageToSurfaceFilter::Pointer surfaceFilter = mitk::ToFDistanceImageToSurfaceFilter::New();
       // apply intrinsic parameters to filter
-      surfaceFilter->SetCameraModel(cameraIntrinsics);
+      surfaceFilter->SetCameraIntrinsics(cameraIntrinsics);
       // set distance image as input
       surfaceFilter->SetInput(distanceImage);
       // update the filter
       surfaceFilter->Update();
       // get surface from filter
       mitk::Surface::Pointer surface = surfaceFilter->GetOutput();
       // add surface to data storage
       mitk::DataNode::Pointer surfaceNode = mitk::DataNode::New();
       surfaceNode->SetName("ToF surface");
       surfaceNode->SetData(surface);
       this->GetDefaultDataStorage()->Add(surfaceNode);
       // adjust views to new data in DataStorage
       mitk::RenderingManager::GetInstance()->InitializeViews(surface->GetGeometry());
       mitk::RenderingManager::GetInstance()->InitializeViews(surface->GetGeometry());
     }
     else
     {
       QMessageBox::warning(NULL,"ToF Tutorial","Node 'Distance Image' contains no image");
     }
   }
   else
   {
     QMessageBox::warning(NULL,"ToF Tutorial","Perform Step 1 first to acquire a distance image");
   }
 }
 
 void QmitkToFTutorialView::RemoveAllNodesFromDataStorage()
 {
   mitk::DataStorage::SetOfObjects::ConstPointer allNodes = this->GetDefaultDataStorage()->GetAll();
   this->GetDefaultDataStorage()->Remove(allNodes);
 }
 
 
 
 
 
diff --git a/Modules/ToFProcessing/CMakeLists.txt b/Modules/ToFProcessing/CMakeLists.txt
index bc2090729c..d1df90e238 100644
--- a/Modules/ToFProcessing/CMakeLists.txt
+++ b/Modules/ToFProcessing/CMakeLists.txt
@@ -1,13 +1,13 @@
 MITK_CREATE_MODULE(mitkToFProcessing
     SUBPROJECTS MITK-ToF
-    DEPENDS Mitk MitkExt
+    DEPENDS Mitk MitkExt mitkCameraCalibration
     PACKAGE_DEPENDS OpenCV
     PROVIDES mitkToFProcessing
   )
   
 IF(BUILD_TESTING)
 
   ADD_SUBDIRECTORY(Testing)
 
 ENDIF(BUILD_TESTING)
 
diff --git a/Modules/ToFProcessing/Testing/mitkToFDistanceImageToSurfaceFilterTest.cpp b/Modules/ToFProcessing/Testing/mitkToFDistanceImageToSurfaceFilterTest.cpp
index 0c2e118ce9..901c061aed 100644
--- a/Modules/ToFProcessing/Testing/mitkToFDistanceImageToSurfaceFilterTest.cpp
+++ b/Modules/ToFProcessing/Testing/mitkToFDistanceImageToSurfaceFilterTest.cpp
@@ -1,246 +1,221 @@
 /*=========================================================================
 
 Program:   Medical Imaging & Interaction Toolkit
 Language:  C++
 Date:      $Date: $
 Version:   $Revision: $
 
 Copyright (c) German Cancer Research Center, Division of Medical and
 Biological Informatics. All rights reserved.
 See MITKCopyright.txt or http://www.mitk.org/copyright.html for details.
 
 This software is distributed WITHOUT ANY WARRANTY; without even
 the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
 PURPOSE.  See the above copyright notices for more information.
 
 =========================================================================*/
 
 #include <mitkTestingMacros.h>
 #include <mitkToFDistanceImageToSurfaceFilter.h>
 //#include <mitkToFSurfaceGenerationFilter.h>
 
 #include <mitkImage.h>
 #include <mitkSurface.h>
 #include <mitkToFProcessingCommon.h>
 #include <mitkVector.h>
 #include <mitkToFTestingCommon.h>
 
 //#include <itkImage.h>
 //#include <itkImageRegionIterator.h>
 //#include <itkMersenneTwisterRandomVariateGenerator.h>
 
 #include <vtkPoints.h>
 #include <vtkPolyData.h>
 #include <vtkSmartPointer.h>
 
 /**Documentation
  *  test for the class "ToFDistanceImageToSurfaceFilter".
  */
 
 typedef mitk::ToFProcessingCommon::ToFPoint2D ToFPoint2D;
 typedef mitk::ToFProcessingCommon::ToFPoint3D ToFPoint3D;
 typedef mitk::ToFProcessingCommon::ToFScalarType ToFScalarType;
 
 int mitkToFDistanceImageToSurfaceFilterTest(int /* argc */, char* /*argv*/[])
 {
   MITK_TEST_BEGIN("ToFDistanceImageToSurfaceFilter");
   mitk::ToFDistanceImageToSurfaceFilter::Pointer filter = mitk::ToFDistanceImageToSurfaceFilter::New();
   // create test image
   unsigned int dimX =204;
   unsigned int dimY =204;
   mitk::Image::Pointer image = mitk::ToFTestingCommon::CreateTestImage(dimX,dimY);
   //initialize intrinsic parameters with some arbitrary values
-  ToFScalarType focalLength = 13.654368;
-  ToFPoint2D interPixelDistance;
-  interPixelDistance[0] = 0.04564;
-  interPixelDistance[1] = 0.0451564;
+  ToFScalarType focalLengthX = 13.654368;
+  ToFScalarType focalLengthY = 13.854;
+  ToFScalarType k1=-0.36,k2=-0.14,p1=0.001,p2=-0.00;
   ToFPoint2D principalPoint;
   principalPoint[0] = 103.576546;
   principalPoint[1] = 100.1532;
-
-  mitk::PinholeCameraModel::Pointer camera = mitk::PinholeCameraModel::New();
-  camera->SetFocalLength(focalLength);
-  camera->SetPrincipalPoint(principalPoint);
-  camera->SetInterPixelDistance(interPixelDistance);
-
-  filter->SetCameraModel(camera);
-
-  MITK_TEST_CONDITION_REQUIRED((focalLength==filter->GetCameraModel()->GetFocalLength()),"Testing Set/GetFocalLength()");
-  filter->GetCameraModel()->SetInterPixelDistance(interPixelDistance);
-  MITK_TEST_CONDITION_REQUIRED((interPixelDistance==filter->GetCameraModel()->GetInterPixelDistance()),"Testing Set/GetInterPixelDistance()");
-  filter->GetCameraModel()->SetPrincipalPoint(principalPoint);
-  ToFPoint2D pp = filter->GetCameraModel()->GetPrincipalPoint();
-  MITK_TEST_CONDITION_REQUIRED(mitk::Equal(principalPoint,pp),"Testing Set/GetPrincipalPoint()");
-  // test SetIntrinsicParameters()
-  mitk::PinholeCameraModel::Pointer cameraIntrinics = mitk::PinholeCameraModel::New();
-  ToFPoint2D imageArea;
-  imageArea[0] = dimX*interPixelDistance[0];
-  imageArea[1] = dimY*interPixelDistance[1];
-  cameraIntrinics->SetImageArea(imageArea);
-  cameraIntrinics->SetFocalLength(focalLength);
-  ToFPoint2D pixelSize;
-  pixelSize[0] = interPixelDistance[0];
-  pixelSize[1] = interPixelDistance[1];
-  cameraIntrinics->SetInterPixelDistance(pixelSize);
-  ToFPoint2D imageShift;
-  imageShift[0] = (principalPoint[0] - dimX/2)*interPixelDistance[0];
-  imageShift[1] = (principalPoint[1] - dimX/2)*interPixelDistance[1];
-  cameraIntrinics->SetImageShift(imageShift);
-  cameraIntrinics->SetPrincipalPoint(principalPoint);
-  filter->SetCameraModel(cameraIntrinics);
-  MITK_TEST_CONDITION_REQUIRED((focalLength==filter->GetCameraModel()->GetFocalLength()),"Testing SetIntrinsicParameters() - focal length");
-  MITK_TEST_CONDITION_REQUIRED((interPixelDistance==filter->GetCameraModel()->GetInterPixelDistance()),"Testing SetIntrinsicParameters() - inter pixel distance");
-  pp = filter->GetCameraModel()->GetPrincipalPoint();
-  MITK_TEST_CONDITION_REQUIRED(mitk::Equal(principalPoint,pp),"Testing SetIntrinsicParameters() - principal point");
+  mitk::CameraIntrinsics::Pointer cameraIntrinsics = mitk::CameraIntrinsics::New();
+  cameraIntrinsics->SetFocalLength(focalLengthX,focalLengthY);
+  cameraIntrinsics->SetPrincipalPoint(principalPoint[0],principalPoint[1]);
+  cameraIntrinsics->SetDistorsionCoeffs(k1,k2,p1,p2);
+  // test SetCameraIntrinsics()
+  filter->SetCameraIntrinsics(cameraIntrinsics);
+  MITK_TEST_CONDITION_REQUIRED((focalLengthX==filter->GetCameraIntrinsics()->GetFocalLengthX()),"Testing SetCameraIntrinsics with focalLength");
+  ToFPoint2D pp;
+  pp[0] = filter->GetCameraIntrinsics()->GetPrincipalPointX();
+  pp[1] = filter->GetCameraIntrinsics()->GetPrincipalPointY();
+  MITK_TEST_CONDITION_REQUIRED(mitk::Equal(principalPoint,pp),"Testing SetCameraIntrinsics with principalPoint()");
+  // test SetInterPixelDistance()
+  ToFPoint2D interPixelDistance;
+  interPixelDistance[0] = 0.04564;
+  interPixelDistance[1] = 0.0451564;
+  filter->SetInterPixelDistance(interPixelDistance);
+  ToFPoint2D ipD = filter->GetInterPixelDistance();
+  MITK_TEST_CONDITION_REQUIRED(mitk::Equal(ipD,interPixelDistance),"Testing Set/GetInterPixelDistance()");
 
   // test Set/GetInput()
   filter->SetInput(image);
   MITK_TEST_CONDITION_REQUIRED((image==filter->GetInput()),"Testing Set/GetInput()");
 
   // test filter without subset
   MITK_INFO<<"Test filter ";
+  // calculate focal length considering inter pixel distance
+  ToFScalarType focalLength = (focalLengthX*interPixelDistance[0]+focalLengthY*interPixelDistance[1])/2.0;
   vtkSmartPointer<vtkPoints> expectedResult = vtkSmartPointer<vtkPoints>::New();
   expectedResult->SetDataTypeToDouble();
   unsigned int counter = 0;
   double* point = new double[3];
 //  MITK_INFO<<"Test";
 //  MITK_INFO<<"focal: "<<focalLength;
 //  MITK_INFO<<"inter: "<<interPixelDistance;
 //  MITK_INFO<<"prinicipal: "<<principalPoint;
   for (unsigned int j=0; j<dimX; j++)
   {
     for (unsigned int i=0; i<dimY; i++)
     {
       mitk::Index3D index;
       index[0] = i;
       index[1] = j;
       index[2] = 0;
       ToFScalarType distance = image->GetPixelValueByIndex(index);
       ToFPoint3D coordinate = mitk::ToFProcessingCommon::IndexToCartesianCoordinates(i,j,distance,focalLength,interPixelDistance,principalPoint);
 //      if ((i==0)&&(j==0))
 //      {
 //        MITK_INFO<<"Distance test: "<<distance;
 //        MITK_INFO<<"coordinate test: "<<coordinate;
 //      }
       point[0] = coordinate[0];
       point[1] = coordinate[1];
       point[2] = coordinate[2];
       unsigned int pointID = index[0] + index[1]*dimY;
       //MITK_INFO<<"id: "<<pointID;
       //MITK_INFO<<"counter: "<<counter;
       if (distance!=0)
       {
         expectedResult->InsertPoint(pointID,point);
       }
       counter++;
     }
   }
   filter->Update();
   mitk::Surface::Pointer resultSurface = filter->GetOutput();
   vtkSmartPointer<vtkPoints> result = vtkSmartPointer<vtkPoints>::New();
   result->SetDataTypeToDouble();
   result = resultSurface->GetVtkPolyData()->GetPoints();
   MITK_TEST_CONDITION_REQUIRED((expectedResult->GetNumberOfPoints()==result->GetNumberOfPoints()),"Test if number of points in surface is equal");
   bool pointSetsEqual = true;
   for (unsigned int i=0; i<expectedResult->GetNumberOfPoints(); i++)
   {
     double* expected = expectedResult->GetPoint(i);
     double* res = result->GetPoint(i);
 
     ToFPoint3D expectedPoint;
     expectedPoint[0] = expected[0];
     expectedPoint[1] = expected[1];
     expectedPoint[2] = expected[2];
     ToFPoint3D resultPoint;
     resultPoint[0] = res[0];
     resultPoint[1] = res[1];
     resultPoint[2] = res[2];
 
     if (!mitk::Equal(expectedPoint,resultPoint))
     {
 //      MITK_INFO << i;
       MITK_INFO<<"expected: "<<expectedPoint;
       MITK_INFO<<"result: "<<resultPoint;
       pointSetsEqual = false;
     }
   }
   MITK_TEST_CONDITION_REQUIRED(pointSetsEqual,"Testing filter without subset");
 
   //Backwardtransformation test
   bool backwardTransformationsPointsEqual = true;
   for (unsigned int i=0; i<expectedResult->GetNumberOfPoints(); i++)
   {
     double* expected = expectedResult->GetPoint(i);
     double* res = result->GetPoint(i);
 
     ToFPoint3D expectedPoint;
     expectedPoint[0] = expected[0];
     expectedPoint[1] = expected[1];
     expectedPoint[2] = expected[2];
     ToFPoint3D resultPoint;
     resultPoint[0] = res[0];
     resultPoint[1] = res[1];
     resultPoint[2] = res[2];
 
     ToFPoint3D expectedPointBackward =
-        mitk::ToFProcessingCommon::CartesianToIndexCoordinates(expectedPoint,
-                                                               filter->GetCameraModel()->GetFocalLength(),
-                                                               filter->GetCameraModel()->GetInterPixelDistance(),
-                                                               filter->GetCameraModel()->GetPrincipalPoint());
+        mitk::ToFProcessingCommon::CartesianToIndexCoordinates(expectedPoint,focalLength,interPixelDistance,principalPoint);
 
     ToFPoint3D resultPointBackward =
-        mitk::ToFProcessingCommon::CartesianToIndexCoordinates(resultPoint,
-                                                               filter->GetCameraModel()->GetFocalLength(),
-                                                               filter->GetCameraModel()->GetInterPixelDistance(),
-                                                               filter->GetCameraModel()->GetPrincipalPoint());
+        mitk::ToFProcessingCommon::CartesianToIndexCoordinates(resultPoint,focalLength,interPixelDistance,principalPoint);
 
     if (!mitk::Equal(expectedPointBackward,resultPointBackward))
     {
 //      MITK_INFO << i;
 //      MITK_INFO<<"expected: "<<expectedPoint;
 //      MITK_INFO<<"result: "<<resultPoint;
       backwardTransformationsPointsEqual = false;
     }
   }
   MITK_TEST_CONDITION_REQUIRED(backwardTransformationsPointsEqual,"Testing backward transformation");
 
   //Backwardtransformation test compare to original input
   bool compareToInput = true;
   for (unsigned int i=0; i<result->GetNumberOfPoints(); i++)
   {
     double* res = result->GetPoint(i);
 
     ToFPoint3D resultPoint;
     resultPoint[0] = res[0];
     resultPoint[1] = res[1];
     resultPoint[2] = res[2];
 
     ToFPoint3D resultPointBackward =
-        mitk::ToFProcessingCommon::CartesianToIndexCoordinates(resultPoint,
-                                                               filter->GetCameraModel()->GetFocalLength(),
-                                                               filter->GetCameraModel()->GetInterPixelDistance(),
-                                                               filter->GetCameraModel()->GetPrincipalPoint());
+        mitk::ToFProcessingCommon::CartesianToIndexCoordinates(resultPoint,focalLength,interPixelDistance,principalPoint);
 
     mitk::Index3D pixelIndex;
     pixelIndex[0] = (int) (resultPointBackward[0]+0.5);
     pixelIndex[1] = (int) (resultPointBackward[1]+0.5);
     pixelIndex[2] = 0;
 
     if (!mitk::Equal(image->GetPixelValueByIndex(pixelIndex),resultPointBackward[2]))
     {
 //      MITK_INFO<<"expected: "<< image->GetPixelValueByIndex(pixelIndex);
 //      MITK_INFO<<"result: "<< resultPoint;
       compareToInput = false;
     }
   }
   MITK_TEST_CONDITION_REQUIRED(compareToInput,"Testing backward transformation compared to original image");
 
   //clean up
   delete point;
   //  expectedResult->Delete();
 
   MITK_TEST_END();
 
 }
 
 
diff --git a/Modules/ToFProcessing/mitkToFDistanceImageToSurfaceFilter.cpp b/Modules/ToFProcessing/mitkToFDistanceImageToSurfaceFilter.cpp
index f15471756a..8566eb4de4 100644
--- a/Modules/ToFProcessing/mitkToFDistanceImageToSurfaceFilter.cpp
+++ b/Modules/ToFProcessing/mitkToFDistanceImageToSurfaceFilter.cpp
@@ -1,247 +1,254 @@
 /*=========================================================================
 Program:   Medical Imaging & Interaction Toolkit
 Module:    $RCSfile$
 Language:  C++
 Date:      $Date$
 Version:   $Revision$
 
 Copyright (c) German Cancer Research Center, Division of Medical and
 Biological Informatics. All rights reserved.
 See MITKCopyright.txt or http://www.mitk.org/copyright.html for details.
 
 This software is distributed WITHOUT ANY WARRANTY; without even
 the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
 PURPOSE.  See the above copyright notices for more information.
 
 =========================================================================*/
 
 #include <mitkToFDistanceImageToSurfaceFilter.h>
 #include <mitkInstantiateAccessFunctions.h>
 #include <mitkSurface.h>
-#include <mitkPinholeCameraModel.h>
-#include <mitkToFProcessingCommon.h>
+//#include <mitkPinholeCameraModel.h>
 
 #include <itkImage.h>
 
 #include <vtkCellArray.h>
 #include <vtkPoints.h>
 #include <vtkPolyData.h>
 #include <vtkPointData.h>
 #include <vtkFloatArray.h>
 #include <vtkPolyDataNormals.h>
 #include <vtkCleanPolyData.h>
 #include <vtkSmartPointer.h>
 
 #include <math.h>
 
-mitk::ToFDistanceImageToSurfaceFilter::ToFDistanceImageToSurfaceFilter(): m_CameraModel(),
-m_TextureImageWidth(0), m_TextureImageHeight(0), m_IplScalarImage(NULL)
+mitk::ToFDistanceImageToSurfaceFilter::ToFDistanceImageToSurfaceFilter(): m_CameraIntrinsics(),
+m_TextureImageWidth(0), m_TextureImageHeight(0), m_IplScalarImage(NULL), m_InterPixelDistance()
 {
-  m_CameraModel = mitk::PinholeCameraModel::New();
+  m_InterPixelDistance.Fill(0.045);
+  m_CameraIntrinsics = mitk::CameraIntrinsics::New();
+  m_CameraIntrinsics->SetFocalLength(295.78960196187319,296.1255427948447);
+  m_CameraIntrinsics->SetPrincipalPoint(113.29063841714108,97.243216122015184);
+  m_CameraIntrinsics->SetDistorsionCoeffs(-0.36874385358645773f,-0.14339503290129013,0.0033210108720361795,-0.004277703352074105);
 }
 
 mitk::ToFDistanceImageToSurfaceFilter::~ToFDistanceImageToSurfaceFilter()
 {
 }
 
-void mitk::ToFDistanceImageToSurfaceFilter::SetInput( Image* distanceImage, mitk::PinholeCameraModel::Pointer CameraModel )
+void mitk::ToFDistanceImageToSurfaceFilter::SetInput( Image* distanceImage, mitk::CameraIntrinsics::Pointer cameraIntrinsics )
 {
-  this->SetCameraModel(CameraModel);
+  this->SetCameraIntrinsics(cameraIntrinsics);
   this->SetInput(0,distanceImage);
 }
 
-void mitk::ToFDistanceImageToSurfaceFilter::SetInput( unsigned int idx,  Image* distanceImage, mitk::PinholeCameraModel::Pointer CameraModel )
+void mitk::ToFDistanceImageToSurfaceFilter::SetInput( unsigned int idx,  Image* distanceImage, mitk::CameraIntrinsics::Pointer cameraIntrinsics )
 {
-  this->SetCameraModel(CameraModel);
+  this->SetCameraIntrinsics(cameraIntrinsics);
   this->SetInput(idx,distanceImage);
 }
 
 void mitk::ToFDistanceImageToSurfaceFilter::SetInput(  mitk::Image* distanceImage )
 {
   this->SetInput(0,distanceImage);
 }
 
 //TODO: braucht man diese Methode?
 void mitk::ToFDistanceImageToSurfaceFilter::SetInput( unsigned int idx,  mitk::Image* distanceImage )
 {
   if ((distanceImage == NULL) && (idx == this->GetNumberOfInputs() - 1)) // if the last input is set to NULL, reduce the number of inputs by one
     this->SetNumberOfInputs(this->GetNumberOfInputs() - 1);
   else
     this->ProcessObject::SetNthInput(idx, distanceImage);   // Process object is not const-correct so the const_cast is required here
 
   this->CreateOutputsForAllInputs();
 }
 
 mitk::Image* mitk::ToFDistanceImageToSurfaceFilter::GetInput()
 {
   return this->GetInput(0);
 }
 
 mitk::Image* mitk::ToFDistanceImageToSurfaceFilter::GetInput( unsigned int idx )
 {
   if (this->GetNumberOfInputs() < 1)
     return NULL; //TODO: geeignete exception werfen
   return static_cast< mitk::Image*>(this->ProcessObject::GetInput(idx));
 }
 
 void mitk::ToFDistanceImageToSurfaceFilter::GenerateData()
 {
   mitk::Surface::Pointer output = this->GetOutput();
   assert(output);
   mitk::Image::Pointer input = this->GetInput();
   assert(input);
   // mesh points
   unsigned int xDimension = input->GetDimension(0);
   unsigned int yDimension = input->GetDimension(1);
   unsigned int size = xDimension*yDimension; //size of the image-array
   std::vector<bool> isPointValid;
   isPointValid.resize(size);
   int pointCount = 0;
   vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
   points->SetDataTypeToDouble();
   vtkSmartPointer<vtkCellArray> polys = vtkSmartPointer<vtkCellArray>::New();
   vtkSmartPointer<vtkFloatArray> scalarArray = vtkSmartPointer<vtkFloatArray>::New();
   vtkSmartPointer<vtkFloatArray> textureCoords = vtkSmartPointer<vtkFloatArray>::New();
   textureCoords->SetNumberOfComponents(2);
 
   float textureScaleCorrection1 = 0.0;
   float textureScaleCorrection2 = 0.0;
   if (this->m_TextureImageHeight > 0.0 && this->m_TextureImageWidth > 0.0)
   {
     textureScaleCorrection1 = float(this->m_TextureImageHeight) / float(this->m_TextureImageWidth);
     textureScaleCorrection2 = ((float(this->m_TextureImageWidth) - float(this->m_TextureImageHeight))/2) / float(this->m_TextureImageWidth);
   }
 
   float* scalarFloatData = NULL;
 
   if (this->m_IplScalarImage) // if scalar image is defined use it for texturing
   {
     scalarFloatData = (float*)this->m_IplScalarImage->imageData;
   }
   else if ((this->GetNumberOfInputs()>2)&&this->GetInput(2)) // otherwise use intensity image (input(2))
   {
     scalarFloatData = (float*)this->GetInput(2)->GetData();
   }
 
   float* inputFloatData = (float*)(input->GetSliceData(0, 0, 0)->GetData());
 
   //calculate world coordinates
   for (int j=0; j<yDimension; j++)
   {
     for (int i=0; i<xDimension; i++)
     {
       // distance value
       mitk::Index3D pixel;
       pixel[0] = i;
       pixel[1] = j;
       pixel[2] = 0;
 
       unsigned int pixelID = pixel[0]+pixel[1]*yDimension;
 
-      ToFScalarType distance = (double)inputFloatData[pixelID];
-      //ToFScalarType distance = input->GetPixelValueByIndex(pixel); //TODO: float array zugriff
+      mitk::ToFProcessingCommon::ToFScalarType distance = (double)inputFloatData[pixelID];
+
+      mitk::ToFProcessingCommon::ToFScalarType focalLength = (m_CameraIntrinsics->GetFocalLengthX()*m_InterPixelDistance[0]+m_CameraIntrinsics->GetFocalLengthY()*m_InterPixelDistance[1])/2.0;
+      mitk::ToFProcessingCommon::ToFPoint2D principalPoint;
+      principalPoint[0] = m_CameraIntrinsics->GetPrincipalPointX();
+      principalPoint[1] = m_CameraIntrinsics->GetPrincipalPointY();
 
       mitk::ToFProcessingCommon::ToFPoint3D cartesianCoordinates =
-          mitk::ToFProcessingCommon::IndexToCartesianCoordinates(i,j,distance,m_CameraModel->GetFocalLength(),m_CameraModel->GetInterPixelDistance(),m_CameraModel->GetPrincipalPoint());
+          mitk::ToFProcessingCommon::IndexToCartesianCoordinates(i,j,distance,focalLength,m_InterPixelDistance,principalPoint);
 
       //TODO: why epsilon here and what value should it have?
 //      if (cartesianCoordinates[2] == 0)
       if (distance<=mitk::eps)
       {
         isPointValid[pointCount] = false;
       }
       else
       {
         isPointValid[pointCount] = true;
       
         points->InsertPoint(pixelID, cartesianCoordinates.GetDataPointer());
 
         if((i >= 1) && (j >= 1))
         {
           vtkIdType xy = i+j*xDimension;
           vtkIdType x_1y = i-1+j*xDimension;
           vtkIdType xy_1 = i+(j-1)*xDimension;
           vtkIdType x_1y_1 = (i-1)+(j-1)*xDimension;
 
           if (isPointValid[xy]&&isPointValid[x_1y]&&isPointValid[x_1y_1]&&isPointValid[xy_1]) // check if points of cell are valid
           {
             polys->InsertNextCell(3);
             polys->InsertCellPoint(xy);
             polys->InsertCellPoint(x_1y);
             polys->InsertCellPoint(x_1y_1);
 
             polys->InsertNextCell(3);
             polys->InsertCellPoint(xy);
             polys->InsertCellPoint(x_1y_1);
             polys->InsertCellPoint(xy_1);
           }
         }
 
         if (scalarFloatData)
         {          
           scalarArray->InsertTuple1(pixelID, scalarFloatData[pixel[0]+pixel[1]*xDimension]);
         }
         if (this->m_TextureImageHeight > 0.0 && this->m_TextureImageWidth > 0.0)
         {
 
           float xNorm = (((float)pixel[0])/xDimension)*textureScaleCorrection1 + textureScaleCorrection2 ; // correct video texture scale 640 * 480!! 
           float yNorm = 1.0 - ((float)pixel[1])/yDimension; //flip y-axis
           textureCoords->InsertTuple2(pixelID, xNorm, yNorm);
         }
       }
       pointCount++;
     }
   }
 
   vtkSmartPointer<vtkPolyData> mesh = vtkSmartPointer<vtkPolyData>::New();
   mesh->SetPoints(points);
   mesh->SetPolys(polys);
   if (scalarArray->GetNumberOfTuples()>0)
   {
     mesh->GetPointData()->SetScalars(scalarArray);
     if (this->m_TextureImageHeight > 0.0 && this->m_TextureImageWidth > 0.0)
     {
       mesh->GetPointData()->SetTCoords(textureCoords);
     }
   }
   output->SetVtkPolyData(mesh);
 }
 
 void mitk::ToFDistanceImageToSurfaceFilter::CreateOutputsForAllInputs()
 {
   this->SetNumberOfOutputs(this->GetNumberOfInputs());  // create outputs for all inputs
   for (unsigned int idx = 0; idx < this->GetNumberOfOutputs(); ++idx)
     if (this->GetOutput(idx) == NULL)
     {
     DataObjectPointer newOutput = this->MakeOutput(idx);
     this->SetNthOutput(idx, newOutput);
   }
   this->Modified();
 }
 
 void mitk::ToFDistanceImageToSurfaceFilter::GenerateOutputInformation()
 {
   this->GetOutput();
   itkDebugMacro(<<"GenerateOutputInformation()");
 }
 
 void mitk::ToFDistanceImageToSurfaceFilter::SetScalarImage(IplImage* iplScalarImage)
 {
   this->m_IplScalarImage = iplScalarImage;
   this->Modified();
 }
 
 IplImage* mitk::ToFDistanceImageToSurfaceFilter::GetScalarImage()
 {
   return this->m_IplScalarImage;
 }
 
 void mitk::ToFDistanceImageToSurfaceFilter::SetTextureImageWidth(int width)
 {
   this->m_TextureImageWidth = width;
 }
 
 void mitk::ToFDistanceImageToSurfaceFilter::SetTextureImageHeight(int height)
 {
   this->m_TextureImageHeight = height;
 }
diff --git a/Modules/ToFProcessing/mitkToFDistanceImageToSurfaceFilter.h b/Modules/ToFProcessing/mitkToFDistanceImageToSurfaceFilter.h
index eb044ba596..88768a5a19 100644
--- a/Modules/ToFProcessing/mitkToFDistanceImageToSurfaceFilter.h
+++ b/Modules/ToFProcessing/mitkToFDistanceImageToSurfaceFilter.h
@@ -1,144 +1,149 @@
 /*=========================================================================
 
 Program:   Medical Imaging & Interaction Toolkit
 Module:    $RCSfile$
 Language:  C++
 Date:      $Date$
 Version:   $Revision$
 
 Copyright (c) German Cancer Research Center, Division of Medical and
 Biological Informatics. All rights reserved.
 See MITKCopyright.txt or http://www.mitk.org/copyright.html for details.
 
 This software is distributed WITHOUT ANY WARRANTY; without even
 the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
 PURPOSE.  See the above copyright notices for more information.
 
 =========================================================================*/
 #ifndef __mitkToFDistanceImageToSurfaceFilter_h
 #define __mitkToFDistanceImageToSurfaceFilter_h
 
 
 #include <mitkImage.h>
 #include <mitkSurfaceSource.h>
 #include <mitkToFProcessingExports.h>
-#include "mitkPinholeCameraModel.h"
+#include <mitkToFProcessingCommon.h>
+#include <mitkCameraIntrinsics.h>
+#include "mitkCameraIntrinsics.h"
 #include <mitkPointSet.h>
 #include <cv.h>
 
 namespace mitk
 {
   /**
   * @brief Converts a Time-of-Flight (ToF) distance image to a 3D surface using the pinhole camera model for coordinate computation.
   * The intrinsic parameters of the camera (FocalLength, PrincipalPoint, InterPixelDistance) are set via SetCameraIntrinsics(). The
   * measured distance for each pixel corresponds to the distance between the object point and the corresponding image point on the
   * image plane.
   *
-  * <img src="PinholeCameraModel.png" alt="Pinhole camera model">
+  * <img src="CameraIntrinsics.png" alt="Pinhole camera model">
   * <img src="ImagePlane.png" alt="Image plane">
   *
   * @ingroup SurfaceFilters
   * @ingroup ToFProcessing
   */
   class mitkToFProcessing_EXPORT ToFDistanceImageToSurfaceFilter : public SurfaceSource
   {
   public:
-    typedef mitk::PinholeCameraModel::ToFScalarType ToFScalarType;
 
     mitkClassMacro( ToFDistanceImageToSurfaceFilter , SurfaceSource );
     itkNewMacro( Self );
 
 
-    itkSetMacro(CameraModel, mitk::PinholeCameraModel::Pointer);
-    itkGetMacro(CameraModel, mitk::PinholeCameraModel::Pointer);
+    itkSetMacro(CameraIntrinsics, mitk::CameraIntrinsics::Pointer);
+    itkGetMacro(CameraIntrinsics, mitk::CameraIntrinsics::Pointer);
+    itkSetMacro(InterPixelDistance,ToFProcessingCommon::ToFPoint2D);
+    itkGetMacro(InterPixelDistance,ToFProcessingCommon::ToFPoint2D);
     /*!
     \brief Set scalar image used as texture of the surface.
     \param iplScalarImage OpenCV image for texturing
     */
     void SetScalarImage(IplImage* iplScalarImage);
     /*!
     \brief Set scalar image used as texture of the surface.
     \return OpenCV image for texturing
     */
     IplImage* GetScalarImage();
     /*!
     \brief Set width of the scalar image used for texturing the surface
     \param width width (x-dimension) of the texture image
     */
     void SetTextureImageWidth(int width);
     /*!
     \brief Set height of the scalar image used for texturing the surface
     \param height height (y-dimension) of the texture image
     */
     void SetTextureImageHeight(int height);
     /*!
     \brief Sets the input of this filter
     \param distanceImage input is the distance image of e.g. a ToF camera
     */
     virtual void SetInput( Image* distanceImage);
 
     /*!
     \brief Sets the input of this filter and the intrinsic parameters
     \param distanceImage input is the distance image of e.g. a ToF camera
     */
-    virtual void SetInput( Image* distanceImage, mitk::PinholeCameraModel::Pointer CameraModel );
+    virtual void SetInput( Image* distanceImage, mitk::CameraIntrinsics::Pointer cameraIntrinsics );
 
     /*!
     \brief Sets the input of this filter at idx
     \param idx number of the current input
     \param distanceImage input is the distance image of e.g. a ToF camera
     */
     virtual void SetInput(unsigned int idx,  Image* distanceImage);
 
     /*!
     \brief Sets the input of this filter at idx and the intrinsic parameters
     \param idx number of the current input
     \param distanceImage input is the distance image of e.g. a ToF camera
-    \param CameraModel This is the camera model which holds parameters like focal length, pixel size, etc. which are needed for the reconstruction of the surface.
+    \param cameraIntrinsics This is the camera model which holds parameters like focal length, pixel size, etc. which are needed for the reconstruction of the surface.
     */
-    virtual void SetInput( unsigned int idx,  Image* distanceImage, mitk::PinholeCameraModel::Pointer CameraModel );
+    virtual void SetInput( unsigned int idx,  Image* distanceImage, mitk::CameraIntrinsics::Pointer cameraIntrinsics );
 
     /*!
     \brief Returns the input of this filter
     */
      Image* GetInput();
 
     /*!
     \brief Returns the input with id idx of this filter
     */
      Image* GetInput(unsigned int idx);
 
   protected:
     /*!
     \brief Standard constructor
     */
     ToFDistanceImageToSurfaceFilter();
     /*!
     \brief Standard destructor
     */
     ~ToFDistanceImageToSurfaceFilter();
     virtual void GenerateOutputInformation();
     /*!
     \brief Method generating the output of this filter. Called in the updated process of the pipeline.
     This method generates the output of the ToFSurfaceSource: The generated surface of the 3d points
     */
     virtual void GenerateData();
     /**
     * \brief Create an output for each input
     *
     * This Method sets the number of outputs to the number of inputs
     * and creates missing outputs objects.
     * \warning any additional outputs that exist before the method is called are deleted
     */
     void CreateOutputsForAllInputs();
 
     IplImage* m_IplScalarImage; ///< Scalar image used for surface texturing
 
-    mitk::PinholeCameraModel::Pointer m_CameraModel; ///< Specifies the intrinsic parameters
+    mitk::CameraIntrinsics::Pointer m_CameraIntrinsics; ///< Specifies the intrinsic parameters
+    //mitk::CameraIntrinsics::Pointer m_CameraModel; ///< Specifies the intrinsic parameters
 
     int m_TextureImageWidth; ///< Width (x-dimension) of the texture image
     int m_TextureImageHeight; ///< Height (y-dimension) of the texture image
+    ToFProcessingCommon::ToFPoint2D m_InterPixelDistance; ///< distance in mm between two adjacent pixels on the ToF camera chip
 
   };
 } //END mitk namespace
 #endif