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 "jsk_perception/rect_array_actual_size_filter.h"
00037 #include <cv_bridge/cv_bridge.h>
00038 #include <sensor_msgs/image_encodings.h>
00039 #include <jsk_recognition_utils/sensor_model/camera_depth_sensor.h>
00040
00041 namespace jsk_perception
00042 {
00043 void RectArrayActualSizeFilter::onInit()
00044 {
00045 DiagnosticNodelet::onInit();
00046 pnh_->param("approximate_sync", approximate_sync_, false);
00047 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00048 dynamic_reconfigure::Server<Config>::CallbackType f =
00049 boost::bind (
00050 &RectArrayActualSizeFilter::configCallback, this, _1, _2);
00051 srv_->setCallback (f);
00052
00053 pub_ = advertise<jsk_recognition_msgs::RectArray>(
00054 *pnh_, "output", 1);
00055 onInitPostProcess();
00056 }
00057
00058 void RectArrayActualSizeFilter::subscribe()
00059 {
00060 sub_rect_array_.subscribe(*pnh_, "input", 1);
00061 sub_image_.subscribe(*pnh_, "input/depth_image", 1);
00062 sub_info_.subscribe(*pnh_, "input/info", 1);
00063 if (approximate_sync_) {
00064 async_ = boost::make_shared<message_filters::Synchronizer<ApproxSyncPolicy> >(100);
00065 async_->connectInput(sub_rect_array_, sub_image_, sub_info_);
00066 async_->registerCallback(boost::bind(&RectArrayActualSizeFilter::filter, this, _1, _2, _3));
00067 }
00068 else {
00069 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00070 sync_->connectInput(sub_rect_array_, sub_image_, sub_info_);
00071 sync_->registerCallback(boost::bind(&RectArrayActualSizeFilter::filter, this, _1, _2, _3));
00072 }
00073 }
00074
00075 void RectArrayActualSizeFilter::unsubscribe()
00076 {
00077 sub_rect_array_.unsubscribe();
00078 sub_image_.unsubscribe();
00079 sub_info_.unsubscribe();
00080 }
00081
00082 void RectArrayActualSizeFilter::configCallback(Config& config,
00083 uint32_t level)
00084 {
00085 boost::mutex::scoped_lock lock(mutex_);
00086 kernel_size_ = config.kernel_size;
00087 min_x_ = config.min_x;
00088 min_y_ = config.min_y;
00089 max_x_ = config.max_x;
00090 max_y_ = config.max_y;
00091 }
00092
00093 double RectArrayActualSizeFilter::averageDistance(
00094 const int center_x, const int center_y, const cv::Mat& img) const
00095 {
00096 double d = 0.0;
00097 int valid = 0;
00098 for (int j = -kernel_size_; j <= kernel_size_; j++) {
00099 for (int i = -kernel_size_; i <= kernel_size_; i++) {
00100 const int x = center_x + i;
00101 const int y = center_y + j;
00102 if (0 <= x && x <= img.cols &&
00103 0 <= y && y <= img.rows) {
00104 d += img.at<float>(y, x);
00105 ++valid;
00106 }
00107 }
00108 }
00109 return d / valid;
00110 }
00111
00112 void RectArrayActualSizeFilter::filter
00113 (const jsk_recognition_msgs::RectArray::ConstPtr& rect_array_msg,
00114 const sensor_msgs::Image::ConstPtr& depth_image_msg,
00115 const sensor_msgs::CameraInfo::ConstPtr& info_msg)
00116 {
00117 boost::mutex::scoped_lock lock(mutex_);
00118
00119
00120
00121 jsk_recognition_msgs::RectArray result_msg;
00122 result_msg.header = rect_array_msg->header;
00123 cv_bridge::CvImagePtr cv_depth = cv_bridge::toCvCopy(depth_image_msg, sensor_msgs::image_encodings::TYPE_32FC1);
00124 cv::Mat depth = cv_depth->image;
00125 cv::Mat average_depth;
00126
00127 jsk_recognition_utils::CameraDepthSensor model;
00128 model.setCameraInfo(*info_msg);
00129 image_geometry::PinholeCameraModel camera_model = model.getPinholeCameraModel();
00130 for (size_t i = 0; i< rect_array_msg->rects.size(); i++) {
00131 jsk_recognition_msgs::Rect rect = rect_array_msg->rects[i];
00132
00133 const int center_x = rect.x + rect.width / 2;
00134 const int center_y = rect.y + rect.height / 2;
00135 const cv::Point A(rect.x, rect.y);
00136 const cv::Point C(rect.x + rect.width, rect.y + rect.height);
00137
00138 const double distance = averageDistance(center_x, center_y, depth);
00139 cv::Point3d a_ray = camera_model.projectPixelTo3dRay(A);
00140 cv::Point3d c_ray = camera_model.projectPixelTo3dRay(C);
00141 if (a_ray.z != 0.0 && c_ray.z != 0.0) {
00142 cv::Point3d a_3d = a_ray * (distance / a_ray.z);
00143 cv::Point3d c_3d = c_ray * (distance / c_ray.z);
00144 const double width = std::abs(a_3d.x - c_3d.x);
00145 const double height = std::abs(a_3d.y - c_3d.y);
00146 if (min_x_ <= width && width <= max_x_ &&
00147 min_y_ <= height && height <= max_y_) {
00148 result_msg.rects.push_back(rect);
00149 }
00150 }
00151 else {
00152 NODELET_ERROR("rect has z=0 ray");
00153 return;
00154 }
00155 }
00156
00157 pub_.publish(result_msg);
00158 }
00159
00160 }
00161
00162 #include <pluginlib/class_list_macros.h>
00163 PLUGINLIB_EXPORT_CLASS (jsk_perception::RectArrayActualSizeFilter, nodelet::Nodelet);