submod_search_tsp.cpp
Go to the documentation of this file.
00001 
00002 #include <iostream>
00003 #include <limits>
00004 #include <cmath>
00005 #include <cstdlib>
00006 #include <map>
00007 
00008 #include <boost/thread/thread.hpp>
00009 #include <pcl/common/common_headers.h>
00010 #include <pcl/common/common_headers.h>
00011 #include <pcl/features/normal_3d.h>
00012 #include <pcl/io/pcd_io.h>
00013 #include <pcl/console/parse.h>
00014 #include <pcl/point_types.h>
00015 #include <pcl/filters/statistical_outlier_removal.h>
00016 #include <pcl/filters/passthrough.h>
00017 
00018 #include <sensor_msgs/PointCloud2.h>
00019 #include <sensor_msgs/point_cloud_conversion.h>
00020 
00021 #include <ros/ros.h>
00022 #include <pcl_ros/transforms.h>
00023 #include <sensor_msgs/PointCloud2.h>
00024 
00025 #include <tf/tf.h>
00026 #include <visualization_msgs/Marker.h>
00027 #include <visualization_msgs/MarkerArray.h>
00028 #include <eigen3/Eigen/src/Core/EigenBase.h>
00029 #include <eigen3/Eigen/src/Geometry/Quaternion.h>
00030 
00031 #include <octomap_ros/OctomapROS.h>
00032 #include <octomap/octomap.h>
00033 #include <octomap_ros/conversions.h>
00034 
00035 #include <surfacelet/SurfacePatch.h>
00036 #include <surfacelet/SubmodMap.h>
00037 #include <surfacelet/PatchMap.h>
00038 #include <boost/program_options.hpp>
00039 #include <omp.h>
00040 using namespace std;
00041 
00042 unsigned int text_id = 0;
00043 pcl::PointCloud<pcl::PointXYZ> cloud;
00044 pcl::PointCloud<pcl::PointXYZ> cloud_nans_removed;
00045 pcl::PointCloud<pcl::PointXYZ> cloud_not_used;
00046 bool cloud_received = false;
00047 
00048 void filterXYZ(pcl::PointCloud<pcl::PointXYZ> &cloud, pcl::PointCloud<pcl::PointXYZ> &cloud_filtered, double min_x,
00049         double max_x, double min_y, double max_y, double min_z, double max_z) {
00050 
00051     pcl::PassThrough<pcl::PointXYZ> pass;
00052 
00053     // filter x
00054     pass.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud));
00055     pass.setFilterFieldName("x");
00056     pass.setFilterLimits(min_x, max_x);
00057     pass.filter(cloud_filtered);
00058 
00059     // filter y
00060     pass.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud_filtered));
00061     pass.setFilterFieldName("y");
00062     pass.setFilterLimits(min_y, max_y);
00063     pass.filter(cloud_filtered);
00064 
00065     // filter z
00066     pass.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud_filtered));
00067     pass.setFilterFieldName("z");
00068     pass.setFilterLimits(min_z, max_z);
00069     pass.filter(cloud_filtered);
00070 
00071 }
00072 
00073 inline pcl::PointXYZ VectorEigen2PointXYZ(Eigen::Vector3f v) {
00074     pcl::PointXYZ p;
00075     p.x = v[0];
00076     p.y = v[1];
00077     p.z = v[2];
00078     return p;
00079 }
00080 
00081 void graphSegmentation(const Eigen::MatrixXi adjacency, std::vector<std::set<int> >& subgraphs) {
00082 
00083     subgraphs.clear();
00084 
00085     std::set<int> usable_indexes;
00086     for (unsigned int i = 0; i < adjacency.rows(); i++) {
00087         usable_indexes.insert(i);
00088     }
00089 
00090     while (usable_indexes.size() > 0) {
00091 
00092         std::cout << "index size " << usable_indexes.size() << std::endl;
00093         std::set<int> tmp_set;
00094 
00095         //sample point from set
00096         std::set<int>::iterator it = usable_indexes.begin();
00097         std::advance(it, rand() % usable_indexes.size());
00098         int index = *it;
00099 
00100         //check connectivity for the index
00101         for (int j = 0; j < adjacency.rows(); ++j) {
00102             if (adjacency(index, j) == 1) {
00103                 tmp_set.insert(j);
00104 
00105                 //remove from set
00106                 it = usable_indexes.find(j);
00107                 if (!(it == usable_indexes.end())) usable_indexes.erase(it);
00108             }
00109         }
00110         //remove from set
00111         it = usable_indexes.find(index);
00112         if (!(it == usable_indexes.end())) usable_indexes.erase(it);
00113 
00114         subgraphs.push_back(tmp_set);
00115     }
00116 }
00117 
00118 void depthFirstSearch(const int node, Eigen::MatrixXi& adjacency, std::set<int>& graph) {
00119 
00120     graph.insert(node);
00121     for (int i = 0; i < adjacency.cols(); ++i) {
00122         if (adjacency(node, i) == 1) {
00123             adjacency(node, i) = 0;
00124             depthFirstSearch(i, adjacency, graph);
00125         }
00126     }
00127 }
00128 
00129 void connectedComponents(const Eigen::MatrixXi& adjacency, std::vector<std::set<int> >& subgraphs) {
00130     subgraphs.clear();
00131 
00132     std::set<int> usable_indexes;
00133     for (unsigned int i = 0; i < adjacency.rows(); i++) {
00134         usable_indexes.insert(i);
00135     }
00136     Eigen::MatrixXi tmp = adjacency;
00137 
00138     while (usable_indexes.size() > 0) {
00139 
00140         std::cout << "index size " << usable_indexes.size() << std::endl;
00141         std::set<int> tmp_set;
00142 
00143         //sample point from set
00144         std::set<int>::iterator it = usable_indexes.begin();
00145         std::advance(it, rand() % usable_indexes.size());
00146         int index = *it;
00147 
00148         depthFirstSearch(index, tmp, tmp_set);
00149 
00150         int i = 0;
00151         //check connectivity for the index
00152         BOOST_FOREACH(i, tmp_set)
00153         {
00154             //remove from set
00155             it = usable_indexes.find(i);
00156             if (!(it == usable_indexes.end())) usable_indexes.erase(it);
00157         }
00158         if (tmp_set.size() > 1) {
00159             subgraphs.push_back(tmp_set);
00160         }
00161     }
00162 }
00163 
00164 void cloudCallBack(const sensor_msgs::PointCloud2ConstPtr& msg) {
00165 
00166     if (!cloud_received) {
00167 
00168         pcl::fromROSMsg(*msg, cloud);
00169         pcl::PointCloud<pcl::PointXYZ> tmp_cloud;
00170         filterXYZ(cloud, tmp_cloud, -0.5, 0.3, -0.5, 0.4, -2.0, 1.0);
00171 //        filterXYZ(cloud,tmp_cloud, -2.0, 2.0, -2.0, 2.0, -2.0,2.0);
00172         for (unsigned int i = 0; i < tmp_cloud.points.size(); ++i) {
00173             if (!pcl_isnan (tmp_cloud.points[i].x) && !pcl_isnan (tmp_cloud.points[i].y)
00174                     && !pcl_isnan (tmp_cloud.points[i].z)) {
00175                 cloud_nans_removed.points.push_back(tmp_cloud.points[i]);
00176             }
00177         }
00178         cloud_nans_removed.is_dense = false;
00179         cloud_nans_removed.height = 1;
00180         cloud_nans_removed.width = cloud_nans_removed.points.size();
00181 
00182         pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
00183         sor.setInputCloud(cloud_nans_removed.makeShared());
00184         sor.setMeanK(50);
00185         sor.setStddevMulThresh(1.0);
00186         sor.filter(tmp_cloud);
00187         cloud_nans_removed = tmp_cloud;
00188         cloud_received = true;
00189     }
00190 
00191 }
00192 
00193 void constructSolutionMarker(pcl::PointCloud<pcl::PointXYZ> surface_samples,
00194         const std::vector<std::vector<unsigned int> >& solutions, const std::string frame_id,
00195         std::vector<visualization_msgs::Marker>& marker_list) {
00196 
00197     marker_list.clear();
00198     visualization_msgs::Marker marker;
00199 
00200     // Set the frame ID and timestamp.  See the TF tutorials for information on these.
00201     marker.header.frame_id = frame_id;
00202     marker.header.stamp = ros::Time::now();
00203 
00204     // Set the namespace and id for this marker.  This serves to create a unique ID
00205     // Any marker sent with the same namespace and id will overwrite the old one
00206     marker.ns = "coverage";
00207 
00208     // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
00209     marker.type = visualization_msgs::Marker::LINE_LIST;
00210 
00211     // Set the marker action.  Options are ADD and DELETE
00212     marker.action = visualization_msgs::Marker::MODIFY;
00213 
00214     // Set the scale of the marker -- 1x1x1 here means 1m on a side
00215     marker.scale.x = 0.01;
00216     marker.scale.y = 0.01;
00217     marker.scale.z = 0.01;
00218 
00219     // Set the color -- be sure to set alpha to something non-zero!
00220     marker.color.r = 1;
00221     marker.color.g = 1;
00222     marker.color.b = 0.5;
00223     marker.color.a = 1.0;
00224 
00225     geometry_msgs::Point start;
00226     geometry_msgs::Point end;
00227     std::cout << "Contructing TSP markers for " << solutions.size() << " graphs " << std::endl;
00228 
00229     for (unsigned int i = 0; i < solutions.size(); i++) {
00230         marker.id = i;
00231         marker.points.clear();
00232 
00233         for (unsigned int j = 0; j < solutions[i].size() - 2; j++) {
00234             start.x = surface_samples[solutions[i][j]].x;
00235             start.y = surface_samples[solutions[i][j]].y;
00236             start.z = surface_samples[solutions[i][j]].z;
00237             end.x = surface_samples[solutions[i][j + 1]].x;
00238             end.y = surface_samples[solutions[i][j + 1]].y;
00239             end.z = surface_samples[solutions[i][j + 1]].z;
00240 
00241             marker.points.push_back(start);
00242             marker.points.push_back(end);
00243         }
00244         marker_list.push_back(marker);
00245     }
00246 
00247 }
00248 
00249 // --------------
00250 // -----Main-----
00251 // --------------
00252 int main(int argc, char** argv) {
00253 
00254     ros::init(argc, argv, "listener");
00255     ros::NodeHandle n;
00256 
00257 //      ros::Subscriber sub = n.subscribe("/kinect/depth/points_filtered", 1, cloudCallBack);
00258         ros::Publisher pub = n.advertise<sensor_msgs::PointCloud2>("cloud_used", 1);
00259         ros::Publisher pub2 = n.advertise<sensor_msgs::PointCloud2>("cloud_not_used", 1);
00260         ros::Publisher pub_marker = n.advertise<visualization_msgs::MarkerArray>("surface_patches", 1);
00261         ros::Publisher pub_marker2 = n.advertise<visualization_msgs::MarkerArray>("surface_normals", 1);
00262 
00263 //    ros::Rate loop_rate(10);
00264 //    while (!cloud_received) {
00265 //
00266 //        ros::spinOnce();
00267 //        loop_rate.sleep();
00268 //        cout << "No cloud received.";
00269 //    }
00270 
00271 //Patch construction and publishing
00272     std::string file("/home/hess/data/coverage_data/chair4/chair4xyz.pcd");
00273     std::string frame_id("/torso_lift_link");
00274     int pcd = pcl::io::loadPCDFile(file, cloud_nans_removed);
00275     surfacelet::SubmodMap map;
00276     pcl::PointCloud<pcl::PointXYZ> not_used_cloud;
00277     map.insertScan(cloud_nans_removed, not_used_cloud);
00278 
00279     sensor_msgs::PointCloud2 not_used_msg;
00280     pcl::toROSMsg(not_used_cloud, not_used_msg);
00281     not_used_msg.header.stamp = ros::Time::now();
00282     not_used_msg.header.frame_id = frame_id;
00283     pub2.publish(not_used_msg);
00284 
00285     sensor_msgs::PointCloud2 msg;
00286     pcl::toROSMsg(cloud_nans_removed, msg);
00287     msg.header.stamp = ros::Time::now();
00288     msg.header.frame_id = frame_id;
00289     pub.publish(msg);
00290 
00291     //Octomap construction
00292     pcl::PointXYZ view_point(0, 0, 0);
00293     view_point.data = cloud_nans_removed.sensor_origin_;
00294     std::cout << "Constructing Octomap" << std::endl;
00295     octomap::OctomapROS<octomap::OcTree> octomap(0.01);
00296     octomap.octree.setOccupancyThres(0.6);
00297     octomap.insertScan(cloud_nans_removed, view_point);
00298 
00299     ROS_INFO("Constructing Neigborhood Graph Object");
00300     std::cout << "before nh graph " << map.map_.size();
00301     NeighborhoodGraph nh_graph(n, map.map_, &octomap);
00302     nh_graph.setSurfaceDist(0.02);
00303     std::vector<SurfacePatch> g_patches;
00304     std::vector<pcl::PointXYZ> g_surface_points;
00305     Eigen::MatrixXi adjacency;
00306     std::vector<std::vector<tf::Pose> > coll_free_poses;
00307     std::vector<std::vector<std::vector<std::vector<double> > > > coll_free_ik;
00308     nh_graph.getGraph(g_patches, g_surface_points, adjacency, coll_free_poses, coll_free_ik);
00309 
00310 
00311     visualization_msgs::MarkerArray array;
00312     map.constructMarker(frame_id, array);
00313     pub_marker.publish(array);
00314     ros::Rate(10).sleep();
00315     pub_marker.publish(array);
00316     visualization_msgs::MarkerArray normal_marker;
00317     map.constructNormalMarker(frame_id, normal_marker);
00318     pub_marker2.publish(normal_marker);
00319     ros::Rate(10).sleep();
00320     pub_marker2.publish(normal_marker);
00321 
00322     int sum_rand_patches=0;
00323     int min_patches=std::numeric_limits<int>::max();
00324     int max_patches=std::numeric_limits<int>::min();
00325     for (int i = 0; i < 10000; ++i) {
00326 
00327         surfacelet::PatchMap p_map;
00328         p_map.insertScan(cloud_nans_removed, not_used_cloud);
00329         int curr_n =p_map.map_.size();
00330         sum_rand_patches +=  curr_n;
00331         std::cout <<"Patch map " << i  << " " << curr_n << std::endl;
00332         if (curr_n>max_patches) max_patches=curr_n;
00333         if (curr_n<min_patches) min_patches=curr_n;
00334     }
00335     std::cout <<"Submod map " << map.map_.size() << std::endl;
00336     double avg=(double) sum_rand_patches/ (double)10000.0;
00337     std::cout <<"Patch map  average" <<avg<< std::endl;
00338     std::cout <<"Patch map  min" <<min_patches<< std::endl;
00339     std::cout <<"Patch map  max" <<max_patches<< std::endl;
00340 
00341 //        ros::Rate loop_rate(10);
00342 //        while (!cloud_received) {
00343 //            normal_marker.markers[]
00344 //            ros::spinOnce();
00345 //            loop_rate.sleep();
00346 //        }
00347 
00348 }
00349 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


surfacelet
Author(s): Juergen Hess
autogenerated on Wed Dec 26 2012 15:24:47