C++ VectorXd::rows方法代码示例

		Eigen::VectorXd LDAModel::predict(const vector<double> & substance, bool transform)
		{
			if (sigma_.cols() == 0)
			{
				throw Exception::InconsistentUsage(__FILE__, __LINE__, "Model must be trained before it can predict the activitiy of substances!"); 
			}
			// search class to which the given substance has the smallest distance
			Eigen::VectorXd s = getSubstanceVector(substance, transform); // dim: 1xm
			
			int min_k = 0;
			double min_dist = 99999999;
			Eigen::VectorXd result(mean_vectors_.size());
			
			for (unsigned int c = 0; c < mean_vectors_.size(); c++)
			{
				for (int k = 0; k < mean_vectors_[c].rows(); k++)
				{
					Eigen::VectorXd diff(s.rows()); 
					for (unsigned int i = 0; i < s.rows(); i++)
					{
						diff(i) = s(i)-mean_vectors_[c](k, i); 
					}
					double dist = diff.transpose()*sigma_*diff;
					if (dist < min_dist)
					{
						min_dist = dist;
						min_k = k;
					}
				}
				result(c) = labels_[min_k];
			}
			
			return result;
			
		}

void Manipulator::update(const Eigen::VectorXd& q_msr, const Eigen::VectorXd& dq_msr)
{
  if(q_msr.rows() != dof_)
  {
    std::stringstream msg;
    msg << "q_.rows() != dof" << std::endl
        << "  q_.rows   : " << q_msr.rows() << std::endl
        << "  dof : " << dof_;
    throw ahl_utils::Exception("Manipulator::update", msg.str());
  }

  if(dq_msr.rows() != dof_)
  {
    std::stringstream msg;
    msg << "dq_.rows() != dof" << std::endl
        << "  dq_.rows   : " << dq_msr.rows() << std::endl
        << "  dof       : " << dof_;
    throw ahl_utils::Exception("Manipulator::update", msg.str());
  }

  q_  = q_msr;
  dq_ = dq_msr;
  this->computeForwardKinematics();
  updated_joint_ = true;
}

FitSurface::Domain::Domain(const Eigen::VectorXd& param0, const Eigen::VectorXd& param1)
{
  if(param0.rows()!=param1.rows())
    throw std::runtime_error("[FitSurface::Domain::Domain] Error, param vectors must be of same length.");

  double x_min(DBL_MAX), x_max(DBL_MIN);
  double y_min(DBL_MAX), y_max(DBL_MIN);

  for(Eigen::MatrixXd::Index i=0; i<param0.rows(); i++)
  {
    const double& p0 = param0(i);
    const double& p1 = param1(i);

    if(p0<x_min)
      x_min=p0;
    if(p0>x_max)
      x_max=p0;

    if(p1<y_min)
      y_min=p1;
    if(p1>y_max)
      y_max=p1;
  }

  x = x_min;
  y = y_min;
  width = x_max-x_min;
  height = y_max-y_min;
}

gaussian_process::gaussian_process( const Eigen::MatrixXd& domains
                                , const Eigen::VectorXd& targets
                                , const gaussian_process::covariance& covariance
                                , double self_covariance )
    : domains_( domains )
    , targets_( targets )
    , covariance_( covariance )
    , self_covariance_( self_covariance )
    , offset_( targets.sum() / targets.rows() )
    , K_( domains.rows(), domains.rows() )
{
    if( domains.rows() != targets.rows() ) { COMMA_THROW( comma::exception, "expected " << domains.rows() << " row(s) in targets, got " << targets.rows() << " row(s)" ); }
    targets_.array() -= offset_; // normalise
    //use m_K as Kxx + variance*I, then invert it
    //fill Kxx with values from covariance function
    //for elements r,c in upper triangle
    for( std::size_t r = 0; r < std::size_t( domains.rows() ); ++r )
    {
        K_( r, r ) = self_covariance_;
        const Eigen::VectorXd& row = domains.row( r );
        for( std::size_t c = r + 1; c < std::size_t( domains.rows() ); ++c )
        {
            K_( c, r ) = K_( r, c ) = covariance_( row, domains.row( c ) );
        }
    }
    L_.compute( K_ ); // invert Kxx + variance * I to become (by definition) B
    alpha_ = L_.solve( targets_ );
}

  int TDynMinimalJerk::init(const std::vector<std::string>& parameters,
                                RobotStatePtr robot_state,
                                const Eigen::VectorXd& e_initial,
                                const Eigen::VectorXd& e_final) {
    int size = parameters.size();
    if (size != 3) {
      printHiqpWarning("TDynMinimalJerk requires 3 parameters, got " 
        + std::to_string(size) + "! Initialization failed!");
      return -1;
    }

    performance_measures_.resize(e_initial.rows());
    e_dot_star_.resize(e_initial.rows());

    time_start_ = robot_state->sampling_time_point_;

    total_duration_ = std::stod( parameters.at(1) );
    gain_ = std::stod( parameters.at(2) );

    f_ = 30 / total_duration_;

    e_initial_ = e_initial;
    e_final_ = e_final;
    e_diff_ = e_final - e_initial;

    return 0;
  }

void lpengine::ChooseLeavingVar(Eigen::VectorXd& delta_var,
                                Eigen::VectorXd& var_hat,
                                int* return_index,
                                double* return_step_length) {
  Eigen::VectorXd tmp(var_hat.rows());
  // choose smallest index (Bland's rule)
  for (int ii = 0; ii < var_hat.rows(); ii++) {
    if (var_hat[ii] == 0) {
      tmp[ii] = 0;
      *return_index = ii;
      return_step_length = 0;
      return;
    } else {
      tmp[ii] = delta_var[ii] / var_hat[ii];
    }
  }
  std::ptrdiff_t max_index;
  tmp.maxCoeff(&max_index);
  *return_index = max_index;

  // calculate primal step size
  if (tmp[max_index] == 0) {
    *return_step_length = 0;
  } else {
    *return_step_length = 1 / tmp[max_index];
  }
}

ColMat<double, 3> FaceUnwrapper::interpolateFlatFace(const TopoDS_Face& face)
{
    if (this->uv_nodes.size() == 0)
        throw(std::runtime_error("no uv-coordinates found, interpolating with nurbs is only possible if the Flattener was constructed with a nurbs."));
    
    // extract xyz poles, knots, weights, degree
    const Handle(Geom_Surface) &_surface = BRep_Tool::Surface(face);
    const Handle(Geom_BSplineSurface) &_bspline = Handle(Geom_BSplineSurface)::DownCast(_surface);
#if OCC_VERSION_HEX < 0x070000
    TColStd_Array1OfReal _uknots(1, _bspline->NbUPoles() + _bspline->UDegree() + 1);
    TColStd_Array1OfReal _vknots(1, _bspline->NbVPoles() + _bspline->VDegree() + 1);
    _bspline->UKnotSequence(_uknots);
    _bspline->VKnotSequence(_vknots);
#else
    const TColStd_Array1OfReal &_uknots = _bspline->UKnotSequence();
    const TColStd_Array1OfReal &_vknots = _bspline->VKnotSequence();
#endif

    Eigen::VectorXd weights;
    weights.resize(_bspline->NbUPoles() * _bspline->NbVPoles());
    long i = 0;
    for (long u=1; u <= _bspline->NbUPoles(); u++)
    {
        for (long v=1; v <= _bspline->NbVPoles(); v++)
        {
            weights[i] = _bspline->Weight(u, v);
            i++;
        }
    }

    Eigen::VectorXd u_knots;
    Eigen::VectorXd v_knots;
    u_knots.resize(_uknots.Length());
    v_knots.resize(_vknots.Length());
    for (long u=1; u <= _uknots.Length(); u++)
    {
        u_knots[u - 1] = _uknots.Value(u);
    }
    for (long v=1; v <= _vknots.Length(); v++)
    {
        v_knots[v - 1] = _vknots.Value(v);
    }
    

    nu = nurbs::NurbsBase2D(u_knots, v_knots, weights, _bspline->UDegree(), _bspline->VDegree());
    A = nu.getInfluenceMatrix(this->uv_nodes);
    
    Eigen::LeastSquaresConjugateGradient<spMat > solver;
    solver.compute(A);
    ColMat<double, 2> ze_poles;
    ColMat<double, 3> flat_poles;
    ze_poles.resize(weights.rows(), 2);
    flat_poles.resize(weights.rows(), 3);
    flat_poles.setZero();
    ze_poles = solver.solve(ze_nodes);
    flat_poles.col(0) << ze_poles.col(0);
    flat_poles.col(1) << ze_poles.col(1);
    return flat_poles;    
}

	Eigen::MatrixXd Jacobian(double t, Eigen::VectorXd y){
		Eigen::MatrixXd J(y.rows(),y.rows());
		J(0,0)	=	0.0;
		J(0,1)	=	1.0;
		J(1,0)	=	-2000.0*y(0)*y(1)-y(0);
		J(1,1)	=	1000.0*(1-y(0)*y(0));
		return J;
	};

void FitOpenCurve::initSolver(const Eigen::VectorXd& params)
{
    if(m_nurbs.CVCount() <= 0)
        throw std::runtime_error("[FitOpenCurve::initSolver] Error, curve not initialized (initCurve).");

    if(params.rows()<2)
        throw std::runtime_error("[FitOpenCurve::initSolver] Error, insufficient parameter points (<2).");

    int dim = m_nurbs.Dimension();
    m_A = SparseMatrix( params.rows()*dim, m_nurbs.CVCount()*dim );

    typedef Eigen::Triplet<double> Tri;
    std::vector<Tri> tripletList;
    tripletList.resize( dim * params.rows() * m_nurbs.Order() );

    if(!m_quiet)
        printf("[FitOpenCurve::initSolver] entries: %lu  rows: %lu  cols: %d\n",
               dim * params.rows()* m_nurbs.Order(),
               dim * params.rows(),
               m_nurbs.CVCount());

    double *N = new double[m_nurbs.m_order * m_nurbs.m_order];
    int E;
    int ti(0);

    for(Eigen::MatrixXd::Index row=0; row<params.rows(); row++)
    {
        E = ON_NurbsSpanIndex (m_nurbs.m_order, m_nurbs.m_cv_count, m_nurbs.m_knot, params(row), 0, 0);

        ON_EvaluateNurbsBasis (m_nurbs.m_order, m_nurbs.m_knot + E, params(row), N);

        for (int i = 0; i < m_nurbs.Order(); i++)
        {
            tripletList[ti] = Tri( dim*row, dim*(E+i), N[i] );
            if(dim>1)
                tripletList[ti+1] = Tri( dim*row+1, dim*(E+i)+1, N[i] );
            if(dim>2)
                tripletList[ti+2] = Tri( dim*row+2, dim*(E+i)+2, N[i] );

            ti+=dim;
        } // i

    } // row

    delete [] N;

    m_A.setFromTriplets(tripletList.begin(), tripletList.end());

    if(m_solver==NULL)
        m_solver = new SPQR();
    m_solver->compute(m_A);
    if(m_solver->info()!=Eigen::Success)
        throw std::runtime_error("[FitOpenCurve::initSolver] decomposition failed.");

    if(!m_quiet)
        printf("[FitOpenCurve::initSolver] decomposition done\n");
}

void utils_eigen::print_single_line(const Eigen::VectorXd &vec, int indents)
{
  for(int k = 0; k<indents; k++)
    std::cout << "\t";
  
  std::cout << "(";
  for(int i = 0; i<vec.rows()-1; i++)
    std::cout << vec(i,0) << ", ";
  std::cout << vec(vec.rows()-1,0) << ")" << std::endl;
}

void ExperimentalTrajectory::addNewWaypoint(Eigen::VectorXd newWaypoint, double waypointTime)
{
    std::cout << "Adding waypoint at: " << waypointTime << " seconds..." << std::endl;

    if ( (newWaypoint.rows() == nDoF) && (waypointTime>=0.0) && (waypointTime<=kernelCenters.maxCoeff()) )
    {
        //Find out where the new T falls...
        for (int i=0; i<kernelCenters.size(); i++)
        {
            std::cout << "i = " << i << std::endl;
            if (kernelCenters(i)>waypointTime) {
                youngestWaypointIndex=i;
                std::cout << "youngestWaypointIndex" << youngestWaypointIndex << std::endl;
                i = kernelCenters.size();
            }
        }


        Eigen::MatrixXd newWaypointsMat = Eigen::MatrixXd::Zero(nDoF, nWaypoints+1);
        newWaypointsMat.leftCols(youngestWaypointIndex) = waypoints.leftCols(youngestWaypointIndex);
        newWaypointsMat.col(youngestWaypointIndex) = newWaypoint;
        newWaypointsMat.rightCols(nWaypoints - youngestWaypointIndex) = waypoints.rightCols(nWaypoints - youngestWaypointIndex);

        waypoints.resize(nDoF, nWaypoints+1);
        waypoints = newWaypointsMat;

        Eigen::VectorXd durationVectorTmp = Eigen::VectorXd::Zero(nWaypoints);

        std::cout << "\npointToPointDurationVector\n " << pointToPointDurationVector << std::endl;

        durationVectorTmp.head(youngestWaypointIndex-1) = pointToPointDurationVector.head(youngestWaypointIndex-1);
        durationVectorTmp.row(youngestWaypointIndex-1) << waypointTime - kernelCenters(youngestWaypointIndex-1);
        durationVectorTmp.tail(nWaypoints - youngestWaypointIndex) = pointToPointDurationVector.tail(nWaypoints - youngestWaypointIndex);

        pointToPointDurationVector.resize(durationVectorTmp.size());
        pointToPointDurationVector = durationVectorTmp;

        std::cout << "\npointToPointDurationVector\n " << pointToPointDurationVector << std::endl;


        reinitialize();


    }
    else{
        if (newWaypoint.rows() != nDoF){
            std::cout << "[ERROR] (ExperimentalTrajectory::addNewWaypoint): New waypoint is not the right size. Should have dim = " <<nDoF << "x1." << std::endl;
        }
        if ((waypointTime<=0.0) || (waypointTime>=kernelCenters.maxCoeff())){
            std::cout << "[ERROR] (ExperimentalTrajectory::addNewWaypoint): New waypoint time is out of time bounds. Should have fall between 0.0 and " << kernelCenters.maxCoeff() << " seconds." << std::endl;
        }
    }


}

sva::RBInertiad sVectorToInertia(const Eigen::VectorXd& vec)
{
	if(vec.rows() != 10)
	{
		std::ostringstream str;
		str << "Vector size mismatch: expected size is 10 gived is "
				<< vec.rows();
		throw std::out_of_range(str.str());
	}
	return vectorToInertia(vec);
}

  EReturn AICOProblem::initDerived(tinyxml2::XMLHandle & handle)
  {
    EReturn ret_value = SUCCESS;
    tinyxml2::XMLElement* xmltmp;
    bool hastime = false;
    XML_CHECK("T");
    XML_OK(getInt(*xmltmp, T));
    if (T <= 2)
    {
      INDICATE_FAILURE
      ;
      return PAR_ERR;
    }
    xmltmp = handle.FirstChildElement("duration").ToElement();
    if (xmltmp)
    {
      XML_OK(getDouble(*xmltmp, tau));
      tau = tau / ((double) T);
      hastime = true;
    }
    if (hastime)
    {
      xmltmp = handle.FirstChildElement("tau").ToElement();
      if (xmltmp)
        WARNING("Duration has already been specified, tau is ignored.");
    }
    else
    {
      XML_CHECK("tau");
      XML_OK(getDouble(*xmltmp, tau));
    }

    XML_CHECK("Qrate");
    XML_OK(getDouble(*xmltmp, Q_rate));
    XML_CHECK("Hrate");
    XML_OK(getDouble(*xmltmp, H_rate));
    XML_CHECK("Wrate");
    XML_OK(getDouble(*xmltmp, W_rate));
    {
      Eigen::VectorXd tmp;
      XML_CHECK("W");
      XML_OK(getVector(*xmltmp, tmp));
      W = Eigen::MatrixXd::Identity(tmp.rows(), tmp.rows());
      W.diagonal() = tmp;
    }
    for (TaskDefinition_map::const_iterator it = task_defs_.begin();
        it != task_defs_.end() and ok(ret_value); ++it)
    {
      if (it->second->type().compare(std::string("TaskSqrError")) == 0)
        ERROR("Task variable " << it->first << " is not an squared error!");
    }
    // Set number of time steps
    return setTime(T);
  }

void Manipulator::init(uint32_t init_dof, const Eigen::VectorXd& init_q)
{
  if(init_dof != init_q.rows())
  {
    std::stringstream msg;
    msg << "dof != init_q.rows()" << std::endl
        << "  dof           : " << init_dof << std::endl
        << "  init_q.rows() : " << init_q.rows();
    throw ahl_utils::Exception("Manipulator::init", msg.str());
  }

  dof_ = init_dof;

  // Resize vectors and matrices
  q_  = Eigen::VectorXd::Zero(dof_);
  dq_ = Eigen::VectorXd::Zero(dof_);

  T_.resize(link_.size());
  for(uint32_t i = 0; i < T_.size(); ++i)
  {
    T_[i] = link_[i]->T_org;
  }

  T_abs_.resize(dof_ + 1);
  for(uint32_t i = 0; i < T_abs_.size(); ++i)
  {
    T_abs_[i] = Eigen::Matrix4d::Identity();
  }
  C_abs_.resize(dof_ + 1);
  for(uint32_t i = 0; i < C_abs_.size(); ++i)
  {
    C_abs_[i] = Eigen::Matrix4d::Identity();
  }
  Pin_.resize(dof_ + 1);
  for(uint32_t i = 0; i < Pin_.size(); ++i)
  {
    Pin_[i] = Eigen::Vector3d::Zero();
  }

  q_ = init_q;

  differentiator_ = std::make_shared<ahl_filter::PseudoDifferentiator>(update_rate_, cutoff_frequency_);
  differentiator_->init(q_, dq_);
  this->computeForwardKinematics();

  J_.resize(link_.size());
  M_.resize(dof_, dof_);
  M_inv_.resize(dof_, dof_);

  for(uint32_t i = 0; i < link_.size(); ++i)
  {
    name_to_idx_[link_[i]->name] = i;
  }
}

 void OMPLRNStateSpace::OMPLToExoticaState(const ompl::base::State *state,
     Eigen::VectorXd &q) const
 {
   if (!state)
   {
     throw_pretty("Invalid state!");
   }
   if (q.rows() != (int) getDimension()) q.resize((int) getDimension());
   memcpy(q.data(),
       state->as<OMPLRNStateSpace::StateType>()->getRNSpace().values,
       sizeof(double) * q.rows());
 }