inline bool TRobustRegressionL1PD(
const MATRIX_TYPE& A,
const Eigen::Matrix<REAL, Eigen::Dynamic, 1>& y,
Eigen::Matrix<REAL, Eigen::Dynamic, 1>& xp,
REAL pdtol=1e-3, unsigned pdmaxiter=50)
{
typedef Eigen::Matrix<REAL, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Matrix;
typedef Eigen::Matrix<REAL, Eigen::Dynamic, 1> Vector;
const unsigned M = (unsigned)y.size();
const unsigned N = (unsigned)xp.size();
assert(A.rows() == M && A.cols() == N);
const REAL alpha(0.01);
const REAL beta(0.5);
const REAL mu(10);
Vector x(xp);
Vector Ax(A*x);
Vector tmpM1(y-Ax);
Vector tmpM2(-tmpM1);
Vector tmpM3(tmpM1.cwiseAbs()), tmpM4(M);
Vector u = (tmpM3*REAL(0.95)).array() + tmpM3.maxCoeff()*REAL(0.10);
Vector fu1 = tmpM2-u;
Vector fu2 = tmpM1-u;
Vector lamu1(M), lamu2(M);
for (unsigned i=0; i<M; ++i) {
lamu1(i) = -1.0/fu1(i);
lamu2(i) = -1.0/fu2(i);
}
const MATRIX_TYPE At(A.transpose());
Vector Atv(At*(lamu1-lamu2));
REAL AtvNormSq = Atv.squaredNorm();
Vector rdual((-lamu1-lamu2).array() + REAL(1));
REAL rdualNormSq = rdual.squaredNorm();
Vector w2(M), sig1(M), sig2(M), sigx(M), dx(N), up(N), Atdv(N);
Vector Axp(M), Atvp(M);
Vector &Adx(sigx), &du(w2), &w1p(dx);
Matrix H11p(N,N);
Vector &dlamu1(tmpM3), &dlamu2(tmpM4);
for (unsigned pditer=0; pditer<pdmaxiter; ++pditer) {
// surrogate duality gap
const REAL sdg(-(fu1.dot(lamu1) + fu2.dot(lamu2)));
if (sdg < pdtol)
break;
const REAL tau(mu*2*M/sdg);
const REAL inv_tau = REAL(-1)/tau;
tmpM1 = (-lamu1.cwiseProduct(fu1)).array() + inv_tau;
tmpM2 = (-lamu2.cwiseProduct(fu2)).array() + inv_tau;
const REAL resnorm = sqrt(AtvNormSq + rdualNormSq + tmpM1.squaredNorm() + tmpM2.squaredNorm());
for (unsigned i=0; i<M; ++i) {
REAL& tmpM3i = tmpM3(i);
tmpM3i = inv_tau/fu1(i);
REAL& tmpM4i = tmpM4(i);
tmpM4i = inv_tau/fu2(i);
w2(i) = tmpM3i + tmpM4i - REAL(1);
}
tmpM1 = lamu1.cwiseQuotient(fu1);
tmpM2 = lamu2.cwiseQuotient(fu2);
sig1 = -tmpM1 - tmpM2;
sig2 = tmpM1 - tmpM2;
sigx = sig1 - sig2.cwiseAbs2().cwiseQuotient(sig1);
H11p = At*(Eigen::DiagonalMatrix<REAL,Eigen::Dynamic>(sigx)*A);
w1p = At*(tmpM4 - tmpM3 - (sig2.cwiseQuotient(sig1).cwiseProduct(w2)));
// optimized solver as A is positive definite and symmetric
dx = H11p.ldlt().solve(w1p);
Adx = A*dx;
du = (w2 - sig2.cwiseProduct(Adx)).cwiseQuotient(sig1);
dlamu1 = -tmpM1.cwiseProduct(Adx-du) - lamu1 + tmpM3;
dlamu2 = tmpM2.cwiseProduct(Adx+du) - lamu2 + tmpM4;
Atdv = At*(dlamu1-dlamu2);
// make sure that the step is feasible: keeps lamu1,lamu2 > 0, fu1,fu2 < 0
REAL s(1);
for (unsigned i=0; i<M; ++i) {
REAL& dlamu1i = dlamu1(i);
if (dlamu1i < 0) {
const REAL tmp = -lamu1(i)/dlamu1i;
if (s > tmp)
s = tmp;
}
REAL& dlamu2i = dlamu2(i);
if (dlamu2i < 0) {
const REAL tmp = -lamu2(i)/dlamu2i;
if (s > tmp)
s = tmp;
}
}
for (unsigned i=0; i<M; ++i) {
REAL& Adxi = Adx(i);
REAL& dui = du(i);
REAL Adx_du = Adxi-dui;
if (Adx_du > 0) {
const REAL tmp = -fu1(i)/Adx_du;
if (s > tmp)
s = tmp;
}
Adx_du = -Adxi-dui;
if (Adx_du > 0) {
const REAL tmp = -fu2(i)/Adx_du;
if (s > tmp)
s = tmp;
}
}
s *= REAL(0.99);
// backtrack
lamu1 += s*dlamu1; lamu2 += s*dlamu2;
rdual = (-lamu1-lamu2).array() + REAL(1);
rdualNormSq = rdual.squaredNorm();
bool suffdec = false;
unsigned backiter = 0;
do {
xp = x + s*dx; up = u + s*du;
Axp = Ax + s*Adx; Atvp = Atv + s*Atdv;
fu1 = Axp - y - up; fu2 = -Axp + y - up;
AtvNormSq = Atvp.squaredNorm();
tmpM1 = (-lamu1.cwiseProduct(fu1)).array() + inv_tau;
tmpM2 = (-lamu2.cwiseProduct(fu2)).array() + inv_tau;
const REAL newresnorm = sqrt(AtvNormSq + rdualNormSq + tmpM1.squaredNorm() + tmpM2.squaredNorm());
suffdec = (newresnorm <= (REAL(1)-alpha*s)*resnorm);
s = beta*s;
if (++backiter > 32) {
//("error: stuck backtracking, returning last iterate"); // see Section 4 of notes for more information
xp.swap(x);
return false;
}
} while (!suffdec);
// next iteration
x.swap(xp); u.swap(up);
Ax.swap(Axp); Atv.swap(Atvp);
}
return true;
}
inline void swap(Eigen::Matrix<_Scalar,_Rows,_Cols,_StorageOrder,_MaxRows,_MaxCols>& lhs,Eigen::Matrix<_Scalar,_Rows,_Cols,_StorageOrder,_MaxRows,_MaxCols>& rhs)
{
lhs.swap(rhs);
}