Index: mitkCameraIntrinsics.cpp
===================================================================
--- mitkCameraIntrinsics.cpp	(revision 26004)
+++ mitkCameraIntrinsics.cpp	(working copy)
@@ -52,6 +52,8 @@
   m_CameraMatrix.at<double>(0,2) = principalPoint[0];
   m_CameraMatrix.at<double>(1,2) = principalPoint[1];
 
+  m_CameraMatrix.at<double>(2,2) = 1.0;
+
   m_DistorsionCoeffs.at<double>(0,0) = distortionCoefficients[0];
   m_DistorsionCoeffs.at<double>(0,1) = distortionCoefficients[1];
   m_DistorsionCoeffs.at<double>(0,2) = distortionCoefficients[2];
@@ -144,6 +146,8 @@
   myAssert( PrincipalYElem != 0 );
   CameraMatrix.at<double>(1,2) = atof( PrincipalYElem->GetText() );
 
+  CameraMatrix.at<double>(2,2) = 1.0;
+
   // DISTORSION COEFFS
 
   TiXmlElement* Dist1Elem = elem->FirstChildElement("Dist1");
@@ -209,6 +213,8 @@
   else
     err << "cy, ";
 
+  CameraMatrix.at<double>(2,2) = 1.0;
+
   // DISTORSION COEFFS
   cv::Mat DistorsionCoeffs = cv::Mat::zeros(1, 4, cv::DataType<double>::type);
   if(elem->QueryFloatAttribute("k1", &val) == TIXML_SUCCESS)
Index: mitkHandEyeCalibration.cpp
===================================================================
--- mitkHandEyeCalibration.cpp	(revision 26004)
+++ mitkHandEyeCalibration.cpp	(working copy)
@@ -82,6 +82,21 @@
             cv::Point3f(it->x*m_CameraCalibration->GetSquareSize()
               , it->y*m_CameraCalibration->GetSquareSize(), 0) );
       }
+      //int x = 1;
+      //int y = m_CameraCalibration->GetChessBoardSize().height;
+      //for( vector<cv::Point2f>::iterator it=imagepoints.begin()
+      //  ; it!=imagepoints.end(); ++it )
+      //{
+      //  objectpoints.push_back(
+      //      cv::Point3f(x*m_CameraCalibration->GetSquareSize()
+      //        , y*m_CameraCalibration->GetSquareSize(), 0) );
+      //  y--;
+      //  if (y==1)
+      //  {
+      //    x++;
+      //    y = m_CameraCalibration->GetChessBoardSize().height;
+      //  }
+      //}
 
       cv::Mat rvec, tvec;
 
@@ -200,6 +215,7 @@
     // m_AngleThreshold to 180 or 0/360 axis will be removed
 
     // now begin to remove moves (beginning with the smallest
+    std::vector<unsigned int> indexToRemove;
     std::vector<mitk::Transform::Pointer>::iterator it2;
     unsigned int index = 0;
     for( std::map<double, unsigned int>::iterator it = angleToIndex.begin()
@@ -222,10 +238,11 @@
         // rotation angle too small
         MITK_DEBUG << "rotation angle too small. removing move " << index+1
             << ". rotation angle was " << rotAngle;
-        it2 = m_EyeMoves.begin() + index;
-        m_EyeMoves.erase(it2);
-        it2 = m_HandMoves.begin() + index;
-        m_HandMoves.erase(it2);
+        indexToRemove.push_back(index);
+        //it2 = m_EyeMoves.begin() + index;
+        //m_EyeMoves.erase(it2);
+        //it2 = m_HandMoves.begin() + index;
+        //m_HandMoves.erase(it2);
       }
       // ascending order, if rotAngle is bigger than the threshold
       // the upcomign angles are also bigger
@@ -233,6 +250,14 @@
         break;
     }
 
+    std::sort(indexToRemove.begin(), indexToRemove.end());
+    for (int i=indexToRemove.size()-1; i>=0; i--)
+    {
+      it2 = m_EyeMoves.begin() + indexToRemove.at(i);
+      m_EyeMoves.erase(it2);
+      it2 = m_HandMoves.begin() + indexToRemove.at(i);
+      m_HandMoves.erase(it2);
+    }
 
     // now find the best movement pairs
     std::map<double, std::pair<unsigned int, unsigned int> > ratingToMovePair;
@@ -241,7 +266,7 @@
     unsigned int n = 0;
     for( unsigned int i=0; i<m_EyeMoves.size(); ++i )
     {
-      for( unsigned int j=i; j<m_EyePoses.size(); ++j )
+      for( unsigned int j=i; j<m_EyeMoves.size(); ++j )
       {
         // no self movements here
         if(i==j)
@@ -263,7 +288,6 @@
     meanRating = meanRating / n;
     MITK_DEBUG << "meanRating: " << meanRating;
     MITK_DEBUG << "number of moves left before removing by rating : " << m_EyeMoves.size();
-
     // remove underrated moves (moves that are higher than the mean)
     // but leave out at least three moves
     std::map<double, std::pair<unsigned int, unsigned int> >::iterator ratingToMovePairIt
Index: mitkPointReprojection.cpp
===================================================================
--- mitkPointReprojection.cpp	(revision 26004)
+++ mitkPointReprojection.cpp	(working copy)
@@ -106,6 +106,21 @@
         ->GetSquareSize(), it->y*d->m_Calibration->GetSquareSize(), 0) );
     }
 
+    //int x = 1;
+    //int y = d->m_Calibration->GetSquareSize();
+    //for( vector<cv::Point2f>::iterator it=d->m_ImagePoints.begin()
+    //  ; it!=d->m_ImagePoints.end(); ++it )
+    //{
+    //   d->m_Objectpoints.push_back( cv::Point3f(x*d->m_Calibration
+    //    ->GetSquareSize(), y*d->m_Calibration->GetSquareSize(), 0) );
+    //  y--;
+    //  if (y==1)
+    //  {
+    //    x++;
+    //    y = d->m_Calibration->GetSquareSize();
+    //  }
+    //}
+
     // create camera extrinsics
     cv::Mat rvec, tvec;
     cv::solvePnP( cv::Mat(d->m_Objectpoints), cv::Mat(d->m_ImagePoints),