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

            Eigen::VectorXd operator()(const F& f, const Eigen::VectorXd& init, bool bounded) const
            {
                // Assert that the algorithm is non-gradient
                // TO-DO: Add support for MLSL (Multi-Level Single-Linkage)
                // TO-DO: Add better support for ISRES (Improved Stochastic Ranking Evolution Strategy)
                // clang-format off
                static_assert(Algorithm == nlopt::LN_COBYLA || Algorithm == nlopt::LN_BOBYQA ||
                    Algorithm == nlopt::LN_NEWUOA || Algorithm == nlopt::LN_NEWUOA_BOUND ||
                    Algorithm == nlopt::LN_PRAXIS || Algorithm == nlopt::LN_NELDERMEAD ||
                    Algorithm == nlopt::LN_SBPLX || Algorithm == nlopt::GN_DIRECT ||
                    Algorithm == nlopt::GN_DIRECT_L || Algorithm == nlopt::GN_DIRECT_L_RAND ||
                    Algorithm == nlopt::GN_DIRECT_NOSCAL || Algorithm == nlopt::GN_DIRECT_L_NOSCAL ||
                    Algorithm == nlopt::GN_DIRECT_L_RAND_NOSCAL || Algorithm == nlopt::GN_ORIG_DIRECT ||
                    Algorithm == nlopt::GN_ORIG_DIRECT_L || Algorithm == nlopt::GN_CRS2_LM ||
                    Algorithm == nlopt::GD_STOGO || Algorithm == nlopt::GD_STOGO_RAND ||
                    Algorithm == nlopt::GN_ISRES || Algorithm == nlopt::GN_ESCH, "NLOptNoGrad accepts gradient free nlopt algorithms only");
                // clang-format on

                int dim = init.size();
                nlopt::opt opt(Algorithm, dim);

                opt.set_max_objective(nlopt_func<F>, (void*)&f);

                std::vector<double> x(dim);
                Eigen::VectorXd::Map(&x[0], dim) = init;

                opt.set_maxeval(Params::opt_nloptnograd::iterations());

                if (bounded) {
                    opt.set_lower_bounds(std::vector<double>(dim, 0));
                    opt.set_upper_bounds(std::vector<double>(dim, 1));
                }

                double max;

                opt.optimize(x, max);

                return Eigen::VectorXd::Map(x.data(), x.size());
            }

      bool learn_variance(Eigen::VectorXd& var, const Eigen::VectorXd& q) {
        if (adaptation_window())
          estimator_.add_sample(q);

        if (end_adaptation_window()) {
          compute_next_window();

          estimator_.sample_variance(var);

          double n = static_cast<double>(estimator_.num_samples());
          var = (n / (n + 5.0)) * var
                + 1e-3 * (5.0 / (n + 5.0)) * Eigen::VectorXd::Ones(var.size());

          estimator_.restart();

          ++adapt_window_counter_;
          return true;
        }

        ++adapt_window_counter_;
        return false;
      }

void ObjectModelLine::optimizeModelCoefficients (const std::vector<int> &inliers,
		const Eigen::VectorXd &model_coefficients,
		Eigen::VectorXd &optimized_coefficients){
	assert (model_coefficients.size () == 6);

	if (inliers.size () == 0)
	{
		cout<<"[ObjectModelLine::optimizeModelCoefficients] Inliers vector empty! Returning the same coefficients";
		optimized_coefficients = model_coefficients;
		return;
	}

	assert (inliers.size () > 2);

	optimized_coefficients.resize (6);

	// Compute the 3x3 covariance matrix
	Eigen::Vector4d centroid = Centroid3D::computeCentroid(this->inputPointCloud, inliers);
//	compute3DCentroid (this->inputPointCloud, inliers, centroid);
	Eigen::Matrix3d covariance_matrix;
//	computeCovarianceMatrix (inputPointCloud, inliers, centroid, covariance_matrix);
	 covariance_matrix = Covariance3D::computeCovarianceMatrix (inputPointCloud, inliers, centroid);
	optimized_coefficients[0] = centroid[0];
	optimized_coefficients[1] = centroid[1];
	optimized_coefficients[2] = centroid[2];

	// Extract the eigenvalues and eigenvectors
	//cloud_geometry::eigen_cov (covariance_matrix, eigen_values, eigen_vectors);
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> ei_symm (covariance_matrix);
//	EIGEN_ALIGN_MEMORY Eigen::Vector3d eigen_values  = ei_symm.eigenvalues ();
	EIGEN_ALIGN_MEMORY Eigen::Matrix3d eigen_vectors = ei_symm.eigenvectors ();

#ifdef EIGEN3
	optimized_coefficients.tail<3> () = eigen_vectors.col (2).normalized ();
#else
	optimized_coefficients.end<3> () = eigen_vectors.col (2).normalized ();
#endif

}

bool JointTrajGeneratorRML::insertSegments(
        const Eigen::VectorXd &point,
        const ros::Time &time,
        TrajSegments &segments,
        std::vector<size_t> &index_permutation) const
{
  // Check the size of the jointspace command
  if(point.size() == n_dof_) {
    // Handle a position given as an Eigen vector
    TrajSegment segment(n_dof_,true);

    segment.start_time = time;
    segment.goal_positions = point;
    segments.clear();
    segments.push_back(segment);
  } else {
    RTT::log(RTT::Debug) << "Received trajectory of invalid size." <<RTT::endlog();
    return false;
  }

  return true;
}

UnifiedModel::UnifiedModel(const Eigen::MatrixXd& centers, const Eigen::MatrixXd& widths, const Eigen::MatrixXd& slopes, const Eigen::VectorXd& offsets, bool normalized_basis_functions, bool lines_pivot_at_max_activation)
{
  int n_basis_functions = centers.rows();
#ifndef NDEBUG // Variables below are only required for asserts; check for NDEBUG to avoid warnings.
  int n_dims = centers.cols();
#endif 

  assert(n_basis_functions==widths.rows());
  assert(n_dims           ==widths.cols());
  assert(n_basis_functions==slopes.rows());
  assert(n_dims           ==slopes.cols());
  assert(n_basis_functions==offsets.size());
  
  centers_.resize(n_basis_functions);
  covars_.resize(n_basis_functions);
  slopes_.resize(n_basis_functions);
  offsets_.resize(n_basis_functions);
  priors_.resize(n_basis_functions);
  
  for (int i=0; i<n_basis_functions; i++)
  {
    centers_[i] = centers.row(i);
    covars_[i] = widths.row(i).asDiagonal();
    covars_[i] = covars_[i].array().square();
    slopes_[i] = slopes.row(i);
    offsets_[i] = offsets[i];
    
    priors_[i] = 1.0;
  }
  
  normalized_basis_functions_ = normalized_basis_functions;
  lines_pivot_at_max_activation_ = lines_pivot_at_max_activation;
  slopes_as_angles_ = false;
  
  cosine_basis_functions_ = false;

  initializeAllValuesVectorSize();  
}

void TrajActionServer::command_joints(Eigen::VectorXd q_cmd) {
    int njnts = q_cmd.size();
    std_msgs::Float64 qval_msg;
    qval_msg.data = q_cmd(0);
    j1_pub_.publish(qval_msg);
    if (njnts>1) {
        qval_msg.data=q_cmd(1);
        j2_pub_.publish(qval_msg);
        // mimic this for pitch mechanism joints:
        j2_1_pub_.publish(qval_msg);
        j2_2_pub_.publish(qval_msg);
        j2_5_pub_.publish(qval_msg);
        // the following 2 joints follow w/ negative of j2
        qval_msg.data= -q_cmd(1);
        j2_3_pub_.publish(qval_msg);
        j2_4_pub_.publish(qval_msg);        
    }
    if (njnts>2) {
        qval_msg.data=q_cmd(2);
        j3_pub_.publish(qval_msg);
    }
    if (njnts>3) {
        qval_msg.data=q_cmd(3);
        j4_pub_.publish(qval_msg);
    }
    if (njnts>4) {
        qval_msg.data=q_cmd(4);
        j5_pub_.publish(qval_msg);
    }
    if (njnts>5) {
        qval_msg.data=q_cmd(5);
        j6_pub_.publish(qval_msg);
    }
    if (njnts>6) {
        qval_msg.data=q_cmd(6);
        j7_pub_.publish(qval_msg);
    }
}

void VectorSum::updateHook()
{
  // Reset the accumulator
  sum_.setZero();

  // Get the addends
  bool has_new_data = false;
  Eigen::VectorXd addend;
  while(addends_in_.read( addend, false ) == RTT::NewData) {
    if(addend.size() == dim_) {
      sum_ += addend;
      has_new_data = true;
    } else {
      RTT::log(RTT::Error) << "Input to VectorSum component does not have the correct dimension. All inputs should have dimension "<<dim_<<" but this input had dimension " << addend.size();
      this->error();
    }
  }

  // Write the sum
  if(has_new_data) {
    sum_out_.write( sum_ );
  }
}

Eigen::VectorXd BlockSparseMatrix::operator* (const Eigen::VectorXd& rhs) const
{
	CHECK(!mbCSRDirty);
	int vectorLength = static_cast<int>(rhs.size());
	double* rhsData = new double[vectorLength];
	double* result = new double[vectorLength];
	memcpy(rhsData, rhs.data(), vectorLength * sizeof(double));
	char trans = 'n';
	int numRows = mBlockHeight * mNumBlocksHeight;

#ifdef _MKL_IMPLEMENT
	mkl_dcsrgemv(&trans, &numRows, const_cast<double*>(mCSREquivalent.GetValueData()), const_cast<int*>(mCSREquivalent.GetRowId()), const_cast<int*>(mCSREquivalent.GetColumnId()), rhsData, result);
#else 
	CHECK(0) << "_MKL_IMPLEMENT not defined!";
#endif
	
	Eigen::VectorXd ret(vectorLength);
	for (int i = 0; i < vectorLength; ++i)
		ret[i] = result[i];
	delete[] rhsData;
	delete[] result;
	return ret;
}

template <typename PointT> void
pcl::SampleConsensusModelCircle3D<PointT>::optimizeModelCoefficients (
      const std::vector<int> &inliers, 
      const Eigen::VectorXf &model_coefficients, 
      Eigen::VectorXf &optimized_coefficients)
{
  optimized_coefficients = model_coefficients;

  // Needs a set of valid model coefficients
  if (model_coefficients.size () != 7)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
    return;
  }

  // Need at least 3 samples
  if (inliers.size () <= 3)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ());
    return;
  }

  tmp_inliers_ = &inliers;

  OptimizationFunctor functor (static_cast<int> (inliers.size ()), this);
  Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, double> lm (num_diff);
  Eigen::VectorXd coeff;
  int info = lm.minimize (coeff);
  for (int i = 0; i < coeff.size (); ++i)
    optimized_coefficients[i] = static_cast<float> (coeff[i]);

  // Compute the L2 norm of the residuals
  PCL_DEBUG ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g %g %g %g \nFinal solution: %g %g %g %g %g %g %g\n",
             info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3], model_coefficients[4], model_coefficients[5], model_coefficients[6], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2], optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5], optimized_coefficients[6]);
}

            Eigen::VectorXd operator()(const F& f, const Eigen::VectorXd& init, bool bounded) const
            {
                tools::par::init();
                typedef std::pair<Eigen::VectorXd, double> pair_t;
                auto body = [&](int i) {
                    // clang-format off
                    Eigen::VectorXd r_init = tools::random_vector(init.size());
                    Eigen::VectorXd v = Optimizer()(f, init, bounded);
                    double lik = opt::eval(f, v);
                    return std::make_pair(v, lik);
                    // clang-format on
                };

                auto comp = [](const pair_t& v1, const pair_t& v2) {
                    // clang-format off
                    return v1.second > v2.second;
                    // clang-format on
                };

                pair_t init_v = std::make_pair(init, -std::numeric_limits<float>::max());
                auto m = tools::par::max(init_v, Params::opt_parallelrepeater::repeats(), body, comp);

                return m.first;
            };

 double log_prob_grad(const M& model,
                      Eigen::VectorXd& params_r,
                      Eigen::VectorXd& gradient,
                      std::ostream* msgs = 0) {
   using std::vector;
   using stan::agrad::var;
   Eigen::Matrix<var,Eigen::Dynamic,1> ad_params_r(params_r.size());
   for (size_t i = 0; i < model.num_params_r(); ++i) {
     stan::agrad::var var_i(params_r[i]);
     ad_params_r[i] = var_i;
   }
   try {
     var adLogProb
       = model
         .template log_prob<propto,
                            jacobian_adjust_transform>(ad_params_r, msgs);
     double val = adLogProb.val();
     stan::agrad::grad(adLogProb, ad_params_r, gradient);
     return val;
   } catch (std::exception &ex) {
     stan::agrad::recover_memory();
     throw;
   }
 }

IGL_INLINE Eigen::MatrixXd igl::rotate_vectors(
                    const Eigen::MatrixXd& V,
                    const Eigen::VectorXd& A,
                    const Eigen::MatrixXd& B1,
                    const Eigen::MatrixXd& B2)
{
  Eigen::MatrixXd RV(V.rows(),V.cols());

  for (unsigned i=0; i<V.rows();++i)
  {
    double norm = V.row(i).norm();
    
    // project onto the tangent plane and convert to angle
    double a = atan2(B2.row(i).dot(V.row(i)),B1.row(i).dot(V.row(i)));

    // rotate
    a += (A.size() == 1) ? A(0) : A(i);

    // move it back to global coordinates
    RV.row(i) = norm*cos(a) * B1.row(i) + norm*sin(a) * B2.row(i);
  }

  return RV;
}

// Fit a polynomial.
// Adapted from
// https://github.com/JuliaMath/Polynomials.jl/blob/master/src/Polynomials.jl#L676-L716
Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order)
{
  assert(xvals.size() == yvals.size());
  assert(order >= 1 && order <= xvals.size() - 1);
  Eigen::MatrixXd A(xvals.size(), order + 1);

  for (int i = 0; i < xvals.size(); i++)
  {
    A(i, 0) = 1.0;
  }

  for (int j = 0; j < xvals.size(); j++)
  {
    for (int i = 0; i < order; i++)
    {
      A(j, i + 1) = A(j, i) * xvals(j);
    }
  }

  auto Q = A.householderQr();
  auto result = Q.solve(yvals);
  return result;
}

void display()
{
  using namespace Eigen;
  using namespace igl;
  using namespace std;

  if(!trackball_on && tot_num_samples < 10000)
  {
    if(S.size() == 0)
    {
      S.resize(V.rows());
      S.setZero();
    }
    VectorXd Si;
    const int num_samples = 20;
    ambient_occlusion(ei,V,N,num_samples,Si);
    S *= (double)tot_num_samples;
    S += Si*(double)num_samples;
    tot_num_samples += num_samples;
    S /= (double)tot_num_samples;
  }

  // Convert to 1-intensity
  C.conservativeResize(S.rows(),3);
  if(ao_on)
  {
    C<<S,S,S;
    C.array() = (1.0-ao_factor*C.array());
  }else
  {
    C.setConstant(1.0);
  }
  if(ao_normalize)
  {
    C.col(0) *= ((double)C.rows())/C.col(0).sum();
    C.col(1) *= ((double)C.rows())/C.col(1).sum();
    C.col(2) *= ((double)C.rows())/C.col(2).sum();
  }
  C.col(0) *= color(0);
  C.col(1) *= color(1);
  C.col(2) *= color(2);

  glClearColor(back[0],back[1],back[2],0);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  // All smooth points
  glEnable( GL_POINT_SMOOTH );

  glDisable(GL_LIGHTING);
  if(lights_on)
  {
    lights();
  }
  push_scene();
  glEnable(GL_DEPTH_TEST);
  glDepthFunc(GL_LEQUAL);
  glEnable(GL_NORMALIZE);
  glEnable(GL_COLOR_MATERIAL);
  glColorMaterial(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE);
  push_object();

  // Draw the model
  // Set material properties
  glEnable(GL_COLOR_MATERIAL);
  draw_mesh(V,F,N,C);

  pop_object();

  // Draw a nice floor
  glPushMatrix();
  const double floor_offset =
    -2./bbd*(V.col(1).maxCoeff()-mid(1));
  glTranslated(0,floor_offset,0);
  const float GREY[4] = {0.5,0.5,0.6,1.0};
  const float DARK_GREY[4] = {0.2,0.2,0.3,1.0};
  draw_floor(GREY,DARK_GREY);
  glPopMatrix();

  pop_scene();

  report_gl_error();

  TwDraw();
  glutSwapBuffers();
  glutPostRedisplay();
}

//==============================================================================
TEST(World, Cloning)
{
  // Create a list of skel files to test with
  std::vector<std::string> fileList;
  fileList.push_back(DART_DATA_PATH"skel/test/chainwhipa.skel");
  fileList.push_back(DART_DATA_PATH"skel/test/single_pendulum.skel");
  fileList.push_back(DART_DATA_PATH"skel/test/single_pendulum_euler_joint.skel");
  fileList.push_back(DART_DATA_PATH"skel/test/single_pendulum_ball_joint.skel");
  fileList.push_back(DART_DATA_PATH"skel/test/double_pendulum.skel");
  fileList.push_back(DART_DATA_PATH"skel/test/double_pendulum_euler_joint.skel");
  fileList.push_back(DART_DATA_PATH"skel/test/double_pendulum_ball_joint.skel");
  fileList.push_back(DART_DATA_PATH"skel/test/serial_chain_revolute_joint.skel");
  fileList.push_back(DART_DATA_PATH"skel/test/serial_chain_eulerxyz_joint.skel");
  fileList.push_back(DART_DATA_PATH"skel/test/serial_chain_ball_joint.skel");
  fileList.push_back(DART_DATA_PATH"skel/test/serial_chain_ball_joint_20.skel");
  fileList.push_back(DART_DATA_PATH"skel/test/serial_chain_ball_joint_40.skel");
  fileList.push_back(DART_DATA_PATH"skel/test/simple_tree_structure.skel");
  fileList.push_back(DART_DATA_PATH"skel/test/simple_tree_structure_euler_joint.skel");
  fileList.push_back(DART_DATA_PATH"skel/test/simple_tree_structure_ball_joint.skel");
  fileList.push_back(DART_DATA_PATH"skel/test/tree_structure.skel");
  fileList.push_back(DART_DATA_PATH"skel/test/tree_structure_euler_joint.skel");
  fileList.push_back(DART_DATA_PATH"skel/test/tree_structure_ball_joint.skel");
  fileList.push_back(DART_DATA_PATH"skel/fullbody1.skel");

  std::vector<dart::simulation::WorldPtr> worlds;
  for(size_t i=0; i<fileList.size(); ++i)
    worlds.push_back(dart::utils::SkelParser::readWorld(fileList[i]));

  for(size_t i=0; i<worlds.size(); ++i)
  {
    dart::simulation::WorldPtr original = worlds[i];
    std::vector<dart::simulation::WorldPtr> clones;
    clones.push_back(original);
    for(size_t j=1; j<5; ++j)
      clones.push_back(clones[j-1]->clone());

    // Make sure all the Skeleton states match at the start
    // TODO(MXG): This should be removed once state also gets copied over during a cloning
    for(size_t j=1; j<clones.size(); ++j)
    {
      for(size_t k=0; k<original->getNumSkeletons(); ++k)
      {
        dart::dynamics::SkeletonPtr skel = original->getSkeleton(k);
        dart::dynamics::SkeletonPtr clone = clones[j]->getSkeleton(k);

        clone->setPositions(skel->getPositions());
        clone->setVelocities(skel->getVelocities());
        clone->setAccelerations(skel->getAccelerations());
        clone->setForces(skel->getForces());
      }
    }

#ifndef NDEBUG // Debug mode
    size_t numIterations = 3;
#else
    size_t numIterations = 500;
#endif

    for(size_t j=0; j<numIterations; ++j)
    {
      for(size_t k=0; k<original->getNumSkeletons(); ++k)
      {
        dart::dynamics::SkeletonPtr skel = original->getSkeleton(k);

        // Generate a random command vector
        Eigen::VectorXd commands = skel->getCommands();
        for(int q=0; q<commands.size(); ++q)
          commands[q] = random(-0.1, 0.1);

        // Assign the command vector to each clone of the kth skeleton
        for(size_t c=0; c<clones.size(); ++c)
        {
          dart::dynamics::SkeletonPtr skelClone = clones[c]->getSkeleton(k);
          skelClone->setCommands(commands);
        }
      }

      // Step each clone forward
      for(size_t c=0; c<clones.size(); ++c)
        clones[c]->step(false);
    }

    for(size_t c=0; c<clones.size(); ++c)
    {
      for(size_t k=0; k<original->getNumSkeletons(); ++k)
      {
        dart::dynamics::SkeletonPtr skel = original->getSkeleton(k);
        dart::dynamics::SkeletonPtr clone = clones[c]->getSkeleton(k);

        EXPECT_TRUE( equals(skel->getPositions(), clone->getPositions(), 0));
        EXPECT_TRUE( equals(skel->getVelocities(), clone->getVelocities(), 0));
        EXPECT_TRUE( equals(skel->getAccelerations(), clone->getAccelerations(), 0));
        EXPECT_TRUE( equals(skel->getForces(), clone->getForces(), 0));
      }
    }
  }
}