43 DiagnosticNodelet::onInit();
45 srv_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
46 dynamic_reconfigure::Server<Config>::CallbackType
f =
50 pub_polygons_ = advertise<jsk_recognition_msgs::PolygonArray>(
51 *
pnh_,
"output_polygons", 1);
54 if (use_coefficients_) {
56 *
pnh_,
"output_coefficients", 1);
65 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(
queue_size_);
69 sync_->registerCallback(boost::bind(
101 const jsk_recognition_msgs::PolygonArray::ConstPtr& polygons)
103 jsk_recognition_msgs::ModelCoefficientsArray::Ptr dummy;
109 const jsk_recognition_msgs::PolygonArray::ConstPtr& polygons,
110 const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients)
113 if (polygons->polygons.size() != polygons->likelihood.size()) {
115 <<
" must be same as the size of likelihood " 116 << polygons->likelihood.size());
120 polygons->polygons.size() != coefficients->coefficients.size()) {
122 <<
"must be same as the size of coeeficients " 123 << coefficients->coefficients.size());
128 bool use_labels = polygons->polygons.size() == polygons->labels.size();
130 std::vector<std::pair<double, int> > lookup_table;
131 lookup_table.resize(polygons->polygons.size());
132 for (
int i = 0; i < polygons->polygons.size(); ++i) {
133 lookup_table[i] = std::pair<double, int>(polygons->likelihood[i], i);
135 std::sort(lookup_table.rbegin(), lookup_table.rend());
137 jsk_recognition_msgs::PolygonArray ret_polygons;
138 jsk_recognition_msgs::ModelCoefficientsArray ret_coefficients;
140 for (
int i = 0; i < lookup_table.size(); ++i) {
142 int idx = lookup_table[i].second;
145 ret_polygons.polygons.push_back(polygons->polygons[idx]);
146 ret_polygons.likelihood.push_back(polygons->likelihood[idx]);
148 ret_polygons.labels.push_back(polygons->labels[idx]);
151 ret_coefficients.coefficients.push_back(coefficients->coefficients[idx]);
156 ret_polygons.header = polygons->header;
159 ret_coefficients.header = coefficients->header;
message_filters::Subscriber< jsk_recognition_msgs::PolygonArray > sub_polygons_
void publish(const boost::shared_ptr< M > &message) const
virtual void filter(const jsk_recognition_msgs::PolygonArray::ConstPtr &polygons)
ros::Subscriber sub_polygons_alone_
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::PolygonArrayLikelihoodFilter, nodelet::Nodelet)
PolygonArrayLikelihoodFilterConfig Config
ros::Publisher pub_polygons_
virtual void configCallback(Config &config, uint32_t level)
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
ros::Publisher pub_coefficients_
virtual void unsubscribe()
#define ROS_ERROR_STREAM_THROTTLE(rate, args)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
message_filters::Subscriber< jsk_recognition_msgs::ModelCoefficientsArray > sub_coefficients_