8 #ifndef DEM_BONES_DEM_BONES
9 #define DEM_BONES_DEM_BONES
11 #include <Eigen/Dense>
12 #include <Eigen/Sparse>
13 #include <Eigen/StdVector>
19 #ifndef DEM_BONES_MAT_BLOCKS
21 #define DEM_BONES_DEM_BONES_MAT_BLOCKS_UNDEFINED
65 template<
class _Scalar,
class _AniMeshScalar>
68 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
70 using MatrixX=Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic>;
71 using Matrix4=Eigen::Matrix<_Scalar, 4, 4>;
72 using Matrix3=Eigen::Matrix<_Scalar, 3, 3>;
73 using VectorX=Eigen::Matrix<_Scalar, Eigen::Dynamic, 1>;
74 using Vector4=Eigen::Matrix<_Scalar, 4, 1>;
75 using Vector3=Eigen::Matrix<_Scalar, 3, 1>;
145 Eigen::Matrix<_AniMeshScalar, Eigen::Dynamic, Eigen::Dynamic>
v;
148 std::vector<std::vector<int>>
fv;
173 laplacian.resize(0, 0);
187 if (modelSize<0) modelSize=sqrt((
u-(
u.rowwise().sum()/
nV).replicate(1,
nV)).squaredNorm()/
nV/
nS);
188 if (laplacian.cols()!=
nV) computeSmoothSolver();
190 if (((
int)
w.rows()!=
nB)||((
int)
w.cols()!=
nV)) {
191 if (((
int)
m.rows()!=
nF*4)||((
int)
m.cols()!=
nB*4)) {
195 label=Eigen::VectorXi::Zero(
nV);
196 computeTransFromLabel();
204 computeTransFromLabel();
208 cont=(
nB<targetNB)&&(
nB>prev);
211 lockM=Eigen::VectorXi::Zero(
nB);
213 }
else initWeights();
215 if (((
int)
m.rows()!=
nF*4)||((
int)
m.cols()!=
nB*4)) {
216 m=Matrix4::Identity().replicate(
nF,
nB);
217 lockM=Eigen::VectorXi::Zero(
nB);
246 for (_iterTransformations=0; _iterTransformations<
nTransIters; _iterTransformations++) {
248 #pragma omp parallel for
249 for (
int k=0; k<
nF; k++)
250 for (
int j=0; j<
nB; j++)
253 for (
int it=uuT.outerIdx(j); it<uuT.outerIdx(j+1); it++)
254 if (uuT.innerIdx(it)!=j) qpT-=
m.blk4(k, uuT.innerIdx(it))*uuT.val.blk4(
subjectID(k), it);
284 aTb=MatrixX::Zero(
nB,
nV);
286 std::vector<Triplet, Eigen::aligned_allocator<Triplet>> trip;
287 trip.reserve(
nV*
nnz);
289 for (_iterWeights=0; _iterWeights<
nWeightsIters; _iterWeights++) {
295 double reg_scale=pow(modelSize, 2)*
nF;
298 #pragma omp parallel for
299 for (
int i=0; i<
nV; i++) {
301 compute_aTa(i, aTai);
306 Eigen::ArrayXi idx=Eigen::ArrayXi::LinSpaced(
nB, 0,
nB-1);
307 std::sort(idx.data(), idx.data()+
nB, [&x](
int i1,
int i2) {
return x(i1)>x(i2); });
308 int nnzi=std::min(
nnz,
nB);
311 VectorX x0=
w.col(i).toDense().cwiseMax(0.0);
314 if (s>_Scalar(0.1)) x/=s;
else x=VectorX::Constant(nnzi, _Scalar(1)/nnzi);
319 for (
int j=0; j<nnzi; j++)
320 if (x(j)!=0) trip.push_back(
Triplet(idx[j], i, x(j)));
324 w.setFromTriplets(trip.begin(), trip.end());
347 for (_iter=0; _iter<
nIters; _iter++) {
358 #pragma omp parallel for
359 for (
int i=0; i<
nV; i++) {
362 for (
int k=0; k<
nF; k++) {
364 for (
typename SparseMatrix::InnerIterator it(
w, i); it; ++it) mki+=it.value()*
m.blk4(k, it.row());
365 ei+=(mki.template topLeftCorner<3, 3>()*
u.vec3(
subjectID(k), i)+mki.template topRightCorner<3, 1>()-
v.vec3(k, i).template cast<_Scalar>()).squaredNorm();
370 return std::sqrt(e/
nF/
nV);
404 int _iter, _iterTransformations, _iterWeights;
411 void qpT2m(
const Matrix4& _qpT,
int k,
int j) {
414 Eigen::JacobiSVD<Matrix3> svd(qpT.template topLeftCorner<3, 3>()-qpT.template topRightCorner<3, 1>()*qpT.template bottomLeftCorner<1, 3>(), Eigen::ComputeFullU|Eigen::ComputeFullV);
416 d(2, 2)=(svd.matrixU()*svd.matrixV().transpose()).determinant();
417 m.rotMat(k, j)=svd.matrixU()*d*svd.matrixV().transpose();
418 m.transVec(k, j)=qpT.template topRightCorner<3, 1>()-
m.rotMat(k, j)*qpT.template bottomLeftCorner<1, 3>().transpose();
426 _Scalar errorVtxBone(
int i,
int j,
bool par=
true) {
428 #pragma omp parallel for if(par)
429 for (
int k=0; k<
nF; k++)
431 e+=(
m.rotMat(k, j)*
u.vec3(
subjectID(k), i)+
m.transVec(k, j)-
v.vec3(k, i).template cast<_Scalar>()).squaredNorm();
436 Eigen::VectorXi label;
441 return t1.value()>t2.value();
447 void computeLabel() {
449 Eigen::VectorXi seed=Eigen::VectorXi::Constant(
nB, -1);
451 #pragma omp parallel for
452 for (
int i=0; i<
nV; i++) {
455 ei(i)=errorVtxBone(i, j,
false);
456 if ((seed(j)==-1)||(ei(i)<gMin(j))) {
458 if ((seed(j)==-1)||(ei(i)<gMin(j))) {
466 std::priority_queue<Triplet, std::vector<Triplet, Eigen::aligned_allocator<Triplet>>, TripletLess> heap;
467 for (
int j=0; j<
nB; j++)
if (seed(j)!=-1) heap.push(
Triplet(j, seed(j), ei(seed(j))));
469 if (laplacian.cols()!=
nV) computeSmoothSolver();
471 std::vector<bool> dirty(
nV,
true);
472 while (!heap.empty()) {
475 int i=(int)top.col();
476 int j=(int)top.row();
481 for (
typename SparseMatrix::InnerIterator it(laplacian, i); it; ++it) {
482 int i2=(int)it.row();
484 double tmp=(label(i2)==j)?ei(i2):errorVtxBone(i2, j);
485 heap.push(
Triplet(j, i2, tmp));
491 #pragma omp parallel for
492 for (
int i=0; i<
nV; i++)
495 for (
int j=0; j<
nB; j++) {
496 _Scalar ej=errorVtxBone(i, j,
false);
497 if ((label(i)==-1)||(gMin>ej)) {
507 void computeTransFromLabel() {
508 m=Matrix4::Identity().replicate(
nF,
nB);
509 #pragma omp parallel for
510 for (
int k=0; k<
nF; k++) {
512 for (
int i=0; i<
nV; i++)
513 if (label(i)!=-1) qpT.blk4(0, label(i))+=
Vector4(
v.vec3(k, i).template cast<_Scalar>().homogeneous())*
u.vec3(
subjectID(k), i).homogeneous().transpose();
514 for (
int j=0; j<
nB; j++) qpT2m(qpT.blk4(0, j), k, j);
520 void labelToWeights() {
521 std::vector<Triplet, Eigen::aligned_allocator<Triplet>> trip(
nV);
522 for (
int i=0; i<
nV; i++) trip[i]=
Triplet(label(i), i, _Scalar(1));
524 w.setFromTriplets(trip.begin(), trip.end());
532 void split(
int maxB,
int threshold) {
535 Eigen::VectorXi s=Eigen::VectorXi::Zero(
nB);
536 for (
int i=0; i<
nV; i++) {
537 cu.col(label(i))+=
u.col(i);
540 for (
int j=0; j<
nB; j++)
if (s(j)!=0) cu.col(j)/=_Scalar(s(j));
544 VectorX minD=VectorX::Constant(
nB, std::numeric_limits<_Scalar>::max());
545 VectorX minE=VectorX::Constant(
nB, std::numeric_limits<_Scalar>::max());
548 #pragma omp parallel for
549 for (
int i=0; i<
nV; i++) {
551 d(i)=(
u.col(i)-cu.col(j)).norm();
552 e(i)=sqrt(errorVtxBone(i, j,
false));
555 minD(j)=std::min(minD(j), d(i));
559 minE(j)=std::min(minE(j), e(i));
566 Eigen::VectorXi seed=Eigen::VectorXi::Constant(
nB, -1);
569 #pragma omp parallel for
570 for (
int i=0; i<
nV; i++) {
572 double tmp=abs((e(i)-minE(j))*(d(i)-minD(j)));
574 if ((seed(j)==-1)||(tmp>gMax(j))) {
576 if ((seed(j)==-1)||(tmp>gMax(j))) {
584 _Scalar avgErr=ce.sum()/
nB;
585 for (
int j=0; j<
nB; j++)
586 if ((countID<maxB)&&(s(j)>threshold*2)&&(ce(j)>avgErr/100)) {
587 int newLabel=countID++;
589 for (
typename SparseMatrix::InnerIterator it(laplacian, i); it; ++it) label(it.row())=newLabel;
597 void pruneBones(
int threshold) {
598 Eigen::VectorXi s=Eigen::VectorXi::Zero(
nB);
599 #pragma omp parallel for
600 for (
int i=0; i<
nV; i++) {
605 Eigen::VectorXi newID(
nB);
607 for (
int j=0; j<
nB; j++)
608 if (s(j)<threshold) newID(j)=-1;
else newID(j)=countID++;
610 if (countID==
nB)
return;
612 for (
int j=0; j<
nB; j++)
613 if (newID(j)!=-1)
m.template middleCols<4>(newID(j)*4)=
m.template middleCols<4>(j*4);
615 #pragma omp parallel for
616 for (
int i=0; i<
nV; i++) label(i)=newID(label(i));
619 m.conservativeResize(
nF*4,
nB*4);
626 label=Eigen::VectorXi::Constant(
nV, -1);
627 #pragma omp parallel for
628 for (
int i=0; i<
nV; i++) {
630 for (
int j=0; j<
nB; j++) {
631 _Scalar ej=errorVtxBone(i, j,
false);
632 if ((label(i)==-1)||(gMin>ej)) {
648 vuT=MatrixX::Zero(
nF*4,
nB*4);
649 #pragma omp parallel for
650 for (
int k=0; k<
nF; k++) {
652 for (
int i=0; i<
nV; i++)
653 for (
typename SparseMatrix::InnerIterator it(
w, i); it; ++it) {
654 Matrix4 tmp=
Vector4(
v.vec3(k, i).template cast<_Scalar>().homogeneous())*
u.vec3(
subjectID(k), i).homogeneous().transpose();
655 vuT.blk4(k, it.row())+=it.value()*tmp;
658 for (
int j=0; j<
nB; j++)
659 if (vuTp(3, j*4+3)!=0)
660 vuT.blk4(k, j)+=(
transAffine*vuT(k*4+3, j*4+3)/vuTp(3, j*4+3))*vuTp.blk4(0, j);
665 struct SparseMatrixBlock {
666 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
668 Eigen::VectorXi innerIdx, outerIdx;
674 Eigen::MatrixXi pos=Eigen::MatrixXi::Constant(
nB,
nB, -1);
675 #pragma omp parallel for
676 for (
int i=0; i<
nV; i++)
677 for (
typename SparseMatrix::InnerIterator it(
w, i); it; ++it)
678 for (
typename SparseMatrix::InnerIterator jt(
w, i); jt; ++jt)
679 pos(it.row(), jt.row())=1;
681 uuT.outerIdx.resize(
nB+1);
682 uuT.innerIdx.resize(
nB*
nB);
684 for (
int j=0; j<
nB; j++) {
686 for (
int i=0; i<
nB; i++)
692 uuT.outerIdx(
nB)=
nnz;
693 uuT.innerIdx.conservativeResize(
nnz);
694 uuT.val=MatrixX::Zero(
nS*4,
nnz*4);
696 #pragma omp parallel for
697 for (
int i=0; i<
nV; i++)
698 for (
typename SparseMatrix::InnerIterator it(
w, i); it; ++it)
699 for (
typename SparseMatrix::InnerIterator jt(
w, i); jt; ++jt)
700 if (it.row()>=jt.row()) {
701 double _w=it.value()*jt.value();
704 for (
int s=0; s<
nS; s++) {
705 _u=
u.vec3(s, i).homogeneous();
706 _uuT.blk4(s, 0)=_w*_u*_u.transpose();
708 int p=pos(it.row(), jt.row())*4;
709 for (
int c=0; c<4; c++)
710 for (
int r=0; r<4*
nS; r++)
712 uuT.val(r, p+c)+=_uuT(r, c);
715 for (
int i=0; i<
nB; i++)
716 for (
int j=i+1; j<
nB; j++)
718 uuT.val.middleCols(pos(i, j)*4, 4)=uuT.val.middleCols(pos(j, i)*4, 4);
729 Eigen::MatrixXi idx(2,
nB*(
nB+1)/2);
731 for (
int i=0; i<
nB; i++)
732 for (
int j=i; j<
nB; j++) {
738 mTm=MatrixX::Zero(
nS*
nB*4,
nB*4);
739 #pragma omp parallel for
740 for (
int p=0; p<nPairs; p++) {
743 for (
int k=0; k<
nF; k++)
744 mTm.blk4(
subjectID(k)*
nB+i, j)+=
m.blk4(k, i).template topRows<3>().transpose()*
m.blk4(k, j).template topRows<3>();
745 if (i!=j)
for (
int s=0; s<
nS; s++) mTm.blk4(s*
nB+j, i)=mTm.blk4(s*
nB+i, j);
755 #pragma omp parallel for
756 for (
int i=0; i<
nV; i++)
757 for (
int j=0; j<
nB; j++)
758 if ((aTb(j, i)==0)&&(ws(j, i)>
weightEps))
759 for (
int k=0; k<
nF; k++)
760 aTb(j, i)+=
v.vec3(k, i).template cast<_Scalar>().dot(
m.blk4(k, j).template topRows<3>()*
u.vec3(
subjectID(k), i).homogeneous());
770 Eigen::SparseLU<SparseMatrix> smoothSolver;
774 void computeSmoothSolver() {
775 int nFV=(int)
fv.size();
778 for (
int f=0; f<nFV; f++) {
779 int nf=(int)
fv[f].size();
780 for (
int g=0; g<nf; g++) {
782 int j=
fv[f][(g+1)%nf];
783 epsDis+=(
u.col(i)-
u.col(j)).norm();
788 std::vector<Triplet, Eigen::aligned_allocator<Triplet>> triplet;
790 std::vector<std::set<int>> isComputed(
nV);
792 #pragma omp parallel for
793 for (
int f=0; f<nFV; f++) {
794 int nf=(int)
fv[f].size();
795 for (
int g=0; g<nf; g++) {
797 int j=
fv[f][(g+1)%nf];
799 bool needCompute=
false;
801 if (isComputed[i].find(j)==isComputed[i].end()) {
803 isComputed[i].insert(j);
804 isComputed[j].insert(i);
809 for (
int s=0; s<
nS; s++) {
810 double du=(
u.vec3(s, i)-
u.vec3(s, j)).norm();
812 val+=pow((
v.vec3(k, i).template cast<_Scalar>()-
v.vec3(k, j).template cast<_Scalar>()).norm()-du, 2);
814 val=1/(sqrt(val/
nF)+epsDis);
817 triplet.push_back(
Triplet(i, j, -val));
822 triplet.push_back(
Triplet(j, i, -val));
829 for (
int i=0; i<
nV; i++)
830 triplet.push_back(
Triplet(i, i, d(i)));
832 laplacian.resize(
nV,
nV);
833 laplacian.setFromTriplets(triplet.begin(), triplet.end());
835 for (
int i=0; i<
nV; i++)
836 if (d(i)!=0) laplacian.row(i)/=d(i);
839 smoothSolver.compute(laplacian);
849 #pragma omp parallel for
850 for (
int j=0; j<
nB; j++) ws.col(j)=smoothSolver.solve(ws.col(j));
851 ws.transposeInPlace();
853 #pragma omp parallel for
854 for (
int i=0; i<
nV; i++) {
855 ws.col(i)=ws.col(i).cwiseMax(0.0);
856 _Scalar si=ws.col(i).sum();
857 if (si<_Scalar(0.1)) ws.col(i)=VectorX::Constant(
nB, _Scalar(1)/
nB);
else ws.col(i)/=si;
862 ConvexLS<_Scalar> wSolver;
868 void compute_aTa(
int i,
MatrixX& aTa) {
869 aTa=MatrixX::Zero(
nB,
nB);
870 for (
int j1=0; j1<
nB; j1++)
871 for (
int j2=j1; j2<
nB; j2++) {
872 for (
int s=0; s<
nS; s++) aTa(j1, j2)+=
u.vec3(s, i).homogeneous().dot(mTm.blk4(s*
nB+j1, j2)*
u.vec3(s, i).homogeneous());
873 if (j1!=j2) aTa(j2, j1)=aTa(j1, j2);
880 #ifdef DEM_BONES_DEM_BONES_MAT_BLOCKS_UNDEFINED
885 #undef DEM_BONES_MAT_BLOCKS