object_recognition.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 <iostream>
using namespace std;

#include <narf_recognition/object_recognition_helper.h>
#include <boost/thread/thread.hpp>
#include "pcl/console/parse.h"

#include "pcl/visualization/range_image_visualizer.h"
#include "pcl/visualization/pcl_visualizer.h"

#include "Eigen/Geometry"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"

#include "pcl/io/pcd_io.h"
#include "pcl/range_image/range_image.h"
#include "pcl/features/narf.h"
//#include "object_recognition/object_model_list.h"
#include "pcl/common/point_correspondence.h"
#include "pcl/common/poses_from_matches.h"
#include "pcl/features/range_image_border_extractor.h"
#include "pcl/keypoints/narf_keypoint.h"
#include "pcl/range_image/range_image_planar.h"
#include "sensor_msgs/Image.h"
#include "pcl/common/file_io.h"
//#include "stereo_msgs/DisparityImage.h"
//#include "sensor_msgs/CameraInfo.h"

#include "pcl/common/common_headers.h"

using namespace pcl;
using namespace pcl::visualization;

ObjectRecognitionHelper object_recognition_helper;
ObjectRecognitionHelper::Parameters& orh_params = object_recognition_helper.getParameters ();
FalsePositivesFilter& false_positives_filter = object_recognition_helper.getFalsePositivesFilter ();
FalsePositivesFilter::Parameters& fpf_params = false_positives_filter.getParameters ();

int& max_no_of_threads = orh_params.max_no_of_threads;
bool& use_rotation_invariance = orh_params.use_rotation_invariance;
float& angular_resolution = orh_params.angular_resolution;
RangeImage::CoordinateFrame& coordinate_frame = orh_params.coordinate_frame;
float& max_descriptor_distance = orh_params.max_descriptor_distance;
bool& set_unseen_to_max_range = orh_params.set_unseen_to_max_range;
bool single_view_model = orh_params.single_view_model;

float noise_level_factor = 0.1;    // Factor regarding average model radius
float support_size_factor = 0.5f;  // Factor regarding average model radius

bool show_model_views = 1;
bool show_view_simulation_results = 0;
bool show_validation_points = 0;
bool show_found_objects = 1;
bool show_feature_match_positions = 0;
bool show_interest_points = 0;
bool print_timings = true;

void printUsage (const char* progName)
{
  cout << "\n\nUsage: "<<progName<<" [options] [scene.pcd] <model.pcl> [model_2.pcl] ... [model_n.pcl]\n\n"
       << "Options:\n"
       << "-------------------------------------------\n"
       //<< "-s           source used to create the scene range image:\n"
       //<<              "  -1 from disk (default), 0 PointCloud2 msg, 1 depth image msg, 2 disparity image msg.\n"
       //<<              "  don't forget to set input:=... for options [0,1,2], input2:=<camera_info> for option 1\n"
       //<<              "  or to set the scene point cloud for option -1.\n"
       << "-i <bool>    do ICP (default "<< (int)fpf_params.do_icp<<").\n"
       << "-r <float>   angular resolution in degrees (default "<<rad2deg (angular_resolution)<<")\n"
       << "-v <float>   min validation point score (default "<<fpf_params.min_validation_point_score<<")\n"
       << "-c <int>     coordinate frame (default "<<coordinate_frame<<")\n"
       << "-b           activate single view model, which tells the system that the given model point cloud\n"
       <<             " is not of the complete object, but only one view of it - no view sampling will be performed.\n"
       << "-o           use orientation invariant version\n"
       << "-f <float>   do view simulation filtering with this minimum score.\n"
       << "-x <float>   use maximum range for found objects.\n"
       << "-m           set unseen pixels to max range\n"
       << "-d <float>   set the maximum descriptor distance (default: "<<max_descriptor_distance<<")\n"
       << "-t <int>     set the maximum number of threads for parallelization (default: "<<max_no_of_threads<<")\n"
       << "-h           this help\n"
       << "\n\n";
}

//sensor_msgs::PointCloud2ConstPtr cloud_msg;
//PointCloud<PointXYZ> point_cloud;
sensor_msgs::ImageConstPtr depth_image_msg;
//stereo_msgs::DisparityImageConstPtr disparity_image_msg;
//sensor_msgs::CameraInfoConstPtr camera_info_msg;
boost::mutex m;
bool got_new_data=false;
//void
  //disparity_image_msg_cb (const stereo_msgs::DisparityImageConstPtr& msg)
//{
  //m.lock ();
  //disparity_image_msg = msg;
  //m.unlock ();
  //got_new_data = true;
//}

//void
  //depth_image_msg_cb (const sensor_msgs::ImageConstPtr& msg)
//{
  //m.lock ();
  //std::cout << "Received depth image of size "<<msg->width<<"x"<<msg->height<<".\n";
  //depth_image_msg = msg;
  //m.unlock ();
  //got_new_data = camera_info_msg;  // Only accept message if we already have the camera info
//}

//void
  //cloud_msg_cb (const sensor_msgs::PointCloud2ConstPtr& msg)
//{
  //m.lock ();
  //cloud_msg = msg;
  //far_ranges.points.clear ();
  //m.unlock ();
  //got_new_data = true;
//}

//void
  //camera_info_msg_cb (const sensor_msgs::CameraInfoConstPtr& msg)
//{
  //m.lock ();
  //std::cout << "Received camera info: " << "width="<<msg->width<<", height="<<msg->height<<", "
                                        //<< "center_x="<<msg->P[2]<<", center_y="<<msg->P[6]<<", "
                                       //<<"fl_x="<<msg->P[0]<<", fl_y="<<msg->P[5]<<".\n";
  //camera_info_msg = msg;
  //m.unlock ();
//}

int main (int argc, char** argv)
{
  // --------------------------------------
  // -----Parse Command Line Arguments-----
  // --------------------------------------
  int range_image_source = -1;
  orh_params.view_sampling_first_layer = 0;
  orh_params.view_sampling_last_layer = 0;
  
  // Read command line arguments.
  for (char c; (c = getopt (argc, argv, "bmx:i:f:hr:c:v:od:t:")) != -1; ) {
    switch (c) {
      case 't':
        max_no_of_threads = strtol (optarg, NULL, 0);
        cout << PVARN (max_no_of_threads);
        break;
      case 'i':
        fpf_params.do_icp = (bool)strtol (optarg, NULL, 0);
        cout << "Will "<< (fpf_params.do_icp?"":"not ")<<"use ICP.\n";
        break;
      //case 's':
        //range_image_source = (bool)strtol (optarg, NULL, 0);
        //cout << PVARN (range_image_source);
        //break;
      case 'b':
      {
        single_view_model = true;
        cout << "Setting single_view_model to true.\n";
        break;
      }
      case 'o':
      {
        use_rotation_invariance = true;
        cout << "Using rotational invariant version of the features.\n";
        break;
      }
      case 'r':
      {
        angular_resolution = deg2rad (strtod (optarg, NULL));
        if (angular_resolution <= 0.0)
          cerr << "Angular resolution "<<optarg<<" invalid.\n";
        else
          cout << "Setting angular resolution to "<<rad2deg (angular_resolution)<<".\n";
        break;
      }
      case 'v':
      {
        fpf_params.do_validation_point_filtering = true;
        fpf_params.min_validation_point_score = strtod (optarg, NULL);
        cout << PVARN (fpf_params.min_validation_point_score);
        break;
      }
      case 'x':
      {
        fpf_params.maximum_range_for_found_objects = strtod (optarg, NULL);
        cout << PVARN (fpf_params.maximum_range_for_found_objects);
        break;
      }
      case 'd':
      {
        max_descriptor_distance = strtod (optarg, NULL);
        cout << PVARN (max_descriptor_distance);
        break;
      }
      case 'f':
      {
        fpf_params.do_view_simulation_filtering = true;
        fpf_params.min_view_simulation_score = strtod (optarg, NULL);
        cout << PVARN (fpf_params.min_view_simulation_score);
        break;
      }

      case 'c':
      {
        coordinate_frame = (RangeImage::CoordinateFrame)strtol (optarg, NULL, 0);
        cout << "Using coordinate frame "<<coordinate_frame<<".\n";
        break;
      }
      case 'm':
      {
        set_unseen_to_max_range = true;
        cout << "Unseen pixels will be considered max ranges.\n";
        break;
      }
      case 'h':
      default:
        printUsage (argv[0]);
        exit (0);
    }
  }
  
  vector<RangeImage> view_simulation_results;
  if (show_view_simulation_results)
    false_positives_filter.setViewSimulationImagesPtr (&view_simulation_results);
  
  // -----------------------
  // -----Read pcd file-----
  // -----------------------
  //int model_filename_idx = (live_scenes ? optind : optind+1);
  int model_filename_idx = optind+1;
  if (model_filename_idx >= argc)
  {
    cout << "\nNo *.pcd file for model given.\n\n";
    exit (0);
  }
  
  vector<string> model_filenames;
  for (; model_filename_idx<argc; ++model_filename_idx)
    model_filenames.push_back (argv[model_filename_idx]);
  ObjectModelList& object_models = object_recognition_helper.getIntermediateElements ().object_models;
  if (!object_recognition_helper.addObjectModelsFromPcdFiles (model_filenames))
  {
    printUsage (argv[0]);
    exit (1);
  }
  
  float average_model_radius = object_models.getAverageModelRadius ();
  float support_size = support_size_factor * average_model_radius;
  float noise_level = noise_level_factor * average_model_radius;
  cout << "\n-----\n"
       << "The average object model radius is "<<average_model_radius<<"m.\n"
       << "  => using support size "<<support_size<<"m.\n"
       << "  => using noise level "<<noise_level<<"m.\n"
       << "-----\n\n";
  orh_params.support_size = support_size;
  orh_params.noise_level = noise_level;
  
  object_recognition_helper.extractModelFeatures ();
  std::vector<Narf*>& database_features = object_recognition_helper.getIntermediateElements ().database_features;
  std::vector<ObjectModelList::FeatureSource>& database_feature_sources = 
    object_recognition_helper.getIntermediateElements ().database_feature_sources;
  
  // ------------------------------------------------------------------
  
  if (optind >= argc)
  {
    cout << "\nNo *.pcd file for scene given.\n\n";
    printUsage (argv[0]);
    exit (0);
  }
  std::string scene_filename = argv[optind];
  
  if (!object_recognition_helper.createRangeImageFromPcdFile (scene_filename))
  {
    cerr << "\n-----\nRange image creation failed.\n\n";
    exit (1);
  }
  got_new_data = true;
  
  vector<string> point_clouds_in_viewer;
  RangeImageVisualizer scene_range_image_widget ("scene range image");
  vector<RangeImageVisualizer*> view_simulation_result_widgets;
  vector<RangeImageVisualizer*> interest_points_widgets;
  vector<RangeImageVisualizer*> range_image_widgets;
  
  PCLVisualizer viewer ("3D Viewer");
  viewer.setBackgroundColor (1, 1, 1);

  while (!viewer.wasStopped ()) // && (node_handle==NULL || node_handle->ok ()))
  {
    viewer.spinOnce (100);
    scene_range_image_widget.spinOnce ();
    for (size_t i=0; i<view_simulation_result_widgets.size (); ++i)
      view_simulation_result_widgets[i]->spinOnce ();
    for (size_t i=0; i<interest_points_widgets.size (); ++i)
      interest_points_widgets[i]->spinOnce ();
    for (size_t i=0; i<range_image_widgets.size (); ++i)
      range_image_widgets[i]->spinOnce ();


    pcl_sleep (0.01);

    if (!got_new_data)
      continue;
    got_new_data = false;
    cout << "Got new data.\n";
    
    object_recognition_helper.extractSceneFeatures ();
    object_recognition_helper.extractPoseEstimates ();
    std::vector<PosesFromMatches::PoseEstimatesVector>& pose_estimates_per_model = 
      object_recognition_helper.filterFalsePositives ();
    
    object_recognition_helper.printTimings ();
    
    
    
    NarfKeypoint& interest_point_detector_scene =
      object_recognition_helper.getIntermediateElements ().interest_point_detector_scene;
    
    boost::shared_ptr<RangeImage>& scene_range_image_ptr =
      object_recognition_helper.getIntermediateElements ().scene_range_image_ptr;
    RangeImage& scene_range_image = *scene_range_image_ptr;   
   
    
    // Visualize view simulation result
    while (!view_simulation_result_widgets.empty ())
    {
      delete view_simulation_result_widgets.back ();
      view_simulation_result_widgets.pop_back ();
    }
    for (unsigned int i=0; i<view_simulation_results.size (); i+=4)
    {
      float min_range, max_range;
      view_simulation_results[i].getMinMaxRanges (min_range, max_range);
      //cout << PVARC (min_range)<<PVARN (max_range);
      stringstream ss;  ss << setfill ('0') << setw (3) << (i/4)+1;
      view_simulation_result_widgets.push_back (RangeImageVisualizer::getRangeImageWidget (view_simulation_results[i+1],
                                               min_range, max_range, true, ss.str ()+".1: real view"));
      view_simulation_result_widgets.push_back (RangeImageVisualizer::getRangeImageWidget (view_simulation_results[i],
                                               min_range, max_range, true, ss.str ()+".2: simulated view"));
      view_simulation_result_widgets.push_back (RangeImageVisualizer::getRangeImageWidget (view_simulation_results[i+2],
                                               0.0f, 1.0f, true, ss.str ()+".3: scores"));
      if (!view_simulation_results[i+3].points.empty ())
        view_simulation_result_widgets.push_back (RangeImageVisualizer::getRangeImageWidget (view_simulation_results[i+3],
                                                 0.0f, 1.0f, true, ss.str ()+".4: scores_normals"));
    }
    
    vector<boost::shared_ptr<ObjectModel::PointCloudType> > objects_in_scene;  // For visualization
    for (unsigned int model_idx=0; model_idx<object_models.size (); ++model_idx)
    {
      PosesFromMatches::PoseEstimatesVector& pose_estimates = pose_estimates_per_model[model_idx];
      ObjectModel& object_model = *object_models[model_idx];
      for (unsigned int pose_estimate_idx=0; pose_estimate_idx<pose_estimates.size () && show_found_objects;
                                                                                        ++pose_estimate_idx)
      {
        const PosesFromMatches::PoseEstimate& pose_estimate = pose_estimates[pose_estimate_idx];
        objects_in_scene.push_back (boost::shared_ptr<ObjectModel::PointCloudType> (new ObjectModel::PointCloudType));
        object_model.getTransformedPointCloud (pose_estimate.transformation, *objects_in_scene.back ());
      }
    }
    
    std::vector<std::vector<PointCorrespondences6DVector > >& feature_matches =
      object_recognition_helper.getIntermediateElements ().feature_matches;
    boost::shared_ptr<PointCloud<PointXYZ> > feature_match_positions_ptr (new PointCloud<PointXYZ>);
    PointCloud<PointXYZ>& feature_match_positions = *feature_match_positions_ptr;
    if (show_feature_match_positions)
    {
      feature_match_positions.points.clear ();
      for (unsigned int model_idx=0; model_idx<feature_matches.size (); ++model_idx)
      {
        vector<PointCorrespondences6DVector>& model_feature_matches = feature_matches[model_idx];
        for (unsigned int view_idx=0; view_idx<model_feature_matches.size (); ++view_idx)
        {
          const PointCorrespondences6DVector& view_feature_matches = model_feature_matches[view_idx];
          for (unsigned int match_idx=0; match_idx<view_feature_matches.size (); ++match_idx)
          {
            const PointCorrespondence6D& match = view_feature_matches[match_idx];
            PointXYZ match_position;
            match_position.x=match.point1[0];
            match_position.y=match.point1[1];
            match_position.z=match.point1[2];
            feature_match_positions.points.push_back (match_position);
          }
        }
      }
    }
    
    ObjectModel& first_object_model = *object_models[0];
    while (!interest_points_widgets.empty ())
    {
      delete interest_points_widgets.back ();
      interest_points_widgets.pop_back ();
    }
    if (show_interest_points)
    {
      interest_points_widgets.push_back (RangeImageVisualizer::getInterestPointsWidget (scene_range_image,
                                        interest_point_detector_scene.getInterestImage (), 0.0f, 1.0f,
                                        interest_point_detector_scene.getInterestPoints (), "Scene interest points"));
      const std::vector<RangeImage, Eigen::aligned_allocator<RangeImage> >& model_views = first_object_model.getViews ();
      NarfKeypoint& interest_point_detector_model =
        object_recognition_helper.getIntermediateElements ().interest_point_detector_model;
      interest_point_detector_model.getParameters ().support_size = support_size;
      for (unsigned int model_view_idx=0; model_view_idx<model_views.size (); ++model_view_idx)
      {
        const RangeImage& model_view = model_views[model_view_idx];
        interest_point_detector_model.setRangeImage (&model_view);
        //RangeImageVisualizer* surface_change_scores_widget = new RangeImageVisualizer;
        //surface_change_scores_widget->setFloatImage (border_extractor_model.getSurfaceChangeScores (),
        //                                            model_view.width, model_view.height,
        //                                            "model view surface_change_scores", 0.0f, 1.0f, true);
        //interest_points_widgets.push_back (surface_change_scores_widget);
        
        interest_points_widgets.push_back (RangeImageVisualizer::getInterestPointsWidget (model_view,
                                          interest_point_detector_model.getInterestImage (), 0.0f, 1.0f,
                                          interest_point_detector_model.getInterestPoints (), "Model interest points"));

        interest_point_detector_model.setRangeImage (NULL);
      }
    }
    
    //----------------------
    // -----Show images-----
    // ---------------------
    // Scene:
    scene_range_image_widget.showRangeImage (scene_range_image);
    // Object views:
    const std::vector<RangeImage, Eigen::aligned_allocator<RangeImage> >& model_views = first_object_model.getViews ();
    for (unsigned int i=0; i<model_views.size () && show_model_views; ++i)
    {
      const RangeImage& model_view = model_views[i];
      range_image_widgets.push_back (RangeImageVisualizer::getRangeImageWidget (model_view, -INFINITY, INFINITY, true, "Model view"));
      if (show_validation_points)
      {
        const ObjectModel::ValidationPoints& validation_points = first_object_model.getValidationPoints ()[i];
        for (unsigned int j=0; j<validation_points.surface.size (); ++j)
        {
          float image_x, image_y;
          model_view.getImagePoint (validation_points.surface[j], image_x, image_y);
          range_image_widgets.back ()->markPoint (image_x, image_y, pcl::visualization::Vector3ub (0,255,0));
        }
        for (unsigned int j=0; j<validation_points.border.size (); ++j)
        {
          float image_x, image_y;
          model_view.getImagePoint (validation_points.border[j], image_x, image_y);
          range_image_widgets.back ()->markPoint (image_x, image_y, pcl::visualization::Vector3ub (255,0,0));
        }
      }
    }
    //---------------------------
    
    for (unsigned int i=0; i<point_clouds_in_viewer.size (); ++i)
      viewer.removePointCloud (point_clouds_in_viewer[i]);
    point_clouds_in_viewer.clear ();
    
    //viewer.addCoordinateSystem (1.0f);
    PointCloudColorHandlerCustom<pcl::PointWithRange> color_handler_cloud (scene_range_image_ptr, 0, 0, 0);
    point_clouds_in_viewer.push_back ("scene_range_image");
    viewer.addPointCloud<PointWithRange> (scene_range_image_ptr, color_handler_cloud, point_clouds_in_viewer.back ());
    point_clouds_in_viewer.push_back ("feature_match_positions");
    viewer.addPointCloud (feature_match_positions_ptr, point_clouds_in_viewer.back ());
    viewer.setPointCloudRenderingProperties (PCL_VISUALIZER_POINT_SIZE, 7, point_clouds_in_viewer.back ());
    for (unsigned int i=0; i<objects_in_scene.size (); ++i)
    {
      stringstream ss;
      ss<<"Found object "<<i;
      point_clouds_in_viewer.push_back (ss.str ());
      PointCloudColorHandlerRandom<ObjectModel::PointCloudType::PointType> color_handler (objects_in_scene[i]);
      viewer.addPointCloud<ObjectModel::PointCloudType::PointType> (objects_in_scene[i], color_handler, point_clouds_in_viewer.back ());
    }
  }
}