manolas
/
vcglib
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Added the new version of the point matching code. Now it is under space and no more under math

This commit is contained in:
Paolo Cignoni 2012-11-22 23:29:43 +00:00
parent 26fc41faf5
commit 039bc9bafa
1 changed files with 205 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

205
vcg/space/point_matching.h Normal file
View File

@ -0,0 +1,205 @@
/****************************************************************************
* VCGLib o o *
* Visual and Computer Graphics Library o o *
* _ O _ *
* Copyright(C) 2004 \/)\/ *
* Visual Computing Lab /\/| *
* ISTI - Italian National Research Council | *
* \ *
* All rights reserved. *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License, or *
* (at your option) any later version. *
* *
* This program 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 General Public License (http://www.gnu.org/licenses/gpl.txt) *
* for more details. *
* *
****************************************************************************/
#ifndef _VCG_MATH_POINTMATCHING_H
#define _VCG_MATH_POINTMATCHING_H
#include <vcg/math/quaternion.h>
#include <vcg/math/matrix44.h>
#include <eigenlib/Eigen/Dense>
#include <eigenlib/Eigen/Eigenvalues>
#include <iostream>
namespace vcg
{
/*! \brief Compute cross covariance
It computes the cross covariance matrix of two set of 3D points P and X;
it returns also the barycenters of P and X.
Ref:
Besl, McKay
A method for registration of 3d Shapes
IEEE TPAMI Vol 14, No 2 1992
*/
template <class S >
void ComputeCrossCovarianceMatrix(const std::vector<Point3<S> > &spVec, Point3<S> &spBarycenter,
const std::vector<Point3<S> > &tpVec, Point3<S> &tpBarycenter,
Eigen::Matrix3d &m)
{
assert(spVec.size()==tpVec.size());
m.setZero();
spBarycenter.SetZero();
tpBarycenter.SetZero();
Eigen::Vector3d spe;
Eigen::Vector3d tpe;
typename std::vector <Point3<S> >::const_iterator si,ti;
for(si=spVec.begin(),ti=tpVec.begin();si!=spVec.end();++si,++ti){
spBarycenter+=*si;
tpBarycenter+=*ti;
si->ToEigenVector(spe);
ti->ToEigenVector(tpe);
m+=spe*tpe.transpose();
}
spBarycenter/=spVec.size();
tpBarycenter/=tpVec.size();
spBarycenter.ToEigenVector(spe);
tpBarycenter.ToEigenVector(tpe);
m/=spVec.size();
m-=spe*tpe.transpose();
}
/*! \brief Compute the roto-translation that applied to PMov bring them onto Pfix
* Rotation is computed as a quaternion.
*
* E.g. it find a matrix such that:
*
* Pfix[i] = res * Pmov[i]
*
* Ref:
* Besl, McKay
* A method for registration of 3d Shapes
* IEEE TPAMI Vol 14, No 2 1992
*/
template < class S >
void ComputeRigidMatchMatrix(std::vector<Point3<S> > &Pfix,
std::vector<Point3<S> > &Pmov,
Quaternion<S> &q,
Point3<S> &tr)
{
Eigen::Matrix3d ccm;
Point3<S> bfix,bmov; // baricenter of src e trg
ComputeCrossCovarianceMatrix(Pmov,bmov,Pfix,bfix,ccm);
Eigen::Matrix3d cyc; // the cyclic components of the cross covariance matrix.
cyc=ccm-ccm.transpose();
Eigen::Matrix4d QQ;
QQ.setZero();
Eigen::Vector3d D(cyc(1,2),cyc(2,0),cyc(0,1));
Eigen::Matrix3d RM;
RM.setZero();
RM(0,0)=-ccm.trace();
RM(1,1)=-ccm.trace();
RM(2,2)=-ccm.trace();
RM += ccm + ccm.transpose();
QQ(0,0) = ccm.trace();
QQ.block<1,3> (0,1) = D.transpose();
QQ.block<3,1> (1,0) = D;
QQ.block<3,3> (1,1) = RM;
Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> eig(QQ);
Eigen::Vector4d eval = eig.eigenvalues();
Eigen::Matrix4d evec = eig.eigenvectors();
std::cout << "EigenVectors:" << std::endl << evec << std::endl;
std::cout << "eigenvalues:" << std::endl << eval << std::endl;
int ind;
eval.cwiseAbs().maxCoeff(&ind);
q=Quaternion<S>(evec.col(ind)[0],evec.col(ind)[1],evec.col(ind)[2],evec.col(ind)[3]);
Matrix44<S> Rot;
q.ToMatrix(Rot);
tr= (bfix - Rot*bmov);
}
/*! \brief Compute the roto-translation that applied to PMov bring them onto Pfix
* Rotation is computed as a quaternion.
*
* E.g. it find a matrix such that:
*
* Pfix[i] = res * Pmov[i]
*
* Ref:
* Besl, McKay
* A method for registration of 3d Shapes
* IEEE TPAMI Vol 14, No 2 1992
*/
template < class S >
void ComputeRigidMatchMatrix(std::vector<Point3<S> > &Pfix,
std::vector<Point3<S> > &Pmov,
Matrix44<S> &res)
{
Quaternion<S> q;
Point3<S> tr;
ComputeRigidMatchMatrix(Pfix,Pmov,q,tr);
Matrix44<S> Rot;
q.ToMatrix(Rot);
Matrix44<S> Trn;
Trn.SetTranslate(tr);
res=Trn*Rot;
}
#if 0
/*
Compute a similarity matching (rigid + uniform scaling)
simply create a temporary point set with the correct scaling factor
*/
static bool ComputeSimilarityMatchMatrix( Matrix44x &res,
std::vector<Point3x> &Pfix, // vertici corrispondenti su fix (rossi)
std::vector<Point3x> &Pmov) // normali scelti su mov (verdi)
{
Quaternionx qtmp;
Point3x tr;
std::vector<Point3x> Pnew(Pmov.size());
ScalarType scalingFactor=0;
for(size_t i=0;i<( Pmov.size()-1);++i)
{
scalingFactor += Distance(Pmov[i],Pmov[i+1])/ Distance(Pfix[i],Pfix[i+1]);
#ifdef _DEBUG
printf("Scaling Factor is %f",scalingFactor/(i+1));
#endif
}
scalingFactor/=(Pmov.size()-1);
for(size_t i=0;i<Pmov.size();++i)
Pnew[i]=Pmov[i]/scalingFactor;
bool ret=ComputeRigidMatchMatrix(res,Pfix,Pnew,qtmp,tr);
if(!ret) return false;
Matrix44x scaleM; scaleM.SetDiagonal(1.0/scalingFactor);
res = res * scaleM;
return true;
}
#endif
} // end namespace
#endif