depth_image_triangulation.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 CLOUD_ALGOS_DEPTH_IMAGE_TRIANGULATION_H
00032 #define CLOUD_ALGOS_DEPTH_IMAGE_TRIANGULATION_H
00033 
00034 #include <pcl_cloud_algos/cloud_algos.h>
00035 #include <pcl_cloud_algos/pcl_cloud_algos_point_types.h>
00036 
00037 // For Extra Eigen functions
00038 #include <Eigen/Core>
00039 #include <Eigen/LU> // matrix inversion
00040 #include <Eigen/Geometry> // cross product
00041 
00042 // TriangleMesh to Output Triangles
00043 #include <sensor_msgs/PointCloud.h>
00044 #include <sensor_msgs/PointCloud2.h>
00045 #include <geometry_msgs/Point32.h>
00046 #include <triangle_mesh_msgs/TriangleMesh.h>
00047 
00048 // boost
00049 #include <boost/thread/mutex.hpp>
00050 
00051 namespace pcl_cloud_algos
00052 {
00053 
00054 class DepthImageTriangulation : public CloudAlgo
00055 {
00056  public:
00058   struct triangle 
00059   {  
00060     int a,b,c;
00061   };
00062 
00063   // Input/Output type
00064   typedef sensor_msgs::PointCloud2 InputType;
00065   typedef triangle_mesh_msgs::TriangleMesh OutputType;
00066   
00067   // Topic name to advertise
00068   static std::string default_output_topic ()
00069   {
00070     return std::string ("cloud_triangulated");
00071   }
00072   
00073   // Topic name to subscribe to
00074   static std::string default_input_topic ()
00075     {
00076       return std::string ("cloud_pcd");
00077     }
00078 
00079   // Node name
00080   static std::string default_node_name ()
00081     {
00082       return std::string ("depth_image_triangulation_node");
00083     }
00084 
00085   // Algorithm methods
00086   void init (ros::NodeHandle&);
00087   void pre  ();
00088   void post ();
00089   std::vector<std::string> requires ();
00090   std::vector<std::string> provides ();
00091   std::string process (const boost::shared_ptr<const InputType>&);
00092   boost::shared_ptr<const OutputType> output ();
00093 
00098   void get_scan_and_point_id (pcl::PointCloud<pcl::PointXYZINormalScanLine> &cloud_in);
00099 
00106   float dist_3d (const pcl::PointCloud<pcl::PointXYZINormalScanLine> &cloud_in, int a, int b);
00107 
00115   void write_vtk_file (std::string output, std::vector<triangle> triangles, const pcl::PointCloud<pcl::PointXYZINormalScanLine> &cloud_in, int nr_tr);
00116 
00117   // Constructor-Destructor
00118   DepthImageTriangulation () : CloudAlgo ()
00119   {
00120     max_length = 0.05;
00121     max_index_ = max_line_ = 0;
00122     write_to_vtk_ = true;
00123     save_pcd_ = false;
00124     line_nr_in_channel_ = index_nr_in_channel_ = -1;
00125   }
00126 
00127   // DepthImageTriangulation () { }
00128   ros::Publisher createPublisher (ros::NodeHandle& nh)
00129   {
00130     ros::Publisher p = nh.advertise<OutputType> (default_output_topic (), 5);
00131     return p;
00132   }
00133 
00134 private:
00136   boost::mutex  cloud_lock_;
00137   
00138   // ROS stuff
00139   ros::NodeHandle nh_;
00140 
00142   pcl::PointCloud<pcl::PointXYZINormalScanLine> cloud_with_line_;
00143   
00145   int max_index_, max_line_;
00146   
00148   signed int line_nr_in_channel_, index_nr_in_channel_;
00149 
00151   float max_length;
00152 
00154   bool write_to_vtk_, save_pcd_;
00155 
00157   boost::shared_ptr<OutputType> mesh_;
00158 };
00159 }
00160 #endif
00161 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


pcl_cloud_algos
Author(s): Nico Blodow, Dejan Pangercic, Zoltan-Csaba Marton
autogenerated on Thu May 23 2013 15:44:48