C++ Particles::begin方法代码示例

//LC: added for local (re)localization
void MapModel::distributeLocally(Particles& particles, double roll, double pitch, double yaw,
                                    double x, double y, double z,
                                    UniformGeneratorT& rngUniform, double dist_linear, double dist_angular){        //maxHeight added by LC
  double sizeX,sizeY,sizeZ, minX, minY, minZ;
  //LC: get total map size
  m_map->getMetricSize(sizeX,sizeY,sizeZ);
  //LC: get minimum values of map bounding box
  m_map->getMetricMin(minX, minY, minZ);

  double weight = 1.0 / particles.size();
  Particles::iterator it = particles.begin();
  while (true){
    if (it == particles.end())
      break;

    // obtain a pose hypothesis:
    double x_new = x + dist_linear * ((rngUniform() - 0.5) * 2);
    double y_new = y + dist_linear * ((rngUniform() - 0.5) * 2);
    double z_new = z + dist_linear * ((rngUniform() - 0.5) * 2);

    double roll_new = roll;// + dist_angular * ((rngUniform() - 0.5) * 2);
    double pitch_new = pitch;// + dist_angular * ((rngUniform() - 0.5) * 2);
    double yaw_new = yaw + dist_angular * ((rngUniform() - 0.5) * 2);

    if(it == particles.begin()) {
        // obtain a pose hypothesis:
        x_new = x;
        y_new = y;
        z_new = z;

        roll_new = roll;
        pitch_new = pitch;
        yaw_new = yaw;
    }

    //set new pose
    it->pose.getOrigin().setX(x_new);
    it->pose.getOrigin().setY(y_new);
    it->pose.getOrigin().setZ(z_new);

    //set orientation
    it->pose.setRotation(tf::createQuaternionFromRPY(roll_new, pitch_new, yaw_new));

    it->weight = weight;
    it++;

  }

}

SecondaryStructureResidue setup_coarse_secondary_structure_residue(
    const Particles &ssr_ps, Model *mdl,
    bool winner_takes_all_per_res) {
  Floats scores;
  scores.push_back(0.0);
  scores.push_back(0.0);
  scores.push_back(0.0);
  int count = 0;
  for (Particles::const_iterator p = ssr_ps.begin(); p != ssr_ps.end();
       ++p) {
    IMP_USAGE_CHECK(SecondaryStructureResidue::get_is_setup(*p),
                    "all particles must be SecondaryStructureResidues");
    SecondaryStructureResidue ssr(*p);
    Floats tmp_scores;
    tmp_scores.push_back(ssr.get_prob_helix());
    tmp_scores.push_back(ssr.get_prob_strand());
    tmp_scores.push_back(ssr.get_prob_coil());
    int max_i = 0;
    Float max = 0.0;
    for (int i = 0; i < 3; i++) {
      if (tmp_scores[i] > max) {
        max = tmp_scores[i];
        max_i = i;
      }
      if (!winner_takes_all_per_res) scores[i] += tmp_scores[i];
    }
    if (winner_takes_all_per_res) scores[max_i] += 1.0;
    count++;
  }
  IMP_NEW(Particle, coarse_p, (mdl));
  SecondaryStructureResidue ssres = SecondaryStructureResidue::setup_particle(
      coarse_p, scores[0] / count, scores[1] / count, scores[2] / count);
  return ssres;
}

void
System::
init_particles(
    Particles<Particle_T> & particles,
    size_t in,
    size_t out,
    int mass)
{
  auto it(particles.begin());
  auto end(particles.end());

  for (size_t i = 0; it != end && i < in; ++it, ++i)
  {
    Point p = random_insphere(0, cell_.radius);

    it->x = cell_.x + p.x;
    it->y = cell_.y + p.y;
    it->z = cell_.z + p.z;
    it->mass = mass;

    cell_.put_inside(*it);
  }

  for (size_t i = 0; it != end && i < out; ++it, ++i)
  {
    Point p = random_insphere(cell_.radius, outer_limit_.radius);

    it->x = cell_.x + p.x;
    it->y = cell_.y + p.y;
    it->z = cell_.z + p.z;
    it->mass = mass;

    cell_.put_outside(*it);
  }
}

	void _remove(Particle *p)
	{
		Particles::iterator pos = std::find(particles.begin(), particles.end(), p);
		if(pos != particles.end())
		{
			particles.erase(pos);
		}
	}

	void projectPositions()
	{
		for(Particles::iterator iter = particles.begin();
			iter != particles.end();
			++iter)
		{
			Particle *p = *iter;
			p->p = p->x + p->v * PHYSICS_DT;
		}
	}

	void ClearAllParticles(){ 
		for (ParticlesIterator pIter = g_particles.begin(); pIter != g_particles.end(); ++pIter ) {
			Particle* p = *(pIter);
			if (p) {
				delete p;
				p = NULL;
			}
		}
		g_particles.clear();
	}

	void removeFlagged()
	{
		for(Particles::iterator iter = removeList.begin();
			iter != removeList.end();
			++iter)
		{
			Particle *p = *iter;
			_remove(p);
		}
		removeList.clear();
	}

	void unprojectVelocities()
	{
		for(Particles::iterator iter = particles.begin();
			iter != particles.end();
			++iter)
		{
			Particle *p = *iter;
			p->v = (p->p - p->x)/PHYSICS_DT;
			p->x = p->p;
		}
	}

	void applyVoxelGridConstraint(VoxelGrid *g)
	{
		for(Particles::iterator iter = particles.begin();
			iter != particles.end();
			++iter)
		{
			Particle *p = *iter;
			glm::ivec3 v;
			if(p->type == PARTICLE_PROJECTILE && g->applyConstraint(p,v))
			{
				g->set(v, 0);
				remove(p);
			}
		}
	}

void ProblemSpecificBoundaryConditions::operator() (Particles<N>& particles)
{
    using namespace config;

    const real Lx1 = 1.0/Lx;
    const real Lz1 = 1.0/Lz;

    Particle *p = particles.begin ();

    for (uint dummy = 0; dummy < N; ++dummy)
    {
        p->x -= floor ((p->x - x0)*Lx1)*Lx;
        p->z -= floor ((p->z - z0)*Lz1)*Lz;

        ++p;
    }
}

void KMLProxy::initialize(Model *m,const Particles &ps,
    const FloatKeys &atts,unsigned int num_centers){
  for(Particles::const_iterator it = ps.begin(); it != ps.end();it++)
  {ps_.push_back(*it);}
  for(FloatKeys::const_iterator it = atts.begin();
      it != atts.end();it++)
  {atts_.push_back(*it);}
  m_ = m;
  kcenters_=num_centers;
  dim_=atts.size();
  centroids_ = Particles();
  data_ = new KMData(dim_,ps_.size());
  for(unsigned int i=0;i<ps_.size(); i++) {
    for(unsigned int j=0;j<atts.size();j++){
      (*(*data_)[i])[j]=ps_[i]->get_value(atts[j]);
    }
  }
  is_init_=true;
}

void MapModel::initGlobal(Particles& particles, double z, double roll, double pitch,
                          const Vector6d& initNoise,
                          UniformGeneratorT& rngUniform, NormalGeneratorT& rngNormal, double maxHeight, double minHeight){        //maxHeight added by LC
  double sizeX,sizeY,sizeZ, minX, minY, minZ;
  //LC: get total map size
  m_map->getMetricSize(sizeX,sizeY,sizeZ);
  //LC: get minimum values of map bounding box
  m_map->getMetricMin(minX, minY, minZ);

  double weight = 1.0 / particles.size();
  Particles::iterator it = particles.begin();
  while (true){
    if (it == particles.end())
      break;
    // obtain a pose hypothesis:
    double x = minX + sizeX * rngUniform();
    double y = minY + sizeY * rngUniform();
    std::vector<double> z_list;
    getHeightlist(x, y, minHeight, maxHeight,z_list);

    //LC: create a pose with random yaw orientation at (x,y) for every entry in the height list
    for (unsigned zIdx = 0; zIdx < z_list.size(); zIdx++){
      if (it == particles.end())
        break;

      // not needed => we already know that z contains valid poses
      // distance map: used distance from obstacles:
      //std::abs(node->getLogOdds()) < 0.1){
      //			if (!isOccupied(octomap::point3d(x, y, z[zIdx]))){

      it->pose.getOrigin().setX(x);
      it->pose.getOrigin().setY(y);
      // TODO: sample z, roll, pitch
      it->pose.getOrigin().setZ(z_list.at(zIdx) + z + rngNormal() * initNoise(2));
      //LC: create yaw orientation from -PI to +PI
      double yaw = rngUniform() * 2 * M_PI  -M_PI;
      it->pose.setRotation(tf::createQuaternionFromRPY(roll, pitch, yaw));
      it->weight = weight;
      it++;
    }
  }

}

void MapModel::initGlobal(Particles& particles,
                          const Vector6d& initPose, const Vector6d& initNoise,
                          UniformGeneratorT& rngUniform, NormalGeneratorT& rngNormal){
  double sizeX,sizeY,sizeZ, minX, minY, minZ;
  m_map->getMetricSize(sizeX,sizeY,sizeZ);
  m_map->getMetricMin(minX, minY, minZ);

  double weight = 1.0 / particles.size();
  Particles::iterator it = particles.begin();
  while (true){
    if (it == particles.end())
      break;
    // obtain a pose hypothesis:
    double x = minX + sizeX * rngUniform();
    double y = minY + sizeY * rngUniform();
    std::vector<double> z;
    getHeightlist(x, y, 0.6,z);

    for (unsigned zIdx = 0; zIdx < z.size(); zIdx++){
      if (it == particles.end())
        break;

      // not needed => we already know that z contains valid poses
      // distance map: used distance from obstacles:
      //std::abs(node->getLogOdds()) < 0.1){
      //			if (!isOccupied(octomap::point3d(x, y, z[zIdx]))){

      it->pose.getOrigin().setX(x);
      it->pose.getOrigin().setY(y);
      // TODO: sample z, roll, pitch around odom pose!
      it->pose.getOrigin().setZ(z.at(zIdx) + initPose(2) + rngNormal() * initNoise(2));
      double yaw = rngUniform() * 2 * M_PI  -M_PI;
      it->pose.setRotation(tf::createQuaternionFromYaw(yaw));
      it->weight = weight;
      it++;
    }
  }

}

Particles CHARMMParameters::create_dihedrals(Particles bonds) const
{
  IMP_OBJECT_LOG;
  Particles ps;
  BondMap particle_bonds;
  make_bond_map(bonds, particle_bonds);

  // Iterate over all bonds
  for (Particles::const_iterator bit1 = bonds.begin();
       bit1 != bonds.end(); ++bit1) {
    IMP::atom::Bond bd = IMP::atom::Bond(*bit1);
    Particle *p2 = bd.get_bonded(0).get_particle();
    Particle *p3 = bd.get_bonded(1).get_particle();

    // Extend along each bond from p2 and p3 to get candidate
    // p1-p2-p3-p4 dihedrals
    for (base::Vector<IMP::atom::Bond>::const_iterator bit2
         = particle_bonds[p2].begin();
         bit2 != particle_bonds[p2].end(); ++bit2) {
      Particle *p1 = get_other_end_of_bond(p2, *bit2);

      if (p1 != p3) {
        for (base::Vector<IMP::atom::Bond>::const_iterator bit3
             = particle_bonds[p3].begin();
             bit3 != particle_bonds[p3].end(); ++bit3) {
          Particle *p4 = get_other_end_of_bond(p3, *bit3);

          // Avoid generating dihedrals for three-membered rings
          if (p1 != p4 && p2 != p4) {
            internal::add_dihedral_to_list(this, p1, p2, p3, p4, ps);
          }
        }
      }
    }
  }
  return ps;
}

Particles CHARMMParameters::create_angles(Particles bonds) const
{
  IMP_OBJECT_LOG;
  Particles ps;
  BondMap particle_bonds;
  make_bond_map(bonds, particle_bonds);

  // Iterate over all bonds
  for (Particles::const_iterator bit1 = bonds.begin();
       bit1 != bonds.end(); ++bit1) {
    IMP::atom::Bond bd = IMP::atom::Bond(*bit1);
    Particle *p2 = bd.get_bonded(0).get_particle();
    Particle *p3 = bd.get_bonded(1).get_particle();

    // Extend along each adjoining p2 bond to get candidate p1-p2-p3 angles
    for (base::Vector<IMP::atom::Bond>::const_iterator bit2
         = particle_bonds[p2].begin();
         bit2 != particle_bonds[p2].end(); ++bit2) {
      Particle *p1 = get_other_end_of_bond(p2, *bit2);
      // Avoid making angles where p1 == p3, and avoid double-counting
      if (p3 > p1) {
        add_angle(p1, p2, p3, ps);
      }
    }
    // Do the same for p2-p3-p4 angles
    for (base::Vector<IMP::atom::Bond>::const_iterator bit2
         = particle_bonds[p3].begin();
         bit2 != particle_bonds[p3].end(); ++bit2) {
      Particle *p4 = get_other_end_of_bond(p3, *bit2);
      if (p4 < p2) {
        add_angle(p2, p3, p4, ps);
      }
    }
  }
  return ps;
}