diff --git a/Modules/ToFProcessing/mitkToFDistanceImageToPointSetFilter.cpp b/Modules/ToFProcessing/mitkToFDistanceImageToPointSetFilter.cpp
index 2d1c431f7d..129ee21683 100644
--- a/Modules/ToFProcessing/mitkToFDistanceImageToPointSetFilter.cpp
+++ b/Modules/ToFProcessing/mitkToFDistanceImageToPointSetFilter.cpp
@@ -1,178 +1,200 @@
 /*===================================================================
 
 The Medical Imaging Interaction Toolkit (MITK)
 
 Copyright (c) German Cancer Research Center, 
 Division of Medical and Biological Informatics.
 All rights reserved.
 
 This software is distributed WITHOUT ANY WARRANTY; without 
 even the implied warranty of MERCHANTABILITY or FITNESS FOR 
 A PARTICULAR PURPOSE.
 
 See LICENSE.txt or http://www.mitk.org for details.
 
 ===================================================================*/
 
 #include "mitkToFDistanceImageToPointSetFilter.h"
 
 #include "mitkImageDataItem.h"
 #include "mitkPointSet.h"
 #include "mitkToFProcessingCommon.h"
 
 mitk::ToFDistanceImageToPointSetFilter::ToFDistanceImageToPointSetFilter()
 : m_Subset(NULL), m_CameraIntrinsics(), m_InterPixelDistance()
 {
   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 (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;
+  //calculate world coordinates
+  mitk::ToFProcessingCommon::ToFPoint2D focalLengthInPixelUnits;
+  mitk::ToFProcessingCommon::ToFScalarType focalLengthInMm;
+  if (m_ReconstructionMode)
+  {
+    focalLengthInPixelUnits[0] = m_CameraIntrinsics->GetFocalLengthX();
+    focalLengthInPixelUnits[1] = m_CameraIntrinsics->GetFocalLengthY();
+  }
+  else
+    mitk::ToFProcessingCommon::ToFScalarType focalLengthInMm = (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);;
       mitk::ToFProcessingCommon::ToFScalarType distance = (double)input->GetPixelValueByIndex(currentIndex);
 
-      mitk::Point3D currentPoint =
-          mitk::ToFProcessingCommon::IndexToCartesianCoordinates(currentIndex,distance,focalLength,m_InterPixelDistance,principalPoint);
+      mitk::Point3D currentPoint;
+      if (m_ReconstructionMode)
+        currentPoint = mitk::ToFProcessingCommon::IndexToCartesianCoordinates(currentIndex,distance,focalLengthInPixelUnits,principalPoint);
+      else
+        currentPoint = mitk::ToFProcessingCommon::IndexToCartesianCoordinatesWithInterpixdist(currentIndex,distance,focalLengthInMm,m_InterPixelDistance,principalPoint);
+
       output->InsertPoint(i,currentPoint);
     }
   }
   else //compute PointSet holding cartesian coordinates for every image point
   {
     int xDimension = (int)input->GetDimension(0);
     int yDimension = (int)input->GetDimension(1);
     int pointCount = 0;
     for (int j=0; j<yDimension; j++)
     {
       for (int i=0; i<xDimension; i++)
       {
         mitk::Index3D pixel;
         pixel[0] = i;
         pixel[1] = j;
         pixel[2] = 0;
 
         mitk::ToFProcessingCommon::ToFScalarType distance = (double)input->GetPixelValueByIndex(pixel);
 
-        mitk::Point3D currentPoint = 
-          mitk::ToFProcessingCommon::IndexToCartesianCoordinates(i,j,distance,focalLength,m_InterPixelDistance,principalPoint);
+      mitk::Point3D currentPoint;
+      if (m_ReconstructionMode)
+        currentPoint = mitk::ToFProcessingCommon::IndexToCartesianCoordinates(i,j,distance,focalLengthInPixelUnits,principalPoint);
+      else
+        currentPoint = mitk::ToFProcessingCommon::IndexToCartesianCoordinatesWithInterpixdist(i,j,distance,focalLengthInMm,m_InterPixelDistance,principalPoint);
 
         if (distance>mitk::eps)
         {
           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()");
 }
+
+void mitk::ToFDistanceImageToPointSetFilter::SetReconstructionMode(bool withoutInterpixdist)
+{
+  this->m_ReconstructionMode = withoutInterpixdist;
+}
diff --git a/Modules/ToFProcessing/mitkToFDistanceImageToPointSetFilter.h b/Modules/ToFProcessing/mitkToFDistanceImageToPointSetFilter.h
index 98515edcd3..518bc37542 100644
--- a/Modules/ToFProcessing/mitkToFDistanceImageToPointSetFilter.h
+++ b/Modules/ToFProcessing/mitkToFDistanceImageToPointSetFilter.h
@@ -1,126 +1,133 @@
 /*===================================================================
 
 The Medical Imaging Interaction Toolkit (MITK)
 
 Copyright (c) German Cancer Research Center, 
 Division of Medical and Biological Informatics.
 All rights reserved.
 
 This software is distributed WITHOUT ANY WARRANTY; without 
 even the implied warranty of MERCHANTABILITY or FITNESS FOR 
 A PARTICULAR PURPOSE.
 
 See LICENSE.txt or http://www.mitk.org for details.
 
 ===================================================================*/
 #ifndef __mitkToFDistanceImageToPointSetFilter_h
 #define __mitkToFDistanceImageToPointSetFilter_h
 
 
 #include <mitkCameraIntrinsics.h>
 #include "mitkImage.h"
 #include "mitkPointSet.h"
 #include <mitkPointSetSource.h> 
 #include "mitkImageSource.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.
   *
   * The coordinate conversion follows the model of a common pinhole camera where the origin of the camera 
   * coordinate system (world coordinates) is at the pinhole
   * \image html ../Modules/ToFProcessing/Documentation/PinholeCameraModel.png
   * The definition of the image plane and its coordinate systems (pixel and mm) is depicted in the following image
   * \image html ../Modules/ToFProcessing/Documentation/ImagePlane.png
   *
   * @ingroup SurfaceFilters
   * @ingroup ToFProcessing
   */
   class mitkToFProcessing_EXPORT ToFDistanceImageToPointSetFilter : public PointSetSource
   {
   public:
 
     mitkClassMacro( ToFDistanceImageToPointSetFilter , PointSetSource );
     itkNewMacro( Self );
 
     itkSetMacro(CameraIntrinsics,mitk::CameraIntrinsics::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);
 
+    /*!
+    \brief Set the reconstruction mode, if using no interpixeldistances and focal lenghts in pixel units (=true) or interpixeldistances and focal length in mm (=false)
+    */
+    void SetReconstructionMode(bool withoutInterpixdist = true);
+
+
   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::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
+    bool m_ReconstructionMode; ///< true = Reconstruction without interpixeldistance and with focal lengths in pixel units. false = Reconstruction with interpixeldistance and with focal length in mm.
   };
 } //END mitk namespace
 #endif
diff --git a/Modules/ToFProcessing/mitkToFDistanceImageToSurfaceFilter.cpp b/Modules/ToFProcessing/mitkToFDistanceImageToSurfaceFilter.cpp
index e00d0e8414..32da083a09 100644
--- a/Modules/ToFProcessing/mitkToFDistanceImageToSurfaceFilter.cpp
+++ b/Modules/ToFProcessing/mitkToFDistanceImageToSurfaceFilter.cpp
@@ -1,252 +1,269 @@
 /*===================================================================
 
 The Medical Imaging Interaction Toolkit (MITK)
 
 Copyright (c) German Cancer Research Center, 
 Division of Medical and Biological Informatics.
 All rights reserved.
 
 This software is distributed WITHOUT ANY WARRANTY; without 
 even the implied warranty of MERCHANTABILITY or FITNESS FOR 
 A PARTICULAR PURPOSE.
 
 See LICENSE.txt or http://www.mitk.org for details.
 
 ===================================================================*/
 
 #include <mitkToFDistanceImageToSurfaceFilter.h>
 #include <mitkInstantiateAccessFunctions.h>
 #include <mitkSurface.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_IplScalarImage(NULL), m_CameraIntrinsics(), m_TextureImageWidth(0), m_TextureImageHeight(0), m_InterPixelDistance(), m_TextureIndex(0)
 {
   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::CameraIntrinsics::Pointer cameraIntrinsics )
 {
   this->SetCameraIntrinsics(cameraIntrinsics);
   this->SetInput(0,distanceImage);
 }
 
 void mitk::ToFDistanceImageToSurfaceFilter::SetInput( unsigned int idx,  Image* distanceImage, mitk::CameraIntrinsics::Pointer cameraIntrinsics )
 {
   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
   int xDimension = input->GetDimension(0);
   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->GetInput(m_TextureIndex)) // otherwise use intensity image (input(2))
   {
     scalarFloatData = (float*)this->GetInput(m_TextureIndex)->GetData();
   }
 
   float* inputFloatData = (float*)(input->GetSliceData(0, 0, 0)->GetData());
   //calculate world coordinates
-  mitk::ToFProcessingCommon::ToFScalarType focalLength = (m_CameraIntrinsics->GetFocalLengthX()*m_InterPixelDistance[0]+m_CameraIntrinsics->GetFocalLengthY()*m_InterPixelDistance[1])/2.0;
+  mitk::ToFProcessingCommon::ToFPoint2D focalLengthInPixelUnits;
+  mitk::ToFProcessingCommon::ToFScalarType focalLengthInMm;
+  if (m_ReconstructionMode)
+  {
+    focalLengthInPixelUnits[0] = m_CameraIntrinsics->GetFocalLengthX();
+    focalLengthInPixelUnits[1] = m_CameraIntrinsics->GetFocalLengthY();
+  }
+  else
+    mitk::ToFProcessingCommon::ToFScalarType focalLengthInMm = (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();
 
   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]*xDimension;
 
       mitk::ToFProcessingCommon::ToFScalarType distance = (double)inputFloatData[pixelID];
 
-      mitk::ToFProcessingCommon::ToFPoint3D cartesianCoordinates =
-          mitk::ToFProcessingCommon::IndexToCartesianCoordinates(i,j,distance,focalLength,m_InterPixelDistance,principalPoint);
+      mitk::ToFProcessingCommon::ToFPoint3D cartesianCoordinates;
+      if (m_ReconstructionMode)
+        cartesianCoordinates = mitk::ToFProcessingCommon::IndexToCartesianCoordinates(i,j,distance,focalLengthInPixelUnits,principalPoint);
+      else
+        cartesianCoordinates = mitk::ToFProcessingCommon::IndexToCartesianCoordinatesWithInterpixdist(i,j,distance,focalLengthInMm,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(x_1y);
             polys->InsertCellPoint(xy);
             polys->InsertCellPoint(x_1y_1);
 
             polys->InsertNextCell(3);
             polys->InsertCellPoint(x_1y_1);
             polys->InsertCellPoint(xy);
             polys->InsertCellPoint(xy_1);
           }
         }
 
         if (scalarFloatData)
         {          
           scalarArray->InsertTuple1(pixelID, scalarFloatData[pixel[0]+pixel[1]*xDimension]);
           //scalarArray->InsertTuple1(pixelID, scalarFloatData[pixelID]);
         }
         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;
 }
+
+void mitk::ToFDistanceImageToSurfaceFilter::SetReconstructionMode(bool withoutInterpixdist)
+{
+  this->m_ReconstructionMode = withoutInterpixdist;
+}
diff --git a/Modules/ToFProcessing/mitkToFDistanceImageToSurfaceFilter.h b/Modules/ToFProcessing/mitkToFDistanceImageToSurfaceFilter.h
index 7d2c9d34de..9ffda37de9 100644
--- a/Modules/ToFProcessing/mitkToFDistanceImageToSurfaceFilter.h
+++ b/Modules/ToFProcessing/mitkToFDistanceImageToSurfaceFilter.h
@@ -1,153 +1,158 @@
 /*===================================================================
 
 The Medical Imaging Interaction Toolkit (MITK)
 
 Copyright (c) German Cancer Research Center, 
 Division of Medical and Biological Informatics.
 All rights reserved.
 
 This software is distributed WITHOUT ANY WARRANTY; without 
 even the implied warranty of MERCHANTABILITY or FITNESS FOR 
 A PARTICULAR PURPOSE.
 
 See LICENSE.txt or http://www.mitk.org for details.
 
 ===================================================================*/
 #ifndef __mitkToFDistanceImageToSurfaceFilter_h
 #define __mitkToFDistanceImageToSurfaceFilter_h
 
 
 #include <mitkImage.h>
 #include <mitkSurfaceSource.h>
 #include <mitkToFProcessingExports.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.
   *
   * The coordinate conversion follows the model of a common pinhole camera where the origin of the camera 
   * coordinate system (world coordinates) is at the pinhole
   * \image html ../Modules/ToFProcessing/Documentation/PinholeCameraModel.png
   * The definition of the image plane and its coordinate systems (pixel and mm) is depicted in the following image
   * \image html ../Modules/ToFProcessing/Documentation/ImagePlane.png
   *
   * @ingroup SurfaceFilters
   * @ingroup ToFProcessing
   */
   class mitkToFProcessing_EXPORT ToFDistanceImageToSurfaceFilter : public SurfaceSource
   {
   public:
 
     mitkClassMacro( ToFDistanceImageToSurfaceFilter , SurfaceSource );
     itkNewMacro( Self );
 
 
     itkSetMacro(CameraIntrinsics, mitk::CameraIntrinsics::Pointer);
     itkGetMacro(CameraIntrinsics, mitk::CameraIntrinsics::Pointer);
     itkSetMacro(InterPixelDistance,ToFProcessingCommon::ToFPoint2D);
     itkGetMacro(InterPixelDistance,ToFProcessingCommon::ToFPoint2D);
     itkSetMacro(TextureIndex,int);
     /*!
     \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 Set the reconstruction mode, if using no interpixeldistances and focal lenghts in pixel units (=true) or interpixeldistances and focal length in mm (=false)
+    */
+    void SetReconstructionMode(bool withoutInterpixdist = true);
+    /*!
     \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::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 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::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::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
 
     int m_TextureIndex; ///< Index of the input used as texture image when no scalar image was set via SetIplScalarImage(). 0 = Distance, 1 = Amplitude, 2 = Intensity
 
+    bool m_ReconstructionMode; ///< true = Reconstruction without interpixeldistance and with focal lengths in pixel units. false = Reconstruction with interpixeldistance and with focal length in mm.
   };
 } //END mitk namespace
 #endif
diff --git a/Modules/ToFProcessing/mitkToFProcessingCommon.cpp b/Modules/ToFProcessing/mitkToFProcessingCommon.cpp
index 65a23c83f7..fbb762928e 100644
--- a/Modules/ToFProcessing/mitkToFProcessingCommon.cpp
+++ b/Modules/ToFProcessing/mitkToFProcessingCommon.cpp
@@ -1,123 +1,124 @@
 /*===================================================================
 
 The Medical Imaging Interaction Toolkit (MITK)
 
 Copyright (c) German Cancer Research Center, 
 Division of Medical and Biological Informatics.
 All rights reserved.
 
 This software is distributed WITHOUT ANY WARRANTY; without 
 even the implied warranty of MERCHANTABILITY or FITNESS FOR 
 A PARTICULAR PURPOSE.
 
 See LICENSE.txt or http://www.mitk.org for details.
 
 ===================================================================*/
 #include "mitkToFProcessingCommon.h"
 
 namespace mitk
 {
   ToFProcessingCommon::ToFPoint3D ToFProcessingCommon::IndexToCartesianCoordinates(unsigned int i, unsigned int j, ToFScalarType distance,
     ToFScalarType focalLengthX, ToFScalarType focalLengthY, ToFScalarType principalPointX, ToFScalarType principalPointY)
   {
     ToFPoint3D cartesianCoordinates;
 
     // calculate image coordinates in pixel units;
     // Note: pixel unit (pX) in x direction does normally not equal pixel unit (pY) in y direction.
     // Therefore, a transformation in one of the pixel units is necessary
     // Here, pX as image coordinate unit is chosen
     // pY = (focalLengthX / focalLengthY) * pX
     ToFScalarType imageX = i - principalPointX;
     ToFScalarType imageY = j - principalPointY;
     ToFScalarType imageY_in_pX = imageY * (focalLengthX / focalLengthY);
 
     //distance from pinhole to pixel (i,j) in pX units (pixel unit in x direction)
     ToFScalarType d_in_pX = sqrt(imageX*imageX + imageY_in_pX*imageY_in_pX + focalLengthX*focalLengthX);
 
     cartesianCoordinates[0] = distance * imageX / d_in_pX; //Strahlensatz: x / imageX = distance / d
     cartesianCoordinates[1] = distance * imageY_in_pX / d_in_pX; //Strahlensatz: y / imageY = distances / d
     cartesianCoordinates[2] = distance * focalLengthX / d_in_pX; //Strahlensatz: z / f = distance / d.
 
     return cartesianCoordinates;
   }
 
-//deprecated
+
   ToFProcessingCommon::ToFPoint3D ToFProcessingCommon::IndexToCartesianCoordinatesWithInterpixdist(unsigned int i, unsigned int j, ToFScalarType distance, ToFScalarType focalLength,
     ToFScalarType interPixelDistanceX, ToFScalarType interPixelDistanceY,
     ToFScalarType principalPointX, ToFScalarType principalPointY)
   {
     ToFPoint3D cartesianCoordinates;
 
     // calculate image coordinates in mm;
     ToFScalarType imageX = (( i - principalPointX ) * interPixelDistanceX);
     ToFScalarType imageY = (( j - principalPointY ) * interPixelDistanceY);
 
     //distance from pinhole to pixel
     ToFScalarType d = sqrt(imageX*imageX + imageY*imageY + focalLength*focalLength);
 
     cartesianCoordinates[0] = (distance)*imageX / d; //Strahlensatz: x / imageX = (distance) / d
     cartesianCoordinates[1] = (distance)*imageY / d; //Strahlensatz: y / imageY = (distance) / d
     cartesianCoordinates[2] = ((distance*focalLength) / d); //Strahlensatz: z / f = distance / d.
 
     return cartesianCoordinates;
   }
 
 
+
   ToFProcessingCommon::ToFPoint3D ToFProcessingCommon::CartesianToIndexCoordinates(ToFScalarType cartesianPointX, ToFScalarType cartesianPointY,ToFScalarType cartesianPointZ,
     ToFScalarType focalLengthX, ToFScalarType focalLengthY,
     ToFScalarType principalPointX, ToFScalarType principalPointY, bool calculateDistance)
   {
     ToFPoint3D indexCoordinatesAndDistanceValue;
 
     ToFScalarType imageX = cartesianPointX*focalLengthX/cartesianPointZ; //Strahlensatz: cartesianPointX / imageX = cartesianPointZ / focalLengthX
     ToFScalarType imageY = cartesianPointY*focalLengthY/cartesianPointZ; //Strahlensatz: cartesianPointY / imageY = cartesianPointZ / focalLengthY
 
     indexCoordinatesAndDistanceValue[0] = imageX + principalPointX;
     indexCoordinatesAndDistanceValue[1] = imageY + principalPointY;
 
     // Note: pixel unit (pX) in x direction does normally not equal pixel unit (pY) in y direction.
-    // Therefore, a transformation in one of the pixel units is necessary
+    // Therefore, a transformation in one of the pixel units is necessary (for calculation of the distance value only)
     // Here, pX as image coordinate unit is chosen
     // pY = (focalLengthX / focalLengthY) * pX
     ToFScalarType imageY_in_pX = imageY * focalLengthX/focalLengthY;
 
     //distance from pinhole to pixel
     ToFScalarType d_in_pX = sqrt(imageX*imageX + imageY_in_pX*imageY_in_pX + focalLengthX*focalLengthX);
 
     if (calculateDistance)
     {
-      indexCoordinatesAndDistanceValue[2] = d*(cartesianPointZ) / focalLengthX;
+      indexCoordinatesAndDistanceValue[2] = d_in_pX*(cartesianPointZ) / focalLengthX;
     }
     else
     {
       indexCoordinatesAndDistanceValue[2] = 0.0;
     }
     return indexCoordinatesAndDistanceValue;
   }
 
-//deprecated
+
   ToFProcessingCommon::ToFPoint3D ToFProcessingCommon::CartesianToIndexCoordinatesWithInterpixdist(ToFScalarType cartesianPointX, ToFScalarType cartesianPointY,ToFScalarType cartesianPointZ,
     ToFScalarType focalLength, ToFScalarType interPixelDistanceX, ToFScalarType interPixelDistanceY, 
     ToFScalarType principalPointX, ToFScalarType principalPointY, bool calculateDistance)
   {
     ToFPoint3D indexCoordinatesAndDistanceValue;
 
     ToFScalarType imageX = cartesianPointX*focalLength/cartesianPointZ;
     ToFScalarType imageY = cartesianPointY*focalLength/cartesianPointZ;
 
     indexCoordinatesAndDistanceValue[0] = imageX/interPixelDistanceX + principalPointX;
     indexCoordinatesAndDistanceValue[1] = imageY/interPixelDistanceY + principalPointY;
 
     ToFScalarType d = sqrt(imageX*imageX + imageY*imageY + focalLength*focalLength);
 
     if (calculateDistance)
     {
       indexCoordinatesAndDistanceValue[2] = d*(cartesianPointZ) / focalLength;
     }
     else
     {
       indexCoordinatesAndDistanceValue[2] = 0.0;
     }
     return indexCoordinatesAndDistanceValue;
   }
 }
diff --git a/Modules/ToFProcessing/mitkToFProcessingCommon.h b/Modules/ToFProcessing/mitkToFProcessingCommon.h
index 80cffe98f4..1d4a0807bb 100644
--- a/Modules/ToFProcessing/mitkToFProcessingCommon.h
+++ b/Modules/ToFProcessing/mitkToFProcessingCommon.h
@@ -1,219 +1,267 @@
 /*===================================================================
 
 The Medical Imaging Interaction Toolkit (MITK)
 
 Copyright (c) German Cancer Research Center, 
 Division of Medical and Biological Informatics.
 All rights reserved.
 
 This software is distributed WITHOUT ANY WARRANTY; without 
 even the implied warranty of MERCHANTABILITY or FITNESS FOR 
 A PARTICULAR PURPOSE.
 
 See LICENSE.txt or http://www.mitk.org for details.
 
 ===================================================================*/
 
 #ifndef MITKTOFPROCESSINGCOMMON_H
 #define MITKTOFPROCESSINGCOMMON_H
 
 #include "mitkToFProcessingExports.h"
 #include "mitkVector.h"
 #include <vnl/vnl_math.h>
 
 
 
 namespace mitk
 {
   /**
   * @brief Helper class providing functions which are useful for multiple usage
   *
   * Currently the following methods are provided:
   * <ul>
   * <li> Conversion from 2D image coordinates to 3D world coordinates (IndexToCartesianCoordinates())
   * <li> Conversion from 3D world coordinates to 2D image coordinates (CartesianToIndexCoordinates())
   * </ul>
   * The coordinate conversion follows the model of a common pinhole camera where the origin of the camera 
   * coordinate system (world coordinates) is at the pinhole
   * \image html ../Modules/ToFProcessing/Documentation/PinholeCameraModel.png
   * The definition of the image plane and its coordinate systems (pixel and mm) is depicted in the following image
   * \image html ../Modules/ToFProcessing/Documentation/ImagePlane.png
   * @ingroup ToFProcessing
   */
   class mitkToFProcessing_EXPORT ToFProcessingCommon
   {
   public:
     typedef double ToFScalarType;
     typedef itk::Point<ToFScalarType, 2> ToFPoint2D;
     typedef itk::Point<ToFScalarType, 3> ToFPoint3D;
     typedef itk::Vector<ToFScalarType, 2> ToFVector2D;
     typedef itk::Vector<ToFScalarType, 3> ToFVector3D;
 
     /*!
     \brief Convert index based distances to cartesian coordinates
     \param i index in x direction of image plane
     \param j index in y direction of image plane
     \param distance distance value at given index in mm
     \param focalLength focalX length of optical system in pixel units in x-direction (mostly obtained from camera calibration)
     \param focalLength focalY length of optical system in pixel units in y-direction (mostly obtained from camera calibration)
     \param principalPointX x coordinate of principal point on image plane in pixel
     \param principalPointY y coordinate of principal point on image plane in pixel
     \return cartesian coordinates for current index will be written here
     */  
     static ToFPoint3D IndexToCartesianCoordinates(unsigned int i, unsigned int j, ToFScalarType distance,
       ToFScalarType focalLengthX, ToFScalarType focalLengthY, ToFScalarType principalPointX, ToFScalarType principalPointY);
 
     /*!
     \brief Convert index based distances to cartesian coordinates
     \param i index in x direction of image plane
     \param j index in y direction of image plane
     \param distance distance value at given index in mm
     \param focalLength focal length of optical system in pixel units (mostly obtained from camera calibration)
     \param principalPoint coordinates of principal point on image plane in pixel
     \return cartesian coordinates for current index will be written here
     */  
     inline static ToFPoint3D IndexToCartesianCoordinates(unsigned int i, unsigned int j, ToFScalarType distance,
       ToFPoint2D focalLength, ToFPoint2D principalPoint)
     {
       return IndexToCartesianCoordinates(i,j,distance,focalLength[0],focalLength[1],principalPoint[0],principalPoint[1]);
     }
 
     /*!
     \brief Convert index based distances to cartesian coordinates
     \param index index coordinates
     \param distance distance value at given index in mm
     \param focalLength focal length of optical system in pixel units (mostly obtained from camera calibration)
     \param principalPoint coordinates of principal point on image plane in pixel
     \return cartesian coordinates for current index will be written here
     */
     inline static ToFPoint3D IndexToCartesianCoordinates(mitk::Index3D index, ToFScalarType distance,
       ToFPoint2D focalLength, ToFPoint2D principalPoint)
     {
       return IndexToCartesianCoordinates(index[0],index[1],distance,focalLength[0],focalLength[1],principalPoint[0], principalPoint[1]);
     }
     /*!
     \brief Convenience method to convert index based distances to cartesian coordinates using array as input
     \param i index in x direction of image plane
     \param j index in y direction of image plane
     \param distance distance value at given index in mm
     \param focalLength focal length of optical system in pixel units (mostly obtained from camera calibration)
     \param principalPoint coordinates of principal point on image plane in pixel
     \return cartesian coordinates for current index will be written here
     */
     inline static ToFPoint3D IndexToCartesianCoordinates(unsigned int i, unsigned int j, ToFScalarType distance,
       ToFScalarType focalLength[2], ToFScalarType principalPoint[2])
     {
       return IndexToCartesianCoordinates(i,j,distance,focalLength[0],focalLength[1],principalPoint[0],principalPoint[1]);
     }
 
 
     /*!
     \brief Convert index based distances to cartesian coordinates
     \param i index in x direction of image plane
     \param j index in y direction of image plane
     \param distance distance value at given index in mm
     \param focalLength focal length of optical system in mm (mostly obtained from camera calibration)
     \param interPixelDistanceX distance in x direction between adjacent pixels in mm
     \param interPixelDistanceY distance in y direction between adjacent pixels in mm
     \param principalPointX x coordinate of principal point on image plane in pixel
     \param principalPointY y coordinate of principal point on image plane in pixel
     \return cartesian coordinates for current index will be written here
     */  
     static ToFPoint3D IndexToCartesianCoordinatesWithInterpixdist(unsigned int i, unsigned int j, ToFScalarType distance, ToFScalarType focalLength,
       ToFScalarType interPixelDistanceX, ToFScalarType interPixelDistanceY, ToFScalarType principalPointX, ToFScalarType principalPointY);
 
     /*!
     \brief Convert index based distances to cartesian coordinates
     \param i index in x direction of image plane
     \param j index in y direction of image plane
     \param distance distance value at given index in mm
     \param focalLength focal length of optical system in mm (mostly obtained from camera calibration)
     \param interPixelDistance distance between adjacent pixels in mm
     \param principalPoint coordinates of principal point on image plane in pixel
     \return cartesian coordinates for current index will be written here
     */  
     inline static ToFPoint3D IndexToCartesianCoordinatesWithInterpixdist(unsigned int i, unsigned int j, ToFScalarType distance, ToFScalarType focalLength,
       ToFPoint2D interPixelDistance, ToFPoint2D principalPoint)
     {
       return IndexToCartesianCoordinatesWithInterpixdist(i,j,distance,focalLength,interPixelDistance[0],interPixelDistance[1],principalPoint[0],principalPoint[1]);
     }
     /*!
     \brief Convert index based distances to cartesian coordinates
     \param index index coordinates
     \param distance distance value at given index in mm
     \param focalLength focal length of optical system (mostly obtained from camera calibration)
     \param interPixelDistance distance between adjacent pixels in mm for x and y direction
     \param principalPoint coordinates of principal point on image plane in pixel
     \return cartesian coordinates for current index will be written here
     */
     inline static ToFPoint3D IndexToCartesianCoordinatesWithInterpixdist(mitk::Index3D index, ToFScalarType distance, ToFScalarType focalLength,
       ToFPoint2D interPixelDistance, ToFPoint2D principalPoint)
     {
-      return IndexToCartesianCoordinatesWithInterpixdist(index[0],index[1],distance,focalLength,interPixelDistance,principalPoint);
+      return IndexToCartesianCoordinatesWithInterpixdist(index[0],index[1],distance,focalLength,interPixelDistance[0], interPixelDistance[0],principalPoint[0], principalPoint[0]);
     }
     /*!
     \brief Convenience method to convert index based distances to cartesian coordinates using array as input
     \param i index in x direction of image plane
     \param j index in y direction of image plane
     \param distance distance value at given index in mm
     \param focalLength focal length of optical system in mm (mostly obtained from camera calibration)
     \param interPixelDistance distance between adjacent pixels in mm
     \param principalPoint coordinates of principal point on image plane in pixel
     \return cartesian coordinates for current index will be written here
     */
     inline static ToFPoint3D IndexToCartesianCoordinatesWithInterpixdist(unsigned int i, unsigned int j, ToFScalarType distance, ToFScalarType focalLength,
       ToFScalarType interPixelDistance[2], ToFScalarType principalPoint[2])
     {
       return IndexToCartesianCoordinatesWithInterpixdist(i,j,distance,focalLength,interPixelDistance[0],interPixelDistance[1],principalPoint[0],principalPoint[1]);
     }
+
+
     /*!
-    \brief Convert index based distances to cartesian coordinates
+    \brief Convert cartesian coordinates to index based distances
+    \param cartesianPointX x coordinate of point (of a surface or point set) to convert in 3D coordinates
+    \param cartesianPointY y coordinate of point (of a surface or point set) to convert in 3D coordinates
+    \param cartesianPointZ z coordinate of point (of a surface or point set) to convert in 3D coordinates
+    \param focalLength focalX length of optical system in pixel units in x-direction (mostly obtained from camera calibration)
+    \param focalLength focalY length of optical system in pixel units in y-direction (mostly obtained from camera calibration)
+    \param principalPointX x coordinate of principal point on image plane in pixel
+    \param principalPointY y coordinate of principal point on image plane in pixel
+    \param calculateDistance if this flag is set, the distance value is stored in the z position of the output otherwise z=0
+    \return a ToFPoint3D. (int)ToFPoint3D[0]+0.5 and (int)ToFPoint3D[0]+0.5 will return the x and y index coordinates. ToFPoint3D[2] contains the distance value
+    */ 
+    static ToFPoint3D CartesianToIndexCoordinates(ToFScalarType cartesianPointX, ToFScalarType cartesianPointY,ToFScalarType cartesianPointZ,
+                                                  ToFScalarType focalLengthX, ToFScalarType focalLengthY,
+                                                  ToFScalarType principalPointX, ToFScalarType principalPointY, bool calculateDistance=true);
+
+    /*!
+    \brief Convenience method to convert cartesian coordinates to index based distances using arrays
+    \param cartesianPoint point (of a surface or point set) to convert in 3D coordinates
+    \param focalLength focal length of optical system in pixel units (mostly obtained from camera calibration)
+    \param principalPoint coordinates of principal point on image plane in pixel
+    \param calculateDistance if this flag is set, the distance value is stored in the z position of the output otherwise z=0
+    \return a ToFPoint3D. (int)ToFPoint3D[0]+0.5 and (int)ToFPoint3D[0]+0.5 will return the x and y index coordinates. ToFPoint3D[2] contains the distance value
+    */
+    inline static ToFPoint3D CartesianToIndexCoordinates(ToFScalarType cartesianPoint[3], ToFScalarType focalLength[2],
+                                                         ToFScalarType principalPoint[2], bool calculateDistance=true)
+    {
+      return CartesianToIndexCoordinates(cartesianPoint[0],cartesianPoint[1],cartesianPoint[2],focalLength[0], focalLength[1],
+                                         principalPoint[0],principalPoint[1],calculateDistance);
+    }
+    /*!
+    \brief Convert cartesian coordinates to index based distances
+    \param cartesianPoint point (of a surface or point set) to convert in 3D coordinates
+    \param focalLength focal length of optical system in pixel units (mostly obtained from camera calibration)
+    \param principalPoint coordinates of principal point on image plane in pixel
+    \param calculateDistance if this flag is set, the distance value is stored in the z position of the output otherwise z=0
+    \return a ToFPoint3D. (int)ToFPoint3D[0]+0.5 and (int)ToFPoint3D[0]+0.5 will return the x and y index coordinates. ToFPoint3D[2] contains the distance value
+    */
+    inline static ToFPoint3D CartesianToIndexCoordinates(ToFPoint3D cartesianPoint, ToFPoint2D focalLength,
+      ToFPoint2D principalPoint, bool calculateDistance=true)
+    {
+      return CartesianToIndexCoordinates(cartesianPoint[0],cartesianPoint[1],cartesianPoint[2],focalLength[0], focalLength[1],
+        principalPoint[0],principalPoint[1],calculateDistance);
+    }
+
+
+    /*!
+    \brief Convert cartesian coordinates to index based distances
     \param cartesianPointX x coordinate of point (of a surface or point set) to convert in 3D coordinates
     \param cartesianPointY y coordinate of point (of a surface or point set) to convert in 3D coordinates
     \param cartesianPointZ z coordinate of point (of a surface or point set) to convert in 3D coordinates
     \param focalLength focal length of optical system in mm (mostly obtained from camera calibration)
     \param interPixelDistanceX distance in x direction between adjacent pixels in mm
     \param interPixelDistanceY distance in y direction between adjacent pixels in mm
     \param principalPointX x coordinate of principal point on image plane in pixel
     \param principalPointY y coordinate of principal point on image plane in pixel
     \param calculateDistance if this flag is set, the distance value is stored in the z position of the output otherwise z=0
     \return a ToFPoint3D. (int)ToFPoint3D[0]+0.5 and (int)ToFPoint3D[0]+0.5 will return the x and y index coordinates. ToFPoint3D[2] contains the distance value
     */
-    static ToFPoint3D CartesianToIndexCoordinates(ToFScalarType cartesianPointX, ToFScalarType cartesianPointY,ToFScalarType cartesianPointZ,
+    static ToFPoint3D CartesianToIndexCoordinatesWithInterpixdist(ToFScalarType cartesianPointX, ToFScalarType cartesianPointY,ToFScalarType cartesianPointZ,
                                                   ToFScalarType focalLength, ToFScalarType interPixelDistanceX, ToFScalarType interPixelDistanceY, 
                                                   ToFScalarType principalPointX, ToFScalarType principalPointY, bool calculateDistance=true);
 
     /*!
-    \brief Convenience method to convert index based distances to cartesian coordinates using arrays
+    \brief Convenience method to convert cartesian coordinates to index based distances using arrays
     \param cartesianPoint point (of a surface or point set) to convert in 3D coordinates
     \param focalLength focal length of optical system in mm (mostly obtained from camera calibration)
     \param interPixelDistance distance between adjacent pixels in mm for x and y direction
     \param principalPoint coordinates of principal point on image plane in pixel
     \param calculateDistance if this flag is set, the distance value is stored in the z position of the output otherwise z=0
     \return a ToFPoint3D. (int)ToFPoint3D[0]+0.5 and (int)ToFPoint3D[0]+0.5 will return the x and y index coordinates. ToFPoint3D[2] contains the distance value
     */
-    inline static ToFPoint3D CartesianToIndexCoordinates(ToFScalarType cartesianPoint[3], ToFScalarType focalLength,
+    inline static ToFPoint3D CartesianToIndexCoordinatesWithInterpixdist(ToFScalarType cartesianPoint[3], ToFScalarType focalLength,
                                                          ToFScalarType interPixelDistance[2], ToFScalarType principalPoint[2], 
                                                          bool calculateDistance=true)
     {
-      return CartesianToIndexCoordinates(cartesianPoint[0],cartesianPoint[1],cartesianPoint[2],focalLength,
+      return CartesianToIndexCoordinatesWithInterpixdist(cartesianPoint[0],cartesianPoint[1],cartesianPoint[2],focalLength,
                                          interPixelDistance[0],interPixelDistance[1],principalPoint[0],principalPoint[1],calculateDistance);
     }
     /*!
-    \brief Convert index based distances to cartesian coordinates
+    \brief Convert cartesian coordinates to index based distances
     \param cartesianPoint point (of a surface or point set) to convert in 3D coordinates
     \param focalLength focal length of optical system in mm (mostly obtained from camera calibration)
     \param interPixelDistance distance between adjacent pixels in mm for x and y direction
     \param principalPoint coordinates of principal point on image plane in pixel
     \param calculateDistance if this flag is set, the distance value is stored in the z position of the output otherwise z=0
     \return a ToFPoint3D. (int)ToFPoint3D[0]+0.5 and (int)ToFPoint3D[0]+0.5 will return the x and y index coordinates. ToFPoint3D[2] contains the distance value
     */
-    inline static ToFPoint3D CartesianToIndexCoordinates(ToFPoint3D cartesianPoint, ToFScalarType focalLength,
+    inline static ToFPoint3D CartesianToIndexCoordinatesWithInterpixdist(ToFPoint3D cartesianPoint, ToFScalarType focalLength,
       ToFPoint2D interPixelDistance, ToFPoint2D principalPoint, bool calculateDistance=true)
     {
-      return CartesianToIndexCoordinates(cartesianPoint[0],cartesianPoint[1],cartesianPoint[2],focalLength,
+      return CartesianToIndexCoordinatesWithInterpixdist(cartesianPoint[0],cartesianPoint[1],cartesianPoint[2],focalLength,
         interPixelDistance[0],interPixelDistance[1],principalPoint[0],principalPoint[1],calculateDistance);
     }
   };
 }
 #endif