sigIllDemo.cpp
Go to the documentation of this file.
00001 #include <pcl/point_types.h>
00002 #include <pcl/ros/conversions.h>
00003 #include <pcl/point_cloud.h>
00004 #include <pcl/io/pcd_io.h>
00005 
00006 //#include <roboterzelle/CountBox.h>
00007 
00008 #include <sensor_msgs/PointCloud2.h>
00009 #include <ros/ros.h>
00010 #include <Eigen/StdVector>
00011 #include <Eigen/Geometry>
00012 //#include <std_srvs/Empty.h>
00013 #include <math.h>
00014 #include <stdio.h>
00015 #include <stdlib.h>
00016 #include <string.h>
00017 #include <unistd.h>
00018 #include <pcl/filters/passthrough.h>
00019 #include <pcl_ros/filters/passthrough.h>
00020 #include <pcl/features/normal_3d.h>
00021 #include <pcl/sample_consensus/ransac.h>
00022 #include <pcl/sample_consensus/impl/ransac.hpp>
00023 
00024 //#include <roboterzelle/pcl_cloud_algos/box_fit2_algo.cpp>
00025 #include <pcl_cloud_algos/box_fit2_algo.h>
00026 #include "visualization_msgs/Marker.h"
00027 #include <pcl/filters/radius_outlier_removal.h>
00028 
00029 using namespace std;
00030 using namespace pcl;
00031 
00032 
00033 
00034 bool boxFitting(boost::shared_ptr<const pcl::PointCloud <pcl::PointXYZINormal> > cloud, std::vector<double> &coeff)
00035 {
00036   pcl_cloud_algos::RobustBoxEstimation b;
00037   b.setInputCloud(cloud);
00038   b.find_model(cloud, coeff);
00039   return true;
00040 }
00041 
00042 
00043 
00044 bool process(int argc, char** argv)
00045 {
00046   if (argc != 2)
00047     {
00048       ROS_ERROR("Usage: %s <input_cloud.pcd>", argv[0]);
00049       exit(2);
00050     }
00051   
00052   sensor_msgs::PointCloud2 pc2;
00053   pcl::io::loadPCDFile(argv[1],pc2);
00054   ROS_INFO ("PointCloud has: %zu data points.", pc2.width * pc2.height);
00055 
00056 
00057 
00058   // ----- PCL objects -----
00059   pcl::PassThrough<pcl::PointXYZRGB> filter;
00060   pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
00061   pcl::KdTreeFLANN<pcl::PointXYZRGB>::Ptr pTree (new pcl::KdTreeFLANN<pcl::PointXYZRGB> ());
00062   // -----------------------
00063 
00064   // ----- Cloud declaration -----
00065   sensor_msgs::PointCloud2 pc2Cloud;
00066   pcl::PointCloud<pcl::Normal>::Ptr pNormals (new pcl::PointCloud<pcl::Normal> ());
00067   pcl::PointCloud<pcl::PointXYZRGB>::Ptr pCloud(new pcl::PointCloud< pcl::PointXYZRGB >());
00068   pcl::PointCloud<pcl::PointXYZINormal>::Ptr pCloudN(new pcl::PointCloud< pcl::PointXYZINormal >());
00069   pcl::PointCloud<pcl::PointXYZINormal> pCloudNFiltered;
00070   // -----------------------------
00071 
00072   pcl::fromROSMsg(pc2,*pCloud);
00073 
00074   // ----- Normal estimation -----
00075   ne.setSearchMethod (pTree);
00076   ne.setInputCloud (pCloud);
00077   ne.setKSearch (10);
00078   ne.compute (*pNormals);
00079   // -----------------------------
00080 
00081   pCloudN->points.resize(std::min(pNormals->points.size(),pCloud->points.size()));
00082   float sum = 0;
00083   for (size_t i = 0; i< pCloudN->points.size() ; i++)
00084     {
00085       pCloudN->points[i].x = pCloud->points[i].x;
00086       pCloudN->points[i].y = pCloud->points[i].y;
00087       pCloudN->points[i].z = pCloud->points[i].z;
00088       pCloudN->points[i].normal_x = pNormals->points[i].normal_x;
00089       pCloudN->points[i].normal_y = pNormals->points[i].normal_y;
00090       pCloudN->points[i].normal_z = pNormals->points[i].normal_z;
00091       pCloudN->points[i].intensity = 0;
00092       sum = pNormals->points[i].normal_x;
00093     }
00094   std::cerr<<sum<<std::endl;
00095   std::vector<double> coeff(15, 0.0);
00096 
00097   //remove noise
00098   pcl::RadiusOutlierRemoval<pcl::PointXYZINormal> outrem_;
00099   outrem_.setInputCloud (pCloudN);
00100   outrem_.setRadiusSearch (0.02);
00101   outrem_.setMinNeighborsInRadius (10);
00102   outrem_.filter (pCloudNFiltered);
00103 
00104   boxFitting(pCloudNFiltered.makeShared(), coeff);
00105 
00106   //publish_marker
00107   ros::NodeHandle nh_;
00108   ros::Publisher marker_pub_, cloud_pub_;
00109   //advertise as latched
00110   marker_pub_ = nh_.advertise<visualization_msgs::Marker>("box_marker", 1, true);
00111   cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("box_cloud", 1, true);
00112   visualization_msgs::Marker marker_;
00113   btMatrix3x3 box_rot (coeff[6], coeff[7], coeff[8],
00114                        coeff[9], coeff[10], coeff[11],
00115                        coeff[12], coeff[13], coeff[14]);
00116   btMatrix3x3 box_rot_trans (box_rot.transpose());
00117   btQuaternion qt;
00118   box_rot_trans.getRotation(qt);
00119   marker_.header.frame_id = "base_link";
00120   marker_.header.stamp = ros::Time::now();
00121   marker_.ns = "BoxEstimation";
00122   marker_.id = 0;
00123   marker_.type = visualization_msgs::Marker::CUBE;
00124   marker_.action = visualization_msgs::Marker::ADD;
00125   marker_.pose.position.x = coeff[0];
00126   marker_.pose.position.y = coeff[1];
00127   marker_.pose.position.z = coeff[2];
00128   marker_.pose.orientation.x = qt.x();
00129   marker_.pose.orientation.y = qt.y();
00130   marker_.pose.orientation.z = qt.z();
00131   marker_.pose.orientation.w = qt.w();
00132   marker_.scale.x = coeff[3];
00133   marker_.scale.y = coeff[4];
00134   marker_.scale.z = coeff[5];
00135   marker_.color.a = 0.75;
00136   marker_.color.r = 0.0;
00137   marker_.color.g = 1.0;
00138   marker_.color.b = 0.0;
00139 
00140 
00141   ROS_INFO("Publishing box marker and box cloud");
00142   marker_pub_.publish(marker_);
00143   pCloudNFiltered.header.frame_id = "base_link";
00144   pCloudNFiltered.header.stamp = ros::Time::now();
00145   cloud_pub_.publish(pCloudNFiltered);
00146 
00147   while (nh_.ok())
00148     {
00149       sleep(1);
00150     }
00151   return true;
00152 }
00153 
00154 int main(int argc, char** argv)
00155 {
00156   ros::init(argc, argv, "sigIllDemo");
00157   ros::NodeHandle node;
00158 
00159   process(argc, argv);
00160   return 0;
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:49