8 #ifndef DEM_BONES_CONVEX_LS
9 #define DEM_BONES_CONVEX_LS
11 #include <Eigen/Dense>
12 #include <Eigen/StdVector>
30 template<
class _Scalar>
33 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
34 using MatrixX=Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic>;
35 using VectorX=Eigen::Matrix<_Scalar, Eigen::Dynamic, 1>;
49 int curN=(int)q2.size()+1;
52 #pragma omp parallel for
53 for (
int n=curN-1; n<maxSize-1; n++)
54 q2[n]=
MatrixX(VectorX::Constant(n+2, _Scalar(1)).householderQr().householderQ()).rightCols(n+1);
66 int n=int(aTa.cols());
68 if (!warmStart) x=VectorX::Constant(n, _Scalar(1)/n);
70 Eigen::ArrayXi idx(n);
72 for (
int i=0; i<n; i++)
73 if (x(i)>0) idx[np++]=i;
else idx[n-i+np-1]=i;
77 for (
int rep=0; rep<n; rep++) {
78 solveP(aTa, aTb, x, idx, np, affine, p);
85 std::swap(idx[iMax+np], idx[np]);
90 for (
int i=0; i<np; i++)
92 if ((iMin==-1)||(x(idx[i])<-alpha*p(idx[i]))) {
93 alpha=-x(idx[i])/p(idx[i]);
98 _Scalar eps=std::abs(x(idx[iMin]));
100 for (
int i=0; i<np; i++)
101 if (x(idx[i])<=eps) std::swap(idx[i--], idx[--np]);
103 if (affine) x/=x.sum();
109 std::vector<MatrixX, Eigen::aligned_allocator<MatrixX>> q2;
122 p.setZero(aTb.size());
124 z=
indexing_row_col(aTa, idx.head(np), idx.head(np)).colPivHouseholderQr().solve(
126 for (
int ip=0; ip<np; ip++) p(idx[ip])=z(ip);
129 (q2[np-2].transpose()*
indexing_row_col(aTa, idx.head(np), idx.head(np))*q2[np-2]).colPivHouseholderQr().solve(
131 for (
int ip=0; ip<np; ip++) p(idx[ip])=z(ip);