Commit fd9a5728 authored by untereiner's avatar untereiner

[CGoGN] adding ShapeMatching + Linear Shape Matching + Quadratic Shape Matching

parent 6a779de9
/*******************************************************************************
* CGoGN: Combinatorial and Geometric modeling with Generic N-dimensional Maps *
* version 0.1 *
* Copyright (C) 2009-2013, IGG Team, ICube, University of Strasbourg *
* *
* This library 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 2.1 of the License, or (at your *
* option) any later version. *
* *
* This library 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 this library; if not, write to the Free Software Foundation, *
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. *
* *
* Web site: http://cgogn.unistra.fr/ *
* Contact information: cgogn@unistra.fr *
* *
*******************************************************************************/
#include <Eigen/Dense>
#ifndef _SHAPE_MATCHING_H_
#define _SHAPE_MATCHING_H_
namespace CGoGN
{
namespace Algo
{
namespace Surface
{
namespace Simulation
{
namespace ShapeMatching
{
template <typename PFP>
class ShapeMatching
{
public:
typedef typename PFP::MAP MAP;
typedef typename PFP::VEC3 VEC3;
typedef typename PFP::REAL REAL;
protected:
VertexAttribute<VEC3>& m_position;
VertexAttribute<REAL>& m_mass;
VertexAttribute<REAL> m_goal;
// q_{i} = x^{0} - x^{0}_{cm}
std::vector<VEC3> m_q;
// p_{i}
std::vector<VEC3> m_p;
public:
ShapeMatching(VertexAttribute<VEC3>& position, VertexAttribute<REAL>& mass);
~ShapeMatching();
VEC3 massCenter();
void initialize();
void shapeMatch();
};
} // namespace ShapeMatching
} // namespace Simulation
} // namespace Surface
} // namespace Algo
} // namespace CGoGN
#include "shapeMatching.hpp"
#endif
/*******************************************************************************
* CGoGN: Combinatorial and Geometric modeling with Generic N-dimensional Maps *
* version 0.1 *
* Copyright (C) 2009-2013, IGG Team, ICube, University of Strasbourg *
* *
* This library 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 2.1 of the License, or (at your *
* option) any later version. *
* *
* This library 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 this library; if not, write to the Free Software Foundation, *
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. *
* *
* Web site: http://cgogn.unistra.fr/ *
* Contact information: cgogn@unistra.fr *
* *
*******************************************************************************/
namespace CGoGN
{
namespace Algo
{
namespace Surface
{
namespace Simulation
{
namespace ShapeMatching
{
template <typename PFP>
ShapeMatching<PFP>::ShapeMatching(VertexAttribute<VEC3>& position, VertexAttribute<REAL>& mass):
m_position(position),
m_mass(mass)
{
unsigned int nbE = m_position.nbElements();
m_p.reserve(nbE);
m_q.reserve(nbE);
m_goal = this->m_map.template getAttribute<VEC3, VERTEX>("goal");
if(!m_goal.isValid())
m_goal = this->m_map.template addAttribute<VEC3, VERTEX>("goal");
}
template <typename PFP>
ShapeMatching<PFP>::~ShapeMatching()
{
if(m_goal.isValid())
m_map.template removeAttribute<VEC3, VERTEX>(m_goal);
}
VEC3 ShapeMatching<PFP>::massCenter()
{
VEC3 xcm(0);
REAL m = 0.0;
for(unsigned int i = m_position.begin() ; i < m_position.end() ; i = m_position.next())
{
xcm += m_mass[i] * m_p[i];
m += m_mass[i];
}
xcm /= m;
return xcm;
}
/**
* Initialize pre-computed ....
* First, \f$ x^{0}_{cm} \f$
* In a second step, \f$ q_{i} = x^{0}_{i} - x^{0}_{cm} \f$
*/
template <typename PFP>
void ShapeMatching<PFP>::initialize()
{
VEC3 x0cm = massCenter();
for(unsigned int i = m_position.begin() ; i < m_position.end() ; i = m_position.next())
{
m_q[i] = VEC3(m_position[i] - x0cm); //q_{i} = x^{0}_{i} - x^{0}_{cm}
}
}
template <typename PFP>
void ShapeMatching<PFP>::shapeMatch()
{
//1.
VEC3 xcm = massCenter();
for(unsigned int i = m_position.begin() ; i < m_position.end() ; i = m_position.next())
{
m_p[i] = VEC3(m_position[i] - xcm); //p_{i} = x_{i} - x_{cm}
}
//2.
Eigen::Matrix3f apq;
apq = Eigen::Matrix3f::Zero();
for(unsigned int i=0 ; i < p.size() ; ++i)
{
apq(0,0) += m_p[i][0] * m_q[i][0];
apq(0,1) += m_p[i][0] * m_q[i][1];
apq(0,2) += m_p[i][0] * m_q[i][2];
apq(1,0) += m_p[i][1] * m_q[i][0];
apq(1,1) += m_p[i][1] * m_q[i][1];
apq(1,2) += m_p[i][1] * m_q[i][2];
apq(2,0) += m_p[i][2] * m_q[i][0];
apq(2,1) += m_p[i][2] * m_q[i][1];
apq(2,2) += m_p[i][2] * m_q[i][2];
}
Eigen::Matrix3f S = apq.transpose() * apq ; //symmetric matrix
//3. Jacobi Diagonlisation
Eigen::JacobiSVD<Eigen::Matrix3f> svd(S, ComputeFullU);
Eigen::Matrix3f SR = svd.matrixU(); // diagonalization with Jacobi rotations
S(0,0) = 1.0f/sqrt(S(0,0));
S(1,1) = 1.0f/sqrt(S(1,1));
S(2,2) = 1.0f/sqrt(S(2,2));
S = SR * S * SR.transpose();
Eigen::Matrix3f R = apq * S; //S^{-1}
//4.
for(unsigned int i = m_goal.begin() ; i < m_goal.end() ; i = m_goal.next())
{
m_goal[i] = R * m_q[i] + xcm; // g_{i} = R * q_i + x_{cm}
}
}
} // namespace ShapeMatching
} // namespace Simulation
} // namespace Surface
} // namespace Algo
} // namespace CGoGN
/*******************************************************************************
* CGoGN: Combinatorial and Geometric modeling with Generic N-dimensional Maps *
* version 0.1 *
* Copyright (C) 2009-2013, IGG Team, ICube, University of Strasbourg *
* *
* This library 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 2.1 of the License, or (at your *
* option) any later version. *
* *
* This library 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 this library; if not, write to the Free Software Foundation, *
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. *
* *
* Web site: http://cgogn.unistra.fr/ *
* Contact information: cgogn@unistra.fr *
* *
*******************************************************************************/
#ifndef _SHAPE_MATCHING_LINEAR_H_
#define _SHAPE_MATCHING_LINEAR_H_
namespace CGoGN
{
namespace Algo
{
namespace Surface
{
namespace Simulation
{
namespace ShapeMatching
{
template <typename PFP>
class ShapeMatchingLinear : public ShapeMatching<PFP>
{
public:
typedef typename PFP::MAP MAP;
typedef typename PFP::VEC3 VEC3;
typedef typename PFP::REAL REAL;
protected:
REAL m_beta;
// A_{qq}
Eigen::Matrix3f m_aqq;
private:
void computeAqqMatrix();
public:
ShapeMatchingLinear(VertexAttribute<VEC3>& position, VertexAttribute<REAL>& mass, REAL beta):
ShapeMatching(position, mass),
m_beta(beta)
{ }
~ShapeMatchingLinear()
{ }
void initialize();
void shapeMatch();
};
} // namespace ShapeMatching
} // namespace Simulation
} // namespace Surface
} // namespace Algo
} // namespace CGoGN
#include "shapeMatchingLinear.hpp"
#endif
/*******************************************************************************
* CGoGN: Combinatorial and Geometric modeling with Generic N-dimensional Maps *
* version 0.1 *
* Copyright (C) 2009-2013, IGG Team, ICube, University of Strasbourg *
* *
* This library 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 2.1 of the License, or (at your *
* option) any later version. *
* *
* This library 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 this library; if not, write to the Free Software Foundation, *
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. *
* *
* Web site: http://cgogn.unistra.fr/ *
* Contact information: cgogn@unistra.fr *
* *
*******************************************************************************/
namespace CGoGN
{
namespace Algo
{
namespace Surface
{
namespace Simulation
{
void ShapeMatching<PFP>::computeAqqMatrix()
{
//A_{qq} = (Sum_i{ m_{i} q_{i} q_i^{T} })^{-1}
m_aqq = Eigen::Matrix3f::Zero();
for(unsigned int i = 0 ; i < m_q.size() ; ++i)
{
m_aqq(0,0) += m_q[i][0] * m_q[i][0];
m_aqq(1,0) += m_q[i][0] * m_q[i][1];
m_aqq(2,0) += m_q[i][0] * m_q[i][2];
m_aqq(0,1) += m_q[i][1] * m_q[i][0];
m_aqq(1,1) += m_q[i][1] * m_q[i][1];
m_aqq(2,1) += m_q[i][1] * m_q[i][2];
m_aqq(0,2) += m_q[i][2] * m_q[i][0];
m_aqq(1,2) += m_q[i][2] * m_q[i][1];
m_aqq(2,2) += m_q[i][2] * m_q[i][2];
}
m_aqq = m_aqq.inverse();
}
template <typename PFP>
void ShapeMatchingLinear<PFP>::initialize()
{
this->initialize();
computeAqqMatrix();
}
template <typename PFP>
void ShapeMatchingLinear<PFP>::shapeMatch()
{
//1.
VEC3 xcm = massCenter();
for(unsigned int i = m_position.begin() ; i < m_position.end() ; i = m_position.next())
{
m_p[i] = VEC3(m_position[i] - xcm); //p_{i} = x_{i} - x_{cm}
}
//2.
Eigen::Matrix3f apq;
apq = Eigen::Matrix3f::Zero();
for(unsigned int i=0 ; i < p.size() ; ++i)
{
apq(0,0) += m_p[i][0] * m_q[i][0];
apq(0,1) += m_p[i][0] * m_q[i][1];
apq(0,2) += m_p[i][0] * m_q[i][2];
apq(1,0) += m_p[i][1] * m_q[i][0];
apq(1,1) += m_p[i][1] * m_q[i][1];
apq(1,2) += m_p[i][1] * m_q[i][2];
apq(2,0) += m_p[i][2] * m_q[i][0];
apq(2,1) += m_p[i][2] * m_q[i][1];
apq(2,2) += m_p[i][2] * m_q[i][2];
}
Eigen::Matrix3f S = apq.transpose() * apq ; //symmetric matrix
//3. Jacobi Diagonlisation
Eigen::JacobiSVD<Eigen::Matrix3f> svd(S, ComputeFullU);
Eigen::Matrix3f SR = svd.matrixU(); // diagonalization with Jacobi rotations
S(0,0) = 1.0f/sqrt(S(0,0));
S(1,1) = 1.0f/sqrt(S(1,1));
S(2,2) = 1.0f/sqrt(S(2,2));
S = SR * S * SR.transpose();
Eigen::Matrix3f R = apq * S; //S^{-1}
//4.
Eigen::Matrix3f A = apq * m_aqq; //
REAL det = A.determinant();
REAL detAInv = 1.0f/powf(det,1.0f/3.0f);
if(std::isfinite(detAInv))
{
R(0,0) += (m_beta * A(0,0) * detAInv) + ((1.0f - m_beta) * R(0,0));
R(0,1) += (m_beta * A(0,1) * detAInv) + ((1.0f - m_beta) * R(0,1));
R(0,2) += (m_beta * A(0,2) * detAInv) + ((1.0f - m_beta) * R(0,2));
R(1,0) += (m_beta * A(1,0) * detAInv) + ((1.0f - m_beta) * R(1,0));
R(1,1) += (m_beta * A(1,1) * detAInv) + ((1.0f - m_beta) * R(1,1));
R(1,2) += (m_beta * A(1,2) * detAInv) + ((1.0f - m_beta) * R(1,2));
R(2,0) += (m_beta * A(2,0) * detAInv) + ((1.0f - m_beta) * R(2,0));
R(2,1) += (m_beta * A(2,1) * detAInv) + ((1.0f - m_beta) * R(2,1));
R(2,2) += (m_beta * A(2,2) * detAInv) + ((1.0f - m_beta) * R(2,2));
}
else
{
//no linear deformation (planar config ?), applying identity
R.identity();
}
//5.
for(unsigned int i = m_goal.begin() ; i < m_goal.end() ; i = m_goal.next())
{
m_goal[i] = R * m_q[i] + xcm; // g_{i} = R * q_i + x_{cm}
}
}
} // namespace Simulation
} // namespace Surface
} // namespace Algo
} // namespace CGoGN
/*******************************************************************************
* CGoGN: Combinatorial and Geometric modeling with Generic N-dimensional Maps *
* version 0.1 *
* Copyright (C) 2009-2013, IGG Team, ICube, University of Strasbourg *
* *
* This library 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 2.1 of the License, or (at your *
* option) any later version. *
* *
* This library 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 this library; if not, write to the Free Software Foundation, *
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. *
* *
* Web site: http://cgogn.unistra.fr/ *
* Contact information: cgogn@unistra.fr *
* *
*******************************************************************************/
#ifndef _SHAPE_MATCHING_QUADRATIC_H_
#define _SHAPE_MATCHING_QUADRATIC_H_
namespace CGoGN
{
namespace Algo
{
namespace Surface
{
namespace Simulation
{
namespace ShapeMatching
{
template <typename PFP>
class ShapeMatchingQuadratic : public ShapeMatching<PFP>
{
public:
typedef typename PFP::MAP MAP;
typedef typename PFP::VEC3 VEC3;
typedef typename PFP::REAL REAL;
protected:
REAL m_beta;
// A_{qq}
Eigen::Matrix3f m_aqq;
Eigen::Vector9f m_qtild;
// q^{~}_{i} where q^{~} = [q_x, q_y, q_z, q^2_x, q^2_y, q^2_z, q_x q_y , q_y q_z, q_z q_x]
std::vector<Eigen::Vector9f> m_qtild;
private:
void computeAqqMatrix();
public:
ShapeMatchingQuadratic(VertexAttribute<VEC3>& position, VertexAttribute<REAL>& mass, REAL beta):
ShapeMatching(position, mass),
m_beta(beta)
{ }
~ShapeMatchingQuadratic()
{ }
void initialize();
void shapeMatch();
};
} // namespace ShapeMatching
} // namespace Simulation
} // namespace Surface
} // namespace Algo
} // namespace CGoGN
#include "shapeMatchingQuadratic.hpp"
#endif
/*******************************************************************************
* CGoGN: Combinatorial and Geometric modeling with Generic N-dimensional Maps *
* version 0.1 *
* Copyright (C) 2009-2013, IGG Team, ICube, University of Strasbourg *
* *
* This library 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 2.1 of the License, or (at your *
* option) any later version. *
* *
* This library 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 this library; if not, write to the Free Software Foundation, *
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. *
* *
* Web site: http://cgogn.unistra.fr/ *
* Contact information: cgogn@unistra.fr *
* *
*******************************************************************************/
namespace CGoGN
{
namespace Algo
{
namespace Surface
{
namespace Simulation
{
template <typename PFP>
void ShapeMatchingLinear<PFP>::initialize()
{
VEC3 x0cm = massCenter();
computeAqqMatrix();
}
} // namespace Simulation
} // namespace Surface
} // namespace Algo
} // namespace CGoGN
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment