subscribe_object.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/foreach.hpp>
#include <pcl/io/pcd_io.h>
#include <iostream>
#include <fstream>
#include <stdio.h>
//----------------------------------------
#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"
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>

#include <unistd.h>

using namespace pcl;
using namespace pcl::visualization;

//typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

 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 = 0;
 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;
 sensor_msgs::ImageConstPtr depth_image_msg;
 boost::mutex m;
 bool got_new_data=false;


bool fileExist(const char *fileName)
{ FILE *pFile;
  pFile = fopen(fileName, "r");
  if(pFile != NULL)
    { fclose(pFile);
      return true;
    }
  return false;
}

void callback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& msg, ros::Publisher &pub, ros::Publisher &pub2)
{
  static bool foundObject = false;

  //------------------------------------------------
  //---------save msg in a new PointCloud ----------
  //------------------------------------------------
  PointCloud<PointXYZ> scene(*msg);

  vector<string> point_clouds_in_viewer;
    object_recognition_helper.createRangeImageFromPointCloud (*msg);
    object_recognition_helper.extractSceneFeatures ();
    object_recognition_helper.extractPoseEstimates ();
    std::vector<PosesFromMatches::PoseEstimatesVector>& pose_estimates_per_model =
      object_recognition_helper.filterFalsePositives ();

    object_recognition_helper.printTimings ();
    vector<boost::shared_ptr<ObjectModel::PointCloudType> > objects_in_scene;  // For visualization
    PosesFromMatches::PoseEstimatesVector& pose_estimates = pose_estimates_per_model[0];
    ObjectModelList& object_models = object_recognition_helper.getIntermediateElements ().object_models;

    for (unsigned int pose_estimate_idx=0; pose_estimate_idx<pose_estimates.size (); ++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_models[0]->getTransformedPointCloud (pose_estimate.transformation, *objects_in_scene.back ());
           //pose_estimate.transformation
           float x, y, z, roll, pitch, yaw;
           pcl::getTranslationAndEulerAngles (pose_estimate.transformation, x, y, z, roll, pitch, yaw);
           foundObject = true;
           cout << "Found object at "<<pose_estimate.transformation(0,0)<<".....\n";
           cout << "Found object at "<<x<<", "<<y<<", "<<z<<" with rotation "<<pcl::rad2deg(roll)<<"deg, "<<pcl::rad2deg(pitch)<<"deg, "<<pcl::rad2deg(yaw)<<"deg.\n";

           objects_in_scene.back()->header.stamp = msg->header.stamp;
           objects_in_scene.back()->header.frame_id = msg->header.frame_id;
           if(pub.getNumSubscribers() != 0) pub.publish(objects_in_scene.back());
         }
    if(pub2.getNumSubscribers() != 0) pub2.publish(*msg);

    ROS_INFO("Callback Finished");
         usleep(10000);
}


int main(int argc, char** argv)
{
        std::string object_name;

    ros::init(argc, argv, "object_filter");
    ros::NodeHandle nh;
    ros::NodeHandle pnh("~");

    if (pnh.getParam("object_name", object_name))
    {
        //----------generate Modell---------------------
            // int range_image_source = -1;
            orh_params.view_sampling_first_layer = 0;
            orh_params.view_sampling_last_layer = 2;

            vector<string> model_filenames;
            model_filenames.push_back (object_name);
            if (!object_recognition_helper.addObjectModelsFromPcdFiles (model_filenames))
            {
              ROS_FATAL("Could not add object model: %s", model_filenames.back().c_str());
              exit (1);
            }
            object_recognition_helper.extractModelFeatures ();
            ObjectModelList& object_models = object_recognition_helper.getIntermediateElements ().object_models;
            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;


            ros::Publisher pub = nh.advertise<ObjectModel::PointCloudType> ("points2", 1);
            ros::Publisher pub2 = nh.advertise<PointCloud<PointXYZ> > ("office_scene", 1);

            ros::Subscriber hearCamera;
            hearCamera = nh.subscribe<pcl::PointCloud<pcl::PointXYZ> >("cloud_in", 1, boost::bind(callback, _1, pub, pub2));

          ros::spin();

    }
    else ROS_ERROR("There is no model existing");
}