boundingbox_occlusion_rejector_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #define BOOST_PARAMETER_MAX_ARITY 7
00037 
00038 #include "jsk_pcl_ros/boundingbox_occlusion_rejector.h"
00039 #include "jsk_recognition_utils/pcl_conversion_util.h"
00040 #include <sensor_msgs/image_encodings.h>
00041 
00042 namespace jsk_pcl_ros
00043 {
00044   void BoundingBoxOcclusionRejector::onInit()
00045   {
00046     DiagnosticNodelet::onInit();
00047     tf_listener_ = TfListenerSingleton::getInstance();
00048     pub_ = advertise<jsk_recognition_msgs::BoundingBoxArray>(*pnh_, "output", 1);
00049     pub_target_image_ = advertise<sensor_msgs::Image>(*pnh_, "output/target_image", 1);
00050     pub_candidate_image_ = advertise<sensor_msgs::Image>(*pnh_, "output/candidate_image", 1);
00051     onInitPostProcess();
00052   }
00053 
00054   void BoundingBoxOcclusionRejector::subscribe()
00055   {
00056     sub_camera_info_ = pnh_->subscribe("input/camera_info", 1, &BoundingBoxOcclusionRejector::infoCallback, this);
00057     sub_target_boxes_ = pnh_->subscribe("input/target_boxes", 1, &BoundingBoxOcclusionRejector::targetBoxesCallback, this);
00058     sub_candidate_boxes_ = pnh_->subscribe("input/candidate_boxes", 1, &BoundingBoxOcclusionRejector::reject, this);
00059   }
00060 
00061   void BoundingBoxOcclusionRejector::unsubscribe()
00062   {
00063     sub_camera_info_.shutdown();
00064     sub_target_boxes_.shutdown();
00065     sub_candidate_boxes_.shutdown();
00066   }
00067 
00068   void BoundingBoxOcclusionRejector::infoCallback(
00069     const sensor_msgs::CameraInfo::ConstPtr& info_msg)
00070   {
00071     boost::mutex::scoped_lock lock(mutex_);
00072     latest_info_msg_ = info_msg;
00073   }
00074 
00075   void BoundingBoxOcclusionRejector::targetBoxesCallback(
00076     const jsk_recognition_msgs::BoundingBoxArray::ConstPtr& target_boxes_msg)
00077   {
00078     boost::mutex::scoped_lock lock(mutex_);
00079     latest_target_boxes_msg_ = target_boxes_msg;
00080   }
00081 
00082   
00083   void BoundingBoxOcclusionRejector::reject(
00084     const jsk_recognition_msgs::BoundingBoxArray::ConstPtr& candidate_boxes_msg)
00085   {
00086     boost::mutex::scoped_lock lock(mutex_);
00087 
00088     if (!latest_info_msg_ || !latest_target_boxes_msg_) {
00089       NODELET_ERROR("No camera info or target_boxes is available");
00090       return;
00091     }
00092     
00093     // All the header should be same
00094     if (latest_info_msg_->header.frame_id != latest_target_boxes_msg_->header.frame_id ||
00095         latest_target_boxes_msg_->header.frame_id != candidate_boxes_msg->header.frame_id) {
00096       NODELET_ERROR("Different frame_id");
00097       return;
00098     }
00099 
00100     image_geometry::PinholeCameraModel model;
00101     model.fromCameraInfo(*latest_info_msg_);
00102     cv::Mat image = cv::Mat::zeros(latest_info_msg_->height, latest_info_msg_->width, CV_8UC1);
00103     std::vector<std::vector<cv::Point2i> > faces;
00104     // Draw target boxes
00105     for (size_t i = 0; i < latest_target_boxes_msg_->boxes.size(); i++) {
00106       jsk_recognition_msgs::BoundingBox box = latest_target_boxes_msg_->boxes[i];
00107       // Get vertices
00108       std::vector<cv::Point3d> vertices = getVertices(box);
00109       std::vector<cv::Point2i> projected_vertices = projectVertices(vertices, model);
00110       std::vector<std::vector<cv::Point2i> > projected_faces = separateIntoFaces(projected_vertices);
00111       for (size_t j = 0; j < projected_faces.size(); j++) {
00112         faces.push_back(projected_faces[j]);
00113       }
00114     }
00115     for (size_t i = 0; i < faces.size(); i++) {
00116       std::vector<std::vector<cv::Point2i> > one_face;
00117       one_face.push_back(faces[i]);
00118       cv::fillPoly(image, one_face, cv::Scalar(255));
00119     }
00120 
00121     // Publish image
00122     if (pub_target_image_.getNumSubscribers() > 0) {
00123       cv_bridge::CvImage target_image_bridge(
00124         latest_info_msg_->header,
00125         sensor_msgs::image_encodings::MONO8,
00126         image);
00127       pub_target_image_.publish(target_image_bridge.toImageMsg());
00128     }
00129 
00130     // Reject
00131     std::vector<size_t> candidate_indices;
00132     for (size_t i = 0; i < candidate_boxes_msg->boxes.size(); i++) {
00133       //cv::Mat copied_image = image.clone();
00134       cv::Mat candidate_image = cv::Mat::zeros(latest_info_msg_->height, latest_info_msg_->width, CV_8UC1);
00135       jsk_recognition_msgs::BoundingBox box = candidate_boxes_msg->boxes[i];
00136       // Get vertices
00137       std::vector<cv::Point3d> vertices = getVertices(box);
00138       std::vector<cv::Point2i> projected_vertices = projectVertices(vertices, model);
00139       std::vector<std::vector<cv::Point2i> > projected_faces = separateIntoFaces(projected_vertices);
00140       for (size_t j = 0; j < projected_faces.size(); j++) {
00141         std::vector<std::vector<cv::Point2i> > one_face;
00142         one_face.push_back(projected_faces[j]);
00143         cv::fillPoly(candidate_image, one_face, cv::Scalar(255));
00144         if (pub_candidate_image_.getNumSubscribers() > 0) {
00145           pub_candidate_image_.publish(cv_bridge::CvImage(latest_info_msg_->header, sensor_msgs::image_encodings::MONO8,
00146                                                           candidate_image).toImageMsg());
00147         }
00148       }
00149       cv::bitwise_and(image, candidate_image, candidate_image);
00150       if (cv::countNonZero(candidate_image) == 0) {
00151         candidate_indices.push_back(i);
00152       }
00153     }
00154     jsk_recognition_msgs::BoundingBoxArray rejected_boxes;
00155     rejected_boxes.header = candidate_boxes_msg->header;
00156     for (size_t i = 0; i < candidate_indices.size(); i++) {
00157       rejected_boxes.boxes.push_back(candidate_boxes_msg->boxes[candidate_indices[i]]);
00158     }
00159     pub_.publish(rejected_boxes);
00160   }
00161 
00162   std::vector<std::vector<cv::Point2i> > BoundingBoxOcclusionRejector::separateIntoFaces(
00163     const std::vector<cv::Point2i>& vertices)
00164   {
00165     // a, b, c, d, e, f, g, h
00166     std::vector<std::vector<cv::Point2i> > ret;
00167     std::vector<cv::Point2i> face0, face1, face2, face3, face4, face5;
00168     cv::Point2i a = vertices[0];
00169     cv::Point2i b = vertices[1];
00170     cv::Point2i c = vertices[2];
00171     cv::Point2i d = vertices[3];
00172     cv::Point2i e = vertices[4];
00173     cv::Point2i f = vertices[5];
00174     cv::Point2i g = vertices[6];
00175     cv::Point2i h = vertices[7];
00176     face0.push_back(a); face0.push_back(e); face0.push_back(f); face0.push_back(b);
00177     face1.push_back(b); face1.push_back(f); face1.push_back(g); face1.push_back(c);
00178     face2.push_back(c); face2.push_back(g); face2.push_back(h); face2.push_back(d);
00179     face3.push_back(d); face3.push_back(h); face3.push_back(e); face3.push_back(a);
00180     face4.push_back(a); face4.push_back(b); face4.push_back(c); face4.push_back(d);
00181     face5.push_back(e); face5.push_back(h); face5.push_back(g); face5.push_back(f);
00182     
00183     ret.push_back(face0);
00184     ret.push_back(face1);
00185     ret.push_back(face2);
00186     ret.push_back(face3);
00187     ret.push_back(face4);
00188     ret.push_back(face5);
00189     return ret;
00190   }
00191   
00192   std::vector<cv::Point2i> BoundingBoxOcclusionRejector::projectVertices(const std::vector<cv::Point3d>& vertices,
00193                                                                          const image_geometry::PinholeCameraModel& model)
00194   {
00195     std::vector<cv::Point2i> ret;
00196     for (size_t i = 0; i < vertices.size(); i++) {
00197       ret.push_back(model.project3dToPixel(vertices[i]));
00198     }
00199     return ret;
00200   }
00201   
00202   std::vector<cv::Point3d> BoundingBoxOcclusionRejector::getVertices(const jsk_recognition_msgs::BoundingBox& box)
00203   {
00204     Eigen::Affine3f pose;
00205     tf::poseMsgToEigen(box.pose, pose);
00206     Eigen::Vector3f local_a(box.dimensions.x / 2, box.dimensions.y / 2, box.dimensions.z / 2);
00207     Eigen::Vector3f local_b(-box.dimensions.x / 2, box.dimensions.y / 2, box.dimensions.z / 2);
00208     Eigen::Vector3f local_c(-box.dimensions.x / 2, -box.dimensions.y / 2, box.dimensions.z / 2);
00209     Eigen::Vector3f local_d(box.dimensions.x / 2, -box.dimensions.y / 2, box.dimensions.z / 2);
00210     Eigen::Vector3f local_e(box.dimensions.x / 2, box.dimensions.y / 2, -box.dimensions.z / 2);
00211     Eigen::Vector3f local_f(-box.dimensions.x / 2, box.dimensions.y / 2, -box.dimensions.z / 2);
00212     Eigen::Vector3f local_g(-box.dimensions.x / 2, -box.dimensions.y / 2, -box.dimensions.z / 2);
00213     Eigen::Vector3f local_h(box.dimensions.x / 2, -box.dimensions.y / 2, -box.dimensions.z / 2);
00214     Eigen::Vector3f a = pose * local_a;
00215     Eigen::Vector3f b = pose * local_b;
00216     Eigen::Vector3f c = pose * local_c;
00217     Eigen::Vector3f d = pose * local_d;
00218     Eigen::Vector3f e = pose * local_e;
00219     Eigen::Vector3f f = pose * local_f;
00220     Eigen::Vector3f g = pose * local_g;
00221     Eigen::Vector3f h = pose * local_h;
00222     cv::Point3d cv_a(a[0], a[1], a[2]);
00223     cv::Point3d cv_b(b[0], b[1], b[2]);
00224     cv::Point3d cv_c(c[0], c[1], c[2]);
00225     cv::Point3d cv_d(d[0], d[1], d[2]);
00226     cv::Point3d cv_e(e[0], e[1], e[2]);
00227     cv::Point3d cv_f(f[0], f[1], f[2]);
00228     cv::Point3d cv_g(g[0], g[1], g[2]);
00229     cv::Point3d cv_h(h[0], h[1], h[2]);
00230     std::vector<cv::Point3d> ret;
00231     ret.push_back(cv_a);
00232     ret.push_back(cv_b);
00233     ret.push_back(cv_c);
00234     ret.push_back(cv_d);
00235     ret.push_back(cv_e);
00236     ret.push_back(cv_f);
00237     ret.push_back(cv_g);
00238     ret.push_back(cv_h);
00239     return ret;
00240   }
00241   
00242 }
00243 
00244 #include <pluginlib/class_list_macros.h>
00245 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::BoundingBoxOcclusionRejector, nodelet::Nodelet);


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Sun Oct 8 2017 02:43:49