Commit 1ca729c4 authored by Jan Möbius's avatar Jan Möbius
parent 9dd58fb2
#== SYSTEM PART -- DON'T TOUCH ==============================================
include $(ACGMAKE)/Config
#==============================================================================
SUBDIRS = $(call find-subdirs)
PACKAGES := ACG2 math openmesh2
PROJ_LIBS =
MODULES := cxx
#== SYSTEM PART -- DON'T TOUCH ==============================================
include $(ACGMAKE)/Rules
#==============================================================================
//=============================================================================
//
// OpenFlipper
// Copyright (C) 2008 by Computer Graphics Group, RWTH Aachen
// www.openflipper.org
//
//-----------------------------------------------------------------------------
//
// License
//
// OpenFlipper is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// OpenFlipper is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with OpenFlipper. If not, see <http://www.gnu.org/licenses/>.
//
//-----------------------------------------------------------------------------
//
// $Revision$
// $Author$
// $Date$
//
//=============================================================================
//=============================================================================
//
// IMPLEMENTATION
//
//=============================================================================
#define GEOMETRYFUNCTIONS_C
//== INCLUDES =================================================================
#include "GeometryFunctions.hh"
#include <OpenMesh/Core/Geometry/MathDefs.hh>
#include <math.h>
#include <iostream>
//== NAMESPACES ===============================================================
namespace GeometryFunctions {
//== IMPLEMENTATION ==========================================================
/// Return false if x is not a number
inline bool isNan(double x) {
return (x != x);
}
template < typename VectorT >
VectorT project_to_edge(const VectorT& _start , const VectorT& _end , const VectorT& _point )
{
VectorT direction = ( _end - _start );
assert( fabs(direction.norm()) > 0.0000000001) ;
const VectorT projected_point = ( ( _point - _start ) | direction ) * direction + _start;
if ( ( ( projected_point - _start ) | direction ) > ( ( _end - _start ) | direction ) )
return _end;
if ( ( ( projected_point - _start ) | direction ) < 0 )
return _start;
return projected_point;
}
template < typename VectorT , typename ValueT >
inline
ValueT
dist_plane(const VectorT& _porigin,
const VectorT& _pnormal,
const VectorT& _point)
{
assert( fabs(_pnormal.norm()) > 0.0000000001) ;
return( ((_point-_porigin) | _pnormal));
}
template < typename VectorT >
inline
VectorT
project_to_plane(const VectorT& _porigin,
const VectorT& _pnormal,
const VectorT& _point)
{
return (_point - _pnormal * dist_plane< VectorT , double >( _porigin , _pnormal , _point ) );
}
template < typename VectorT , typename ValueT >
ValueT
get_fullangle( VectorT _vector1 , VectorT _vector2 , const VectorT& _normal , bool& skip )
{
//Project vectors into tangent plane defined by _normal
_vector1 = _vector1 - _normal * ( _vector1 | _normal );
_vector2 = _vector2 - _normal * ( _vector2 | _normal );
_vector1.normalize();
_vector2.normalize();
//calculate projection onto right Vector (used to decide if vector2 is left or right of vector1
const double right = ( ( _normal % _vector1 ) | _vector2 ) ;
double sp = ( _vector1 | _vector2 );
//Catch some errors with scalar product and the following acos
if (sp < -1.0) {
sp = -1.0;
}
if (sp > 1.0) {
sp = 1.0;
}
double angle = acos(sp);
// catch some possible nans
skip = ( isNan(right) || isNan(angle) ) ;
if ( right < 0 ) {
angle = 2 * M_PI - angle;
}
return angle;
}
template < typename ValueT >
inline
ValueT
angle_dist( const ValueT& angle0 , const ValueT& angle1 ) {
ValueT dist = fabs( angle1 - angle0 );
return ( std::min( dist , 2 * M_PI - dist) );
}
template < typename ValueT >
inline
ValueT
get_angle( const ValueT& _cos ,
const ValueT& _sin )
{
const double angle_asin = asin( OpenMesh::sane_aarg(_sin) );
const double angle_acos = acos( OpenMesh::sane_aarg(_cos) );
if ( angle_asin >= 0 ) { //Quadrant 1,2
if ( angle_acos >= 0 ) { // Quadrant 1
return angle_asin;
} else { //Quadrant 2
return (M_PI - angle_asin);
}
} else { //Quadrant 3,4
if ( angle_acos >= 0 ) { // Quadrant 4
return (2 * M_PI + angle_asin);
} else { //Quadrant 3
return (M_PI - angle_asin);
}
}
}
template < typename ValueT >
inline
ValueT
rad_to_deg( const ValueT& _angle ) {
return ( _angle / M_PI * 180);
}
template < typename ValueT >
inline
ValueT
deg_to_rad( const ValueT& _angle ) {
return ( _angle / 180 * M_PI );
}
template<class VectorT>
int is_obtuse(const VectorT& _p0,
const VectorT& _p1,
const VectorT& _p2 )
{
const double a0 = ((_p1-_p0)|(_p2-_p0));
if ( a0<0.0) return 1;
else
{
const double a1 = ((_p0-_p1)|(_p2-_p1));
if (a1 < 0.0 ) return 2;
else
{
const double a2 = ((_p0-_p2)|(_p1-_p2));
if (a2 < 0.0 ) return 3;
else return 0;
}
}
}
//=============================================================================
} // GeometryFunctions Namespace
//=============================================================================
//=============================================================================
//
// OpenFlipper
// Copyright (C) 2008 by Computer Graphics Group, RWTH Aachen
// www.openflipper.org
//
//-----------------------------------------------------------------------------
//
// License
//
// OpenFlipper is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// OpenFlipper is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with OpenFlipper. If not, see <http://www.gnu.org/licenses/>.
//
//-----------------------------------------------------------------------------
//
// $Revision$
// $Author$
// $Date$
//
//=============================================================================
//=============================================================================
//
//
//=============================================================================
#ifndef GEOMETRYFUNCTIONS_HH
#define GEOMETRYFUNCTIONS_HH
/*! \file GeometryFunctions.hh
\brief Functions for geometric operations
General file with template functions doing for example
projections,...
*/
//== INCLUDES =================================================================
//== FORWARDDECLARATIONS ======================================================
//== NAMESPACES ===============================================================
/// Namespace providing different geometric functions
namespace GeometryFunctions {
//== CLASS DEFINITION =========================================================
/**
project one point to an edge. If its projection is not on the edge itself, the start or the endpoint is returned
@param _start beginning of edge
@param _end end of the edge
@param _point point to be projected
*/
template < typename VectorT >
VectorT project_to_edge(const VectorT& _start ,
const VectorT& _end ,
const VectorT& _point );
/**
Checks the distance from a point to a plane
@param _porigin Planes origin
@param _pnormal Plane normal
@param _point point to test
@return distance
*/
template < typename VectorT , typename ValueT >
inline
ValueT
dist_plane(const VectorT& _porigin,
const VectorT& _pnormal,
const VectorT& _point);
/**
projects a point to a plane
@param _porigin Planes origin
@param _pnormal Plane normal
@param _point point to project
@return projected point
*/
template < typename VectorT >
inline
VectorT
project_to_plane(const VectorT& _porigin,
const VectorT& _pnormal,
const VectorT& _point);
/** Return a fully parametrized angle
@param _vector1 vector pointing away from center, angle = 0
@param _vector2 vector for which the angle should be calculated
@param _normal the normal used to compute if vector2 is left or right of vecor1
@param _skip Flag to catch nan. If true nan occured and value should not be used
@return the angle (0 .. 2 * PI)
*/
template < typename VectorT , typename ValueT >
ValueT
get_fullangle( VectorT _vector1 ,
VectorT _vector2 ,
const VectorT& _normal ,
bool& skip );
/** Calculate the difference between two angles
@param _angle0 angle1
@param _angle1 angle2
@return The difference between the angles (0..PI)
*/
template < typename ValueT >
inline
ValueT
angle_dist( const ValueT& angle0 ,
const ValueT& angle1 );
/** Calculate the correct 2D angle if cos and sin of the angle are given
This function calculates based on the signs of the acos and asin of the
given angles, in which quadrant the angle is and returns the full angle
in radians
@param _cos cos of angle
@param _sin sin of angle
@return angle
*/
template < typename ValueT >
inline
ValueT
get_angle( const ValueT& _cos ,
const ValueT& _sin );
/** Convert angle from radians to degree
@param _angle in radians
@return angle in degree
*/
template < typename ValueT >
inline
ValueT
rad_to_deg( const ValueT& _angle );
/** Convert angle from degree to radians
@param _angle in degree
@return angle in radians
*/
template < typename ValueT >
inline
ValueT
deg_to_rad( const ValueT& _angle );
/**
* test angles in triangle
* return 0 if all smaller than 90°
* return 1 if angle at _p0 ist greater than 90°
* return 2 if angle at _p1 ist greater than 90°
* return 3 if angle at _p2 ist greater than 90°
*/
template<class VectorT>
int is_obtuse(const VectorT& _p0,
const VectorT& _p1,
const VectorT& _p2 );
//=============================================================================
} // GeometryFunctions Namespace
//=============================================================================
#if defined(INCLUDE_TEMPLATES) && !defined(GEOMETRYFUNCTIONS_C)
#define GEOMETRYFUNCTIONS_TEMPLATES
#include "GeometryFunctions.cc"
#endif
//=============================================================================
#endif // GEOMETRYFUNCTIONS_HH defined
//=============================================================================
#== SYSTEM PART -- DON'T TOUCH ==============================================
include $(ACGMAKE)/Config
#==============================================================================
SUBDIRS = $(call find-subdirs)
PACKAGES := gmm3
PROJ_LIBS = Tools/math
MODULES := cxx
#== SYSTEM PART -- DON'T TOUCH ==============================================
include $(ACGMAKE)/Rules
#==============================================================================
//=============================================================================
//
// OpenFlipper
// Copyright (C) 2008 by Computer Graphics Group, RWTH Aachen
// www.openflipper.org
//
//-----------------------------------------------------------------------------
//
// License
//
// OpenFlipper is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// OpenFlipper is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with OpenFlipper. If not, see <http://www.gnu.org/licenses/>.
//
//-----------------------------------------------------------------------------
//
// $Revision$
// $Author$
// $Date$
//
//=============================================================================
//=============================================================================
//
// CLASS ICP - IMPLEMENTATION
//
//=============================================================================
#define ICP_C
//== INCLUDES =================================================================
#include "ICP.hh"
#include <float.h>
//== 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(std::vector< VectorT >& _points1 , 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);
}
//=============================================================================
} //namespace ICP
//=============================================================================
//=============================================================================
//
// OpenFlipper
// Copyright (C) 2008 by Computer Graphics Group, RWTH Aachen
// www.openflipper.org
//
//-----------------------------------------------------------------------------
//
// License
//
// OpenFlipper is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// OpenFlipper is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with OpenFlipper. If not, see <http://www.gnu.org/licenses/>.
//
//-----------------------------------------------------------------------------
//
// $Revision$
// $Author$
// $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.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.
* @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 variance of first point set
* @param _scale1 variance of second point set
* @param _rotation Rotation between point sets ( rotation(_points1) -> _points2
*
*/
template < typename VectorT , typename QuaternionT >
void icp(std::vector< VectorT >& _points1 , std::vector< VectorT >& _points2 , VectorT& _cog1 , VectorT& _cog2 , double& _scale1 , double& _scale2 , QuaternionT& _rotation );
//=============================================================================
} //namespace ICP
//=============================================================================
#if defined(INCLUDE_TEMPLATES) && !defined(ICP_C)
#define ICP_TEMPLATES
#include "ICP.cc"
#endif
//=============================================================================
#endif // ICP_HH defined
//=============================================================================
//=============================================================================
//
// OpenFlipper
// Copyright (C) 2008 by Computer Graphics Group, RWTH Aachen
// www.openflipper.org
//
//-----------------------------------------------------------------------------
//
// License
//
// OpenFlipper is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// OpenFlipper is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with OpenFlipper. If not, see <http://www.gnu.org/licenses/>.
//
//-----------------------------------------------------------------------------
//
// $Revision$
// $Author$
// $Date$
//
//=============================================================================
//=============================================================================
//
// Math_Tools - IMPLEMENTATION
//
//=============================================================================
#define MATH_TOOLS_C
//== INCLUDES =================================================================
#include "Math_Tools.hh"
#include <OpenMesh/Core/Geometry/MathDefs.hh>