openni_object_recognition.cpp

Go to the documentation of this file.

/* \author Bastian Steder */

/* ---[ */
#include <iostream>
using namespace std;
#include "object_recognition/object_recognition_helper.h"
#include <pcl/io/openni_grabber.h>
#include "pcl/console/parse.h"

#include "pcl/io/pcd_io.h"
#include "pcl/range_image/range_image.h"
#include "pcl/features/narf.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 "pcl/common/common_headers.h"
#include "pcl/visualization/range_image_visualizer.h"
#include "pcl/visualization/pcl_visualizer.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.05;    // Factor regarding average model radius
float support_size_factor = 0.5f;  // Factor regarding average model radius
float support_size = -1.0f;

bool show_found_objects = 1;
bool do_plane_extraction = false;

std::string device_id = "#1";

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"
       << "-d <device_id>  set the device id (default \""<<device_id<<"\")\n"
       << "-i <bool>    do ICP (default "<< (int)fpf_params.do_icp<<").\n"
       << "-r <float>   angular resolution in degrees (default "<<rad2deg (angular_resolution)<<"deg)\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"
       << "-n <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"
       << "-p           plane extraction - remove big planes from the scene before trying to find objects.\n"
       << "-s <float>   the support size used for keypoint and feature extraction"
       <<               " (default: "<<support_size_factor<<" times the average model radius)\n"
       << "-l1 <int>    start sampling layer (default: "<<orh_params.view_sampling_first_layer<<")\n"
       << "-l2 <int>    end sampling layer (default: "<<orh_params.view_sampling_last_layer<<")\n"
       << "-h           this help\n"
       << "\n\n";
}


boost::mutex depth_image_mutex;
boost::shared_ptr<openni_wrapper::DepthImage> depth_image_ptr;

bool received_new_depth_data = false;
struct EventHelper
{
  void
  depth_image_cb (const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image)
  {
    if (depth_image_mutex.try_lock ())
    {
      depth_image_ptr = depth_image;
      depth_image_mutex.unlock ();
      received_new_depth_data = true;
    }
  }
};


int main (int argc, char** argv)
{
  orh_params.view_sampling_first_layer = 0;
  orh_params.view_sampling_last_layer = 0;

  // --------------------------------------
  // -----Parse Command Line Arguments-----
  // --------------------------------------
  if (pcl::console::find_argument (argc, argv, "-h") >= 0)
  {
    printUsage (argv[0]);
    return 0;
  }
  if (pcl::console::parse (argc, argv, "-t", max_no_of_threads) >= 0)
    cout << "Setting maximum number of threads to "<< max_no_of_threads<<".\n";
  if (pcl::console::parse (argc, argv, "-i", fpf_params.do_icp) >= 0)
    cout << "Will "<< (fpf_params.do_icp?"":"not ")<<"use ICP.\n";
  if (pcl::console::find_argument (argc, argv, "-b") >= 0)
  {
    single_view_model = true;
    cout << "Setting single_view_model to true.\n";
  }
  if (pcl::console::find_argument (argc, argv, "-p") >= 0)
  {
    do_plane_extraction = true;
    cout << "Will perform plane extraction.\n";
  }
  if (pcl::console::parse (argc, argv, "-d", device_id) >= 0)
    cout << "Using device id \""<<device_id<<"\".\n";
  if (pcl::console::find_argument (argc, argv, "-o") >= 0)
  {
    use_rotation_invariance = true;
    cout << "Using rotational invariant version of the features.\n";
  }
  int tmp_coordinate_frame;
  if (pcl::console::parse (argc, argv, "-c", tmp_coordinate_frame) >= 0)
  {
    coordinate_frame = pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame);
    cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n";
  }
  if (pcl::console::parse (argc, argv, "-s", support_size) >= 0)
    cout << "Setting support size to "<<support_size<<".\n";
  if (pcl::console::parse (argc, argv, "-l1", orh_params.view_sampling_first_layer) >= 0)
    cout << "Setting first sampling layer to "<<orh_params.view_sampling_first_layer<<".\n";
  if (pcl::console::parse (argc, argv, "-l2", orh_params.view_sampling_last_layer) >= 0)
    cout << "Setting last sampling layer to "<<orh_params.view_sampling_last_layer<<".\n";
  if (orh_params.view_sampling_first_layer > orh_params.view_sampling_last_layer) {
    cerr << "\n\n\nFirst sampling layer has to be less or equal to last sampling layer!\n\n";
    return 1;
  }
  if (pcl::console::parse (argc, argv, "-v", fpf_params.min_validation_point_score) >= 0)
  {
    fpf_params.do_validation_point_filtering = true;
    cout << "Setting minimum validation point score to "<<fpf_params.min_view_simulation_score<<".\n";
  }
  if (pcl::console::parse (argc, argv, "-x", fpf_params.maximum_range_for_found_objects) >= 0)
    cout << "Setting maximum range for found objects to "<<fpf_params.maximum_range_for_found_objects<<".\n";
  if (pcl::console::parse (argc, argv, "-n", max_descriptor_distance) >= 0)
    cout << "Setting maximum descriptor distance to "<<max_descriptor_distance<<".\n";
  if (pcl::console::parse (argc, argv, "-f", fpf_params.min_view_simulation_score) >= 0)
  {
    fpf_params.do_view_simulation_filtering = true;
    cout << "Setting minimum view simulation score to "<<fpf_params.min_view_simulation_score<<".\n";
  }
  if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0) {
    cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
    angular_resolution = deg2rad (angular_resolution);
  }
  
  
  set_unseen_to_max_range = true;
  
  // -----------------------
  // -----Read pcd file-----
  // -----------------------
  std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd");
  if (pcd_filename_indices.empty ())
  {
    cout << "\nNo *.pcd file for model given.\n\n";
    exit (0);
  }
  vector<string> model_filenames;
  for (int i=0; i<pcd_filename_indices.size (); ++i)
    model_filenames.push_back (argv[pcd_filename_indices[i]]);
  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 ();
  
  if (support_size < 0.0f)
    support_size = support_size_factor * average_model_radius;
  float noise_level = noise_level_factor * average_model_radius;
  
  //float initial_max_plane_error = 0.02f;
  float initial_max_plane_error = 0.2f*average_model_radius;
  float minimum_plane_size = 0.5f;
  if (do_plane_extraction)
  {
    minimum_plane_size = 1.2f * object_models.getMaximumPlaneSize (initial_max_plane_error);
    cout << PVARN (minimum_plane_size);
  }
  
  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;
  
  // ------------------------------------------------------------------
  
  vector<string> point_clouds_in_viewer;
  RangeImageVisualizer scene_range_image_widget ("scene range image");
  
  PCLVisualizer viewer ("3D Viewer");
  viewer.addCoordinateSystem (1.0f);
  viewer.setBackgroundColor (1, 1, 1);
  
  viewer.initCameraParameters ();
  viewer.camera_.pos[0] = 0.0;
  viewer.camera_.pos[1] = -0.3;
  viewer.camera_.pos[2] = -2.0;
  viewer.camera_.focal[0] = 0.0;
  viewer.camera_.focal[1] = 0.0+viewer.camera_.pos[1];
  viewer.camera_.focal[2] = 1.0;
  viewer.camera_.view[0] = 0.0;
  viewer.camera_.view[1] = -1.0;
  viewer.camera_.view[2] = 0.0;
  viewer.updateCamera ();

  openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
  if (driver.getNumberDevices () > 0)
  {
    for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
    {
      cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx)
           << ", product: " << driver.getProductName (deviceIdx) << ", connected: "
           << (int) driver.getBus (deviceIdx) << " @ " << (int) driver.getAddress (deviceIdx)
           << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'\n";
    }
  }
  else
  {
    cout << "\nNo devices connected.\n\n";
    return 1;
  }
  
  pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id);
  EventHelper event_helper;
  //boost::function<void (const pcl::PointCloud<pcl::PointXYZ>::ConstPtr&) > f_point_cloud =
    //boost::bind (&EventHelper::cloud_cb, &event_helper, _1);
  //boost::signals2::connection c_point_cloud = interface->registerCallback (f_point_cloud);
  
  boost::function<void (const boost::shared_ptr<openni_wrapper::DepthImage>&) > f_depth_image =
    boost::bind (&EventHelper::depth_image_cb, &event_helper, _1);
  boost::signals2::connection c_depth_image = interface->registerCallback (f_depth_image);
  
  cout << "Starting grabber\n";
  interface->start ();
  cout << "Done\n";
  //bool first = true;
  
  while (!viewer.wasStopped ()) // && (node_handle==NULL || node_handle->ok ()))
  {
    viewer.spinOnce ();
    scene_range_image_widget.spinOnce ();  // process GUI events
    //if (live_scenes)
      //ros::spinOnce ();
    boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    
    bool got_new_range_image = false;
    if (received_new_depth_data && depth_image_mutex.try_lock ())
    {
      received_new_depth_data = false;
      if (object_recognition_helper.createRangeImageFromOpenniDepthImage (*depth_image_ptr))
        got_new_range_image = true;
      depth_image_mutex.unlock ();
    }
    
    if (!got_new_range_image)
      continue;
    got_new_range_image = false;
    cout << "Got new data.\n";
    
    boost::shared_ptr<RangeImage>& scene_range_image_ptr =
      object_recognition_helper.getIntermediateElements ().scene_range_image_ptr;
    RangeImage& scene_range_image = *scene_range_image_ptr;   
    
    if (do_plane_extraction)
    {
      PointWithRange farRangePoint;
      farRangePoint.x=farRangePoint.y=farRangePoint.z = std::numeric_limits<float>::quiet_NaN ();
      farRangePoint.range = std::numeric_limits<float>::infinity ();
      std::vector<RangeImage::ExtractedPlane> planes;
      scene_range_image.extractPlanes (initial_max_plane_error, planes);
      //std::sort (planes.begin (), planes.end (), RangeImage::isLargerPlane);
      //for (int plane_idx=0; plane_idx<=0; ++plane_idx)
      for (int plane_idx=0; plane_idx<int (planes.size ()); ++plane_idx)
      {
        const RangeImage::ExtractedPlane& plane = planes[plane_idx];
        if (plane.maximum_extensions[2] <= minimum_plane_size)
          continue;
        //cout << "Removing plane of size "<<plane.maximum_extensions[2]<<"m ("<<plane.point_indices.size ()<<" points).\n";
        for (int plane_point_idx=0; plane_point_idx<int (plane.point_indices.size ()); ++plane_point_idx)
          scene_range_image.points[plane.point_indices[plane_point_idx]] = farRangePoint;
      }
    }
    
    object_recognition_helper.extractSceneFeatures ();
    object_recognition_helper.extractPoseEstimates ();
    std::vector<PosesFromMatches::PoseEstimatesVector>& pose_estimates_per_model = 
      object_recognition_helper.filterFalsePositives ();
    //object_recognition_helper.printTimings ();
    
    
    scene_range_image_widget.showRangeImage (scene_range_image);
    
    static bool first_range_image = true;
    PointCloudColorHandlerCustom<pcl::PointWithRange> color_handler_cloud (scene_range_image_ptr, 0, 0, 0);
    if (first_range_image)
    {
      viewer.addPointCloud<pcl::PointWithRange> (scene_range_image_ptr, color_handler_cloud, "scene range image");
      first_range_image = false;
    }
    else
    {
      viewer.updatePointCloud<pcl::PointWithRange> (scene_range_image_ptr, color_handler_cloud, "scene range image");
    }
    
    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 ());
      }
    }
    
    for (unsigned int i=0; i<point_clouds_in_viewer.size (); ++i)
      viewer.removePointCloud (point_clouds_in_viewer[i]);
    point_clouds_in_viewer.clear ();
    
    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 ());
    }
  }

  interface->stop ();
}