/**
* Compute covariance array (m_V) for no autocorrelation case
* This just sets m_V to an identity matrix + 1.
*
* SJDM. Appears to be working fine if eps = 0.
* If eps > 0, can have a larges m_sig2 depending on value of eps.
**/
void logistic_normal::compute_covariance_matrix()
{
m_V.allocate(m_y1,m_y2,m_b1,m_nB2-1,m_b1,m_nB2-1);
m_V.initialize();
int i,nb;
for( i = m_y1; i <= m_y2; i++ )
{
nb = m_nB2(i);
dmatrix I = identity_matrix(m_b1,nb-1);
m_V(i) = 1 + I;
}
// compute mle estimate of sigma
compute_mle_sigma(m_V);
// scale covariance matrix
for( i = m_y1; i <= m_y2; i++ )
{
for(int j = m_b1; j < m_nB2(i); j++ )
{
m_V(i)(j,j) *= m_sig2;
}
}
}
/**
* Compute the covvariance array for AR1 case
* This is based on the parameter rho.
* This is done in two steps:
* 1) fist calculate the correlation matrix dCor given phi
* 2) Get the mle estimate of m_sig2 given the correlation.
**/
void logistic_normal::compute_covariance_matrix(const dvariable& phi)
{
m_V.allocate(m_y1,m_y2,m_b1,m_nB2-1,m_b1,m_nB2-1);
m_V.initialize();
dvar3_array dCor(m_y1,m_y2,m_b1,m_nB2,m_b1,m_nB2);
dCor.initialize();
int i,j,k,nb;
for( i = m_y1; i <= m_y2; i++ )
{
nb = m_nB2(i);
dCor(i) = identity_matrix(m_b1,nb);
// 2). Compute the vector of coefficients.
dvar_vector drho(m_b1,nb);
for( j = m_b1; j <= nb; j++ )
{
drho(j) = pow(phi,j-m_b1+1);
}
// 3). Compute correlation matrix dCor
for( j = m_b1; j <= nb; j++ )
{
for( k = m_b1; k <= nb; k++ )
{
if( j != k ) dCor(i)(j,k) = drho(m_b1+abs(j-k));
}
}
m_V(i) = trans(trans(dCor(i).sub(m_b1,nb-1)).sub(m_b1,nb-1));
}
// compute mle estimate of sigma
compute_mle_sigma(m_V);
// cout<<"Got to here sigma = "<<m_sig<<endl;
for( i = m_y1; i <= m_y2; i++ )
{
nb = m_nB2(i);
for( j = m_b1; j <= nb; j++ )
{
dCor(i).rowfill(j, extract_row(dCor(i),j)*m_sig );
}
for( k = m_b1; k <= nb; k++ )
{
dCor(i).colfill(k, extract_column(dCor(i),k)*m_sig );
}
//cout<<dCor(i)<<endl;
// Kmat
dmatrix I = identity_matrix(m_b1,nb-1);
dmatrix tKmat(m_b1,nb,m_b1,nb-1);
tKmat.sub(m_b1,nb-1) = I;
tKmat(nb) = -1;
dmatrix Kmat = trans(tKmat);
m_V(i) = Kmat * dCor(i) * tKmat;
}
}
MRF::EnergyVal TRWS::smoothnessEnergy()
{
EnergyVal eng = (EnergyVal) 0;
EnergyVal weight;
int x,y,pix;
if ( m_grid_graph )
{
if ( m_smoothType != FUNCTION )
{
for ( y = 0; y < m_height; y++ )
for ( x = 1; x < m_width; x++ )
{
pix = x+y*m_width;
weight = m_varWeights ? m_horzWeights[pix-1] : 1;
eng = eng + m_V(m_answer[pix],m_answer[pix-1])*weight;
}
for ( y = 1; y < m_height; y++ )
for ( x = 0; x < m_width; x++ )
{
pix = x+y*m_width;
weight = m_varWeights ? m_vertWeights[pix-m_width] : 1;
eng = eng + m_V(m_answer[pix],m_answer[pix-m_width])*weight;
}
}
else
{
for ( y = 0; y < m_height; y++ )
for ( x = 1; x < m_width; x++ )
{
pix = x+y*m_width;
eng = eng + m_smoothFn(pix,pix-1,m_answer[pix],m_answer[pix-1]);
}
for ( y = 1; y < m_height; y++ )
for ( x = 0; x < m_width; x++ )
{
pix = x+y*m_width;
eng = eng + m_smoothFn(pix,pix-m_width,m_answer[pix],m_answer[pix-m_width]);
}
}
}
else
{
// not implemented
}
return(eng);
}
inline void operator()(value_type *x, value_type *Fx)
{
std::copy(x, x + m_x.size(), m_x.begin());
m_V(m_t, x, m_v);
m_euler(m_t, x, m_v, m_dt);
std::transform(m_x.begin(), m_x.end(), x, Fx, std::minus<value_type>());
}
/**
* Compute negative loglikelihood:
* nll = t1 + t2 + t3 + t4 + t5 + t6; where:
* bym1 = sum_y(B_y-1);
* t1 = 0.5*log(2pi) * bym1;
* t2 = sum_{by} log(O_{by});
* t3 = log(sigma) * bym1;
* t4 = 0.5*sum_y log(det(V_y))
* t5 = sum_y (By-1)*log(W_y)
* t6 = 0.5*sigma^{-2}* sum_y (w'_y V_y^{-1} w_y)/W_y^2
**/
void logistic_normal::compute_negative_loglikelihood()
{
m_nll.initialize();
double bym1 = sum( dvector(m_nB2-m_b1) - 1.);
double t1 = 0.5 * log(2. * PI) * bym1;
double t2 = sum( log(m_Op) );
dvariable t3 = log(m_sig) * bym1;
dvariable t4 = 0;
dvariable t5 = 0;
dvariable t6 = 0;
for(int i = m_y1; i <= m_y2; i++ )
{
dvar_matrix Vinv = inv(m_V(i));
t4 += 0.5*log( det(m_V(i)) );
t5 += (m_nB2(i)-m_b1-1) * log(m_dWy(i));
t6 += (0.5/(m_dWy(i)*m_dWy(i))) * m_w(i) * Vinv * m_w(i);
}
m_nll = t1 + t2 + t3 + t4 + t5 + t6;
}
/**
* Compute standardized residuals.
* res{_by} = [log(O_{by}/\tilde(O)) - log(E_{by}/\tilde(E))] / (W_y G_y^{0.5})
**/
void logistic_normal::compute_standardized_residuals()
{
// Assumed the covariance matrix (m_V) has already been calculated.
// when the likelihood was called.
m_residual.allocate(m_O);
m_residual.initialize();
int i,j,k;
double n;
for( i = m_y1; i <= m_y2; i++ )
{
n = m_nB2(i);
double gOmean = pow(prod(m_Op(i),n),1./n);
dvector t1 = log(m_Op(i)/gOmean);
double gEmean = pow(prod(value(m_Ep(i)),n),1./n);
dvector t2 = log(value(m_Ep(i))/gEmean);
dmatrix I = identity_matrix(m_b1,n-1);
dmatrix tF(m_b1,n,m_b1,n-1);
tF.sub(m_b1,n-1) = I;
tF(n) = 1;
dmatrix Hinv = inv(I + 1);
dmatrix FHinv = tF * Hinv;
dmatrix G = FHinv * value(m_V(i)) * trans(FHinv);
dvector sd = sqrt(diagonal(G));
for( j = m_b1; j <= m_nB2(i); j++ )
{
k = m_nAgeIndex(i)(j);
m_residual(i)(k) = (t1(j)-t2(j)) / (sd(j)*m_dWy(i));
}
}
}
bool WSDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef)
{
unsigned int i, j, l;
e_scalar N, M;
// Create the Weighted jacobian
m_AWq = (A*Wq).lazy();
for (i=0; i<m_nc; i++)
m_WyAWq.row(i) = Wy(i)*m_AWq.row(i);
// Compute the SVD of the weighted jacobian
int ret;
if (m_transpose) {
m_WyAWqt = m_WyAWq.transpose();
ret = KDL::svd_eigen_HH(m_WyAWqt,m_V,m_S,m_U,m_temp);
} else {
ret = KDL::svd_eigen_HH(m_WyAWq,m_U,m_S,m_V,m_temp);
}
if(ret<0)
return false;
m_Wy_ydot = Wy.cwise() * ydot;
m_WqV = (Wq*m_V).lazy();
qdot.setZero();
e_scalar maxDeltaS = e_scalar(0.0);
e_scalar prevS = e_scalar(0.0);
e_scalar maxS = e_scalar(1.0);
for(i=0;i<m_ns;++i) {
e_scalar norm, mag, alpha, _qmax, Sinv, vmax, damp;
e_scalar S = m_S(i);
bool prev;
if (S < KDL::epsilon)
break;
Sinv = e_scalar(1.)/S;
if (i > 0) {
if ((prevS-S) > maxDeltaS) {
maxDeltaS = (prevS-S);
maxS = prevS;
}
}
N = M = e_scalar(0.);
for (l=0, prev=m_ytask[0], norm=e_scalar(0.); l<m_nc; l++) {
if (prev == m_ytask[l]) {
norm += m_U(l,i)*m_U(l,i);
} else {
N += std::sqrt(norm);
norm = m_U(l,i)*m_U(l,i);
}
prev = m_ytask[l];
}
N += std::sqrt(norm);
for (j=0; j<m_nq; j++) {
for (l=0, prev=m_ytask[0], norm=e_scalar(0.), mag=e_scalar(0.); l<m_nc; l++) {
if (prev == m_ytask[l]) {
norm += m_WyAWq(l,j)*m_WyAWq(l,j);
} else {
mag += std::sqrt(norm);
norm = m_WyAWq(l,j)*m_WyAWq(l,j);
}
prev = m_ytask[l];
}
mag += std::sqrt(norm);
M += fabs(m_V(j,i))*mag;
}
M *= Sinv;
alpha = m_U.col(i).dot(m_Wy_ydot);
_qmax = (N < M) ? m_qmax*N/M : m_qmax;
vmax = m_WqV.col(i).cwise().abs().maxCoeff();
norm = fabs(Sinv*alpha*vmax);
if (norm > _qmax) {
damp = Sinv*alpha*_qmax/norm;
} else {
damp = Sinv*alpha;
}
qdot += m_WqV.col(i)*damp;
prevS = S;
}
if (maxDeltaS == e_scalar(0.0))
nlcoef = e_scalar(KDL::epsilon);
else
nlcoef = (maxS-maxDeltaS)/maxS;
return true;
}
/**
* Compute the covvariance array for AR2 case
* This is based on the parameters rho and psi.
* Note that phi1 is bounded (-1,1)
* Then set phi2 = -1 + (1 + |phi1|)*psi
* and psi is bounded (0,1)
* This implies the upper bound for phi2 is 1.0 when |phi1|= 1 and psi = 1
**/
void logistic_normal::compute_covariance_matrix(const dvariable& phi1, const dvariable& psi)
{
m_V.allocate(m_y1,m_y2,m_b1,m_nB2-1,m_b1,m_nB2-1);
m_V.initialize();
dvar3_array dCor(m_y1,m_y2,m_b1,m_nB2,m_b1,m_nB2);
dCor.initialize();
int i,j,k,nb;
for( i = m_y1; i <= m_y2; i++ )
{
nb = m_nB2(i);
// dmatrix I = identity_matrix(m_b1,nb-1);
// m_V(i) = I;
dCor(i) = identity_matrix(m_b1,nb);
// 2). Compute the vector of coefficients.
dvariable phi2 = -1. + (1. + sfabs(phi1)) * psi;
dvar_vector drho(m_b1-1,nb-1);
drho = 1.0;
drho(m_b1) = phi1 / (1.0 - phi2);
for( j = m_b1+1; j <= nb-1; j++ )
{
drho(j) = phi1*drho(j-1) + phi2*drho(j-2);
}
// 3). Compute correlation matrix m_V
for( j = m_b1; j < nb; j++ )
{
for( k = m_b1; k < nb; k++ )
{
//if( j != k ) m_V(i)(j,k) = drho(m_b1+abs(j-k));
if( j != k ) dCor(i)(j,k) = drho(m_b1+abs(j-k));
}
}
m_V(i) = trans(trans(dCor(i).sub(m_b1,nb-1)).sub(m_b1,nb-1));
}
// compute mle estimate of sigma
compute_mle_sigma(m_V);
for( i = m_y1; i <= m_y2; i++ )
{
nb = m_nB2(i);
for( j = m_b1; j < nb; j++ )
{
m_V(i).rowfill(j, extract_row(m_V(i),j) * m_sig );
}
for( k = m_b1; k < nb; k++ )
{
m_V(i).colfill(k, extract_column( m_V(i),k ) * m_sig );
}
// Kmat
dmatrix I = identity_matrix(m_b1,nb-1);
dmatrix tKmat(m_b1,nb,m_b1,nb-1);
tKmat.sub(m_b1,nb-1) = I;
tKmat(nb) = -1;
dmatrix Kmat = trans(tKmat);
m_V(i) = Kmat * dCor(i) * tKmat;
}
}
