36 #define BOOST_PARAMETER_MAX_ARITY 7 47 DiagnosticNodelet::onInit();
49 ROS_ERROR(
"You need to specify ~target_frame_id");
54 pub_ = advertise<jsk_recognition_msgs::PolygonArray>(*
pnh_,
"output", 1);
77 const jsk_recognition_msgs::PolygonArray::ConstPtr& msg)
81 jsk_recognition_msgs::PolygonArray new_msg(*msg);
93 Eigen::Vector3f
p(pose.translation());
95 double min_distance = DBL_MAX;
96 double max_distance = - DBL_MAX;
97 std::vector<double> distances;
98 for (
size_t i = 0; i < msg->polygons.size(); i++) {
102 polygon->nearestPoint(
p, distance);
103 min_distance = std::min(distance, min_distance);
104 max_distance = std::max(distance, max_distance);
105 distances.push_back(distance);
109 for (
size_t i = 0; i < distances.size(); i++) {
112 double likelihood = 1 / (1 + distances[i] * distances[i]);
114 if (msg->likelihood.size() == 0) {
115 new_msg.likelihood.push_back(likelihood);
118 new_msg.likelihood[i] = new_msg.likelihood[i] *
likelihood;
tf::TransformListener * tf_listener_
#define NODELET_ERROR(...)
void publish(const boost::shared_ptr< M > &message) const
void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e)
boost::shared_ptr< tf::MessageFilter< jsk_recognition_msgs::PolygonArray > > tf_filter_
message_filters::Subscriber< jsk_recognition_msgs::PolygonArray > sub_
static Polygon::Ptr fromROSMsgPtr(const geometry_msgs::Polygon &polygon)
std::string target_frame_id_
virtual void likelihood(const jsk_recognition_msgs::PolygonArray::ConstPtr &msg)
virtual void unsubscribe()
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)
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::PolygonArrayDistanceLikelihood, nodelet::Nodelet)
static tf::TransformListener * getInstance()
double distance(const urdf::Pose &transform)