diff --git a/Modules/ToFProcessing/Documentation/PinholeCameraModel.png b/Modules/ToFProcessing/Documentation/PinholeCameraModel.png
index 5021f74e59..cdd810e9cc 100644
Binary files a/Modules/ToFProcessing/Documentation/PinholeCameraModel.png and b/Modules/ToFProcessing/Documentation/PinholeCameraModel.png differ
diff --git a/Modules/ToFProcessing/Testing/mitkToFProcessingCommonTest.cpp b/Modules/ToFProcessing/Testing/mitkToFProcessingCommonTest.cpp
index 560dc9cfa5..324cb3a57f 100644
--- a/Modules/ToFProcessing/Testing/mitkToFProcessingCommonTest.cpp
+++ b/Modules/ToFProcessing/Testing/mitkToFProcessingCommonTest.cpp
@@ -1,58 +1,58 @@
 /*=========================================================================
 
 Program:   Medical Imaging & Interaction Toolkit
 Language:  C++
 Date:      $Date: 2010-03-12 14:05:50 +0100 (Fr, 12 Mrz 2010) $
 Version:   $Revision: 16010 $
 
 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 <mitkToFProcessingCommon.h>
 
 /**Documentation
  *  test for the class "ToFProcessingCommon".
  */
 int mitkToFProcessingCommonTest(int /* argc */, char* /*argv*/[])
 {
   MITK_TEST_BEGIN("ToFProcessingCommon");
 
   unsigned int i = 10;
   unsigned int j = 50;
   mitk::ScalarType distance = 1000;
   mitk::ScalarType focalLength = 10;
   mitk::Point2D interPixelDistance;
   interPixelDistance[0] = 0.05;
   interPixelDistance[1] = 0.05;
   mitk::Point2D principalPoint;
   principalPoint[0] = 100;
   principalPoint[1] = 100;
   // expected coordinate
   mitk::ToFProcessingCommon::ToFPoint3D expectedCoordinate;
-  expectedCoordinate[0] = -395.5988;
-  expectedCoordinate[1] = -219.7771;
-  expectedCoordinate[2] =  879.1084;
+  expectedCoordinate[0] = -400.0988;
+  expectedCoordinate[1] = -222.2771;
+  expectedCoordinate[2] =  889.1084;
   // resulting coordinate
   mitk::ToFProcessingCommon::ToFPoint3D resultingCoordinate = mitk::ToFProcessingCommon::IndexToCartesianCoordinates(i,j,distance,focalLength,interPixelDistance,principalPoint);
   MITK_TEST_CONDITION_REQUIRED(mitk::Equal(expectedCoordinate,resultingCoordinate),"Testing IndexToCartesianCoordinates()");
   // expected index
   mitk::ToFProcessingCommon::ToFPoint3D expectedIndex;
   expectedIndex[0] = i;
   expectedIndex[1] = j;
   expectedIndex[2] = 1000;
   mitk::ToFProcessingCommon::ToFPoint3D resultingIndex = mitk::ToFProcessingCommon::CartesianToIndexCoordinates(expectedCoordinate,focalLength,interPixelDistance,principalPoint);
   MITK_TEST_CONDITION_REQUIRED(mitk::Equal(expectedIndex,resultingIndex),"Testing CartesianToIndexCoordinates()");
 
   MITK_TEST_END();
 
 }
 
 
diff --git a/Modules/ToFProcessing/mitkToFProcessingCommon.cpp b/Modules/ToFProcessing/mitkToFProcessingCommon.cpp
index 0bd3bfc5cb..e1e22e7c34 100644
--- a/Modules/ToFProcessing/mitkToFProcessingCommon.cpp
+++ b/Modules/ToFProcessing/mitkToFProcessingCommon.cpp
@@ -1,66 +1,66 @@
 /*=========================================================================
 
 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 "mitkToFProcessingCommon.h"
 
 namespace mitk
 {
   ToFProcessingCommon::ToFPoint3D ToFProcessingCommon::IndexToCartesianCoordinates(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-d)*imageX / d; //Strahlensatz: x / imageX = (distance-d) / d
-    cartesianCoordinates[1] = (distance-d)*imageY / d; //Strahlensatz: y / imageY = (distance-d) / d
-    cartesianCoordinates[2] = ((distance*focalLength) / d) - focalLength; //Strahlensatz: z+f / f = distance / d.
+    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 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) / focalLength;
+      indexCoordinatesAndDistanceValue[2] = d*(cartesianPointZ) / focalLength;
     }
     else
     {
       indexCoordinatesAndDistanceValue[2] = 0.0;
     }
     return indexCoordinatesAndDistanceValue;
   }
 }