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

 /** Query the function approximator to make a prediction, and also to predict its variance
  *  \param[in]  inputs   Input values of the query (n_samples X n_dims_in)
  *  \param[out] outputs  Predicted output values (n_samples X n_dims_out)
  *  \param[out] variances Predicted variances for the output values  (n_samples X n_dims_out). Note that if the output has a dimensionality>1, these variances should actuall be covariance matrices (use function predict(const Eigen::MatrixXd& inputs, Eigen::MatrixXd& outputs, std::vector<Eigen::MatrixXd>& variances) to get the full covariance matrices). So for an output dimensionality of 1 this function works fine. For dimensionality>1 we return only the diagional of the covariance matrix, which may not always be what you want.
  *
  * \remark This method should be const. But third party functions which is called in this function
  * have not always been implemented as const (Examples: LWPRObject::predict or IRFRLS::predict ).
  * Therefore, this function cannot be const.
  */
 virtual void predict(const Eigen::MatrixXd& inputs, Eigen::MatrixXd& outputs, Eigen::MatrixXd& variances)
 {
   predict(inputs, outputs);
   variances.fill(0);
 }

 /** Query the function approximator to get the variance of a prediction
  * This function is not implemented by all function approximators. Therefore, the default
  * implementation fills outputs with 0s.
  *  \param[in]  inputs   Input values of the query (n_samples X n_dims_in)
  *  \param[out] variances Predicted variances for the output values  (n_samples X n_dims_out). Note that if the output has a dimensionality>1, these variances should actuall be covariance matrices (use function predict(const Eigen::MatrixXd& inputs, Eigen::MatrixXd& outputs, std::vector<Eigen::MatrixXd>& variances) to get the full covariance matrices). So for an output dimensionality of 1 this function works fine. For dimensionality>1 we return only the diagional of the covariance matrix, which may not always be what you want.
  *
  * \remark This method should be const. But third party functions which is called in this function
  * have not always been implemented as const (Examples: LWPRObject::predict or IRFRLS::predict ).
  * Therefore, this function cannot be const.
  */
 virtual void predictVariance(const Eigen::MatrixXd& inputs, Eigen::MatrixXd& variances)
 {
   variances.fill(0);
 }

void Dmp::analyticalSolution(const VectorXd& ts, MatrixXd& xs, MatrixXd& xds, Eigen::MatrixXd& forcing_terms, Eigen::MatrixXd& fa_outputs) const
{
  int n_time_steps = ts.size();
  
  // INTEGRATE SYSTEMS ANALYTICALLY AS MUCH AS POSSIBLE

  // Integrate phase
  MatrixXd xs_phase;
  MatrixXd xds_phase;
  phase_system_->analyticalSolution(ts,xs_phase,xds_phase);
  
  // Compute gating term
  MatrixXd xs_gating;
  MatrixXd xds_gating;
  gating_system_->analyticalSolution(ts,xs_gating,xds_gating);

  // Compute the output of the function approximator
  fa_outputs.resize(ts.size(),dim_orig());
  fa_outputs.fill(0.0);
  //if (isTrained())
  computeFunctionApproximatorOutput(xs_phase, fa_outputs);

  MatrixXd xs_gating_rep = xs_gating.replicate(1,fa_outputs.cols());
  MatrixXd g_minus_y0_rep = (attractor_state()-initial_state()).transpose().replicate(n_time_steps,1);
  forcing_terms = fa_outputs.array()*xs_gating_rep.array(); // zzz *g_minus_y0_rep.array();
  
  MatrixXd xs_goal, xds_goal;
  // Get current delayed goal
  if (goal_system_==NULL)
  {
    // If there is no dynamical system for the delayed goal, the goal is
    // simply the attractor state               
    xs_goal  = attractor_state().transpose().replicate(n_time_steps,1);
    // with zero change
    xds_goal = MatrixXd::Zero(n_time_steps,dim_orig());
  } 
  else
  {
    // Integrate goal system and get current goal state
    goal_system_->analyticalSolution(ts,xs_goal,xds_goal);
  }

  xs.resize(n_time_steps,dim());
  xds.resize(n_time_steps,dim());

  int T = n_time_steps;
    
  xs.GOALM(T) = xs_goal;     xds.GOALM(T) = xds_goal;
  xs.PHASEM(T) = xs_phase;   xds.PHASEM(T) = xds_phase;
  xs.GATINGM(T) = xs_gating; xds.GATINGM(T) = xds_gating;

  
  // THE REST CANNOT BE DONE ANALYTICALLY
  
  // Reset the dynamical system, and get the first state
  double damping = spring_system_->damping_coefficient();
  SpringDamperSystem localspring_system_(tau(),initial_state(),attractor_state(),damping);
  
  // Set first attractor state
  localspring_system_.set_attractor_state(xs_goal.row(0));
  
  // Start integrating spring damper system
  int dim_spring = localspring_system_.dim();
  VectorXd x_spring(dim_spring), xd_spring(dim_spring); // zzz Why are these needed?
  int t0 = 0;
  localspring_system_.integrateStart(x_spring, xd_spring);
  xs.row(t0).SPRING  = x_spring;
  xds.row(t0).SPRING = xd_spring;

  // Add forcing term to the acceleration of the spring state  
  xds.SPRINGM_Z(1) = xds.SPRINGM_Z(1) + forcing_terms.row(t0)/tau();
  
  for (int tt=1; tt<n_time_steps; tt++)
  {
    double dt = ts[tt]-ts[tt-1];
    
    // Euler integration
    xs.row(tt).SPRING  = xs.row(tt-1).SPRING + dt*xds.row(tt-1).SPRING;
  
    // Set the attractor state of the spring system
    localspring_system_.set_attractor_state(xs.row(tt).GOAL);

    // Integrate spring damper system
    localspring_system_.differentialEquation(xs.row(tt).SPRING, xd_spring);
    xds.row(tt).SPRING = xd_spring;
    
    // Add forcing term to the acceleration of the spring state
    xds.row(tt).SPRING_Z = xds.row(tt).SPRING_Z + forcing_terms.row(tt)/tau();
    // Compute y component from z
    xds.row(tt).SPRING_Y = xs.row(tt).SPRING_Z/tau();
    
  } 
}