submod_search_tsp.cpp

Go to the documentation of this file.

#include <iostream>
#include <limits>
#include <cmath>
#include <cstdlib>
#include <map>

#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/console/parse.h>
#include <pcl/point_types.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/passthrough.h>

#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>

#include <ros/ros.h>
#include <pcl_ros/transforms.h>
#include <sensor_msgs/PointCloud2.h>

#include <tf/tf.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <eigen3/Eigen/src/Core/EigenBase.h>
#include <eigen3/Eigen/src/Geometry/Quaternion.h>

#include <octomap_ros/OctomapROS.h>
#include <octomap/octomap.h>
#include <octomap_ros/conversions.h>

#include <surfacelet/SurfacePatch.h>
#include <surfacelet/SubmodMap.h>
#include <surfacelet/PatchMap.h>
#include <boost/program_options.hpp>
#include <omp.h>
using namespace std;

unsigned int text_id = 0;
pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::PointCloud<pcl::PointXYZ> cloud_nans_removed;
pcl::PointCloud<pcl::PointXYZ> cloud_not_used;
bool cloud_received = false;

void filterXYZ(pcl::PointCloud<pcl::PointXYZ> &cloud, pcl::PointCloud<pcl::PointXYZ> &cloud_filtered, double min_x,
        double max_x, double min_y, double max_y, double min_z, double max_z) {

    pcl::PassThrough<pcl::PointXYZ> pass;

    // filter x
    pass.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud));
    pass.setFilterFieldName("x");
    pass.setFilterLimits(min_x, max_x);
    pass.filter(cloud_filtered);

    // filter y
    pass.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud_filtered));
    pass.setFilterFieldName("y");
    pass.setFilterLimits(min_y, max_y);
    pass.filter(cloud_filtered);

    // filter z
    pass.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud_filtered));
    pass.setFilterFieldName("z");
    pass.setFilterLimits(min_z, max_z);
    pass.filter(cloud_filtered);

}

inline pcl::PointXYZ VectorEigen2PointXYZ(Eigen::Vector3f v) {
    pcl::PointXYZ p;
    p.x = v[0];
    p.y = v[1];
    p.z = v[2];
    return p;
}

void graphSegmentation(const Eigen::MatrixXi adjacency, std::vector<std::set<int> >& subgraphs) {

    subgraphs.clear();

    std::set<int> usable_indexes;
    for (unsigned int i = 0; i < adjacency.rows(); i++) {
        usable_indexes.insert(i);
    }

    while (usable_indexes.size() > 0) {

        std::cout << "index size " << usable_indexes.size() << std::endl;
        std::set<int> tmp_set;

        //sample point from set
        std::set<int>::iterator it = usable_indexes.begin();
        std::advance(it, rand() % usable_indexes.size());
        int index = *it;

        //check connectivity for the index
        for (int j = 0; j < adjacency.rows(); ++j) {
            if (adjacency(index, j) == 1) {
                tmp_set.insert(j);

                //remove from set
                it = usable_indexes.find(j);
                if (!(it == usable_indexes.end())) usable_indexes.erase(it);
            }
        }
        //remove from set
        it = usable_indexes.find(index);
        if (!(it == usable_indexes.end())) usable_indexes.erase(it);

        subgraphs.push_back(tmp_set);
    }
}

void depthFirstSearch(const int node, Eigen::MatrixXi& adjacency, std::set<int>& graph) {

    graph.insert(node);
    for (int i = 0; i < adjacency.cols(); ++i) {
        if (adjacency(node, i) == 1) {
            adjacency(node, i) = 0;
            depthFirstSearch(i, adjacency, graph);
        }
    }
}

void connectedComponents(const Eigen::MatrixXi& adjacency, std::vector<std::set<int> >& subgraphs) {
    subgraphs.clear();

    std::set<int> usable_indexes;
    for (unsigned int i = 0; i < adjacency.rows(); i++) {
        usable_indexes.insert(i);
    }
    Eigen::MatrixXi tmp = adjacency;

    while (usable_indexes.size() > 0) {

        std::cout << "index size " << usable_indexes.size() << std::endl;
        std::set<int> tmp_set;

        //sample point from set
        std::set<int>::iterator it = usable_indexes.begin();
        std::advance(it, rand() % usable_indexes.size());
        int index = *it;

        depthFirstSearch(index, tmp, tmp_set);

        int i = 0;
        //check connectivity for the index
        BOOST_FOREACH(i, tmp_set)
        {
            //remove from set
            it = usable_indexes.find(i);
            if (!(it == usable_indexes.end())) usable_indexes.erase(it);
        }
        if (tmp_set.size() > 1) {
            subgraphs.push_back(tmp_set);
        }
    }
}

void cloudCallBack(const sensor_msgs::PointCloud2ConstPtr& msg) {

    if (!cloud_received) {

        pcl::fromROSMsg(*msg, cloud);
        pcl::PointCloud<pcl::PointXYZ> tmp_cloud;
        filterXYZ(cloud, tmp_cloud, -0.5, 0.3, -0.5, 0.4, -2.0, 1.0);
//        filterXYZ(cloud,tmp_cloud, -2.0, 2.0, -2.0, 2.0, -2.0,2.0);
        for (unsigned int i = 0; i < tmp_cloud.points.size(); ++i) {
            if (!pcl_isnan (tmp_cloud.points[i].x) && !pcl_isnan (tmp_cloud.points[i].y)
                    && !pcl_isnan (tmp_cloud.points[i].z)) {
                cloud_nans_removed.points.push_back(tmp_cloud.points[i]);
            }
        }
        cloud_nans_removed.is_dense = false;
        cloud_nans_removed.height = 1;
        cloud_nans_removed.width = cloud_nans_removed.points.size();

        pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
        sor.setInputCloud(cloud_nans_removed.makeShared());
        sor.setMeanK(50);
        sor.setStddevMulThresh(1.0);
        sor.filter(tmp_cloud);
        cloud_nans_removed = tmp_cloud;
        cloud_received = true;
    }

}

void constructSolutionMarker(pcl::PointCloud<pcl::PointXYZ> surface_samples,
        const std::vector<std::vector<unsigned int> >& solutions, const std::string frame_id,
        std::vector<visualization_msgs::Marker>& marker_list) {

    marker_list.clear();
    visualization_msgs::Marker marker;

    // Set the frame ID and timestamp.  See the TF tutorials for information on these.
    marker.header.frame_id = frame_id;
    marker.header.stamp = ros::Time::now();

    // Set the namespace and id for this marker.  This serves to create a unique ID
    // Any marker sent with the same namespace and id will overwrite the old one
    marker.ns = "coverage";

    // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
    marker.type = visualization_msgs::Marker::LINE_LIST;

    // Set the marker action.  Options are ADD and DELETE
    marker.action = visualization_msgs::Marker::MODIFY;

    // Set the scale of the marker -- 1x1x1 here means 1m on a side
    marker.scale.x = 0.01;
    marker.scale.y = 0.01;
    marker.scale.z = 0.01;

    // Set the color -- be sure to set alpha to something non-zero!
    marker.color.r = 1;
    marker.color.g = 1;
    marker.color.b = 0.5;
    marker.color.a = 1.0;

    geometry_msgs::Point start;
    geometry_msgs::Point end;
    std::cout << "Contructing TSP markers for " << solutions.size() << " graphs " << std::endl;

    for (unsigned int i = 0; i < solutions.size(); i++) {
        marker.id = i;
        marker.points.clear();

        for (unsigned int j = 0; j < solutions[i].size() - 2; j++) {
            start.x = surface_samples[solutions[i][j]].x;
            start.y = surface_samples[solutions[i][j]].y;
            start.z = surface_samples[solutions[i][j]].z;
            end.x = surface_samples[solutions[i][j + 1]].x;
            end.y = surface_samples[solutions[i][j + 1]].y;
            end.z = surface_samples[solutions[i][j + 1]].z;

            marker.points.push_back(start);
            marker.points.push_back(end);
        }
        marker_list.push_back(marker);
    }

}

// --------------
// -----Main-----
// --------------
int main(int argc, char** argv) {

    ros::init(argc, argv, "listener");
    ros::NodeHandle n;

//      ros::Subscriber sub = n.subscribe("/kinect/depth/points_filtered", 1, cloudCallBack);
        ros::Publisher pub = n.advertise<sensor_msgs::PointCloud2>("cloud_used", 1);
        ros::Publisher pub2 = n.advertise<sensor_msgs::PointCloud2>("cloud_not_used", 1);
        ros::Publisher pub_marker = n.advertise<visualization_msgs::MarkerArray>("surface_patches", 1);
        ros::Publisher pub_marker2 = n.advertise<visualization_msgs::MarkerArray>("surface_normals", 1);

//    ros::Rate loop_rate(10);
//    while (!cloud_received) {
//
//        ros::spinOnce();
//        loop_rate.sleep();
//        cout << "No cloud received.";
//    }

//Patch construction and publishing
    std::string file("/home/hess/data/coverage_data/chair4/chair4xyz.pcd");
    std::string frame_id("/torso_lift_link");
    int pcd = pcl::io::loadPCDFile(file, cloud_nans_removed);
    surfacelet::SubmodMap map;
    pcl::PointCloud<pcl::PointXYZ> not_used_cloud;
    map.insertScan(cloud_nans_removed, not_used_cloud);

    sensor_msgs::PointCloud2 not_used_msg;
    pcl::toROSMsg(not_used_cloud, not_used_msg);
    not_used_msg.header.stamp = ros::Time::now();
    not_used_msg.header.frame_id = frame_id;
    pub2.publish(not_used_msg);

    sensor_msgs::PointCloud2 msg;
    pcl::toROSMsg(cloud_nans_removed, msg);
    msg.header.stamp = ros::Time::now();
    msg.header.frame_id = frame_id;
    pub.publish(msg);

    //Octomap construction
    pcl::PointXYZ view_point(0, 0, 0);
    view_point.data = cloud_nans_removed.sensor_origin_;
    std::cout << "Constructing Octomap" << std::endl;
    octomap::OctomapROS<octomap::OcTree> octomap(0.01);
    octomap.octree.setOccupancyThres(0.6);
    octomap.insertScan(cloud_nans_removed, view_point);

    ROS_INFO("Constructing Neigborhood Graph Object");
    std::cout << "before nh graph " << map.map_.size();
    NeighborhoodGraph nh_graph(n, map.map_, &octomap);
    nh_graph.setSurfaceDist(0.02);
    std::vector<SurfacePatch> g_patches;
    std::vector<pcl::PointXYZ> g_surface_points;
    Eigen::MatrixXi adjacency;
    std::vector<std::vector<tf::Pose> > coll_free_poses;
    std::vector<std::vector<std::vector<std::vector<double> > > > coll_free_ik;
    nh_graph.getGraph(g_patches, g_surface_points, adjacency, coll_free_poses, coll_free_ik);


    visualization_msgs::MarkerArray array;
    map.constructMarker(frame_id, array);
    pub_marker.publish(array);
    ros::Rate(10).sleep();
    pub_marker.publish(array);
    visualization_msgs::MarkerArray normal_marker;
    map.constructNormalMarker(frame_id, normal_marker);
    pub_marker2.publish(normal_marker);
    ros::Rate(10).sleep();
    pub_marker2.publish(normal_marker);

    int sum_rand_patches=0;
    int min_patches=std::numeric_limits<int>::max();
    int max_patches=std::numeric_limits<int>::min();
    for (int i = 0; i < 10000; ++i) {

        surfacelet::PatchMap p_map;
        p_map.insertScan(cloud_nans_removed, not_used_cloud);
        int curr_n =p_map.map_.size();
        sum_rand_patches +=  curr_n;
        std::cout <<"Patch map " << i  << " " << curr_n << std::endl;
        if (curr_n>max_patches) max_patches=curr_n;
        if (curr_n<min_patches) min_patches=curr_n;
    }
    std::cout <<"Submod map " << map.map_.size() << std::endl;
    double avg=(double) sum_rand_patches/ (double)10000.0;
    std::cout <<"Patch map  average" <<avg<< std::endl;
    std::cout <<"Patch map  min" <<min_patches<< std::endl;
    std::cout <<"Patch map  max" <<max_patches<< std::endl;

//        ros::Rate loop_rate(10);
//        while (!cloud_received) {
//            normal_marker.markers[]
//            ros::spinOnce();
//            loop_rate.sleep();
//        }

}