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 #define BOOST_PARAMETER_MAX_ARITY 7
00037
00038 #include "jsk_pcl_ros_utils/polygon_array_angle_likelihood.h"
00039 #include "jsk_recognition_utils/tf_listener_singleton.h"
00040 #include "jsk_recognition_utils/geo_util.h"
00041 #include "jsk_recognition_utils/pcl_conversion_util.h"
00042 #include <jsk_topic_tools/rosparam_utils.h>
00043
00044 namespace jsk_pcl_ros_utils
00045 {
00046 void PolygonArrayAngleLikelihood::onInit()
00047 {
00048 DiagnosticNodelet::onInit();
00049 if (!pnh_->getParam("target_frame_id", target_frame_id_)) {
00050 ROS_ERROR("You need to specify ~target_frame_id");
00051 return;
00052 }
00053 pnh_->param("tf_queue_size", tf_queue_size_, 10);
00054 std::vector<double> axis(3);
00055 if (!jsk_topic_tools::readVectorParameter(*pnh_, "axis", axis)) {
00056 axis[0] = 1;
00057 axis[1] = 0;
00058 axis[2] = 0;
00059 }
00060 axis_[0] = axis[0];
00061 axis_[1] = axis[1];
00062 axis_[2] = axis[2];
00063 tf_listener_ = jsk_recognition_utils::TfListenerSingleton::getInstance();
00064 pub_ = advertise<jsk_recognition_msgs::PolygonArray>(*pnh_, "output", 1);
00065 onInitPostProcess();
00066 }
00067
00068 void PolygonArrayAngleLikelihood::subscribe()
00069 {
00070 sub_.subscribe(*pnh_, "input", 10);
00071 tf_filter_.reset(new tf::MessageFilter<jsk_recognition_msgs::PolygonArray>(
00072 sub_,
00073 *tf_listener_,
00074 target_frame_id_,
00075 tf_queue_size_));
00076 tf_filter_->registerCallback(
00077 boost::bind(&PolygonArrayAngleLikelihood::likelihood, this, _1));
00078 }
00079
00080 void PolygonArrayAngleLikelihood::unsubscribe()
00081 {
00082 sub_.unsubscribe();
00083 }
00084
00085 void PolygonArrayAngleLikelihood::likelihood(
00086 const jsk_recognition_msgs::PolygonArray::ConstPtr& msg)
00087 {
00088 boost::mutex::scoped_lock lock(mutex_);
00089 vital_checker_->poke();
00090 jsk_recognition_msgs::PolygonArray new_msg(*msg);
00091
00092 try
00093 {
00094
00095
00096
00097 tf::StampedTransform transform;
00098 tf_listener_->lookupTransform(
00099 target_frame_id_, msg->header.frame_id, msg->header.stamp, transform);
00100 Eigen::Affine3f pose;
00101 tf::transformTFToEigen(transform, pose);
00102
00103
00104 Eigen::Vector3f reference_axis = pose.rotation() * axis_;
00105
00106 double min_distance = DBL_MAX;
00107 double max_distance = - DBL_MAX;
00108 std::vector<double> distances;
00109 for (size_t i = 0; i < msg->polygons.size(); i++) {
00110 jsk_recognition_utils::Polygon::Ptr polygon
00111 = jsk_recognition_utils::Polygon::fromROSMsgPtr(msg->polygons[i].polygon);
00112 Eigen::Vector3f n = polygon->getNormal();
00113 double distance = std::abs(reference_axis.dot(n));
00114 min_distance = std::min(distance, min_distance);
00115 max_distance = std::max(distance, max_distance);
00116 distances.push_back(distance);
00117 }
00118
00119
00120 for (size_t i = 0; i < distances.size(); i++) {
00121
00122
00123 double likelihood = 1 / (1 + (distances[i] - 1) * (distances[i] - 1));
00124
00125 if (msg->likelihood.size() == 0) {
00126 new_msg.likelihood.push_back(likelihood);
00127 }
00128 else {
00129 new_msg.likelihood[i] = new_msg.likelihood[i] * likelihood;
00130 }
00131 }
00132 pub_.publish(new_msg);
00133 }
00134 catch (...)
00135 {
00136 NODELET_ERROR("Unknown transform error");
00137 }
00138
00139 }
00140 }
00141
00142 #include <pluginlib/class_list_macros.h>
00143 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros_utils::PolygonArrayAngleLikelihood,
00144 nodelet::Nodelet);