00001 #include "ros/ros.h" 00002 #include "narf_recognition/LocateObjects.h" 00003 #include <iostream> 00004 #include <exception> 00005 00006 #include <narf_recognition/object_recognition_helper.h> 00007 #include <boost/thread/thread.hpp> 00008 #include "pcl/console/parse.h" 00009 00010 #include "pcl/visualization/range_image_visualizer.h" 00011 #include "pcl/visualization/pcl_visualizer.h" 00012 00013 #include "Eigen/Geometry" 00014 #include "pcl/point_cloud.h" 00015 #include "pcl/point_types.h" 00016 00017 #include "pcl/io/pcd_io.h" 00018 #include "pcl/range_image/range_image.h" 00019 #include "pcl/features/narf.h" 00020 #include "pcl/common/point_correspondence.h" 00021 #include "pcl/common/poses_from_matches.h" 00022 #include "pcl/features/range_image_border_extractor.h" 00023 #include "pcl/keypoints/narf_keypoint.h" 00024 #include "pcl/range_image/range_image_planar.h" 00025 #include "sensor_msgs/Image.h" 00026 #include "pcl/common/file_io.h" 00027 //#include "stereo_msgs/DisparityImage.h" 00028 //#include "sensor_msgs/CameraInfo.h" 00029 00030 #include "pcl/common/common_headers.h" 00031 00032 #include "ros/ros.h" 00033 00034 #include <pcl/visualization/cloud_viewer.h> 00035 #include <ros/ros.h> 00036 #include <pcl_ros/point_cloud.h> 00037 #include <pcl/point_types.h> 00038 #include <boost/foreach.hpp> 00039 #include <pcl/ModelCoefficients.h> 00040 #include <pcl/io/pcd_io.h> 00041 #include <pcl/point_types.h> 00042 #include <pcl/sample_consensus/method_types.h> 00043 #include <pcl/sample_consensus/model_types.h> 00044 #include <pcl/segmentation/sac_segmentation.h> 00045 #include "pcl/visualization/pcl_visualizer.h" 00046 #include <pcl/filters/extract_indices.h> 00047 #include <pcl/filters/passthrough.h> 00048 #include <pcl/features/normal_3d.h> 00049 #include <pcl/filters/voxel_grid.h> 00050 #include <pcl/kdtree/kdtree.h> 00051 #include <pcl/segmentation/extract_clusters.h> 00052 #include <pcl/console/parse.h> 00053 #include <cstdlib> 00054 #include <iostream> 00055 #include <boost/thread/thread.hpp> 00056 #include "pcl/range_image/range_image.h" 00057 #include "pcl/features/range_image_border_extractor.h" 00058 #include <pcl_ros/impl/transforms.hpp> 00059 00060 00061 00062 class Locate_Server{ 00063 public: 00064 00065 Locate_Server(ros::NodeHandle& n); 00066 ~Locate_Server(); 00067 ros::NodeHandle nh; 00068 00069 pcl::ObjectRecognitionHelper object_recognition_helper; 00070 pcl::ObjectRecognitionHelper::Parameters orh_params; 00071 pcl::FalsePositivesFilter false_positives_filter; 00072 pcl::FalsePositivesFilter::Parameters fpf_params; 00073 00074 int max_no_of_threads; 00075 bool use_rotation_invariance; 00076 float angular_resolution; 00077 pcl::RangeImage::CoordinateFrame coordinate_frame; 00078 float max_descriptor_distance; 00079 bool set_unseen_to_max_range; 00080 00081 bool single_view_model; 00082 00083 float noise_level_factor; // Factor regarding average model radius 00084 float support_size_factor; // Factor regarding average model radius 00085 00086 bool show_model_views; 00087 bool show_view_simulation_results; 00088 bool show_validation_points; 00089 bool show_found_objects; 00090 bool show_feature_match_positions; 00091 bool show_interest_points; 00092 bool print_timings; 00093 00094 sensor_msgs::ImageConstPtr depth_image_msg; 00095 boost::mutex m; 00096 bool cloud_received; 00097 bool got_new_data; 00098 pcl::ObjectModelList object_models; 00099 00100 pcl::visualization::PCLVisualizer* viewer; 00101 00102 ros::Subscriber sub; 00103 ros::ServiceServer service; 00104 00105 void getModels(std::vector<string> model_filenames); 00106 void cloud_cb_(const sensor_msgs::PointCloud2 msg); 00107 void visualize( 00108 pcl::visualization::PCLVisualizer& viewer, 00109 std::vector<pcl::PosesFromMatches::PoseEstimatesVector>& pose_estimates_per_model, 00110 std::vector<string>& point_clouds_in_viewer); 00111 bool locate(narf_recognition::LocateObjects::Request &req, 00112 narf_recognition::LocateObjects::Response &res); 00113 00114 };
