// --------------------------------------------------------------------------------------------------------
void KPerspectiveProjection::rotate ( const GLfloat x, const GLfloat y, const GLfloat z )
{
KVector savePos = getLookAtPosition();
translate(-getPosition());
KVector up = getYVector();
KVector look = getZVector();
KMatrix rotxz; rotxz.rotate (x, 0.0, z);
KMatrix roty; roty.rotate (0.0, y, 0.0);
KVector yunit(0.0, 1.0, 0.0), zunit (0.0, 0.0, 1.0);
KVector lookperp = look.perpendicular (yunit); // y-axis rotation
if (lookperp.length() > 0)
{
look = roty * lookperp + look.parallel(yunit);
up = roty * up.perpendicular(yunit) + up.parallel(yunit);
}
// x & z-axis rotation
KMatrix transmat(up.cross(look), up, look);
KVector uprotxz = rotxz * yunit;
KVector lookrotxz = rotxz * zunit;
up = transmat * uprotxz;
look = transmat * lookrotxz;
*((KMatrix*)this) = KMatrix(up.cross(look), up, look);
setPosition( savePos + eye_distance * getZVector());
}
//
// Returns alpha and loglik
// [alpha ] = function SmoothBackC(transmat, softev)
//
// beta is [K x T]
//
// softev is [K x T]
// transmat is [K x K]
//
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
if (nrhs != 2) {
mexErrMsgTxt("Needs 2 arguments -- transmat, softev");
return;
}
double* A = mxGetPr(prhs[0]);
double* D = mxGetPr(prhs[1]);
const mwSize* A_dims = mxGetDimensions(prhs[0]);
const mwSize* D_dims = mxGetDimensions(prhs[1]);
int K = D_dims[0];
int T = D_dims[1];
if (K != A_dims[0]) {
mexErrMsgTxt("Softev must be K x T");
return;
}
Eigen::Map<MatrixType> softev(D, K, T);
Eigen::Map<MatrixType> transmat(A, K, K);
MatrixType beta = MatrixType::Zero(K,T);
SmoothBack(transmat, softev, beta);
double* outputToolPtr;
plhs[0] = mxCreateDoubleMatrix(K, T, mxREAL);
outputToolPtr = mxGetPr(plhs[0]);
memcpy(outputToolPtr, beta.data(), K*T*sizeof(double));
}
void HMM::_fwdback(mat init_state_distrib, mat _transmat, mat obslik,
mat &alpha, mat &beta, mat& gamma, double &loglik, mat &xi_summed,
cube &gamma2, cube &obslik2,
bool fwd_only, bool compute_gamma2) {
/*
* Compute the posterior probs. in an HMM using the forwards backwards algo.
*
* Notation:
* Y(t) = observation, Q(t) = hidden state, M(t) = mixture variable (for MOG outputs)
* A(t) = discrete input (action) (for POMDP models)
*
* INPUT:
* init_state_distrib(i) = Pr(Q(1) = i)
* transmat(i,j) = Pr(Q(t) = j | Q(t-1)=i)
* or transmat{a}(i,j) = Pr(Q(t) = j | Q(t-1)=i, A(t-1)=a) if there are discrete inputs
* obslik(i,t) = Pr(Y(t)| Q(t)=i)
*
*/
bool scaled = true;
bool maximize = false;
bool compute_xi = true;
int Q = obslik.n_rows;
int T = obslik.n_cols;
mat mixmat;
mat act; // qui act è tutti zero, altrimenti potrebbe essere un input, TODO aggiungere &act negli input
mat scale;
if (obslik2.is_empty())
compute_gamma2 = false;
act = zeros(1,T); // TODO this could be a colvec
scale = ones(1,T);
field<mat> transmat(1,1);
transmat(0,0) = _transmat;
// scale(t) = Pr(O(t) | O(1:t-1)) = gamma21/c(t) as defined by Rabiner (1989).
// Hence prod_t scale(t) = Pr(O(1)) Pr(O(2)|O(1)) Pr(O(3) | O(1:2)) ... = Pr(O(1), ... ,O(T))
// or log P = sum_t log scale(t).
// Rabiner suggests multiplying beta(t) by scale(t), but we can instead
// normalise beta(t) - the constants will cancel when we compute gamma.
if (compute_xi)
xi_summed = zeros(Q,Q);
//else
// xi_summed = [];
//%%%%%%%%% Forwards %%%%%%%%%%
//cout << "fwdback > Forwards" << endl;
int t = 0;
alpha.col(0) = vectorize(init_state_distrib) % obslik.col(t);
if (scaled){
std::pair<mat,double> _tmp = normaliseC(alpha.col(t));
alpha.col(t) = _tmp.first;
scale(t) = _tmp.second;
}
for(int t=1; t<T; t++) {
mat trans;
mat m;
trans = transmat(act(t-1));
if (maximize){
//m = max_mult(trans.t(), alpha.col(t-1)); // TODO max_mult
} else {
m = trans.t() * alpha.col(t-1);
}
alpha.col(t) = vectorize(m) % obslik.col(t);
if (scaled) {
std::pair<mat,double> _tmp = normaliseC(alpha.col(t));
alpha.col(t) = _tmp.first;
scale(t) = _tmp.second;
}
if (compute_xi && fwd_only) {// useful for online EM
xi_summed = xi_summed + normalise((alpha.col(t-1) * obslik.col(t).t()) % trans);
}
}
if (scaled) {
uvec _s = find(scale); // se c'è almeno uno zero
// portando a logaritmo c'è almeno un infinito
// quindi somma tutto a infinito
if ( _s.is_empty() ) {
loglik = -std::numeric_limits<double>::max();
} else {
loglik = sum(sum(log(scale))); // nested arma::sum because sum(mat X) return a rowvec
}
} else {
loglik = log(sum(alpha.col(T)));
}
if (fwd_only) {
gamma = alpha;
return;
}
//%%%%%%%%% Backwards %%%%%%%%%%
//cout << "fwdback > Backwards" << endl;
int M;
mat trans;
mat denom;
beta = zeros(Q,T);
if (compute_gamma2) {
M = mixmat.n_cols;
gamma2 = zeros(Q,M,T);
} else {
//gamma2 = []
}
beta.col(T-1) = ones(Q,1);
gamma.col(T-1) = normalise(alpha.col(T-1) % beta.col(T-1));
t=T-1;
if (compute_gamma2) {
denom = obslik.col(t) + (obslik.col(t)==0); // replace 0s with 1s before dividing
gamma2.slice(t) = obslik2.slice(t) % mixmat % repmat(gamma.col(t), 1, M) % repmat(denom, 1, M);
}
for (int t=T-2; t>=0; t--) { // T-2 because there are some calls to t+1
// and col(T) will generate the error Mat::col(): out of bounds
// so we must assure the limit of col(T-1)
mat b = beta.col(t+1) % obslik.col(t+1);
trans = transmat(act(t));
if (maximize){
mat B = repmat(vectorize(b).t(), Q, 1);
beta.col(t) = max(trans % B, 1);
} else
beta.col(t) = trans * b;
if (scaled)
beta.col(t) = normalise( beta.col(t) );
gamma.col(t) = normalise(alpha.col(t) % beta.col(t));
if (compute_xi){
xi_summed = xi_summed + normalise((trans % (alpha.col(t) * b.t())));
}
if (compute_gamma2){
denom = obslik.col(t) + (obslik(t)==0); // replace 0s with 1s before dividing
gamma2.slice(t) = obslik2.slice(t) % mixmat % repmat(gamma.col(t), 1, M) % repmat(denom, 1, M);
}
}
}
