diff --git a/Modules/ToFProcessing/Testing/CMakeLists.txt b/Modules/ToFProcessing/Testing/CMakeLists.txt
index c675ff30a9..4b5e2c297b 100644
--- a/Modules/ToFProcessing/Testing/CMakeLists.txt
+++ b/Modules/ToFProcessing/Testing/CMakeLists.txt
@@ -1,10 +1,9 @@
 MITK_CREATE_MODULE_TESTS()
 
 mitkAddCustomModuleTest(mitkKinectReconstructionTest_LegoPhantom mitkKinectReconstructionTest
                         ${MITK_DATA_DIR}/ToF-Data/CalibrationFiles/Kinect_RGB_camera.xml #camera intrinsics
                         ${MITK_DATA_DIR}/ToF-Data/Kinect_Lego_Phantom_DistanceImage.nrrd #kinect distance image
-                        ${MITK_DATA_DIR}/ToF-Data/KinectReconstruction_Lego_Phantom.vtp #ground truth surface
 )
 
 #mitkAddCustomModuleTest(mitkToFImageDownsamplingFilterTest_20 mitkToFImageDownsamplingFilterTest PMDCamCube2_MF0_IT0_20Images_DistanceImage.pic)
 #mitkAddCustomModuleTest(mitkToFImageDownsamplingFilterTest_1 mitkToFImageDownsamplingFilterTest PMDCamCube2_MF0_IT0_1Images_DistanceImage.pic)
diff --git a/Modules/ToFProcessing/Testing/mitkKinectReconstructionTest.cpp b/Modules/ToFProcessing/Testing/mitkKinectReconstructionTest.cpp
index 1cdc7ba2a6..3455c83d20 100644
--- a/Modules/ToFProcessing/Testing/mitkKinectReconstructionTest.cpp
+++ b/Modules/ToFProcessing/Testing/mitkKinectReconstructionTest.cpp
@@ -1,69 +1,98 @@
 /*===================================================================
 
 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 <mitkTestingMacros.h>
 #include <mitkToFDistanceImageToSurfaceFilter.h>
 
 #include <mitkImage.h>
+#include <mitkImagePixelReadAccessor.h>
 #include <mitkSurface.h>
 #include <mitkToFProcessingCommon.h>
 #include <mitkVector.h>
 #include <mitkToFTestingCommon.h>
 #include <mitkIOUtil.h>
 
 #include <vtkPoints.h>
 #include <vtkPolyData.h>
 #include <vtkSmartPointer.h>
 
 /**
  * @brief mitkKinectReconstructionTest Testing method for the Kinect reconstruction mode. Specially meant for Kinect.
- * This tests loads a special data set from MITK-Data and compares it to a previously generated surface.
+ * This tests loads a special data set from MITK-Data and compares it to a reference surface.
  * This test has no dependency to the mitkKinectModule, although it is thematically connected to it.
  */
 int mitkKinectReconstructionTest(int  argc , char* argv[])
 {
     MITK_TEST_BEGIN("mitkKinectReconstructionTest");
 
-    MITK_TEST_CONDITION_REQUIRED(argc > 3, "Testing if enough arguments are set.");
+    MITK_TEST_CONDITION_REQUIRED(argc > 2, "Testing if enough arguments are set.");
     std::string calibrationFilePath(argv[1]);
     std::string kinectImagePath(argv[2]);
-    std::string groundTruthSurfacePath(argv[3]);
 
     mitk::ToFDistanceImageToSurfaceFilter::Pointer distToSurf = mitk::ToFDistanceImageToSurfaceFilter::New();
     mitk::CameraIntrinsics::Pointer intrinsics = mitk::CameraIntrinsics::New();
     //load our personal kinect calibration
     intrinsics->FromXMLFile(calibrationFilePath);
     MITK_TEST_CONDITION_REQUIRED(intrinsics.IsNotNull(), "Testing if a calibration file could be loaded.");
     distToSurf->SetCameraIntrinsics(intrinsics);
     distToSurf->SetReconstructionMode(mitk::ToFDistanceImageToSurfaceFilter::Kinect);
     //load a data set
     mitk::Image::Pointer kinectImage = mitk::IOUtil::LoadImage(kinectImagePath);
     MITK_TEST_CONDITION_REQUIRED(kinectImage.IsNotNull(), "Testing if a kinect image could be loaded.");
     distToSurf->SetInput(kinectImage);
     distToSurf->Update();
 
-    //load ground truth data
-    mitk::Surface::Pointer groundTruth = mitk::IOUtil::LoadSurface(groundTruthSurfacePath);
-    MITK_TEST_CONDITION_REQUIRED(groundTruth.IsNotNull(), "Testing if ground truth could be loaded.");
-
     mitk::Surface::Pointer resultOfFilter = distToSurf->GetOutput();
     MITK_TEST_CONDITION_REQUIRED(resultOfFilter.IsNotNull(), "Testing if any output was generated.");
+    mitk::PointSet::Pointer resultPointSet = mitk::ToFTestingCommon::VtkPolyDataToMitkPointSet(resultOfFilter->GetVtkPolyData());
+
+    // generate ground truth data
+    mitk::PointSet::Pointer groundTruthPointSet = mitk::PointSet::New();
+    mitk::ToFProcessingCommon::ToFPoint2D focalLength;
+    focalLength[0] = intrinsics->GetFocalLengthX();
+    focalLength[1] = intrinsics->GetFocalLengthY();
+    mitk::ToFProcessingCommon::ToFPoint2D principalPoint;
+    principalPoint[0] = intrinsics->GetPrincipalPointX();
+    principalPoint[1] = intrinsics->GetPrincipalPointY();
+
+    int xDimension = (int)kinectImage->GetDimension(0);
+    int yDimension = (int)kinectImage->GetDimension(1);
+    int pointCount = 0;
+    mitk::ImagePixelReadAccessor<float,2> imageAcces(kinectImage, kinectImage->GetSliceData(0));
+    for (int j=0; j<yDimension; j++)
+    {
+      for (int i=0; i<xDimension; i++)
+      {
+        itk::Index<2> pixel;
+        pixel[0] = i;
+        pixel[1] = j;
+
+        mitk::ToFProcessingCommon::ToFScalarType distance = (double)imageAcces.GetPixelByIndex(pixel);
+
+        mitk::Point3D currentPoint;
+        currentPoint = mitk::ToFProcessingCommon::KinectIndexToCartesianCoordinates(i,j,distance,focalLength[0],focalLength[1],principalPoint[0],principalPoint[1]);
 
-    MITK_TEST_CONDITION_REQUIRED( mitk::ToFTestingCommon::VtkPolyDatasEqual(resultOfFilter->GetVtkPolyData(),
-                                                                            groundTruth->GetVtkPolyData() ),
+        if (distance>mitk::eps)
+        {
+          groundTruthPointSet->InsertPoint( pointCount, currentPoint );
+          pointCount++;
+        }
+      }
+    }
+    MITK_TEST_CONDITION_REQUIRED( mitk::ToFTestingCommon::PointSetsEqual(resultPointSet,groundTruthPointSet),
                                   "Testing if point sets are equal (with a small epsilon).");
     MITK_TEST_END();
 }
diff --git a/Modules/ToFProcessing/mitkToFTestingCommon.h b/Modules/ToFProcessing/mitkToFTestingCommon.h
index fa895679e1..b0219af732 100644
--- a/Modules/ToFProcessing/mitkToFTestingCommon.h
+++ b/Modules/ToFProcessing/mitkToFTestingCommon.h
@@ -1,102 +1,103 @@
 /*===================================================================
 
 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 mitkToFTestingCOMMON_H
 #define mitkToFTestingCOMMON_H
 
 #include "mitkToFProcessingExports.h"
 #include "mitkVector.h"
 #include <mitkToFProcessingCommon.h>
 #include <mitkSurface.h>
 #include <mitkPointSet.h>
 #include <itksys/SystemTools.hxx>
 
 #include <vtkSmartPointer.h>
 #include <vtkPolyData.h>
 
 namespace mitk
 {
 class mitkToFProcessing_EXPORT ToFTestingCommon
 {
 public:
 
     /**
      * @brief PointSetsEqual Method two test if two point sets contain the same points. mitk::Equal is used for comparison of the points.
      * @param pointSet1
      * @param pointSet2
      * @return True if pointsets are equal.
      */
     static bool PointSetsEqual(mitk::PointSet::Pointer pointSet1, mitk::PointSet::Pointer pointSet2)
     {
         bool pointSetsEqual = true;
         if (pointSet1->GetSize()==pointSet2->GetSize())
         {
             for (int i=0; i<pointSet1->GetSize(); i++)
             {
                 mitk::Point3D expectedPoint = pointSet1->GetPoint(i);
                 mitk::Point3D resultPoint = pointSet2->GetPoint(i);
                 if (!mitk::Equal(expectedPoint,resultPoint))
                 {
                     std::cout << std::endl;
                     std::cout << std::setprecision(12) << "expected: " << expectedPoint;
                     std::cout << std::endl;
                     std::cout << std::setprecision(12) << "resultPoint: " << resultPoint;
                     std::cout << std::endl;
                     pointSetsEqual = false;
                 }
             }
         }
         else
         {
             pointSetsEqual = false;
+            MITK_INFO<<"Point sets have different size: "<<pointSet1->GetSize()<<" vs. "<<pointSet2->GetSize();
         }
         return pointSetsEqual;
     }
 
     /**
      * @brief VtkPolyDatasEqual Convenience method for comparing the points of two vtkPolyData (using PointSetsEqual).
      * @param poly1
      * @param poly2
      * @return True if polydatas are equal.
      */
     static bool VtkPolyDatasEqual( vtkSmartPointer<vtkPolyData> poly1, vtkSmartPointer<vtkPolyData> poly2  )
     {
         return PointSetsEqual(VtkPolyDataToMitkPointSet(poly1), VtkPolyDataToMitkPointSet(poly2));
     }
 
     /**
      * @brief VtkPolyDataToMitkPointSet Converts a vtkPolyData into an mitkPointSet
      * @param poly Input polydata.
      * @return mitk::PointSet::Pointer The resulting point set.
      */
     static mitk::PointSet::Pointer VtkPolyDataToMitkPointSet( vtkSmartPointer<vtkPolyData> poly )
     {
         mitk::PointSet::Pointer result = mitk::PointSet::New();
         int numberOfPoints = poly->GetNumberOfPoints();
         for (int i=0; i<numberOfPoints; i++)
         {
           double* currentPoint = poly->GetPoint(i);
           mitk::Point3D point;
           point[0] = currentPoint[0];
           point[1] = currentPoint[1];
           point[2] = currentPoint[2];
           result->InsertPoint(i,point);
         }
         return result;
     }
 };
 
 }
 #endif