C++ MeshPtr::beginVertexes方法代码示例

void
pcl::ihs::Integration::age (const MeshPtr& mesh, const bool cleanup) const
{
  for (Mesh::VertexIterator it = mesh->beginVertexes (); it!=mesh->endVertexes (); ++it)
  {
    if(it->age < age_max_)
    {
       // Point survives
       ++it->age;
    }
    else if(it->age == age_max_) // Judgement Day
    {
      if(it->visconf.getValue () < visconf_min_)
      {
        // Point dies (no need to transform it)
        mesh->deleteVertex (*it);
      }
      else
      {
        // Point becomes immortal
        it->age = std::numeric_limits <unsigned int>::max ();
      }
    }
  }

  if (cleanup)
  {
    mesh->cleanUp ();
  }
}

bool
pcl::ihs::Integration::merge (const CloudProcessedConstPtr& cloud_data,
                              const MeshPtr&                mesh_model,
                              const Transformation&         T) const
{
  if (!cloud_data->isOrganized ())
  {
    std::cerr << "ERROR in integration.cpp: Data cloud is not organized\n";
    return (false);
  }
  const uint32_t width  = cloud_data->width;
  const uint32_t height = cloud_data->height;
  const size_t   size   = cloud_data->size ();

  if (!mesh_model->sizeVertexes ())
  {
    std::cerr << "ERROR in integration.cpp: Model mesh is empty\n";
    return (false);
  }

  // Nearest neighbor search
  // TODO: remove this unnecessary copy
  CloudXYZPtr xyz_model (new CloudXYZ ());
  xyz_model->reserve (mesh_model->sizeVertexes ());
  for (VertexConstIterator it=mesh_model->beginVertexes (); it!=mesh_model->endVertexes (); ++it)
  {
    xyz_model->push_back (PointXYZ (it->x, it->y, it->z));
  }
  kd_tree_->setInputCloud (xyz_model);
  std::vector <int>   index (1);
  std::vector <float> squared_distance (1);

  mesh_model->reserveVertexes (mesh_model->sizeVertexes () + cloud_data->size ());
  mesh_model->reserveFaces (mesh_model->sizeFaces () + 2 * (width-1) * (height-1));

  // Data cloud in model coordinates (this does not change the connectivity information) and weights
  CloudModelPtr cloud_data_transformed (new CloudModel ());
  cloud_data_transformed->resize (size);

  // Sensor position in model coordinates
  const Eigen::Vector4f& sensor_eye = T * Eigen::Vector4f (0.f, 0.f, 0.f, 1.f);

  // Store which vertex is set at which position (initialized with invalid indexes)
  VertexIndexes vertex_indexes (size, VertexIndex ());

  // Set the transformed points not reached by the main loop
  for (uint32_t c=0; c<width; ++c)
  {
    const PointProcessed& pt_d = cloud_data->operator [] (c);
    const float weight = -pt_d.normal_z; // weight = -dot (normal, [0; 0; 1])

    if (pcl::isFinite (pt_d) && weight >= weight_min_)
    {
      PointModel& pt_d_t = cloud_data_transformed->operator [] (c);
      pt_d_t = PointModel (pt_d, weight);
      pt_d_t.getVector4fMap ()       = T * pt_d_t.getVector4fMap ();
      pt_d_t.getNormalVector4fMap () = T * pt_d_t.getNormalVector4fMap ();
    }
  }
  for (uint32_t r=1; r<height; ++r)
  {
    for (uint32_t c=0; c<2; ++c)
    {
      const PointProcessed& pt_d = cloud_data->operator [] (r*width + c);
      const float weight = -pt_d.normal_z; // weight = -dot (normal, [0; 0; 1])

      if (pcl::isFinite (pt_d) && weight >= weight_min_)
      {
        PointModel& pt_d_t = cloud_data_transformed->operator [] (r*width + c);
        pt_d_t = PointModel (pt_d, weight);
        pt_d_t.getVector4fMap ()       = T * pt_d_t.getVector4fMap ();
        pt_d_t.getNormalVector4fMap () = T * pt_d_t.getNormalVector4fMap ();
      }
    }
  }

  // 4   2 - 1  //
  //     |   |  //
  // *   3 - 0  //
  //            //
  // 4 - 2   1  //
  //   \   \    //
  // *   3 - 0  //
  CloudProcessed::const_iterator it_d_0   = cloud_data->begin ()             + width + 2;
  CloudModel::iterator           it_d_t_0 = cloud_data_transformed->begin () + width + 2;
  CloudModel::const_iterator     it_d_t_1 = cloud_data_transformed->begin ()         + 2;
  CloudModel::const_iterator     it_d_t_2 = cloud_data_transformed->begin ()         + 1;
  CloudModel::const_iterator     it_d_t_3 = cloud_data_transformed->begin () + width + 1;
  CloudModel::const_iterator     it_d_t_4 = cloud_data_transformed->begin ()            ;

  VertexIndexes::iterator it_vi_0 = vertex_indexes.begin () + width + 2;
  VertexIndexes::iterator it_vi_1 = vertex_indexes.begin ()         + 2;
  VertexIndexes::iterator it_vi_2 = vertex_indexes.begin ()         + 1;
  VertexIndexes::iterator it_vi_3 = vertex_indexes.begin () + width + 1;
  VertexIndexes::iterator it_vi_4 = vertex_indexes.begin ()            ;

  for (uint32_t r=1; r<height; ++r)
  {
    for (uint32_t c=2; c<width; ++c)
    {
//.........这里部分代码省略.........