pcd_to_image_projector_algo.h
Go to the documentation of this file.
00001 /* 
00002  * Copyright (c) 2010, Dejan Pangercic <dejan.pangercic@cs.tum.edu>, 
00003  Zoltan-Csaba Marton <marton@cs.tum.edu>, Nico Blodow <blodow@cs.tum.edu>
00004  * All rights reserved.
00005  * 
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  * 
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *     * Neither the name of Willow Garage, Inc. nor the names of its
00015  *       contributors may be used to endorse or promote products derived from
00016  *       this software without specific prior written permission.
00017  * 
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
00029  */
00030 
00031 #ifndef IMAGE_ALGOS_PCD_TO_IMAGE_PROJECTOR_H
00032 #define IMAGE_ALGOS_PCD_TO_IMAGE_PROJECTOR_H
00033 #include <fstream>
00034 #include <image_algos/image_algos.h>
00035 #include <ros/ros.h>
00036 #include <ros/node_handle.h>
00037 #include <image_transport/image_transport.h>
00038 #include <opencv/cv.h>
00039 #include <opencv/highgui.h>
00040 #include <cv_bridge/CvBridge.h>
00041 #include <image_geometry/pinhole_camera_model.h>
00042 #include <tf/transform_listener.h>
00043 #include <boost/foreach.hpp>
00044 #include <tf/transform_broadcaster.h>
00045 #include <angles/angles.h>
00046 #include <sensor_msgs/PointCloud.h>
00047 #include <geometry_msgs/Polygon.h>
00048 //boost
00049 #include <boost/thread/mutex.hpp>
00050 
00051 namespace image_algos
00052 {
00053 
00054 class PCDToImageProjector : public ImageAlgo
00055 {
00056 public:
00057   PCDToImageProjector () 
00058   { 
00059   };
00060   typedef sensor_msgs::Image InputType;
00061   typedef sensor_msgs::PointCloud InputTypeII;
00062 
00063   typedef IplImage* OutputType;
00064 
00065   static std::string default_input_topic ()
00066     {return std::string ("image");}
00067 
00068   static std::string default_output_topic ()
00069     {return std::string ("roi");};
00070 
00071   static std::string default_node_name () 
00072     {return std::string ("pcd_to_image_projector_node");};
00073 
00074   void init (ros::NodeHandle&);
00075   void pre  ();
00076   void post ();
00077   std::vector<std::string> requires ();
00078   std::vector<std::string> provides ();
00079   //std::string process (const boost::shared_ptr<const InputType>);
00080   void project_3D_point(const double &x, const double &y, const double &z, int &row, int &column);
00081   std::string process (sensor_msgs::PointCloud &cloud_in, IplImage *image);
00082   std::string process (geometry_msgs::Polygon &cloud_in, IplImage *image);
00083   OutputType output ();
00084 
00085   //protected:
00086   //subscribers/publishers
00087   ros::NodeHandle nh_;
00088   image_transport::Publisher image_pub_;
00089   image_transport::Subscriber image_sub_;
00090   ros::Subscriber cloud_sub_;
00091   
00092   tf::TransformListener tf_listener_;
00093   //opencv bridge
00094   //image_transport::ImageTransport it_;
00095   sensor_msgs::CvBridge bridge_;
00096   //sensor_msgs::Image roi_;
00097   OutputType subimage_;
00098   std::string input_image_topic_, input_cloud_topic_, output_cluster_topic_, output_image_topic_;
00099   //calibration parameters for svistec cameras
00100   double focal_length_, proj_center_x_, proj_center_y_, pix_size_x_, pix_size_y_;
00101   //ROS msgs
00102   //sensor_msgs::PointCloud cloud_in_;
00103   //IplImage* image_;
00104   std::string origin_, interim_, child_;
00105   boost::mutex  cloud_lock_;
00106   std::vector <sensor_msgs::PointCloud> cloud_queue_;
00107 };
00108 
00109 }//namespace image_algos
00110 #endif
00111 
00112 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


image_algos
Author(s): Dejan Pangercic
autogenerated on Thu May 23 2013 18:41:31