int contactConstraintsBV(
const RigidBodyTree<double>& r,
const KinematicsCache<double>& cache, int nc,
std::vector<double> support_mus,
// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
drake::eigen_aligned_std_vector<SupportStateElement>& supp, MatrixXd& B,
// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
MatrixXd& JB, MatrixXd& Jp, VectorXd& Jpdotv, MatrixXd& normals) {
int j, k = 0, nq = r.get_num_positions();
B.resize(3, nc * 2 * m_surface_tangents);
JB.resize(nq, nc * 2 * m_surface_tangents);
Jp.resize(3 * nc, nq);
Jpdotv.resize(3 * nc);
normals.resize(3, nc);
Vector3d contact_pos, pos, normal;
MatrixXd J(3, nq);
Matrix<double, 3, m_surface_tangents> d;
for (auto iter = supp.begin(); iter != supp.end(); iter++) {
double mu = support_mus[iter - supp.begin()];
double norm = sqrt(1 + mu * mu); // because normals and ds are orthogonal,
// the norm has a simple form
if (nc > 0) {
for (auto pt_iter = iter->contact_pts.begin();
pt_iter != iter->contact_pts.end(); pt_iter++) {
contact_pos = r.transformPoints(cache, *pt_iter, iter->body_idx, 0);
J = r.transformPointsJacobian(cache, *pt_iter, iter->body_idx, 0, true);
normal = iter->support_surface.head(3);
surfaceTangents(normal, d);
for (j = 0; j < m_surface_tangents; j++) {
B.col(2 * k * m_surface_tangents + j) =
(normal + mu * d.col(j)) / norm;
B.col((2 * k + 1) * m_surface_tangents + j) =
(normal - mu * d.col(j)) / norm;
JB.col(2 * k * m_surface_tangents + j) =
J.transpose() * B.col(2 * k * m_surface_tangents + j);
JB.col((2 * k + 1) * m_surface_tangents + j) =
J.transpose() * B.col((2 * k + 1) * m_surface_tangents + j);
}
// store away kin sols into Jp and Jpdotv
// NOTE: I'm cheating and using a slightly different ordering of J and
// Jdot here
Jp.block(3 * k, 0, 3, nq) = J;
Vector3d pt = (*pt_iter).head(3);
Jpdotv.block(3 * k, 0, 3, 1) =
r.transformPointsJacobianDotTimesV(cache, pt, iter->body_idx, 0);
normals.col(k) = normal;
k++;
}
}
}
return k;
}
int contactConstraintsBV(RigidBodyManipulator *r, int nc, std::vector<double> support_mus, std::vector<SupportStateElement,Eigen::aligned_allocator<SupportStateElement>>& supp, void *map_ptr, MatrixXd &B, MatrixXd &JB, MatrixXd &Jp, VectorXd &Jpdotv, MatrixXd &normals, double terrain_height)
{
int j, k=0, nq = r->num_positions;
B.resize(3,nc*2*m_surface_tangents);
JB.resize(nq,nc*2*m_surface_tangents);
Jp.resize(3*nc,nq);
Jpdotv.resize(3*nc);
normals.resize(3, nc);
Vector3d contact_pos,pos,normal;
MatrixXd J(3,nq);
Matrix<double,3,m_surface_tangents> d;
for (std::vector<SupportStateElement,Eigen::aligned_allocator<SupportStateElement>>::iterator iter = supp.begin(); iter!=supp.end(); iter++) {
double mu = support_mus[iter - supp.begin()];
double norm = sqrt(1+mu*mu); // because normals and ds are orthogonal, the norm has a simple form
if (nc>0) {
for (auto pt_iter=iter->contact_pts.begin(); pt_iter!=iter->contact_pts.end(); pt_iter++) {
auto contact_pos_gradientvar = r->forwardKin(*pt_iter, iter->body_idx, 0, 0, 1);
contact_pos = contact_pos_gradientvar.value();
J = contact_pos_gradientvar.gradient().value();
if (iter->use_support_surface) {
normal = iter->support_surface.head(3);
}
else {
collisionDetect(map_ptr,contact_pos,pos,&normal,terrain_height);
}
surfaceTangents(normal,d);
for (j=0; j<m_surface_tangents; j++) {
B.col(2*k*m_surface_tangents+j) = (normal + mu*d.col(j)) / norm;
B.col((2*k+1)*m_surface_tangents+j) = (normal - mu*d.col(j)) / norm;
JB.col(2 * k * m_surface_tangents + j) = J.transpose() * B.col(2 * k * m_surface_tangents + j);
JB.col((2 * k + 1) * m_surface_tangents + j) = J.transpose() * B.col((2*k+1)*m_surface_tangents+j);
}
// store away kin sols into Jp and Jpdotv
// NOTE: I'm cheating and using a slightly different ordering of J and Jdot here
Jp.block(3*k,0,3,nq) = J;
Vector3d pt = (*pt_iter).head(3);
auto Jpdotv_grad = r->forwardJacDotTimesV(pt,iter->body_idx,0,0,0);
Jpdotv.block(3*k,0,3,1) = Jpdotv_grad.value();
normals.col(k) = normal;
k++;
}
}
}
return k;
}
Self& fit(Features input, bool cold_start = true) {
VectorXd init(_hidden_units * input.rows() + _hidden_units + input.rows());
if (_fitted && !cold_start) {
init.block(0, 0, _weights.size(), 1) = ravel(_weights);
init.block(_weights.size(), 0, _hidden_intercepts.size(), 1) = _hidden_intercepts;
init.block(_weights.size() + _hidden_intercepts.size(),
0, _reconstruction_intercepts.size(), 1) = _reconstruction_intercepts;
} else {
init = (init.setRandom() * 4 / sqrt(6.0 / (_hidden_units + input.rows())));
}
VectorXd coeffs = _optimizer(*this, input, input, init, cold_start);
_weights = unravel(coeffs.block(0, 0, _hidden_units * input.rows(), 1), _hidden_units, input.rows());
_hidden_intercepts = coeffs.block(_hidden_units * input.rows(), 0, _hidden_units, 1);
_reconstruction_intercepts = coeffs.block(_hidden_units * input.rows() + _hidden_units, 0, input.rows(), 1);
_fitted = true;
return _self();
}
IGL_INLINE bool igl::arap_solve(
const Eigen::PlainObjectBase<Derivedbc> & bc,
ARAPData & data,
Eigen::PlainObjectBase<DerivedU> & U)
{
using namespace Eigen;
using namespace std;
assert(data.b.size() == bc.rows());
if(bc.size() > 0)
{
assert(bc.cols() == data.dim && "bc.cols() match data.dim");
}
const int n = data.n;
int iter = 0;
if(U.size() == 0)
{
// terrible initial guess.. should at least copy input mesh
#ifndef NDEBUG
cerr<<"arap_solve: Using terrible initial guess for U. Try U = V."<<endl;
#endif
U = MatrixXd::Zero(data.n,data.dim);
} else
{
assert(U.cols() == data.dim && "U.cols() match data.dim");
}
// changes each arap iteration
MatrixXd U_prev = U;
// doesn't change for fixed with_dynamics timestep
MatrixXd U0;
if(data.with_dynamics)
{
U0 = U_prev;
}
while(iter < data.max_iter)
{
U_prev = U;
// enforce boundary conditions exactly
for(int bi = 0; bi<bc.rows(); bi++)
{
U.row(data.b(bi)) = bc.row(bi);
}
const auto & Udim = U.replicate(data.dim,1);
assert(U.cols() == data.dim);
// As if U.col(2) was 0
MatrixXd S = data.CSM * Udim;
// THIS NORMALIZATION IS IMPORTANT TO GET SINGLE PRECISION SVD CODE TO WORK
// CORRECTLY.
S /= S.array().abs().maxCoeff();
const int Rdim = data.dim;
MatrixXd R(Rdim,data.CSM.rows());
if(R.rows() == 2)
{
fit_rotations_planar(S,R);
} else
{
fit_rotations(S,true,R);
//#ifdef __SSE__ // fit_rotations_SSE will convert to float if necessary
// fit_rotations_SSE(S,R);
//#else
// fit_rotations(S,true,R);
//#endif
}
//for(int k = 0;k<(data.CSM.rows()/dim);k++)
//{
// R.block(0,dim*k,dim,dim) = MatrixXd::Identity(dim,dim);
//}
// Number of rotations: #vertices or #elements
int num_rots = data.K.cols()/Rdim/Rdim;
// distribute group rotations to vertices in each group
MatrixXd eff_R;
if(data.G.size() == 0)
{
// copy...
eff_R = R;
} else
{
eff_R.resize(Rdim,num_rots*Rdim);
for(int r = 0; r<num_rots; r++)
{
eff_R.block(0,Rdim*r,Rdim,Rdim) =
R.block(0,Rdim*data.G(r),Rdim,Rdim);
}
}
MatrixXd Dl;
if(data.with_dynamics)
{
assert(data.M.rows() == n &&
"No mass matrix. Call arap_precomputation if changing with_dynamics");
const double h = data.h;
assert(h != 0);
//Dl = 1./(h*h*h)*M*(-2.*V0 + Vm1) - fext;
// data.vel = (V0-Vm1)/h
// h*data.vel = (V0-Vm1)
// -h*data.vel = -V0+Vm1)
// -V0-h*data.vel = -2V0+Vm1
const double dw = (1./data.ym)*(h*h);
Dl = dw * (1./(h*h)*data.M*(-U0 - h*data.vel) - data.f_ext);
}
VectorXd Rcol;
columnize(eff_R,num_rots,2,Rcol);
VectorXd Bcol = -data.K * Rcol;
assert(Bcol.size() == data.n*data.dim);
for(int c = 0; c<data.dim; c++)
{
VectorXd Uc,Bc,bcc,Beq;
Bc = Bcol.block(c*n,0,n,1);
if(data.with_dynamics)
{
Bc += Dl.col(c);
}
if(bc.size()>0)
{
bcc = bc.col(c);
}
min_quad_with_fixed_solve(
data.solver_data,
Bc,bcc,Beq,
Uc);
U.col(c) = Uc;
}
iter++;
}
if(data.with_dynamics)
{
// Keep track of velocity for next time
data.vel = (U-U0)/data.h;
}
return true;
}
//Table 11.4 Page 349
//TODO Make COV_SIGMA to a list of cov sigma according to time
double correspondence_test(MatrixXd omega, VectorXd xi, VectorXd mu,
MatrixXd sigma, int j, int k, int t) {
/*
* Create tau for both features j and k
*/
std::vector<int> tauJK;
int row = (t + 1 + j) * 3;
for (int feature = 0; feature < 2; feature++) {
int i = 0;
for (int column = 0; column < (t + 1) * 3; column += 3) {
bool allZeros = true;
for (int featureRow = row; featureRow < row + 3; featureRow++) {
for (int featureCol = column; featureCol < column + 3;
featureCol++) {
if (omega(featureRow, featureCol) != 0) {
allZeros = false;
break;
}
}
if (!allZeros) {
break;
}
}
if (!allZeros) {
// Add only if tauJ doesnt already contain the element
if (std::find(tauJK.begin(), tauJK.end(), i) == tauJK.end()) {
tauJK.push_back(i);
}
}
i++;
}
row = (t + 1 + k) * 3;
}
// std::cout << "tauJK = \n";
// for (int i = 0; i < tauJK.size(); i++) {
// std::cout << tauJK.at(i) << std::endl;
// }
/*
* ----------------------------------------------------------
* LINE 2 OF ALGORITHM PAGE 364
* ----------------------------------------------------------
*/
//CONSTRUCT SIGMA Tau(j,k),Tau(j,k)
MatrixXd sigmapart = MatrixXd::Zero(tauJK.size() * 3, tauJK.size() * 3);
for (int z = 0; z < tauJK.size(); z++) {
for (int x = 0; x < tauJK.size(); x++) {
sigmapart.block(z * 3, x * 3, 3, 3) += sigma.block(tauJK[z]*3,
tauJK[x]*3, 3, 3);
}
}
// std::cout << "sigmaPart = \n" << sigmapart << std::endl;
//CONSTRUCT Omegajk,Tau(j,k)
MatrixXd omega_jk_tauJK = MatrixXd::Zero(6, tauJK.size() * 3);
for (int z = 0; z < tauJK.size(); z++) {
//Assumption the jk in Omega jk,Tau(j,k) means we want position j and k resulting in a 6x(Tau(j,k)*3) long matrix
omega_jk_tauJK.block(0, z * 3, 3, 3) = omega.block(3*(t+1+j), tauJK[z]*3, 3, 3);
omega_jk_tauJK.block(3, z * 3, 3, 3) = omega.block(3*(t+1+k), tauJK[z]*3, 3, 3);
}
// std::cout << "omega_jk_tauJK = \n" << omega_jk_tauJK << std::endl;
//CONSTRUCT Tau(j,k),Omegajk
MatrixXd omega_tauJK_jk = MatrixXd::Zero(tauJK.size() * 3, 6);
for (int z = 0; z < tauJK.size(); z++) {
omega_tauJK_jk.block(z * 3, 0, 3, 3) = omega.block(tauJK[z]*3, 3*(t+1+j), 3, 3);
omega_tauJK_jk.block(z * 3, 3, 3, 3) = omega.block(tauJK[z]*3, 3*(t+1+k), 3, 3);
}
// std::cout << "omega_tauJK_jk = \n" << omega_tauJK_jk << std::endl;
MatrixXd omega_jk_jk = MatrixXd::Zero(6, 6);
omega_jk_jk.block(0, 0, 3, 3) += omega.block(3*(t+1+j), 3*(t+1+j), 3, 3); //Omega jk_jk = Omegaj,j ; Omega j,k ; Omega k,j; Omega k,k ??
omega_jk_jk.block(0, 3, 3, 3) += omega.block(3*(t+1+j), 3*(t+1+k), 3, 3);
omega_jk_jk.block(3, 0, 3, 3) += omega.block(3*(t+1+k), 3*(t+1+j), 3, 3);
omega_jk_jk.block(3, 3, 3, 3) += omega.block(3*(t+1+k), 3*(t+1+k), 3, 3);
// std::cout << "omega_jk_jk = \n" << omega_jk_jk << std::endl;
MatrixXd omega_j_k = omega_jk_jk - omega_jk_tauJK * sigmapart * omega_tauJK_jk;
// std::cout << "omega_j_k = \n" << omega_j_k << std::endl;
/*
* ----------------------------------------------------------
* LINE 2 FINISHED
* ----------------------------------------------------------
*/
/*
* ----------------------------------------------------------
* LINE 3 OF ALGORITHM PAGE 364
* ----------------------------------------------------------
*/
//Construct mu j,k as seen on page 367
MatrixXd omega_jk_jk_inverse = omega_jk_jk.inverse();
// std::cout << "omega_jk_jk_inverse = \n" << omega_jk_jk_inverse << std::endl;
VectorXd mu_taujk = VectorXd::Zero(tauJK.size() * 3);
for (int z = 0; z < tauJK.size(); z ++) {
mu_taujk.block(z * 3, 0, 3, 1) += mu.block(tauJK[z] * 3, 0, 3, 1);
}
// std::cout << "mu_taujk = \n" << mu_taujk << std::endl;
VectorXd xi_j_k = VectorXd::Zero(6);
row = (t + 1 + j) * 3;
for (int z = 0; z < 2; z++) {
xi_j_k.block(z * 3, 0, 3, 1) += xi.block(row, 0, 3, 1);
row = (t + 1 + k) * 3;
}
// std::cout << "xi_j_k = \n" << xi_j_k << std::endl;
MatrixXd mu_j_k = omega_jk_jk_inverse*(xi_j_k+omega_jk_tauJK*mu_taujk);
// std::cout << "mu_j_k = \n" << mu_j_k << std::endl;
xi_j_k = omega_j_k*mu_j_k;
// std::cout << "xi_j_k = \n" << xi_j_k << std::endl;
/*
* ----------------------------------------------------------
* LINE 3 FINISHED
* ----------------------------------------------------------
*/
/*
* ----------------------------------------------------------
* LINE 4 OF ALGORITHM PAGE 364
* ----------------------------------------------------------
*/
MatrixXd unity_one = MatrixXd::Zero(6, 3); // Why three columns?
unity_one <<
1, 0, 0,
0, 1, 0,
0, 0, 1,
-1, 0, 0,
0,-1, 0,
0, 0,-1;
MatrixXd omega_deltaJ_k = unity_one.transpose() * omega_j_k * unity_one;
// std::cout << "omega_deltaJ_k = \n" << omega_deltaJ_k << std::endl;
/*
* ----------------------------------------------------------
* LINE 4 FINISHED
* ----------------------------------------------------------
*/
/*
* ----------------------------------------------------------
* LINE 5 OF ALGORITHM PAGE 364
* ----------------------------------------------------------
*/
VectorXd xi_deltaJ_k = unity_one.transpose()* xi_j_k;
// std::cout << "xi_deltaJ_k = \n" << xi_deltaJ_k << std::endl;
/*
* ----------------------------------------------------------
* LINE 5 FINISHED
* ----------------------------------------------------------
*/
/*
* ----------------------------------------------------------
* LINE 6 OF ALGORITHM PAGE 364
* ----------------------------------------------------------
*/
VectorXd mu_deltaJ_k = omega_deltaJ_k.inverse() * xi_deltaJ_k;
// std::cout << "mu_deltaJ_k = \n" << mu_deltaJ_k << std::endl;
/*
* ----------------------------------------------------------
* LINE 6 FINISHED
* ----------------------------------------------------------
*/
/*
* ----------------------------------------------------------
* LINE 7 OF ALGORITHM PAGE 364
* ----------------------------------------------------------
*/
MatrixXd m = 2*M_PI*omega_deltaJ_k.inverse();
std::cout << "m = \n" << m << std::endl;
double determinant =
m(0,0)*m(1,1)*m(2,2)+
m(0,1)*m(1,2)*m(2,0)+
m(0,2)*m(1,0)*m(2,1)-
m(0,2)*m(1,1)*m(2,2)-
m(0,1)*m(1,0)*m(2,2)-
m(0,0)*m(1,2)*m(2,1);
determinant = abs(determinant); // @TODO: Check if this is correct or neccessary
double normalizer = pow(determinant, -0.5);
double exponent = -0.5*mu_deltaJ_k.transpose()*omega_deltaJ_k.inverse()*mu_deltaJ_k;
double e = exp(exponent);
return normalizer*e;
}
IGL_INLINE bool igl::arap_solve(
const Eigen::PlainObjectBase<Derivedbc> & bc,
ARAPData & data,
Eigen::PlainObjectBase<DerivedU> & U)
{
using namespace igl;
using namespace Eigen;
using namespace std;
assert(data.b.size() == bc.rows());
const int dim = bc.cols();
const int n = data.n;
int iter = 0;
if(U.size() == 0)
{
// terrible initial guess.. should at least copy input mesh
U = MatrixXd::Zero(data.n,dim);
}
// changes each arap iteration
MatrixXd U_prev = U;
// doesn't change for fixed with_dynamics timestep
MatrixXd U0;
if(data.with_dynamics)
{
U0 = U_prev;
}
while(iter < data.max_iter)
{
U_prev = U;
// enforce boundary conditions exactly
for(int bi = 0;bi<bc.rows();bi++)
{
U.row(data.b(bi)) = bc.row(bi);
}
MatrixXd S = data.CSM * U.replicate(dim,1);
MatrixXd R(dim,data.CSM.rows());
#ifdef __SSE__ // fit_rotations_SSE will convert to float if necessary
fit_rotations_SSE(S,R);
#else
fit_rotations(S,R);
#endif
//for(int k = 0;k<(data.CSM.rows()/dim);k++)
//{
// R.block(0,dim*k,dim,dim) = MatrixXd::Identity(dim,dim);
//}
// Number of rotations: #vertices or #elements
int num_rots = data.K.cols()/dim/dim;
// distribute group rotations to vertices in each group
MatrixXd eff_R;
if(data.G.size() == 0)
{
// copy...
eff_R = R;
}else
{
eff_R.resize(dim,num_rots*dim);
for(int r = 0;r<num_rots;r++)
{
eff_R.block(0,dim*r,dim,dim) =
R.block(0,dim*data.G(r),dim,dim);
}
}
MatrixXd Dl;
if(data.with_dynamics)
{
assert(M.rows() == n &&
"No mass matrix. Call arap_precomputation if changing with_dynamics");
const double h = data.h;
assert(h != 0);
//Dl = 1./(h*h*h)*M*(-2.*V0 + Vm1) - fext;
// data.vel = (V0-Vm1)/h
// h*data.vel = (V0-Vm1)
// -h*data.vel = -V0+Vm1)
// -V0-h*data.vel = -2V0+Vm1
Dl = 1./(h*h)*data.M*(-U0 - h*data.vel) - data.f_ext;
}
VectorXd Rcol;
columnize(eff_R,num_rots,2,Rcol);
VectorXd Bcol = -data.K * Rcol;
for(int c = 0;c<dim;c++)
{
VectorXd Uc,Bc,bcc,Beq;
Bc = Bcol.block(c*n,0,n,1);
if(data.with_dynamics)
{
Bc += Dl.col(c);
}
bcc = bc.col(c);
min_quad_with_fixed_solve(
data.solver_data,
Bc,bcc,Beq,
Uc);
U.col(c) = Uc;
}
iter++;
}
if(data.with_dynamics)
{
// Keep track of velocity for next time
data.vel = (U-U0)/data.h;
}
return true;
}
mfloat_t CKroneckerLMM::nLLeval(mfloat_t ldelta, const MatrixXdVec& A,const MatrixXdVec& X, const MatrixXd& Y, const VectorXd& S_C1, const VectorXd& S_R1, const VectorXd& S_C2, const VectorXd& S_R2)
{
//#define debugll
muint_t R = (muint_t)Y.rows();
muint_t C = (muint_t)Y.cols();
assert(A.size() == X.size());
assert(R == (muint_t)S_R1.rows());
assert(C == (muint_t)S_C1.rows());
assert(R == (muint_t)S_R2.rows());
assert(C == (muint_t)S_C2.rows());
muint_t nWeights = 0;
for(muint_t term = 0; term < A.size();++term)
{
assert((muint_t)(X[term].rows())==R);
assert((muint_t)(A[term].cols())==C);
nWeights+=(muint_t)(A[term].rows()) * (muint_t)(X[term].cols());
}
mfloat_t delta = exp(ldelta);
mfloat_t ldet = 0.0;//R * C * ldelta;
//build D and compute the logDet of D
MatrixXd D = MatrixXd(R,C);
for (muint_t r=0; r<R;++r)
{
if(S_R2(r)>1e-10)
{
ldet += (mfloat_t)C * log(S_R2(r));//ldet
}
else
{
std::cout << "S_R2(" << r << ")="<< S_R2(r)<<"\n";
}
}
#ifdef debugll
std::cout << ldet;
std::cout << "\n";
#endif
for (muint_t c=0; c<C;++c)
{
if(S_C2(c)>1e-10)
{
ldet += (mfloat_t)R * log(S_C2(c));//ldet
}
else
{
std::cout << "S_C2(" << c << ")="<< S_C2(c)<<"\n";
}
}
#ifdef debugll
std::cout << ldet;
std::cout << "\n";
#endif
for (muint_t r=0; r<R;++r)
{
for (muint_t c=0; c<C;++c)
{
mfloat_t SSd = S_R1.data()[r]*S_C1.data()[c] + delta;
ldet+=log(SSd);
D(r,c) = 1.0/SSd;
}
}
#ifdef debugll
std::cout << ldet;
std::cout << "\n";
#endif
MatrixXd DY = Y.array() * D.array();
VectorXd XYA = VectorXd(nWeights);
muint_t cumSumR = 0;
MatrixXd covW = MatrixXd(nWeights,nWeights);
for(muint_t termR = 0; termR < A.size();++termR){
muint_t nW_AR = A[termR].rows();
muint_t nW_XR = X[termR].cols();
muint_t rowsBlock = nW_AR * nW_XR;
MatrixXd XYAblock = X[termR].transpose() * DY * A[termR].transpose();
XYAblock.resize(rowsBlock,1);
XYA.block(cumSumR,0,rowsBlock,1) = XYAblock;
muint_t cumSumC = 0;
for(muint_t termC = 0; termC < A.size(); ++termC){
muint_t nW_AC = A[termC].rows();
muint_t nW_XC = X[termC].cols();
muint_t colsBlock = nW_AC * nW_XC;
MatrixXd block = MatrixXd::Zero(rowsBlock,colsBlock);
if (R<C)
{
for(muint_t r=0; r<R; ++r)
{
MatrixXd AD = A[termR];
AD.array().rowwise() *= D.row(r).array();
MatrixXd AA = AD * A[termC].transpose();
//sum up col matrices
MatrixXd XX = X[termR].row(r).transpose() * X[termC].row(r);
akron(block,AA,XX,true);
}
}
else
{//sum up col matrices
for(muint_t c=0; c<C; ++c)
{
MatrixXd XD = X[termR];
XD.array().colwise() *= D.col(c).array();
MatrixXd XX = XD.transpose() * X[termC];
//sum up col matrices
MatrixXd AA = A[termR].col(c) * A[termC].col(c).transpose();
akron(block,AA,XX,true);
}
}
covW.block(cumSumR, cumSumC, rowsBlock, colsBlock) = block;
cumSumC+=colsBlock;
}
cumSumR+=rowsBlock;
}
//std::cout << "covW = " << covW<<std::endl;
MatrixXd W_vec = covW.colPivHouseholderQr().solve(XYA);
//MatrixXd W_vec = covW * XYA;
//std::cout << "W = " << W_vec<<std::endl;
//std::cout << "XYA = " << XYA<<std::endl;
mfloat_t res = (Y.array()*DY.array()).sum();
mfloat_t varPred = (W_vec.array() * XYA.array()).sum();
res-= varPred;
mfloat_t sigma = res/(mfloat_t)(R*C);
mfloat_t nLL = 0.5 * ( R * C * (L2pi + log(sigma) + 1.0) + ldet);
#ifdef returnW
covW = covW.inverse();
//std::cout << "covW.inverse() = " << covW<<std::endl;
muint_t cumSum = 0;
VectorXd F_vec = W_vec.array() * W_vec.array() /covW.diagonal().array() / sigma;
for(muint_t term = 0; term < A.size();++term)
{
muint_t currSize = X[term].cols() * A[term].rows();
//W[term] = MatrixXd(X[term].cols(),A[term].rows());
W[term] = W_vec.block(cumSum,0,currSize,1);//
W[term].resize(X[term].cols(),A[term].rows());
//F_tests[term] = MatrixXd(X[term].cols(),A[term].rows());
F_tests[term] = F_vec.block(cumSum,0,currSize,1);//
F_tests[term].resize(X[term].cols(),A[term].rows());
cumSum+=currSize;
}
#endif
return nLL;
}