diff --git a/Modules/ToFProcessing/Testing/mitkToFDistanceImageToPointSetFilterTest.cpp b/Modules/ToFProcessing/Testing/mitkToFDistanceImageToPointSetFilterTest.cpp
index 6225b3652b..69d16b54ed 100644
--- a/Modules/ToFProcessing/Testing/mitkToFDistanceImageToPointSetFilterTest.cpp
+++ b/Modules/ToFProcessing/Testing/mitkToFDistanceImageToPointSetFilterTest.cpp
@@ -1,248 +1,232 @@
 /*=========================================================================
 
 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 <mitkToFDistanceImageToPointSetFilter.h>
 
 #include <mitkImage.h>
 #include <mitkPointSet.h>
 #include <mitkToFProcessingCommon.h>
 #include <mitkVector.h>
 #include <mitkPinholeCameraModel.h>
 
 #include <itkImage.h>
 #include <itkImageRegionIterator.h>
 
 /**Documentation
 *  test for the class "ToFDistanceImageToPointSetFilter".
 */
 mitk::PointSet::Pointer CreateTestPointSet()
 {
   mitk::PointSet::Pointer subSet = mitk::PointSet::New();
   mitk::Point3D point;
   point[0] = 10;
   point[1] = 20;
   point[2] = 0;
   subSet->InsertPoint(0,point);
   point[0] = 100;
   point[1] = 150;
   point[2] = 0;
   subSet->InsertPoint(1,point);
   point[0] = 110;
   point[1] = 30;
   point[2] = 0;
   subSet->InsertPoint(2,point);
   point[0] = 40;
   point[1] = 200;
   point[2] = 0;
   subSet->InsertPoint(3,point);
   return subSet;
 }
 
 inline static mitk::Image::Pointer CreateTestImageWithPointSet(mitk::ScalarType pixelValue, unsigned int dimX, unsigned int dimY, mitk::PointSet::Pointer subSet)
 {
   typedef itk::Image<mitk::ScalarType,2> ItkImageType2D;
   typedef itk::ImageRegionIterator<ItkImageType2D> ItkImageRegionIteratorType2D;
 
   ItkImageType2D::Pointer image = ItkImageType2D::New();
   ItkImageType2D::IndexType start;
   start[0] = 0;
   start[1] = 0;
   ItkImageType2D::SizeType size;
   size[0] = dimX;
   size[1] = dimY;
   ItkImageType2D::RegionType region;
   region.SetSize(size);
   region.SetIndex( start);
   ItkImageType2D::SpacingType spacing;
   spacing[0] = 1.0;
   spacing[1] = 1.0;
 
   image->SetRegions( region );
   image->SetSpacing ( spacing );
   image->Allocate();
 
   //Obtaining image data from ToF camera//
 
   //Correlate inten values to PixelIndex//
   ItkImageRegionIteratorType2D imageIterator(image,image->GetLargestPossibleRegion());
   imageIterator.GoToBegin();
 
   while (!imageIterator.IsAtEnd())
   {
     imageIterator.Set(pixelValue);
     ++imageIterator;
   }
   // distances varying from pixelValue
   std::vector<mitk::ScalarType> distances;
   distances.push_back(50);
   distances.push_back(500);
   distances.push_back(2050);
   distances.push_back(300);
   // set the pixel values for the subset
   for (unsigned int i=0; i<subSet->GetSize(); i++)
   {
     mitk::Point3D point = subSet->GetPoint(i);
     ItkImageType2D::IndexType index;
     index[0] = point[0];
     index[1] = point[1];
     mitk::ScalarType distance = distances.at(i);
     image->SetPixel(index,distance);
   }
   mitk::Image::Pointer mitkImage = mitk::Image::New();
   mitk::CastToMitkImage(image,mitkImage);
   return mitkImage;
 }
 
 int mitkToFDistanceImageToPointSetFilterTest(int /* argc */, char* /*argv*/[])
 {
   MITK_TEST_BEGIN("ToFDistanceImageToPointSetFilter");
 
   mitk::ToFDistanceImageToPointSetFilter::Pointer filter = mitk::ToFDistanceImageToPointSetFilter::New();
   //create test sub set
   MITK_INFO<<"Create test pointset";
   mitk::PointSet::Pointer subSet = CreateTestPointSet();
   //create test image
   unsigned int dimX = 204;
   unsigned int dimY = 204;
   MITK_INFO<<"Create test image";
   mitk::Image::Pointer image = CreateTestImageWithPointSet(1000.0f,dimX,dimY,subSet);
   //initialize intrinsic parameters
-  mitk::ScalarType focalLength = 10;
-  mitk::Point2D interPixelDistance;
-  interPixelDistance[0] = 0.05;
-  interPixelDistance[1] = 0.05;
-  mitk::Point2D principalPoint;
-  principalPoint[0] = 100;
-  principalPoint[1] = 100;
-
-  mitk::PinholeCameraModel::Pointer camera = mitk::PinholeCameraModel::New();
-  camera->SetFocalLength(focalLength);
-  camera->SetPrincipalPoint(principalPoint);
-  camera->SetInterPixelDistance(interPixelDistance);
-
-
-  // test setter/getter of intrinsic parameters
-  filter->SetCameraModel(camera);
-  MITK_TEST_CONDITION_REQUIRED((focalLength==filter->GetCameraModel()->GetFocalLength()),"Testing Set/GetFocalLength()");
-//  filter->SetInterPixelDistance(interPixelDistance);
-  MITK_TEST_CONDITION_REQUIRED((interPixelDistance==filter->GetCameraModel()->GetInterPixelDistance()),"Testing Set/GetInterPixelDistance()");
-//  filter->SetPrincipalPoint(principalPoint);
-  mitk::Point2D pp = filter->GetCameraModel()->GetPrincipalPoint();
-  MITK_TEST_CONDITION_REQUIRED(mitk::Equal(principalPoint,pp),"Testing Set/GetPrincipalPoint()");
-  // test SetIntrinsicParameters()
-  mitk::PinholeCameraModel::Pointer cameraIntrinics = mitk::PinholeCameraModel::New();
-  mitk::Point2D imageArea;
-  imageArea[0] = dimX*interPixelDistance[0];
-  imageArea[1] = dimY*interPixelDistance[1];
-  cameraIntrinics->SetImageArea(imageArea);
-  cameraIntrinics->SetFocalLength(10);
-  mitk::Point2D pixelSize;
-  pixelSize[0] = interPixelDistance[0];
-  pixelSize[1] = interPixelDistance[1];
-  cameraIntrinics->SetInterPixelDistance(pixelSize);
-  mitk::Point2D imageShift;
-  imageShift[0] = (principalPoint[0] - dimX/2)*interPixelDistance[0];
-  imageShift[1] = (principalPoint[1] - dimX/2)*interPixelDistance[1];
-  cameraIntrinics->SetImageShift(imageShift);
-  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 = camera->GetPrincipalPoint();
-  MITK_TEST_CONDITION_REQUIRED(mitk::Equal(principalPoint,pp),"Testing SetIntrinsicParameters() - principal point");
+  //initialize intrinsic parameters with some arbitrary values
+  mitk::ToFProcessingCommon::ToFPoint2D interPixelDistance;
+  interPixelDistance[0] = 0.04564;
+  interPixelDistance[1] = 0.0451564;
+  mitk::ToFProcessingCommon::ToFScalarType focalLengthX = 295.78960;
+  mitk::ToFProcessingCommon::ToFScalarType focalLengthY = 296.348535;
+  mitk::ToFProcessingCommon::ToFScalarType focalLength = (focalLengthX*interPixelDistance[0]+focalLengthY*interPixelDistance[1])/2.0;
+  mitk::ToFProcessingCommon::ToFScalarType k1=-0.36,k2=-0.14,p1=0.001,p2=-0.00;
+  mitk::ToFProcessingCommon::ToFPoint2D principalPoint;
+  principalPoint[0] = 103.576546;
+  principalPoint[1] = 100.1532;
+  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");
+  mitk::ToFProcessingCommon::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()
+
+  filter->SetInterPixelDistance(interPixelDistance);
+  mitk::ToFProcessingCommon::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 without subset";
   mitk::PointSet::Pointer expectedResult = mitk::PointSet::New();
   unsigned int counter = 0;
   for (unsigned int i=0; i<dimX; i++)
   {
     for (unsigned int j=0; j<dimY; j++)
     {
       mitk::Index3D index;
       index[0] = i;
       index[1] = j;
       index[2] = 0;
       mitk::ScalarType distance = image->GetPixelValueByIndex(index);
-      mitk::Point3D coordinate = mitk::ToFProcessingCommon::IndexToCartesianCoordinates(i,j,distance,cameraIntrinics->GetFocalLength(),cameraIntrinics->GetInterPixelDistance(),
-                                                                                        cameraIntrinics->GetPrincipalPoint());
+      mitk::Point3D coordinate = mitk::ToFProcessingCommon::IndexToCartesianCoordinates(i,j,distance,focalLength,interPixelDistance,principalPoint);
       expectedResult->InsertPoint(counter,coordinate);
       counter++;
     }
   }
   filter->Update();
   mitk::PointSet::Pointer result = filter->GetOutput();
   MITK_TEST_CONDITION_REQUIRED((expectedResult->GetSize()==result->GetSize()),"Test if point set size is equal");
   bool pointSetsEqual = true;
   for (unsigned int i=0; i<expectedResult->GetSize(); i++)
   {
     mitk::Point3D expectedPoint = expectedResult->GetPoint(i);
     mitk::Point3D resultPoint = result->GetPoint(i);
     if (!mitk::Equal(expectedPoint,resultPoint))
     {
-      MITK_INFO << "erwartet " << expectedPoint;
-      MITK_INFO << "result " << resultPoint;
+      //MITK_INFO << "erwartet " << expectedPoint;
+      //MITK_INFO << "result " << resultPoint;
       pointSetsEqual = false;
     }
   }
   MITK_TEST_CONDITION_REQUIRED(pointSetsEqual,"Testing filter without subset");
 
   // test filter with subset
   MITK_INFO<<"Test filter with subset";
   filter = mitk::ToFDistanceImageToPointSetFilter::New();
   filter->SetInput(image);
-  filter->SetCameraModel(cameraIntrinics);
+  filter->SetInterPixelDistance(interPixelDistance);
+  filter->SetCameraIntrinsics(cameraIntrinsics);
   expectedResult = mitk::PointSet::New();
   counter = 0;
   for (unsigned int i=0; i<subSet->GetSize(); i++)
   {
     mitk::Point3D point = subSet->GetPoint(i);
     mitk::Index3D index;
     index[0] = point[0];
     index[1] = point[1];
     index[2] = 0;
     mitk::ScalarType distance = image->GetPixelValueByIndex(index);
     mitk::Point3D coordinate = mitk::ToFProcessingCommon::IndexToCartesianCoordinates(point[0],point[1],
-                                                                                      distance,cameraIntrinics->GetFocalLength(),cameraIntrinics->GetInterPixelDistance(),
-                                                                                      cameraIntrinics->GetPrincipalPoint());
+                                                                                      distance,focalLength,interPixelDistance,principalPoint);
     expectedResult->InsertPoint(counter,coordinate);
     counter++;
   }
   filter->SetSubset(subSet);
   filter->Modified();
   filter->Update();
   result = filter->GetOutput();
   MITK_TEST_CONDITION_REQUIRED((expectedResult->GetSize()==result->GetSize()),"Test if point set size is equal");
   pointSetsEqual = true;
   for (unsigned int i=0; i<expectedResult->GetSize(); i++)
   {
     mitk::Point3D expectedPoint = expectedResult->GetPoint(i);
     mitk::Point3D resultPoint = result->GetPoint(i);
     if (!mitk::Equal(expectedPoint,resultPoint))
     {
       pointSetsEqual = false;
     }
   }
   MITK_TEST_CONDITION_REQUIRED(pointSetsEqual,"Testing filter without subset");
   MITK_TEST_END();
 
 }
 
diff --git a/Modules/ToFProcessing/Testing/mitkToFDistanceImageToSurfaceFilterTest.cpp b/Modules/ToFProcessing/Testing/mitkToFDistanceImageToSurfaceFilterTest.cpp
index 901c061aed..7a1a4845c2 100644
--- a/Modules/ToFProcessing/Testing/mitkToFDistanceImageToSurfaceFilterTest.cpp
+++ b/Modules/ToFProcessing/Testing/mitkToFDistanceImageToSurfaceFilterTest.cpp
@@ -1,221 +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 focalLengthX = 13.654368;
-  ToFScalarType focalLengthY = 13.854;
+  ToFScalarType focalLengthX = 295.78960;
+  ToFScalarType focalLengthY = 296.348535;
   ToFScalarType k1=-0.36,k2=-0.14,p1=0.001,p2=-0.00;
   ToFPoint2D principalPoint;
   principalPoint[0] = 103.576546;
   principalPoint[1] = 100.1532;
   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,focalLength,interPixelDistance,principalPoint);
 
     ToFPoint3D resultPointBackward =
         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,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/mitkToFDistanceImageToPointSetFilter.cpp b/Modules/ToFProcessing/mitkToFDistanceImageToPointSetFilter.cpp
index a8ea197028..89e00f14ee 100644
--- a/Modules/ToFProcessing/mitkToFDistanceImageToPointSetFilter.cpp
+++ b/Modules/ToFProcessing/mitkToFDistanceImageToPointSetFilter.cpp
@@ -1,167 +1,176 @@
 /*=========================================================================
 
 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 "mitkToFDistanceImageToPointSetFilter.h"
 
 #include "mitkPointSet.h"
 #include "mitkToFProcessingCommon.h"
 
 mitk::ToFDistanceImageToPointSetFilter::ToFDistanceImageToPointSetFilter()
-: m_CameraModel(), m_Subset(NULL)
+: m_CameraIntrinsics(), m_Subset(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::ToFDistanceImageToPointSetFilter::~ToFDistanceImageToPointSetFilter()
 {
 }
 
 void mitk::ToFDistanceImageToPointSetFilter::SetInput(const mitk::Image* distanceImage )
 {
   this->SetInput(0,distanceImage);
 }
 
 void mitk::ToFDistanceImageToPointSetFilter::SetInput( unsigned int idx,const 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, const_cast<mitk::Image*>(distanceImage));   // Process object is not const-correct so the const_cast is required here
     this->CreateOutputsForAllInputs();
   }
 }
 
 mitk::Image* mitk::ToFDistanceImageToPointSetFilter::GetInput()
 {
   return this->GetInput(0);
 }
 
 mitk::Image* mitk::ToFDistanceImageToPointSetFilter::GetInput( unsigned int idx )
 {
   if (this->GetNumberOfInputs() < 1)
     return NULL;
 
   return static_cast< mitk::Image*>(this->ProcessObject::GetInput(idx));
 }
 
 void mitk::ToFDistanceImageToPointSetFilter::SetSubset(std::vector<mitk::Index3D> subset)
 {
   // check if points of PointSet are inside the input image
   mitk::Image::Pointer input = this->GetInput();
   unsigned int xDim = input->GetDimension(0);
   unsigned int yDim = input->GetDimension(1);
   bool pointSetValid = true;
   for (unsigned int i=0; i<subset.size(); i++)
   {
     mitk::Index3D currentIndex = subset.at(i);
     if (currentIndex[0]<0||currentIndex[0]>xDim||currentIndex[1]<0||currentIndex[1]>yDim)
     {
       pointSetValid = false;
     }
   }
   if (pointSetValid)
   {
     m_Subset = subset;
   }
   else
   {
     MITK_ERROR<<"One or more indizes are located outside the image domain";
   }
 }
 
 void mitk::ToFDistanceImageToPointSetFilter::SetSubset( mitk::PointSet::Pointer pointSet)
 {
   std::vector<mitk::Index3D> subset;
   for (unsigned int i=0; i<pointSet->GetSize(); i++)
   {
     mitk::Point3D currentPoint = pointSet->GetPoint(i);
     mitk::Index3D currentIndex;
     currentIndex[0] = currentPoint[0];
     currentIndex[1] = currentPoint[1];
     currentIndex[2] = currentPoint[2];
     subset.push_back(currentIndex);
   }
   this->SetSubset(subset);
 }
 
 void mitk::ToFDistanceImageToPointSetFilter::GenerateData()
 {
+  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::PointSet::Pointer output = this->GetOutput();
   assert(output);
   mitk::Image::Pointer input = this->GetInput();
   assert(input);
   //compute subset of points if input PointSet is defined
   if (m_Subset.size()!=0)
   {
     for (unsigned int i=0; i<m_Subset.size(); i++)
     {
       mitk::Index3D currentIndex = m_Subset.at(i);;
-      ToFScalarType distance = (double)input->GetPixelValueByIndex(currentIndex);
+      mitk::ToFProcessingCommon::ToFScalarType distance = (double)input->GetPixelValueByIndex(currentIndex);
+
       mitk::Point3D currentPoint =
-          mitk::ToFProcessingCommon::IndexToCartesianCoordinates(currentIndex,distance,m_CameraModel->GetFocalLength(),
-                                                                 m_CameraModel->GetInterPixelDistance(),m_CameraModel->GetPrincipalPoint());
+          mitk::ToFProcessingCommon::IndexToCartesianCoordinates(currentIndex,distance,focalLength,m_InterPixelDistance,principalPoint);
       output->InsertPoint(i,currentPoint);
     }
   }
   else //compute PointSet holding cartesian coordinates for every image point
   {
     unsigned int xDimension = input->GetDimension(0);
     unsigned int yDimension = input->GetDimension(1);
     int pointCount = 0;
     for (int i=0; i<xDimension; i++)
     {
       for (int j=0; j<yDimension; j++)
       {
         mitk::Index3D pixel;
         pixel[0] = i;
         pixel[1] = j;
         pixel[2] = 0;
-        ToFScalarType distance = (double)input->GetPixelValueByIndex(pixel);
+        mitk::ToFProcessingCommon::ToFScalarType distance = (double)input->GetPixelValueByIndex(pixel);
         mitk::Point3D currentPoint = 
-          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);
 
         if (distance!=0)
         {
           output->InsertPoint( pointCount, currentPoint );
           pointCount++;
         }
       }
     }
   }
 }
 
 void mitk::ToFDistanceImageToPointSetFilter::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::ToFDistanceImageToPointSetFilter::GenerateOutputInformation()
 {
   this->GetOutput();
   itkDebugMacro(<<"GenerateOutputInformation()");
 }
diff --git a/Modules/ToFProcessing/mitkToFDistanceImageToPointSetFilter.h b/Modules/ToFProcessing/mitkToFDistanceImageToPointSetFilter.h
index d39a1698ff..f78be3ea64 100644
--- a/Modules/ToFProcessing/mitkToFDistanceImageToPointSetFilter.h
+++ b/Modules/ToFProcessing/mitkToFDistanceImageToPointSetFilter.h
@@ -1,122 +1,125 @@
 /*=========================================================================
 
 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 __mitkToFDistanceImageToPointSetFilter_h
 #define __mitkToFDistanceImageToPointSetFilter_h
 
 
+#include <mitkCameraIntrinsics.h>
 #include "mitkImage.h"
 #include "mitkPointSet.h"
 #include <mitkPointSetSource.h> 
 #include "mitkImageSource.h"
-#include <mitkPinholeCameraModel.h>
+#include <mitkToFProcessingCommon.h>
 #include "mitkToFProcessingExports.h"
 
 namespace mitk
 {
   /**
   * @brief Converts a Time-of-Flight (ToF) distance image to a PointSet using the pinhole camera model for coordinate computation.
   * The intrinsic parameters of the camera (FocalLength, PrincipalPoint, InterPixelDistance) are set via SetIntrinsicParameters(). The
   * measured distance for each pixel corresponds to the distance between the object point and the corresponding image point on the
   * image plane.
   * If a subset of indizes of the image is defined via SetSubset(), the output PointSet will only contain the cartesian coordinates
   * of the corresponding 3D points.
   *
   * <img src="PinholeCameraModel.png" alt="Pinhole camera model"> 
   * <img src="ImagePlane.png" alt="Image plane"> 
   *
   * @ingroup SurfaceFilters
   * @ingroup ToFProcessing
   */
   class mitkToFProcessing_EXPORT ToFDistanceImageToPointSetFilter : public PointSetSource
   {
   public:
-    typedef mitk::PinholeCameraModel::ToFScalarType ToFScalarType;
 
     mitkClassMacro( ToFDistanceImageToPointSetFilter , PointSetSource );
     itkNewMacro( Self );
 
-    itkSetMacro(CameraModel,mitk::PinholeCameraModel::Pointer);
+    itkSetMacro(CameraIntrinsics,mitk::CameraIntrinsics::Pointer);
 
-    itkGetMacro(CameraModel,mitk::PinholeCameraModel::Pointer);
+    itkGetMacro(CameraIntrinsics,mitk::CameraIntrinsics::Pointer);
+    itkSetMacro(InterPixelDistance,mitk::ToFProcessingCommon::ToFPoint2D);
+    itkGetMacro(InterPixelDistance,mitk::ToFProcessingCommon::ToFPoint2D);
 
 
     /*!
     \brief Sets the input of this filter
     \param distanceImage input is the distance image of e.g. a ToF camera
     */
     virtual void SetInput(const Image* distanceImage);
 
     /*!
     \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,const Image* distanceImage);
 
     /*!
     \brief Returns the input of this filter
     */
      Image* GetInput();
    
     /*!
     \brief Returns the input with id idx of this filter
     */
      Image* GetInput(unsigned int idx);
 
     /*!
     \brief If this subset is defined, the cartesian coordinates are only computed for the contained indizes. Make sure the indizes are contained in the input image
     \param subset index subset specified in index coordinates.
     */
     void SetSubset( std::vector<Index3D> subset);
 
     /*!
     \brief Sets the subset of indizes used for caluclation of output PointSet as a PointSet.
     Warning: make sure the points in your PointSet are index coordinates.
     \param PointSet specified in index coordinates.
     */
     void SetSubset( mitk::PointSet::Pointer pointSet);
 
   protected:
     /*!
     \brief Standard constructor
     */
     ToFDistanceImageToPointSetFilter();
     /*!
     \brief Standard destructor
     */
     ~ToFDistanceImageToPointSetFilter();
     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();
 
     std::vector<Index3D> m_Subset; ///< If this subset is specified only the contained indizes are converted to cartesian coordinates
-    mitk::PinholeCameraModel::Pointer m_CameraModel; ///< Pinhole camera model holding the intrinsic parameters needed for PointSet calculation
+    mitk::CameraIntrinsics::Pointer m_CameraIntrinsics; ///< Member holding the intrinsic parameters needed for PointSet calculation
+    ToFProcessingCommon::ToFPoint2D m_InterPixelDistance; ///< distance in mm between two adjacent pixels on the ToF camera chip
   };
 } //END mitk namespace
 #endif