예제 #1
0
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 ();
  }
}
예제 #2
0
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)
    {
      const float weight = -it_d_0->normal_z; // weight = -dot (normal, [0; 0; 1])

      if (pcl::isFinite (*it_d_0) && weight >= weight_min_)
      {
        *it_d_t_0 = PointModel (*it_d_0, weight);
        it_d_t_0->getVector4fMap ()       = T * it_d_t_0->getVector4fMap ();
        it_d_t_0->getNormalVector4fMap () = T * it_d_t_0->getNormalVector4fMap ();

        // NN search
        if (!kd_tree_->nearestKSearchT (*it_d_t_0, 1, index, squared_distance))
        {
          std::cerr << "ERROR in integration.cpp: nearestKSearch failed!\n";
          return (false);
        }

        // Average out corresponding points
        if (squared_distance[0] <= squared_distance_max_)
        {
          Vertex& v_m = mesh_model->getElement (VertexIndex (index[0])); // Non-const reference!

          if (v_m.getNormalVector4fMap ().dot (it_d_t_0->getNormalVector4fMap ()) >= dot_normal_min_)
          {
            *it_vi_0 = VertexIndex (index[0]);

            const float W   = v_m.weight;         // Old accumulated weight
            const float w   = it_d_t_0->weight;   // Weight of new point
            const float WW  = v_m.weight = W + w; // New accumulated weight

            const float r_m = static_cast <float> (v_m.r);
            const float g_m = static_cast <float> (v_m.g);
            const float b_m = static_cast <float> (v_m.b);

            const float r_d = static_cast <float> (it_d_t_0->r);
            const float g_d = static_cast <float> (it_d_t_0->g);
            const float b_d = static_cast <float> (it_d_t_0->b);

            v_m.getVector4fMap ()       = ( W*v_m.getVector4fMap ()       + w*it_d_t_0->getVector4fMap ())       / WW;
            v_m.getNormalVector4fMap () = ((W*v_m.getNormalVector4fMap () + w*it_d_t_0->getNormalVector4fMap ()) / WW).normalized ();
            v_m.r                       = this->trimRGB ((W*r_m + w*r_d) / WW);
            v_m.g                       = this->trimRGB ((W*g_m + w*g_d) / WW);
            v_m.b                       = this->trimRGB ((W*b_m + w*b_d) / WW);

            // Point has been observed again -> give it some extra time to live
            v_m.age = 0;

            // add a direction to the visibility confidence
            v_m.visconf.addDirection (v_m.getNormalVector4fMap (), sensor_eye-v_m.getVector4fMap (), w);

          } // dot normals
        } // squared distance
      } // isfinite && min weight

      // Connect
      // 4   2 - 1  //
      //     |   |  //
      // *   3 - 0  //
      //            //
      // 4 - 2   1  //
      //   \   \    //
      // *   3 - 0  //
      this->addToMesh (it_d_t_0, it_d_t_1, it_d_t_2, it_d_t_3,
                       it_vi_0,  it_vi_1,  it_vi_2,  it_vi_3,
                       mesh_model);
      this->addToMesh (it_d_t_0, it_d_t_2, it_d_t_4, it_d_t_3,
                       it_vi_0,  it_vi_2,  it_vi_4,  it_vi_3,
                       mesh_model);

      ++it_d_0;
      ++it_d_t_0; ++it_d_t_1; ++it_d_t_2; ++it_d_t_3; ++it_d_t_4;
      ++it_vi_0;  ++it_vi_1;  ++it_vi_2;  ++it_vi_3;  ++it_vi_4;
    } // for (uint32_t c=2; c<width; ++c)

    it_d_0   += 2;
    it_d_t_0 += 2; it_d_t_1 += 2; it_d_t_2 += 2; it_d_t_3 += 2, it_d_t_4 += 2;
    it_vi_0  += 2; it_vi_1  += 2; it_vi_2  += 2; it_vi_3  += 2, it_vi_4  += 2;
  } // for (uint32_t r=1; r<height; ++r)

  return (true);
}