Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include <algorithm>
00037 #include <jsk_pcl_ros_utils/polygon_array_likelihood_filter.h>
00038
00039 namespace jsk_pcl_ros_utils
00040 {
00041 void PolygonArrayLikelihoodFilter::onInit()
00042 {
00043 DiagnosticNodelet::onInit();
00044
00045 srv_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
00046 dynamic_reconfigure::Server<Config>::CallbackType f =
00047 boost::bind(&PolygonArrayLikelihoodFilter::configCallback, this, _1, _2);
00048 srv_->setCallback(f);
00049
00050 pub_polygons_ = advertise<jsk_recognition_msgs::PolygonArray>(
00051 *pnh_, "output_polygons", 1);
00052
00053 pnh_->param<bool>("use_coefficients", use_coefficients_, true);
00054 if (use_coefficients_) {
00055 pub_coefficients_ = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(
00056 *pnh_, "output_coefficients", 1);
00057 }
00058
00059 onInitPostProcess();
00060 }
00061
00062 void PolygonArrayLikelihoodFilter::subscribe()
00063 {
00064 if (use_coefficients_) {
00065 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(queue_size_);
00066 sub_polygons_.subscribe(*pnh_, "input_polygons", 1);
00067 sub_coefficients_.subscribe(*pnh_, "input_coefficients", 1);
00068 sync_->connectInput(sub_polygons_, sub_coefficients_);
00069 sync_->registerCallback(boost::bind(
00070 &PolygonArrayLikelihoodFilter::filter,
00071 this, _1, _2));
00072 } else {
00073 sub_polygons_alone_ = pnh_->subscribe(
00074 "input_polygons", 1, &PolygonArrayLikelihoodFilter::filter, this);
00075 }
00076 }
00077
00078 void PolygonArrayLikelihoodFilter::unsubscribe()
00079 {
00080 if (use_coefficients_) {
00081 sub_polygons_.unsubscribe();
00082 sub_coefficients_.unsubscribe();
00083 } else {
00084 sub_polygons_alone_.shutdown();
00085 }
00086 }
00087
00088 void PolygonArrayLikelihoodFilter::configCallback(Config& config, uint32_t level)
00089 {
00090 boost::mutex::scoped_lock lock(mutex_);
00091 threshold_ = config.threshold;
00092 negative_ = config.negative;
00093 if (queue_size_ != config.queue_size) {
00094 queue_size_ = config.queue_size;
00095 unsubscribe();
00096 subscribe();
00097 }
00098 }
00099
00100 void PolygonArrayLikelihoodFilter::filter(
00101 const jsk_recognition_msgs::PolygonArray::ConstPtr& polygons)
00102 {
00103 jsk_recognition_msgs::ModelCoefficientsArray::Ptr dummy;
00104 dummy.reset();
00105 filter(polygons, dummy);
00106 }
00107
00108 void PolygonArrayLikelihoodFilter::filter(
00109 const jsk_recognition_msgs::PolygonArray::ConstPtr& polygons,
00110 const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients)
00111 {
00112 boost::mutex::scoped_lock lock(mutex_);
00113 if (polygons->polygons.size() != polygons->likelihood.size()) {
00114 ROS_ERROR_STREAM_THROTTLE(1.0, "The size of polygons " << polygons->polygons.size()
00115 << " must be same as the size of likelihood "
00116 << polygons->likelihood.size());
00117 return;
00118 }
00119 if (use_coefficients_ &&
00120 polygons->polygons.size() != coefficients->coefficients.size()) {
00121 ROS_ERROR_STREAM_THROTTLE(1.0, "The size of polygons " << polygons->polygons.size()
00122 << "must be same as the size of coeeficients "
00123 << coefficients->coefficients.size());
00124 return;
00125 }
00126 vital_checker_->poke();
00127
00128 bool use_labels = polygons->polygons.size() == polygons->labels.size();
00129
00130 std::vector<std::pair<double, int> > lookup_table;
00131 lookup_table.resize(polygons->polygons.size());
00132 for (int i = 0; i < polygons->polygons.size(); ++i) {
00133 lookup_table[i] = std::pair<double, int>(polygons->likelihood[i], i);
00134 }
00135 std::sort(lookup_table.rbegin(), lookup_table.rend());
00136
00137 jsk_recognition_msgs::PolygonArray ret_polygons;
00138 jsk_recognition_msgs::ModelCoefficientsArray ret_coefficients;
00139
00140 for (int i = 0; i < lookup_table.size(); ++i) {
00141 double likelihood = lookup_table[i].first;
00142 int idx = lookup_table[i].second;
00143 if ((!negative_ && likelihood >= threshold_) ||
00144 (negative_ && likelihood < threshold_)) {
00145 ret_polygons.polygons.push_back(polygons->polygons[idx]);
00146 ret_polygons.likelihood.push_back(polygons->likelihood[idx]);
00147 if (use_labels) {
00148 ret_polygons.labels.push_back(polygons->labels[idx]);
00149 }
00150 if (use_coefficients_) {
00151 ret_coefficients.coefficients.push_back(coefficients->coefficients[idx]);
00152 }
00153 }
00154 }
00155
00156 ret_polygons.header = polygons->header;
00157 pub_polygons_.publish(ret_polygons);
00158 if (use_coefficients_) {
00159 ret_coefficients.header = coefficients->header;
00160 pub_coefficients_.publish(ret_coefficients);
00161 }
00162 }
00163 }
00164
00165 #include <pluginlib/class_list_macros.h>
00166 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros_utils::PolygonArrayLikelihoodFilter, nodelet::Nodelet);