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

implemented least squares rigid motion

This commit is contained in:
Luigi Malomo 2019-06-10 12:09:48 +02:00
parent 50939143f5
commit d46c581ffb
1 changed files with 52 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
vcg/space/point_matching.h
View File

@ -161,6 +161,58 @@ void ComputeRigidMatchMatrix(std::vector<Point3<S> > &Pfix,
res=Trn*Rot; res=Trn*Rot;
} }
/*! \brief Computes the best fitting rigid transformations to align two sets of corresponding points
*
* Ref:
* Olga Sorkine-Hornung and Michael Rabinovich
* Least-Squares Rigid Motion Using SVD
*/
template <class S>
Matrix44<S> ComputeLeastSquaresRigidMotion(std::vector<Point3<S> > &pFix,
std::vector<Point3<S> > &pMov)
{
if (pFix.size() != pMov.size() || pFix.size() < 3)
return Matrix44<S>::Identity();
Eigen::Matrix3Xd p(3, pMov.size()); // moving
Eigen::MatrixX3d q(pFix.size(), 3); // fixed
for (size_t i=0; i<pMov.size(); i++)
{
Eigen::Vector3d v;
pMov[i].ToEigenVector(v);
p.col(i) = v;
}
Eigen::Vector3d avgP = p.rowwise().mean();
p.colwise() -= avgP;
for (size_t i=0; i<pFix.size(); i++)
{
Eigen::Vector3d v;
pFix[i].ToEigenVector(v);
q.row(i) = v;
}
Eigen::Vector3d avgQ = q.colwise().mean();
q.rowwise() -= avgQ.transpose();
Eigen::Matrix3d cov = p * q;
Eigen::JacobiSVD<Eigen::Matrix3d> svd;
svd.compute(cov, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3d d = Eigen::Matrix3d::Identity();
d(2,2) = (svd.matrixV() * svd.matrixU().transpose()).determinant() > 0 ? 1 : -1;
Eigen::Matrix3d R = (svd.matrixV() * d * svd.matrixU().transpose());
Eigen::Vector3d t = avgQ - R * avgP;
Eigen::Matrix4d res = Eigen::Matrix4d::Identity();
res.block<3,3>(0,0) = R;
res.block<3,1>(0,3) = t;
Matrix44<S> ret;
ret.FromEigenMatrix(res);
return ret;
}
/* /*
Compute a similarity matching (rigid + uniform scaling) Compute a similarity matching (rigid + uniform scaling)