static vec qp_diag(const mat& Q, const mat& A) { vec res(Q.cols() + A.rows() ); res << Q.diagonal(), vec::Ones(A.rows()); return res; }
void DiagonalResponse::factor(const mat& H ) { if( _constant.getValue() && !_firstFactorization ) return; _firstFactorization = false; diag = H.diagonal().cwiseInverse(); assert( !has_nan(diag) ); }
vec diagp1(const mat& M) { return M.diagonal().array() + 1; }
inline void set_diag(mat &A, vec & v){ A.diagonal()=v; }