intermittent_image_annotator_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, 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 #define BOOST_PARAMETER_MAX_ARITY 7
00036 #include "jsk_pcl_ros/intermittent_image_annotator.h"
00037 #include <tf_conversions/tf_eigen.h>
00038 #include <cv_bridge/cv_bridge.h>
00039 #include <sensor_msgs/image_encodings.h>
00040 #include <eigen_conversions/eigen_msg.h>
00041 #include "jsk_recognition_utils/pcl_conversion_util.h"
00042 #include <visualization_msgs/Marker.h>
00043 #include "jsk_recognition_utils/geo_util.h"
00044 #include <pcl/filters/extract_indices.h>
00045 namespace jsk_pcl_ros
00046 {
00047   void IntermittentImageAnnotator::onInit()
00048   {
00049     DiagnosticNodelet::onInit();
00050     listener_ = TfListenerSingleton::getInstance();
00051     last_publish_time_ = ros::Time::now();
00052     pnh_->param("fixed_frame_id", fixed_frame_id_, std::string("odom"));
00053     pnh_->param("max_image_buffer", max_image_buffer_, 5);
00054     pnh_->param("rate", rate_, 1.0);
00055     pnh_->param("store_pointcloud", store_pointcloud_, false);
00056     pnh_->param("keep_organized", keep_organized_, false);
00057     pub_pose_ = pnh_->advertise<geometry_msgs::PoseStamped>(
00058       "output/direction", 1);
00059     pub_cloud_ = pnh_->advertise<sensor_msgs::PointCloud2>(
00060       "output/cloud", 1);
00061     pub_roi_ = pnh_->advertise<jsk_recognition_msgs::PosedCameraInfo>(
00062       "output/roi", 1);
00063     pub_marker_ = pnh_->advertise<visualization_msgs::Marker>(
00064       "output/marker", 1);
00065     // resize ring buffer
00066     snapshot_buffer_ = boost::circular_buffer<SnapshotInformation::Ptr>(
00067       max_image_buffer_);
00068     image_transport::ImageTransport it(*pnh_);
00069     image_pub_ = it.advertise("output", 1);
00070     image_sub_ = it.subscribeCamera(
00071       "input/image", 1,
00072       &IntermittentImageAnnotator::cameraCallback,
00073       this);
00074     rect_sub_ = pnh_->subscribe("output/screenrectangle", 1,
00075                                 &IntermittentImageAnnotator::rectCallback,
00076                                 this);
00077     if (store_pointcloud_) {
00078       cloud_sub_ = pnh_->subscribe("input/cloud", 1,
00079                                    &IntermittentImageAnnotator::cloudCallback,
00080                                    this);
00081     }
00082     shutter_service_ = pnh_->advertiseService(
00083       "shutter",
00084       &IntermittentImageAnnotator::shutterCallback, this);
00085     request_service_ = pnh_->advertiseService(
00086       "request",
00087       &IntermittentImageAnnotator::requestCallback, this);
00088     clear_service_ = pnh_->advertiseService(
00089       "clear",
00090       &IntermittentImageAnnotator::clearCallback, this);
00091     onInitPostProcess();
00092   }
00093 
00094   // we donnot use subscribe/unsubscribe on demand
00095   void IntermittentImageAnnotator::subscribe()
00096   {
00097   }
00098 
00099   void IntermittentImageAnnotator::unsubscribe()
00100   {
00101   }
00102 
00103   void IntermittentImageAnnotator::rectCallback(
00104     const geometry_msgs::PolygonStamped::ConstPtr& rect)
00105   {
00106     boost::mutex::scoped_lock lock(mutex_);
00107     int x0 = rect->polygon.points[0].x;
00108     int x1 = rect->polygon.points[1].x;
00109     int y0 = rect->polygon.points[0].y;
00110     int y1 = rect->polygon.points[1].y;
00111     if (x0 > x1) {
00112       std::swap(x0, x1);
00113     }
00114     if (y0 > y1) {
00115       std::swap(y0, y1);
00116     }
00117     // check x region
00118     int width = latest_image_msg_->width;
00119     int x0_index = x0 / width;
00120     int x1_index = x1 / width;
00121     if (x0_index != x1_index) {
00122       NODELET_WARN("malformed rectangle");
00123       return;
00124     }
00125     else {
00126       int image_index = x0_index;
00127       NODELET_INFO("image index: %d", image_index);
00128       SnapshotInformation::Ptr info = snapshot_buffer_[image_index];
00129       // local point
00130       int width_offset = width * image_index;
00131       int x0_wo_offset = x0 - width_offset;
00132       int x1_wo_offset = x1 - width_offset;
00133       cv::Point2d mid((x0_wo_offset + x1_wo_offset) / 2.0,
00134                       (y0 + y1) / 2.0);
00135       Eigen::Affine3d pose(info->camera_pose_);
00136       image_geometry::PinholeCameraModel camera_model = info->camera_;
00137       cv::Point3d mid_3d = camera_model.projectPixelTo3dRay(mid);
00138       Eigen::Vector3d ray(mid_3d.x, mid_3d.y, mid_3d.z); // ray is camera local
00139       ray = ray / ray.norm();
00140       Eigen::Vector3d ray_global = pose.rotation() * ray;
00141       NODELET_INFO("ray: [%f, %f, %f]", ray_global[0], ray_global[1], ray_global[2]);
00142       
00143       Eigen::Vector3d z = pose.rotation() * Eigen::Vector3d::UnitZ();
00144       NODELET_INFO("z: [%f, %f, %f]", z[0], z[1], z[2]);
00145       Eigen::Vector3d original_pos = pose.translation();
00146       Eigen::Quaterniond q;
00147       q.setFromTwoVectors(z, ray_global);
00148       NODELET_INFO("q: [%f, %f, %f, %f]", q.x(), q.y(), q.z(), q.w());
00149       Eigen::Affine3d output_pose = pose.rotate(q);
00150       output_pose.translation() = original_pos;
00151       geometry_msgs::PoseStamped ros_pose;
00152       tf::poseEigenToMsg(output_pose, ros_pose.pose);
00153       ros_pose.header.stamp = latest_image_msg_->header.stamp;
00154       ros_pose.header.frame_id = fixed_frame_id_;
00155       pub_pose_.publish(ros_pose);
00156 
00157       // publish ROI
00158       jsk_recognition_msgs::PosedCameraInfo camera_info;
00159       camera_info.header.stamp = latest_image_msg_->header.stamp;
00160       camera_info.header.frame_id = fixed_frame_id_;
00161       camera_info.camera_info
00162         = sensor_msgs::CameraInfo(info->camera_.cameraInfo());
00163       camera_info.camera_info.roi.x_offset = x0_wo_offset;
00164       camera_info.camera_info.roi.y_offset = y0;
00165       camera_info.camera_info.roi.width = x1_wo_offset - x0_wo_offset;
00166       camera_info.camera_info.roi.height = y1 - y0;
00167       tf::poseEigenToMsg(info->camera_pose_, camera_info.offset);
00168       pub_roi_.publish(camera_info);
00169       
00170       // marker
00171       // 2d points
00172       cv::Point2d A(x0_wo_offset, y0);
00173       cv::Point2d B(x0_wo_offset, y1);
00174       cv::Point2d C(x1_wo_offset, y1);
00175       cv::Point2d D(x1_wo_offset, y0);
00176       // 3d local points
00177       cv::Point3d A_3d = info->camera_.projectPixelTo3dRay(A) * 3;
00178       cv::Point3d B_3d = info->camera_.projectPixelTo3dRay(B) * 3;
00179       cv::Point3d C_3d = info->camera_.projectPixelTo3dRay(C) * 3;
00180       cv::Point3d D_3d = info->camera_.projectPixelTo3dRay(D) * 3;
00181       cv::Point3d O_3d;
00182       // convert to ros point
00183       geometry_msgs::Point A_ros, B_ros, C_ros, D_ros, O_ros;
00184       jsk_recognition_utils::pointFromXYZToXYZ<cv::Point3d, geometry_msgs::Point>(A_3d, A_ros);
00185       jsk_recognition_utils::pointFromXYZToXYZ<cv::Point3d, geometry_msgs::Point>(B_3d, B_ros);
00186       jsk_recognition_utils::pointFromXYZToXYZ<cv::Point3d, geometry_msgs::Point>(C_3d, C_ros);
00187       jsk_recognition_utils::pointFromXYZToXYZ<cv::Point3d, geometry_msgs::Point>(D_3d, D_ros);
00188       jsk_recognition_utils::pointFromXYZToXYZ<cv::Point3d, geometry_msgs::Point>(O_3d, O_ros);
00189       // build edges
00190       visualization_msgs::Marker marker;
00191       marker.header.stamp = latest_image_msg_->header.stamp;
00192       marker.header.frame_id = fixed_frame_id_;
00193       tf::poseEigenToMsg(info->camera_pose_, marker.pose);
00194       marker.type = visualization_msgs::Marker::LINE_LIST;
00195       marker.points.push_back(O_ros); marker.points.push_back(A_ros);
00196       marker.points.push_back(O_ros); marker.points.push_back(B_ros);
00197       marker.points.push_back(O_ros); marker.points.push_back(C_ros);
00198       marker.points.push_back(O_ros); marker.points.push_back(D_ros);
00199       marker.points.push_back(A_ros); marker.points.push_back(B_ros);
00200       marker.points.push_back(B_ros); marker.points.push_back(C_ros);
00201       marker.points.push_back(C_ros); marker.points.push_back(D_ros);
00202       marker.points.push_back(D_ros); marker.points.push_back(A_ros);
00203       marker.scale.x = 0.01;
00204       marker.scale.y = 0.01;
00205       marker.scale.z = 0.01;
00206       marker.color.a = 1.0;
00207       marker.color.r = 1.0;
00208       pub_marker_.publish(marker);
00209 
00210       // crop pointcloud
00211       if (store_pointcloud_) {
00212         publishCroppedPointCloud(info->cloud_,
00213                                  A_3d, B_3d, C_3d, D_3d,
00214                                  info->camera_pose_);
00215       }
00216     }
00217   }
00218   
00219 
00220   void IntermittentImageAnnotator::publishCroppedPointCloud(
00221     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
00222     const cv::Point3d& A, const cv::Point3d& B,
00223     const cv::Point3d& C, const cv::Point3d& D,
00224     const Eigen::Affine3d& pose)
00225   {
00226     Eigen::Vector3f A_eigen, B_eigen, C_eigen, D_eigen;
00227     pointFromXYZToVector<cv::Point3d, Eigen::Vector3f>(
00228         A, A_eigen);
00229     pointFromXYZToVector<cv::Point3d, Eigen::Vector3f>(
00230         B, B_eigen);
00231     pointFromXYZToVector<cv::Point3d, Eigen::Vector3f>(
00232         C, C_eigen);
00233     pointFromXYZToVector<cv::Point3d, Eigen::Vector3f>(
00234         D, D_eigen);
00235     Eigen::Affine3f posef;
00236     convertEigenAffine3(pose, posef);
00237     Eigen::Vector3f A_global = posef * A_eigen;
00238     Eigen::Vector3f B_global = posef * B_eigen;
00239     Eigen::Vector3f C_global = posef * C_eigen;
00240     Eigen::Vector3f D_global = posef * D_eigen;
00241     Eigen::Vector3f O_global = posef.translation();
00242     jsk_recognition_utils::Vertices vertices0, vertices1, vertices2, vertices3;
00243     vertices0.push_back(O_global); vertices0.push_back(A_global); vertices0.push_back(D_global);
00244     vertices1.push_back(O_global); vertices1.push_back(B_global); vertices1.push_back(A_global);
00245     vertices2.push_back(O_global); vertices2.push_back(C_global); vertices2.push_back(B_global);
00246     vertices3.push_back(O_global); vertices3.push_back(D_global); vertices3.push_back(C_global);
00247     Polygon::Ptr plane0 (new Polygon(vertices0));
00248     Polygon::Ptr plane1 (new Polygon(vertices1));
00249     Polygon::Ptr plane2 (new Polygon(vertices2));
00250     Polygon::Ptr plane3 (new Polygon(vertices3));
00251     pcl::PointIndices::Ptr indices(new pcl::PointIndices);
00252     for (size_t i = 0; i < cloud->points.size(); i++) {
00253       pcl::PointXYZRGB p = cloud->points[i];
00254       Eigen::Vector3f pf = p.getVector3fMap();
00255       if (!std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.z)) {
00256         if (plane0->signedDistanceToPoint(pf) > 0 &&
00257             plane1->signedDistanceToPoint(pf) > 0 &&
00258             plane2->signedDistanceToPoint(pf) > 0 &&
00259             plane3->signedDistanceToPoint(pf) > 0) {
00260           indices->indices.push_back(i);
00261         }
00262       }
00263     }
00264     pcl::ExtractIndices<pcl::PointXYZRGB> ex;
00265     pcl::PointCloud<pcl::PointXYZRGB>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00266     ex.setInputCloud(cloud);
00267     ex.setKeepOrganized(keep_organized_);
00268     ex.setIndices(indices);
00269     ex.filter(*output_cloud);
00270     sensor_msgs::PointCloud2 ros_cloud;
00271     pcl::toROSMsg(*output_cloud, ros_cloud);
00272     ros_cloud.header.stamp = latest_image_msg_->header.stamp;
00273     ros_cloud.header.frame_id = fixed_frame_id_;
00274     pub_cloud_.publish(ros_cloud);
00275   }
00276 
00277   void IntermittentImageAnnotator::cloudCallback(
00278       const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
00279   {
00280     boost::mutex::scoped_lock lock(mutex_);
00281     latest_cloud_msg_ = cloud_msg;
00282   }
00283 
00284   void IntermittentImageAnnotator::cameraCallback(
00285       const sensor_msgs::Image::ConstPtr& image_msg,
00286       const sensor_msgs::CameraInfo::ConstPtr& info_msg)
00287   {
00288     boost::mutex::scoped_lock lock(mutex_);
00289     vital_checker_->poke();
00290     latest_image_msg_ = image_msg;
00291     latest_camera_info_msg_ = info_msg;
00292 
00293     if (snapshot_buffer_.size() != 0) {
00294       ros::Time now = ros::Time::now();
00295       if ((now - last_publish_time_).toSec() > 1.0 / rate_) {
00296         cv::Mat concatenated_image;
00297         std::vector<cv::Mat> images;
00298         //ROS_INFO("%lu images", snapshot_buffer_.size());
00299         for (size_t i = 0; i < snapshot_buffer_.size(); i++) {
00300           images.push_back(snapshot_buffer_[i]->image_);
00301         }
00302         cv::hconcat(images, concatenated_image);
00303         cv_bridge::CvImage concatenate_bridge(latest_camera_info_msg_->header, // ??
00304                                               sensor_msgs::image_encodings::BGR8,
00305                                               concatenated_image);
00306         image_pub_.publish(concatenate_bridge.toImageMsg());
00307         last_publish_time_ = now;
00308       }
00309     }
00310     
00311   }
00312 
00313   void IntermittentImageAnnotator::waitForNextImage()
00314   {
00315     ros::Time now = ros::Time::now();
00316     ros::Rate r(10);
00317     while (ros::ok()) {
00318       {
00319         boost::mutex::scoped_lock lock(mutex_);
00320         if (latest_image_msg_ && latest_image_msg_->header.stamp > now) {
00321           return;
00322         }
00323       }
00324       r.sleep();
00325     }
00326   }
00327  
00328   bool IntermittentImageAnnotator::shutterCallback(
00329     std_srvs::Empty::Request& req,
00330     std_srvs::Empty::Response& res)
00331   {
00332     
00333     waitForNextImage();
00334     {
00335       boost::mutex::scoped_lock lock(mutex_);
00336       if (latest_camera_info_msg_) {
00337         SnapshotInformation::Ptr
00338           info (new SnapshotInformation());
00339         // resolve tf
00340         try {
00341           if (listener_->waitForTransform(
00342                 fixed_frame_id_,
00343                 latest_camera_info_msg_->header.frame_id,
00344                 latest_camera_info_msg_->header.stamp,
00345                 ros::Duration(1.0))) {
00346             tf::StampedTransform transform;
00347             listener_->lookupTransform(fixed_frame_id_,
00348                                        latest_camera_info_msg_->header.frame_id,
00349                                        latest_camera_info_msg_->header.stamp,
00350                                        transform);
00351             cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(
00352               latest_image_msg_,
00353               sensor_msgs::image_encodings::BGR8);
00354             Eigen::Affine3d eigen_transform;
00355             image_geometry::PinholeCameraModel camera;
00356             camera.fromCameraInfo(latest_camera_info_msg_);
00357             tf::transformTFToEigen(transform, eigen_transform);
00358             info->camera_pose_ = eigen_transform;
00359             info->camera_ = camera;
00360             info->image_ = cv_ptr->image;
00361             if (store_pointcloud_) {
00362               // use pointcloud
00363               if (!latest_cloud_msg_) {
00364                 NODELET_ERROR("no pointcloud is available");
00365                 return false;
00366               }
00367               // transform pointcloud to fixed frame
00368               pcl::PointCloud<pcl::PointXYZRGB>::Ptr 
00369                 nontransformed_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00370               pcl::PointCloud<pcl::PointXYZRGB>::Ptr 
00371                 transformed_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00372               pcl::fromROSMsg(*latest_cloud_msg_, *nontransformed_cloud);
00373               if (pcl_ros::transformPointCloud(fixed_frame_id_, 
00374                                                *nontransformed_cloud,
00375                                                *transformed_cloud,
00376                                                *listener_)) {
00377                 info->cloud_ = transformed_cloud;
00378               }
00379               else {
00380                 NODELET_ERROR("failed to transform pointcloud");
00381                 return false;
00382               }
00383             }
00384             snapshot_buffer_.push_back(info);
00385             return true;
00386           }
00387           else {
00388             NODELET_ERROR("failed to resolve tf from %s to %s",
00389                           fixed_frame_id_.c_str(),
00390                           latest_camera_info_msg_->header.frame_id.c_str());
00391             return false;
00392           }
00393         }
00394         catch (tf2::ConnectivityException &e)
00395         {
00396           NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
00397           return false;
00398         }
00399         catch (tf2::InvalidArgumentException &e)
00400         {
00401           NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
00402           return false;
00403         }
00404       }
00405       else {
00406         NODELET_ERROR("not yet camera message is available");
00407         return false;
00408       }
00409     }
00410   }
00411 
00412   bool IntermittentImageAnnotator::clearCallback(
00413     std_srvs::Empty::Request& req,
00414     std_srvs::Empty::Response& res)
00415   {
00416     boost::mutex::scoped_lock lock(mutex_);
00417     snapshot_buffer_.clear();
00418     return true;
00419   }
00420 
00421   bool IntermittentImageAnnotator::requestCallback(
00422     std_srvs::Empty::Request& req,
00423     std_srvs::Empty::Response& res)
00424   {
00425     // concatenate images
00426     boost::mutex::scoped_lock lock(mutex_);
00427     if (snapshot_buffer_.size() == 0) {
00428       NODELET_ERROR("no image is stored");
00429       return false;
00430     }
00431     else {
00432       cv::Mat concatenated_image;
00433       std::vector<cv::Mat> images;
00434       ROS_INFO("%lu images", snapshot_buffer_.size());
00435       for (size_t i = 0; i < snapshot_buffer_.size(); i++) {
00436         images.push_back(snapshot_buffer_[i]->image_);
00437       }
00438       cv::hconcat(images, concatenated_image);
00439       cv_bridge::CvImage concatenate_bridge(latest_camera_info_msg_->header, // ??
00440                                             sensor_msgs::image_encodings::BGR8,
00441                                             concatenated_image);
00442       image_pub_.publish(concatenate_bridge.toImageMsg());
00443       return true;
00444     }
00445   }
00446 }
00447 
00448 #include <pluginlib/class_list_macros.h>
00449 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::IntermittentImageAnnotator, nodelet::Nodelet);


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Tue Jul 2 2019 19:41:43