C++ MatrixXd::coeffRef方法代码示例

  inline void BasisSet::pointD(BasisSet *set, const Eigen::Vector3d &delta,
                               const double &dr2, int basis,
                               Eigen::MatrixXd &out)
  {
    // D type orbitals have six components and each component has a different
    // independent MO weighting. Many things can be cached to save time though
    double xx = 0.0, yy = 0.0, zz = 0.0, xy = 0.0, xz = 0.0, yz = 0.0;

    // Now iterate through the D type GTOs and sum their contributions
    unsigned int cIndex = set->m_cIndices[basis];
    for (unsigned int i = set->m_gtoIndices[basis];
         i < set->m_gtoIndices[basis+1]; ++i) {
      // Calculate the common factor
      double tmpGTO = exp(-set->m_gtoA[i] * dr2);
      xx += set->m_gtoCN[cIndex++] * tmpGTO; // Dxx
      yy += set->m_gtoCN[cIndex++] * tmpGTO; // Dyy
      zz += set->m_gtoCN[cIndex++] * tmpGTO; // Dzz
      xy += set->m_gtoCN[cIndex++] * tmpGTO; // Dxy
      xz += set->m_gtoCN[cIndex++] * tmpGTO; // Dxz
      yz += set->m_gtoCN[cIndex++] * tmpGTO; // Dyz
    }

    // Save values to the matrix
    int baseIndex = set->m_moIndices[basis];
    out.coeffRef(baseIndex  , 0) = delta.x() * delta.x() * xx;
    out.coeffRef(baseIndex+1, 0) = delta.y() * delta.y() * yy;
    out.coeffRef(baseIndex+2, 0) = delta.z() * delta.z() * zz;
    out.coeffRef(baseIndex+3, 0) = delta.x() * delta.y() * xy;
    out.coeffRef(baseIndex+4, 0) = delta.x() * delta.z() * xz;
    out.coeffRef(baseIndex+5, 0) = delta.y() * delta.z() * yz;
  }

void Scaler::calcMaxMin(const std::vector<Eigen::MatrixXd>& src, Eigen::MatrixXd& max, Eigen::MatrixXd& min)
{
  if(src.size() == 0)
  {
    throw nn::Exception("Scaler::calcMaxMin", "Vector size of src is zero.");
  }
  if(src[0].rows() == 0)
  {
    throw nn::Exception("Scaler::calcMaxMin", "src[0].rows() is zero.");
  }

  max = src[0];
  min = src[0];

  for(uint32_t i = 0; i < src.size(); ++i)
  {
    for(uint32_t j = 0; j < src[i].rows(); ++j)
    {
      if(max.coeff(j) < src[i].coeff(j))
        max.coeffRef(j) = src[i].coeff(j);
      if(min.coeff(j) > src[i].coeff(j))
        min.coeffRef(j) = src[i].coeff(j);
    }
  }
}

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 computeC_symmetric_full( Eigen::MatrixXd &W, Eigen::MatrixXd &C, const Eigen::MatrixXd &D, int n)
{
			#pragma omp parallel for schedule(dynamic)
			for (int i=0; i<n; ++i)
			{
				for (int j=0; j<n; ++j)
				{
					double s=0.0;
					s = 0.5*(W.col(j)+W.col(i)).sum(); // if the matrix is dense this option is incredibly FAST!
					C.coeffRef(i,j) += 2.0*(W.coeffRef(i,j)+s)/(D.coeffRef(i)+D.coeffRef(j));
				}
			}
}

void compute_C_asymmetric_full(Eigen::MatrixXd &W, Eigen::MatrixXd &C, const Eigen::MatrixXd &D, int n)
{
			#pragma omp parallel for schedule(dynamic)
			for (int i=0; i<n; ++i)
			{
				for (int j=0; j<n; ++j)
				{
					double s=0.0;
					s = 0.5*(W.row(j)+W.row(i)).sum();// row access is slower than columns access
					C.coeffRef(i,j) += 2.0*(W.coeffRef(i,j)+s)/(D.coeffRef(i)+D.coeffRef(j));
				}
			}
}

void JointControl::computeGeneralizedForce(Eigen::VectorXd& tau)
{
  tau = Eigen::VectorXd::Zero(mnp_->getDOF());

  Eigen::VectorXd error = qd_ - mnp_->q();
  Eigen::MatrixXd Kpv = param_->getKpJoint().block(0, 0, mnp_->getDOF(), mnp_->getDOF());

  for(uint32_t i = 0; i < Kpv.rows(); ++i)
  {
    Kpv.coeffRef(i, i) /= param_->getKvJoint().block(0, 0, mnp_->getDOF(), mnp_->getDOF()).coeff(i, i);
  }
  Eigen::VectorXd dqd = Kpv * error;

  //const double dq_max = 25.0;

  for(uint32_t i = 0; i < dqd.rows(); ++i)
  {
    if(dqd[i] < -param_->getDqMax())
    {
      dqd[i] = -param_->getDqMax();
    }
    else if(dqd[i] > param_->getDqMax())
    {
      dqd[i] = param_->getDqMax();
    }
  }

  Eigen::VectorXd tau_unit = -param_->getKvJoint().block(0, 0, mnp_->getDOF(), mnp_->getDOF()) * (mnp_->dq() - dqd);

  tau = tau_ = mnp_->getMassMatrix() * tau_unit;
}

void TrainingData::setOutput(uint32_t idx, std::vector<double>& output)
{
  if(idx >= output_.size())
  {
    std::stringstream msg;
    msg << "Failed to set training data. idx should be smaller than " << output_.size() << "." << std::endl
        << "        idx : " << idx;

    throw nn::Exception("TrainingData::setOutput", msg.str());
  }

  if(output.size() != output_[0].rows())
  {
    std::stringstream msg;
    msg << "Specified data's row number is different from pre existed data's row number." << std::endl
        << "        data row num         : " << output.size() << std::endl
        << "        existed data row num : " << output_[0].rows();

    throw nn::Exception("TrainingData::setOutput", msg.str());
  }

  Eigen::MatrixXd tmp = Eigen::MatrixXd::Zero(output.size(), 1);

  for(uint32_t i = 0; i < output.size(); ++i)
  {
    tmp.coeffRef(i) = output[i];
  }

  output_[idx] = tmp;
}

void Scaler::denormalize(Eigen::MatrixXd& src, const Eigen::MatrixXd& min, const Eigen::MatrixXd& range)
{
  src = (src - Eigen::MatrixXd::Constant(src.rows(), src.cols(), min_)) / range_;
  for(uint32_t i = 0; i < src.rows(); ++i)
  {
    src.coeffRef(i) *= range.coeff(i);
  }
  src = src + min;
}

void Scaler::normalize(Eigen::MatrixXd& src, const Eigen::MatrixXd& min, const Eigen::MatrixXd& range)
{
  src = src - min;
  for(unsigned int i = 0; i < src.rows(); ++i)
  {
    src.coeffRef(i) /= range.coeff(i);
  }
  src = src * range_ + Eigen::MatrixXd::Constant(src.rows(), src.cols(), min_);
}

 inline void BasisSet::pointS(BasisSet *set, const double &dr2, int basis,
                              Eigen::MatrixXd &out)
 {
   // S type orbitals - the simplest of the calculations with one component
   double tmp = 0.0;
   unsigned int cIndex = set->m_cIndices[basis];
   for (unsigned int i = set->m_gtoIndices[basis];
        i < set->m_gtoIndices[basis+1]; ++i) {
     tmp += set->m_gtoCN[cIndex++] * exp(-set->m_gtoA[i] * dr2);
   }
   out.coeffRef(set->m_moIndices[basis], 0) = tmp;
 }

void compute_C_asymmetric_sparse(Eigen::MatrixXd &W, Eigen::MatrixXd &C, const Eigen::MatrixXd &D, int n)
{
			#pragma omp parallel for schedule(dynamic)
			for (int i=0; i<n; ++i)
			{
				for (int j=0; j<n; ++j)
				{
					double s=0.0;
					for (int k=0; k<n; ++k)
					{
						// if the matrix is symmetric this option is much faster, access k 
						// as rows instead as columns
						double Wjk = W.coeffRef(k,j);	
						int delta_jk = (Wjk!=0);
						if (delta_jk==0)
							continue;
						double Wik = W.coeffRef(k,i);
						int delta_ik = (Wik!=0);
						if (delta_ik==0)
							continue;
						s+= (delta_ik*delta_jk*(Wik+Wjk)*0.5);
					}
					C.coeffRef(i,j) += 2.0*(W.coeffRef(i,j)+s)/(D.coeffRef(i)+D.coeffRef(j));
				}
			}
}

  inline void BasisSet::pointP(BasisSet *set, const Vector3d &delta,
                               const double &dr2, int basis,
                               Eigen::MatrixXd &out)
  {
    double x = 0.0, y = 0.0, z = 0.0;

    // Now iterate through the P type GTOs and sum their contributions
    unsigned int cIndex = set->m_cIndices[basis];
    for (unsigned int i = set->m_gtoIndices[basis];
         i < set->m_gtoIndices[basis+1]; ++i) {
      double tmpGTO = exp(-set->m_gtoA[i] * dr2);
      x += set->m_gtoCN[cIndex++] * tmpGTO;
      y += set->m_gtoCN[cIndex++] * tmpGTO;
      z += set->m_gtoCN[cIndex++] * tmpGTO;
    }

    // Save values to the matrix
    int baseIndex = set->m_moIndices[basis];
    out.coeffRef(baseIndex  , 0) = x * delta.x();
    out.coeffRef(baseIndex+1, 0) = y * delta.y();
    out.coeffRef(baseIndex+2, 0) = z * delta.z();
  }

  inline void BasisSet::pointD5(BasisSet *set, const Eigen::Vector3d &delta,
                                const double &dr2, int basis,
                                Eigen::MatrixXd &out)
  {
    // D type orbitals have six components and each component has a different
    // independent MO weighting. Many things can be cached to save time though
    double d0 = 0.0, d1p = 0.0, d1n = 0.0, d2p = 0.0, d2n = 0.0;

    // Now iterate through the D type GTOs and sum their contributions
    unsigned int cIndex = set->m_cIndices[basis];
    for (unsigned int i = set->m_gtoIndices[basis];
         i < set->m_gtoIndices[basis+1]; ++i) {
      // Calculate the common factor
      double tmpGTO = exp(-set->m_gtoA[i] * dr2);
      d0  += set->m_gtoCN[cIndex++] * tmpGTO;
      d1p += set->m_gtoCN[cIndex++] * tmpGTO;
      d1n += set->m_gtoCN[cIndex++] * tmpGTO;
      d2p += set->m_gtoCN[cIndex++] * tmpGTO;
      d2n += set->m_gtoCN[cIndex++] * tmpGTO;
    }

    // Calculate the prefactors
    double xx = delta.x() * delta.x();
    double yy = delta.y() * delta.y();
    double zz = delta.z() * delta.z();
    double xy = delta.x() * delta.y();
    double xz = delta.x() * delta.z();
    double yz = delta.y() * delta.z();

    // Save values to the matrix
    int baseIndex = set->m_moIndices[basis];
    out.coeffRef(baseIndex  , 0) = (zz - dr2) * d0;
    out.coeffRef(baseIndex+1, 0) = xz * d1p;
    out.coeffRef(baseIndex+2, 0) = yz * d1n;
    out.coeffRef(baseIndex+3, 0) = (xx - yy) * d2p;
    out.coeffRef(baseIndex+4, 0) = xy * d2n;
  }

void Scaler::normalize(Eigen::MatrixXd& src, const Eigen::MatrixXd& min, const Eigen::MatrixXd& range)
{
  src = src - min;
  for(uint32_t i = 0; i < src.rows(); ++i)
  {
    if(range.coeff(i) == 0.0)
    {
      std::stringstream msg;
      msg << "range.coeff(" << i << ") is zero." << std::endl
          << "This means " << i << "-th data is always constant and shouldn't be included as a training data." << std::endl
          << "If you'd like to include the data somehow, it is recommended that you use neural network without using scaler.";
      throw nn::Exception("Scaler::normalize", msg.str());
    }

    src.coeffRef(i) /= range.coeff(i);
  }
  src = src * range_ + Eigen::MatrixXd::Constant(src.rows(), src.cols(), min_);
}

bool IOUtils::getValues(std::ifstream& ifs, Eigen::MatrixXd& dst)
{
  if(!ifs)
    return false;

  std::string str;
  if(!std::getline(ifs, str))
    return false;

  std::vector<std::string> words;
  StrUtils::separate(str, words, ",;: \t");

  dst = Eigen::MatrixXd::Zero(words.size(), 1);
  for(unsigned int j = 0; j < words.size(); ++j)
  {
    StrUtils::convertToNum(words[j], dst.coeffRef(j, 0));
  }

  return true;
}