Beispiel #1
0
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;
}
Beispiel #2
0
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();
		}
Beispiel #4
0
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;

}
Beispiel #6
0
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;
}
Beispiel #7
0
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;
}