36 #define BOOST_PARAMETER_MAX_ARITY 7 48 DiagnosticNodelet::onInit();
50 ROS_ERROR(
"You need to specify ~target_frame_id");
54 std::vector<double> axis(3);
64 pub_ = advertise<jsk_recognition_msgs::PolygonArray>(*
pnh_,
"output", 1);
86 const jsk_recognition_msgs::PolygonArray::ConstPtr& msg)
90 jsk_recognition_msgs::PolygonArray new_msg(*msg);
100 Eigen::Affine3f
pose;
104 Eigen::Vector3f reference_axis = pose.rotation() *
axis_;
106 double min_distance = DBL_MAX;
107 double max_distance = - DBL_MAX;
108 std::vector<double> distances;
109 for (
size_t i = 0; i < msg->polygons.size(); i++) {
112 Eigen::Vector3f n = polygon->getNormal();
113 double distance = std::abs(reference_axis.dot(n));
114 min_distance = std::min(distance, min_distance);
115 max_distance = std::max(distance, max_distance);
116 distances.push_back(distance);
120 for (
size_t i = 0; i < distances.size(); i++) {
123 double likelihood = 1 / (1 + (distances[i] - 1) * (distances[i] - 1));
125 if (msg->likelihood.size() == 0) {
126 new_msg.likelihood.push_back(likelihood);
129 new_msg.likelihood[i] = new_msg.likelihood[i] *
likelihood;
#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_
virtual void likelihood(const jsk_recognition_msgs::PolygonArray::ConstPtr &msg)
std::string target_frame_id_
static Polygon::Ptr fromROSMsgPtr(const geometry_msgs::Polygon &polygon)
message_filters::Subscriber< jsk_recognition_msgs::PolygonArray > sub_
tf::TransformListener * tf_listener_
virtual void unsubscribe()
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::PolygonArrayAngleLikelihood, nodelet::Nodelet)
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)
static tf::TransformListener * getInstance()
double distance(const urdf::Pose &transform)