Moved Math_Tools to subdir

libs_required/Math_Tools/ICP/ICP.hh

+/*===========================================================================*\
+*                                                                            *
+*                              OpenFlipper                                   *
+ *           Copyright (c) 2001-2015, RWTH-Aachen University                 *
+ *           Department of Computer Graphics and Multimedia                  *
+ *                          All rights reserved.                             *
+ *                            www.openflipper.org                            *
+ *                                                                           *
+ *---------------------------------------------------------------------------*
+ * This file is part of OpenFlipper.                                         *
+ *---------------------------------------------------------------------------*
+ *                                                                           *
+ * Redistribution and use in source and binary forms, with or without        *
+ * modification, are permitted provided that the following conditions        *
+ * are met:                                                                  *
+ *                                                                           *
+ * 1. Redistributions of source code must retain the above copyright notice, *
+ *    this list of conditions and the following disclaimer.                  *
+ *                                                                           *
+ * 2. Redistributions in binary form must reproduce the above copyright      *
+ *    notice, this list of conditions and the following disclaimer in the    *
+ *    documentation and/or other materials provided with the distribution.   *
+ *                                                                           *
+ * 3. Neither the name of the copyright holder nor the names of its          *
+ *    contributors may be used to endorse or promote products derived from   *
+ *    this software without specific prior written permission.               *
+ *                                                                           *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS       *
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED *
+ * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A           *
+ * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER *
+ * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,  *
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,       *
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR        *
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF    *
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING      *
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS        *
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.              *
+*                                                                            *
+\*===========================================================================*/
+
+/*===========================================================================*\
+*                                                                            *
+*   $Revision$                                                       *
+*   $LastChangedBy$                                                *
+*   $Date$                     *
+*                                                                            *
+\*===========================================================================*/
+
+
+
+
+//=============================================================================
+//
+//  CLASS ICP
+//
+//=============================================================================
+
+
+#ifndef ICP_HH
+#define ICP_HH
+
+/*! \file ICP.hh
+    \brief Classes for ICP Algorithm
+    
+    Classes for doing Principal component analysis
+*/
+
+//== INCLUDES =================================================================
+
+#include <gmm/gmm.h>
+
+//== FORWARDDECLARATIONS ======================================================
+
+//== NAMESPACES ===============================================================
+
+/// Namespace for ICP
+namespace ICP {
+ 
+//== CLASS DEFINITION =========================================================
+  
+/** \brief Compute rigid transformation from first point set to second point set
+   * 
+   * 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
+   *  - scale points with 1/scale1 \n
+   *  - rotate with given rotation \n
+   *  - scale with scale2 \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 _scale1 scaling factor of first point set
+   * @param _scale2 scaling factor of 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 , 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
+//=============================================================================
+#if defined(INCLUDE_TEMPLATES) && !defined(ICP_C)
+#define ICP_TEMPLATES
+#include "ICPT.cc"
+#endif
+//=============================================================================
+#endif // ICP_HH defined
+//=============================================================================
+

libs_required/Math_Tools/ICP/ICPT.cc

+/*===========================================================================*\
+*                                                                            *
+*                              OpenFlipper                                   *
+ *           Copyright (c) 2001-2015, RWTH-Aachen University                 *
+ *           Department of Computer Graphics and Multimedia                  *
+ *                          All rights reserved.                             *
+ *                            www.openflipper.org                            *
+ *                                                                           *
+ *---------------------------------------------------------------------------*
+ * This file is part of OpenFlipper.                                         *
+ *---------------------------------------------------------------------------*
+ *                                                                           *
+ * Redistribution and use in source and binary forms, with or without        *
+ * modification, are permitted provided that the following conditions        *
+ * are met:                                                                  *
+ *                                                                           *
+ * 1. Redistributions of source code must retain the above copyright notice, *
+ *    this list of conditions and the following disclaimer.                  *
+ *                                                                           *
+ * 2. Redistributions in binary form must reproduce the above copyright      *
+ *    notice, this list of conditions and the following disclaimer in the    *
+ *    documentation and/or other materials provided with the distribution.   *
+ *                                                                           *
+ * 3. Neither the name of the copyright holder nor the names of its          *
+ *    contributors may be used to endorse or promote products derived from   *
+ *    this software without specific prior written permission.               *
+ *                                                                           *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS       *
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED *
+ * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A           *
+ * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER *
+ * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,  *
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,       *
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR        *
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF    *
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING      *
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS        *
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.              *
+*                                                                            *
+\*===========================================================================*/
+
+/*===========================================================================*\
+*                                                                            *
+*   $Revision$                                                       *
+*   $LastChangedBy$                                                *
+*   $Date$                     *
+*                                                                            *
+\*===========================================================================*/
+
+
+
+
+//=============================================================================
+//
+//  CLASS ICP - IMPLEMENTATION
+//
+//=============================================================================
+
+#define ICP_C
+
+//== INCLUDES =================================================================
+
+#include "ICP.hh"
+#include <cfloat>
+
+
+//== NAMESPACES ===============================================================
+
+namespace ICP {
+
+//== IMPLEMENTATION ==========================================================
+
+template < typename VectorT >
+inline
+VectorT  
+center_of_gravity(const std::vector< VectorT >& _points ) {
+   VectorT cog(0.0,0.0,0.0);
+   
+   for (uint i = 0 ; i < _points.size() ; ++i ) {
+     cog = cog + _points[i];
+   }
+   
+   cog = cog / ( typename VectorT::value_type )_points.size();
+
+   return cog;
+}
+
+
+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 )
+{
+   // compute Mean of Points
+   _cog1 = center_of_gravity(_points1);
+   _cog2 = center_of_gravity(_points2);
+   
+   
+   VectorT diff;
+   _scale1 = 0.0;
+   _scale2 = 0.0;
+   for ( uint i = 0 ; i < _points1.size() ; ++i ) {
+       diff = _points1[i] - _cog1;
+       _scale1 = std::max( _scale1 , sqrt( diff.norm() ) );
+       diff = _points2[i] - _cog2;
+       _scale2 = std::max( _scale2 , sqrt( diff.norm() ) );       
+   }
+   
+   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] );
+   }
+   
+   const double scale = _scale1 * _scale2;
+   
+   Sxx = Sxx * 1.0 / scale;
+   Sxy = Sxy * 1.0 / scale;
+   Sxz = Sxz * 1.0 / scale;
+   Syx = Syx * 1.0 / scale;
+   Syy = Syy * 1.0 / scale;
+   Syz = Syz * 1.0 / scale;
+   Szx = Szx * 1.0 / scale;
+   Szy = Szy * 1.0 / scale;
+   Szz = Szz * 1.0 / scale;
+   
+   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);
+   
+   _scale1 *= _scale1;
+   _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);
+
+}
+
+
+//=============================================================================
+
+} //namespace ICP
+
+//=============================================================================

libs_required/Math_Tools/Math_Tools.hh

+/*===========================================================================*\
+*                                                                            *
+*                              OpenFlipper                                   *
+ *           Copyright (c) 2001-2015, RWTH-Aachen University                 *
+ *           Department of Computer Graphics and Multimedia                  *
+ *                          All rights reserved.                             *
+ *                            www.openflipper.org                            *
+ *                                                                           *
+ *---------------------------------------------------------------------------*
+ * This file is part of OpenFlipper.                                         *
+ *---------------------------------------------------------------------------*
+ *                                                                           *
+ * Redistribution and use in source and binary forms, with or without        *
+ * modification, are permitted provided that the following conditions        *
+ * are met:                                                                  *
+ *                                                                           *
+ * 1. Redistributions of source code must retain the above copyright notice, *
+ *    this list of conditions and the following disclaimer.                  *
+ *                                                                           *
+ * 2. Redistributions in binary form must reproduce the above copyright      *
+ *    notice, this list of conditions and the following disclaimer in the    *
+ *    documentation and/or other materials provided with the distribution.   *
+ *                                                                           *
+ * 3. Neither the name of the copyright holder nor the names of its          *
+ *    contributors may be used to endorse or promote products derived from   *
+ *    this software without specific prior written permission.               *
+ *                                                                           *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS       *
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED *
+ * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A           *
+ * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER *
+ * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,  *
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,       *
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR        *
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF    *
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING      *
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS        *
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.              *
+*                                                                            *
+\*===========================================================================*/
+
+/*===========================================================================*\
+*                                                                            *
+*   $Revision$                                                       *
+*   $LastChangedBy$                                                *
+*   $Date$                     *
+*                                                                            *
+\*===========================================================================*/
+
+
+
+//=============================================================================
+//
+//  CLASS GeodesicFastMarchT
+//
+//=============================================================================
+
+
+#ifndef MATH_TOOLS_HH
+#define MATH_TOOLS_HH
+
+
+//== INCLUDES =================================================================
+
+//== NAMESPACES ===============================================================
+
+namespace MathTools {
+
+/// Normalize Vector (if vector size is zero, return zero point);
+template < typename VectorT > 
+inline
+void
+sane_normalize( VectorT & _vec);
+
+/// Normalize Vector (if vector size is zero, return zero point);
+template < typename VectorT > 
+inline
+VectorT
+sane_normalized( VectorT _vec);
+
+//=============================================================================
+} // namespace MathTools
+//=============================================================================
+#if defined(INCLUDE_TEMPLATES) && !defined(MATH_TOOLS_C)
+#define MATH_TOOLS_TEMPLATES
+#include "Math_ToolsT.cc"
+#endif
+//=============================================================================
+#endif // ACG_GEODESICFASTMARCHT_HH defined
+//=============================================================================

libs_required/Math_Tools/Math_ToolsT.cc

+/*===========================================================================*\
+*                                                                            *
+*                              OpenFlipper                                   *
+ *           Copyright (c) 2001-2015, RWTH-Aachen University                 *
+ *           Department of Computer Graphics and Multimedia                  *
+ *                          All rights reserved.                             *
+ *                            www.openflipper.org                            *
+ *                                                                           *
+ *---------------------------------------------------------------------------*
+ * This file is part of OpenFlipper.                                         *
+ *---------------------------------------------------------------------------*
+ *                                                                           *
+ * Redistribution and use in source and binary forms, with or without        *
+ * modification, are permitted provided that the following conditions        *
+ * are met:                                                                  *
+ *                                                                           *
+ * 1. Redistributions of source code must retain the above copyright notice, *
+ *    this list of conditions and the following disclaimer.                  *
+ *                                                                           *
+ * 2. Redistributions in binary form must reproduce the above copyright      *
+ *    notice, this list of conditions and the following disclaimer in the    *
+ *    documentation and/or other materials provided with the distribution.   *
+ *                                                                           *
+ * 3. Neither the name of the copyright holder nor the names of its          *
+ *    contributors may be used to endorse or promote products derived from   *
+ *    this software without specific prior written permission.               *
+ *                                                                           *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS       *
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED *
+ * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A           *
+ * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER *
+ * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,  *
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,       *
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR        *
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF    *
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING      *
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS        *
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.              *
+*                                                                            *
+\*===========================================================================*/
+
+/*===========================================================================*\
+*                                                                            *
+*   $Revision$                                                       *
+*   $LastChangedBy$                                                *
+*   $Date$                     *
+*                                                                            *
+\*===========================================================================*/
+
+
+
+//=============================================================================
+//
+//  Math_Tools - IMPLEMENTATION
+//
+//=============================================================================
+
+#define MATH_TOOLS_C
+
+//== INCLUDES =================================================================
+
+#include "Math_Tools.hh"
+#include <OpenMesh/Core/Geometry/MathDefs.hh>
+
+//== NAMESPACES ===============================================================
+
+namespace MathTools {
+
+template < typename VectorT > 
+inline
+void
+sane_normalize( VectorT & _vec ) {
+   
+    // Normalize all vectors (Save version)
+    if ( ! OpenMesh::is_zero( _vec.sqrnorm() ) )
+       _vec.normalize();      
+    else 
+       _vec = VectorT(0.0,0.0,0.0);
+}
+
+/// Normalize Vector (if vector size is zero, return zero point);
+template < typename VectorT > 
+inline
+VectorT
+sane_normalized( VectorT _vec) {
+     // Normalize all vectors (Save version)
+    if ( ! OpenMesh::is_zero( _vec.sqrnorm() ) )
+       return (_vec.normalize());      
+    else 
+       return ( VectorT(0.0,0.0,0.0) );
+}
+
+
+
+//=============================================================================
+} // namespace MathTools
+//=============================================================================

libs_required/Math_Tools/PCA/PCA.hh

+/*===========================================================================*\
+*                                                                            *
+*                              OpenFlipper                                   *
+ *           Copyright (c) 2001-2015, RWTH-Aachen University                 *
+ *           Department of Computer Graphics and Multimedia                  *
+ *                          All rights reserved.                             *
+ *                            www.openflipper.org                            *
+ *                                                                           *
+ *---------------------------------------------------------------------------*
+ * This file is part of OpenFlipper.                                         *
+ *---------------------------------------------------------------------------*
+ *                                                                           *
+ * Redistribution and use in source and binary forms, with or without        *
+ * modification, are permitted provided that the following conditions        *
+ * are met:                                                                  *
+ *                                                                           *
+ * 1. Redistributions of source code must retain the above copyright notice, *
+ *    this list of conditions and the following disclaimer.                  *
+ *                                                                           *
+ * 2. Redistributions in binary form must reproduce the above copyright      *
+ *    notice, this list of conditions and the following disclaimer in the    *
+ *    documentation and/or other materials provided with the distribution.   *
+ *                                                                           *
+ * 3. Neither the name of the copyright holder nor the names of its          *
+ *    contributors may be used to endorse or promote products derived from   *
+ *    this software without specific prior written permission.               *
+ *                                                                           *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS       *
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED *
+ * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A           *
+ * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER *
+ * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,  *
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,       *
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR        *
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF    *
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING      *
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS        *
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.              *
+*                                                                            *
+\*===========================================================================*/
+
+/*===========================================================================*\
+*                                                                            *
+*   $Revision$                                                       *
+*   $LastChangedBy$                                                *
+*   $Date$                     *
+*                                                                            *
+\*===========================================================================*/
+
+
+
+
+//=============================================================================
+//
+//  CLASS PCA
+//
+//=============================================================================
+
+
+#ifndef PCA_HH
+#define PCA_HH
+
+/*! \file PCA.hh
+    \brief Classes for doing Principal component analysis
+    
+    Classes for doing Principal component analysis
+*/
+
+//== INCLUDES =================================================================
+
+#include <gmm/gmm.h>
+
+//== FORWARDDECLARATIONS ======================================================
+
+//== NAMESPACES ===============================================================
+
+/// Namespace for principal Component Analysis
+namespace Pca {
+ 
+//== CLASS DEFINITION =========================================================
+	      
+/** \class PCA PCA.hh </PCA.hh>
+    \brief Class for principal component Analysis
+
+    Class for principal component Analysis
+*/
+
+template < typename VectorT >
+class PCA
+{
+  typedef gmm::dense_matrix<double>  Matrix;
+  typedef std::vector< double >      Vector;
+
+public:
+   
+  /// Constructor
+  PCA() {}
+  
+  /** Extended constructor
+    Parameters : see pca()
+  */
+  PCA( std::vector< VectorT >& _points, VectorT& _first , VectorT& _second , VectorT& _third);
+ 
+  /// Destructor
+  ~PCA();
+  
+  /**
+    Compute center of gravity for a vector of points
+    @param _points set of points
+  */
+  inline VectorT center_of_gravity(const std::vector< VectorT >& _points );
+  
+  /**
+    Compute the principal component analysys for a vector of points
+    @param _points set of points
+    @param _first  First main axis
+    @param _second second main axis
+    @param _third  third main axis
+  */
+  void pca(std::vector< VectorT >& _points , VectorT& _first , VectorT& _second , VectorT& _third);
+
+
+  std::vector<double>& getLastEigenValues();
+
+  bool SymRightEigenproblem( Matrix &_mat_A, Matrix & _mat_VR,
+                             std::vector< double > & _vec_EV );
+private:
+
+  std::vector<double> lastEigenValues_;
+
+};
+
+
+//=============================================================================
+} //namespace Pca
+//=============================================================================
+#if defined(INCLUDE_TEMPLATES) && !defined(PCA_C)
+#define PCA_TEMPLATES
+#include "PCAT.cc"
+#endif
+//=============================================================================
+#endif // PCA_HH defined
+//=============================================================================
+

libs_required/Math_Tools/PCA/PCAT.cc

+/*===========================================================================*\
+*                                                                            *
+*                              OpenFlipper                                   *
+ *           Copyright (c) 2001-2015, RWTH-Aachen University                 *
+ *           Department of Computer Graphics and Multimedia                  *
+ *                          All rights reserved.                             *
+ *                            www.openflipper.org                            *
+ *                                                                           *
+ *---------------------------------------------------------------------------*
+ * This file is part of OpenFlipper.                                         *
+ *---------------------------------------------------------------------------*
+ *                                                                           *
+ * Redistribution and use in source and binary forms, with or without        *
+ * modification, are permitted provided that the following conditions        *
+ * are met:                                                                  *
+ *                                                                           *
+ * 1. Redistributions of source code must retain the above copyright notice, *
+ *    this list of conditions and the following disclaimer.                  *
+ *                                                                           *
+ * 2. Redistributions in binary form must reproduce the above copyright      *
+ *    notice, this list of conditions and the following disclaimer in the    *
+ *    documentation and/or other materials provided with the distribution.   *
+ *                                                                           *
+ * 3. Neither the name of the copyright holder nor the names of its          *
+ *    contributors may be used to endorse or promote products derived from   *
+ *    this software without specific prior written permission.               *
+ *                                                                           *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS       *
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED *
+ * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A           *
+ * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER *
+ * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,  *
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,       *
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR        *
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF    *
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING      *
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS        *
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.              *
+*                                                                            *
+\*===========================================================================*/
+
+/*===========================================================================*\
+*                                                                            *
+*   $Revision$                                                       *
+*   $LastChangedBy$                                                *
+*   $Date$                     *
+*                                                                            *
+\*===========================================================================*/
+
+
+
+
+//=============================================================================
+//
+//  CLASS PCA - IMPLEMENTATION
+//
+//=============================================================================
+
+#define PCA_C
+
+//== INCLUDES =================================================================
+
+#include "PCA.hh"
+
+extern "C" {
+
+    typedef void (*U_fp)(...);
+
+    int dsyev_( char *jobz, char *uplo, long int *n, double *a,
+                long int *lda, double *w, double *work, long int *lwork,
+                long int *info);
+
+}
+
+// typedef gmm::dense_matrix< double > Matrix;
+// typedef std::vector< double > Vector;
+
+//! Module-wide pointer to working array
+static double * p_work_double = 0;
+
+//! Module-wide size of working array
+static int m_workSize_double = 0;
+
+//== NAMESPACES ===============================================================
+
+namespace Pca {
+
+//== IMPLEMENTATION ==========================================================
+
+template < typename VectorT >
+PCA< VectorT >::PCA( std::vector< VectorT >& _points , VectorT& _first , VectorT& _second , VectorT& _third)
+{
+  pca(_points,_first,_second,_third);
+}
+
+template < typename VectorT >
+PCA< VectorT >::~PCA()
+{
+  if ( p_work_double != 0){
+    delete [] p_work_double;
+    p_work_double = 0;
+    m_workSize_double = 0;
+  }
+}
+
+template < typename VectorT >
+inline
+VectorT  
+PCA< VectorT >::
+center_of_gravity(const std::vector< VectorT >& _points ) {
+   // compute cog of Points
+   VectorT cog(0.0, 0.0, 0.0);
+   
+   for (uint i = 0 ; i < _points.size() ; ++i ) {
+     cog = cog + _points[i];
+   }
+   
+   cog = cog / ( typename VectorT::value_type )_points.size();
+
+   return cog;
+}
+
+template < typename VectorT >
+bool 
+PCA< VectorT >::SymRightEigenproblem( Matrix &_mat_A, Matrix & _mat_VR,
+                           std::vector< double > & _vec_EV )
+{
+    char jobz, uplo;
+    long int n, lda;
+    long int lwork, info;
+    double size;
+
+    jobz = 'V';
+    uplo = 'U';
+    n = gmm::mat_nrows( _mat_A );
+    lda = n;
+
+    info = 0;
+    // compute optimal size of working array
+    lwork = -1;
+    dsyev_( &jobz, &uplo, &n, &_mat_A( 0, 0 ), &lda, &_vec_EV[ 0 ],
+            &size, &lwork, &info );
+
+    if( info != 0 )
+        return false;
+
+
+    if( (long int) size > m_workSize_double )
+    {
+        if( p_work_double)
+            delete [] p_work_double;
+
+        m_workSize_double = (long int) size;
+
+        p_work_double= new double[ m_workSize_double ];
+    }
+    lwork = m_workSize_double;
+
+    // compute eigenvalues / eigenvectors
+    dsyev_( &jobz, &uplo, &n, &_mat_A( 0, 0 ), &lda, &_vec_EV[ 0 ],
+            p_work_double, &lwork, &info );
+
+    if( info != 0 )
+        return false;
+
+    // copy eigenvectors to matrix
+    gmm::copy( _mat_A, _mat_VR );
+
+    return true;
+}
+
+
+template < typename VectorT >
+void 
+PCA< VectorT >::pca(std::vector< VectorT >& _points , VectorT& _first , VectorT& _second , VectorT& _third)
+{
+   // compute Mean of Points
+   const VectorT cog = center_of_gravity(_points);
+   
+   //build covariance Matrix
+   Matrix points(3 , _points.size() );
+  
+   for ( uint i = 0 ; i < _points.size() ; ++i)
+   {
+       points(0 , i ) = ( _points[i] - cog) [0];
+       points(1 , i ) = ( _points[i] - cog) [1];
+       points(2 , i ) = ( _points[i] - cog) [2];
+   }
+  
+  Matrix cov(3,3);
+  gmm::mult(points,gmm::transposed(points),cov );
+  
+  Matrix EV(3,3);
+  std::vector< double > ew(3);
+  
+
+  if ( !SymRightEigenproblem( cov, EV ,ew ) )
+    std::cerr << "Error: Could not compute Eigenvectors for PCA" << std::endl;
+
+  int maximum,middle,minimum;
+  
+  if ( (ew[0] > ew[1]) && (ew[0] > ew[2]) ) {
+    maximum = 0;
+  } else {
+    if ( (ew[1] > ew[0]) && (ew[1] > ew[2]) )
+         maximum = 1;
+       else
+         maximum = 2;
+  }
+  
+  if ( (ew[0] < ew[1]) && (ew[0] < ew[2]) )
+    minimum = 0;
+  else if ( (ew[1] < ew[0]) && (ew[1] < ew[2]) )
+         minimum = 1;
+       else
+         minimum = 2;         
+
+  if ( (minimum != 0 ) && ( maximum != 0 ) )       
+    middle = 0;
+  else  
+    if ( (minimum != 1 ) && ( maximum != 1 ) )       
+      middle = 1;
+    else
+      middle = 2;
+  
+  _first  = VectorT( EV(0,maximum) , EV(1,maximum) , EV(2,maximum) );
+  _second = VectorT( EV(0,middle)  , EV(1,middle)  , EV(2,middle ) );
+
+  _first  = _first.normalize();
+  _second = _second.normalize();
+  _third  = _first % _second; 
+
+  //remember the eigenvalues
+  lastEigenValues_.clear();
+  lastEigenValues_.push_back( ew[maximum] );
+  lastEigenValues_.push_back( ew[middle] );
+  lastEigenValues_.push_back( ew[minimum] );
+}
+//-----------------------------------------------------------------------------
+
+template < typename VectorT >
+inline
+std::vector<double>&
+PCA< VectorT >::getLastEigenValues()
+{
+  return lastEigenValues_;
+}
+
+
+//=============================================================================
+
+} //namespace Pca
+
+//=============================================================================