37 #ifndef JSK_PCL_ROS_LINEMOD_H_ 38 #define JSK_PCL_ROS_LINEMOD_H_ 43 #include <pcl/recognition/linemod/line_rgbd.h> 44 #include <pcl/recognition/color_gradient_modality.h> 45 #include <pcl/recognition/surface_normal_modality.h> 47 #include <jsk_pcl_ros/LINEMODDetectorConfig.h> 48 #include <sensor_msgs/PointCloud2.h> 49 #include <sensor_msgs/CameraInfo.h> 50 #include <dynamic_reconfigure/server.h> 52 #include <std_srvs/Empty.h> 56 #include <pcl/recognition/linemod.h> 57 #include <pcl/recognition/color_gradient_modality.h> 58 #include <pcl/recognition/surface_normal_modality.h> 59 #include <jsk_recognition_msgs/BoundingBox.h> 60 #include <jsk_recognition_msgs/BoundingBoxArray.h> 62 #include <yaml-cpp/yaml.h> 69 typedef LINEMODDetectorConfig
Config;
80 const sensor_msgs::PointCloud2::ConstPtr& cloud_msg);
82 Config&
config, uint32_t level);
84 pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
cloud,
85 const pcl::SparseQuantizedMultiModTemplate& linemod_template,
86 const pcl::LINEMODDetection& linemod_detection,
87 Eigen::Vector3f& center);
121 sensor_msgs::PointCloud2,
129 const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
130 const PCLIndicesMsg::ConstPtr& indices_msg);
131 virtual void subscribeCloud(
132 const sensor_msgs::PointCloud2::ConstPtr& cloud_msg);
133 virtual void subscribeCameraInfo(
134 const sensor_msgs::CameraInfo::ConstPtr& info_msg);
135 virtual bool startTraining(std_srvs::Empty::Request&
req,
136 std_srvs::Empty::Response&
res);
137 virtual std::vector<std::string> trainOneData(
138 pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud,
139 pcl::PointIndices::Ptr mask,
140 std::string& tempstr,
142 virtual void tar(
const std::string& directory,
const std::string& output);
143 virtual bool clearData(std_srvs::Empty::Request&
req,
144 std_srvs::Empty::Response&
res);
145 virtual void trainWithoutViewpointSampling();
146 virtual void trainWithViewpointSampling();
147 virtual void organizedPointCloudWithViewPoint(
148 const Eigen::Affine3f& transform,
149 pcl::PointCloud<pcl::PointXYZRGBA>::Ptr raw_cloud,
151 pcl::PointCloud<pcl::PointXYZRGBA>::Ptr output,
152 pcl::PointIndices& mask);
153 virtual void generateLINEMODTrainingData(
154 pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud,
155 pcl::PointIndices::Ptr mask,
156 pcl::ColorGradientModality<pcl::PointXYZRGBA>& color_grad_mod,
157 pcl::SurfaceNormalModality<pcl::PointXYZRGBA>& surface_norm_mod,
158 pcl::MaskMap& mask_map,
159 pcl::RegionXY& region);
175 std::vector<pcl::PointCloud<pcl::PointXYZRGBA>::Ptr>
samples_;
std::string template_file_
ros::ServiceServer clear_data_srv_
std::vector< pcl::PointIndices::Ptr > sample_indices_
double sample_viewpoint_radius_max_
virtual void setTemplate(YAML::Node doc)
ros::Subscriber sub_cloud_
ros::Publisher pub_cloud_
std::vector< jsk_recognition_msgs::BoundingBox > template_bboxes_
pcl::ColorGradientModality< pcl::PointXYZRGBA > color_gradient_mod_
ros::Publisher pub_detect_mask_
sensor_msgs::CameraInfo::ConstPtr camera_info_
double sample_viewpoint_angle_min_
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr template_cloud_
std::vector< pcl::PointCloud< pcl::PointXYZRGBA >::Ptr > samples_before_sampling_
double sample_viewpoint_radius_min_
ros::Publisher pub_range_image_
virtual void computeCenterOfTemplate(pcl::PointCloud< pcl::PointXYZRGBA >::Ptr cloud, const pcl::SparseQuantizedMultiModTemplate &linemod_template, const pcl::LINEMODDetection &linemod_detection, Eigen::Vector3f ¢er)
message_filters::Subscriber< PCLIndicesMsg > sub_indices_
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_input_
std::vector< pcl::PointCloud< pcl::PointXYZRGBA >::Ptr > samples_
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
ros::Subscriber sub_input_nonsync_
virtual void unsubscribe()
ros::Publisher pub_original_template_cloud_
ros::ServiceServer start_training_srv_
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
boost::shared_ptr< LINEMODTrainer > Ptr
virtual void detect(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
ros::Publisher pub_sample_cloud_
ros::Subscriber sub_camera_info_nonsync_
boost::mutex mutex
global mutex.
LINEMODDetectorConfig Config
message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, PCLIndicesMsg > SyncPolicy
pcl::SurfaceNormalModality< pcl::PointXYZRGBA > surface_normal_mod_
double sample_viewpoint_angle_step_
double sample_viewpoint_angle_max_
virtual void configCallback(Config &config, uint32_t level)
std::vector< Eigen::Affine3f > template_poses_
double gradient_magnitude_threshold_
ros::Publisher pub_colored_range_image_
pcl::PointIndices PCLIndicesMsg
double sample_viewpoint_radius_step_
double detection_threshold_