box_fit_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 PCL_CLOUD_ALGOS_BOX_ESTIMATION_H
00032 #define PCL_CLOUD_ALGOS_BOX_ESTIMATION_H
00033 #include <pcl_cloud_algos/cloud_algos.h>
00034 //#include <mapping_msgs/PolygonalMap.h>
00035 //#include <position_string_msgs/PositionStringList.h>
00036 #include <triangle_mesh_msgs/TriangleMesh.h>
00037 //#include <point_cloud_mapping/geometry/point.h>
00038 #include <pcl_cloud_algos/pcl_cloud_algos_point_types.h>
00039 #include "visualization_msgs/Marker.h"
00040 #include "visualization_msgs/MarkerArray.h"
00041 #include <pcl/point_cloud.h>
00042 
00043 namespace pcl_cloud_algos
00044 {
00045 
00046 class BoxEstimation : public CloudAlgo
00047 {
00048  public:
00049   // should the result marker be published or only rotated
00050   bool publish_marker_; // TODO: add setter and getter and make it protected (also in cylinder fit)
00051 
00052   BoxEstimation () : CloudAlgo ()
00053   {
00054     output_box_topic_ = std::string("box_marker");
00055     threshold_in_ = 0.025;
00056     threshold_out_ = 0.00001;
00057     publish_marker_ = true;
00058   };
00059 
00060   typedef triangle_mesh_msgs::TriangleMesh OutputType;
00061   typedef sensor_msgs::PointCloud2 InputType;
00062 
00063   static std::string default_input_topic ()
00064     {return std::string ("/merged_cloud");}
00065 
00066   static std::string default_output_topic ()
00067     {return std::string ("mesh_box");};
00068 
00069   static std::string default_node_name ()
00070     {return std::string ("box_estimation_node");};
00071 
00072   void init (ros::NodeHandle&);
00073   void pre  ();
00074   void post ();
00075   std::vector<std::string> requires ();
00076   std::vector<std::string> provides ();
00077   std::string process (const boost::shared_ptr<const InputType>);
00078   boost::shared_ptr<const OutputType> output ();
00079 
00080   // Get inlier and outlier points
00081   virtual boost::shared_ptr<sensor_msgs::PointCloud2 > getInliers ();
00082   virtual boost::shared_ptr<sensor_msgs::PointCloud2 > getOutliers ();
00083   virtual boost::shared_ptr<sensor_msgs::PointCloud2 > getContained ();
00084   virtual boost::shared_ptr<pcl::PointCloud<pcl::PointXYZINormal> > getThresholdedInliers (double eps_angle);
00085   virtual void computeInAndOutliers (boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZINormal> > cloud, std::vector<double> coeff, double threshold_in, double threshold_out);
00086 
00088 
00096   virtual bool find_model (boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZINormal> > cloud, std::vector<double> &coeff);
00097 
00099 
00104   void triangulate_box (boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZINormal> > cloud, std::vector<double> &coeff);
00105 
00107 
00112   void publish_marker (boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZINormal> > cloud, std::vector<double> &coeff);
00113 
00115 
00120   void computeMarker (boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZINormal> > cloud, std::vector<double> coeff);
00121 
00123 
00128   visualization_msgs::Marker getMarker () { return marker_; }
00129 
00131 
00136   std::vector<double> getCoeff () { return coeff_; }
00137 
00138   ros::Publisher createPublisher (ros::NodeHandle& nh)
00139   {
00140     ros::Publisher p = nh.advertise<OutputType> (default_output_topic (), 5);
00141     return p;
00142   }
00143   
00148   void setInputCloud(boost::shared_ptr<const pcl::PointCloud <pcl::PointXYZINormal> > cloud)
00149   {
00150     cloud_ = boost::make_shared<pcl::PointCloud<pcl::PointXYZINormal> > (*cloud);
00151   }
00152 
00153  protected:
00154   boost::shared_ptr<OutputType> mesh_;
00155   std::vector<int> inliers_;
00156   std::vector<int> outliers_;
00157   std::vector<int> contained_;
00158   double threshold_in_;
00159   double threshold_out_;
00160 
00161   boost::shared_ptr<pcl::PointCloud <pcl::PointXYZINormal> > cloud_;
00162 
00163   std::string frame_id_;
00164   ros::NodeHandle nh_;
00165 
00166   //model rviz publisher
00167   ros::Publisher marker_pub_;
00168   ros::Publisher inliers_pub_;
00169   ros::Publisher outliers_pub_;
00170   ros::Publisher contained_pub_;
00171 
00172   //box coefficients: cx, cy, cz, dx, dy, dz, e1_x, e1y, e1z, e2_x, e2y, e2z, e3_x, e3y, e3z
00173   std::vector<double> coeff_;
00174   pcl::PointXYZINormal box_centroid_;
00175   visualization_msgs::Marker marker_;
00176 
00177   //publish box as marker
00178   std::string output_box_topic_;
00179 
00180   //point color
00181   float r_, g_, b_;
00182   //lock point cloud
00183   //boost::mutex lock;
00184 };
00185 
00186 }
00187 #endif
00188 
00189 
 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