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

Eigen::MatrixXd Tra_via0( double x0 , double v0 , double a0,
		                  double xf , double vf , double af,
						  double smp , double tf )
/*
   simple minimum jerk trajectory

   x0 : position at initial state
   v0 : velocity at initial state
   a0 : acceleration at initial state

   xf : position at final state
   vf : velocity at final state
   af : acceleration at final state

   smp : sampling time

   tf : movement time
*/

{
	Eigen::MatrixXd A( 3 , 3 );
	Eigen::MatrixXd B( 3 , 1 );

	A << pow( tf , 3 )	   , pow( tf , 4 )	    , pow( tf , 5 ),
		 3 * pow( tf , 2 ) , 4 * pow( tf , 3 )	, 5 * pow( tf , 4 ),
		 6 * tf		       , 12 * pow( tf , 2 ) , 20 * pow( tf , 3 );

	B << xf - x0 - v0 * tf - a0 * pow( tf , 2 ) / 2,
		 vf - v0 - a0 * tf,
		 af - a0 ;

	Eigen::Matrix<double,3,1> C = A.inverse() * B;

	double N;

	N = tf / smp;
	int NN = round( N + 1 );

	Eigen::MatrixXd Time = Eigen::MatrixXd::Zero( NN , 1 );
	Eigen::MatrixXd Tra = Eigen::MatrixXd::Zero( NN , 1 );

	int i;

	for ( i = 1; i <= NN; i++ )
		Time.coeffRef( i - 1 , 0 ) = ( i - 1 ) * smp;

	for ( i = 1; i <= NN; i++ )
	{
		Tra.coeffRef(i-1,0) =
				x0 +
				v0 * Time.coeff( i - 1 ) +
				0.5 * a0 * pow( Time.coeff( i - 1 ) , 2 ) +
				C.coeff( 0 , 0 ) * pow( Time.coeff( i - 1 ) , 3 ) +
				C.coeff( 1 , 0 ) * pow( Time.coeff( i - 1 ) , 4 ) +
				C.coeff( 2 , 0 ) * pow( Time.coeff( i - 1 ) , 5 );
	}

	return Tra;
}

void GreenStrain_LIMSolver2D::computeHessian(const Eigen::Matrix<double,Eigen::Dynamic,1>& x, const Eigen::Matrix<double*,Eigen::Dynamic,1>& hess)
{
  // green strain tensor energy
  Eigen::Matrix<double,2,3> S;
  for(int t=0;t<mesh->Triangles->rows();t++)
  {
    Eigen::Vector2d A(x[TriangleVertexIdx.coeff(0,t)],x[TriangleVertexIdx.coeff(1,t)]);
    Eigen::Vector2d B(x[TriangleVertexIdx.coeff(2,t)],x[TriangleVertexIdx.coeff(3,t)]);
    Eigen::Vector2d C(x[TriangleVertexIdx.coeff(4,t)],x[TriangleVertexIdx.coeff(5,t)]);

    Eigen::Matrix<double,2,3> V;
    V.col(0) = A;
    V.col(1) = B;
    V.col(2) = C;

    // hessian(E) = 4*r_x'*((SMM'V'V+VMM'*(V'S+SV))*MM' - SMM')*c_x
    Eigen::Matrix3d VTV = V.transpose()*V;
    Eigen::Matrix3d MMT = MMTs.block<3,3>(0,3*t);
    Eigen::Matrix<double,2,3> VMMT = V*MMT;
    Eigen::Matrix3d MMTVTV = MMT*VTV;

    int numElem = 0;
    for(int r=0;r<6;r++)
    {
      S = Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>::Zero(2,3);
      S.coeffRef(r) = 1;

      Eigen::Matrix<double,2,3> Temp = 4*((S*MMTVTV + VMMT*(V.transpose()*S+S.transpose()*V))*MMT - S*MMT);
      
      for(int c=r;c<6;c++)
        *denseHessianCoeffs(numElem++,t) += Temp.coeff(c)*Divider;
    }
  }
}

void GreenStrain_LIMSolver3D::computeGradient(const Eigen::Matrix<double,Eigen::Dynamic,1>& x, Eigen::Matrix<double,Eigen::Dynamic,1>& grad)
{
  // green strain energy
  for(int t=0;t<mesh->Tetrahedra->rows();t++)
  {
    Eigen::Vector3d A(x[TetrahedronVertexIdx.coeff(0,t)],x[TetrahedronVertexIdx.coeff(1,t)],x[TetrahedronVertexIdx.coeff(2,t)]);
    Eigen::Vector3d B(x[TetrahedronVertexIdx.coeff(3,t)],x[TetrahedronVertexIdx.coeff(4,t)],x[TetrahedronVertexIdx.coeff(5,t)]);
    Eigen::Vector3d C(x[TetrahedronVertexIdx.coeff(6,t)],x[TetrahedronVertexIdx.coeff(7,t)],x[TetrahedronVertexIdx.coeff(8,t)]);
    Eigen::Vector3d D(x[TetrahedronVertexIdx.coeff(9,t)],x[TetrahedronVertexIdx.coeff(10,t)],x[TetrahedronVertexIdx.coeff(11,t)]);

    Eigen::Matrix<double,3,4> V;
    V.col(0) = A;
    V.col(1) = B;
    V.col(2) = C;
    V.col(3) = D;

    // jacobian(E) = 4(VMM'V'VMM' - VMM')
    Eigen::Matrix<double,3,4> VMMT = V*MMTs.block<4,4>(0,4*t);
    Eigen::Matrix<double,3,4> T = 4*(VMMT*V.transpose()*VMMT - VMMT);

    for(int i=0;i<12;i++)
      grad[TetrahedronVertexIdx.coeff(i,t)] += T.coeff(i)*Divider;
  }
}

template<typename PointT> void
pcl::search::OrganizedNeighbor<PointT>::estimateProjectionMatrix ()
{
  // internally we calculate with double but store the result into float matrices.
  typedef double Scalar;
  projection_matrix_.setZero ();
  if (input_->height == 1 || input_->width == 1)
  {
    PCL_ERROR ("[pcl::%s::estimateProjectionMatrix] Input dataset is not organized!\n", getName ().c_str ());
    return;
  }

  // we just want to use every 16th column and row -> skip = 2^4
  const unsigned int skip = input_->width >> 4;
  Eigen::Matrix<Scalar, 4, 4> A = Eigen::Matrix<Scalar, 4, 4>::Zero ();
  Eigen::Matrix<Scalar, 4, 4> B = Eigen::Matrix<Scalar, 4, 4>::Zero ();
  Eigen::Matrix<Scalar, 4, 4> C = Eigen::Matrix<Scalar, 4, 4>::Zero ();
  Eigen::Matrix<Scalar, 4, 4> D = Eigen::Matrix<Scalar, 4, 4>::Zero ();

  for (unsigned yIdx = 0, idx = 0; yIdx < input_->height; yIdx += skip, idx += input_->width * (skip-1))
  {
    for (unsigned xIdx = 0; xIdx < input_->width; xIdx += skip, idx += skip)
    {
      const PointT& point = input_->points[idx];
      if (isFinite (point))
      {
        Scalar xx = point.x * point.x;
        Scalar xy = point.x * point.y;
        Scalar xz = point.x * point.z;
        Scalar yy = point.y * point.y;
        Scalar yz = point.y * point.z;
        Scalar zz = point.z * point.z;
        Scalar xx_yy = xIdx * xIdx + yIdx * yIdx;

        A.coeffRef (0) += xx;
        A.coeffRef (1) += xy;
        A.coeffRef (2) += xz;
        A.coeffRef (3) += point.x;

        A.coeffRef (5) += yy;
        A.coeffRef (6) += yz;
        A.coeffRef (7) += point.y;

        A.coeffRef (10) += zz;
        A.coeffRef (11) += point.z;
        A.coeffRef (15) += 1.0;

        B.coeffRef (0) -= xx * xIdx;
        B.coeffRef (1) -= xy * xIdx;
        B.coeffRef (2) -= xz * xIdx;
        B.coeffRef (3) -= point.x * xIdx;

        B.coeffRef (5) -= yy * xIdx;
        B.coeffRef (6) -= yz * xIdx;
        B.coeffRef (7) -= point.y * xIdx;

        B.coeffRef (10) -= zz * xIdx;
        B.coeffRef (11) -= point.z * xIdx;

        B.coeffRef (15) -= xIdx;

        C.coeffRef (0) -= xx * yIdx;
        C.coeffRef (1) -= xy * yIdx;
        C.coeffRef (2) -= xz * yIdx;
        C.coeffRef (3) -= point.x * yIdx;

        C.coeffRef (5) -= yy * yIdx;
        C.coeffRef (6) -= yz * yIdx;
        C.coeffRef (7) -= point.y * yIdx;

        C.coeffRef (10) -= zz * yIdx;
        C.coeffRef (11) -= point.z * yIdx;

        C.coeffRef (15) -= yIdx;

        D.coeffRef (0) += xx * xx_yy;
        D.coeffRef (1) += xy * xx_yy;
        D.coeffRef (2) += xz * xx_yy;
        D.coeffRef (3) += point.x * xx_yy;

        D.coeffRef (5) += yy * xx_yy;
        D.coeffRef (6) += yz * xx_yy;
        D.coeffRef (7) += point.y * xx_yy;

        D.coeffRef (10) += zz * xx_yy;
        D.coeffRef (11) += point.z * xx_yy;

        D.coeffRef (15) += xx_yy;
      }
    }
  }

  makeSymmetric(A);
  makeSymmetric(B);
  makeSymmetric(C);
  makeSymmetric(D);

  Eigen::Matrix<Scalar, 12, 12> X = Eigen::Matrix<Scalar, 12, 12>::Zero ();
  X.topLeftCorner<4,4> () = A;
  X.block<4,4> (0, 8) = B;
//.........这里部分代码省略.........

template <typename PointT> double 
pcl::estimateProjectionMatrix (
    typename pcl::PointCloud<PointT>::ConstPtr cloud, 
    Eigen::Matrix<float, 3, 4, Eigen::RowMajor>& projection_matrix, 
    const std::vector<int>& indices)
{
  // internally we calculate with double but store the result into float matrices.
  typedef double Scalar;
  projection_matrix.setZero ();
  if (cloud->height == 1 || cloud->width == 1)
  {
    PCL_ERROR ("[pcl::estimateProjectionMatrix] Input dataset is not organized!\n");
    return (-1.0);
  }
  
  Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> A = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
  Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> B = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
  Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> C = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
  Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> D = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();

  pcl::ConstCloudIterator <PointT> pointIt (*cloud, indices);
  
  while (pointIt)
  {
    unsigned yIdx = pointIt.getCurrentPointIndex () / cloud->width;
    unsigned xIdx = pointIt.getCurrentPointIndex () % cloud->width;
    
    const PointT& point = *pointIt;
    if (pcl_isfinite (point.x))
    {
      Scalar xx = point.x * point.x;
      Scalar xy = point.x * point.y;
      Scalar xz = point.x * point.z;
      Scalar yy = point.y * point.y;
      Scalar yz = point.y * point.z;
      Scalar zz = point.z * point.z;
      Scalar xx_yy = xIdx * xIdx + yIdx * yIdx;

      A.coeffRef (0) += xx;
      A.coeffRef (1) += xy;
      A.coeffRef (2) += xz;
      A.coeffRef (3) += point.x;

      A.coeffRef (5) += yy;
      A.coeffRef (6) += yz;
      A.coeffRef (7) += point.y;

      A.coeffRef (10) += zz;
      A.coeffRef (11) += point.z;
      A.coeffRef (15) += 1.0;

      B.coeffRef (0) -= xx * xIdx;
      B.coeffRef (1) -= xy * xIdx;
      B.coeffRef (2) -= xz * xIdx;
      B.coeffRef (3) -= point.x * static_cast<double>(xIdx);

      B.coeffRef (5) -= yy * xIdx;
      B.coeffRef (6) -= yz * xIdx;
      B.coeffRef (7) -= point.y * static_cast<double>(xIdx);

      B.coeffRef (10) -= zz * xIdx;
      B.coeffRef (11) -= point.z * static_cast<double>(xIdx);

      B.coeffRef (15) -= xIdx;

      C.coeffRef (0) -= xx * yIdx;
      C.coeffRef (1) -= xy * yIdx;
      C.coeffRef (2) -= xz * yIdx;
      C.coeffRef (3) -= point.x * static_cast<double>(yIdx);

      C.coeffRef (5) -= yy * yIdx;
      C.coeffRef (6) -= yz * yIdx;
      C.coeffRef (7) -= point.y * static_cast<double>(yIdx);

      C.coeffRef (10) -= zz * yIdx;
      C.coeffRef (11) -= point.z * static_cast<double>(yIdx);

      C.coeffRef (15) -= yIdx;

      D.coeffRef (0) += xx * xx_yy;
      D.coeffRef (1) += xy * xx_yy;
      D.coeffRef (2) += xz * xx_yy;
      D.coeffRef (3) += point.x * xx_yy;

      D.coeffRef (5) += yy * xx_yy;
      D.coeffRef (6) += yz * xx_yy;
      D.coeffRef (7) += point.y * xx_yy;

      D.coeffRef (10) += zz * xx_yy;
      D.coeffRef (11) += point.z * xx_yy;

      D.coeffRef (15) += xx_yy;
    }
    
    ++pointIt;
  } // while  
  
  pcl::common::internal::makeSymmetric (A);
  pcl::common::internal::makeSymmetric (B);
  pcl::common::internal::makeSymmetric (C);
//.........这里部分代码省略.........