diff --git a/Math_Tools/ICP/ICP.cc b/Math_Tools/ICP/ICP.cc
index de61b27a9b3f089444060238e988cfae5ca20e34..39bf5876dbe9db3cbcc0f18e41b9c7d217541124 100644
--- a/Math_Tools/ICP/ICP.cc
+++ b/Math_Tools/ICP/ICP.cc
@@ -179,6 +179,79 @@ icp(const std::vector< VectorT >& _points1 , const std::vector< VectorT >& _poin
    _scale2 *= _scale2;
 }
 
+template < typename VectorT , typename QuaternionT >
+void
+icp(const std::vector< VectorT >& _points1 , const std::vector< VectorT >& _points2  , VectorT& _cog1 ,  VectorT& _cog2 , QuaternionT& _rotation )
+{
+   // compute Mean of Points
+   _cog1 = center_of_gravity(_points1);
+   _cog2 = center_of_gravity(_points2);
+
+   double Sxx = 0.0;
+   double Sxy = 0.0;
+   double Sxz = 0.0;
+   double Syx = 0.0;
+   double Syy = 0.0;
+   double Syz = 0.0;
+   double Szx = 0.0;
+   double Szy = 0.0;
+   double Szz = 0.0;
+
+    for ( uint i = 0 ; i < _points1.size() ; ++i  ) {
+      Sxx += ( _points1[i][0] - _cog1[0] ) * ( _points2[i][0] - _cog2[0] );
+      Sxy += ( _points1[i][0] - _cog1[0] ) * ( _points2[i][1] - _cog2[1] );
+      Sxz += ( _points1[i][0] - _cog1[0] ) * ( _points2[i][2] - _cog2[2] );
+
+      Syx += ( _points1[i][1] - _cog1[1] ) * ( _points2[i][0] - _cog2[0] );
+      Syy += ( _points1[i][1] - _cog1[1] ) * ( _points2[i][1] - _cog2[1] );
+      Syz += ( _points1[i][1] - _cog1[1] ) * ( _points2[i][2] - _cog2[2] );
+
+      Szx += ( _points1[i][2] - _cog1[2] ) * ( _points2[i][0] - _cog2[0] );
+      Szy += ( _points1[i][2] - _cog1[2] ) * ( _points2[i][1] - _cog2[1] );
+      Szz += ( _points1[i][2] - _cog1[2] ) * ( _points2[i][2] - _cog2[2] );
+   }
+
+   gmm::dense_matrix<double> N; gmm::resize(N,4,4); gmm::clear(N);
+   N(0,0) = Sxx + Syy + Szz;
+   N(0,1) = Syz - Szy;
+   N(0,2) = Szx - Sxz;
+   N(0,3) = Sxy - Syx;
+
+   N(1,0) = Syz - Szy;
+   N(1,1) = Sxx - Syy - Szz;
+   N(1,2) = Sxy + Syx;
+   N(1,3) = Szx + Sxz;
+
+   N(2,0) = Szx - Sxz;
+   N(2,1) = Sxy + Syx;
+   N(2,2) = -Sxx + Syy - Szz;
+   N(2,3) = Syz + Szy;
+
+   N(3,0) = Sxy - Syx;
+   N(3,1) = Szx + Sxz;
+   N(3,2) = Syz + Szy;
+   N(3,3) = -Sxx - Syy + Szz;
+
+   std::vector< double > eigval; eigval.resize(4);
+   gmm::dense_matrix< double > eigvect; gmm::resize(eigvect, 4,4); gmm::clear(eigvect);
+   gmm::symmetric_qr_algorithm(N, eigval, eigvect);
+
+   int gr = 0;
+   double max = -FLT_MAX;
+   for (int i = 0; i < 4; i++)
+   if (eigval[i] > max)
+   {
+      gr = i;
+      max = eigval[i];
+   }
+
+   _rotation[0] = eigvect(0,gr);
+   _rotation[1] = eigvect(1,gr);
+   _rotation[2] = eigvect(2,gr);
+   _rotation[3] = eigvect(3,gr);
+
+}
+
 
 //=============================================================================
 
diff --git a/Math_Tools/ICP/ICP.hh b/Math_Tools/ICP/ICP.hh
index a8bd6beebd52236b232a7c62a3025dc2f278d424..0c13d8282fdaf22181a4af4820588a00631643c0 100644
--- a/Math_Tools/ICP/ICP.hh
+++ b/Math_Tools/ICP/ICP.hh
@@ -96,6 +96,26 @@ namespace ICP {
 template < typename VectorT , typename QuaternionT >
 void icp(const std::vector< VectorT >& _points1 , const std::vector< VectorT >& _points2  , VectorT& _cog1 ,  VectorT& _cog2 , double& _scale1 , double& _scale2 , QuaternionT& _rotation );
 
+/** \brief Compute rigid transformation from first point set to second point set without scaling
+   *
+   * Compute ICP Parameters ( No iteration is done ) Points are unchanged, only parameters are computed.
+   *
+   * To transform pointset 1 into pointset 2 do the folowing:\n
+   *  - substract cog1 from pointset 1 \n
+   *  - rotate with given rotation \n
+   *  - add cog2  \n
+   *
+   * @param _points1 first set of points
+   * @param _points2 second set of points
+   * @param  _cog1 center of gravity first point set
+   * @param  _cog2 center of gravity second point set
+   * @param _rotation Rotation between point sets ( rotation(_points1) -> _points2
+   *
+   *
+*/
+template < typename VectorT , typename QuaternionT >
+void icp(const std::vector< VectorT >& _points1 , const std::vector< VectorT >& _points2  , VectorT& _cog1 ,  VectorT& _cog2 , QuaternionT& _rotation );
+
 //=============================================================================
 } //namespace ICP
 //=============================================================================