Пример #1
0
 // Finds a child of the node. Returns NULL if it's not found.
 Val_T * getChild(int ID)
 {
     typename std::map<Id_T, Val_T*>::iterator it = m_children.find(ID);
     if(it != m_children.end())
         return it->second;
     return NULL;
 }
Пример #2
0
void do_scan(typename betree<Key, Value>::iterator &betit,
	     typename std::map<Key, Value>::iterator &refit,
	     betree<Key, Value> &b,
	     typename std::map<Key, Value> &reference)
{
  while (refit != reference.end()) {
    assert(betit != b.end());
    assert(betit.first == refit->first);
    assert(betit.second == refit->second);
    ++refit;
    if (refit == reference.end()) {
      debug(std::cout << "Almost done" << std::endl);
    }
    ++betit;
  }
  assert(betit == b.end());
}
Пример #3
0
 /** Returns the id of the given name, throws on lookup failure */
 int get(const T& name) const
 {
   typename std::map<T, int>::const_iterator it = m_name2int.find(name);
   if (it == m_name2int.end())
   {
     raise_exception(std::runtime_error, "lookup failure for: '" << name << "'");
   }
   else
   {
     return it->second;
   }
 }
Пример #4
0
 /** Returns the id of the given name, on lookup failure the name is
     put into the table */
 int getput(const T& name)
 {
   typename std::map<T, int>::const_iterator it = m_name2int.find(name);
   if (it == m_name2int.end())
   {
     return put(name);
   }
   else
   {
     return it->second;
   }
 }
Пример #5
0
        // Prints the node's sub-tree in a stream, using indentation
        // to represent parents and children.
        // d < 0 : display at real depth
        // d = 0 : display as root node
        // d > 0 : display at specified depth
        virtual void printTree(std::ostream & os, int d = -1) const
        {
            if(d < 0)
                d = getDepth();

            // Indentation
            for(int i = 0; i < d; i++)
                std::cout << " ";

            // Printing the node
            print(os);

            // Printing children
            typename std::map<Id_T, Val_T*>::iterator it;
            for(it = m_children.begin(); it != m_children.end(); it++)
            {
                it->second->printTree(os, d + 1);
            }
        }
Пример #6
0
 // Destroys the tree node and its children.
 virtual ~TreeNode()
 {
     typename std::map<Id_T, Val_T*>::iterator it;
     for(it = m_children.begin(); it != m_children.end(); it++)
         delete it->second;
 }
Пример #7
0
 bool has(const T& name) const
 {
   return m_name2int.find(name) != m_name2int.end();
 }
Пример #8
0
  void
  pcl::rec_3d_framework::GlobalNNCVFHRecognizer<Distance, PointInT, FeatureT>::recognize ()
  {

    models_.reset (new std::vector<ModelT>);
    transforms_.reset (new std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> >);

    PointInTPtr processed (new pcl::PointCloud<PointInT>);
    PointInTPtr in (new pcl::PointCloud<PointInT>);

    std::vector<pcl::PointCloud<FeatureT>, Eigen::aligned_allocator<pcl::PointCloud<FeatureT> > > signatures;
    std::vector < Eigen::Vector3d > centroids;

    if (indices_.size ())
      pcl::copyPointCloud (*input_, indices_, *in);
    else
      in = input_;

    {
      pcl::ScopeTime t ("Estimate feature");
      micvfh_estimator_->estimate (in, processed, signatures, centroids);
    }

    std::vector<index_score> indices_scores;
    descriptor_distances_.clear ();

    if (signatures.size () > 0)
    {

      {
        pcl::ScopeTime t_matching ("Matching and roll...");

        if (use_single_categories_ && (categories_to_be_searched_.size () > 0))
        {

          //perform search of the different signatures in the categories_to_be_searched_
          for (size_t c = 0; c < categories_to_be_searched_.size (); c++)
          {
            std::cout << "Using category:" << categories_to_be_searched_[c] << std::endl;
            for (size_t idx = 0; idx < signatures.size (); idx++)
            {
              /*double* hist = signatures[idx].points[0].histogram;
               std::vector<double> std_hist (hist, hist + getHistogramLength (dummy));
               flann_model histogram ("", std_hist);
               flann::Matrix<int> indices;
               flann::Matrix<double> distances;

               std::map<std::string, int>::iterator it;
               it = category_to_vectors_indices_.find (categories_to_be_searched_[c]);

               assert (it != category_to_vectors_indices_.end ());
               nearestKSearch (single_categories_index_[it->second], histogram, nmodels_, indices, distances);*/

              double* hist = signatures[idx].points[0].histogram;
              int size_feat = sizeof(signatures[idx].points[0].histogram) / sizeof(double);
              std::vector<double> std_hist (hist, hist + size_feat);
              //ModelT empty;

              flann_model histogram;
              histogram.descr = std_hist;
              flann::Matrix<int> indices;
              flann::Matrix<double> distances;

              std::map<std::string, int>::iterator it;
              it = category_to_vectors_indices_.find (categories_to_be_searched_[c]);
              assert (it != category_to_vectors_indices_.end ());

              nearestKSearch (single_categories_index_[it->second], histogram, NN_, indices, distances);
              //gather NN-search results
              double score = 0;
              for (size_t i = 0; i < NN_; ++i)
              {
                score = distances[0][i];
                index_score is;
                is.idx_models_ = single_categories_pointers_to_models_[it->second]->at (indices[0][i]);
                is.idx_input_ = static_cast<int> (idx);
                is.score_ = score;
                indices_scores.push_back (is);
              }
            }

            //we cannot add more than nmodels per category, so sort here and remove offending ones...
            std::sort (indices_scores.begin (), indices_scores.end (), sortIndexScoresOp);
            indices_scores.resize ((c + 1) * NN_);
          }
        }
        else
        {
          for (size_t idx = 0; idx < signatures.size (); idx++)
          {

            double* hist = signatures[idx].points[0].histogram;
            int size_feat = sizeof(signatures[idx].points[0].histogram) / sizeof(double);
            std::vector<double> std_hist (hist, hist + size_feat);
            //ModelT empty;

            flann_model histogram;
            histogram.descr = std_hist;

            flann::Matrix<int> indices;
            flann::Matrix<double> distances;
            nearestKSearch (flann_index_, histogram, NN_, indices, distances);

            //gather NN-search results
            double score = 0;
            for (int i = 0; i < NN_; ++i)
            {
              score = distances[0][i];
              index_score is;
              is.idx_models_ = indices[0][i];
              is.idx_input_ = static_cast<int> (idx);
              is.score_ = score;
              indices_scores.push_back (is);

              //ModelT m = flann_models_[indices[0][i]].model;
            }
          }
        }

        std::sort (indices_scores.begin (), indices_scores.end (), sortIndexScoresOp);

        /*
         * There might be duplicated candidates, in those cases it makes sense to take
         * the closer one in descriptor space
         */

        typename std::map<flann_model, bool> found;
        typename std::map<flann_model, bool>::iterator it_map;
        for (size_t i = 0; i < indices_scores.size (); i++)
        {
          flann_model m = flann_models_[indices_scores[i].idx_models_];
          it_map = found.find (m);
          if (it_map == found.end ())
          {
            indices_scores[found.size ()] = indices_scores[i];
            found[m] = true;
          }
        }
        indices_scores.resize (found.size ());

        int num_n = std::min (NN_, static_cast<int> (indices_scores.size ()));

        /*
         * Filter some hypothesis regarding to their distance to the first neighbour
         */

        /*std::vector<index_score> indices_scores_filtered;
         indices_scores_filtered.resize (num_n);
         indices_scores_filtered[0] = indices_scores[0];

         double best_score = indices_scores[0].score_;
         int kept = 1;
         for (int i = 1; i < num_n; ++i)
         {
         //std::cout << best_score << indices_scores[i].score_ << (best_score / indices_scores[i].score_) << std::endl;
         if ((best_score / indices_scores[i].score_) > 0.65)
         {
         indices_scores_filtered[i] = indices_scores[i];
         kept++;
         }

         //best_score = indices_scores[i].score_;
         }

         indices_scores_filtered.resize (kept);
         //std::cout << indices_scores_filtered.size () << " § " << num_n << std::endl;

         indices_scores = indices_scores_filtered;
         num_n = indices_scores.size ();*/
        std::cout << "Number of object hypotheses... " << num_n << std::endl;

        std::vector<bool> valid_trans;
        std::vector < Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> > transformations;

        micvfh_estimator_->getValidTransformsVec (valid_trans);
        micvfh_estimator_->getTransformsVec (transformations);

        for (int i = 0; i < num_n; ++i)
        {
          ModelT m = flann_models_[indices_scores[i].idx_models_].model;
          int view_id = flann_models_[indices_scores[i].idx_models_].view_id;
          int desc_id = flann_models_[indices_scores[i].idx_models_].descriptor_id;

          int idx_input = indices_scores[i].idx_input_;

          std::cout << m.class_ << "/" << m.id_ << " ==> " << indices_scores[i].score_ << std::endl;

          Eigen::Matrix4d roll_view_pose;
          bool roll_pose_found = getRollPose (m, view_id, desc_id, roll_view_pose);

          if (roll_pose_found && valid_trans[idx_input])
          {
            Eigen::Matrix4d transposed = roll_view_pose.transpose ();

            //std::cout << transposed << std::endl;

            PointInTPtr view;
            getView (m, view_id, view);

            Eigen::Matrix4d model_view_pose;
            getPose (m, view_id, model_view_pose);

            Eigen::Matrix4d scale_mat;
            scale_mat.setIdentity (4, 4);

            if (compute_scale_)
            {
              //compute scale using the whole view
              PointInTPtr view_transformed (new pcl::PointCloud<PointInT>);
              Eigen::Matrix4d hom_from_OVC_to_CC;
              hom_from_OVC_to_CC = transformations[idx_input].inverse () * transposed;
              pcl::transformPointCloud (*view, *view_transformed, hom_from_OVC_to_CC);

              Eigen::Vector3d input_centroid = centroids[indices_scores[i].idx_input_];
              Eigen::Vector3d view_centroid;
              getCentroid (m, view_id, desc_id, view_centroid);

              Eigen::Vector4d cmatch4f (view_centroid[0], view_centroid[1], view_centroid[2], 0);
              Eigen::Vector4d cinput4f (input_centroid[0], input_centroid[1], input_centroid[2], 0);

              Eigen::Vector4d max_pt_input;
              pcl::getMaxDistance (*processed, cinput4f, max_pt_input);
              max_pt_input[3] = 0;
              double max_dist_input = (cinput4f - max_pt_input).norm ();

              //compute max dist for transformed model_view
              pcl::getMaxDistance (*view, cmatch4f, max_pt_input);
              max_pt_input[3] = 0;
              double max_dist_view = (cmatch4f - max_pt_input).norm ();

              cmatch4f = hom_from_OVC_to_CC * cmatch4f;
              std::cout << max_dist_view << " " << max_dist_input << std::endl;

              double scale_factor_view = max_dist_input / max_dist_view;
              std::cout << "Scale factor:" << scale_factor_view << std::endl;

              Eigen::Matrix4d center, center_inv;

              center.setIdentity (4, 4);
              center (0, 3) = -cinput4f[0];
              center (1, 3) = -cinput4f[1];
              center (2, 3) = -cinput4f[2];

              center_inv.setIdentity (4, 4);
              center_inv (0, 3) = cinput4f[0];
              center_inv (1, 3) = cinput4f[1];
              center_inv (2, 3) = cinput4f[2];

              scale_mat (0, 0) = scale_factor_view;
              scale_mat (1, 1) = scale_factor_view;
              scale_mat (2, 2) = scale_factor_view;

              scale_mat = center_inv * scale_mat * center;
            }

            Eigen::Matrix4d hom_from_OC_to_CC;
            hom_from_OC_to_CC = scale_mat * transformations[idx_input].inverse () * transposed * model_view_pose;

            models_->push_back (m);
            transforms_->push_back (hom_from_OC_to_CC);
            descriptor_distances_.push_back (static_cast<double> (indices_scores[i].score_));
          }
          else
          {
            PCL_WARN("The roll pose was not found, should use CRH here... \n");
          }
        }
      }

      std::cout << "Number of object hypotheses:" << models_->size () << std::endl;

      /**
       * POSE REFINEMENT
       **/

      if (ICP_iterations_ > 0)
      {
        pcl::ScopeTime t ("Pose refinement");

        //Prepare scene and model clouds for the pose refinement step
        double VOXEL_SIZE_ICP_ = 0.005;
        PointInTPtr cloud_voxelized_icp (new pcl::PointCloud<PointInT> ());

        {
          pcl::ScopeTime t ("Voxelize stuff...");
          pcl::VoxelGrid<PointInT> voxel_grid_icp;
          voxel_grid_icp.setInputCloud (processed);
          voxel_grid_icp.setLeafSize (VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_);
          voxel_grid_icp.filter (*cloud_voxelized_icp);
          source_->voxelizeAllModels (VOXEL_SIZE_ICP_);
        }

#pragma omp parallel for num_threads(omp_get_num_procs())
        for (int i = 0; i < static_cast<int> (models_->size ()); i++)
        {

          ConstPointInTPtr model_cloud;
          PointInTPtr model_aligned (new pcl::PointCloud<PointInT>);

          if (compute_scale_)
          {
            model_cloud = models_->at (i).getAssembled (-1);
            PointInTPtr model_aligned_m (new pcl::PointCloud<PointInT>);
            pcl::transformPointCloud (*model_cloud, *model_aligned_m, transforms_->at (i));
            pcl::VoxelGrid<PointInT> voxel_grid_icp;
            voxel_grid_icp.setInputCloud (model_aligned_m);
            voxel_grid_icp.setLeafSize (VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_);
            voxel_grid_icp.filter (*model_aligned);
          }
          else
          {
            model_cloud = models_->at (i).getAssembled (VOXEL_SIZE_ICP_);
            pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i));
          }

          pcl::IterativeClosestPoint<PointInT, PointInT> reg;
          reg.setInputCloud (model_aligned); //model
          reg.setInputTarget (cloud_voxelized_icp); //scene
          reg.setMaximumIterations (ICP_iterations_);
          reg.setMaxCorrespondenceDistance (VOXEL_SIZE_ICP_ * 3.);
          reg.setTransformationEpsilon (1e-6);

          typename pcl::PointCloud<PointInT>::Ptr output_ (new pcl::PointCloud<PointInT> ());
          reg.align (*output_);

          Eigen::Matrix4d icp_trans = reg.getFinalTransformation ();
          transforms_->at (i) = icp_trans * transforms_->at (i);
        }
      }

      /**
       * HYPOTHESES VERIFICATION
       **/

      if (hv_algorithm_)
      {

        pcl::ScopeTime t ("HYPOTHESES VERIFICATION");

        std::vector<typename pcl::PointCloud<PointInT>::ConstPtr> aligned_models;
        aligned_models.resize (models_->size ());

        for (size_t i = 0; i < models_->size (); i++)
        {
          ConstPointInTPtr model_cloud;
          PointInTPtr model_aligned (new pcl::PointCloud<PointInT>);

          if (compute_scale_)
          {
            model_cloud = models_->at (i).getAssembled (-1);
            PointInTPtr model_aligned_m (new pcl::PointCloud<PointInT>);
            pcl::transformPointCloud (*model_cloud, *model_aligned_m, transforms_->at (i));
            pcl::VoxelGrid<PointInT> voxel_grid_icp;
            voxel_grid_icp.setInputCloud (model_aligned_m);
            voxel_grid_icp.setLeafSize (0.005, 0.005, 0.005);
            voxel_grid_icp.filter (*model_aligned);
          }
          else
          {
            model_cloud = models_->at (i).getAssembled (0.005);
            pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i));
          }

          //ConstPointInTPtr model_cloud = models_->at (i).getAssembled (0.005);
          //PointInTPtr model_aligned (new pcl::PointCloud<PointInT>);
          //pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i));
          aligned_models[i] = model_aligned;
        }

        std::vector<bool> mask_hv;
        hv_algorithm_->setSceneCloud (input_);
        hv_algorithm_->addModels (aligned_models, true);
        hv_algorithm_->verify ();
        hv_algorithm_->getMask (mask_hv);

        boost::shared_ptr < std::vector<ModelT> > models_temp;
        boost::shared_ptr < std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> > > transforms_temp;

        models_temp.reset (new std::vector<ModelT>);
        transforms_temp.reset (new std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> >);

        for (size_t i = 0; i < models_->size (); i++)
        {
          if (!mask_hv[i])
            continue;

          models_temp->push_back (models_->at (i));
          transforms_temp->push_back (transforms_->at (i));
        }

        models_ = models_temp;
        transforms_ = transforms_temp;
      }

    }
  }