false_positives_filter.cpp

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 */


#include <narf_recognition/false_positives_filter.h>

using Eigen::Affine3f;
using Eigen::Vector3f;
using std::cout;
using std::cerr;
using std::min;
using std::max;

namespace pcl {

FalsePositivesFilter::FalsePositivesFilter() : view_simulation_images_(NULL)
{
}

FalsePositivesFilter::~FalsePositivesFilter()
{
}

void
FalsePositivesFilter::getRangeImagesForComparison(const RangeImage& scene, const ObjectModel& model,
                                                       const PoseEstimate& pose_estimate,
                                                       RangeImage& object_image, RangeImage& scene_sub_image) const
{
  int wanted_image_size = 50;  // Ideally so many pixels
  float max_angle_size = model.getMaxAngleSize(pose_estimate.transformation);
  float wanted_angular_resolution = max_angle_size/float(wanted_image_size);
  int combine_pixels = 1;  // How many pixels of the original scene image should be merged to reduce resolution
  if (scene.getAngularResolution() < 0.5f*wanted_angular_resolution)
    combine_pixels = lrint(floor(wanted_angular_resolution/scene.getAngularResolution()));
  float angular_resolution = scene.getAngularResolution()*combine_pixels;
  //cout << PVARAC(max_angle_size)<<PVARAC(wanted_angular_resolution)<<PVARC(combine_pixels)<<PVARN(angular_resolution);
  
  Affine3f model_viewer_pose = pose_estimate.transformation.inverse()*scene.getTransformationToWorldSystem();
  model.createView(model_viewer_pose, angular_resolution, object_image);
  scene.getSubImage(object_image.getImageOffsetX(), object_image.getImageOffsetY(), object_image.width,
                    object_image.height, combine_pixels, scene_sub_image);
  
  object_image.change3dPointsToLocalCoordinateFrame();
  scene_sub_image.change3dPointsToLocalCoordinateFrame();
}

float
FalsePositivesFilter::getViewSimulationNormalsScore(const RangeImage& object_image, const RangeImage& scene_sub_image,
                                                    RangeImage* score_image) const
{
  int search_radius = 2;
  int no_of_nearest_neighbors = pow(search_radius+1, 2);

  float score=0.0f, weight=0.0f;
  int width=object_image.width, height=object_image.height;
  
  if (score_image != NULL)
    *score_image = object_image;
  
  for (int x=0; x<width; ++x)
  {
    for (int y=0; y<height; ++y)
    {
      const PointWithRange& object_pixel = object_image.getPoint(x, y);
      if (!pcl_isfinite(object_pixel.range))
        continue;
      const PointWithRange& model_point = object_image.getPoint(x,y);
      Vector3f normal1, normal2;
      if (!object_image.getNormalForClosestNeighbors(x, y, search_radius, model_point, no_of_nearest_neighbors, normal1))
        continue;
      scene_sub_image.getNormalForClosestNeighbors(x, y, search_radius, model_point, no_of_nearest_neighbors, normal2);
      
      float pixel_score = pow(std::max(normal1.dot(normal2), 0.0f), 2);
      
      score += pixel_score;
      weight += 1.0f;
      if (score_image != NULL)
      {
        score_image->getPoint(x,y).range = pixel_score;
      }
    }
  }
  score /= weight;
  //cout << PVARN(score);
  
  return score;
}

float
FalsePositivesFilter::getViewSimulationScore(const RangeImage& object_image, const RangeImage& scene_sub_image,
                                             float max_dist_error, float& border_score, RangeImage* score_image) const
{
  float weight_for_see_through_points = parameters_.weight_for_see_through_points;
  
  //cout << PVARN(object_image)<<PVARN(scene_sub_image)<<PVARN(max_dist_error);
  int search_radius = parameters_.pixel_search_radius_for_view_simulation_score;
  int search_radius_for_normals = std::max(search_radius, 2);
  int no_of_nearest_neighbors = pow(search_radius_for_normals+1, 2);
  float max_pixel_distance_reciprocal = 1.0f/(sqrtf(2.0f)*float(search_radius));
  float max_dist_error_reciprocal=1.0f/max_dist_error;
  
  float score=0.0f, weight=0.0f, border_weight=0.0f;
  border_score = 0.0f;
  int width=object_image.width, height=object_image.height;
  
  if (score_image != NULL)
    *score_image = object_image;
  
  for (int x=0; x<width; ++x)
  {
    for (int y=0; y<height; ++y)
    {
      const PointWithRange& object_pixel = object_image.getPoint(x, y);
      if (!pcl_isfinite(object_pixel.range))
        continue;
      
      float pixel_score = 0.0f;
      
      bool is_border_point = false;
      if (parameters_.check_borders_for_view_simulation)
        for (int x2=x-1; x2<=x+1; ++x2)
          for (int y2=y-1; y2<=y+1; ++y2)
            if (object_image.isMaxRange(x2, y2))
              is_border_point = true;

      float best_range_dist = -INFINITY;
      const PointWithRange* maximum_range_point=NULL, * best_scene_point = NULL;
      for (int x2=max(0, x-search_radius); x2<=min(x+search_radius, width-1); ++x2)
      {
        for (int y2=max(0, y-search_radius); y2<=min(y+search_radius, height-1); ++y2)
        {
          const PointWithRange& scene_neighbor_pixel= scene_sub_image.getPoint(x2, y2);
          if (maximum_range_point==NULL || scene_neighbor_pixel.range>maximum_range_point->range)
            maximum_range_point = &scene_neighbor_pixel;
          float range_dist = object_pixel.range - scene_neighbor_pixel.range;
          best_range_dist = max(best_range_dist, range_dist);
          if (fabsf(range_dist) >= max_dist_error)
            continue;
          float pixel_distance_factor = 1.0f - 0.5f*hypot(x2-x, y2-y)*max_pixel_distance_reciprocal;
          //float distance = euclideanDistance(object_pixel, scene_neighbor_pixel);
          //if (distance < max_dist_error)
          //float current_pixel_score = pixel_distance_factor*(1.0f - distance*max_dist_error_reciprocal);
          float current_pixel_score = pixel_distance_factor*(1.0f - fabsf(range_dist)*max_dist_error_reciprocal);
          //current_pixel_score = 1.0f - powf(1.0f-current_pixel_score, 2);  // Do some scaling
          if (current_pixel_score > pixel_score)
          {
            pixel_score = current_pixel_score;
            best_scene_point = &scene_neighbor_pixel;
          }
        }
      }
      bool is_see_through = best_range_dist < -max_dist_error;
      
      float border_pixel_score = 0.0f;
      if (!is_border_point)
      {
        if (parameters_.check_normals_for_view_simulation && pixel_score > 0.0f)
        {
          Vector3f normal1, normal2;
          if (!object_image.getNormalForClosestNeighbors(x, y, search_radius_for_normals,
                                                         object_pixel, no_of_nearest_neighbors, normal1))
            continue;
          scene_sub_image.getNormalForClosestNeighbors(x, y, search_radius_for_normals, object_pixel,
                                                       no_of_nearest_neighbors, normal2);
          float normal_score = pow(std::max(normal1.dot(normal2), 0.0f), 1);
          //cout << "Score: "<<pixel_score<<", normal score: "<<normal_score<<"\n";
          
          pixel_score *= normal_score;
          
          //if (score_image != NULL)
          //{
            //score_image->getPoint(x,y).range = pixel_score;
          //}
        }
        
        score += pixel_score;
        weight += (is_see_through ? weight_for_see_through_points : 1.0f);
      }
      else
      {
        if (best_scene_point!=NULL && scene_sub_image.getImpactAngle(*best_scene_point, *maximum_range_point) <
                                                                                               deg2rad(15.0f))
        {
          border_pixel_score = pixel_score;
          border_score += border_pixel_score;
        }
        border_weight += 1.0f;
      }
      
      if (score_image != NULL)
      {
        score_image->getPoint(x,y).range = (is_see_through ? NAN :
                                             (is_border_point&&border_pixel_score==0.0f ? -INFINITY : pixel_score));
      }
    }
  }
  score /= weight;
  border_score /= border_weight;
  //cout << PVARC(score) << PVARN(border_score);
  
  return score;
}

float
FalsePositivesFilter::getBorderValidationPointScore(const std::vector<Eigen::Vector3f>& border_points,
                                                    const Eigen::Affine3f& pose, const RangeImage& scene,
                                                    float max_dist_error, float min_score) const
{
  int search_radius = parameters_.pixel_search_radius_for_validation_points_score;
  int min_no_of_tested_points = parameters_.min_no_of_tested_validation_points;
  float min_validation_point_score_to_go_on = parameters_.min_validation_point_score_to_go_on_factor*min_score;
  float max_pixel_distance_reciprocal = 1.0f/(sqrtf(2.0f)*float(search_radius));
  float score = 0.0f, weight=0.0f;
  float max_dist_error_squared = max_dist_error*max_dist_error;
  float max_dist_error_reciprocal=1.0f/max_dist_error;
  
  for (unsigned int points_idx=0; points_idx<border_points.size(); ++points_idx)
  {
    const Eigen::Vector3f& validation_point = border_points[points_idx];
    PointWithRange vp;
    vp.getVector3fMap() = pose * validation_point;
    
    int x, y;
    scene.getImagePoint(vp.getVector3fMap(), x, y);
    
    const PointWithRange* best_found_point=NULL, * maximum_range_point=NULL;
    float pixel_score = 0.0f;
    for (int x2=x-search_radius; x2<=x+search_radius; ++x2)
    {
      for (int y2=y-search_radius; y2<=y+search_radius; ++y2)
      {
        if (!scene.isInImage(x2, y2))
          continue;
        const PointWithRange& scene_point = scene.getPoint(x2, y2);
        if (std::isinf(scene_point.range))
        {
          if (scene_point.range < 0.0f)
            continue;
          maximum_range_point = &scene_point;
        }
        
        if (maximum_range_point==NULL || maximum_range_point->range<scene_point.range)
          maximum_range_point = &scene_point;
        
        float distance_squared = squaredEuclideanDistance(scene_point, vp);
        if (distance_squared > max_dist_error_squared)
          continue;
        float pixel_distance = hypot(x2-x, y2-y);
        float pixel_distance_factor = 1.0f - 0.25f*pixel_distance*max_pixel_distance_reciprocal;
        //cout << PVARC(pixel_distance)<<PVARN(pixel_distance_factor);
        float distance = sqrtf(distance_squared);
        float current_pixel_score = 1.0f - distance*max_dist_error_reciprocal;
        current_pixel_score *= pixel_distance_factor;
        //current_pixel_score = powf(current_pixel_score, 0.3f);  // Do some scaling
        current_pixel_score = 1.0f - powf(1.0f - current_pixel_score, 2);  // Do some scaling
        //cout << PVARC(distance)<<PVARN(current_pixel_score);
        
        if (current_pixel_score > pixel_score)
        {
          pixel_score = current_pixel_score;
          best_found_point = &scene_point;
        }
      }
    }
    if (pixel_score > 0.0f)
    {
      float impact_angle = scene.getImpactAngle(*best_found_point, *maximum_range_point);
      //cout << PVARAN(impact_angle);
//      if (impact_angle > deg2rad(15.0f))
      if (impact_angle > deg2rad(15.0f))
        pixel_score = 0.0f;
    }
    
    float current_weight = 1.0f;
    weight += current_weight;
    score += current_weight * (pixel_score-score)/weight;
    //cout << points_idx<<": "<<PVARN(score);
    
    if ((int)points_idx>=min_no_of_tested_points && score<min_validation_point_score_to_go_on)
      break;
  }
  
  
  return score;
}
float
FalsePositivesFilter::getSurfaceValidationPointScore(const std::vector<Eigen::Vector3f>& surface_points,
                                                     const Eigen::Affine3f& pose, const RangeImage& scene,
                                                     float max_dist_error, float min_score) const
{
  int search_radius = parameters_.pixel_search_radius_for_validation_points_score;
  // We give a higher weight to points that have a higher range in the actual scan than predicted,
  // since this means we would have seen through our object
  float weight_for_see_through_points = parameters_.weight_for_see_through_points;
  int min_no_of_tested_points = parameters_.min_no_of_tested_validation_points;
  float min_validation_point_score_to_go_on = parameters_.min_validation_point_score_to_go_on_factor*min_score;
  float max_pixel_distance_reciprocal = 1.0f/(sqrtf(2.0f)*float(search_radius));
  float score = 0.0f, weight=0.0f;
  float max_dist_error_squared = max_dist_error*max_dist_error;
  float max_dist_error_reciprocal=1.0f/max_dist_error;
  Vector3f sensor_pos = scene.getSensorPos();
  
  for (unsigned int points_idx=0; points_idx<surface_points.size(); ++points_idx)
  {
    const Eigen::Vector3f& validation_point = surface_points[points_idx];
    Eigen::Vector3f transformed_point = pose * validation_point;
    PointWithRange vp;
    vp.x=transformed_point[0]; vp.y=transformed_point[1]; vp.z=transformed_point[2];
    vp.range = (sensor_pos-transformed_point).norm();
    float max_range_in_scene = vp.range+max_dist_error;
    
    int x, y;
    scene.getImagePoint(transformed_point, x, y);
    
    float pixel_score = 0.0f;
    bool all_checked_pixel_too_far_away = true;
    for (int x2=x-search_radius; x2<=x+search_radius; ++x2)
    {
      for (int y2=y-search_radius; y2<=y+search_radius; ++y2)
      {
        if (!scene.isInImage(x2, y2))
        {
          all_checked_pixel_too_far_away = false;
          continue;
        }
        const PointWithRange& scene_point = scene.getPoint(x2, y2);
        if (!pcl_isfinite(scene_point.range))
        {
          if (scene_point.range < 0.0f)
            all_checked_pixel_too_far_away = false;
          continue;
        }
        if (scene_point.range < max_range_in_scene)
          all_checked_pixel_too_far_away = false;
        
        float distance_squared = squaredEuclideanDistance(scene_point, vp);
        if (distance_squared > max_dist_error_squared)
          continue;
        
        float pixel_distance = hypot(x2-x, y2-y);
        float pixel_distance_factor = 1.0f - 0.25f*pixel_distance*max_pixel_distance_reciprocal;
        //cout << PVARC(pixel_distance)<<PVARN(pixel_distance_factor);
        float distance = sqrtf(distance_squared);
        float current_pixel_score = 1.0f - distance*max_dist_error_reciprocal;
        current_pixel_score *= pixel_distance_factor;
        current_pixel_score = 1.0f - powf(1.0f-current_pixel_score, 2);  // Do some scaling
        //cout << PVARC(distance)<<PVARN(current_pixel_score);
        
        pixel_score = max(pixel_score, current_pixel_score);
      }
    }
    
    float current_weight = (all_checked_pixel_too_far_away ? weight_for_see_through_points : 1.0f);
    weight += current_weight;
    score += current_weight * (pixel_score-score)/weight;
    //cout << points_idx<<": "<<PVARN(score);
    
    if ((int)points_idx>=min_no_of_tested_points && score<min_validation_point_score_to_go_on)
      break;
  }
  
  return score;
}

void
FalsePositivesFilter::validationPoints(const RangeImage& scene, const ObjectModel& model, PoseEstimates& pose_estimates,
                                       float min_score, float max_dist_error) const
{
  //MEASURE_FUNCTION_TIME;
  //cout << __PRETTY_FUNCTION__<<" called with "<<PVARC(min_score)<<PVARN(max_dist_error);
  
  if (max_dist_error < 0.0f)
    max_dist_error = 0.2*model.getRadius();
  //PoseEstimates updated_pose_estimates;
  
  #pragma omp parallel for default(shared) num_threads(parameters_.max_no_of_threads) schedule(dynamic, 10)
  for (unsigned int pose_estimate_idx=0; pose_estimate_idx<pose_estimates.size(); ++pose_estimate_idx)
  {
    PoseEstimate& pose_estimate = pose_estimates[pose_estimate_idx];
    Affine3f model_viewer_pose = pose_estimate.transformation.inverse()*scene.getTransformationToWorldSystem();
    int view_idx = model.getClosestValidationPointViewIdx(model_viewer_pose);
    const ObjectModel::ValidationPoints& validation_points = model.getValidationPoints()[view_idx];
    
    float surface_score = getSurfaceValidationPointScore(validation_points.surface, pose_estimate.transformation,
                                                         scene, max_dist_error, min_score);
    if (surface_score < min_score) {
      pose_estimate.score = surface_score;
      //cout << "Rejecting a result with surface score "<<surface_score<<"(min score is "<<min_score<<").\n";
      continue;
    }
    float score = surface_score;
    
    if (!validation_points.border.empty() && parameters_.check_borders_for_validation_points)
    {
      float border_score = getBorderValidationPointScore(validation_points.border, pose_estimate.transformation,
                                                         scene, max_dist_error, min_score);
      //if (border_score < min_score)
      //{
        //continue;
      //}
      
      //cout << "Accepting a result with border score "<<border_score<<"(min score is "<<min_score<<").\n";
      //score = 0.5f*(surface_score+border_score);
      score = std::min(surface_score, border_score);
    }
    
    //if (score < min_score)
    //{
      //continue;
    //}
    
    //cout << "Result has score "<<score<<".\n";
    
    //cout << "Used view: "<<view_idx<<"\n";
    //#pragma omp critical
    //{
      //updated_pose_estimates.push_back(pose_estimate);
      //updated_pose_estimates.back().score = score;
    //}
    pose_estimate.score = score;
  }
  
  //cout << updated_pose_estimates.size()<<"/"<<pose_estimates.size()<<" object poses of model \""<<model.getName()
  //     << "\" left after validation point filtering.";
  //int maxNoOfPrintedScores = 10;
  //cout << (pose_estimates.empty() ? "" : " Scores are ");
  //for (unsigned int i=0; i<std::min(maxNoOfPrintedScores, int(pose_estimates.size())); ++i)
    //cout << (i>0?", ":"") << pose_estimates[i].score; cout<<".";
  //cout << "\n";
  
  //pose_estimates = updated_pose_estimates;
}

void
FalsePositivesFilter::keepBest(PoseEstimates& pose_estimates, int max_no_of_results) const
{
  //MEASURE_FUNCTION_TIME;
  
  std::sort(pose_estimates.begin(), pose_estimates.end(), PoseEstimate::IsBetter());
  pose_estimates.resize(max_no_of_results);
  //while ((int)pose_estimates.size() > max_no_of_results)
    //pose_estimates.pop_back();
  
  //cout << "Kept only "<<pose_estimates.size()<<" best matches.";
  //cout << (pose_estimates.empty() ? "" : " Scores are ");
  //for (unsigned int i=0; i<pose_estimates.size(); ++i) cout << (i>0?", ":"") << pose_estimates[i].score; cout<<".";
  //cout << "\n";
  
}

void
FalsePositivesFilter::viewSimulation(const RangeImage& scene, const ObjectModel& model, PoseEstimates& pose_estimates,
                                     float min_score, float max_dist_error, std::vector<RangeImage>* results) const
{
  //MEASURE_FUNCTION_TIME;

  if (max_dist_error < 0.0f)
    max_dist_error = 0.2*model.getRadius();
  //PoseEstimates updated_pose_estimates;
  //cout << PVARN(parameters_.max_no_of_threads);
  
  #pragma omp parallel for num_threads(parameters_.max_no_of_threads) default(shared) schedule(dynamic, 1)
  for (int pose_estimate_idx=0; pose_estimate_idx<pose_estimates.size(); ++pose_estimate_idx)
  {
    PoseEstimate& pose_estimate = pose_estimates[pose_estimate_idx];
    pose_estimate.score = 0.0f;
    
    RangeImage object_image, scene_sub_image, score_image, score_image2;
    getRangeImagesForComparison(scene, model, pose_estimate, object_image, scene_sub_image);
    float score_borders;
    float score = getViewSimulationScore(object_image, scene_sub_image, max_dist_error, score_borders,
                                         (results!=NULL ? &score_image : NULL));
    //cout << "Result has score "<<score<<".\n";
    
    //if (score < min_score)
    //{
      //continue;
    //}
    //float score_weight = 1.0f;
    
    if (parameters_.check_borders_for_view_simulation)
    {
      score = std::min(score, score_borders);
      //if (score_borders < min_score)
      //{
        //continue;
      //}
      //score += score_borders;
      //score_weight += 1.0f;
    }
    
    //if (parameters_.check_normals_for_view_simulation)
    //{
      //float score_normals = getViewSimulationNormalsScore(object_image, scene_sub_image,
                                                          //(results!=NULL ? &score_image2 : NULL));
      //if (score_normals < min_score)
      //{
        //continue;
      //}
      //score += score_normals;
      //score_weight += 1.0f;
    //}
    
    //score /= score_weight;
    pose_estimate.score = score;
    
    if (score>min_score && results!=NULL)
    {
      #pragma omp critical
      {
        results->push_back(object_image);
        results->push_back(scene_sub_image);
        results->push_back(score_image);
        results->push_back(score_image2);
      }
    }
  }
  
  //int maxNoOfPrintedScores = 10;
  //cout << (pose_estimates.empty() ? "" : " Scores are ");
  //for (unsigned int i=0; i<std::min(maxNoOfPrintedScores, int(pose_estimates.size())); ++i)
    //cout << (i>0?", ":"") << pose_estimates[i].score; cout<<".";
  //cout << "\n";

  //cout << updated_pose_estimates.size()<<"/"<<pose_estimates.size()<<" object poses of model \""<<model.getName()
       //<< "\" left after view simulation filtering.";
  //int maxNoOfPrintedScores = 10;
  //cout << (pose_estimates.empty() ? "" : " Scores are ");
  //for (unsigned int i=0; i<std::min(maxNoOfPrintedScores, int(pose_estimates.size())); ++i)
    //cout << (i>0?", ":"") << pose_estimates[i].score; cout<<".";
  //cout << "\n";
  
  //pose_estimates = updated_pose_estimates;
}

void
FalsePositivesFilter::maximumRange(const RangeImage& scene, const ObjectModel& model,
                                   PoseEstimates& pose_estimates, float maximum_range) const
{
  //MEASURE_FUNCTION_TIME;
  float maximum_range_squared = maximum_range*maximum_range;
  PoseEstimates updated_pose_estimates;
  Vector3f sensor_pos = scene.getSensorPos();
  for (unsigned int i=0; i<pose_estimates.size(); ++i)
  {
    const PoseEstimate& pose_estimate = pose_estimates[i];
    Vector3f model_center_in_world = pose_estimate.transformation * model.getCenter();
    if ((model_center_in_world-sensor_pos).squaredNorm() > maximum_range_squared)
      continue;
    updated_pose_estimates.push_back(pose_estimate);
  }
  
  cout << updated_pose_estimates.size()<<"/"<<pose_estimates.size()<<" object poses of model \""<<model.getName()
       << "\" left after filtering maximum ranges.\n";
  pose_estimates = updated_pose_estimates;
}

void
FalsePositivesFilter::similarPoses(const ObjectModel& model, PoseEstimates& pose_estimates) const
{
  //MEASURE_FUNCTION_TIME;
  float min_distance = 0.3*model.getRadius(),
        min_distance_squared = min_distance*min_distance;
  //cout << PVARN(min_distance);
  PoseEstimates updated_pose_estimates;
  for (unsigned int i=0; i<pose_estimates.size(); ++i)
  {
    const PoseEstimate& pose_estimate1 = pose_estimates[i];
    Vector3f center1 = pose_estimate1.transformation * model.getCenter();
    bool keep = true;
    for (unsigned int j=0; j<pose_estimates.size(); ++j)
    {
      if (i==j)
        continue;
      const PoseEstimate& pose_estimate2 = pose_estimates[j];
      if (pose_estimate2.score < pose_estimate1.score || (pose_estimate2.score == pose_estimate1.score && i<j))
        continue;
      Vector3f center2 = pose_estimate2.transformation * model.getCenter();
      float squared_distance = (center2-center1).squaredNorm();
      if (squared_distance >= min_distance_squared)
        continue;
      //cout << "Not keeping "<<i<<"("<<pose_estimate1.score<<"), since its distance to "<<j
      //     << "("<<pose_estimate2.score<<") is "<<sqrtf(squared_distance)<<".\n";
      //if (squared_distance==0.0f)
        //cout << "Identical results!\n";
      keep = false;
      break;
    }
    if (keep)
    {
      //cout << "Keeping "<<i<<"("<<pose_estimate1.score<<").\n";
      updated_pose_estimates.push_back(pose_estimate1);
    }
  }
  
  cout << updated_pose_estimates.size()<<"/"<<pose_estimates.size()<<" object poses of model \""<<model.getName()
       << "\" left after filtering of similar poses.";
  //cout << (pose_estimates.empty() ? "" : " Scores are ");
  //for (unsigned int i=0; i<updated_pose_estimates.size(); ++i)
  //  cout << (i>0?", ":"") << updated_pose_estimates[i].score;
  //cout<<".";
  cout << "\n";
  
  pose_estimates = updated_pose_estimates;
}

void
FalsePositivesFilter::sortAndApplyMinScore(PoseEstimates& pose_estimates, float min_score) const
{
  std::sort(pose_estimates.begin(), pose_estimates.end(), pcl::PosesFromMatches::PoseEstimate::IsBetter());
  for (unsigned int pose_estimate_idx=0; pose_estimate_idx<pose_estimates.size(); ++pose_estimate_idx)
    if (pose_estimates[pose_estimate_idx].score < min_score)
      pose_estimates.resize(pose_estimate_idx);
}

void
FalsePositivesFilter::doIcp(const RangeImage& scene, const ObjectModel& model,
                            PoseEstimates& pose_estimates, bool fast_icp) const
{
  #pragma omp parallel for default(shared) num_threads(parameters_.max_no_of_threads) schedule(dynamic, 10)
  for (unsigned int pose_estimate_idx=0; pose_estimate_idx<pose_estimates.size(); ++pose_estimate_idx)
  {
    PosesFromMatches::PoseEstimate& pose_estimate = pose_estimates[pose_estimate_idx];
    if (fast_icp)
    {
      pose_estimate.transformation = model.doFastICP(pose_estimate.transformation, scene,
                                                     parameters_.icp_pixel_search_radius,
                                                     parameters_.fast_icp_no_of_surface_points_to_use,
                                                     parameters_.fast_icp_no_of_border_points_to_use,
                                                     parameters_.icp_num_iterations,
                                                     parameters_.icp_max_distance_start,
                                                     parameters_.icp_max_distance_end);
    }
    else
    {
      pose_estimate.transformation = model.doSlowICP(pose_estimate.transformation, scene,
                                                     parameters_.icp_pixel_search_radius,
                                                     parameters_.icp_num_iterations,
                                                     parameters_.icp_max_distance_start,
                                                     parameters_.icp_max_distance_end);
    }
  }
}

void
FalsePositivesFilter::filterResults(const RangeImage& scene, const ObjectModel& model,
                                    PoseEstimates& pose_estimates) const
{
  cout << "Initial no of pose estimates: "<<pose_estimates.size()<<"\n";
  int maxNoOfPrintedScores = 10;
  
  if (parameters_.maximum_range_for_found_objects > 0.0f)
    maximumRange(scene, model, pose_estimates, parameters_.maximum_range_for_found_objects);
  

  if (parameters_.do_icp)
  {
    if (parameters_.do_validation_point_filtering)
    {
      double validation_point_score1_time = -getTime();
      float max_validation_point_error_before_icp = parameters_.max_validation_point_error_before_icp_factor *
                                                    parameters_.max_validation_point_error,
            min_validation_point_score_before_icp = parameters_.min_validation_point_score_before_icp_factor *
                                                    parameters_.min_validation_point_score;
      validationPoints(scene, model, pose_estimates,
                       min_validation_point_score_before_icp,
                       max_validation_point_error_before_icp);
      sortAndApplyMinScore(pose_estimates, min_validation_point_score_before_icp);
      if (parameters_.do_pose_similarity_filtering)
        similarPoses(model, pose_estimates);
      //poseEstimates.resize(std::min(int(poseEstimates.size()), parameters.maxNoOfRemainingPoseEstimatesForICP));
      //time_pose_scoring += get_time();
      validation_point_score1_time += getTime();
      cout << PVARN(validation_point_score1_time);
      cout << "No of pose estimates after first validation point scoring: "<<pose_estimates.size()<<"\n";
      
      cout << (pose_estimates.empty() ? "" : " Scores are ");
      for (unsigned int i=0; i<std::min(maxNoOfPrintedScores, int(pose_estimates.size())); ++i)
        cout << (i>0?", ":"") << pose_estimates[i].score; cout<<".";
      cout << "\n";
    }
    
    double icp_time = -getTime();
    doIcp(scene, model, pose_estimates, true);
    icp_time += getTime();
    cout << PVARN(icp_time);
  }
  
  if (parameters_.do_validation_point_filtering)
  {
    double validation_point_score2_time = -getTime();
    float max_validation_point_error = parameters_.max_validation_point_error,
          min_validation_point_score = parameters_.min_validation_point_score;
    validationPoints(scene, model, pose_estimates, min_validation_point_score, max_validation_point_error);
    sortAndApplyMinScore(pose_estimates, min_validation_point_score);
    if (parameters_.do_pose_similarity_filtering)
      similarPoses(model, pose_estimates);
    validation_point_score2_time += getTime();
    cout << PVARN(validation_point_score2_time);
    cout << "No of pose estimates after second validation point scoring: "<<pose_estimates.size()<<"\n";
    
    cout << (pose_estimates.empty() ? "" : " Scores are ");
    for (unsigned int i=0; i<std::min(maxNoOfPrintedScores, int(pose_estimates.size())); ++i)
      cout << (i>0?", ":"") << pose_estimates[i].score; cout<<".";
    cout << "\n";
  }

  if (parameters_.do_view_simulation_filtering)
  {
    double view_simulation_time = -getTime();
    float max_view_simulation_point_error = parameters_.max_validation_point_error,
          min_view_simulation_score = parameters_.min_view_simulation_score;
    viewSimulation(scene, model, pose_estimates, min_view_simulation_score,
                   max_view_simulation_point_error, view_simulation_images_);
    sortAndApplyMinScore(pose_estimates, parameters_.min_view_simulation_score);
    view_simulation_time += getTime();
    cout << PVARN(view_simulation_time);
    cout << "No of pose estimates after view simulation scoring: "<<pose_estimates.size()<<"\n";
    
    cout << (pose_estimates.empty() ? "" : " Scores are ");
    for (unsigned int i=0; i<std::min(maxNoOfPrintedScores, int(pose_estimates.size())); ++i)
      cout << (i>0?", ":"") << pose_estimates[i].score; cout<<".";
    cout << "\n";
  }
  
  //double icp2_time = -getTime();
  //doIcp(scene, model, pose_estimates, false);
  //icp2_time += getTime();
  //cout << PVARN(icp2_time);
  
  //if (parameters_.do_view_simulation_filtering)
  //{
    //double view_simulation2_time = -getTime();
    //float max_view_simulation_point_error = parameters_.max_validation_point_error,
          //min_view_simulation_score = parameters_.min_view_simulation_score;
    //viewSimulation(scene, model, pose_estimates, min_view_simulation_score,
                   //0.5f*max_view_simulation_point_error, view_simulation_images_);
    //sortAndApplyMinScore(pose_estimates, parameters_.min_view_simulation_score);
    //view_simulation2_time += getTime();
    //cout << PVARN(view_simulation2_time);
    //cout << "No of pose estimates after second view simulation scoring: "<<pose_estimates.size()<<"\n";
    
    //cout << (pose_estimates.empty() ? "" : " Scores are ");
    //for (unsigned int i=0; i<std::min(maxNoOfPrintedScores, int(pose_estimates.size())); ++i)
      //cout << (i>0?", ":"") << pose_estimates[i].score; cout<<".";
    //cout << "\n";
  //}
    
  //if (print_timings)
  //{
    //std::multimap<double, string> timings;
    //double time_sum=0.0, time_sum_constant=0.0;
    //#define ADD_TIMING(time_variable, dependent_on_database_size) \
    //{\
      //timings.insert(std::make_pair(time_variable, #time_variable)); \
      //time_sum += time_variable;\
      //if (!dependent_on_database_size) \
        //time_sum_constant += time_variable; \
    //}
    //ADD_TIMING(time_scene_range_image_creation, false);
    //ADD_TIMING(time_interest_point_extraction, false);
    //ADD_TIMING(time_feature_extraction, false);
    //ADD_TIMING(time_feature_matching, true);
    //ADD_TIMING(time_pose_estimation, true);
    //ADD_TIMING(time_false_positives_removal, true);
    //if (do_icp)
      //ADD_TIMING(time_icp, true);
    //if (do_view_simulation_filtering)
      //ADD_TIMING(time_view_simulation, true);
    //#undef ADD_TIMING
    //cout << "\nTIMINGS:\n========\n";
    //for (std::multimap<double, string>::const_reverse_iterator it=timings.rbegin(); it!=timings.rend(); ++it)
      //cout << it->second<<" = "<<it->first<<"s\n";
    //cout << "\nComplete process took "<<time_sum<<"s ("<<time_sum_constant<<"s for scan analysis, "
    //     << time_sum-time_sum_constant<<"s for object search).\n\n";
  //}
  
}

} // namespace end