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

std::string YAMLUtils::getVectorStr(const std::string& tag, const Eigen::MatrixXd& m)
{
  std::stringstream msg;

  msg << tag << ": [";

  if(m.rows() == 0)
  {
    msg << "]" << std::endl;
    return msg.str();
  }

  if(m.cols() > 1)
  {
    std::stringstream msg;
    msg << "Eigen::MatrixXd::cols returned value which is bigger than 1." << std::endl
        << "YAMLUtils::VectorStr assumes a vertical vector.";
    throw ahl_utils::Exception("YAMLUtils::getVectorStr", msg.str());
  }

  msg << m.coeff(0, 0);
  for(unsigned int i = 1; i < m.rows(); ++i)
  {
    msg << ", " << m.coeff(i, 0);
  }
  msg << "]" << std::endl;

  return msg.str();
}

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

Eigen::MatrixXd MainWindow::rotation2rpy( Eigen::MatrixXd rotation )
{
  Eigen::MatrixXd _rpy = Eigen::MatrixXd::Zero( 3 , 1 );

  _rpy.coeffRef( 0 , 0 ) = atan2( rotation.coeff( 2 , 1 ), rotation.coeff( 2 , 2 ) );
  _rpy.coeffRef( 1 , 0 ) = atan2( -rotation.coeff( 2 , 0 ), sqrt( pow( rotation.coeff( 2 , 1 ) , 2 ) + pow( rotation.coeff( 2 , 2 ) , 2 ) ) );
  _rpy.coeffRef( 2 , 0 ) = atan2 ( rotation.coeff( 1 , 0 ) , rotation.coeff( 0 , 0 ) );

  return _rpy;
}

void MainWindow::updateCurrKinematicsPoseSpinbox( manipulator_h_base_module_msgs::KinematicsPose msg )
{
  ui.pos_x_spinbox->setValue( msg.pose.position.x );
  ui.pos_y_spinbox->setValue( msg.pose.position.y );
  ui.pos_z_spinbox->setValue( msg.pose.position.z );

  Eigen::Quaterniond QR( msg.pose.orientation.w , msg.pose.orientation.x , msg.pose.orientation.y , msg.pose.orientation.z );

  Eigen::MatrixXd rpy = quaternion2rpy( QR );

  double roll = rpy.coeff( 0 , 0 ) * 180.0 / M_PI;
  double pitch = rpy.coeff( 1 , 0 ) * 180.0 / M_PI;
  double yaw = rpy.coeff( 2, 0 ) * 180.0 /M_PI;

  ui.ori_roll_spinbox->setValue( roll );
  ui.ori_pitch_spinbox->setValue( pitch );
  ui.ori_yaw_spinbox->setValue( yaw );
}

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

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

  void prettyPrint (Eigen::MatrixXd const & mm, std::ostream & os,
		    std::string const & title, std::string const & prefix,
		    bool vecmode, bool nonl)
  {
    char const * nlornot ("\n");
    if (nonl) {
      nlornot = "";
    }
    if ( ! title.empty()) {
      os << title << nlornot;
    }
    if ((mm.rows() <= 0) || (mm.cols() <= 0)) {
      os << prefix << " (empty)" << nlornot;
    }
    else {
      
      if (vecmode) {
	if ( ! prefix.empty()) {
	  os << prefix;
	}
	for (int ir (0); ir < mm.rows(); ++ir) {
	  prettyPrint (mm.coeff (ir, 0), os);
	}
	os << nlornot;
	
      }
      else {

	for (int ir (0); ir < mm.rows(); ++ir) {
	  if ( ! prefix.empty()) {
	    os << prefix;
	  }
	  for (int ic (0); ic < mm.cols(); ++ic) {
	    prettyPrint (mm.coeff (ir, ic), os);
	  }
	  os << nlornot;
	}
	
      }
    }
  }

void compute_vertex_area_fast(Eigen::MatrixXd& V, Eigen::MatrixXd& F, double* va)
{
	int n1,n2,n3;
	Eigen::Vector3d v1,v2,v3;
	Eigen::Vector3d e1,e2;
	Eigen::Vector3d tmpV;
	double tmp;
	double farea;
	for( int i = 0; i < F.rows(); ++i)
	{
		n1 = F.coeff(i,0)-1; n2 = F.coeff(i,1)-1; n3 = F.coeff(i,2)-1;
		v1 = V.row(n1); v2 = V.row(n2); v3 = V.row(n3);
		e1 = v2 - v1; e2 = v3 - v1;

		tmpV = e1.cross(e2);
		farea = tmpV.norm()*0.5;
		tmp = farea / 3.0;
		
		va[n1] += tmp;
		va[n2] += tmp;
		va[n3] += tmp;
	}
}

void ATIForceTorqueSensorTWE::setCurrentVoltageOutput(Eigen::MatrixXd voltage)
{
	if((voltage.rows() != 6) || (voltage.cols() != 1)) {
		ROS_ERROR("Invalid voltage size");
		return;
	}

	setCurrentVoltageOutput(voltage.coeff(0,0), voltage.coeff(1,0), voltage.coeff(2,0),
							voltage.coeff(3,0), voltage.coeff(4,0), voltage.coeff(5,0));
}

void IOUtils::print(const Eigen::MatrixXd& m)
{
  std::cout << "[ ";
  for(unsigned int i = 0; i < m.rows(); ++i)
  {
    for(unsigned int j = 0; j < m.cols(); ++j)
    {
      std::cout << m.coeff(i, j);
      if(j < m.cols() - 1)
        std::cout << ", ";
    }
    if(i < m.rows() - 1)
      std::cout << std::endl;
  }
  std::cout << " ]" << std::endl;
}

Eigen::MatrixXd Tra_vian_q( int n ,
		                    double x0 , double v0 , double a0 ,
		                    Eigen::MatrixXd x ,
		                    double xf , double vf , double af ,
		                    double smp , Eigen::MatrixXd t , double tf)
/*
   minimum jerk trajectory with via-points
   (via-point constraints: position at each point)

   n  : the number of via-points

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

   x  : position matrix at via-points state ( size : n x 1 )

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

   smp : sampling time

   t  : time matrix passing through via-points state ( size : n x 1 )
   tf : movement time
*/

{
	int i , j , k ;

	/* Calculation Matrix B	*/

	Eigen::MatrixXd B = Eigen::MatrixXd::Zero( n + 3 , 1 );

	for ( i = 1; i <= n; i++ )
	{
		B.coeffRef( i - 1 , 0 ) =
				x.coeff( i - 1 , 0 ) -
				x0 -
				v0 * t.coeff( i - 1 , 0 ) -
				( a0 / 2 ) * pow( t.coeff( i - 1 , 0 ) , 2 ) ;
	}

	B.coeffRef( n , 0 ) =
			xf -
			x0 -
			v0 * tf -
			( a0 / 2 ) * pow( tf , 2 ) ;

	B.coeffRef( n + 1 , 0 ) =
			vf -
			v0 -
			a0 * tf ;

	B.coeffRef( n + 2 , 0 ) =
			af -
			a0 ;

	/* Calculation Matrix A	*/

	Eigen::MatrixXd A1 = Eigen::MatrixXd::Zero( n , 3 );

	for ( i = 1; i <= n; i++ )
	{
		A1.coeffRef( i - 1 , 0 ) = pow( t.coeff( i - 1 , 0 ) , 3 );
		A1.coeffRef( i - 1 , 1 ) = pow( t.coeff( i - 1 , 0 ) , 4 );
		A1.coeffRef( i - 1 , 2 ) = pow( t.coeff( i - 1 , 0 ) , 5 );
	}

	Eigen::MatrixXd A2 = Eigen::MatrixXd::Zero( n , n );

	for ( i = 1; i <= n; i++ )
	{
		for ( j = 1; j <= n; j++ )
		{
			if ( i > j )
				k = i;
			else
				k = j;

			A2.coeffRef( j - 1 , i - 1 ) =
					pow( ( t.coeff( k - 1 , 0 ) - t.coeff( i - 1 , 0 ) ) , 5 ) / 120;
		}
	}

	Eigen::MatrixXd A3 = Eigen::MatrixXd::Zero( 3 , n + 3 );

	A3.coeffRef( 0 , 0 ) = pow( tf , 3 );
	A3.coeffRef( 0 , 1 ) = pow( tf , 4 );
	A3.coeffRef( 0 , 2 ) = pow( tf , 5 );

	A3.coeffRef( 1 , 0 ) = 3 * pow( tf , 2 );
	A3.coeffRef( 1 , 1 ) = 4 * pow( tf , 3 );
	A3.coeffRef( 1 , 2 ) = 5 * pow( tf , 4 );

	A3.coeffRef( 2 , 0 ) = 6 * tf;
	A3.coeffRef( 2 , 1 ) = 12 * pow( tf , 2 );
	A3.coeffRef( 2 , 2 ) = 20 * pow( tf , 3 );

	for ( i = 1; i <= n; i++ )
//.........这里部分代码省略.........