NeigborhoodGraph.cpp

Go to the documentation of this file.

#include <coverage_3d_planning/NeighborhoodGraph.h>
#include <coverage_3d_tools/conversions.h>
#include <coverage_3d_arm_navigation/openrave_ik.h>
#include <eigen3/Eigen/src/Core/EigenBase.h>
#include <eigen3/Eigen/src/Geometry/Quaternion.h>
#include <pcl/kdtree/kdtree_flann.h>

#include <tf/tf.h>
#include <voro/voro++.hh>

NeighborhoodGraph::NeighborhoodGraph(ros::NodeHandle& nh) :
                nh_(nh), dist_above_surface_(0.06), max_neigborhood_(8), max_neigborhood_radius_(0.08), octomap_(0), coll_checker_(
                                0), n_ik_samples_(1), coll_check_(true) {
        if (coll_check_) {
                coll_checker_ = new CollisionCheck(nh_);
        }
}
NeighborhoodGraph::NeighborhoodGraph(ros::NodeHandle& nh, bool coll_check) :
                nh_(nh), dist_above_surface_(0.06), max_neigborhood_(8), max_neigborhood_radius_(0.08), octomap_(0), coll_checker_(
                                0), n_ik_samples_(1), coll_check_(coll_check) {
        if (coll_check_) {
                coll_checker_ = new CollisionCheck(nh_);
        }
}

NeighborhoodGraph::NeighborhoodGraph(ros::NodeHandle& nh, std::vector<SurfacePatch> patches,
                octomap::OctomapROS<octomap::OcTree>* octomap) :
                nh_(nh), dist_above_surface_(0.06), max_neigborhood_(8), max_neigborhood_radius_(0.08), octomap_(0), coll_checker_(
                                0), n_ik_samples_(1), coll_check_(true) {
        surface_ = patches;
        octomap_ = octomap;
        if (coll_check_) {
                coll_checker_ = new CollisionCheck(nh_);
        }
}

NeighborhoodGraph::NeighborhoodGraph(ros::NodeHandle& nh, std::vector<SurfacePatch> patches,
                octomap::OctomapROS<octomap::OcTree>* octomap, double dist_above_surface, int max_neigborhood,
                double max_neigborhood_radius, int n_ik_samples, bool coll_check) :
                nh_(nh), dist_above_surface_(dist_above_surface), max_neigborhood_(max_neigborhood), max_neigborhood_radius_(
                                max_neigborhood_radius), octomap_(0), coll_checker_(0), n_ik_samples_(n_ik_samples), coll_check_(
                                coll_check) {
        surface_ = patches;
        octomap_ = octomap;
        if (coll_check_) {
                coll_checker_ = new CollisionCheck(nh_);
        }
}

NeighborhoodGraph::~NeighborhoodGraph() {
        if (coll_check_) {
                delete coll_checker_;
        }
        //      delete octomap_;
}
void NeighborhoodGraph::setPatches(std::vector<SurfacePatch> patches) {
        surface_ = patches;
}
void NeighborhoodGraph::setSurfaceDist(double dist) {
        dist_above_surface_ = dist;
}

void NeighborhoodGraph::setOctomap(octomap::OctomapROS<octomap::OcTree>* octomap) {
        octomap_ = octomap;
}

void NeighborhoodGraph::getReachableVector(std::vector<bool>& reachable) {
        reachable = reachable_surface_;
}
void NeighborhoodGraph::sampleSurfacePoints(const std::vector<SurfacePatch>& surface,
                std::vector<pcl::PointXYZ>& samples) const {

        samples.clear();
        float dist = dist_above_surface_;

        for (unsigned int i = 0; i < surface.size(); ++i) {
                //compute point above surface
                SurfacePatch patch = surface[i];
                Eigen::Vector3f tmp = patch.getVector3fMap() + (dist * patch.getNormalVector3fMap().normalized());
                tmp = tmp + ((patch.scale_z / 2.0) * patch.getNormalVector3fMap().normalized());
                samples.push_back(VectorEigen2PointXYZ(tmp));
        }
}
void NeighborhoodGraph::sampleSurfacePoints(const std::vector<SurfacePatch>& surface,
                pcl::PointCloud<pcl::PointXYZ>& samples) const {

        samples.clear();
        float dist = dist_above_surface_;

        for (unsigned int i = 0; i < surface.size(); ++i) {
                //compute point above surface
                SurfacePatch patch = surface[i];
                Eigen::Vector3f tmp = patch.getVector3fMap() + (dist * patch.getNormalVector3fMap().normalized());
                tmp = tmp + ((patch.scale_z / 2.0) * patch.getNormalVector3fMap().normalized());
                samples.push_back(VectorEigen2PointXYZ(tmp));
        }
}
void NeighborhoodGraph::buildAdjacencyMatrixUsingOctomap() {

        //build the graph
        ROS_INFO_STREAM("NeighborhoodGraph Computing Adjacency Matrix from " << surface_.size() << "patches");
        
        pcl::PointCloud<pcl::PointXYZ> samples;
        sampleSurfacePoints(surface_, samples);
        octomap::Pointcloud octocloud;
        octomap::pointcloudPCLToOctomap(samples, octocloud);

        pcl::KdTreeFLANN<pcl::PointXYZ> tree;
        tree.setInputCloud(samples.makeShared());

        adjacency_ = Eigen::MatrixXi::Zero(octocloud.size(), octocloud.size());
        ROS_DEBUG_STREAM("NeighborhoodGraph::buildAdjacencyMatrixUsingOctomap " << octocloud.size() << " " << samples.size());
        ROS_DEBUG_STREAM("NeighborhoodGraph::buildAdjacencyMatrixUsingOctomap max neighborhood " << max_neigborhood_ << " " << max_neigborhood_radius_);
        omp_set_num_threads(6);
        for (unsigned int i = 0; i < octocloud.size(); ++i) {
                std::vector<int> inlier;
                std::vector<float> dist;
                tree.radiusSearch(samples[i], max_neigborhood_radius_, inlier, dist);

                //use a maximum of inliers
                unsigned int n_inliers = max_neigborhood_;
                if (inlier.size() < n_inliers) {
                        n_inliers = inlier.size();
                }
#pragma omp parallel for
                for (unsigned int j = 0; j < n_inliers; ++j) {
                        octomap::point3d origin = octocloud.getPoint(i);
                        octomap::point3d end = octocloud.getPoint(inlier[j]);
                        octomap::point3d dir = end - origin;
                        if (i != (unsigned int) inlier[j]) {
                                float dist = dir.norm();
                                //                              if (dist != 0) {

                                if (!octomap_->octree.castRay(origin, dir, end, false, dist)) {
                                        if (octomap_->octree.search(end)) { //if search did not end in an unknown cell
                                                adjacency_(i, inlier[j]) = 1;
                                                adjacency_(inlier[j], i) = 1;
                                        }
                                }
                                //                              }
                        }
                }
        }
        std::cout << "size Surface Adjacency Matrix " << surface_.size() << " " << adjacency_.rows() << " "
                        << adjacency_.cols() << std::endl;
}

void NeighborhoodGraph::buildAdjacencyMatrixUsingVoronoi() {

        //build the graph
        std::cout << "Computing Adjacency Matrix using Voronoi Graph" << surface_.size() << std::endl;

        pcl::PointCloud<pcl::PointXYZ> samples;
        sampleSurfacePoints(surface_, samples);
        octomap::Pointcloud octocloud;
        octomap::pointcloudPCLToOctomap(samples, octocloud);

        Eigen::MatrixXf e_samples = samples.getMatrixXfMap();
        adjacency_ = Eigen::MatrixXi::Zero(surface_.size(), surface_.size());

        //get the size of the box holding the voronoi graph
        //make the box a little larger so that it also contains the points at the edge of the box
        double x_min = e_samples.row(0).array().minCoeff() - 0.001;
        double x_max = e_samples.row(0).array().maxCoeff() + 0.001;
        double y_min = e_samples.row(1).array().minCoeff() - 0.001;
        double y_max = e_samples.row(1).array().maxCoeff() + 0.001;
        double z_min = e_samples.row(2).array().minCoeff() - 0.001;
        double z_max = e_samples.row(2).array().maxCoeff() + 0.001;

        //set the number of computational units along x,y and z axis
        int n_x = 1, n_y = 1, n_z = 1;

        //construct the voro++ container
        voro::container con(x_min, x_max, y_min, y_max, z_min, z_max, n_x, n_y, n_z, false, false, false, 10000);

        //insert the surface points in the container
        for (unsigned int i = 0; i < e_samples.cols(); i++) {
                con.put(i, e_samples(0, i), e_samples(1, i), e_samples(2, i));
        }

        //loop over all voronoi cells while computing the voronoi cells and their neighbors
        voro::c_loop_all vl(con); //weird looping construct
        voro::voronoicell_neighbor c;
        if (vl.start())
                do
                        if (con.compute_cell(c, vl)) {
                                int idx1 = con.id[vl.ijk][vl.q]; // getting the id of the previously inserted point --> a bit tricky
                                int idx2 = 0;
                                std::vector<int> local_neighbors;
                                c.neighbors(local_neighbors);
                                for (unsigned int j = 0; j < local_neighbors.size(); ++j) {

                                        // in case local_neighbors[j]<0 the neighbor is a side of the container
                                        if (local_neighbors[j] >= 0) {
                                                idx2 = local_neighbors[j];
                                                octomap::point3d origin = octocloud.getPoint(idx1);
                                                octomap::point3d end = octocloud.getPoint(idx2);
                                                octomap::point3d dir = end - origin;
                                                float dist = dir.norm();
                                                //check if we can cast a ray between the corressponding nodes in the octomap
                                                if (!octomap_->octree.castRay(origin, dir, end, false, dist)) {
                                                        //if search did not end in an unknown cell
                                                        if (octomap_->octree.search(end)) {
                                                                if (dist < 0.5) {
                                                                        adjacency_(idx1, idx2) = 1;
                                                                        adjacency_(idx2, idx1) = 1;
                                                                }
                                                        }
                                                }
                                        }
                                }
                        } while (vl.inc());

//    // Sum up the volumes, and check that this matches the container volume
//    const double cvol = (x_max - x_min) * (y_max - y_min) * (z_max * z_min);
//    double vvol = con.sum_cell_volumes();
//    printf("Container volume : %g\n"
//        "Voronoi volume   : %g\n"
//        "Difference       : %g\n", cvol, vvol, vvol - cvol);
//    // Output the particle positions in gnuplot format
//    con.draw_particles("random_points_p.gnu");
//    // Output the Voronoi cells in gnuplot format
//    con.draw_cells_gnuplot("random_points_v.gnu");

}
bool NeighborhoodGraph::isPoseValid(const tf::Pose pose, const bool check_collision,
                std::vector<std::vector<double> >& ik_solutions) {


        ik_solutions.clear();
        std::vector<std::vector<double> > solutions;
        getIKSolutions(pose, solutions);

        if (solutions.size() > 0) {

                //if we shall check for collision also
                for (unsigned int j = 0; j < solutions.size(); ++j) {
                        if (check_collision) {
                                if (coll_checker_->checkJointStateRight(solutions[j])) {
                                        ik_solutions.push_back(solutions[j]);
                                }
                        } else {
                                ik_solutions.push_back(solutions[j]);
                        }
                }
        }
        if (ik_solutions.size() > 0) {
                return true;
        } else {
                return false;
        }
}

void NeighborhoodGraph::getValidIKSolutions(const std::vector<tf::Pose>& poses, const bool check_collision,
                std::vector<bool>& valid, std::vector<std::vector<std::vector<double> > >& ik_solutions) {

        valid.resize(poses.size(), false);
        ik_solutions.resize(poses.size());

        for (unsigned int i = 0; i < poses.size(); ++i) {
                std::vector<std::vector<double> > tmp_solutions;
                if (isPoseValid(poses[i], check_collision, tmp_solutions)) {
                        valid[i] = true;
                        ik_solutions[i] = tmp_solutions;
                }
        }
}

void NeighborhoodGraph::depthFirstConnectedComponentSearch(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;
                        depthFirstConnectedComponentSearch(i, adjacency, graph);
                }
        }
}
void NeighborhoodGraph::computeConnectedComponents() {
        subgraphs_.clear();

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

        std::set<int> tmp_set;
        while (usable_indexes.size() > 0) {

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

                depthFirstConnectedComponentSearch(index, tmp, tmp_set);

                //check connectivity for the index
                BOOST_FOREACH(int 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 NeighborhoodGraph::getSubgraphAdjacency(const std::set<int>& subgraph, std::map<int, int>& index_map,
Eigen::MatrixXi& subgraph_adj, const std::vector<SurfacePatch>& patches,
std::vector<SurfacePatch>& subgraph_patches) {

        index_map.clear();
        subgraph_patches.clear();
        subgraph_adj = Eigen::MatrixXi::Zero(subgraph.size(), subgraph.size());
        int k = 0, l = 0;
        BOOST_FOREACH(int i, subgraph)
                        {
                                l = 0;
                                index_map.insert(std::pair<int, int>(k, i));
                                BOOST_FOREACH(int j, subgraph)
                                                {
                                                        subgraph_adj(k, l) = adjacency_(i, j);

                                                        l++;
                                                }
                                subgraph_patches.push_back(patches[i]);
                                k++;
                        }
}

void NeighborhoodGraph::getIKSolutions(const tf::Pose p, std::vector<std::vector<double> >& ik_solutions) const {
        ik_solutions.clear();
        int n_free_joints = 1;
        int n_joints = 7;

        double vfree[n_free_joints]; //we have one free parameter
        double eerot[9], eetrans[3];

        tf::Matrix3x3 rot = p.getBasis();
        tf::Vector3 trans = p.getOrigin();

        eerot[0] = rot[0][0];
        eerot[1] = rot[0][1];
        eerot[2] = rot[0][2];
        eerot[3] = rot[1][0];
        eerot[4] = rot[1][1];
        eerot[5] = rot[1][2];
        eerot[6] = rot[2][0];
        eerot[7] = rot[2][1];
        eerot[8] = rot[2][2];

        eetrans[0] = trans[0];
        eetrans[1] = trans[1];
        eetrans[2] = trans[2];

        //set the free parameters -- in range -pi -- pi: for right now use the middle: 0
        //free joint limits of r_upper_arm_roll_joint (29) in openrave are -3.9 and 0.79
        //good setting: vfree[0] =-1.51;

        std::vector<float> vfree_samples;
        if (n_ik_samples_ == 1) {
                vfree_samples.push_back(-1.51);
        } else if (n_ik_samples_ == 2) {
                vfree_samples.push_back(-1.75);
                vfree_samples.push_back(-1.27);
        } else if (n_ik_samples_ == 3) {
                vfree_samples.push_back(-1.51);
                vfree_samples.push_back(-1.75);
                vfree_samples.push_back(-1.27);
        } else if (n_ik_samples_ == 5) {
                vfree_samples.push_back(-1.51);
                vfree_samples.push_back(-1.75);
                vfree_samples.push_back(-1.27);
                vfree_samples.push_back(-1.03);
                vfree_samples.push_back(-1.99);
        } else if (n_ik_samples_ == 9) {
                vfree_samples.push_back(-1.51);
                vfree_samples.push_back(-1.75);
                vfree_samples.push_back(-1.27);
                vfree_samples.push_back(-1.03);
                vfree_samples.push_back(-1.99);
                vfree_samples.push_back(-2.23);
                vfree_samples.push_back(-0.79);
                vfree_samples.push_back(-2.46);
                vfree_samples.push_back(-0.55);
        } else if (n_ik_samples_ == 10) {
                float min = -3.7; //-3.9 but we want to stay away from limits
                float max = 0.6;
                float max_samples = 20;
                float dist = (max - min) / max_samples;
                for (float i = -3.7; i < 0.6001; i = i + dist) {
                        vfree_samples.push_back(i);
                }
        } else if (n_ik_samples_ == 11) {
                float min = -3.7; //-3.9 but we want to stay away from limits
                float max = 0.6;
                float max_samples = 50;
                float dist = (max - min) / max_samples;
                for (float i = -3.7; i < 0.6001; i = i + dist) {
                        vfree_samples.push_back(i);
                }
        } else if (n_ik_samples_ == 12) {
                float min = -3.7; //-3.9 but we want to stay away from limits
                float max = 0.6;
                float max_samples = 100;
                float dist = (max - min) / max_samples;
                for (float i = -3.7; i < 0.6001; i = i + dist) {
                        vfree_samples.push_back(i);
                }
        } else {
                ROS_ERROR("not supported sample number");
        }

        for (unsigned int i = 0; i < vfree_samples.size(); ++i) {

                vfree[0] = vfree_samples[i];
                std::vector<IKSolution> vsolutions;
                ik(eetrans, eerot, vfree, vsolutions); //get the ik solutions

                //    ROS_INFO("Found %d ik solutions:\n", (int) vsolutions.size());
                std::vector<double> sol(n_joints);
                for (std::size_t i = 0; i < vsolutions.size(); ++i) {
                        std::vector<double> vsolfree(vsolutions[i].GetFree().size());
                        vsolutions[i].GetSolution(&sol[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL);
                        ik_solutions.push_back(sol);
                }
        }

}

// for a single pose we sample end-effector rotations
void NeighborhoodGraph::computeEndEffectorPose(const SurfacePatch p1, const double dist_above_surface,
                std::vector<tf::Pose>& poses, bool single_pose) const {

        poses.clear();
        //compute point above surface
        Eigen::Vector3f origin = p1.getVector3fMap() + (dist_above_surface * p1.getNormalVector3fMap().normalized())
                        + ((p1.scale_z / 2.0) * p1.getNormalVector3fMap().normalized());

        //set x_axis to point to surface
        Eigen::Vector3f z_axis = p1.getVector3fMap() - origin;
        z_axis.normalize();

        //compute angle-axis representation
        //compute cross product of normal and z-axis and get the rotation with smallest angle
        //cross product is the rotation axis with smallest angle
        Eigen::Vector3f cross_product = Eigen::Vector3f(0, 0, 1).cross(z_axis);
        Eigen::Matrix3f rot_matrix;
        cross_product.normalize();
        float angle = 0;
        if (cross_product.norm() > 0.000000001) { //everything ok
                angle = acos(z_axis.dot(Eigen::Vector3f(0, 0, 1)));
                rot_matrix = Eigen::AngleAxis<float>(angle, cross_product).toRotationMatrix();
        } else { //cross product 0 - normal either in or exactly opposite of z-axis
                double tmp = z_axis.dot(Eigen::Vector3f(0, 0, 1));
                if (tmp > 0) {
                        rot_matrix = Eigen::Matrix3f::Identity();
                } else {
                        angle = M_PI;
                        rot_matrix = Eigen::AngleAxis<float>(angle, cross_product).toRotationMatrix();
                }
        }

        //set everything into the pose
        tf::Pose pose;
        pose.setOrigin(tf::Point(origin[0], origin[1], origin[2]));
        Eigen::Quaternion<float> quat(rot_matrix);
        tf::Quaternion tf_quat(quat.x(), quat.y(), quat.z(), quat.w());
        pose.setRotation(tf_quat);
        poses.push_back(pose);

        if (!single_pose) {

                angle = M_PI / 8.0;
                rot_matrix = Eigen::AngleAxis<float>(angle, Eigen::Vector3f(0, 0, 1)).toRotationMatrix();
                quat = Eigen::Quaternion<float>(rot_matrix);
                tf_quat = tf::Quaternion(quat.x(), quat.y(), quat.z(), quat.w());
                pose.setRotation(tf_quat);
                poses.push_back(pose);

                angle = M_PI / 4.0;
                rot_matrix = Eigen::AngleAxis<float>(angle, Eigen::Vector3f(0, 0, 1)).toRotationMatrix();
                quat = Eigen::Quaternion<float>(rot_matrix);
                tf_quat = tf::Quaternion(quat.x(), quat.y(), quat.z(), quat.w());
                pose.setRotation(tf_quat);
                poses.push_back(pose);
        }
}

void NeighborhoodGraph::computeEndEffectorPose(const SurfacePatch p1, const SurfacePatch p2,
                const double dist_above_surface, tf::Pose& pose) const {

        //compute point above surface
        Eigen::Vector3f origin = p1.getVector3fMap() + (dist_above_surface * p1.getNormalVector3fMap().normalized())
                        + ((p1.scale_z / 2.0) * p1.getNormalVector3fMap().normalized());

        //set x_axis to point to surface
        Eigen::Vector3f z_axis = p1.getVector3fMap() - origin;
        z_axis.normalize();

        //compute angle-axis representation
        //compute cross product of normal and z-axis and get the rotation with smallest angle
        //cross product is the rotation axis with smallest angle
        Eigen::Vector3f cross_product = Eigen::Vector3f(0, 0, 1).cross(z_axis);
        Eigen::Matrix3f rot_matrix;
        cross_product.normalize();
        float angle = 0;
        if (cross_product.norm() > 0.000000001) { //everything ok
                angle = acos(z_axis.dot(Eigen::Vector3f(0, 0, 1)));
                rot_matrix = Eigen::AngleAxis<float>(angle, cross_product).toRotationMatrix();
        } else { //cross product 0 - normal either in or exactly opposite of z-axis
                double tmp = z_axis.dot(Eigen::Vector3f(0, 0, 1));
                if (tmp > 0) {
                        rot_matrix = Eigen::Matrix3f::Identity();
                } else {
                        angle = M_PI;
                        rot_matrix = Eigen::AngleAxis<float>(angle, cross_product).toRotationMatrix();
                }
        }

        //apply transform to next patch
        Eigen::Vector3f origin_next = p2.getVector3fMap() + (dist_above_surface * p2.getNormalVector3fMap().normalized())
                        + ((p2.scale_z / 2.0) * p2.getNormalVector3fMap().normalized());

        origin_next = rot_matrix.transpose() * (origin_next - origin);
        origin_next[2] = 0;
        origin_next.normalize();
        Eigen::Vector3f align_axis = (origin_next);
        Eigen::Vector3f x_axis = Eigen::Vector3f(0, 1, 0);
        cross_product = x_axis.cross(align_axis);
        cross_product.normalize();

        Eigen::Matrix3f rot_matrix2;
        angle = 0;
        if (cross_product.norm() > 0.000000001) { //everything ok
                angle = acos(align_axis.dot(x_axis));

                rot_matrix2 = Eigen::AngleAxis<float>(angle, cross_product).toRotationMatrix();
        } else { //cross product 0 - normal either in or exactly opposite of z-axis
                double tmp = align_axis.dot(x_axis);
                if (tmp > 0) {
                        rot_matrix2 = Eigen::Matrix3f::Identity();
                } else {
                        angle = M_PI;
                        rot_matrix2 = Eigen::AngleAxis<float>(angle, cross_product).toRotationMatrix();
                }
        }

        //set everything into the pose
        pose.setOrigin(tf::Point(origin[0], origin[1], origin[2]));
        Eigen::Quaternion<float> quat(rot_matrix * rot_matrix2);
        tf::Quaternion tf_quat(quat.x(), quat.y(), quat.z(), quat.w());
        pose.setRotation(tf_quat);
        
        //check if quaternion is normalized
        if (fabs(tf_quat.length2() - 1) > tf::QUATERNION_TOLERANCE) {
                ROS_WARN_STREAM(
                                "TF to MSG: Quaternion Not Properly Normalized" << tf_quat.x() << " "<< tf_quat.y() <<" " << tf_quat.z() << " "<<tf_quat.w());
                ROS_WARN_STREAM("MATRIX " << rot_matrix << std::endl << rot_matrix2);
        }
}

void NeighborhoodGraph::getGraph(std::vector<SurfacePatch>& patches, std::vector<pcl::PointXYZ>&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) {

        patches.clear();
        surface_points.clear();
        coll_free_poses.clear();
        coll_free_ik.clear();

        reachable_surface_.resize(surface_.size(), false);

        std::vector<pcl::PointXYZ> samples;
        sampleSurfacePoints(surface_, samples);

        //build the adjacency matrix by checking for octomap freeness
        buildAdjacencyMatrixUsingOctomap();

        std::map<int, int> old_to_new_indexes;
        std::vector<std::pair<int, int> > new_adj_pairs_old_index;

        //revise adjacency matrix using reachability
        for (unsigned int i = 0; i < adjacency_.rows(); ++i) {

                //get possible Cartesian poses for each patch

                int n_edges = adjacency_.row(i).array().sum(); //the current number of edges

                if (n_edges == 0) {
                        //                      std::vector<tf::Pose> patch_poses;
                        //                      computeEndEffectorPose(surface_[i], dist_above_surface_, patch_poses);
                        //
                        //                      //get inverse kinematic solutions and check for collisions
                        //                      std::vector<bool> valid;
                        //                      std::vector<std::vector<std::vector<double> > > ik_solutions;
                        //                      getValidIKSolutions(patch_poses, true, valid, ik_solutions);
                        //
                        //                      std::vector<tf::Pose> valid_poses;
                        //                      std::vector<std::vector<std::vector<double> > > valid_ik_solutions;
                        //                      for (unsigned int j = 0; j < patch_poses.size(); ++j) {
                        //
                        //                              //if the pose is valid fill in all the stuff we need
                        //                              if (valid[j]) {
                        //                                      valid_poses.push_back(patch_poses[j]);
                        //                                      valid_ik_solutions.push_back(ik_solutions[j]); // get all solutions for a single pose
                        //                              }
                        //                      }
                        //                      //if
                        //                      if (valid_poses.size() > 0) {
                        //                              reachable_surface_[i] = true;
                        //                              patches.push_back(surface_[i]);
                        //                              surface_points.push_back(samples[i]);
                        //                              coll_free_poses.push_back(valid_poses);
                        //                              coll_free_ik.push_back(valid_ik_solutions);
                        //                              old_to_new_indexes.insert(std::pair<int, int>(i, patches.size() - 1));
                        //                      }
                } else {

                        std::vector<tf::Pose> valid_poses;
                        std::vector<std::vector<std::vector<double> > > valid_ik_solutions;

                        for (int j = 0; j < adjacency_.cols(); ++j) {
                                if (adjacency_(i, j) == 1) { // if the nodes are adjacent
                                        if (adjacency_(i, j) != 1) {
                                                std::cout << "error 2" << std::endl;
                                        }
                                        tf::Pose tmp_pose;
                                        computeEndEffectorPose(surface_[i], surface_[j], dist_above_surface_, tmp_pose);
                                        std::vector<tf::Pose> patch_poses;
                                        patch_poses.push_back(tmp_pose);
                                        geometry_msgs::Pose geom_pose;
                                        tf::poseTFToMsg(tmp_pose, geom_pose);
                                        //get inverse kinematic solutions and check for collisions
                                        std::vector<bool> valid;
                                        std::vector<std::vector<std::vector<double> > > ik_solutions;
                                        getValidIKSolutions(patch_poses, true, valid, ik_solutions);

                                        tf::Pose tmp_pose2;
                                        computeEndEffectorPose(surface_[j], surface_[i], dist_above_surface_, tmp_pose2);
                                        std::vector<tf::Pose> patch_poses2;
                                        patch_poses2.push_back(tmp_pose2);

                                        //get inverse kinematic solutions and check for collisions
                                        std::vector<bool> valid2;
                                        std::vector<std::vector<std::vector<double> > > ik_solutions2;
                                        getValidIKSolutions(patch_poses2, true, valid2, ik_solutions2);

                                        if (valid[0] && valid2[0]) {
                                                valid_poses.push_back(tmp_pose);
                                                valid_ik_solutions.push_back(ik_solutions[0]); // get all solutions for a single pose
                                                new_adj_pairs_old_index.push_back(std::pair<int, int>(i, j));
                                        }
                                }
                        }
                        if (valid_poses.size() > 0) {
                                reachable_surface_[i] = true;
                                patches.push_back(surface_[i]);
                                surface_points.push_back(samples[i]);
                                coll_free_poses.push_back(valid_poses);
                                coll_free_ik.push_back(valid_ik_solutions);
                                old_to_new_indexes.insert(std::pair<int, int>(i, patches.size() - 1));

                        }

                }

        }

        //construct new adjacency matrix
        adjacency = Eigen::MatrixXi::Zero(patches.size(), patches.size());
        int errors = 0;
        for (unsigned int i = 0; i < new_adj_pairs_old_index.size(); ++i) {
                std::pair<int, int> tmp = new_adj_pairs_old_index[i];
                //find new index to old ones
                std::map<int, int>::iterator it_ind1 = old_to_new_indexes.find(tmp.first);
                std::map<int, int>::iterator it_ind2 = old_to_new_indexes.find(tmp.second);

                //if new indexes for both old ones the edge exists

                if (!(it_ind1 == old_to_new_indexes.end() || it_ind2 == old_to_new_indexes.end())) {
                        adjacency((*it_ind1).second, (*it_ind2).second) = 1;
                } else {
                        errors++;
                        std::cout << "error" << " " << errors << " " << tmp.first << " " << tmp.second << " " << (*it_ind1).second
                                        << " " << (*it_ind2).second << std::endl;
                }
        }

}

void NeighborhoodGraph::constructGraphMarker(const std::vector<pcl::PointXYZ>& surface_samples,
                const Eigen::MatrixXi& adjacency, const std::string frame_id, visualization_msgs::Marker& marker) const {

        marker.points.clear();

        // 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.005;
        marker.scale.y = 0.005;
        marker.scale.z = 0.005;

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

        geometry_msgs::Point start;
        geometry_msgs::Point end;
        marker.points.clear();

        for (unsigned int i = 0; i < surface_samples.size(); ++i) {

                for (unsigned int j = 0; j < surface_samples.size(); ++j) {
                        if (!(i == j) && (adjacency(i, j) == 1)) {
                                start.x = surface_samples[i].x;
                                start.y = surface_samples[i].y;
                                start.z = surface_samples[i].z;
                                end.x = surface_samples[j].x;
                                end.y = surface_samples[j].y;
                                end.z = surface_samples[j].z;

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

}
void NeighborhoodGraph::constructGraphMarker(const std::vector<SurfacePatch>& surface, const Eigen::MatrixXi& adjacency,
                const std::string frame_id, visualization_msgs::Marker& marker) const {

        std::vector<pcl::PointXYZ> samples;
        sampleSurfacePoints(surface, samples);
        constructGraphMarker(samples, adjacency, frame_id, marker);
}