object_model_list.hpp

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 */

/* \author Bastian Steder */

namespace pcl {

template <typename FeatureType>
inline void ObjectModelList::freeFeatureListMemory(std::vector<std::vector<std::vector<FeatureType*>*>*>& feature_list)
{
  for (unsigned int i=0; i<feature_list.size(); ++i)
  {
    ObjectModel::freeFeatureListMemory(*feature_list[i]);
    delete feature_list[i];
  }
  feature_list.clear();
}

template <typename FeatureType>
inline void ObjectModelList::getMergedFeatureList(const std::vector<std::vector<std::vector<FeatureType*>*>*>& feature_list,
                                                  std::vector<FeatureType*>& merged_feature_list, std::vector<FeatureSource>& feature_sources)
{
  merged_feature_list.clear();
  for (unsigned int i=0; i<feature_list.size(); ++i)
  {
    const std::vector<std::vector<FeatureType*>*>& features_per_model = *feature_list[i];
    for (unsigned int j=0; j<features_per_model.size(); ++j)
    {
      const std::vector<FeatureType*>& features_per_view = *features_per_model[j];
      for (unsigned int k=0; k<features_per_view.size(); ++k)
      {
        merged_feature_list.push_back(features_per_view[k]);
        feature_sources.push_back(FeatureSource(i, j, k));
      }
    }
  }
}

template <typename FeatureType>
void ObjectModelList::buildKdTree(const std::vector<FeatureType*>& merged_feature_list, KdTree<FeatureType*>& kdtree)
{
  //MEASURE_FUNCTION_TIME;
  if (merged_feature_list.empty())
    return;
  
  boost::shared_ptr<PointCloud<FeatureType*> > feature_pc(new PointCloud<FeatureType*>);
  for (unsigned int i=0; i<merged_feature_list.size(); ++i)
    feature_pc->points.push_back(merged_feature_list[i]);
  boost::shared_ptr<typename FeatureType::FeaturePointRepresentation> point_representation(
      new typename FeatureType::FeaturePointRepresentation(merged_feature_list[0]->getDescriptorSize()));
  kdtree.setPointRepresentation (point_representation);
  kdtree.setInputCloud(feature_pc);
}

template <typename FeatureType>
void ObjectModelList::getFeatureMatches(const std::vector<FeatureType*>& scene_features,
                                        const std::vector<FeatureType*>& model_database_features,
                                        const std::vector<ObjectModelList::FeatureSource>& model_database_feature_sources,
                                        float max_descriptor_distance, float min_distance_to_same_scene_feature,
                                        std::vector<std::vector<PointCorrespondences6DVector> >& feature_matches,
                                        const KdTree<FeatureType*>* kdtree, float max_descriptor_distance_kdtree) const
{
  if (model_database_features.empty())
    return;
  
  float min_distance_to_same_scene_feature_squared = pow(min_distance_to_same_scene_feature, 2);
  feature_matches.clear();
  feature_matches.resize(size());
  for (unsigned int model_idx=0; model_idx<feature_matches.size(); ++model_idx)
  {
    std::vector<PointCorrespondences6DVector>& model_feature_matches = feature_matches[model_idx];
    model_feature_matches.clear();
    model_feature_matches.resize(at(model_idx)->getViews().size());
  }
  
  if (kdtree != NULL)
  {  // With kdTree
    if (max_descriptor_distance_kdtree < 0.0f)
    {
      max_descriptor_distance_kdtree = max_descriptor_distance * model_database_features[0]->getDescriptorSize();
      std::cerr << "Maximum descriptor distance for usage in kdTree not given. Using "<<max_descriptor_distance_kdtree<<" instead.\n";
    }
    std::vector<int> neighbor_indices;
    std::vector<float> neighbor_distances;
    for (unsigned int scene_feature_idx=0; scene_feature_idx<scene_features.size(); ++scene_feature_idx)
    {
      const FeatureType& scene_feature = *scene_features[scene_feature_idx];
      neighbor_indices.clear();
      neighbor_distances.clear();
      kdtree->radiusSearch((FeatureType*)&scene_feature, max_descriptor_distance_kdtree,
                           neighbor_indices, neighbor_distances, INT_MAX);
      for (unsigned int neighbor_indices_idx=0; neighbor_indices_idx<neighbor_indices.size(); ++neighbor_indices_idx)
      {
        int database_feature_idx = neighbor_indices[neighbor_indices_idx];
        FeatureType& database_feature = *model_database_features[database_feature_idx];
        float descriptor_distance = scene_feature.getDescriptorDistance(database_feature);
        if (descriptor_distance > max_descriptor_distance)
          continue;
        float score = 1.0f-descriptor_distance;
        integrateIntoMatchList(scene_feature_idx, database_feature_idx, score, scene_features, model_database_features,
                               model_database_feature_sources, min_distance_to_same_scene_feature_squared, feature_matches);
      }
    }
  }
  else
  {  // Without kdTree
    for (unsigned int scene_feature_idx=0; scene_feature_idx<scene_features.size(); ++scene_feature_idx)
    {
      const FeatureType& scene_feature = *scene_features[scene_feature_idx];
      
      for (unsigned int database_feature_idx=0; database_feature_idx<model_database_feature_sources.size(); ++database_feature_idx)
      {
        FeatureType& database_feature = *model_database_features[database_feature_idx];
        float descriptor_distance = scene_feature.getDescriptorDistance(database_feature);
        if (descriptor_distance > max_descriptor_distance)
          continue;
        float score = 1.0f-descriptor_distance;
        integrateIntoMatchList(scene_feature_idx, database_feature_idx, score, scene_features, model_database_features,
                               model_database_feature_sources, min_distance_to_same_scene_feature_squared, feature_matches);
      }
    }
  }
}

template <typename FeatureType>
inline void ObjectModelList::integrateIntoMatchList(int scene_feature_idx, int database_feature_idx, float score,
                                                    const std::vector<FeatureType*>& scene_features,
                                                    const std::vector<FeatureType*>& model_database_features,
                                                    const std::vector<ObjectModelList::FeatureSource>& model_database_feature_sources,
                                                    float min_distance_to_same_scene_feature_squared,
                                                    std::vector<std::vector<PointCorrespondences6DVector> >& feature_matches)
{
  const FeatureSource& feature_source = model_database_feature_sources[database_feature_idx];
  PointCorrespondences6DVector& view_feature_matches = feature_matches[feature_source.model_index][feature_source.view_index];
  
  // We will now check, if there is already a match to the same scene feature that is close to the current view feature (in 3D space)
  // If this is the case we will only keep the better match
  int replace_idx = -1;
  bool better_match_already_exists = false;
  const FeatureType& database_feature = *model_database_features[database_feature_idx];
  for (int match_idx=view_feature_matches.size()-1; match_idx>=0; --match_idx)
  {
    PointCorrespondence6D& already_existing_match = view_feature_matches[match_idx];
    if (already_existing_match.index1 != (int)scene_feature_idx)
      break;
    
    if ((database_feature.getPosition()-already_existing_match.point2).squaredNorm() <= min_distance_to_same_scene_feature_squared)
    {
      //cout << "Skipping a match.\n";
      if (score <= already_existing_match.score)
      {
        better_match_already_exists = true;
        break;
      }
      else
      {
        replace_idx = match_idx;
        break;
      }
    }
  }
  
  if (better_match_already_exists)
    return;
  
  const FeatureType& scene_feature = *scene_features[scene_feature_idx];
  //cout << PVARN(descriptor_distance);
  PointCorrespondence6D match;
  match.score  = score;
  match.index1 = scene_feature_idx;
  match.index2 = feature_source.feature_index;
  match.point1 = scene_feature.getPosition();
  match.point2 = database_feature.getPosition();
  match.transformation = scene_feature.getTransformation().inverse() * database_feature.getTransformation();
  if (replace_idx < 0)
    view_feature_matches.push_back(match);
  else
    view_feature_matches[replace_idx] = match;
}

} // namespace end