MatrixXd MinEnFilter::DAk(const MatrixXd& Gk, const MatrixXd& Disps_inhom, int direction) {
MatrixXd Ones = MatrixXd::Ones(1,4);
MatrixXd Eaux = MatrixXd::Identity(6,6);
Matrix4d eta = MEFcommon::matSE(Eaux.col(direction-1));
MatrixXd iEg = (currG.E.lu().solve(Gk.transpose())).transpose();
VectorXd kappa = iEg.col(2);
MatrixXd h = iEg.leftCols(2).cwiseQuotient(kappa*MatrixXd::Ones(1,2));
MatrixXd z = Disps_inhom.leftCols(2) - h;
VectorXd kappaM1 = kappa.cwiseInverse();
VectorXd kappaM2 = (kappa.cwiseProduct(kappa)).cwiseInverse();
VectorXd kappaM3 = (kappa.cwiseProduct(kappa.cwiseProduct(kappa))).cwiseInverse();
MatrixXd lambda = ((currG.E.lu().solve(eta))*(iEg.transpose())).transpose();
VectorXd xi = (lambda.col(2)).cwiseProduct(kappaM2);
// first part of Hessian
VectorXd zeta1 = -2* kappaM3.cwiseProduct((lambda.col(2)).cwiseProduct(iEg.col(0)));
VectorXd zeta2 = -2 * kappaM3.cwiseProduct((lambda.col(2)).cwiseProduct(iEg.col(1)));
VectorXd eta1 = kappaM2.cwiseProduct(lambda.col(0));
VectorXd eta2 = kappaM2.cwiseProduct(lambda.col(1));
VectorXd a31 = zeta1 + eta1;
VectorXd a32 = zeta2 + eta2;
MatrixXd b1row = q1/nPoints * ((z.col(0))*Ones).cwiseProduct(iEg);
MatrixXd b2row = q2/nPoints * ((z.col(1))*Ones).cwiseProduct(iEg);
MatrixXd c1row = ((xi * Ones).cwiseProduct(b1row)).colwise().sum();
MatrixXd c2row = ((xi * Ones).cwiseProduct(b2row)).colwise().sum();
MatrixXd aux1(nPoints,4);
aux1 << (a31.cwiseProduct(b1row.col(0)) + a32.cwiseProduct(b2row.col(0))) , (a31.cwiseProduct(b1row.col(1)) + a32.cwiseProduct(b2row.col(1))), (a31.cwiseProduct(b1row.col(2)) + a32.cwiseProduct(b2row.col(2))), (a31.cwiseProduct(b1row.col(3)) + a32.cwiseProduct(b2row.col(3)));
MatrixXd c3row = aux1.colwise().sum();
MatrixXd C(4,4);
C << c1row,
c2row,
c3row,
MatrixXd::Zero(1,4);
// % second part of Hessian
VectorXd rho1 = -q1/nPoints * kappaM2.cwiseProduct(iEg.col(0));
VectorXd rho2 = -q2/nPoints * kappaM2.cwiseProduct(iEg.col(1));
MatrixXd Frow1 = kappaM1.cwiseProduct(lambda.col(0)) - kappaM2.cwiseProduct((lambda.col(2)).cwiseProduct(iEg.col(0)));
MatrixXd Frow2 = kappaM1.cwiseProduct(lambda.col(1)) - kappaM2.cwiseProduct((lambda.col(2)).cwiseProduct(iEg.col(1)));
MatrixXd G1row1 = (Frow1*Ones).cwiseProduct(iEg);
MatrixXd G1row2 = (Frow2*Ones).cwiseProduct(iEg);
MatrixXd G2row1 = ((z.col(0))*Ones).cwiseProduct(lambda);
MatrixXd G2row2 = ((z.col(1))*Ones).cwiseProduct(lambda);
MatrixXd Grow1 = G1row1 - G2row1;
MatrixXd Grow2 = G1row2 - G2row2;
MatrixXd h1row = (q1/nPoints * (kappaM1*Ones).cwiseProduct(Grow1)).colwise().sum();
MatrixXd h2row = (q2/nPoints * (kappaM1*Ones).cwiseProduct(Grow2)).colwise().sum();
MatrixXd aux2(nPoints,4);
aux2 << rho1.cwiseProduct(Grow1.col(0)) + rho2.cwiseProduct(Grow2.col(0)), rho1.cwiseProduct(Grow1.col(1)) + rho2.cwiseProduct(Grow2.col(1)), rho1.cwiseProduct(Grow1.col(2)) + rho2.cwiseProduct(Grow2.col(2)), rho1.cwiseProduct(Grow1.col(3)) + rho2.cwiseProduct(Grow2.col(3));
MatrixXd h3row = aux2.colwise().sum();
MatrixXd H(4,4);
H << h1row,
h2row,
h3row,
MatrixXd::Zero(1,4);
return C+H;
}
LieAlgebra MinEnFilter::computeGradient(const LieGroup& S, const MatrixXd& Gk, const MatrixXd& Disps_inhom) {
/* DO NOT CHANGE THIS FUNCTION */
LieAlgebra L;
MatrixXd iEg = ((S.E).lu().solve(Gk.transpose())).transpose();
VectorXd kappa = iEg.col(2);
MatrixXd h = iEg.leftCols(2).cwiseQuotient(kappa*MatrixXd::Ones(1,2));
MatrixXd z = Disps_inhom.leftCols(2) - h;
MatrixXd Qvec(1,2);
Qvec << q1/nPoints,q2/nPoints;
MatrixXd qvec = MatrixXd::Ones(nPoints,1) * Qvec;
MatrixXd b12 = (kappa.cwiseInverse()*MatrixXd::Ones(1,2)).cwiseProduct(qvec.cwiseProduct(z));
MatrixXd kappa2 = kappa.cwiseProduct(kappa);
MatrixXd aux1 = (iEg.leftCols(2)).cwiseProduct(z.cwiseProduct(qvec));
MatrixXd b3 = -((kappa2.cwiseInverse()*MatrixXd::Ones(1,2)).cwiseProduct(aux1)).rowwise().sum();
MatrixXd B(nPoints,4);
B << b12, b3, MatrixXd::Zero(nPoints,1);
MatrixXd Ones = MatrixXd::Ones(1,4);
MatrixXd A1 = iEg.col(0) * Ones;
MatrixXd A2 = iEg.col(1) * Ones;
MatrixXd A3 = iEg.col(2) * Ones;
MatrixXd A4 = iEg.col(3) * Ones;
MatrixXd G1 = (B.cwiseProduct(A1)).colwise().sum();
MatrixXd G2 = (B.cwiseProduct(A2)).colwise().sum();
MatrixXd G3 = (B.cwiseProduct(A3)).colwise().sum();
MatrixXd G4 = (B.cwiseProduct(A4)).colwise().sum();
Matrix4d G;
G << G1,
G2,
G3,
G4;
G = MEFcommon::prSE(G.transpose());
if (order == 1) {
L = LieAlgebra(G);
} else if (order >=2) {
L = LieAlgebra(G, VectorXd::Zero((order-1)*6),order);
} else {
cerr << "Parameter order must be greater than zero." << endl;
exit(1);
}
return L;
}
VectorXd sample_coefs_single(
VectorXd UtEta,
MatrixXd UtW,
VectorXd prior_mean,
VectorXd prior_prec,
double h2,
double tot_Eta_prec,
VectorXd randn_theta,
VectorXd randn_e,
VectorXd s,
int b,
int n
) {
VectorXd R_sq_diag = ((h2 * s.array() + (1.0-h2))/tot_Eta_prec).sqrt();
VectorXd theta_star = randn_theta.array()/prior_prec.array().sqrt();
theta_star += prior_mean;
VectorXd e_star = randn_e.array() * R_sq_diag.array();
MatrixXd UtW_theta_star = UtW * theta_star;
VectorXd eta_resid = UtEta - UtW_theta_star - e_star;
MatrixXd RinvSqUtW = R_sq_diag.cwiseInverse().asDiagonal() * UtW;
VectorXd eta_std = eta_resid.array()/R_sq_diag.array();
VectorXd WtURinvy = RinvSqUtW.transpose() * eta_std;
VectorXd theta_tilda;
if(b < n) {
MatrixXd C = RinvSqUtW.transpose() * RinvSqUtW;
C.diagonal() = C.diagonal() + prior_prec;
theta_tilda = C.householderQr().solve(WtURinvy);
} else{
MatrixXd VAi = UtW * prior_prec.cwiseInverse().asDiagonal();
MatrixXd inner = VAi*UtW.transpose();
for(int i = 0; i < n; i++) {
inner(i,i) += (h2 * s(i) + (1.0-h2)) / tot_Eta_prec;
}
VectorXd VAiWtURinvy = VAi * WtURinvy;
VectorXd outerWtURinvy = VAi.transpose() * inner.householderQr().solve(VAiWtURinvy);
theta_tilda = WtURinvy.array() / prior_prec.array();
theta_tilda -= outerWtURinvy;
}
VectorXd coefs = theta_tilda + theta_star;
return coefs;
}
Matrix4d MinEnFilter::iterate(MatrixXd& XY, VectorXd& Depth, MatrixXd& Flow) {
cout << "Iterate: " << iteration << endl;
iteration ++;
LieAlgebra grad, KE;
MatrixXd Hessian, KP;
// get and check dimensions
nPoints = XY.rows();
if (nPoints!= Depth.rows()) {
cerr << "Dimensions of XY do not coincide with Dimensions of Depth." << endl;
}
if (nPoints!= Flow.rows()) {
cerr << "Dimensions of XY do not coincide with Dimensions of Flow." << endl;
}
MatrixXd XYZ_inhom = inhomCoordinates(XY, false); // inhomogenous Coordinatestes
MatrixXd Flow_inhom = inhomCoordinates(Flow, true); // inhomogenous (relative) flow
MatrixXd Disps_inhom = XYZ_inhom + Flow_inhom; // inhomogenous Disparities
MatrixXd temp(nPoints, 4);
temp << XYZ_inhom, Depth.cwiseInverse();
MatrixXd Gk = (Depth*MatrixXd::Ones(1,4)).cwiseProduct(temp);
// compute Gradient of Hamiltonian
grad = computeGradient(currG , Gk, Disps_inhom);
// compute Hessian of Hamiltonian
Hessian = computeHessian(Gk, Disps_inhom);
// Numerical integration of state and second order information
for (size_t i = 0; i < numSteps; i++ ){
KE = integrateGImplicit(grad, Gk, Disps_inhom);
currG = currG * KE.exp_g();
KP = integratePExplicit(grad ,Hessian);
currP = currP + delta * KP;
}
return currG.E;
}
MatrixXd sample_MME_multiple_diagR(
MatrixXd Y,
SpMat W,
SpMat chol_C,
VectorXd pe,
MSpMat chol_K_inv,
VectorXd tot_Eta_prec,
MatrixXd randn_theta,
MatrixXd randn_e
){
MatrixXd theta_star = chol_K_inv.triangularView<Upper>().solve(randn_theta);
MatrixXd e_star = randn_e * pe.cwiseSqrt().cwiseInverse().asDiagonal();
MatrixXd W_theta_star = W * theta_star;
MatrixXd Y_resid = Y - W_theta_star - e_star;
MatrixXd WtRiy = W.transpose() * (Y_resid * pe.asDiagonal());
MatrixXd theta_tilda = chol_C.transpose().triangularView<Upper>().solve(chol_C.triangularView<Lower>().solve(WtRiy));
MatrixXd theta = theta_tilda * tot_Eta_prec.cwiseInverse().asDiagonal() + theta_star;
return theta;
}
CFeatures* CJade::apply(CFeatures* features)
{
ASSERT(features);
SG_REF(features);
SGMatrix<float64_t> X = ((CDenseFeatures<float64_t>*)features)->get_feature_matrix();
int n = X.num_rows;
int T = X.num_cols;
int m = n;
Map<MatrixXd> EX(X.matrix,n,T);
// Mean center X
VectorXd mean = (EX.rowwise().sum() / (float64_t)T);
MatrixXd SPX = EX.colwise() - mean;
MatrixXd cov = (SPX * SPX.transpose()) / (float64_t)T;
#ifdef DEBUG_JADE
std::cout << "cov" << std::endl;
std::cout << cov << std::endl;
#endif
// Whitening & Projection onto signal subspace
SelfAdjointEigenSolver<MatrixXd> eig;
eig.compute(cov);
#ifdef DEBUG_JADE
std::cout << "eigenvectors" << std::endl;
std::cout << eig.eigenvectors() << std::endl;
std::cout << "eigenvalues" << std::endl;
std::cout << eig.eigenvalues().asDiagonal() << std::endl;
#endif
// Scaling
VectorXd scales = eig.eigenvalues().cwiseSqrt();
MatrixXd B = scales.cwiseInverse().asDiagonal() * eig.eigenvectors().transpose();
#ifdef DEBUG_JADE
std::cout << "whitener" << std::endl;
std::cout << B << std::endl;
#endif
// Sphering
SPX = B * SPX;
// Estimation of the cumulant matrices
int dimsymm = (m * ( m + 1)) / 2; // Dim. of the space of real symm matrices
int nbcm = dimsymm; // number of cumulant matrices
m_cumulant_matrix = SGMatrix<float64_t>(m,m*nbcm); // Storage for cumulant matrices
Map<MatrixXd> CM(m_cumulant_matrix.matrix,m,m*nbcm);
MatrixXd R(m,m); R.setIdentity();
MatrixXd Qij = MatrixXd::Zero(m,m); // Temp for a cum. matrix
VectorXd Xim = VectorXd::Zero(m); // Temp
VectorXd Xjm = VectorXd::Zero(m); // Temp
VectorXd Xijm = VectorXd::Zero(m); // Temp
int Range = 0;
for (int im = 0; im < m; im++)
{
Xim = SPX.row(im);
Xijm = Xim.cwiseProduct(Xim);
Qij = SPX.cwiseProduct(Xijm.replicate(1,m).transpose()) * SPX.transpose() / (float)T - R - 2*R.col(im)*R.col(im).transpose();
CM.block(0,Range,m,m) = Qij;
Range = Range + m;
for (int jm = 0; jm < im; jm++)
{
Xjm = SPX.row(jm);
Xijm = Xim.cwiseProduct(Xjm);
Qij = SPX.cwiseProduct(Xijm.replicate(1,m).transpose()) * SPX.transpose() / (float)T - R.col(im)*R.col(jm).transpose() - R.col(jm)*R.col(im).transpose();
CM.block(0,Range,m,m) = sqrt(2)*Qij;
Range = Range + m;
}
}
#ifdef DEBUG_JADE
std::cout << "cumulatant matrices" << std::endl;
std::cout << CM << std::endl;
#endif
// Stack cumulant matrix into ND Array
index_t * M_dims = SG_MALLOC(index_t, 3);
M_dims[0] = m;
M_dims[1] = m;
M_dims[2] = nbcm;
SGNDArray< float64_t > M(M_dims, 3);
for (int i = 0; i < nbcm; i++)
{
Map<MatrixXd> EM(M.get_matrix(i),m,m);
EM = CM.block(0,i*m,m,m);
}
// Diagonalize
SGMatrix<float64_t> Q = CJADiagOrth::diagonalize(M, m_mixing_matrix, tol, max_iter);
Map<MatrixXd> EQ(Q.matrix,m,m);
EQ = -1 * EQ.inverse();
#ifdef DEBUG_JADE
std::cout << "diagonalizer" << std::endl;
std::cout << EQ << std::endl;
#endif
// Separating matrix
SGMatrix<float64_t> sep_matrix = SGMatrix<float64_t>(m,m);
Map<MatrixXd> C(sep_matrix.matrix,m,m);
C = EQ.transpose() * B;
// Sort
VectorXd A = (B.inverse()*EQ).cwiseAbs2().colwise().sum();
bool swap = false;
do
{
swap = false;
for (int j = 1; j < n; j++)
{
if ( A(j) < A(j-1) )
{
std::swap(A(j),A(j-1));
C.col(j).swap(C.col(j-1));
swap = true;
}
}
} while(swap);
for (int j = 0; j < m/2; j++)
C.row(j).swap( C.row(m-1-j) );
// Fix Signs
VectorXd signs = VectorXd::Zero(m);
for (int i = 0; i < m; i++)
{
if( C(i,0) < 0 )
signs(i) = -1;
else
signs(i) = 1;
}
C = signs.asDiagonal() * C;
#ifdef DEBUG_JADE
std::cout << "un mixing matrix" << std::endl;
std::cout << C << std::endl;
#endif
// Unmix
EX = C * EX;
m_mixing_matrix = SGMatrix<float64_t>(m,m);
Map<MatrixXd> Emixing_matrix(m_mixing_matrix.matrix,m,m);
Emixing_matrix = C.inverse();
return features;
}