diff --git a/Modules/ToFHardware/Testing/files.cmake b/Modules/ToFHardware/Testing/files.cmake
index 09670f17d5..67cca63647 100644
--- a/Modules/ToFHardware/Testing/files.cmake
+++ b/Modules/ToFHardware/Testing/files.cmake
@@ -1,26 +1,26 @@
 set(MODULE_TESTS
   #mitkThreadedToFRawDataReconstructionTest.cpp
   mitkAbstractToFDeviceFactoryTest.cpp
   mitkToFCameraMITKPlayerDeviceTest.cpp
   mitkToFCameraMITKPlayerDeviceFactoryTest.cpp
   mitkToFImageCsvWriterTest.cpp
   mitkToFImageGrabberTest.cpp
   #mitkToFImageRecorderTest.cpp
   #mitkToFImageRecorderFilterTest.cpp
   mitkToFImageWriterTest.cpp
   mitkToFNrrdImageWriterTest.cpp
-  #mitkToFOpenCVImageGrabberTest.cpp
+  mitkToFOpenCVImageGrabberTest.cpp
 )
 
 set(MODULE_CUSTOM_TESTS
   mitkPlayerLoadAndRenderDepthDataTest.cpp
   mitkPlayerLoadAndRenderRGBDataTest.cpp
 )
 
 # Create an artificial module initializing class for
 # the mitkToFCameraMITKPlayerDeviceFactoryTest
 usFunctionGenerateExecutableInit(testdriver_init_file
                                  IDENTIFIER ${MODULE_NAME}TestDriver
                                 )
 
 set(TEST_CPP_FILES ${testdriver_init_file})
diff --git a/Modules/ToFHardware/Testing/mitkToFOpenCVImageGrabberTest.cpp b/Modules/ToFHardware/Testing/mitkToFOpenCVImageGrabberTest.cpp
index cfcf1e1ac2..dbba55590d 100644
--- a/Modules/ToFHardware/Testing/mitkToFOpenCVImageGrabberTest.cpp
+++ b/Modules/ToFHardware/Testing/mitkToFOpenCVImageGrabberTest.cpp
@@ -1,93 +1,101 @@
 /*===================================================================
 
 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 <mitkToFOpenCVImageGrabber.h>
 
 #include <mitkImageDataItem.h>
 #include <mitkIOUtil.h>
 #include <mitkToFCameraMITKPlayerDevice.h>
 #include <mitkToFConfig.h>
 #include <mitkImagePixelReadAccessor.h>
 
 static bool CompareImages(mitk::Image::Pointer mitkImage, cv::Mat openCVImage)
 {
   float equal = true;
-  if ((mitkImage->GetDimension(0)!=openCVImage.cols)||(mitkImage->GetDimension(1)!=openCVImage.rows))
+  if (static_cast<int>((mitkImage->GetDimension(0))!=openCVImage.cols)||(static_cast<int>(mitkImage->GetDimension(1))!=openCVImage.rows))
   {
     equal = false;
   }
   mitk::ImagePixelReadAccessor<float,2> imageAcces(mitkImage, mitkImage->GetSliceData(0));
-  for (unsigned int i=0; i<openCVImage.cols; i++)
+  for(int i=0; i<openCVImage.cols; i++)
   {
-    for (unsigned int j=0; j<openCVImage.rows; j++)
+    for(int j=0; j<openCVImage.rows; j++)
     {
       itk::Index<2> currentIndex;
       currentIndex[0] = i;
       currentIndex[1] = j;
       float mitkImageValue = imageAcces.GetPixelByIndex(currentIndex);
       float openCVImageValue = openCVImage.at<float>(j,i);
       if (!mitk::Equal(mitkImageValue,openCVImageValue))
       {
         equal = false;
       }
     }
   }
   return equal;
 }
 
 /**Documentation
  *  test for the class "ToFOpenCVImageGrabber".
  */
 int mitkToFOpenCVImageGrabberTest(int /* argc */, char* /*argv*/[])
 {
   MITK_TEST_BEGIN("ToFOpenCVImageGrabber");
 
   std::string dirName = MITK_TOF_DATA_DIR;
   mitk::ToFImageGrabber::Pointer tofImageGrabber = mitk::ToFImageGrabber::New();
   tofImageGrabber->SetCameraDevice(mitk::ToFCameraMITKPlayerDevice::New());
   std::string distanceFileName = dirName + "/PMDCamCube2_MF0_IT0_1Images_DistanceImage.pic";
   tofImageGrabber->SetProperty("DistanceImageFileName",mitk::StringProperty::New(distanceFileName));
   std::string amplitudeFileName = dirName + "/PMDCamCube2_MF0_IT0_1Images_AmplitudeImage.pic";
   tofImageGrabber->SetProperty("AmplitudeImageFileName",mitk::StringProperty::New(amplitudeFileName));
   std::string intensityFileName = dirName + "/PMDCamCube2_MF0_IT0_1Images_IntensityImage.pic";
   tofImageGrabber->SetProperty("IntensityImageFileName",mitk::StringProperty::New(intensityFileName));
+  tofImageGrabber->Update();
 
   mitk::Image::Pointer image = mitk::IOUtil::LoadImage(distanceFileName);
 
-  mitk::ToFOpenCVImageGrabber::Pointer tofOpenCVImageGrabber = mitk::ToFOpenCVImageGrabber::New();
-  tofOpenCVImageGrabber->SetToFImageGrabber(tofImageGrabber);
-  MITK_TEST_CONDITION_REQUIRED(tofImageGrabber==tofOpenCVImageGrabber->GetToFImageGrabber(),"Test Set/GetToFImageGrabber()");
-  MITK_TEST_OUTPUT(<<"Call StartCapturing()");
-  tofOpenCVImageGrabber->StartCapturing();
-  cv::Mat cvImage = tofOpenCVImageGrabber->GetImage();
-  MITK_TEST_CONDITION_REQUIRED(CompareImages(image,cvImage),"Test distance image");
-  image = mitk::IOUtil::LoadImage(amplitudeFileName);
-  tofOpenCVImageGrabber->SetImageType(1);
-  cvImage = tofOpenCVImageGrabber->GetImage();
-  MITK_TEST_CONDITION_REQUIRED(CompareImages(image,cvImage),"Test amplitude image");
-  image = mitk::IOUtil::LoadImage(intensityFileName);
-  tofOpenCVImageGrabber->SetImageType(2);
-  cvImage = tofOpenCVImageGrabber->GetImage();
-  MITK_TEST_CONDITION_REQUIRED(CompareImages(image,cvImage),"Test intensity image");
-  MITK_TEST_OUTPUT(<<"Call StopCapturing()");
-  tofOpenCVImageGrabber->StopCapturing();
+  try
+  {
+    mitk::ToFOpenCVImageGrabber::Pointer tofOpenCVImageGrabber = mitk::ToFOpenCVImageGrabber::New();
+    tofOpenCVImageGrabber->SetToFImageGrabber(tofImageGrabber);
+    MITK_TEST_CONDITION_REQUIRED(tofImageGrabber==tofOpenCVImageGrabber->GetToFImageGrabber(),"Test Set/GetToFImageGrabber()");
+    MITK_TEST_OUTPUT(<<"Call StartCapturing()");
+    tofOpenCVImageGrabber->StartCapturing();
+    cv::Mat cvImage = tofOpenCVImageGrabber->GetImage();
+    MITK_TEST_CONDITION_REQUIRED(CompareImages(image,cvImage),"Test distance image");
+    image = mitk::IOUtil::LoadImage(amplitudeFileName);
+    tofOpenCVImageGrabber->SetImageType(1);
+    cvImage = tofOpenCVImageGrabber->GetImage();
+    MITK_TEST_CONDITION_REQUIRED(CompareImages(image,cvImage),"Test amplitude image");
+    image = mitk::IOUtil::LoadImage(intensityFileName);
+    tofOpenCVImageGrabber->SetImageType(2);
+    cvImage = tofOpenCVImageGrabber->GetImage();
+    MITK_TEST_CONDITION_REQUIRED(CompareImages(image,cvImage),"Test intensity image");
+    MITK_TEST_OUTPUT(<<"Call StopCapturing()");
+    tofOpenCVImageGrabber->StopCapturing();
+  }
+  catch(std::exception &e)
+  {
+    MITK_INFO << "Exception is: " << e.what();
+  }
 
   MITK_TEST_END();
 
 }
 
 
diff --git a/Modules/ToFHardware/mitkToFOpenCVImageGrabber.cpp b/Modules/ToFHardware/mitkToFOpenCVImageGrabber.cpp
index 708576b19f..8fb4394db7 100644
--- a/Modules/ToFHardware/mitkToFOpenCVImageGrabber.cpp
+++ b/Modules/ToFHardware/mitkToFOpenCVImageGrabber.cpp
@@ -1,187 +1,186 @@
 /*===================================================================
 
 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 "mitkToFOpenCVImageGrabber.h"
 
 // mitk includes
 #include "mitkImageDataItem.h"
 #include <mitkImageStatisticsHolder.h>
 #include "mitkImageReadAccessor.h"
 
 #include "vtkSmartPointer.h"
 #include "vtkColorTransferFunction.h"
 #include "vtkFloatArray.h"
 
 
 namespace mitk
 {
   ToFOpenCVImageGrabber::ToFOpenCVImageGrabber()
   {
     m_CurrentOpenCVIntensityImage = NULL;
     m_CurrentOpenCVAmplitudeImage = NULL;
     m_CurrentOpenCVDistanceImage = NULL;
     m_ImageType = 0;
     m_ImageDepth = IPL_DEPTH_32F;
     m_ImageGrabber = NULL;
   }
 
   ToFOpenCVImageGrabber::~ToFOpenCVImageGrabber()
   {
   }
 
   cv::Mat ToFOpenCVImageGrabber::GetImage()
   {
     m_ImageGrabber->Update();
     unsigned int numOfPixel = m_ImageGrabber->GetCaptureWidth()*m_ImageGrabber->GetCaptureHeight();
     // copy current mitk images
     unsigned int dimensions[4];
     dimensions[0] = this->m_ImageGrabber->GetCaptureWidth();
     dimensions[1] = this->m_ImageGrabber->GetCaptureHeight();
     dimensions[2] = 1;
     dimensions[3] = 1;
 
     // create single component float pixel type
     mitk::PixelType FloatType = MakeScalarPixelType<float>();
 
     ImageReadAccessor imgGrabAcc0(m_ImageGrabber->GetOutput(0), m_ImageGrabber->GetOutput(0)->GetSliceData());
     ImageReadAccessor imgGrabAcc1(m_ImageGrabber->GetOutput(1), m_ImageGrabber->GetOutput(1)->GetSliceData());
     ImageReadAccessor imgGrabAcc2(m_ImageGrabber->GetOutput(2), m_ImageGrabber->GetOutput(2)->GetSliceData());
 
     mitk::Image::Pointer currentMITKIntensityImage = mitk::Image::New();
     currentMITKIntensityImage->Initialize(FloatType, 2, dimensions);
     currentMITKIntensityImage->SetSlice((float*) imgGrabAcc2.GetData(),0,0,0);
 
     mitk::Image::Pointer currentMITKAmplitudeImage = mitk::Image::New();
     currentMITKAmplitudeImage->Initialize(FloatType, 2, dimensions);
     currentMITKAmplitudeImage->SetSlice((float*)imgGrabAcc1.GetData(),0,0,0);
 
     mitk::Image::Pointer currentMITKDistanceImage = mitk::Image::New();
     currentMITKDistanceImage->Initialize(FloatType, 2, dimensions);
     currentMITKDistanceImage->SetSlice((float*)imgGrabAcc0.GetData(),0,0,0);
     // copy mitk images to OpenCV images
     if (m_ImageDepth==IPL_DEPTH_32F)
     {
       if (m_ImageType==1)
       {
         ImageReadAccessor currentAmplAcc(currentMITKAmplitudeImage, currentMITKAmplitudeImage->GetSliceData(0, 0, 0));
         float* amplitudeFloatData = (float*) currentAmplAcc.GetData();
         memcpy(m_CurrentOpenCVAmplitudeImage->imageData,(unsigned char*)amplitudeFloatData,numOfPixel*sizeof(float));
         cv::Mat image(m_CurrentOpenCVAmplitudeImage);
         return image;
       }
       else if (m_ImageType==2)
       {
         ImageReadAccessor currentIntenAcc(currentMITKIntensityImage, currentMITKIntensityImage->GetSliceData(0, 0, 0));
         float* intensityFloatData = (float*) currentIntenAcc.GetData();
         memcpy(m_CurrentOpenCVIntensityImage->imageData,(unsigned char*)intensityFloatData,numOfPixel*sizeof(float));
         cv::Mat image(m_CurrentOpenCVIntensityImage);
         return image;
       }
       else
       {
         ImageReadAccessor currentDistAcc(currentMITKDistanceImage, currentMITKDistanceImage->GetSliceData(0, 0, 0));
         float* distanceFloatData = (float*) currentDistAcc.GetData();
         memcpy(m_CurrentOpenCVDistanceImage->imageData,(unsigned char*)distanceFloatData,numOfPixel*sizeof(float));
         cv::Mat image(m_CurrentOpenCVDistanceImage);
         return image;
       }
     }
     else
     {
       if (m_ImageType==1)
       {
         this->MapScalars(currentMITKAmplitudeImage, m_CurrentOpenCVAmplitudeImage);
         cv::Mat image(m_CurrentOpenCVAmplitudeImage);
         return image;
       }
       else if (m_ImageType==2)
       {
         this->MapScalars(currentMITKIntensityImage, m_CurrentOpenCVIntensityImage);
         cv::Mat image(m_CurrentOpenCVIntensityImage);
         return image;
       }
       else
       {
         this->MapScalars(currentMITKDistanceImage, m_CurrentOpenCVDistanceImage);
         cv::Mat image(m_CurrentOpenCVDistanceImage);
         return image;
       }
     }
   }
 
   void ToFOpenCVImageGrabber::SetImageType(unsigned int imageType)
   {
     m_ImageType = imageType;
   }
 
   void ToFOpenCVImageGrabber::SetImageDepth(unsigned int imageDepth)
   {
     m_ImageDepth = imageDepth;
   }
 
   void ToFOpenCVImageGrabber::SetToFImageGrabber(ToFImageGrabber::Pointer imageGrabber)
   {
     m_ImageGrabber = imageGrabber;
-
-    //initialize OpenCV images
-    m_CurrentOpenCVIntensityImage = cvCreateImage(cvSize(m_ImageGrabber->GetCaptureWidth(), m_ImageGrabber->GetCaptureHeight()), m_ImageDepth, 1);
-    m_CurrentOpenCVAmplitudeImage = cvCreateImage(cvSize(m_ImageGrabber->GetCaptureWidth(), m_ImageGrabber->GetCaptureHeight()), m_ImageDepth, 1);
-    m_CurrentOpenCVDistanceImage = cvCreateImage(cvSize(m_ImageGrabber->GetCaptureWidth(), m_ImageGrabber->GetCaptureHeight()), m_ImageDepth, 1);
   }
 
   ToFImageGrabber::Pointer ToFOpenCVImageGrabber::GetToFImageGrabber()
   {
     return m_ImageGrabber;
   }
 
   void ToFOpenCVImageGrabber::StartCapturing()
   {
     if (m_ImageGrabber.IsNotNull())
     {
       m_ImageGrabber->ConnectCamera();
+      //Initialize cv Images after the camera is conneceted and we know the resolution
+      m_CurrentOpenCVIntensityImage = cvCreateImage(cvSize(m_ImageGrabber->GetCaptureWidth(), m_ImageGrabber->GetCaptureHeight()), m_ImageDepth, 1);
+      m_CurrentOpenCVAmplitudeImage = cvCreateImage(cvSize(m_ImageGrabber->GetCaptureWidth(), m_ImageGrabber->GetCaptureHeight()), m_ImageDepth, 1);
+      m_CurrentOpenCVDistanceImage = cvCreateImage(cvSize(m_ImageGrabber->GetCaptureWidth(), m_ImageGrabber->GetCaptureHeight()), m_ImageDepth, 1);
       m_ImageGrabber->StartCamera();
     }
   }
 
   void ToFOpenCVImageGrabber::StopCapturing()
   {
     if (m_ImageGrabber.IsNotNull())
     {
       m_ImageGrabber->StopCamera();
       m_ImageGrabber->DisconnectCamera();
     }
   }
 
   void ToFOpenCVImageGrabber::MapScalars( mitk::Image::Pointer mitkImage, IplImage* openCVImage)
   {
     unsigned int numOfPixel = m_ImageGrabber->GetCaptureWidth()*m_ImageGrabber->GetCaptureHeight();
     ImageReadAccessor imgAcc(mitkImage, mitkImage->GetSliceData(0, 0, 0));
     float* floatData = (float*)imgAcc.GetData();
     vtkSmartPointer<vtkColorTransferFunction> colorTransferFunction = vtkSmartPointer<vtkColorTransferFunction>::New();
     vtkSmartPointer<vtkFloatArray> floatArrayInt = vtkSmartPointer<vtkFloatArray>::New();
     floatArrayInt->Initialize();
     floatArrayInt->SetArray(floatData, numOfPixel, 0);
     mitk::ScalarType min = mitkImage->GetStatistics()->GetScalarValueMin();
     mitk::ScalarType max = mitkImage->GetStatistics()->GetScalarValueMaxNoRecompute();
     MITK_INFO<<"Minimum: "<<min;
     MITK_INFO<<"Maximum: "<<max;
     colorTransferFunction->RemoveAllPoints();
     colorTransferFunction->AddRGBPoint(min, 0, 0, 0);
     colorTransferFunction->AddRGBPoint(max, 1, 1, 1);
     colorTransferFunction->SetColorSpaceToHSV();
     colorTransferFunction->MapScalarsThroughTable(floatArrayInt, (unsigned char*)openCVImage->imageData, VTK_LUMINANCE);
   }
 
 } // end namespace mitk