Ejemplo n.º 1
0
Eigen::Matrix<double, 3, 3> Filter::unvectorizeMatrix(Eigen::Block<FilterState::StateType, 9, 1> vec) {
    Eigen::Matrix<double, 3, 3> mat;
    mat.row(0) = vec.block<3, 1>(0, 0);
    mat.row(1) = vec.block<3, 1>(3, 0);
    mat.row(2) = vec.block<3, 1>(6, 0);
    return mat;
}
Ejemplo n.º 2
0
Eigen::Matrix<double, 9, 1> Filter::vectorizeMatrix(const Eigen::Matrix<double, 3, 3> &mat) {
    Eigen::Matrix<double, 9, 1> vec;
    vec.block<3, 1>(0, 0) = mat.row(0);
    vec.block<3, 1>(3, 0) = mat.row(1);
    vec.block<3, 1>(6, 0) = mat.row(2);
    return vec;
}
Ejemplo n.º 3
0
    void
    factor_U(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& U,
             Eigen::Array<T, Eigen::Dynamic, 1>& CPCs) {
      size_t K = U.rows();
      size_t position = 0;
      size_t pull = K - 1;

      if (K == 2) {
        CPCs(0) = atanh(U(0, 1));
        return;
      }

      Eigen::Array<T, 1, Eigen::Dynamic> temp = U.row(0).tail(pull);

      CPCs.head(pull) = temp;

      Eigen::Array<T, Eigen::Dynamic, 1> acc(K);
      acc(0) = -0.0;
      acc.tail(pull) = 1.0 - temp.square();
      for (size_t i = 1; i < (K - 1); i++) {
        position += pull;
        pull--;
        temp = U.row(i).tail(pull);
        temp /= sqrt(acc.tail(pull) / acc(i));
        CPCs.segment(position, pull) = temp;
        acc.tail(pull) *= 1.0 - temp.square();
      }
      CPCs = 0.5 * ( (1.0 + CPCs) / (1.0 - CPCs) ).log();  // now unbounded
    }
Ejemplo n.º 4
0
 inline Eigen::Matrix<double, R1, 1>
 rows_dot_product(const Eigen::Matrix<double, R1, C1>& v1, 
                     const Eigen::Matrix<double, R2, C2>& v2) {
   validate_matching_sizes(v1,v2,"rows_dot_product");
   Eigen::Matrix<double, R1, 1> ret(v1.rows(),1);
   for (size_type j = 0; j < v1.rows(); ++j) {
     ret(j) = v1.row(j).dot(v2.row(j));
   }
   return ret;
 }    
Ejemplo n.º 5
0
void ParticleManager::disperseStateForParticle(int particleNum, Eigen::Matrix< double, Eigen::Dynamic, 1 >& newState, bool fSingleParticle)
{
	int currentRow = fSingleParticle? 0 : particleNum * 6;
	for (int i = 0; i < m_particles[particleNum]->mPosition.rows(); i++)
	{
		m_particles[particleNum]->mPosition.row(i) = newState.row(currentRow++);
	}
	for (int i = 0; i < m_particles[particleNum]->mVelocity.rows(); i++)
	{
		m_particles[particleNum]->mVelocity.row(i) = newState.row(currentRow++);
	}
}
Ejemplo n.º 6
0
Eigen::Matrix<typename Derived::Scalar, 3, 3> rpy2rotmat(const Eigen::MatrixBase<Derived>& rpy)
{
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 3);
  auto rpy_array = rpy.array();
  auto s = rpy_array.sin();
  auto c = rpy_array.cos();

  Eigen::Matrix<typename Derived::Scalar, 3, 3> R;
  R.row(0) << c(2) * c(1), c(2) * s(1) * s(0) - s(2) * c(0), c(2) * s(1) * c(0) + s(2) * s(0);
  R.row(1) << s(2) * c(1), s(2) * s(1) * s(0) + c(2) * c(0), s(2) * s(1) * c(0) - c(2) * s(0);
  R.row(2) << -s(1), c(1) * s(0), c(1) * c(0);

  return R;
}
Ejemplo n.º 7
0
void compute_motor_0w_polynomial_coefficients(Eigen::Matrix <double, N_SEGMENTS, N_MOTORS> & m0w_, const Eigen::Matrix <
        double, N_SEGMENTS + 1, N_MOTORS> motor_interpolations_)
{
    // Zero all matrix coefficients.
    m0w_ = Eigen::Matrix <double, N_SEGMENTS, N_MOTORS>::Zero(N_SEGMENTS, N_MOTORS);

    // Compute 03w for all segments.
    for (int sgt = 0; sgt < N_SEGMENTS; ++sgt) {
        m0w_.row(sgt) = motor_interpolations_.row(sgt);
    }

#if 0
    cout << "m0w:\n" << m0w_ << endl;
#endif
}
 static typename DerivedF::Scalar  add_vertex(const Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1> &values,
                                              const Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 3> &points,
                                              unsigned int i0,
                                              unsigned int i1,
                                              PointMatrixType &vertices,
                                              int &num_vertices,
                                              MyMap &edge2vertex)
 {
   // find vertex if it has been computed already
   MyMapIterator it = edge2vertex.find(EdgeKey(i0, i1));
   if (it != edge2vertex.end()) 
     return it->second;
   ;
   
   // generate new vertex
   const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> & p0 = points.row(i0);
   const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> & p1 = points.row(i1);
   
   typename DerivedV::Scalar s0 = fabs(values[i0]);
   typename DerivedV::Scalar s1 = fabs(values[i1]);
   typename DerivedV::Scalar t  = s0 / (s0+s1);
   
   
   num_vertices++;
   if (num_vertices > vertices.rows())
     vertices.conservativeResize(vertices.rows()+10000, Eigen::NoChange);
   
   vertices.row(num_vertices-1)  = (1.0f-t)*p0 + t*p1;
   edge2vertex[EdgeKey(i0, i1)] = num_vertices-1;
   
   return num_vertices-1;
 }
Ejemplo n.º 9
0
std::shared_ptr<Geometry> generateTexQuadGeometry(
	float width, float height, Eigen::Vector3f pos, Eigen::Matrix3f rot) {

	Eigen::Matrix<float, 6, 3 + 2, Eigen::RowMajor> vertex;

	GLfloat vertex_pos_uv[] = {
		-1.0f, 0, -1.0f, 0, 1,
		1.0f, 0, 1.0f, 1, 0,
		-1.0f, 0, 1.0f, 0, 0,

		 1.0f, 0, 1.0f, 1, 0,
		 -1.0f, 0, -1.0f, 0, 1,
		 1.0f, 0, -1.0f, 1, 1,
	};

	for(int i = 0; i < 6; i++) {
		Eigen::Vector3f p_local(
			vertex_pos_uv[i * 5 + 0] * width / 2,
			vertex_pos_uv[i * 5 + 1],
			vertex_pos_uv[i * 5 + 2] * height / 2);

		vertex.row(i).head(3) = rot * p_local + pos;
		vertex.row(i).tail(2) = Eigen::Vector2f(
			vertex_pos_uv[i * 5 + 3], vertex_pos_uv[i * 5 + 4]);
	}

	return Geometry::createPosUV(vertex.rows(), vertex.data());
}
Ejemplo n.º 10
0
Eigen::Matrix<typename Derived::Scalar, 3, 3> quat2rotmat(const Eigen::MatrixBase<Derived>& q)
{
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4);
  auto q_normalized = q.normalized();
  auto w = q_normalized(0);
  auto x = q_normalized(1);
  auto y = q_normalized(2);
  auto z = q_normalized(3);

  Eigen::Matrix<typename Derived::Scalar, 3, 3> M;
  M.row(0) << w * w + x * x - y * y - z * z, 2.0 * x * y - 2.0 * w * z, 2.0 * x * z + 2.0 * w * y;
  M.row(1) << 2.0 * x * y + 2.0 * w * z, w * w + y * y - x * x - z * z, 2.0 * y * z - 2.0 * w * x;
  M.row(2) << 2.0 * x * z - 2.0 * w * y, 2.0 * y * z + 2.0 * w * x, w * w + z * z - x * x - y * y;

  return M;
}
Ejemplo n.º 11
0
IGL_INLINE void igl::mat_min(
  const Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> & X,
  const int dim,
  Eigen::Matrix<T,Eigen::Dynamic,1> & Y,
  Eigen::Matrix<int,Eigen::Dynamic,1> & I)
{
  assert(dim==1||dim==2);

  // output size
  int n = (dim==1?X.cols():X.rows());
  // resize output
  Y.resize(n);
  I.resize(n);

  // loop over dimension opposite of dim
  for(int j = 0;j<n;j++)
  {
    typename Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic>::Index PHONY;
    typename Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic>::Index i;
    T m;
    if(dim==1)
    {
      m = X.col(j).minCoeff(&i,&PHONY);
    }else
    {
      m = X.row(j).minCoeff(&PHONY,&i);
    }
    Y(j) = m;
    I(j) = i;
  }
}
void GreenStrain_LIMSolver3D::prepareProblemData(std::vector<int>& hessRowIdx, std::vector<int>& hessColIdx)
{
  const int numNodes = mesh->InitalVertices->rows();

  // Compute deformation gradients
  int numTets = mesh->Tetrahedra->rows();
  Ms.resize(4,3*numTets);
  MMTs.resize(4,4*numTets);

  Eigen::Matrix<double,4,3> SelectorM;
  SelectorM.block<3,3>(0,0) = Eigen::Matrix3d::Identity();
  SelectorM.row(3) = Eigen::Vector3d::Ones()*-1;
  
  for(int t=0;t<numTets;t++)
  {
    Eigen::VectorXi indices = TetrahedronVertexIdx.col(t);

    Eigen::Vector3d A = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,0)).cast<double>();
    Eigen::Vector3d B = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,1)).cast<double>();
    Eigen::Vector3d C = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,2)).cast<double>();
    Eigen::Vector3d D = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,3)).cast<double>();

    Eigen::Matrix3d V;
    V << A-D,B-D,C-D;
    
    Eigen::Matrix<double,4,3> Mtemp = SelectorM*V.inverse().cast<double>();
    Ms.block<4,3>(0,3*t) = Mtemp;
    MMTs.block<4,4>(0,4*t) = Mtemp*Mtemp.transpose();
  }
}
Ejemplo n.º 13
0
void GreenStrain_LIMSolver2D::prepareProblemData(std::vector<int>& hessRowIdx, std::vector<int>& hessColIdx)
{
  // Compute deformation gradients
  int numTets = mesh->Triangles->rows();
  Ms.resize(3,2*numTets);
  MMTs.resize(3,3*numTets);

  Eigen::Matrix<double,3,2> SelectorM;
  SelectorM.block<2,2>(0,0) = Eigen::Matrix<double,2,2>::Identity();
  SelectorM.row(2) = Eigen::Vector2d::Ones()*-1;
  
  for(int t=0;t<numTets;t++)
  {
    Eigen::Vector2d A,B,C;
    if(mesh->IsCorotatedTriangles)
    {
      A = mesh->CorotatedTriangles->row(t).block<1,2>(0,0).cast<double>();
      B = mesh->CorotatedTriangles->row(t).block<1,2>(0,2).cast<double>();
      C = mesh->CorotatedTriangles->row(t).block<1,2>(0,4).cast<double>();	
    }
    else
    {
      A = mesh->InitalVertices->row(mesh->Triangles->coeff(t,0)).block<1,2>(0,0).cast<double>();
      B = mesh->InitalVertices->row(mesh->Triangles->coeff(t,1)).block<1,2>(0,0).cast<double>();
      C = mesh->InitalVertices->row(mesh->Triangles->coeff(t,2)).block<1,2>(0,0).cast<double>();
    }

    Eigen::Matrix2d V;
    V << A-C,B-C;
    
    Eigen::Matrix<double,3,2> Mtemp = SelectorM*V.inverse().cast<double>();
    Ms.block<3,2>(0,2*t) = Mtemp;
    MMTs.block<3,3>(0,3*t) = Mtemp*Mtemp.transpose();
  }
}
Ejemplo n.º 14
0
inline
Eigen::Matrix<T, 1, Eigen::Dynamic>
row(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& m,
    size_t i) {
    stan::math::check_row_index("row", "j", m, i);

    return m.row(i - 1);
}
Ejemplo n.º 15
0
const CPoint3DCAMERA CMiniVisionToolbox::getPointStereoLinearTriangulationSVDDLT( const cv::Point2d& p_ptPointLEFT, const cv::Point2d& p_ptPointRIGHT, const Eigen::Matrix< double, 3, 4 >& p_matProjectionLEFT, const Eigen::Matrix< double, 3, 4 >& p_matProjectionRIGHT )
{
    //ds A matrix for system: A*X=0
    Eigen::Matrix4d matAHomogeneous;

    matAHomogeneous.row(0) = p_ptPointLEFT.x*p_matProjectionLEFT.row(2)-p_matProjectionLEFT.row(0);
    matAHomogeneous.row(1) = p_ptPointLEFT.y*p_matProjectionLEFT.row(2)-p_matProjectionLEFT.row(1);
    matAHomogeneous.row(2) = p_ptPointRIGHT.x*p_matProjectionRIGHT.row(2)-p_matProjectionRIGHT.row(0);
    matAHomogeneous.row(3) = p_ptPointRIGHT.y*p_matProjectionRIGHT.row(2)-p_matProjectionRIGHT.row(1);

    //ds solve system subject to ||A*x||=0 ||x||=1
    const Eigen::JacobiSVD< Eigen::Matrix4d > matSVD( matAHomogeneous, Eigen::ComputeFullU | Eigen::ComputeFullV );

    //ds solution x is the last column of V
    const Eigen::Vector4d vecX( matSVD.matrixV( ).col( 3 ) );

    return vecX.head( 3 )/vecX(3);
}
Ejemplo n.º 16
0
 inline Eigen::Matrix<fvar<T>,R,1> 
 rows_dot_self(const Eigen::Matrix<fvar<T>,R,C>& x) {
   Eigen::Matrix<fvar<T>,R,1> ret(x.rows(),1);
   for (size_type i = 0; i < x.rows(); i++) {
     Eigen::Matrix<fvar<T>,1,C> crow = x.row(i);
     ret(i,0) = dot_self(crow);
   }
   return ret;
 }
Ejemplo n.º 17
0
void NuTo::CollidableWallBase::VisualizationStatic(NuTo::Visualize::UnstructuredGrid& rVisualizer) const
{

    double size = 1.;
    if (*mBoxes.begin() == mOutsideBox)
        size = 2.;

    Eigen::Matrix<double, 4, 3> corners;

    // get some vector != mDirection
    Eigen::Vector3d random;
    random << 1, 0, 0;
    if (std::abs(random.dot(mDirection)) == 1)
    {
        random << 0, 1, 0;
    }

    Eigen::Vector3d transversal = random.cross(mDirection);
    Eigen::Vector3d transversal2 = transversal.cross(mDirection);

    //   normalize to size/2;
    transversal.normalize();
    transversal2.normalize();

    transversal *= size / 2;
    transversal2 *= size / 2;

    corners.row(0) = (mPosition + transversal + transversal2).transpose();
    corners.row(1) = (mPosition + transversal - transversal2).transpose();
    corners.row(2) = (mPosition - transversal - transversal2).transpose();
    corners.row(3) = (mPosition - transversal + transversal2).transpose();


    std::vector<int> cornerIndex(4);
    for (int i = 0; i < 4; ++i)
    {
        cornerIndex[i] = rVisualizer.AddPoint(corners.row(i));
    }
    int insertIndex = rVisualizer.AddCell(cornerIndex, eCellTypes::QUAD);

    rVisualizer.SetCellData(insertIndex, "Direction", mDirection);
}
Ejemplo n.º 18
0
Archivo: grad.cpp Proyecto: azer89/BBW
IGL_INLINE void igl::grad(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &V,
  const Eigen::Matrix<S, Eigen::Dynamic, Eigen::Dynamic> &F,
  const Eigen::Matrix<T, Eigen::Dynamic, 1>&X,
  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &G )
{
  G = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Zero(F.rows(),3);
  for (int i = 0; i<F.rows(); ++i)
  {
    // renaming indices of vertices of triangles for convenience
    int i1 = F(i,0);
    int i2 = F(i,1);
    int i3 = F(i,2);
    
    // #F x 3 matrices of triangle edge vectors, named after opposite vertices
    Eigen::Matrix<T, 1, 3> v32 = V.row(i3) - V.row(i2);
    Eigen::Matrix<T, 1, 3> v13 = V.row(i1) - V.row(i3);
    Eigen::Matrix<T, 1, 3> v21 = V.row(i2) - V.row(i1);
    
    // area of parallelogram is twice area of triangle
    // area of parallelogram is || v1 x v2 || 
    Eigen::Matrix<T, 1, 3> n  = v32.cross(v13); 
    
    // This does correct l2 norm of rows, so that it contains #F list of twice
    // triangle areas
    double dblA = std::sqrt(n.dot(n));
    
    // now normalize normals to get unit normals
    Eigen::Matrix<T, 1, 3> u = n / dblA;
    
    // rotate each vector 90 degrees around normal
    double norm21 = std::sqrt(v21.dot(v21));
    double norm13 = std::sqrt(v13.dot(v13));
    Eigen::Matrix<T, 1, 3> eperp21 = u.cross(v21);
    eperp21 = eperp21 / std::sqrt(eperp21.dot(eperp21));
    eperp21 *= norm21;
    Eigen::Matrix<T, 1, 3> eperp13 = u.cross(v13);
    eperp13 = eperp13 / std::sqrt(eperp13.dot(eperp13));
    eperp13 *= norm13;
    
    G.row(i) = ((X[i2] -X[i1]) *eperp13 + (X[i3] - X[i1]) *eperp21) / dblA;
  };
}
Ejemplo n.º 19
0
void ParticleManager::accumulateDerivativeForParticle(int particleNum, Eigen::Matrix< double, Eigen::Dynamic, 1 >& derivative, bool fVelocity, bool fAcceleration, bool fSingleParticle)
{
	assert( fVelocity == true || fAcceleration == true ); 
	int stateSize = (fVelocity & fAcceleration) ? 6 : 3;
	int currentRow = fSingleParticle? 0 : particleNum * stateSize;
	if ( fVelocity )
	{
		for (int i = 0; i < m_particles[particleNum]->mVelocity.rows(); i++)
		{
			derivative.row(currentRow++) = m_particles[particleNum]->mVelocity.row(i);
		}
	}
	if ( fAcceleration )
	{
		for (int i = 0; i < m_particles[particleNum]->mAccumulatedForce.rows(); i++)
		{
			derivative.row(currentRow++) = (m_particles[particleNum]->mAccumulatedForce.row(i)/m_particles[particleNum]->mMass);
		}
	}
}
Ejemplo n.º 20
0
void ParticleManager::accumulateStateForParticle(int particleNum, Eigen::Matrix< double, Eigen::Dynamic, 1>& currentState, bool fPosition, bool fVelocity, bool fSingleParticle)
{
	assert( fPosition == true || fVelocity == true ); 
	int stateSize = (fPosition & fVelocity) ? 6 : 3;
	int currentRow = fSingleParticle? 0 : particleNum * stateSize;
	if ( fPosition )
	{
		for (int i = 0; i < m_particles[particleNum]->mPosition.rows(); i++)
		{
			currentState.row(currentRow++) = m_particles[particleNum]->mPosition.row(i);
		}
	}
	if ( fVelocity )
	{
		for (int i = 0; i < m_particles[particleNum]->mVelocity.rows(); i++)
		{
			currentState.row(currentRow++) = m_particles[particleNum]->mVelocity.row(i);
		}
	}
}
Ejemplo n.º 21
0
void compute_motor_3w_polynomial_coefficients(Eigen::Matrix <double, N_SEGMENTS, N_MOTORS> & m3w_, const Eigen::Matrix <
        double, N_SEGMENTS, N_MOTORS> m2w_, const Eigen::Matrix <double, N_SEGMENTS, N_MOTORS> motor_deltas_, const Eigen::Matrix <
        double, N_SEGMENTS, 1> taus_)
{
    // Zero all matrix coefficients.
    m3w_ = Eigen::Matrix <double, N_SEGMENTS, N_MOTORS>::Zero(N_SEGMENTS, N_MOTORS);

    // Compute 3w for last segment.
    m3w_.row(N_SEGMENTS - 1) = -m2w_.row(N_SEGMENTS - 1) / (taus_(N_SEGMENTS - 1) * 2.0) - motor_deltas_.row(N_SEGMENTS
                               - 1) / (taus_(N_SEGMENTS - 1) * taus_(N_SEGMENTS - 1) * taus_(N_SEGMENTS - 1) * 2.0);

    // Compute 3w for other segments.
    for (int sgt = 0; sgt < N_SEGMENTS - 1; ++sgt) {
        m3w_.row(sgt) = (m2w_.row(sgt + 1) - m2w_.row(sgt)) / (taus_(sgt) * 3.0);
    }

#if 0
    cout << "3w:\n" << m3w_ << endl;
#endif
}
IGL_INLINE void igl::average_onto_vertices(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &V,
            const Eigen::Matrix<I, Eigen::Dynamic, Eigen::Dynamic> &F,
            const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &S,
            Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &SV)
{

  SV = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Zero(V.rows(),S.cols());
  Eigen::Matrix<T, Eigen::Dynamic, 1> COUNT = Eigen::Matrix<T, Eigen::Dynamic, 1>::Zero(V.rows());
  for (int i = 0; i <F.rows(); ++i)
  {
    for (int j = 0; j<F.cols(); ++j)
    {
      SV.row(F(i,j)) += S.row(i);
      COUNT[F(i,j)] ++;
    }
  }
  for (int i = 0; i <V.rows(); ++i)
    SV.row(i) /= COUNT[i];

};
Ejemplo n.º 23
0
 Eigen::Matrix<double,3,3>
 Triangle::localStiffMatrix() const{
   Eigen::Matrix<double,3,2> n;
   Triangle const & t(*this);
   double meas2=1/(2.0*t.measure());
   for (int i=0;i<3;++i){
     int j =  (i+1)% 3;
     int k = (j+1) % 3;
     n.row(i)<< (-t[k][1]+t[j][1]),(t[k][0]-t[j][0]);
   }
   n*=meas2;
   double off01=n.row(0).dot(n.row(1));
   double off02=n.row(0).dot(n.row(2));
   double off12=n.row(1).dot(n.row(2));
   Eigen::Matrix<double,3,3> s;
   s<<-(off01+off02),off01,off02,
     off01,-(off01+off12),off12,
     off02,off12,-(off02+off12);
   return s;
 }
Ejemplo n.º 24
0
//TODO msati: Optimize this portion
void ParticleManager::getInvMassMatrix( Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic >& invMassMatrix )
{
	invMassMatrix.resize( m_particles.size() * 3, m_particles.size() * 3 );
	invMassMatrix.setZero();
	for (int i = 0; i < m_particles.size(); i++)
	{
		for (int j = 0; j < 3; j++)
		{
			invMassMatrix.row(3*i+j).array()[3*i+j] = 1/(m_particles[i]->mMass);
		}
	}
}
Ejemplo n.º 25
0
    typename boost::math::tools::promote_args<T_y, T_covar, T_w>::type
    multi_gp_cholesky_log(const Eigen::Matrix
                          <T_y, Eigen::Dynamic, Eigen::Dynamic>& y,
                          const Eigen::Matrix
                          <T_covar, Eigen::Dynamic, Eigen::Dynamic>& L,
                          const Eigen::Matrix<T_w, Eigen::Dynamic, 1>& w) {
      static const char* function("multi_gp_cholesky_log");
      typedef
        typename boost::math::tools::promote_args<T_y, T_covar, T_w>::type T_lp;
      T_lp lp(0.0);


      check_size_match(function,
                       "Size of random variable (rows y)", y.rows(),
                       "Size of kernel scales (w)", w.size());
      check_size_match(function,
                       "Size of random variable", y.cols(),
                       "rows of covariance parameter", L.rows());
      check_finite(function, "Kernel scales", w);
      check_positive(function, "Kernel scales", w);
      check_finite(function, "Random variable", y);

      if (y.rows() == 0)
        return lp;

      if (include_summand<propto>::value) {
        lp += NEG_LOG_SQRT_TWO_PI * y.rows() * y.cols();
      }

      if (include_summand<propto, T_covar>::value) {
        lp -= L.diagonal().array().log().sum() * y.rows();
      }

      if (include_summand<propto, T_w>::value) {
        lp += 0.5 * y.cols() * sum(log(w));
      }

      if (include_summand<propto, T_y, T_w, T_covar>::value) {
        T_lp sum_lp_vec(0.0);
        for (int i = 0; i < y.rows(); i++) {
          Eigen::Matrix<T_y, Eigen::Dynamic, 1> y_row(y.row(i));
          Eigen::Matrix<typename boost::math::tools::promote_args
                        <T_y, T_covar>::type,
                        Eigen::Dynamic, 1>
            half(mdivide_left_tri_low(L, y_row));
          sum_lp_vec += w(i) * dot_self(half);
        }
        lp -= 0.5*sum_lp_vec;
      }

      return lp;
    }
Ejemplo n.º 26
0
  IGL_INLINE Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> barycentric_to_global(
    const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> & V, 
    const Eigen::Matrix<Index,Eigen::Dynamic,Eigen::Dynamic>  & F, 
    const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> & bc)
  {
    Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> R;
    R.resize(bc.rows(),3);

    for (unsigned i=0; i<R.rows(); ++i)
    {
      unsigned id = round(bc(i,0));
      double u   = bc(i,1);  
      double v   = bc(i,2);

      if (id != -1)
        R.row(i) = V.row(F(id,0)) +
                  ((V.row(F(id,1)) - V.row(F(id,0))) * u +
                   (V.row(F(id,2)) - V.row(F(id,0))) * v  );
      else
        R.row(i) << 0,0,0;
    }
    return R;
  }
Ejemplo n.º 27
0
 bool
 check_cholesky_factor_corr(const char* function,
                            const char* name,
                            const Eigen::Matrix<T_y, Dynamic, Dynamic>& y) {
   check_square(function, name, y);
   check_lower_triangular(function, name, y);
   for (int i = 0; i < y.rows(); ++i)
     check_positive(function, name, y(i, i));
   for (int i = 0; i < y.rows(); ++i) {
     Eigen::Matrix<T_y, Dynamic, 1>
       y_i = y.row(i).transpose();
     check_unit_vector(function, name, y_i);
   }
   return true;
 }
Ejemplo n.º 28
0
 inline 
 Eigen::Matrix<fvar<typename stan::return_type<T1,T2>::type>,R1,C2> 
 multiply(const Eigen::Matrix<fvar<T1>,R1,C1>& m1,
          const Eigen::Matrix<T2,R2,C2>& m2) {
   stan::math::validate_multiplicable(m1,m2,"multiply");
   Eigen::Matrix<fvar<typename stan::return_type<T1,T2>::type>,R1,C2> result(m1.rows(),m2.cols());
   for (size_type i = 0; i < m1.rows(); i++) {
     Eigen::Matrix<fvar<T1>,1,C1> crow = m1.row(i);
     for (size_type j = 0; j < m2.cols(); j++) {
       Eigen::Matrix<T2,R2,1> ccol = m2.col(j);
       result(i,j) = stan::agrad::dot_product(crow,ccol);
       }
     }
   return result;
 }
Ejemplo n.º 29
0
 inline 
 Eigen::Matrix<fvar<T>,R1,C2> 
 multiply(const Eigen::Matrix<fvar<T>,R1,C1>& m1,
          const Eigen::Matrix<double,R2,C2>& m2) {
   stan::math::check_multiplicable("multiply(%1%)",m1,"m1",
                                   m2,"m2",(double*)0);
   Eigen::Matrix<fvar<T>,R1,C2> result(m1.rows(),m2.cols());
   for (size_type i = 0; i < m1.rows(); i++) {
     Eigen::Matrix<fvar<T>,1,C1> crow = m1.row(i);
     for (size_type j = 0; j < m2.cols(); j++) {
       Eigen::Matrix<double,R2,1> ccol = m2.col(j);
       result(i,j) = stan::agrad::dot_product(crow,ccol);
     }
   }
   return result;
 }
Ejemplo n.º 30
0
void compute_motor_deltas_for_segments(Eigen::Matrix <double, N_SEGMENTS, N_MOTORS> & motor_deltas_for_segments_, const Eigen::Matrix <
                                       double, N_SEGMENTS + 1, N_MOTORS> motor_interpolations_)
{
    for (int segment = 0; segment < N_SEGMENTS; ++segment) {
        for (int axis = 0; axis < N_MOTORS; ++axis) {
            motor_deltas_for_segments_(segment, axis) = motor_interpolations_(segment + 1, axis)
                    - motor_interpolations_(segment, axis);
        }
    }

#if 0
    // Display all motor increments.
    for (unsigned int l = 0; l < N_SEGMENTS; ++l) {
        cout << "Motor increments for segment " << l << ": " << motor_deltas_for_segments_.row(l) << endl;
    }
#endif
}