C++ Matrix::row方法代码示例

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;
}

    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
    }

 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;
 }    

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++);
	}
}

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;
}

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;
 }

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());
}

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;
}

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();
  }
}

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();
  }
}

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);
}

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);
}

 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;
 }